From c5b1353dea8a426d6eea043fb5aa78722f7b0bc5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 00:13:14 +0200 Subject: digital/io-hub: add US distance sensors --- digital/io-hub/src/robospierre/Makefile | 1 + digital/io-hub/src/robospierre/avrconfig.h | 12 ++++++++++++ digital/io-hub/src/robospierre/main.c | 20 ++++++++++++++++++++ digital/io-hub/src/robospierre/simu.host.c | 14 +++++++++++++- digital/io-hub/src/robospierre/simu.host.h | 2 +- digital/io-hub/tools/io_hub/mex.py | 20 ++++++++++++++++---- host/simu/model/distance_sensor_sensopart.py | 2 +- host/simu/robots/robospierre/model/bag.py | 2 -- 8 files changed, 64 insertions(+), 9 deletions(-) diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index d69918f5..bd8e920f 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -12,6 +12,7 @@ io_hub_SOURCES = main.c \ chrono.c timer.avr.c simu.host.c # Modules needed for IO. MODULES = proto uart twi utils \ + adc devices/usdist \ math/fixed math/geometry AI_MODULES = twi_master common utils fsm # Configuration file. diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h index 0fc2f11d..53e36ccf 100644 --- a/digital/io-hub/src/robospierre/avrconfig.h +++ b/digital/io-hub/src/robospierre/avrconfig.h @@ -100,6 +100,18 @@ /** Use internal pull up. */ #define AC_TWI_PULL_UP 0 +/* usdist - Analog US distance sensor. */ +/** Number of sensors. */ +#define AC_USDIST_NB 4 +/** Measuring period, in number of update call. */ +#define AC_USDIST_PERIOD 1 +/** List of space separated sensor definition, see usdist.h. */ +#define AC_USDIST_SENSORS \ + USDIST_SENSOR (0, A, 0) \ + USDIST_SENSOR (1, A, 1) \ + USDIST_SENSOR (2, A, 2) \ + USDIST_SENSOR (3, A, 3) + /* io-hub - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 10 diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 4e75a92e..8d51c469 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -28,6 +28,8 @@ #include "modules/uart/uart.h" #include "modules/proto/proto.h" +#include "modules/devices/usdist/usdist.h" + #include "timer.h" #include "chrono.h" #include "simu.host.h" @@ -66,6 +68,9 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; /** Contact stats counters. */ static uint8_t main_stats_contact_, main_stats_contact_cpt_; +/** US sensors stats counters. */ +static uint8_t main_stats_usdist_, main_stats_usdist_cpt_; + /** Main initialisation. */ static void main_init (void) @@ -89,6 +94,7 @@ main_init (void) /* IO modules. */ pwm_init (); contact_init (); + usdist_init (); /* AI modules. */ logistic_init (); /* Initialization done. */ @@ -170,6 +176,10 @@ main_loop (void) /* Update IO modules. */ pwm_update (); contact_update (); + if (usdist_update ()) + { + /* TODO: update radar. */ + } /* Update AI modules. */ logistic_update (); /* Only manage events if slaves are synchronised. */ @@ -191,6 +201,12 @@ main_loop (void) proto_send1d ('P', contact_all ()); main_stats_contact_cpt_ = main_stats_contact_; } + 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_; + } } } @@ -269,6 +285,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Contact stats. */ main_stats_contact_ = main_stats_contact_cpt_ = args[0]; break; + case c ('U', 1): + /* US sensors stats. */ + main_stats_usdist_ = main_stats_usdist_cpt_ = args[0]; + break; default: /* Unknown commands */ proto_send0 ('?'); diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index 281a60d6..f7a43eea 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -28,10 +28,20 @@ #include "modules/utils/utils.h" #include "modules/host/host.h" #include "modules/host/mex.h" +#include "modules/adc/adc.h" #include "io.h" /** AVR registers. */ -uint8_t PINA, PINE, PINF; +uint8_t PORTA, DDRA, PINA, PINE, PINF; + +static void +simu_adc_handle (void *user, mex_msg_t *msg) +{ + uint8_t index; + uint16_t value; + mex_msg_pop (msg, "BH", &index, &value); + adc_values[index] = value; +} /** Initialise simulation. */ void @@ -40,6 +50,8 @@ simu_init (void) const char *mex_instance; mex_node_connect (); 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); } /** Make a simulation step. */ diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h index ed6a002d..6a636851 100644 --- a/digital/io-hub/src/robospierre/simu.host.h +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -27,7 +27,7 @@ #ifdef HOST -extern uint8_t PINA, PINE, PINF; +extern uint8_t PORTA, DDRA, PINA, PINE, PINF; #else /* !defined (HOST) */ diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index d9568197..22e9e900 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -44,9 +44,21 @@ class Mex: """ - def __init__ (self): + def __init__ (self, node, instance, index): Observable.__init__ (self) - self.value = None + self.value = 0 + self.__node = node + self.__mtype = node.reserve (instance + ':adc') + self.__index = index + self.register (self.__notified) + + def __notified (self): + m = simu.mex.msg.Msg (self.__mtype) + v = int (1024 * self.value / 5) + v = min (v, 1023) + v = max (0, v) + m.push ('BH', self.__index, v) + self.__node.send (m) class PWM (Observable): """PWM output. @@ -110,9 +122,9 @@ class Mex: self.node.send (m) def __init__ (self, node, instance = 'io-hub0'): - self.adc = tuple (self.ADC () for i in range (0, ADC_NB)) + 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)) - self.__adc_pwm = self.PWM.Pack (node, instance, self.pwm) + self.__pwm_pack = self.PWM.Pack (node, instance, self.pwm) self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) diff --git a/host/simu/model/distance_sensor_sensopart.py b/host/simu/model/distance_sensor_sensopart.py index 16676e5d..26ed604f 100644 --- a/host/simu/model/distance_sensor_sensopart.py +++ b/host/simu/model/distance_sensor_sensopart.py @@ -59,8 +59,8 @@ class DistanceSensorSensopart (Observable): self.link = link self.scheduler = scheduler self.value = None - self.register (self.__update) self.evaluate () + self.register (self.__update) def evaluate (self): # Compute real distance. diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index b1c49d35..de8eaa22 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -54,6 +54,4 @@ class Bag: DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), ] - for adc in link_bag.io_hub.adc[4:]: - adc.value = 0 -- cgit v1.2.3