summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-03 06:15:17 +0200
committerNicolas Schodet2011-06-03 06:53:45 +0200
commit002c8f8819a34cc8d589115cca433de716036d8c (patch)
treebf0298c4a80f37141998ab6f134b37d28386ce8d /digital/io-hub/src/robospierre/top.c
parent2eca57339e792160fe21a17956a1257403296643 (diff)
digital/io-hub: little step backward in green zone
Diffstat (limited to 'digital/io-hub/src/robospierre/top.c')
-rw-r--r--digital/io-hub/src/robospierre/top.c13
1 files changed, 13 insertions, 0 deletions
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 286e819c..9cdefc3e 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -81,6 +81,8 @@ struct top_t
uint8_t chaos;
/** Broken clamp. */
uint8_t broken;
+ /** Saved, direction when picking element. */
+ uint8_t go_to_element_direction;
};
/** Global context. */
@@ -140,6 +142,7 @@ top_go_element (void)
logistic_global.prepare = top_prepare_level ();
}
vect_t element_pos = element_get_pos (ctx.target_element_id);
+ ctx.go_to_element_direction = logistic_global.collect_direction;
uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
? 0 : ASSERV_BACKWARD;
move_start_noangle (element_pos, backward, 0);
@@ -325,6 +328,16 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT)
return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked);
}
+FSM_TRANS (TOP_WAITING_CLAMP, clamp_taken, TOP_WAITING_CLAMP)
+{
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ if (robot_pos.v.x < 400 || robot_pos.v.x > PG_WIDTH - 400)
+ asserv_move_linearly (ctx.go_to_element_direction
+ == DIRECTION_FORWARD ? -50 : 50);
+ return FSM_NEXT (TOP_WAITING_CLAMP, clamp_taken);
+}
+
FSM_TRANS_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, 250,
try_again, TOP_UNBLOCKING_SHAKE,
tryout, TOP_WAITING_CLAMP)