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_cb.c | 203 +++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 195 insertions(+), 8 deletions(-) (limited to 'digital/io/src/cylinder_cb.c') 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); } -- cgit v1.2.3