From a7fca46683362958b1dc40990ef681770e63df20 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Thu, 21 May 2009 15:45:11 +0200 Subject: * digital/io/src: - first version of distributor add-on fsm --- digital/io/src/cylinder.c | 6 +++ digital/io/src/cylinder.fsm | 51 ++++++++++++++++++ digital/io/src/cylinder.h | 6 +++ digital/io/src/cylinder_cb.c | 126 +++++++++++++++++++++++++++++++++++++++++++ digital/io/src/main.c | 7 +++ 5 files changed, 196 insertions(+) (limited to 'digital/io') diff --git a/digital/io/src/cylinder.c b/digital/io/src/cylinder.c index 4dd0e590..62904f4a 100644 --- a/digital/io/src/cylinder.c +++ b/digital/io/src/cylinder.c @@ -38,4 +38,10 @@ uint8_t cylinder_close_order = 0; /* flush puck on the cylinder */ uint8_t cylinder_flush_order = 0; +/* distributor mode switch */ +uint8_t cylinder_distributor_mode = 0; + +/* distributor fucked */ +uint8_t cylinder_distributor_fucked = 0; + /* cylinder order */ diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm index aefe1170..4b2fd49f 100644 --- a/digital/io/src/cylinder.fsm +++ b/digital/io/src/cylinder.fsm @@ -33,6 +33,19 @@ States: drop 1 puck to the fb TURN_PLUS_1_CLOSE close the cylinder + WAIT_DISTRIB_FUCKED + waiting for the bot are really on the distributor + WAIT_BRIDGE_READY_DISTRIB + waiting for the bridge + TURN_PLUS_1_AND_OFO_DISTRIB + get a puck on distrib + TURN_PLUS_1_AND_MINUS_OFO_DISTRIB + we neutralize the offset before return in normal mode + TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP + blah + PROBE_OF + we probe the OF + Events: start @@ -53,6 +66,14 @@ Events: the bot dont have 4 pucks flush_order we receive a flush order + approching_distributor + we go in distributor mode + distrib_fucked + we are really in the distributor + of_puck + we see a puck to the of + of_no_puck + we don't see a puck in of state_timeout useless here @@ -79,6 +100,32 @@ WAIT_A_PUCK: we close cylinder as requested flush_order -> WAIT_BRIDGE_READY_FLUSH flush all pucks to the bridge + approching_distributor -> WAIT_DISTRIB_FUCKED + We wait a distributor + +WAIT_DISTRIB_FUCKED: + distrib_fucked -> WAIT_BRIDGE_READY_DISTRIB + We are on a distributor + +WAIT_BRIDGE_READY_DISTRIB: + bridge_ready -> TURN_PLUS_1_AND_OFO_DISTRIB + we turn to check the of + +TURN_PLUS_1_AND_OFO_DISTRIB: + move_done -> PROBE_OF + We probe the puck + +PROBE_OF: + of_puck: bot_full -> WAIT_BOT_NOT_FULL + return to normal mode with a bot full of puck + of_puck: bot_not_full -> TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP + we get a puck and we are hungry yet, go eat an another puck + of_no_puck -> TURN_PLUS_1_AND_MINUS_OFO_DISTRIB + Distributor empty, go elsewhere + +TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP: + move_done -> WAIT_BRIDGE_READY_DISTRIB + ready to get another puck WAIT_BRIDGE_READY: bridge_ready -> TURN_PLUS_1_AND_OFO @@ -90,6 +137,10 @@ TURN_PLUS_1_AND_OFO: move_done: bot_full -> WAIT_BOT_NOT_FULL bot full, waiting for pucks teleportation +TURN_PLUS_1_AND_MINUS_OFO_DISTRIB: + move_done -> WAIT_A_PUCK + bridge empty, return to normal mode + TURN_PLUS_1_AND_MINUS_OFO: move_done -> WAIT_A_PUCK ready for other pucks diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h index 817b16ab..78a65e7f 100644 --- a/digital/io/src/cylinder.h +++ b/digital/io/src/cylinder.h @@ -42,6 +42,12 @@ extern uint8_t cylinder_close_order; /* flush puck on the cylinder */ extern uint8_t cylinder_flush_order; +/* distributor mode switch */ +extern uint8_t cylinder_distributor_mode; + +/* distributor fucked */ +extern uint8_t cylinder_distributor_fucked; + /* mecanical offset */ #define CYLINDER_OFFSET 0x43e diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c index 5694d95d..d2c31f6a 100644 --- a/digital/io/src/cylinder_cb.c +++ b/digital/io/src/cylinder_cb.c @@ -92,6 +92,8 @@ cylinder__INIT_POS__move_done (void) fsm_branch_t cylinder__WAIT_A_PUCK__new_puck (void) { + cylinder_distributor_mode = 0; + cylinder_distributor_fucked = 0; return cylinder_next (WAIT_A_PUCK, new_puck); } @@ -103,6 +105,8 @@ cylinder__WAIT_A_PUCK__new_puck (void) fsm_branch_t cylinder__WAIT_A_PUCK__close_order (void) { + cylinder_distributor_mode = 0; + cylinder_distributor_fucked = 0; asserv_move_arm(-1*60*ASSERV_ARM_STEP_BY_DEGREE, ASSERV_ARM_SPEED_DEFAULT); return cylinder_next (WAIT_A_PUCK, close_order); @@ -116,9 +120,24 @@ cylinder__WAIT_A_PUCK__close_order (void) fsm_branch_t cylinder__WAIT_A_PUCK__flush_order (void) { + cylinder_distributor_mode = 0; + cylinder_distributor_fucked = 0; return cylinder_next (WAIT_A_PUCK, flush_order); } +/* + * WAIT_A_PUCK =approching_distributor=> + * => WAIT_DISTRIB_FUCKED + * We wait a distributor + */ +fsm_branch_t +cylinder__WAIT_A_PUCK__approching_distributor (void) +{ + cylinder_distributor_mode = 0; + cylinder_distributor_fucked = 0; + return cylinder_next (WAIT_A_PUCK, approching_distributor); +} + /* * WAIT_BRIDGE_READY =bridge_ready=> * => TURN_PLUS_1_AND_OFO @@ -186,6 +205,8 @@ cylinder__TURN_PLUS_1_AND_MINUS_OFO__move_done (void) fsm_branch_t cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void) { + cylinder_distributor_mode = 0; + cylinder_distributor_fucked = 0; return cylinder_next (WAIT_BOT_NOT_FULL, bot_not_full); } @@ -197,6 +218,8 @@ cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void) fsm_branch_t cylinder__WAIT_BOT_NOT_FULL__flush_order (void) { + cylinder_distributor_mode = 0; + cylinder_distributor_fucked = 0; if(of_offset_enabled) { asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, @@ -318,3 +341,106 @@ cylinder__TURN_PLUS_1_CLOSE__move_done (void) } return cylinder_next (TURN_PLUS_1_CLOSE, move_done); } + +/* + * WAIT_DISTRIB_FUCKED =distrib_fucked=> + * => WAIT_BRIDGE_READY_DISTRIB + * We are on a distributor + */ +fsm_branch_t +cylinder__WAIT_DISTRIB_FUCKED__distrib_fucked (void) +{ + return cylinder_next (WAIT_DISTRIB_FUCKED, distrib_fucked); +} + +/* + * WAIT_BRIDGE_READY_DISTRIB =bridge_ready=> + * => TURN_PLUS_1_AND_OFO_DISTRIB + * we turn to check the of + */ +fsm_branch_t +cylinder__WAIT_BRIDGE_READY_DISTRIB__bridge_ready (void) +{ + asserv_move_arm((1+CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + of_offset_enabled = 1; + return cylinder_next (WAIT_BRIDGE_READY_DISTRIB, bridge_ready); +} + +/* + * TURN_PLUS_1_AND_OFO_DISTRIB =move_done=> + * => PROBE_OF + * We probe the puck + */ +fsm_branch_t +cylinder__TURN_PLUS_1_AND_OFO_DISTRIB__move_done (void) +{ + if(puck_on_cylinder) + { + --cylinder_nb_puck; + ++fb_nb_puck; + puck_on_cylinder = 0; + } + return cylinder_next (TURN_PLUS_1_AND_OFO_DISTRIB, move_done); +} + +/* + * TURN_PLUS_1_AND_MINUS_OFO_DISTRIB =move_done=> + * => WAIT_A_PUCK + * bridge empty, return to normal mode + */ +fsm_branch_t +cylinder__TURN_PLUS_1_AND_MINUS_OFO_DISTRIB__move_done (void) +{ + return cylinder_next (TURN_PLUS_1_AND_MINUS_OFO_DISTRIB, move_done); +} + +/* + * TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP =move_done=> + * => WAIT_BRIDGE_READY_DISTRIB + * ready to get another puck + */ +fsm_branch_t +cylinder__TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP__move_done (void) +{ + return cylinder_next (TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP, move_done); +} + +/* + * PROBE_OF =of_puck=> + * bot_full => WAIT_BOT_NOT_FULL + * return to normal mode with a bot full of puck + * bot_not_full => TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP + * we get a puck and we are hungry yet, go eat an another puck + */ +fsm_branch_t +cylinder__PROBE_OF__of_puck (void) +{ + /* we have a new puck on cylinder */ + puck_on_cylinder =1; + ++cylinder_nb_puck; + ++top_total_puck_taken; + ++top_puck_inside_bot; + if(top_puck_inside_bot < 4) + { + asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + of_offset_enabled = 0; + return cylinder_next_branch (PROBE_OF, of_puck, bot_not_full); + } + return cylinder_next_branch (PROBE_OF, of_puck, bot_full); +} + +/* + * PROBE_OF =of_no_puck=> + * => TURN_PLUS_1_AND_MINUS_OFO_DISTRIB + * Distributor empty, go elsewhere + */ +fsm_branch_t +cylinder__PROBE_OF__of_no_puck (void) +{ + asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + of_offset_enabled = 0; + return cylinder_next (PROBE_OF, of_no_puck); +} diff --git a/digital/io/src/main.c b/digital/io/src/main.c index d7c8a870..75b318dd 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -244,6 +244,13 @@ main_event_to_fsm (void) FSM_HANDLE_EVENT (&elevator_fsm, ELEVATOR_EVENT_order_bypass); + if(cylinder_distributor_mode) + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_approching_distributor); + + if(cylinder_distributor_fucked) + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_distrib_fucked); /* bot empty */ if(top_puck_inside_bot < 4) FSM_HANDLE_EVENT (&cylinder_fsm, -- cgit v1.2.3