summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub')
-rw-r--r--digital/io-hub/src/robospierre/Makefile3
-rw-r--r--digital/io-hub/src/robospierre/bot.h3
-rw-r--r--digital/io-hub/src/robospierre/main.c13
-rw-r--r--digital/io-hub/src/robospierre/radar_defs.c46
-rw-r--r--digital/io-hub/src/robospierre/radar_defs.h41
-rw-r--r--digital/io-hub/src/robospierre/simu.host.c15
-rw-r--r--digital/io-hub/src/robospierre/simu.host.h10
-rw-r--r--digital/io-hub/tools/io_hub/mex.py22
8 files changed, 151 insertions, 2 deletions
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)