From 69d657a149edba0ff28085a85feafa0dad8067a7 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Tue, 19 May 2009 01:32:38 +0200 Subject: * digital/io/src: - WIP --- digital/io/src/cylinder.fsm | 57 +++--------------------- digital/io/src/cylinder.h | 2 +- digital/io/src/cylinder_cb.c | 104 ++++--------------------------------------- digital/io/src/elevator_cb.c | 8 +++- digital/io/src/main.c | 6 ++- 5 files changed, 28 insertions(+), 149 deletions(-) diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm index 0d6f5e23..09b381c3 100644 --- a/digital/io/src/cylinder.fsm +++ b/digital/io/src/cylinder.fsm @@ -13,20 +13,8 @@ States: initializing cylinder to a good position (open) WAIT_A_PUCK waiting a puck enter in cylinder - TURN_PLUS_3 - turn cylinder to 3 positions after - WAIT_FOR_BRIDGE_READY - waiting for the bridge is ready - TURN_PLUS_1 - 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 - waiting for pucks release from the lift - TURN_PLUS_1_AGAIN - open the puck entrance + TURN_PLUS_4 + turn cylinder 4 position after Events: start @@ -37,10 +25,6 @@ Events: there is a puck in position 1 move_done cylinder move finished - bridge_ready - bridge ready to get a new puck - bot_empty - all pucks released state_timeout IDLE: @@ -60,38 +44,9 @@ INIT_POS: the cylinder is ready to get pucks WAIT_A_PUCK: - new_puck -> TURN_PLUS_3 - turn cylinder 3 position after + new_puck -> TURN_PLUS_4 + turn cylinder 4 position after -TURN_PLUS_3: - move_done -> WAIT_FOR_BRIDGE_READY - bridge full, waiting for Bison Fute's clearance - -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_2 - bridge clear, an another puck is in the cylinder - -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 - bot full, eject hypothetical puck and close the cylinder - -TURN_PLUS_1: - move_done: ok_for_other_puck -> WAIT_A_PUCK - cylinder empty and ready for new puck - move_done: not_ok_for_other_puck -> TURN_MINUS_1 - bot full, closing cylinder - -TURN_MINUS_1: - move_done -> WAIT_FOR_PUCKS_RELEASE - cylinder close, you shall not pass (but try again later) - -WAIT_FOR_PUCKS_RELEASE: - bot_empty -> TURN_PLUS_1_AGAIN - -TURN_PLUS_1_AGAIN: +TURN_PLUS_4: move_done -> WAIT_A_PUCK - cylinder ready + wait a puck again diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h index d7162353..b095f5a2 100644 --- a/digital/io/src/cylinder.h +++ b/digital/io/src/cylinder.h @@ -34,6 +34,6 @@ extern uint8_t of_state; extern uint8_t nb_puck_cylinder; /* mecanical offset */ -#define CYLINDER_OFFSET -0x43e +#define CYLINDER_OFFSET 0x43e #endif /* cylinder_h */ diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c index 2e3159ea..461230ee 100644 --- a/digital/io/src/cylinder_cb.c +++ b/digital/io/src/cylinder_cb.c @@ -79,114 +79,28 @@ cylinder__INIT_POS__move_done (void) /* * WAIT_A_PUCK =new_puck=> - * => TURN_PLUS_3 - * turn cylinder 3 position after + * => TURN_PLUS_4 + * turn cylinder 4 position after */ fsm_branch_t cylinder__WAIT_A_PUCK__new_puck (void) { - ++nb_puck_cylinder; - asserv_move_arm(3*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - return cylinder_next (WAIT_A_PUCK, new_puck); -} - -/* - * TURN_PLUS_3 =move_done=> - * => WAIT_FOR_BRIDGE_READY - * bridge full, waiting for Bison Fute's clearance - */ -fsm_branch_t -cylinder__TURN_PLUS_3__move_done (void) -{ - return cylinder_next (TURN_PLUS_3, move_done); -} - -/* - * WAIT_FOR_BRIDGE_READY =bridge_ready=> - * no_puck_bo => TURN_PLUS_1 - * bridge clear, and no other puck on cylinder - * puck_bo => TURN_PLUS_2 - * bridge clear, an another puck is in the cylinder - */ -fsm_branch_t -cylinder__WAIT_FOR_BRIDGE_READY__bridge_ready (void) -{ + /* ++nb_puck_cylinder; */ ++nb_puck_fb; - asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - if(!of_state) - { - --nb_puck_cylinder; - return cylinder_next_branch (WAIT_FOR_BRIDGE_READY, bridge_ready, no_puck_bo); - } - return cylinder_next_branch (WAIT_FOR_BRIDGE_READY, bridge_ready, puck_bo); -} - -/* - * TURN_PLUS_1 =move_done=> - * ok_for_other_puck => WAIT_A_PUCK - * cylinder empty and ready for new puck - * not_ok_for_other_puck => TURN_MINUS_1 - * bot full, closing cylinder - */ -fsm_branch_t -cylinder__TURN_PLUS_1__move_done (void) -{ - if(nb_puck_fb < 4) - return cylinder_next_branch (TURN_PLUS_1, move_done, ok_for_other_puck); - asserv_move_arm(-1*60*ASSERV_ARM_STEP_BY_DEGREE, + asserv_move_arm(4*60*ASSERV_ARM_STEP_BY_DEGREE, ASSERV_ARM_SPEED_DEFAULT); - return cylinder_next_branch (TURN_PLUS_1, move_done, not_ok_for_other_puck); -} - -/* - * 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_2__move_done (void) -{ - if(nb_puck_fb < 4) - 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); -} - -/* - * TURN_MINUS_1 =move_done=> - * => WAIT_FOR_PUCKS_RELEASE - * cylinder close, you shall not pass (but try again later) - */ -fsm_branch_t -cylinder__TURN_MINUS_1__move_done (void) -{ - return cylinder_next (TURN_MINUS_1, move_done); -} - -/* - * WAIT_FOR_PUCKS_RELEASE =bot_empty=> - * => TURN_PLUS_1_AGAIN - */ -fsm_branch_t -cylinder__WAIT_FOR_PUCKS_RELEASE__bot_empty (void) -{ - asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); - return cylinder_next (WAIT_FOR_PUCKS_RELEASE, bot_empty); + return cylinder_next (WAIT_A_PUCK, new_puck); } /* - * TURN_PLUS_1_AGAIN =move_done=> + * TURN_PLUS_4 =move_done=> * => WAIT_A_PUCK - * cylinder ready + * wait a puck again */ fsm_branch_t -cylinder__TURN_PLUS_1_AGAIN__move_done (void) +cylinder__TURN_PLUS_4__move_done (void) { - return cylinder_next (TURN_PLUS_1_AGAIN, move_done); + return cylinder_next (TURN_PLUS_4, move_done); } diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index 5ef5be28..0a3ea967 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -113,10 +113,16 @@ elevator__WAIT_A_PUCK__new_puck (void) elevator_is_ready = 0; elvt_new_puck = 0; // TODO time_ok - if(nb_puck_elvt < 4 && + if(nb_puck_elvt < 4) + { +/* && ((chrono_remaining_time() - OK_TIME_LIMIT > 0) || nb_puck_fb != 0)) + */ + asserv_move_elevator_absolute(posx[nb_puck_in_elvt], + ASSERV_ELVT_SPEED_DEFAULT); return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks); + } else return elevator_next_branch (WAIT_A_PUCK, new_puck, not_ok_for_other_pucks); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 2e2c4f60..980448c4 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -213,7 +213,7 @@ main_event_to_fsm (void) } /* Check cylinder status */ - if (arm_status == success) + if (arm_status != none) { FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_move_done); @@ -240,13 +240,17 @@ main_event_to_fsm (void) FSM_HANDLE_EVENT (&elevator_fsm, ELEVATOR_EVENT_doors_opened); /* bridge ready */ + /* if(nb_puck_fb < 2) FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_bridge_ready); + */ /* bot empty */ + /* if(!nb_puck_fb && !nb_puck_in_elvt && !nb_puck_cylinder) FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_bot_empty); + */ /* Jack */ if(switch_get_jack()) -- cgit v1.2.3