From 6548d83aa655176ef939817e36cd05d0e6cfaccd Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 01:23:43 +0200 Subject: digital/{io,io-hub,ai}: add generic radar, add radar to robospierre --- digital/io-hub/src/robospierre/Makefile | 3 +- digital/io-hub/src/robospierre/bot.h | 3 ++ digital/io-hub/src/robospierre/main.c | 13 +++++++- digital/io-hub/src/robospierre/radar_defs.c | 46 +++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/radar_defs.h | 41 +++++++++++++++++++++++++ digital/io-hub/src/robospierre/simu.host.c | 15 ++++++++++ digital/io-hub/src/robospierre/simu.host.h | 10 +++++++ digital/io-hub/tools/io_hub/mex.py | 22 ++++++++++++++ 8 files changed, 151 insertions(+), 2 deletions(-) create mode 100644 digital/io-hub/src/robospierre/radar_defs.c create mode 100644 digital/io-hub/src/robospierre/radar_defs.h (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index bd8e920f..fe2b318e 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,6 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ clamp.c logistic.c \ + radar_defs.c radar.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ @@ -14,7 +15,7 @@ io_hub_SOURCES = main.c \ MODULES = proto uart twi utils \ adc devices/usdist \ math/fixed math/geometry -AI_MODULES = twi_master common utils fsm +AI_MODULES = twi_master common utils fsm move # Configuration file. CONFIGFILE = avrconfig.h AVR_MCU = at90usb1287 diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 5f9d177a..4df9b89d 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -34,6 +34,9 @@ # define BOT_SCALE 0.0415178942124 #endif +/** Distance from the robot axis to the side. */ +#define BOT_SIZE_SIDE 190 + /** Distance between the front contact point and the robot center. */ #define BOT_FRONT_CONTACT_DIST_MM 150 /** Angle error at the front contact point. */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 8d51c469..6d32c28f 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -40,6 +40,7 @@ #include "pwm.h" #include "contact.h" +#include "radar.h" #define FSM_NAME AI #include "fsm.h" @@ -62,6 +63,12 @@ /** Our color. */ enum team_color_e team_color; +/** Obstacles positions, updated using radar module. */ +vect_t main_obstacles_pos[2]; + +/** Number of obstacles in main_obstacles_pos. */ +uint8_t main_obstacles_nb; + /** Asserv stats counters. */ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; @@ -178,7 +185,11 @@ main_loop (void) contact_update (); if (usdist_update ()) { - /* TODO: update radar. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + main_obstacles_nb = radar_update (&robot_pos, main_obstacles_pos); + //move_obstacles_update (); + simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0); } /* Update AI modules. */ logistic_update (); diff --git a/digital/io-hub/src/robospierre/radar_defs.c b/digital/io-hub/src/robospierre/radar_defs.c new file mode 100644 index 00000000..85f015e1 --- /dev/null +++ b/digital/io-hub/src/robospierre/radar_defs.c @@ -0,0 +1,46 @@ +/* radar_defs.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 "radar.h" + +#include "modules/devices/usdist/usdist.h" +#include "playground.h" + +/** Define radar configuration. */ +struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = { + { &usdist_mm[0], { 20, 20 }, G_ANGLE_UF016_DEG (10) }, + { &usdist_mm[1], { 20, -20 }, G_ANGLE_UF016_DEG (-10) }, + { &usdist_mm[2], { -20, -20 }, G_ANGLE_UF016_DEG (180 + 10) }, + { &usdist_mm[3], { -20, 20 }, G_ANGLE_UF016_DEG (180 - 10) }, +}; + +/** Define exclusion area (considered as invalid point). */ +uint8_t +radar_valid (vect_t p) +{ + return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM + && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM; +} + diff --git a/digital/io-hub/src/robospierre/radar_defs.h b/digital/io-hub/src/robospierre/radar_defs.h new file mode 100644 index 00000000..5f6a2a73 --- /dev/null +++ b/digital/io-hub/src/robospierre/radar_defs.h @@ -0,0 +1,41 @@ +#ifndef radar_defs_h +#define radar_defs_h +/* radar_defs.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. + * + * }}} */ + +#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 +#define RADAR_OBSTACLE_RADIUS_MM 150 +#define RADAR_STOP_MM 350 +#define RADAR_CLEARANCE_MM 100 +#define RADAR_EPSILON_MM 70 + +#define RADAR_SENSOR_NB 4 + +#define RADAR_SENSOR_FRONT_FIRST 0 +#define RADAR_SENSOR_FRONT_NB 2 +#define RADAR_SENSOR_BACK_FIRST 2 +#define RADAR_SENSOR_BACK_NB 2 + +#endif /* radar_defs_h */ diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index f7a43eea..7ca7364b 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -34,6 +34,9 @@ /** AVR registers. */ uint8_t PORTA, DDRA, PINA, PINE, PINF; +/** Message types. */ +uint8_t simu_mex_pos_report; + static void simu_adc_handle (void *user, mex_msg_t *msg) { @@ -52,6 +55,7 @@ simu_init (void) mex_instance = host_get_instance ("io-hub0", 0); uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance); mex_node_register (mtype, simu_adc_handle, 0); + simu_mex_pos_report = mex_node_reservef ("%s:pos-report", mex_instance); } /** Make a simulation step. */ @@ -74,3 +78,14 @@ timer_wait (void) return 0; } +void +simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id) +{ + mex_msg_t *m; + m = mex_msg_new (simu_mex_pos_report); + mex_msg_push (m, "b", id); + for (; pos_nb; pos++, pos_nb--) + mex_msg_push (m, "hh", pos->x, pos->y); + mex_node_send (m); +} + diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h index 6a636851..6224e345 100644 --- a/digital/io-hub/src/robospierre/simu.host.h +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -24,13 +24,23 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "defs.h" #ifdef HOST extern uint8_t PORTA, DDRA, PINA, PINE, PINF; +/** Send general purpose positions to indicate computation results. + * - pos: array of positions to report. + * - pos_nb: number of elements in the array. + * - id: identifier so that several unrelated positions could be reported. */ +void +simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id); + #else /* !defined (HOST) */ +#define simu_send_pos_report(pos, pos_nb, id) ((void) 0) + #endif /* !defined (HOST) */ #endif /* simu_host_h */ diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 22e9e900..44c41010 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -121,6 +121,27 @@ class Mex: m.push ('L', self.contacts) self.node.send (m) + class PosReport (Observable): + """General purpose position report. + + - pos: dict of sequence of (x, y) coordinates (millimeters). The dict + is indexed by position identifier. + + """ + + def __init__ (self, node, instance): + Observable.__init__ (self) + self.pos = { } + node.register (instance + ':pos-report', self.__handle) + + def __handle (self, msg): + p = [ ] + id, = msg.pop ('b') + while len (msg) >= 4: + p.append (msg.pop ('hh')) + self.pos[id] = p + self.notify () + def __init__ (self, node, instance = 'io-hub0'): self.adc = tuple (self.ADC (node, instance, i) for i in range (0, ADC_NB)) self.pwm = tuple (self.PWM () for i in range (0, PWM_NB)) @@ -128,4 +149,5 @@ class Mex: self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) + self.pos_report = self.PosReport (node, instance) -- cgit v1.2.3