From 32a21a69bb7adb44c4fc91d025ad8f0ccdaf45a1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 15:07:25 +0200 Subject: digital/io-hub: remove special case for green elements --- digital/io-hub/src/robospierre/bot.h | 7 +-- digital/io-hub/src/robospierre/element.c | 13 ++-- digital/io-hub/src/robospierre/element.h | 5 +- digital/io-hub/src/robospierre/test_element.c | 4 +- digital/io-hub/src/robospierre/top.c | 85 ++++----------------------- 5 files changed, 21 insertions(+), 93 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 8e32a317..de32ebb2 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -58,10 +58,9 @@ /** Distance from border to position in front of a green element. */ #define BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM 600 -/** Distance to go to capture a green element. */ -#define BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM \ - (BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM - BOT_ELEMENT_RADIUS \ - - BOT_SIZE_FRONT - 20) +/** Distance from border to go to capture a green element. */ +#define BOT_GREEN_ELEMENT_DISTANCE_MM \ + (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 20) /** Speed used for initialisation. */ #ifdef HOST diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index bc76a2b0..71f754e0 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -706,24 +706,21 @@ element_opposed (uint8_t element_id) return op; } -position_t +vect_t element_get_pos (uint8_t element_id) { element_t e = element_get (element_id); - position_t pos; - pos.v = e.pos; - pos.a = 0xffff; + vect_t pos; + pos = e.pos; if (e.attr == (ELEMENT_GREEN |ELEMENT_RIGHT)) { /* To the right. */ - pos.a = 0x0000; - pos.v.x = PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM; + pos.x = PG_WIDTH - BOT_GREEN_ELEMENT_DISTANCE_MM; } if (e.attr == (ELEMENT_GREEN |ELEMENT_LEFT)) { /* To the left. */ - pos.a = 0x8000; - pos.v.x = BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM; + pos.x = BOT_GREEN_ELEMENT_DISTANCE_MM; } return pos; } diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 01e8f22e..3ac1f9ba 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -167,11 +167,8 @@ element_nearest_element_id (position_t robot_pos); /** Give the position where the robot need to be placed to get an element. - The position correspond to the emplacement of the element with an angle of - 0xffff. If the element is in the green zone, the returned position include - the angle the robot need to set before moving to the element. */ -position_t +vect_t element_get_pos (uint8_t element_id); #endif /* element_h */ diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c index 2de92361..7e62a492 100644 --- a/digital/io-hub/src/robospierre/test_element.c +++ b/digital/io-hub/src/robospierre/test_element.c @@ -328,8 +328,8 @@ int main () case 'g': printf ("element id: "); scanf ("%i", &x); - position_t pos = element_get_pos (x); - printf ("x: %u y: %u a: %u\n", pos.v.x, pos.v.y, pos.a); + vect_t pos = element_get_pos (x); + printf ("x: %u y: %u\n", pos.x, pos.y); break; case 'q': exit = 1; diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 5c10a0f3..a3ad4c72 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -49,11 +49,7 @@ FSM_STATES ( TOP_GOING_OUT2, TOP_GOING_TO_DROP, - TOP_GOING_TO_ELEMENT, - TOP_GOING_TO_GREEN_POS, - TOP_GOING_TO_GREEN_ELEMENT, - TOP_TAKING_GREEN_ELEMENT, - TOP_GOING_FROM_GREEN_ELEMENT) + TOP_GOING_TO_ELEMENT) FSM_START_WITH (TOP_START) @@ -62,8 +58,6 @@ struct top_t { /** Target element. */ uint8_t target_element_id; - /** Green element move distance. */ - int16_t green_move_distance_mm; }; /** Global context. */ @@ -89,27 +83,11 @@ top_go_element (void) position_t robot_pos; asserv_get_position (&robot_pos); ctx.target_element_id = element_best (robot_pos); - position_t element_pos = element_get_pos (ctx.target_element_id); + vect_t element_pos = element_get_pos (ctx.target_element_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; - if (element_pos.a != 0xffff) - { - /* Green zone element. */ - ctx.green_move_distance_mm = BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM; - if (backward) - { - element_pos.a += 0x8000; - ctx.green_move_distance_mm = -ctx.green_move_distance_mm; - } - move_start (element_pos, backward); - return 2; - } - else - { - /* Regular element. */ - move_start_noangle (element_pos.v, backward, 0); - return 1; - } + move_start_noangle (element_pos, backward, 0); + return 1; } static uint8_t @@ -118,10 +96,10 @@ top_go_drop (void) position_t robot_pos; asserv_get_position (&robot_pos); uint8_t drop_pos_id = 37; - position_t drop_pos = element_get_pos (drop_pos_id); + vect_t drop_pos = element_get_pos (drop_pos_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; - move_start_noangle (drop_pos.v, backward, 0); + move_start_noangle (drop_pos, backward, 0); return 0; } @@ -137,79 +115,36 @@ top_decision (void) FSM_TRANS (TOP_GOING_OUT2, robot_move_success, drop, TOP_GOING_TO_DROP, - element, TOP_GOING_TO_ELEMENT, - green_element, TOP_GOING_TO_GREEN_POS) + element, TOP_GOING_TO_ELEMENT) { switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, drop); case 1: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, element); - case 2: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, - green_element); } } FSM_TRANS (TOP_GOING_TO_DROP, move_success, drop, TOP_GOING_TO_DROP, - element, TOP_GOING_TO_ELEMENT, - green_element, TOP_GOING_TO_GREEN_POS) + element, TOP_GOING_TO_ELEMENT) { clamp_drop (logistic_global.collect_direction); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, drop); case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, element); - case 2: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, - green_element); } } FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, drop, TOP_GOING_TO_DROP, - element, TOP_GOING_TO_ELEMENT, - green_element, TOP_GOING_TO_GREEN_POS) + element, TOP_GOING_TO_ELEMENT) { + element_taken (ctx.target_element_id, ELEMENT_PAWN); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop); case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, element); - case 2: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, - green_element); - } -} - -FSM_TRANS (TOP_GOING_TO_GREEN_POS, move_success, TOP_GOING_TO_GREEN_ELEMENT) -{ - asserv_move_linearly (ctx.green_move_distance_mm); - return FSM_NEXT (TOP_GOING_TO_GREEN_POS, move_success); -} - -FSM_TRANS (TOP_GOING_TO_GREEN_ELEMENT, robot_move_success, - TOP_TAKING_GREEN_ELEMENT) -{ - element_taken (ctx.target_element_id, ELEMENT_PAWN); - return FSM_NEXT (TOP_GOING_TO_GREEN_ELEMENT, robot_move_success); -} - -FSM_TRANS_TIMEOUT (TOP_TAKING_GREEN_ELEMENT, 250, TOP_GOING_FROM_GREEN_ELEMENT) -{ - asserv_move_linearly (-ctx.green_move_distance_mm); - return FSM_NEXT_TIMEOUT (TOP_TAKING_GREEN_ELEMENT); -} - -FSM_TRANS (TOP_GOING_FROM_GREEN_ELEMENT, robot_move_success, - drop, TOP_GOING_TO_DROP, - element, TOP_GOING_TO_ELEMENT, - green_element, TOP_GOING_TO_GREEN_POS) -{ - switch (top_decision ()) - { - default: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, - robot_move_success, drop); - case 1: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, - robot_move_success, element); - case 2: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, - robot_move_success, green_element); } } -- cgit v1.2.3