summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-02 07:09:25 +0200
committerNicolas Schodet2011-06-02 07:09:25 +0200
commitbecf1a33459af05d60f70a9b18bdeb05a987c558 (patch)
treed0b595a78f98e6e7579c6af03e12f05e41d012af /digital/io-hub/src/robospierre/top.c
parentc47f5e0c5a741f16ea115e6912ced21e53daf94f (diff)
digital/io-hub: stop robot when a element is taken
Diffstat (limited to 'digital/io-hub/src/robospierre/top.c')
-rw-r--r--digital/io-hub/src/robospierre/top.c22
1 files changed, 22 insertions, 0 deletions
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 4554c12b..05568099 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -160,6 +160,14 @@ top_decision (void)
return top_go_element ();
}
+static void
+top_taken_pawn (void)
+{
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ element_taken (element_nearest_element_id (robot_pos), ELEMENT_PAWN);
+}
+
FSM_TRANS (TOP_GOING_OUT2, robot_move_success,
clamp_working, TOP_WAITING_CLAMP,
drop, TOP_GOING_TO_DROP,
@@ -224,6 +232,13 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_failure,
}
}
+FSM_TRANS (TOP_GOING_TO_DROP, clamp_working, TOP_WAITING_CLAMP)
+{
+ top_taken_pawn ();
+ move_stop ();
+ return FSM_NEXT (TOP_GOING_TO_DROP, clamp_working);
+}
+
FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
clamp_working, TOP_WAITING_CLAMP,
drop, TOP_GOING_TO_DROP,
@@ -254,6 +269,13 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure,
}
}
+FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP)
+{
+ top_taken_pawn ();
+ move_stop ();
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working);
+}
+
FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
drop, TOP_GOING_TO_DROP,
element, TOP_GOING_TO_ELEMENT)