From 5c915c07f8440097109763848d4fc0e06ea8078d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 16:47:04 +0200 Subject: digital/io-hub: fix undroppable states --- digital/io-hub/src/robospierre/top.c | 16 +++++++++++----- 1 file 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) -- cgit v1.2.3