summaryrefslogtreecommitdiff
path: root/n/es-2006/src
diff options
context:
space:
mode:
authordufourj2006-05-24 01:04:08 +0000
committerdufourj2006-05-24 01:04:08 +0000
commit54bb900c6aa2196d93e89aed585d4bad7f3a87c0 (patch)
tree96149bdaf5bf9d697e47473505a4b8e276bfc681 /n/es-2006/src
parent6ce2cb47ddb722db8519152beb9764a75deb80b1 (diff)
ES (commit de travail) :
- gestion du ack ; - gestion sharps avec seuil possible ; - meilleur mise à jour des pins de l'asserv pour les trous vus.
Diffstat (limited to 'n/es-2006/src')
-rw-r--r--n/es-2006/src/main.c47
-rw-r--r--n/es-2006/src/others.h16
-rw-r--r--n/es-2006/src/sensor_rvb.c25
-rw-r--r--n/es-2006/src/sensor_rvb.h2
-rw-r--r--n/es-2006/src/servo_motor.c4
-rw-r--r--n/es-2006/src/servo_motor.h4
-rw-r--r--n/es-2006/src/sharp.c47
-rw-r--r--n/es-2006/src/sharp.h22
-rw-r--r--n/es-2006/src/sniff_rvb.c1
9 files changed, 138 insertions, 30 deletions
diff --git a/n/es-2006/src/main.c b/n/es-2006/src/main.c
index 8320bfd..856343f 100644
--- a/n/es-2006/src/main.c
+++ b/n/es-2006/src/main.c
@@ -38,6 +38,7 @@
#include "servo_motor.h"/* Servo motor module */
#include "sharp.h" /* SHarps module */
#include "lcd.h" /* Communication with the LCD card */
+#include "ack.h" /* Ack module */
#include "barillet.h"
/* Statistics for RVB sensors */
@@ -65,6 +66,11 @@ uint8_t twi_buffer_read;
/* Keyboard LCD update frequency */
uint8_t lcd_key_enable, lcd_key_freq;
+/* Enable stat for ack */
+uint8_t main_ack_freq = 0xFF;
+uint8_t main_ack_enable = 0xFF;
+uint8_t ack_value;
+
/** Call when we receive some data from proto (uart) */
void
proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
@@ -86,11 +92,21 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
main_stats_freq = main_stats_enable = args[0];
break;
+ case c ('f', 0):
+ /* Shut up ! */
+ ack_reset ();
+ main_ack_freq = 1;
+ break;
+
+ case c ('F', 1):
+ /* Change the frequency of ack */
+ main_ack_enable = main_ack_freq = args[0];
+ break;
+
/*** RVB ***/
/* RVB sensors configuration */
- case c ('p', 3):
- temp_16b = v8_to_v16 (args[1], args[2]);
- sensor_rvb_config (args[0], temp_16b);
+ case c ('p', 2):
+ sensor_rvb_config (args[0], args[1]);
break;
/* RVB sniff configuration */
case c ('x', 5):
@@ -112,7 +128,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
if (temp_16b & _BV(compt))
sniff_rvb_set_ref_color (compt, args[2]);
break;
- /* Print stats if the sensor is seen */
+ /* Print stats of the colour seen by the sensor */
case c ('A', 3):
temp_16b = v8_to_v16 (args[0], args[1]);
for (compt = 0; compt < RVB_MAX_SENSOR; compt++)
@@ -129,6 +145,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('u', 1):
sensor_rvb_upper_sensors (args[0]);
break;
+
/* contact */
case c ('c', 0 ):
proto_send1b ('c', others_contact());
@@ -162,7 +179,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('H', 2):
for (compt = 0; compt < SHARP_NUMBER; compt++)
if (args[0] & _BV(compt))
- sharp_stat_enable[compt] = sharp_stat_freq[compt] = args[1];
+ sharp_stat_enable[compt] = sharp_stat_freq[compt] = args[1];
+ break;
+ case c ('o', 5):
+ sharp_config_threshold (args[0], v8_to_v16 (args[1], args[2]),
+ v8_to_v16 (args[3], args[4]));
break;
/* LCD */
@@ -179,7 +200,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
/* tests primaires pour le barillet */
- case c('v',3):
+ case c ('v',3):
temp_16b = v8_to_v16 (args[1], args[2]);
vitesse_turbine( args[0], temp_16b);
break;
@@ -272,6 +293,18 @@ main (void)
main_stats[0] = TCNT0;
+ /* Ack management */
+ compt = ack_get ();
+ if (compt)
+ if (main_ack_enable && !--main_ack_freq)
+ {
+ main_ack_freq = main_ack_enable;
+ proto_send1b ('F', compt);
+ }
+
+ /* Front contact */
+ others_contact_update ();
+
/* RVB Sensors stats */
for (compt = 0; compt < RVB_MAX_SENSOR; compt++)
{
@@ -300,7 +333,7 @@ main (void)
{
/* Re init stats system for this sensor */
sniff_ball_stats[compt] = sniff_ball_stat_enable[compt];
- proto_send2b ('B', compt, sniff_rvb_analysis_ball (compt + 7));
+ proto_send2b ('B', compt + 7, sniff_rvb_analysis_ball (compt + 7));
}
main_stats[1] = TCNT0;
diff --git a/n/es-2006/src/others.h b/n/es-2006/src/others.h
index 0e25348..9b9611f 100644
--- a/n/es-2006/src/others.h
+++ b/n/es-2006/src/others.h
@@ -25,7 +25,10 @@
//
// }}}
-#include "common/io.h"
+#include "io.h"
+#include "common.h"
+
+#include "ack.h"
/* pin responsible for the color mode */
#define JACK_PIN 0
@@ -49,13 +52,20 @@ inline uint8_t others_selectcoul (void)
/** return the value of pin jack */
inline uint8_t others_jack (void)
{
- return GET_PIN_OF_PORTA (JACK_PIN);
+ return !GET_PIN_OF_PORTA (JACK_PIN);
}
/** return the value of pin contact */
inline uint8_t others_contact (void)
{
- return GET_PIN_OF_PORTA (CONTACT_PIN);
+ return !GET_PIN_OF_PORTA (CONTACT_PIN);
+}
+
+/** Update ack if some contact happened ! */
+inline void others_contact_update (void)
+{
+ if (others_contact ())
+ ack_set (ACK_CONTACT_FRONT);
}
/** Return the jack and the select_colour at the same time */
diff --git a/n/es-2006/src/sensor_rvb.c b/n/es-2006/src/sensor_rvb.c
index 9d15223..41954a9 100644
--- a/n/es-2006/src/sensor_rvb.c
+++ b/n/es-2006/src/sensor_rvb.c
@@ -42,7 +42,6 @@
#define RVB_MAX_FALSE_IC 3
/** Max overflow of the timer 1 before thinking the sensor is HS. */
/* TODO Find a way to compute this value */
-/* FIXME with the new sensor config, we can lowdown this value, I hope */
#define RVB_MAX_OVERFLOW 253
/** Wait time between IO change in ns. */
#define RVB_SENSOR_DELAY_IO 125
@@ -62,8 +61,7 @@
/** Max input capture before considering we can start the real capture. */
uint8_t sensor_rvb_conf_max_false_ic_;
/** Max overflow of the timer 1 before thinking the sensor is HS. */
-/* XXX, 8 bits ? */
-uint16_t sensor_rvb_conf_max_ov_;
+uint8_t sensor_rvb_conf_max_ov_;
/** RVB sensor state flag. */
volatile uint8_t sensor_rvb_state_;
/** Internal computed result for one color. */
@@ -200,7 +198,7 @@ sensor_rvb_init (void)
/** Configure some internal variables. */
void
-sensor_rvb_config (uint8_t false_ic, uint16_t max_ov)
+sensor_rvb_config (uint8_t false_ic, uint8_t max_ov)
{
sensor_rvb_conf_max_false_ic_ = false_ic;
sensor_rvb_conf_max_ov_ = max_ov;
@@ -362,16 +360,19 @@ sensor_rvb_update_asserv_pins (void)
{
uint8_t compt;
- for (compt = 0; compt < 4; compt++)
+ if (sensor_rvb_state_ == RVB_STATE_SLEEP)
{
- /* Is sensor capture valid */
- if (sensor_rvb_values[compt][0] < RVB_INVALID_CAPTURE)
+ for (compt = 0; compt < 4; compt++)
{
- if (sniff_rvb_analysis_color (compt, RVB_SNIFF_ONLY_GREEN)
- != RVB_SNIFF_GREEN)
- PORTA |= _BV (compt + 4);
- else
- PORTA &= ~_BV (compt + 4);
+ /* Is sensor capture valid */
+ if (sensor_rvb_values[compt][0] < RVB_INVALID_CAPTURE)
+ {
+ if (sniff_rvb_analysis_color (compt, RVB_SNIFF_ONLY_GREEN)
+ != RVB_SNIFF_GREEN)
+ PORTA |= _BV (compt + 4);
+ else
+ PORTA &= ~_BV (compt + 4);
+ }
}
}
diff --git a/n/es-2006/src/sensor_rvb.h b/n/es-2006/src/sensor_rvb.h
index 8e0e73f..88a6962 100644
--- a/n/es-2006/src/sensor_rvb.h
+++ b/n/es-2006/src/sensor_rvb.h
@@ -77,7 +77,7 @@ void sensor_rvb_start_capture (void);
* - max_ov : maximum overflow of the timer/counter 1 before setting the
* sensor dead. uint8_t !
*/
-void sensor_rvb_config (uint8_t false_ic, uint16_t max_ov);
+void sensor_rvb_config (uint8_t false_ic, uint8_t max_ov);
/** Update pins connected to the asserv AVR with the colors seen by the
* sensors. */
diff --git a/n/es-2006/src/servo_motor.c b/n/es-2006/src/servo_motor.c
index cd919c7..288f78f 100644
--- a/n/es-2006/src/servo_motor.c
+++ b/n/es-2006/src/servo_motor.c
@@ -74,12 +74,12 @@ servo_motor_init (void)
/** Ask the servo motor to go to a position. */
void
-servo_motor_set_pos (uint8_t servo_num, uint8_t servo_pos)
+servo_motor_set_pos (uint8_t servo_mask, uint8_t servo_pos)
{
uint8_t num;
/* for each servo motor */
for (num = 0; num < 3; num++)
- if (servo_num & _BV (num))
+ if (servo_mask & _BV (num))
{
/* Set the value and bound it */
servo_time[num] = servo_pos;
diff --git a/n/es-2006/src/servo_motor.h b/n/es-2006/src/servo_motor.h
index f14afc9..1f5c1d6 100644
--- a/n/es-2006/src/servo_motor.h
+++ b/n/es-2006/src/servo_motor.h
@@ -41,10 +41,10 @@
void servo_motor_init (void);
/** Ask the servo motor to go to a position.
- * - servo_num : mask for sensor ;
+ * - servo_mask: mask for sensor ;
* - servo_pos : desired position.
*/
-void servo_motor_set_pos (uint8_t servo_nun, uint8_t servo_pos);
+void servo_motor_set_pos (uint8_t servo_mask, uint8_t servo_pos);
/** What is your current position my dear servo motor ?
* - servo_num : sensor number (not a mask) ;
diff --git a/n/es-2006/src/sharp.c b/n/es-2006/src/sharp.c
index 62b0719..167b86b 100644
--- a/n/es-2006/src/sharp.c
+++ b/n/es-2006/src/sharp.c
@@ -30,13 +30,24 @@
/** Array of sharp values. */
uint16_t sharp_values[SHARP_NUMBER];
+/** Thresholds */
+uint16_t sharp_threshold_high_[SHARP_NUMBER],
+ sharp_threshold_low_[SHARP_NUMBER];
+uint8_t sharp_old_state[SHARP_NUMBER];
+
/** Init this module. */
void
sharp_init (void)
{
+ uint8_t compt;
/* Init ADC module */
adc_init ();
/* All pins are on the right direction, nothing to do */
+ for (compt = 0; compt < SHARP_NUMBER; compt++)
+ {
+ sharp_threshold_high_[compt] = 5000;
+ sharp_threshold_low_[compt] = 4000;
+ }
}
/** Read data from sharp sensors. */
@@ -57,3 +68,39 @@ sharp_update_values (uint8_t sharp_num)
sharp_values[sharp_num] = adc_read ();
}
}
+
+/** Analyse a sharp value. */
+int8_t
+sharp_analyse_values (uint8_t sharp_num)
+{
+ /* Check sharp exists (prevent kernel panic) */
+ if (sharp_num < SHARP_NUMBER)
+ {
+ // XXX Check more often first
+ if (sharp_values[sharp_num] < sharp_threshold_low_[sharp_num])
+ {
+ sharp_old_state[sharp_num] = SHARP_CLEAR;
+ return SHARP_CLEAR;
+ }
+ if (sharp_values[sharp_num] > sharp_threshold_high_[sharp_num])
+ {
+ sharp_old_state[sharp_num] = SHARP_OBSTACLE;
+ return SHARP_OBSTACLE;
+ }
+ /* If we are here, we are inside the threshold, look at previous value */
+ return sharp_old_state[sharp_num];
+ }
+ return -1;
+}
+
+/** Configure sharp threshold. */
+void
+sharp_config_threshold (uint8_t sharp_num, uint16_t high, uint16_t low)
+{
+ /* Check sharp exists (prevent kernel panic) */
+ if (sharp_num < SHARP_NUMBER)
+ {
+ sharp_threshold_high_[sharp_num] = high;
+ sharp_threshold_low_[sharp_num] = low;
+ }
+}
diff --git a/n/es-2006/src/sharp.h b/n/es-2006/src/sharp.h
index 03827d7..e5f1a58 100644
--- a/n/es-2006/src/sharp.h
+++ b/n/es-2006/src/sharp.h
@@ -28,9 +28,20 @@
#include "common.h"
+/** Sharps positions
+ * ADC1 -> Rear ball
+ * ADC2 -> Front
+ * ADC3 -> Front
+ */
+
/** Number of sharps. */
#define SHARP_NUMBER 3
+/** Something in front of the sharp */
+#define SHARP_OBSTACLE 1
+/** Nothing in front of the sharp */
+#define SHARP_CLEAR 0
+
/** Array of sharp values. */
extern uint16_t sharp_values[SHARP_NUMBER];
@@ -43,12 +54,17 @@ void sharp_init (void);
void sharp_update_values (uint8_t sharp_num);
/** Analyse a sharp value.
- * - sharp_num : the sharp number you want to update.
- * - return : 0 if nothing in front, 1 otherwise.
+ * - sharp_num : the sharp number you want to analyse.
+ * - return : SHARP_CLEAR if nothing in front, SHARP_OBSTACLE otherwise.
*/
-uint8_t sharp_analyse_values (uint8_t sharp_num);
+int8_t sharp_analyse_values (uint8_t sharp_num);
/** Configure sharp threshold.
+ * - sharp_num : the sharp number you want to configure threshold.
+ * - high : high threshold.
+ * - low : low threshold.
+ * Note, high > low !
*/
+void sharp_config_threshold (uint8_t sharp_num, uint16_t high, uint16_t low);
#endif // sharp_h
diff --git a/n/es-2006/src/sniff_rvb.c b/n/es-2006/src/sniff_rvb.c
index e5cc765..cc3c126 100644
--- a/n/es-2006/src/sniff_rvb.c
+++ b/n/es-2006/src/sniff_rvb.c
@@ -176,6 +176,7 @@ sniff_rvb_set_ref_color_green (uint8_t sensor)
void
sniff_rvb_set_ref_color (uint8_t sensor, uint8_t color)
{
+ // Check machine state ?
switch (color)
{
case RVB_SNIFF_GREEN: