From b2c12931a8378cdb2112da8b21b63099683516f5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 12:51:46 +0200 Subject: digital/io-hub: do not prepare in green zone --- digital/io-hub/src/robospierre/clamp.c | 45 ++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) (limited to 'digital/io-hub/src/robospierre/clamp.c') 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. */ -- cgit v1.2.3