From 266966d7fd9e90a636948e931c8d7a6777114b91 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Wed, 20 May 2009 16:24:19 +0200 Subject: * digital/io/src: - new cylinder fsm - fsm variable prefixed by fsm name --- digital/io/src/cylinder.c | 14 ++- digital/io/src/cylinder.fsm | 95 +++++++++++++++++- digital/io/src/cylinder.h | 13 ++- digital/io/src/cylinder_cb.c | 203 +++++++++++++++++++++++++++++++++++++-- digital/io/src/elevator.c | 13 ++- digital/io/src/elevator.h | 4 +- digital/io/src/elevator_cb.c | 15 ++- digital/io/src/filterbridge.c | 2 +- digital/io/src/filterbridge.h | 2 +- digital/io/src/filterbridge_cb.c | 6 +- digital/io/src/main.c | 20 ++-- 11 files changed, 344 insertions(+), 43 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/cylinder.c b/digital/io/src/cylinder.c index f533e425..4dd0e590 100644 --- a/digital/io/src/cylinder.c +++ b/digital/io/src/cylinder.c @@ -25,7 +25,17 @@ #include "cylinder.h" /* last state of optical fence */ -uint8_t of_state = 0; +uint8_t cylinder_of_state = 0; /* nb puck in cylinder */ -uint8_t nb_puck_cylinder = 0; +uint8_t cylinder_nb_puck = 0; + +uint8_t cylinder_puck_on_of = 0; + +/* force cylinder to be closed */ +uint8_t cylinder_close_order = 0; + +/* flush puck on the cylinder */ +uint8_t cylinder_flush_order = 0; + +/* cylinder order */ diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm index 09b381c3..3f883058 100644 --- a/digital/io/src/cylinder.fsm +++ b/digital/io/src/cylinder.fsm @@ -13,8 +13,28 @@ States: initializing cylinder to a good position (open) WAIT_A_PUCK waiting a puck enter in cylinder - TURN_PLUS_4 - turn cylinder 4 position after + TURN_PLUS_1 + turn 1 position after when we see a new puck + TURN_MINUS_1 + turn 1 position before when the new puck dont really exist + WAIT_BRIDGE_READY + waiting fb is ready to accept new puck + TURN_PLUS_1_AGAIN + turn 1 position after to open cylinder after OF test + TURN_MINUS_1_CLOSE + turn 1 position before to close the cylinder (bot full/close_order) + WAIT_BOT_NOT_FULL + wait the bot to drop pucks + WAIT_CLEAR_ORDER + wait the clear order (unset close_order flags) + TURN_PLUS_1_OPEN + open cylinder after close (bot full/close_order) + TURN_PLUS_1_FLUSH + open cylinder to prepare cyliner flush + WAIT_BRIDGE_READY_FLUSH + we wait fb is ready during the flush + TURN_PLUS_2_FLUSH + drop 1 puck to the fb Events: start @@ -25,7 +45,18 @@ Events: there is a puck in position 1 move_done cylinder move finished + bridge_ready + the fb is ready to get new pucks + close_order + we receive the order to close the cylinder + no_close_order + we can reopen the cylinder + bot_not_full + the bot dont have 4 pucks + flush_order + we receive a flush order state_timeout + useless here IDLE: start -> WAIT_FOR_JACK_IN @@ -44,9 +75,63 @@ INIT_POS: the cylinder is ready to get pucks WAIT_A_PUCK: - new_puck -> TURN_PLUS_4 - turn cylinder 4 position after + new_puck -> TURN_PLUS_1 + turn cylinder 1 position after + close_order -> TURN_MINUS_1_CLOSE + we close cylinder as requested + flush_order -> WAIT_BRIDGE_READY_FLUSH + flush all pucks to the bridge -TURN_PLUS_4: +TURN_PLUS_1: + move_done: of_no_puck -> TURN_MINUS_1 + There is no puck, go backward + move_done: of_puck -> WAIT_BRIDGE_READY + There is a real puck, check if fb is ready to get puck + +TURN_MINUS_1: move_done -> WAIT_A_PUCK wait a puck again + +WAIT_BRIDGE_READY: + bridge_ready -> TURN_PLUS_1_AGAIN + open cylinder after the puck existence confirmation + +TURN_PLUS_1_AGAIN: + move_done: bot_not_full -> WAIT_A_PUCK + ready for a new puck + move_done: bot_full -> TURN_MINUS_1_CLOSE + bot full we close the cylinder + +TURN_MINUS_1_CLOSE: + move_done -> WAIT_BOT_NOT_FULL + we wait the bot is not full to reopen cylinder + +WAIT_BOT_NOT_FULL: + bot_not_full -> WAIT_CLEAR_ORDER + first condition to reopen bot is reached + flush_order -> TURN_PLUS_1_FLUSH + flush gordon + +WAIT_CLEAR_ORDER: + no_close_order -> TURN_PLUS_1_OPEN + last condition to reopen bot reached, opening... + flush_order -> TURN_PLUS_1_FLUSH + flush gordon + +TURN_PLUS_1_OPEN: + move_done -> WAIT_A_PUCK + we wait a puck + +TURN_PLUS_1_FLUSH: + move_done -> WAIT_BRIDGE_READY_FLUSH + cylinder open and ready to flush + +WAIT_BRIDGE_READY_FLUSH: + bridge_ready -> TURN_PLUS_2_FLUSH + put a puck on the fb + +TURN_PLUS_2_FLUSH: + move_done: cylinder_empty -> TURN_MINUS_1_CLOSE + flush finished + move_done: cylinder_not_empty -> WAIT_BRIDGE_READY_FLUSH + flush again diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h index b095f5a2..ad6d88b3 100644 --- a/digital/io/src/cylinder.h +++ b/digital/io/src/cylinder.h @@ -28,10 +28,19 @@ #include "common.h" /* last state of optical fence */ -extern uint8_t of_state; +extern uint8_t cylinder_of_state; /* nb puck in cylinder */ -extern uint8_t nb_puck_cylinder; +extern uint8_t cylinder_nb_puck; + +/* of state */ +extern uint8_t cylinder_puck_on_of; + +/* force cylinder to be closed */ +extern uint8_t cylinder_close_order; + +/* flush puck on the cylinder */ +extern uint8_t cylinder_flush_order; /* mecanical offset */ #define CYLINDER_OFFSET 0x43e diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c index 461230ee..e01b9875 100644 --- a/digital/io/src/cylinder_cb.c +++ b/digital/io/src/cylinder_cb.c @@ -28,6 +28,7 @@ #include "asserv.h" #include "cylinder.h" #include "filterbridge.h" +#include "elevator.h" /* @@ -79,28 +80,214 @@ cylinder__INIT_POS__move_done (void) /* * WAIT_A_PUCK =new_puck=> - * => TURN_PLUS_4 - * turn cylinder 4 position after + * => TURN_PLUS_1 + * turn cylinder 1 position after */ fsm_branch_t cylinder__WAIT_A_PUCK__new_puck (void) { - /* ++nb_puck_cylinder; */ - ++nb_puck_fb; - asserv_move_arm(4*60*ASSERV_ARM_STEP_BY_DEGREE, + asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, ASSERV_ARM_SPEED_DEFAULT); return cylinder_next (WAIT_A_PUCK, new_puck); } /* - * TURN_PLUS_4 =move_done=> + * WAIT_A_PUCK =close_order=> + * => TURN_MINUS_1_CLOSE + * we close cylinder as requested + */ +fsm_branch_t +cylinder__WAIT_A_PUCK__close_order (void) +{ + asserv_move_arm(-1*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + return cylinder_next (WAIT_A_PUCK, close_order); +} + +/* + * WAIT_A_PUCK =flush_order=> + * => WAIT_BRIDGE_READY_FLUSH + * flush all pucks to the bridge + */ +fsm_branch_t +cylinder__WAIT_A_PUCK__flush_order (void) +{ + return cylinder_next (WAIT_A_PUCK, flush_order); +} + +/* + * TURN_PLUS_1 =move_done=> + * of_no_puck => TURN_MINUS_1 + * There is no puck, go backward + * of_puck => WAIT_BRIDGE_READY + * There is a real puck, check if fb is ready to get puck + */ +fsm_branch_t +cylinder__TURN_PLUS_1__move_done (void) +{ + if(cylinder_puck_on_of) + { + ++cylinder_nb_puck; + return cylinder_next_branch (TURN_PLUS_1, move_done, of_puck); + } + return cylinder_next_branch (TURN_PLUS_1, move_done, of_no_puck); +} + +/* + * TURN_MINUS_1 =move_done=> * => WAIT_A_PUCK * wait a puck again */ fsm_branch_t -cylinder__TURN_PLUS_4__move_done (void) +cylinder__TURN_MINUS_1__move_done (void) +{ + return cylinder_next (TURN_MINUS_1, move_done); +} + +/* + * WAIT_BRIDGE_READY =bridge_ready=> + * => TURN_PLUS_1_AGAIN + * open cylinder after the puck existence confirmation + */ +fsm_branch_t +cylinder__WAIT_BRIDGE_READY__bridge_ready (void) +{ + asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + return cylinder_next (WAIT_BRIDGE_READY, bridge_ready); +} + +/* + * TURN_PLUS_1_AGAIN =move_done=> + * bot_not_full => WAIT_A_PUCK + * ready for a new puck + * bot_full => TURN_MINUS_1_CLOSE + * bot full we close the cylinder + */ +fsm_branch_t +cylinder__TURN_PLUS_1_AGAIN__move_done (void) +{ + if(cylinder_nb_puck == 2) + { + --cylinder_nb_puck; + ++fb_nb_puck; + } + if(fb_nb_puck+ cylinder_nb_puck + elvt_nb_puck < 4) + return cylinder_next_branch (TURN_PLUS_1_AGAIN, move_done, bot_not_full); + asserv_move_arm(-1*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + return cylinder_next_branch (TURN_PLUS_1_AGAIN, move_done, bot_full); +} + +/* + * TURN_MINUS_1_CLOSE =move_done=> + * => WAIT_BOT_NOT_FULL + * we wait the bot is not full to reopen cylinder + */ +fsm_branch_t +cylinder__TURN_MINUS_1_CLOSE__move_done (void) +{ + return cylinder_next (TURN_MINUS_1_CLOSE, move_done); +} + +/* + * WAIT_BOT_NOT_FULL =bot_not_full=> + * => WAIT_CLEAR_ORDER + * first condition to reopen bot is reached + */ +fsm_branch_t +cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void) +{ + return cylinder_next (WAIT_BOT_NOT_FULL, bot_not_full); +} + +/* + * WAIT_BOT_NOT_FULL =flush_order=> + * => TURN_PLUS_1_FLUSH + * flush gordon + */ +fsm_branch_t +cylinder__WAIT_BOT_NOT_FULL__flush_order (void) +{ + asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + return cylinder_next (WAIT_BOT_NOT_FULL, flush_order); +} + +/* + * WAIT_CLEAR_ORDER =no_close_order=> + * => TURN_PLUS_1_OPEN + * last condition to reopen bot reached, opening... + */ +fsm_branch_t +cylinder__WAIT_CLEAR_ORDER__no_close_order (void) +{ + asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + return cylinder_next (WAIT_CLEAR_ORDER, no_close_order); +} + +/* + * WAIT_CLEAR_ORDER =flush_order=> + * => TURN_PLUS_1_FLUSH + * flush gordon + */ +fsm_branch_t +cylinder__WAIT_CLEAR_ORDER__flush_order (void) +{ + asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + return cylinder_next (WAIT_CLEAR_ORDER, flush_order); +} + +/* + * TURN_PLUS_1_OPEN =move_done=> + * => WAIT_A_PUCK + * we wait a puck + */ +fsm_branch_t +cylinder__TURN_PLUS_1_OPEN__move_done (void) +{ + return cylinder_next (TURN_PLUS_1_OPEN, move_done); +} + +/* + * TURN_PLUS_1_FLUSH =move_done=> + * => WAIT_BRIDGE_READY_FLUSH + * cylinder open and ready to flush + */ +fsm_branch_t +cylinder__TURN_PLUS_1_FLUSH__move_done (void) +{ + return cylinder_next (TURN_PLUS_1_FLUSH, move_done); +} + +/* + * WAIT_BRIDGE_READY_FLUSH =bridge_ready=> + * => TURN_PLUS_2_FLUSH + * put a puck on the fb + */ +fsm_branch_t +cylinder__WAIT_BRIDGE_READY_FLUSH__bridge_ready (void) +{ + return cylinder_next (WAIT_BRIDGE_READY_FLUSH, bridge_ready); +} + +/* + * TURN_PLUS_2_FLUSH =move_done=> + * cylinder_empty => TURN_MINUS_1_CLOSE + * flush finished + * cylinder_not_empty => WAIT_BRIDGE_READY_FLUSH + * flush again + */ +fsm_branch_t +cylinder__TURN_PLUS_2_FLUSH__move_done (void) { - return cylinder_next (TURN_PLUS_4, move_done); + --cylinder_nb_puck; + ++fb_nb_puck; + if(!cylinder_nb_puck) + return cylinder_next_branch (TURN_PLUS_2_FLUSH, move_done, cylinder_empty); + return cylinder_next_branch (TURN_PLUS_2_FLUSH, move_done, cylinder_not_empty); } diff --git a/digital/io/src/elevator.c b/digital/io/src/elevator.c index d89ff7a1..6319501f 100644 --- a/digital/io/src/elevator.c +++ b/digital/io/src/elevator.c @@ -28,17 +28,22 @@ /** * State of the elevator */ -uint8_t elevator_is_ready = 0; -uint8_t nb_puck_in_elvt = 0; +uint8_t elvt_is_ready = 0; + /** - * elevator orders + * nb puck in the elevator */ -uint8_t elvt_order; +uint8_t elvt_nb_puck = 0; + /** * new_puck information (from filterbridge) */ uint8_t elvt_new_puck = 0; +/** + * elevator orders + */ +uint8_t elvt_order = 0; /* +AutoDec */ /* -AutoDec */ diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h index 49cbd184..6d131861 100644 --- a/digital/io/src/elevator.h +++ b/digital/io/src/elevator.h @@ -28,12 +28,12 @@ /** * State of the elevator */ -extern uint8_t elevator_is_ready; +extern uint8_t elvt_is_ready; /** * nb puck in the elevator */ -extern uint8_t nb_puck_in_elvt; +extern uint8_t elvt_nb_puck; /** * new_puck information (from filterbridge) diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index fe73a717..f6e538a4 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -94,7 +94,7 @@ fsm_branch_t elevator__INIT__doors_opened (void) { /* FIXME: why this is here? */ - elevator_is_ready = 0; + elvt_is_ready = 0; /* Close the door. */ pwm_set (CLOSE_DOOR_PWM, TIME_DOORS_PWM); return elevator_next (INIT, doors_opened); @@ -109,9 +109,9 @@ fsm_branch_t elevator__GO_TO_POS_X__in_position (void) { /* FIXME: this sucks, look at elevator.fsm for a real fix. */ - elevator_is_ready = 1; + elvt_is_ready = 1; /* move to first position. */ - asserv_move_elevator_absolute(posx[nb_puck_in_elvt], + asserv_move_elevator_absolute(posx[nb_puck_elvt], ASSERV_ELVT_SPEED_DEFAULT); return elevator_next (GO_TO_POS_X, in_position); } @@ -128,7 +128,7 @@ elevator__GO_TO_POS_X__in_position (void) fsm_branch_t elevator__WAIT_A_PUCK__new_puck (void) { - elevator_is_ready = 0; + elvt_is_ready = 0; elvt_new_puck = 0; // TODO time_ok if(++nb_puck_elvt < 4) @@ -137,7 +137,7 @@ elevator__WAIT_A_PUCK__new_puck (void) ((chrono_remaining_time() - OK_TIME_LIMIT > 0) || nb_puck_fb != 0)) */ - asserv_move_elevator_absolute(posx[nb_puck_in_elvt], + asserv_move_elevator_absolute(posx[nb_puck_elvt], ASSERV_ELVT_SPEED_DEFAULT); return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks); } @@ -154,7 +154,7 @@ elevator__WAIT_A_PUCK__new_puck (void) fsm_branch_t elevator__WAIT_A_PUCK__time_up (void) { - elevator_is_ready = 0; + elvt_is_ready = 0; return elevator_next (WAIT_A_PUCK, time_up); } @@ -230,7 +230,6 @@ fsm_branch_t elevator__OPEN_DOORS__doors_opened (void) { nb_puck_elvt = 0; - nb_puck_in_elvt = 0; return elevator_next (OPEN_DOORS, doors_opened); } @@ -266,7 +265,7 @@ elevator__WAIT_FOR_CLOSE_ORDER__order_received (void) fsm_branch_t elevator__CLOSE_DOORS__door_move_finished (void) { - asserv_move_elevator_absolute(posx[nb_puck_in_elvt], + asserv_move_elevator_absolute(posx[nb_puck_elvt], ASSERV_ELVT_SPEED_DEFAULT); return elevator_next (CLOSE_DOORS, door_move_finished); } diff --git a/digital/io/src/filterbridge.c b/digital/io/src/filterbridge.c index 0b5001cc..c318c124 100644 --- a/digital/io/src/filterbridge.c +++ b/digital/io/src/filterbridge.c @@ -24,7 +24,7 @@ * }}} */ #include "filterbridge.h" -uint8_t nb_puck_fb; +uint8_t fb_nb_puck; /* +AutoDec */ /* -AutoDec */ diff --git a/digital/io/src/filterbridge.h b/digital/io/src/filterbridge.h index 81fdcc60..235d29ee 100644 --- a/digital/io/src/filterbridge.h +++ b/digital/io/src/filterbridge.h @@ -27,6 +27,6 @@ #include "common.h" -extern uint8_t nb_puck_fb; +extern uint8_t fb_nb_puck; #endif // filterbridge_h diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c index 3f571f66..9393a702 100644 --- a/digital/io/src/filterbridge_cb.c +++ b/digital/io/src/filterbridge_cb.c @@ -75,7 +75,7 @@ filterbridge__CLOSE_DOOR__state_timeout (void) fsm_branch_t filterbridge__WAIT_A_PUCK__puck_on_pos2 (void) { - if(elevator_is_ready) + if(elvt_is_ready) { return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, lift_ready); } @@ -127,8 +127,8 @@ filterbridge__PUSH_PUCK__no_puck_on_pos2 (void) fsm_branch_t filterbridge__TEMPO_ELVETATOR_COMMUNICATION__state_timeout (void) { - ++nb_puck_in_elvt; - --nb_puck_fb; + ++elvt_nb_puck; + --fb_nb_puck; elvt_new_puck = 1; servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE); servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE); diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 7d38d1dd..c84c7ca6 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -240,17 +240,23 @@ main_event_to_fsm (void) FSM_HANDLE_EVENT (&elevator_fsm, ELEVATOR_EVENT_doors_opened); /* bridge ready */ - /* - if(nb_puck_fb < 2) + if(fb_nb_puck < 2) FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_bridge_ready); - */ /* bot empty */ - /* - if(!nb_puck_fb && !nb_puck_in_elvt && !nb_puck_cylinder) + if(fb_nb_puck + elvt_nb_puck + cylinder_nb_puck < 4) FSM_HANDLE_EVENT (&cylinder_fsm, - CYLINDER_EVENT_bot_empty); - */ + CYLINDER_EVENT_bot_not_full); + + /* cylinder_close_order */ + if(cylinder_close_order) + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_close_order); + + /* cylinder__flush_order */ + if(cylinder_flush_order) + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_flush_order); /* Jack */ if(switch_get_jack()) -- cgit v1.2.3