From 1ccdcfb7c74143586c451b6777a9bc27ad0023d9 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Wed, 20 May 2009 23:19:28 +0200 Subject: * digital/io/src: - new cylinder FSM again again - setting top_puck_inside_bot on elevator FSM --- digital/io/src/asserv.c | 9 ++ digital/io/src/asserv.h | 9 ++ digital/io/src/cylinder.fsm | 82 ++++++++----------- digital/io/src/cylinder.h | 2 + digital/io/src/cylinder_cb.c | 185 ++++++++++++++++++++++++------------------ digital/io/src/elevator_cb.c | 6 +- digital/io/src/filterbridge.c | 2 +- 7 files changed, 166 insertions(+), 129 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index d8fc2ea0..913ba412 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -340,6 +340,15 @@ asserv_arm_cmd_status (void) return none; } +/* Is the optical fence see a puck? */ +uint8_t +asserv_arm_of_status(void) +{ + if(asserv_status.input_port & _BV (5)) + return 1; + return 0; +} + /* Is last elevator class command has successfully ended? */ asserv_status_e asserv_elevator_cmd_status (void) diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index 06be268b..09fb88f4 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -123,6 +123,15 @@ asserv_move_cmd_status (void); asserv_status_e asserv_arm_cmd_status (void); +/** + * + * Is the optical fence see a puck? + * This function get the arm optical fence status + * @return the status of the arm optical fence + */ +uint8_t +asserv_arm_of_status(void); + /** * Is last elevator class command has successfully ended? * This function is used to know the status of the last elevator command. It diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm index 3f883058..aefe1170 100644 --- a/digital/io/src/cylinder.fsm +++ b/digital/io/src/cylinder.fsm @@ -13,16 +13,12 @@ States: initializing cylinder to a good position (open) WAIT_A_PUCK waiting a puck enter in cylinder - 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) + TURN_PLUS_1_AND_OFO + turn 1+of_offset position after when we see a new puck + TURN_PLUS_1_AND_MINUS_OFO + turn 1-of_offset position after to be ready to get a new puck WAIT_BOT_NOT_FULL wait the bot to drop pucks WAIT_CLEAR_ORDER @@ -33,8 +29,10 @@ States: open cylinder to prepare cyliner flush WAIT_BRIDGE_READY_FLUSH we wait fb is ready during the flush - TURN_PLUS_2_FLUSH + TURN_PLUS_3_FLUSH drop 1 puck to the fb + TURN_PLUS_1_CLOSE + close the cylinder Events: start @@ -75,63 +73,55 @@ INIT_POS: the cylinder is ready to get pucks WAIT_A_PUCK: - new_puck -> TURN_PLUS_1 - turn cylinder 1 position after - close_order -> TURN_MINUS_1_CLOSE + new_puck -> WAIT_BRIDGE_READY + look if the bridge is ready before move + close_order -> TURN_PLUS_1_CLOSE we close cylinder as requested flush_order -> WAIT_BRIDGE_READY_FLUSH flush all pucks to the bridge -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 + bridge_ready -> TURN_PLUS_1_AND_OFO + open the cylinder with the puck or not. -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_PLUS_1_AND_OFO: + move_done: bot_not_full -> TURN_PLUS_1_AND_MINUS_OFO + open the cylinder to wait a new puck + move_done: bot_full -> WAIT_BOT_NOT_FULL + bot full, waiting for pucks teleportation -TURN_MINUS_1_CLOSE: - move_done -> WAIT_BOT_NOT_FULL - we wait the bot is not full to reopen cylinder +TURN_PLUS_1_AND_MINUS_OFO: + move_done -> WAIT_A_PUCK + ready for other pucks WAIT_BOT_NOT_FULL: bot_not_full -> WAIT_CLEAR_ORDER - first condition to reopen bot is reached + the bot is not full, we go testing the other close condition flush_order -> TURN_PLUS_1_FLUSH - flush gordon + flush order received, go open the cylinder WAIT_CLEAR_ORDER: no_close_order -> TURN_PLUS_1_OPEN - last condition to reopen bot reached, opening... + no close order, we reopen cylinder to get other pucks flush_order -> TURN_PLUS_1_FLUSH - flush gordon + flush order received, go open the cylinder TURN_PLUS_1_OPEN: move_done -> WAIT_A_PUCK - we wait a puck + cylinder ready to get other pucks TURN_PLUS_1_FLUSH: move_done -> WAIT_BRIDGE_READY_FLUSH - cylinder open and ready to flush + we wait the bridge before moving 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 + bridge_ready -> TURN_PLUS_3_FLUSH + bridge is ready, flush gordon + +TURN_PLUS_3_FLUSH: + move_done -> WAIT_BOT_NOT_FULL + cylinder flushed, we test the 2 close conditions before reopen cylinder + +TURN_PLUS_1_CLOSE: + move_done -> WAIT_BOT_NOT_FULL + close order executed, test the 2 close conditions before reopen cylinder diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h index ad6d88b3..817b16ab 100644 --- a/digital/io/src/cylinder.h +++ b/digital/io/src/cylinder.h @@ -45,4 +45,6 @@ extern uint8_t cylinder_flush_order; /* mecanical offset */ #define CYLINDER_OFFSET 0x43e +/* mecanical offset for of probe */ +#define CYLINDER_OF_OFFSET 0.7 #endif /* cylinder_h */ diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c index e01b9875..1b4cc28b 100644 --- a/digital/io/src/cylinder_cb.c +++ b/digital/io/src/cylinder_cb.c @@ -29,7 +29,13 @@ #include "cylinder.h" #include "filterbridge.h" #include "elevator.h" +#include "top.h" +/* locales variables */ +/* is there a puck on pos 2 or 3 */ +uint8_t puck_on_cylinder = 0; +/* is the cylinder are in of_offset mode */ +uint8_t of_offset_enabled = 0; /* * IDLE =start=> @@ -80,20 +86,18 @@ cylinder__INIT_POS__move_done (void) /* * WAIT_A_PUCK =new_puck=> - * => TURN_PLUS_1 - * turn cylinder 1 position after + * => WAIT_BRIDGE_READY + * look if the bridge is ready before move */ fsm_branch_t cylinder__WAIT_A_PUCK__new_puck (void) { - asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); return cylinder_next (WAIT_A_PUCK, new_puck); } /* * WAIT_A_PUCK =close_order=> - * => TURN_MINUS_1_CLOSE + * => TURN_PLUS_1_CLOSE * we close cylinder as requested */ fsm_branch_t @@ -115,85 +119,68 @@ 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_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 + * => TURN_PLUS_1_AND_OFO + * open the cylinder with the puck or not. */ fsm_branch_t cylinder__WAIT_BRIDGE_READY__bridge_ready (void) { - asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, + of_offset_enabled = 1; + asserv_move_arm((1+CYLINDER_OF_OFFSET)*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 + * TURN_PLUS_1_AND_OFO =move_done=> + * bot_not_full => TURN_PLUS_1_AND_MINUS_OFO + * open the cylinder to wait a new puck + * bot_full => WAIT_BOT_NOT_FULL + * bot full, waiting for pucks teleportation */ fsm_branch_t -cylinder__TURN_PLUS_1_AGAIN__move_done (void) +cylinder__TURN_PLUS_1_AND_OFO__move_done (void) { - if(cylinder_nb_puck == 2) + /* we verify if we drop a puck to the bridge */ + if(puck_on_cylinder) { --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); + /* We probe the OF to see if we have a new puck */ + puck_on_cylinder = asserv_arm_of_status(); + if(puck_on_cylinder) + { + ++top_total_puck_taken; + ++top_puck_inside_bot; + } + if(top_puck_inside_bot < 4) + { + of_offset_enabled = 0; + asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + return cylinder_next_branch (TURN_PLUS_1_AND_OFO, move_done, bot_not_full); + } + return cylinder_next_branch (TURN_PLUS_1_AND_OFO, move_done, bot_full); } /* - * TURN_MINUS_1_CLOSE =move_done=> - * => WAIT_BOT_NOT_FULL - * we wait the bot is not full to reopen cylinder + * TURN_PLUS_1_AND_MINUS_OFO =move_done=> + * => WAIT_A_PUCK + * ready for other pucks */ fsm_branch_t -cylinder__TURN_MINUS_1_CLOSE__move_done (void) +cylinder__TURN_PLUS_1_AND_MINUS_OFO__move_done (void) { - return cylinder_next (TURN_MINUS_1_CLOSE, move_done); + return cylinder_next (TURN_PLUS_1_AND_MINUS_OFO, move_done); } /* * WAIT_BOT_NOT_FULL =bot_not_full=> * => WAIT_CLEAR_ORDER - * first condition to reopen bot is reached + * the bot is not full, we go testing the other close condition */ fsm_branch_t cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void) @@ -204,46 +191,67 @@ cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void) /* * WAIT_BOT_NOT_FULL =flush_order=> * => TURN_PLUS_1_FLUSH - * flush gordon + * flush order received, go open the cylinder */ 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); + if(of_offset_enabled) + { + asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + of_offset_enabled = 0; + } + else + 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... + * no close order, we reopen cylinder to get other pucks */ 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); + if(of_offset_enabled) + { + asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + of_offset_enabled = 0; + } + else + 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 + * flush order received, go open the cylinder */ fsm_branch_t cylinder__WAIT_CLEAR_ORDER__flush_order (void) { - asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, - ASSERV_ARM_SPEED_DEFAULT); + if(of_offset_enabled) + { + asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + of_offset_enabled = 0; + } + else + 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 + * cylinder ready to get other pucks */ fsm_branch_t cylinder__TURN_PLUS_1_OPEN__move_done (void) @@ -254,7 +262,7 @@ cylinder__TURN_PLUS_1_OPEN__move_done (void) /* * TURN_PLUS_1_FLUSH =move_done=> * => WAIT_BRIDGE_READY_FLUSH - * cylinder open and ready to flush + * we wait the bridge before moving */ fsm_branch_t cylinder__TURN_PLUS_1_FLUSH__move_done (void) @@ -264,30 +272,47 @@ cylinder__TURN_PLUS_1_FLUSH__move_done (void) /* * WAIT_BRIDGE_READY_FLUSH =bridge_ready=> - * => TURN_PLUS_2_FLUSH - * put a puck on the fb + * => TURN_PLUS_3_FLUSH + * bridge is ready, flush gordon */ fsm_branch_t cylinder__WAIT_BRIDGE_READY_FLUSH__bridge_ready (void) { + asserv_move_arm(3*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); 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 + * TURN_PLUS_3_FLUSH =move_done=> + * => WAIT_BOT_NOT_FULL + * cylinder flushed, we test the 2 close conditions before reopen cylinder */ fsm_branch_t -cylinder__TURN_PLUS_2_FLUSH__move_done (void) +cylinder__TURN_PLUS_3_FLUSH__move_done (void) { - --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); + if(puck_on_cylinder) + { + --cylinder_nb_puck; + ++fb_nb_puck; + puck_on_cylinder = 0; + } + return cylinder_next (TURN_PLUS_3_FLUSH, move_done); } - +/* + * TURN_PLUS_1_CLOSE =move_done=> + * => WAIT_BOT_NOT_FULL + * close order executed, test the 2 close conditions before reopen cylinder + */ +fsm_branch_t +cylinder__TURN_PLUS_1_CLOSE__move_done (void) +{ + if(puck_on_cylinder) + { + --cylinder_nb_puck; + ++fb_nb_puck; + puck_on_cylinder = 0; + } + return cylinder_next (TURN_PLUS_1_CLOSE, move_done); +} diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index f6e538a4..0c32f922 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -30,6 +30,7 @@ #include "pwm.h" #include "chrono.h" #include "filterbridge.h" +#include "top.h" /* Positions when waiting a puck*/ uint16_t posx[4] = @@ -166,7 +167,7 @@ elevator__WAIT_A_PUCK__time_up (void) fsm_branch_t elevator__WAIT_POS_ORDER__order_received (void) { - asserv_move_elevator_absolute(posy[elvt_order] + MAJ_POSY, + asserv_move_elevator_absolute(posy[elvt_order + 1] + MAJ_POSY, ASSERV_ELVT_SPEED_DEFAULT); elvt_order = 0; return elevator_next (WAIT_POS_ORDER, order_received); @@ -191,7 +192,7 @@ elevator__GO_TO_POS_Y__in_position (void) fsm_branch_t elevator__WAIT_FOR_RELEASE_ORDER__order_received (void) { - asserv_move_elevator_absolute(posy[elvt_order] - MIN_POSY, + asserv_move_elevator_absolute(posy[elvt_order + 1] - MIN_POSY, ASSERV_ELVT_SPEED_DEFAULT); elvt_order = 0; return elevator_next (WAIT_FOR_RELEASE_ORDER, order_received); @@ -230,6 +231,7 @@ fsm_branch_t elevator__OPEN_DOORS__doors_opened (void) { nb_puck_elvt = 0; + top_puck_inside_bot = 0; return elevator_next (OPEN_DOORS, doors_opened); } diff --git a/digital/io/src/filterbridge.c b/digital/io/src/filterbridge.c index c318c124..64fcbbfc 100644 --- a/digital/io/src/filterbridge.c +++ b/digital/io/src/filterbridge.c @@ -24,7 +24,7 @@ * }}} */ #include "filterbridge.h" -uint8_t fb_nb_puck; +uint8_t fb_nb_puck = 0; /* +AutoDec */ /* -AutoDec */ -- cgit v1.2.3