summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-03 16:47:04 +0200
committerNicolas Schodet2011-06-03 16:47:04 +0200
commit5c915c07f8440097109763848d4fc0e06ea8078d (patch)
treef8d4b718e1cb5f570928fde49b7b6081228bd08e /digital/io-hub/src/robospierre
parent53483bc33fd092d70be211fcaba1db5372e1706d (diff)
digital/io-hub: fix undroppable stateseurobot-2011-4
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/top.c16
1 files changed, 11 insertions, 5 deletions
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 7ed145a2..3313a908 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -248,8 +248,10 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success,
{
if (logistic_global.ready)
{
- clamp_drop (logistic_global.collect_direction);
- return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready);
+ if (clamp_drop (logistic_global.collect_direction))
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready);
+ else
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp);
}
else
{
@@ -385,10 +387,14 @@ FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_failure,
return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_failure);
}
-FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING)
+FSM_TRANS (TOP_WAITING_READY, clamp_done,
+ drop, TOP_DROP_DROPPING,
+ not_ready, TOP_WAITING_READY)
{
- clamp_drop (logistic_global.collect_direction);
- return FSM_NEXT (TOP_WAITING_READY, clamp_done);
+ if (clamp_drop (logistic_global.collect_direction))
+ return FSM_NEXT (TOP_WAITING_READY, clamp_done, drop);
+ else
+ return FSM_NEXT (TOP_WAITING_READY, clamp_done, not_ready);
}
FSM_TRANS (TOP_WAITING_READY, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT)