From 0eff7675999eef0c1c3175a33e37d8d519cf7fa0 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 7 Apr 2010 23:50:51 +0200 Subject: digital/io/src: add US distance sensor support, refs #113 --- digital/io/src/Makefile | 4 ++-- digital/io/src/main.c | 34 ++++++++++++++++++++------ digital/io/src/usdist.c | 64 +++++++++++++++++++++++++++++++++++++++++++++++++ digital/io/src/usdist.h | 55 ++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 148 insertions(+), 9 deletions(-) create mode 100644 digital/io/src/usdist.c create mode 100644 digital/io/src/usdist.h diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 2896d528..7600baa9 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -4,8 +4,8 @@ BASE = ../../avr PROGS = io # Sources to compile. io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c pwm.c \ - switch.avr.c chrono.c main_timer.avr.c servo_pos.c \ - simu.host.c contact.c + switch.avr.c chrono.c main_timer.avr.c servo_pos.c \ + simu.host.c contact.c usdist.c # Modules needed for IO. MODULES = proto uart twi utils adc math/fixed path trace flash spi # Configuration file. diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 42a7ae17..f61f5268 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -45,6 +45,7 @@ #include "fsm.h" /* fsm_* */ #include "bot.h" #include "servo_pos.h" +#include "usdist.h" #include "chrono.h" /* chrono_end_match */ #include "pwm.h" #include "playground.h" @@ -74,6 +75,11 @@ enum team_color_e bot_color; */ uint8_t main_post_event_for_top_fsm = 0xFF; +/** + * US sensors stats counters. + */ +static uint8_t main_stats_usdist_, main_stats_usdist_cpt_; + /** * Asserv stats counters. */ @@ -174,6 +180,8 @@ main_init (void) /* Path module */ path_init (PG_BORDER_DISTANCE, PG_BORDER_DISTANCE, PG_WIDTH - PG_BORDER_DISTANCE, PG_LENGTH - PG_BORDER_DISTANCE); + /* Distance sensors. */ + usdist_init (); /* Top. */ top_init (); /* Init FSM. */ @@ -232,6 +240,9 @@ main_loop (void) /* Update PWM */ pwm_update (); + /* Update US distance sensors. */ + usdist_update (); + /* Update TWI module to get new data from the asserv board */ asserv_update_status (); @@ -252,6 +263,13 @@ main_loop (void) main_event_to_fsm (); } + /* Send stats if requested. */ + if (main_stats_usdist_ && !--main_stats_usdist_cpt_) + { + proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2], + usdist_mm[3]); + main_stats_usdist_cpt_ = main_stats_usdist_; + } /* Send asserv stats if needed */ if (main_stats_asserv_ && !--main_stats_asserv_cpt_) { @@ -364,14 +382,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) } break; } - /* FSM commands */ + + /* Stats commands. + * - b: interval between stats. */ + case c ('U', 1): + /* US sensors stats. */ + main_stats_usdist_ = main_stats_usdist_cpt_ = args[0]; + break; case c ('A', 1): - { - /* Get position stats - * - 1b: frequency. - */ - main_stats_asserv_ = main_stats_asserv_cpt_ = args[0]; - } + /* Position stats. */ + main_stats_asserv_ = main_stats_asserv_cpt_ = args[0]; break; /* Asserv */ diff --git a/digital/io/src/usdist.c b/digital/io/src/usdist.c new file mode 100644 index 00000000..342358ea --- /dev/null +++ b/digital/io/src/usdist.c @@ -0,0 +1,64 @@ +/* usdist.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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. + * + * }}} */ +#include "common.h" +#include "usdist.h" + +#include "modules/adc/adc.h" +#include "modules/utils/utils.h" +#include "io.h" + +uint16_t usdist_mm[USDIST_NB]; + +void +usdist_init (void) +{ + adc_init (); +} + +void +usdist_update (void) +{ + uint8_t i; + /* Simple algorithm, just make a conversion for every sensor. This should + * be improved because it takes too long. */ + for (i = 0; i < USDIST_NB; i++) + { + adc_start (i); + while (!adc_checkf ()) + ; + uint16_t v = adc_read (); + /* Our sensors return a value between 1 and 5 V proportional to the + * distance between calibrated values. */ + if (v <= 1024 / 5) + usdist_mm[i] = USDIST_MM_MIN; + else + usdist_mm[i] = USDIST_MM_MIN + + (uint32_t) (v - 1024 / 5) * (USDIST_MM_MAX - USDIST_MM_MIN) + / (4 * 1024 / 5); + if (usdist_mm[i] >= USDIST_MM_TOO_FAR) + usdist_mm[i] = 0xffff; + } +} + diff --git a/digital/io/src/usdist.h b/digital/io/src/usdist.h new file mode 100644 index 00000000..f5ceef99 --- /dev/null +++ b/digital/io/src/usdist.h @@ -0,0 +1,55 @@ +#ifndef usdist_h +#define usdist_h +/* usdist.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * 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. + * + * }}} */ + +/* + * Manage ultrasonic distance sensors using analog to digital converter. + */ + +/** Number of sensors. */ +#define USDIST_NB 4 + +/** Minimal calibrated distance. */ +#define USDIST_MM_MIN 100 + +/** Maximal calibrated distance. */ +#define USDIST_MM_MAX 700 + +/** Distance considered as too far to be true. */ +#define USDIST_MM_TOO_FAR 650 + +/** Array containing the last measures in millimeters. */ +extern uint16_t usdist_mm[USDIST_NB]; + +/** Initialise module. */ +void +usdist_init (void); + +/** To be called every cycle to update sensor measures. */ +void +usdist_update (void); + +#endif /* usdist_h */ -- cgit v1.2.3