From 4bf3121c81de96a6c67d7e5d4bb53f0c33fc5e74 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 03:36:20 +0200 Subject: digital/io-hub: near the wall, take the element --- digital/io-hub/src/robospierre/pawn_sensor.c | 11 +++++++++++ 1 file changed, 11 insertions(+) (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index cb4cfe4a..ccb5eb5b 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -31,6 +31,7 @@ #include "element.h" #include "clamp.h" #include "bot.h" +#include "playground.h" #include "codebar.h" #include "modules/utils/utils.h" @@ -91,6 +92,16 @@ pawn_sensor_get (uint8_t direction) asserv_get_position (&robot_position); if (ctx->active) { + /* In green zone, take it when near enougth from the wall. */ + if (robot_position.v.x < BOT_GREEN_ELEMENT_DISTANCE_MM + 10 + || (robot_position.v.x > PG_WIDTH + - BOT_GREEN_ELEMENT_DISTANCE_MM - 10)) + { + ctx->active = 0; + return pawn_sensor_get_type (direction); + } + /* Else, take it if near enough from the supposed element + * position. */ int32_t d = distance_point_point (&ctx->active_position, &robot_position.v); if (d < BOT_PAWN_TAKING_DISTANCE_MM) -- cgit v1.2.3