From 63407d7ed7b959e883c67107b3a80e859144e01d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 11:06:14 +0200 Subject: digital/io-hub: handle clamp blocking --- digital/io-hub/src/robospierre/clamp.c | 101 ++++++++++++++++++++++++++++-- digital/io-hub/src/robospierre/logistic.c | 8 +++ digital/io-hub/src/robospierre/logistic.h | 4 ++ digital/io-hub/src/robospierre/main.c | 3 + digital/io-hub/src/robospierre/top.c | 65 ++++++++++++++++--- 5 files changed, 169 insertions(+), 12 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 812d4890..e05f79d7 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -78,6 +78,8 @@ FSM_STATES ( CLAMP_DROPING_WAITING_ROBOT, /* Clamp locked in a bay. */ CLAMP_LOCKED, + /* Clamp blocked. */ + CLAMP_BLOCKED, /* Waiting movement order. */ CLAMP_MOVE_IDLE, @@ -105,6 +107,8 @@ FSM_EVENTS ( clamp_working, /* Sent when clamp return to idle state. */ clamp_done, + /* Sent when clamp is blocked. */ + clamp_blocked, /* Order to drop elements. */ clamp_drop, /* Sent once drop is done, but robot should advance to completely @@ -116,8 +120,12 @@ FSM_EVENTS ( clamp_move, /* Clamp movement success. */ clamp_move_success, + /* Clamp movement failure. */ + clamp_move_failure, /* Elevation and elevation motor success. */ clamp_elevation_rotation_success, + /* Elevation or elevation motor failure. */ + clamp_elevation_or_rotation_failure, /* Elevation motor success. */ clamp_elevation_success, /* Elevation motor failure. */ @@ -248,7 +256,7 @@ clamp_new_element (uint8_t pos, uint8_t element_type) void clamp_prepare (uint8_t prepare) { - logistic_global.prepare = 1; + logistic_global.prepare = prepare; FSM_HANDLE (AI, clamp_prepare); } @@ -409,6 +417,22 @@ clamp_route (void) ctx.pos_current = pos_new; } +static void +clamp_blocked (void) +{ + /* Free everything. */ + clamp_openclose (1); + clamp_door (CLAMP_SLOT_FRONT_BOTTOM, 1); + clamp_door (CLAMP_SLOT_FRONT_TOP, 1); + clamp_door (CLAMP_SLOT_BACK_BOTTOM, 1); + clamp_door (CLAMP_SLOT_BACK_TOP, 1); + mimot_motor0_free (); + mimot_motor1_free (); + logistic_dump (); + /* Signal problem. */ + fsm_queue_post_event (FSM_EVENT (AI, clamp_move_failure)); +} + /* CLAMP FSM */ FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING) @@ -488,6 +512,12 @@ FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success); } +FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_failure, CLAMP_BLOCKED) +{ + fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked)); + return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_failure); +} + FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) { ctx.working = 1; @@ -510,7 +540,8 @@ FSM_TRANS (CLAMP_IDLE, clamp_prepare, logistic_global.moving_to); return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_element); } - else if (logistic_global.clamp_pos_idle != ctx.pos_current) + else if (logistic_global.prepare != 3 + && logistic_global.clamp_pos_idle != ctx.pos_current) { if (logistic_path_clear (ctx.pos_current, logistic_global.clamp_pos_idle)) @@ -556,7 +587,8 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, logistic_global.moving_to); return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_element); } - else if (logistic_global.clamp_pos_idle != ctx.pos_current) + else if (logistic_global.prepare != 3 + && logistic_global.clamp_pos_idle != ctx.pos_current) { if (logistic_path_clear (ctx.pos_current, logistic_global.clamp_pos_idle)) @@ -593,7 +625,8 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, move_element); } - else if (logistic_global.clamp_pos_idle != ctx.pos_current) + else if (logistic_global.prepare != 3 + && logistic_global.clamp_pos_idle != ctx.pos_current) { if (logistic_path_clear (ctx.pos_current, logistic_global.clamp_pos_idle)) @@ -619,6 +652,12 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, } } +FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_failure, CLAMP_BLOCKED) +{ + fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked)); + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_failure); +} + FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME, CLAMP_DROPING_WAITING_ROBOT) { @@ -640,7 +679,8 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, move_element); } - else if (logistic_global.clamp_pos_idle != ctx.pos_current) + else if (logistic_global.prepare != 3 + && logistic_global.clamp_pos_idle != ctx.pos_current) { if (logistic_path_clear (ctx.pos_current, logistic_global.clamp_pos_idle)) @@ -684,6 +724,34 @@ FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING) return FSM_NEXT (CLAMP_LOCKED, clamp_drop); } +FSM_TRANS (CLAMP_BLOCKED, clamp_prepare, + move_to_idle, CLAMP_GOING_IDLE, + clamp_locked, CLAMP_LOCKED, + done, CLAMP_IDLE) +{ + if (logistic_global.prepare != 3) + { + if (logistic_path_clear (ctx.pos_current, + logistic_global.clamp_pos_idle)) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, move_to_idle); + } + else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, clamp_locked); + } + } + else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, done); + } +} + /* CLAMP_MOVE FSM */ FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, @@ -729,6 +797,13 @@ FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, } } +FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_or_rotation_failure, + CLAMP_MOVE_IDLE) +{ + clamp_blocked (); + return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_or_rotation_failure); +} + FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, done, CLAMP_MOVE_SRC_CLAMP_CLOSING, next, CLAMP_MOVE_SRC_ROUTING) @@ -747,6 +822,14 @@ FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, } } +FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_or_rotation_failure, + CLAMP_MOVE_IDLE) +{ + clamp_blocked (); + return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, + clamp_elevation_or_rotation_failure); +} + FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, BOT_PWM_CLAMP_CLOSE_TIME, open_door, CLAMP_MOVE_SRC_DOOR_OPENDING, move, CLAMP_MOVE_DST_ROUTING) @@ -804,6 +887,14 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, } } +FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_or_rotation_failure, + CLAMP_MOVE_IDLE) +{ + clamp_blocked (); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_or_rotation_failure); +} + FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, CLAMP_MOVE_DST_CLAMP_OPENING) { diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 4b0536f2..afa625bc 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -518,6 +518,14 @@ logistic_drop (uint8_t direction) logistic_decision (); } +void +logistic_dump (void) +{ + uint8_t i; + for (i = 0; i < CLAMP_SLOT_NB; i++) + ctx.slots[i] = 0; +} + static uint8_t logistic_slot_clear (uint8_t slot) { diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 9962874b..6acb681e 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -124,6 +124,10 @@ logistic_element_move_done (void); void logistic_drop (uint8_t direction); +/** Dump every element. */ +void +logistic_dump (void); + /** Is path clear between two positions? */ uint8_t logistic_path_clear (uint8_t slot1, uint8_t slot2); diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 6b2c8d31..1e12c427 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -144,6 +144,9 @@ main_event_to_fsm (void) if (mimot_motor0_status == success && mimot_motor1_status == success) FSM_HANDLE_E (AI, clamp_elevation_rotation_success); + if (mimot_motor0_status == failure + || mimot_motor1_status == failure) + FSM_HANDLE_E (AI, clamp_elevation_or_rotation_failure); if (mimot_motor0_status == success) FSM_HANDLE_E (AI, clamp_elevation_success); else if (mimot_motor0_status == failure) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index c9362203..8ff69ee6 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -56,6 +56,10 @@ FSM_STATES ( TOP_GOING_TO_ELEMENT, /* Waiting clamp has finished its work. */ TOP_WAITING_CLAMP, + /* Unblocking: waiting a little bit. */ + TOP_UNBLOCKING_SHAKE_WAIT, + /* Unblocking: shaking. */ + TOP_UNBLOCKING_SHAKE, /* Waiting construction is ready to drop. */ TOP_WAITING_READY, /* Dropping, opening the doors. */ @@ -72,6 +76,8 @@ struct top_t uint8_t target_element_id; /** Chaos counter. */ uint8_t chaos; + /** Broken clamp. */ + uint8_t broken; }; /** Global context. */ @@ -111,10 +117,13 @@ top_go_element (void) 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; + if (!ctx.broken) + { + 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; @@ -154,11 +163,12 @@ static uint8_t top_decision (void) { /* If we can make a tower. */ - if (logistic_global.construct_possible == 1) + if (logistic_global.construct_possible == 1 + || (ctx.broken && logistic_global.construct_possible == 2)) return top_go_drop (); if (logistic_global.need_prepare) { - clamp_prepare (2); + clamp_prepare (ctx.broken ? 3 : 2); return top_go_drop (); } else @@ -217,7 +227,7 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success, } else { - clamp_prepare (1); + clamp_prepare (ctx.broken ? 3 : 1); return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp); } } @@ -292,12 +302,53 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, } } +FSM_TRANS (TOP_WAITING_CLAMP, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) +{ + return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked); +} + +FSM_TRANS_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, 250, + try_again, TOP_UNBLOCKING_SHAKE, + tryout, TOP_WAITING_CLAMP) +{ + if (ctx.chaos < 4) + { + int16_t dist = logistic_global.collect_direction + == DIRECTION_FORWARD ? 100 : -100; + asserv_move_linearly (++ctx.chaos % 2 ? -dist : dist); + return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, try_again); + } + else + { + ctx.broken = 1; + clamp_prepare (3); + return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, tryout); + } +} + +FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_success, TOP_WAITING_CLAMP) +{ + clamp_prepare (0); + return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_success); +} + +FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_failure, + TOP_UNBLOCKING_SHAKE_WAIT) +{ + return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_failure); +} + 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_WAITING_READY, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) +{ + return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked); +} + FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) { element_down (ctx.target_element_id, ELEMENT_TOWER); -- cgit v1.2.3