From 155e962088c0e321ae9e7aa8d8904166b3b701f8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 3 May 2012 00:24:08 +0200 Subject: host/simu, digital/io-hub/src/guybrush: handle new US sensors positions --- digital/io-hub/src/guybrush/radar_defs.c | 21 ++++++++++++++++----- digital/io-hub/src/guybrush/radar_defs.h | 8 ++++---- host/simu/model/table_eurobot2012.py | 5 +++++ host/simu/robots/guybrush/model/bag.py | 14 ++++---------- host/simu/robots/guybrush/model/clamps.py | 2 ++ 5 files changed, 31 insertions(+), 19 deletions(-) diff --git a/digital/io-hub/src/guybrush/radar_defs.c b/digital/io-hub/src/guybrush/radar_defs.c index 663b2371..d344f958 100644 --- a/digital/io-hub/src/guybrush/radar_defs.c +++ b/digital/io-hub/src/guybrush/radar_defs.c @@ -30,10 +30,14 @@ /** 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 RADAR_SENSOR_FRONT 0 + { &usdist_mm[0], { 120, 0 }, G_ANGLE_UF016_DEG (0) }, +#define RADAR_SENSOR_LEFT 1 + { &usdist_mm[1], { 120, 160 }, G_ANGLE_UF016_DEG (0) }, +#define RADAR_SENSOR_RIGHT 2 + { &usdist_mm[2], { 120, -160 }, G_ANGLE_UF016_DEG (0) }, +#define RADAR_SENSOR_BACK 3 + { &usdist_mm[3], { -130, 0 }, G_ANGLE_UF016_DEG (180) }, }; /** Define exclusion area (considered as invalid point). */ @@ -41,6 +45,13 @@ uint8_t radar_valid (vect_t p, uint8_t sensor) { 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; + && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM + && (sensor == RADAR_SENSOR_FRONT || sensor == RADAR_SENSOR_BACK + || !(p.y >= PG_TOTEM_Y - PG_TOTEM_WIDTH_MM / 2 - RADAR_MARGIN_MM + && p.y < PG_TOTEM_Y + PG_TOTEM_WIDTH_MM / 2 + RADAR_MARGIN_MM + && ((p.x >= PG_TOTEM_LEFT_X - PG_TOTEM_WIDTH_MM / 2 - RADAR_MARGIN_MM + && p.x < PG_TOTEM_LEFT_X + PG_TOTEM_WIDTH_MM / 2 + RADAR_MARGIN_MM) + || (p.x >= PG_TOTEM_RIGHT_X - PG_TOTEM_WIDTH_MM / 2 - RADAR_MARGIN_MM + && p.x < PG_TOTEM_RIGHT_X + PG_TOTEM_WIDTH_MM / 2 + RADAR_MARGIN_MM)))); } diff --git a/digital/io-hub/src/guybrush/radar_defs.h b/digital/io-hub/src/guybrush/radar_defs.h index 5f6a2a73..83f74407 100644 --- a/digital/io-hub/src/guybrush/radar_defs.h +++ b/digital/io-hub/src/guybrush/radar_defs.h @@ -25,7 +25,7 @@ * * }}} */ -#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 +#define RADAR_OBSTACLE_EDGE_RADIUS_MM 150 #define RADAR_OBSTACLE_RADIUS_MM 150 #define RADAR_STOP_MM 350 #define RADAR_CLEARANCE_MM 100 @@ -34,8 +34,8 @@ #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 +#define RADAR_SENSOR_FRONT_NB 3 +#define RADAR_SENSOR_BACK_FIRST 3 +#define RADAR_SENSOR_BACK_NB 1 #endif /* radar_defs_h */ diff --git a/host/simu/model/table_eurobot2012.py b/host/simu/model/table_eurobot2012.py index 38960513..36dbf12f 100644 --- a/host/simu/model/table_eurobot2012.py +++ b/host/simu/model/table_eurobot2012.py @@ -90,4 +90,9 @@ class Table (simu.model.table.Table): # Add everything to obstacles. self.obstacles += self.coins self.obstacles += self.gold_bars + # Add totem as obstacles. + for i in (-1, 1): + o = RectangularObstacle ((250, 250), 3) + o.pos = (1500 + i * 400, 1000) + self.obstacles.append (o) diff --git a/host/simu/robots/guybrush/model/bag.py b/host/simu/robots/guybrush/model/bag.py index f9c98814..a6302f44 100644 --- a/host/simu/robots/guybrush/model/bag.py +++ b/host/simu/robots/guybrush/model/bag.py @@ -72,21 +72,15 @@ class Bag: output[self.OUTPUT_DOOR_OPEN], scheduler, 0., 1., 1., 1., 1., contact[7], contact[6]) ) - def distance_sensor_exclude (o): - return o is self.beacon self.distance_sensor = [ DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table, - (20, 20), pi * 10 / 180, (self.position, ), 5, - distance_sensor_exclude), + (120, 0), 0, (self.position, ), 4), DistanceSensorSensopart (link_bag.io_hub.adc[1], scheduler, table, - (20, -20), -pi * 10 / 180, (self.position, ), 5, - distance_sensor_exclude), + (120, 160), 0, (self.position, ), set ([3, 4])), DistanceSensorSensopart (link_bag.io_hub.adc[2], scheduler, table, - (-20, -20), pi + pi * 10 / 180, (self.position, ), 5, - distance_sensor_exclude), + (120, -160), 0, (self.position, ), set ([3, 4])), DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, - (-20, 20), pi - pi * 10 / 180, (self.position, ), 5, - distance_sensor_exclude), + (-130, 0), pi, (self.position, ), 4), ] self.path = link_bag.io_hub.path self.pos_report = link_bag.io_hub.pos_report diff --git a/host/simu/robots/guybrush/model/clamps.py b/host/simu/robots/guybrush/model/clamps.py index 5d75ecb4..664f0bdd 100644 --- a/host/simu/robots/guybrush/model/clamps.py +++ b/host/simu/robots/guybrush/model/clamps.py @@ -155,6 +155,8 @@ class Clamps (Observable): def __add_load (self, elements): """Add element list to load.""" + for e in elements: + e.level = 1 self.load.extend (elements) def __get_floor_elements (self): -- cgit v1.2.3