From 54bb900c6aa2196d93e89aed585d4bad7f3a87c0 Mon Sep 17 00:00:00 2001 From: dufourj Date: Wed, 24 May 2006 01:04:08 +0000 Subject: ES (commit de travail) : - gestion du ack ; - gestion sharps avec seuil possible ; - meilleur mise à jour des pins de l'asserv pour les trous vus. --- n/es-2006/README | 24 +++++++++++++++++++++++ n/es-2006/src/main.c | 47 ++++++++++++++++++++++++++++++++++++++------- n/es-2006/src/others.h | 16 ++++++++++++--- n/es-2006/src/sensor_rvb.c | 25 ++++++++++++------------ n/es-2006/src/sensor_rvb.h | 2 +- n/es-2006/src/servo_motor.c | 4 ++-- n/es-2006/src/servo_motor.h | 4 ++-- n/es-2006/src/sharp.c | 47 +++++++++++++++++++++++++++++++++++++++++++++ n/es-2006/src/sharp.h | 22 ++++++++++++++++++--- n/es-2006/src/sniff_rvb.c | 1 + 10 files changed, 162 insertions(+), 30 deletions(-) create mode 100644 n/es-2006/README (limited to 'n') diff --git a/n/es-2006/README b/n/es-2006/README new file mode 100644 index 0000000..1687713 --- /dev/null +++ b/n/es-2006/README @@ -0,0 +1,24 @@ +es - Input/Output general purpose board. + +General purpose board using an AVR. + + +Copyright (C) 2006 Thomas Lambert & Dufour Jérémy + +Robot APB Team/Efrei 2006. + Web: http://assos.efrei.fr/robot/ + Email: robot AT efrei DOT fr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 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: -- cgit v1.2.3