summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-31 15:07:25 +0200
committerNicolas Schodet2011-05-31 15:17:37 +0200
commit32a21a69bb7adb44c4fc91d025ad8f0ccdaf45a1 (patch)
tree03aac5ed95b95902279b7604e4dc13697104942f /digital/io-hub/src/robospierre/top.c
parent7110e5fc8d629f10a85a4355bf34e5d022845f1c (diff)
digital/io-hub: remove special case for green elements
Diffstat (limited to 'digital/io-hub/src/robospierre/top.c')
-rw-r--r--digital/io-hub/src/robospierre/top.c85
1 files changed, 10 insertions, 75 deletions
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);
}
}