summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre/clamp.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-01 12:51:46 +0200
committerNicolas Schodet2011-06-01 12:51:46 +0200
commitb2c12931a8378cdb2112da8b21b63099683516f5 (patch)
tree9d9805dc56300df74c59e2c273f27506128d5778 /digital/io-hub/src/robospierre/clamp.c
parent6d3e532a65e14fddb242ecc4841becec44253a6b (diff)
digital/io-hub: do not prepare in green zone
Diffstat (limited to 'digital/io-hub/src/robospierre/clamp.c')
-rw-r--r--digital/io-hub/src/robospierre/clamp.c45
1 files changed, 45 insertions, 0 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index e3e04cf6..3e7fa9fd 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -99,6 +99,8 @@ FSM_STATES (
FSM_EVENTS (
/* New element inside bottom slot. */
clamp_new_element,
+ /* Order to prepare tower. */
+ clamp_prepare,
/* Sent when clamp is working. */
clamp_working,
/* Sent when clamp return to idle state. */
@@ -243,6 +245,13 @@ clamp_new_element (uint8_t pos, uint8_t element_type)
FSM_HANDLE (AI, clamp_new_element);
}
+void
+clamp_prepare (uint8_t prepare)
+{
+ logistic_global.prepare = 1;
+ FSM_HANDLE (AI, clamp_prepare);
+}
+
uint8_t
clamp_drop (uint8_t drop_direction)
{
@@ -477,6 +486,42 @@ FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING)
return FSM_NEXT (CLAMP_IDLE, clamp_new_element);
}
+FSM_TRANS (CLAMP_IDLE, clamp_prepare,
+ move_element, CLAMP_MOVING_ELEMENT,
+ move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
+ done, CLAMP_IDLE)
+{
+ logistic_decision ();
+ if (logistic_global.moving_from != CLAMP_SLOT_NB)
+ {
+ clamp_move_element (logistic_global.moving_from,
+ logistic_global.moving_to);
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_element);
+ }
+ else if (logistic_global.clamp_pos_idle != ctx.pos_current)
+ {
+ if (logistic_path_clear (ctx.pos_current,
+ logistic_global.clamp_pos_idle))
+ {
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_to_idle);
+ }
+ else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, clamp_locked);
+ }
+ }
+ else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, done);
+ }
+}
+
FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING)
{
/* If going forward, drop at back. */