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/top.c | 85 +++++------------------------------- 1 file changed, 10 insertions(+), 75 deletions(-) (limited to 'digital/io-hub/src/robospierre/top.c') 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