From 0b188fa1a29f2b7f73a88e87c82724132ea53f1b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 16:15:43 +0200 Subject: digital/io-hub: add clamp waiting --- digital/io-hub/src/robospierre/clamp.c | 40 ++++++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/clamp.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 17 ++++++++++++++- 3 files changed, 60 insertions(+), 1 deletion(-) (limited to 'digital/io-hub/src') diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index b2a6bf76..0a7654f3 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -95,6 +95,10 @@ FSM_STATES ( FSM_EVENTS ( /* New element inside bottom slot. */ clamp_new_element, + /* Sent when clamp is working. */ + clamp_working, + /* Sent when clamp return to idle state. */ + clamp_done, /* Order to drop elements. */ clamp_drop, /* Sent once drop is done, but robot should advance to completely @@ -123,6 +127,8 @@ FSM_START_WITH (CLAMP_MOVE_IDLE) /** Clamp context. */ struct clamp_t { + /** True if clamp is working. */ + uint8_t working; /** Current position. */ uint8_t pos_current; /** Requested position. */ @@ -196,6 +202,12 @@ clamp_init (void) ctx.open = 1; } +uint8_t +clamp_working (void) +{ + return ctx.working; +} + void clamp_move (uint8_t pos) { @@ -428,11 +440,15 @@ FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success); } FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) { + ctx.working = 1; + fsm_queue_post_event (FSM_EVENT (AI, clamp_working)); pwm_set_timed (clamp_slot_door[ctx.pos_new], BOT_PWM_DOOR_CLOSE (ctx.pos_new)); return FSM_NEXT (CLAMP_IDLE, clamp_new_element); @@ -470,10 +486,18 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle); } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, clamp_locked); + } } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, done); + } } FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, @@ -500,12 +524,20 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, move_to_idle); } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, clamp_locked); + } } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, done); + } } FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME, @@ -539,12 +571,20 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, move_to_idle); } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, clamp_locked); + } } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, done); + } } FSM_TRANS (CLAMP_LOCKED, clamp_new_element, CLAMP_LOCKED) diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 09233cce..60da1615 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -64,6 +64,10 @@ enum { void clamp_init (void); +/** Is clamp working? */ +uint8_t +clamp_working (void); + /** Move clamp to given position. */ void clamp_move (uint8_t pos); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index a3ad4c72..92e42b1c 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -49,7 +49,9 @@ FSM_STATES ( TOP_GOING_OUT2, TOP_GOING_TO_DROP, - TOP_GOING_TO_ELEMENT) + TOP_GOING_TO_ELEMENT, + /* Waiting clamp has finished its work. */ + TOP_WAITING_CLAMP) FSM_START_WITH (TOP_START) @@ -137,10 +139,13 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success, } FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, + clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { element_taken (ctx.target_element_id, ELEMENT_PAWN); + if (clamp_working ()) + return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop); @@ -148,3 +153,13 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, } } +FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, + drop, TOP_GOING_TO_DROP, + element, TOP_GOING_TO_ELEMENT) +{ + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, drop); + case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element); + } +} -- cgit v1.2.3