summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-31 16:15:43 +0200
committerNicolas Schodet2011-05-31 22:44:18 +0200
commit0b188fa1a29f2b7f73a88e87c82724132ea53f1b (patch)
tree69dc09372e0e22e5725d99ae7e40c65e61486ba2 /digital/io-hub/src/robospierre/top.c
parent079e9cd0832d9854b172d120c822b578a2788e9a (diff)
digital/io-hub: add clamp waiting
Diffstat (limited to 'digital/io-hub/src/robospierre/top.c')
-rw-r--r--digital/io-hub/src/robospierre/top.c17
1 files changed, 16 insertions, 1 deletions
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index a3ad4c72..92e42b1c 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -49,7 +49,9 @@ FSM_STATES (
TOP_GOING_OUT2,
TOP_GOING_TO_DROP,
- TOP_GOING_TO_ELEMENT)
+ TOP_GOING_TO_ELEMENT,
+ /* Waiting clamp has finished its work. */
+ TOP_WAITING_CLAMP)
FSM_START_WITH (TOP_START)
@@ -137,10 +139,13 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success,
}
FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
+ clamp_working, TOP_WAITING_CLAMP,
drop, TOP_GOING_TO_DROP,
element, TOP_GOING_TO_ELEMENT)
{
element_taken (ctx.target_element_id, ELEMENT_PAWN);
+ if (clamp_working ())
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working);
switch (top_decision ())
{
default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop);
@@ -148,3 +153,13 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
}
}
+FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, drop);
+ case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element);
+ }
+}