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_cb.c | 104 ++++--------------------------------------- 1 file changed, 9 insertions(+), 95 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 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); } -- cgit v1.2.3