From 23f4603cf160b07376f10b705a84d1cb823e2c46 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Tue, 19 May 2009 01:32:30 +0200 Subject: * digital/io/src: - fixes on cylinder fsm(init offset, grafcet fix) --- digital/io/src/cylinder.fsm | 20 +++++++++++++------- digital/io/src/cylinder.h | 3 +++ digital/io/src/cylinder_cb.c | 31 ++++++++++++++++++++++--------- digital/io/src/main.c | 12 ++++++++++++ 4 files changed, 50 insertions(+), 16 deletions(-) (limited to 'digital') diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm index 2fcf06c4..0d6f5e23 100644 --- a/digital/io/src/cylinder.fsm +++ b/digital/io/src/cylinder.fsm @@ -7,7 +7,9 @@ States: waiting for init WAIT_FOR_JACK_IN waiting for jack - INIT + RESET_POS + reset the cylinder position + INIT_POS initializing cylinder to a good position (open) WAIT_A_PUCK waiting a puck enter in cylinder @@ -17,8 +19,8 @@ States: waiting for the bridge is ready TURN_PLUS_1 turn cylinder to 1 position after - TURN_PLUS_1_LOOP - turn cylinder to 1 position after + TURN_PLUS_2 + turn cylinder to 2 position after TURN_MINUS_1 turn cylinder to 1 position before to prevent new puck enter in the robot WAIT_FOR_PUCKS_RELEASE @@ -46,10 +48,14 @@ IDLE: we wait the jack before moving anything WAIT_FOR_JACK_IN: - jack_inserted_into_bot -> INIT + jack_inserted_into_bot -> RESET_POS we init the cylinder position -INIT: +RESET_POS: + move_done -> INIT_POS + move the cylinder to open it + +INIT_POS: move_done -> WAIT_A_PUCK the cylinder is ready to get pucks @@ -64,10 +70,10 @@ TURN_PLUS_3: WAIT_FOR_BRIDGE_READY: bridge_ready: no_puck_bo -> TURN_PLUS_1 bridge clear, and no other puck on cylinder - bridge_ready: puck_bo -> TURN_PLUS_1_LOOP + bridge_ready: puck_bo -> TURN_PLUS_2 bridge clear, an another puck is in the cylinder -TURN_PLUS_1_LOOP: +TURN_PLUS_2: move_done: ok_for_other_puck -> WAIT_FOR_BRIDGE_READY we test bo again move_done: not_ok_for_other_puck -> WAIT_FOR_PUCKS_RELEASE diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h index f6d7ff58..d7162353 100644 --- a/digital/io/src/cylinder.h +++ b/digital/io/src/cylinder.h @@ -33,4 +33,7 @@ extern uint8_t of_state; /* nb puck in cylinder */ extern uint8_t nb_puck_cylinder; +/* mecanical offset */ +#define CYLINDER_OFFSET -0x43e + #endif /* cylinder_h */ diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c index fa207959..2e3159ea 100644 --- a/digital/io/src/cylinder_cb.c +++ b/digital/io/src/cylinder_cb.c @@ -43,7 +43,7 @@ cylinder__IDLE__start (void) /* * WAIT_FOR_JACK_IN =jack_inserted_into_bot=> - * => INIT + * => RESET_POS * we init the cylinder position */ fsm_branch_t @@ -54,14 +54,27 @@ cylinder__WAIT_FOR_JACK_IN__jack_inserted_into_bot (void) } /* - * INIT =move_done=> + * RESET_POS =move_done=> + * => INIT_POS + * move the cylinder to open it + */ +fsm_branch_t +cylinder__RESET_POS__move_done (void) +{ + asserv_move_arm(CYLINDER_OFFSET, + ASSERV_ARM_SPEED_DEFAULT); + return cylinder_next (RESET_POS, move_done); +} + +/* + * INIT_POS =move_done=> * => WAIT_A_PUCK * the cylinder is ready to get pucks */ fsm_branch_t -cylinder__INIT__move_done (void) +cylinder__INIT_POS__move_done (void) { - return cylinder_next (INIT, move_done); + return cylinder_next (INIT_POS, move_done); } /* @@ -93,7 +106,7 @@ cylinder__TURN_PLUS_3__move_done (void) * WAIT_FOR_BRIDGE_READY =bridge_ready=> * no_puck_bo => TURN_PLUS_1 * bridge clear, and no other puck on cylinder - * puck_bo => TURN_PLUS_1_LOOP + * puck_bo => TURN_PLUS_2 * bridge clear, an another puck is in the cylinder */ fsm_branch_t @@ -128,18 +141,18 @@ cylinder__TURN_PLUS_1__move_done (void) } /* - * TURN_PLUS_1_LOOP =move_done=> + * TURN_PLUS_2 =move_done=> * ok_for_other_puck => WAIT_FOR_BRIDGE_READY * we test bo again * not_ok_for_other_puck => WAIT_FOR_PUCKS_RELEASE * bot full, eject hypothetical puck and close the cylinder */ fsm_branch_t -cylinder__TURN_PLUS_1_LOOP__move_done (void) +cylinder__TURN_PLUS_2__move_done (void) { if(nb_puck_fb < 4) - return cylinder_next_branch (TURN_PLUS_1_LOOP, move_done, ok_for_other_puck); - return cylinder_next_branch (TURN_PLUS_1_LOOP, move_done, not_ok_for_other_puck); + return cylinder_next_branch (TURN_PLUS_2, move_done, ok_for_other_puck); + return cylinder_next_branch (TURN_PLUS_2, move_done, not_ok_for_other_puck); } /* diff --git a/digital/io/src/main.c b/digital/io/src/main.c index e4455844..9219c633 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -214,6 +214,18 @@ main_event_to_fsm (void) { /* TODO: */ } + + /* Check cylinder status */ + if (arm_status == success) + { + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_move_done); + } + else if (elevator_status == failure) + { + /* TODO: */ + } + /* FIXME: use general setting ack. */ /* send event if elevator received an order */ if(elvt_order) -- cgit v1.2.3