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 ++++++++++++++++++++++++++ digital/io-hub/src/robospierre/clamp.h | 4 +++ digital/io-hub/src/robospierre/element.c | 7 ---- digital/io-hub/src/robospierre/element.h | 6 ++++ digital/io-hub/src/robospierre/logistic.c | 3 +- digital/io-hub/src/robospierre/logistic.h | 4 +++ digital/io-hub/src/robospierre/top.c | 54 +++++++++++++++++++++++++++---- 7 files changed, 107 insertions(+), 16 deletions(-) (limited to 'digital') 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. */ diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 60da1615..76dcc34b 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -80,6 +80,10 @@ clamp_move_element (uint8_t from, uint8_t to); void clamp_new_element (uint8_t pos, uint8_t element_type); +/** Change logisitic preparation level and update clamp state. */ +void +clamp_prepare (uint8_t prepare); + /** Drop an element tower. Return 0 if not currently possible. If * drop_direction is forward, drop at the back. */ uint8_t diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 71f754e0..73b165cc 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -131,13 +131,6 @@ struct element_t element_table[] = {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT} /* right blue */ }; -inline element_t -element_get (uint8_t element_id) -{ - assert (element_id < UTILS_COUNT (element_table)); - return element_table[element_id]; -} - inline void element_set (uint8_t element_id, element_t element) { diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 3ac1f9ba..bcc40f43 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -171,4 +171,10 @@ element_nearest_element_id (position_t robot_pos); vect_t element_get_pos (uint8_t element_id); +extern inline element_t +element_get (uint8_t element_id) +{ + return element_table[element_id]; +} + #endif /* element_h */ diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index fea121cf..32b9fdb0 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -365,8 +365,7 @@ logisitic_make_switches () D, _, RIGHT, 0); } -/** Examine current state and take a decision. */ -static void +void logistic_decision (void) { /* Reset. */ diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 13c849a5..334b9db4 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -97,6 +97,10 @@ extern struct logistic_t logistic_global; void logistic_init (void); +/** Examine current state and take a decision. */ +void +logistic_decision (void); + /** To be called at regular interval to check for bad robot state. */ void logistic_update (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index b99e3baf..0af8ce80 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -51,7 +51,13 @@ FSM_STATES ( TOP_GOING_TO_DROP, TOP_GOING_TO_ELEMENT, /* Waiting clamp has finished its work. */ - TOP_WAITING_CLAMP) + TOP_WAITING_CLAMP, + /* Waiting construction is ready to drop. */ + TOP_WAITING_READY, + /* Dropping, opening the doors. */ + TOP_DROP_DROPPING, + /* Dropping, clearing so that doors can be closed. */ + TOP_DROP_CLEARING) FSM_START_WITH (TOP_START) @@ -85,6 +91,11 @@ top_go_element (void) position_t robot_pos; asserv_get_position (&robot_pos); ctx.target_element_id = element_best (robot_pos); + element_t e = element_get (ctx.target_element_id); + if (e.attr & ELEMENT_GREEN) + logistic_global.prepare = 0; + else + logistic_global.prepare = 1; vect_t element_pos = element_get_pos (ctx.target_element_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; @@ -130,14 +141,18 @@ FSM_TRANS (TOP_GOING_OUT2, robot_move_success, } FSM_TRANS (TOP_GOING_TO_DROP, move_success, - drop, TOP_GOING_TO_DROP, - element, TOP_GOING_TO_ELEMENT) + ready, TOP_DROP_DROPPING, + wait_clamp, TOP_WAITING_READY) { - clamp_drop (logistic_global.collect_direction); - switch (top_decision ()) + if (logistic_global.ready) + { + clamp_drop (logistic_global.collect_direction); + return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready); + } + else { - default: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, drop); - case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, element); + clamp_prepare (1); + return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp); } } @@ -166,3 +181,28 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element); } } + +FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING) +{ + clamp_drop (logistic_global.collect_direction); + return FSM_NEXT (TOP_WAITING_READY, clamp_done); +} + +FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) +{ + asserv_move_linearly (200); + return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting); +} + +FSM_TRANS (TOP_DROP_CLEARING, robot_move_success, + drop, TOP_GOING_TO_DROP, + element, TOP_GOING_TO_ELEMENT) +{ + clamp_drop_clear (); + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop); + case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, element); + } +} + -- cgit v1.2.3