summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/io-hub/src/guybrush/radar_defs.c21
-rw-r--r--digital/io-hub/src/guybrush/radar_defs.h8
-rw-r--r--host/simu/model/table_eurobot2012.py5
-rw-r--r--host/simu/robots/guybrush/model/bag.py14
-rw-r--r--host/simu/robots/guybrush/model/clamps.py2
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):