From 5274f4083e08bf82f23eb67065b94ce14de11bcd Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Fri, 15 May 2009 21:26:39 +0200 Subject: * digital/io/src: - cylinder fsm implemented (but not tested) --- digital/io/src/asserv.h | 2 + digital/io/src/cylinder.c | 6 ++ digital/io/src/cylinder.fsm | 4 +- digital/io/src/cylinder.h | 8 +++ digital/io/src/cylinder_cb.c | 148 ++++++++++++++++++++++-------------------- digital/io/src/elevator_cb.c | 2 +- digital/io/src/filterbridge.c | 2 +- digital/io/src/filterbridge.h | 2 +- digital/io/src/main.c | 44 +++++++++---- 9 files changed, 131 insertions(+), 87 deletions(-) diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index f2e29886..9240b2d3 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -41,6 +41,8 @@ #define ASSERV_ARM_SPEED_DEFAULT 0x18 #define ASSERV_ELVT_SPEED_DEFAULT 0x46 +#define ASSERV_ARM_STEP_BY_DEGREE 14.814814 + /** * Initialize the asserv control module. * This functions does not initialize the asserv board, but the underling diff --git a/digital/io/src/cylinder.c b/digital/io/src/cylinder.c index 9520e0b9..f533e425 100644 --- a/digital/io/src/cylinder.c +++ b/digital/io/src/cylinder.c @@ -23,3 +23,9 @@ * * }}} */ #include "cylinder.h" + +/* last state of optical fence */ +uint8_t of_state = 0; + +/* nb puck in cylinder */ +uint8_t nb_puck_cylinder = 0; diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm index a015f61a..2fcf06c4 100644 --- a/digital/io/src/cylinder.fsm +++ b/digital/io/src/cylinder.fsm @@ -29,7 +29,7 @@ States: Events: start start event - jack_in + jack_inserted_into_bot the init signal new_puck there is a puck in position 1 @@ -46,7 +46,7 @@ IDLE: we wait the jack before moving anything WAIT_FOR_JACK_IN: - jack_in -> INIT + jack_inserted_into_bot -> INIT we init the cylinder position INIT: diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h index 8b9cb427..f6d7ff58 100644 --- a/digital/io/src/cylinder.h +++ b/digital/io/src/cylinder.h @@ -25,4 +25,12 @@ * * }}} */ +#include "common.h" + +/* last state of optical fence */ +extern uint8_t of_state; + +/* nb puck in cylinder */ +extern uint8_t nb_puck_cylinder; + #endif /* cylinder_h */ diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c index 1ff30760..fa207959 100644 --- a/digital/io/src/cylinder_cb.c +++ b/digital/io/src/cylinder_cb.c @@ -25,149 +25,155 @@ #include "common.h" #include "fsm.h" #include "cylinder_cb.h" +#include "asserv.h" +#include "cylinder.h" +#include "filterbridge.h" + /* - * WAIT_FOR_BRIDGE_READY =bridge_ready=> - * => TURN_PLUS_2_AGAIN - * bridge clear, delivering puck... + * IDLE =start=> + * => WAIT_FOR_JACK_IN + * we wait the jack before moving anything */ fsm_branch_t -cylinder__WAIT_FOR_BRIDGE_READY__bridge_ready (void) +cylinder__IDLE__start (void) { - return cylinder_next (WAIT_FOR_BRIDGE_READY, bridge_ready); + return cylinder_next (IDLE, start); } /* - * WAIT_A_PUCK =new_puck=> - * bridge_ready => TURN_PLUS_4 - * put puck to the bridge - * bridge_full => TURN_PLUS_2 - * bridge full, waiting before release puck to the bridge + * WAIT_FOR_JACK_IN =jack_inserted_into_bot=> + * => INIT + * we init the cylinder position */ fsm_branch_t -cylinder__WAIT_A_PUCK__new_puck (void) +cylinder__WAIT_FOR_JACK_IN__jack_inserted_into_bot (void) { - return cylinder_next_branch (WAIT_A_PUCK, new_puck, bridge_ready); - return cylinder_next_branch (WAIT_A_PUCK, new_puck, bridge_full); + asserv_arm_zero_position(); + return cylinder_next (WAIT_FOR_JACK_IN, jack_inserted_into_bot); } /* - * INIT =init_done=> + * INIT =move_done=> * => WAIT_A_PUCK * the cylinder is ready to get pucks */ fsm_branch_t -cylinder__INIT__init_done (void) +cylinder__INIT__move_done (void) { - return cylinder_next (INIT, init_done); + return cylinder_next (INIT, move_done); } /* - * WAIT_FOR_PUCKS_RELEASE =lift_ready=> - * => TURN_PLUS_1 - * robot ready for new puck, open door - */ -fsm_branch_t -cylinder__WAIT_FOR_PUCKS_RELEASE__lift_ready (void) -{ - return cylinder_next (WAIT_FOR_PUCKS_RELEASE, lift_ready); -} - -/* - * IDLE =jack_in=> - * => INIT - * initialize robot + * WAIT_A_PUCK =new_puck=> + * => TURN_PLUS_3 + * turn cylinder 3 position after */ fsm_branch_t -cylinder__IDLE__jack_in (void) +cylinder__WAIT_A_PUCK__new_puck (void) { - return cylinder_next (IDLE, jack_in); + ++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_2 =move_done=> + * TURN_PLUS_3 =move_done=> * => WAIT_FOR_BRIDGE_READY * bridge full, waiting for Bison Fute's clearance */ fsm_branch_t -cylinder__TURN_PLUS_2__move_done (void) +cylinder__TURN_PLUS_3__move_done (void) { - return cylinder_next (TURN_PLUS_2, move_done); + return cylinder_next (TURN_PLUS_3, move_done); } /* - * PROBE_OF =probe_done=> - * puck_detected_and_lift_full => TURN_MINUS_3 - * BAD CASE, there is an undesirable puck to the cylinder - * puck_detected_and_lift_ready => WAIT_FOR_BRIDGE_READY - * there is a puck, we take a shortcut... - * cylinder_empty_and_lift_ready => WAIT_A_PUCK - * ready for a new puck - * cylinder_empty_and_lift_full => TURN_MINUS_1 - * stop accepting new puck + * 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 + * bridge clear, an another puck is in the cylinder */ fsm_branch_t -cylinder__PROBE_OF__probe_done (void) +cylinder__WAIT_FOR_BRIDGE_READY__bridge_ready (void) { - return cylinder_next_branch (PROBE_OF, probe_done, puck_detected_and_lift_full); - return cylinder_next_branch (PROBE_OF, probe_done, puck_detected_and_lift_ready); - return cylinder_next_branch (PROBE_OF, probe_done, cylinder_empty_and_lift_ready); - return cylinder_next_branch (PROBE_OF, probe_done, cylinder_empty_and_lift_full); + ++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=> - * => WAIT_A_PUCK - * door opened, ready for a new puck + * 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) { - return cylinder_next (TURN_PLUS_1, move_done); + 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_ARM_SPEED_DEFAULT); + return cylinder_next_branch (TURN_PLUS_1, move_done, not_ok_for_other_puck); } /* - * TURN_MINUS_1 =move_done=> - * => WAIT_FOR_PUCKS_RELEASE - * robot entrance closed + * TURN_PLUS_1_LOOP =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_MINUS_1__move_done (void) +cylinder__TURN_PLUS_1_LOOP__move_done (void) { - return cylinder_next (TURN_MINUS_1, move_done); + 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); } /* - * TURN_MINUS_3 =move_done=> + * TURN_MINUS_1 =move_done=> * => WAIT_FOR_PUCKS_RELEASE - * evil puck dropped and robot entrance closed + * cylinder close, you shall not pass (but try again later) */ fsm_branch_t -cylinder__TURN_MINUS_3__move_done (void) +cylinder__TURN_MINUS_1__move_done (void) { - return cylinder_next (TURN_MINUS_3, move_done); + return cylinder_next (TURN_MINUS_1, move_done); } /* - * TURN_PLUS_2_AGAIN =move_done=> - * => PROBE_OF - * there is a puck to the cylinder? + * WAIT_FOR_PUCKS_RELEASE =bot_empty=> + * => TURN_PLUS_1_AGAIN */ fsm_branch_t -cylinder__TURN_PLUS_2_AGAIN__move_done (void) +cylinder__WAIT_FOR_PUCKS_RELEASE__bot_empty (void) { - return cylinder_next (TURN_PLUS_2_AGAIN, move_done); + asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE, + ASSERV_ARM_SPEED_DEFAULT); + return cylinder_next (WAIT_FOR_PUCKS_RELEASE, bot_empty); } /* - * TURN_PLUS_4 =move_done=> - * => PROBE_OF - * there is a puck to the cylinder? + * TURN_PLUS_1_AGAIN =move_done=> + * => WAIT_A_PUCK + * cylinder ready */ fsm_branch_t -cylinder__TURN_PLUS_4__move_done (void) +cylinder__TURN_PLUS_1_AGAIN__move_done (void) { - return cylinder_next (TURN_PLUS_4, move_done); + return cylinder_next (TURN_PLUS_1_AGAIN, move_done); } diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index d20496dc..2fc029d7 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -114,7 +114,7 @@ elevator__WAIT_A_PUCK__new_puck (void) // TODO time_ok if(nb_puck_elvt < 4 && ((chrono_remaining_time() - OK_TIME_LIMIT > 0) - || nb_puck_pf != 0)) + || nb_puck_fb != 0)) 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/filterbridge.c b/digital/io/src/filterbridge.c index efe4ec5b..0b5001cc 100644 --- a/digital/io/src/filterbridge.c +++ b/digital/io/src/filterbridge.c @@ -24,7 +24,7 @@ * }}} */ #include "filterbridge.h" -uint8_t nb_puck_pf; +uint8_t nb_puck_fb; /* +AutoDec */ /* -AutoDec */ diff --git a/digital/io/src/filterbridge.h b/digital/io/src/filterbridge.h index 1853a496..81fdcc60 100644 --- a/digital/io/src/filterbridge.h +++ b/digital/io/src/filterbridge.h @@ -27,6 +27,6 @@ #include "common.h" -extern uint8_t nb_puck_pf; +extern uint8_t nb_puck_fb; #endif // filterbridge_h diff --git a/digital/io/src/main.c b/digital/io/src/main.c index df111afc..ea200a73 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -49,6 +49,8 @@ #include "playground.h" #include "contact.h" #include "elevator.h" +#include "filterbridge.h" +#include "cylinder.h" #include "io.h" @@ -92,6 +94,7 @@ uint16_t main_move_wait_cycle; /* FIXME to be delete */ uint8_t jack_emulation = 0; +uint8_t cylinder_puck1_emulation = 0; /** * The same for init FSM */ @@ -157,9 +160,8 @@ main_init (void) //fsm_handle_event (&init_fsm, INIT_EVENT_start); fsm_handle_event (&filterbridge_fsm, FILTERBRIDGE_EVENT_start); fsm_handle_event (&elevator_fsm, ELEVATOR_EVENT_start); - /* fsm_handle_event (&top_fsm, TOP_EVENT_start); - fsm_handle_event (&top_fsm, TOP_EVENT_start); - fsm_handle_event (&top_fsm, TOP_EVENT_start); */ + fsm_handle_event (&cylinder_fsm, CYLINDER_EVENT_start); + /* fsm_handle_event (&top_fsm, TOP_EVENT_start); */ /* Sharp module */ sharp_init (); /* PWM module */ @@ -295,6 +297,14 @@ main_loop (void) TODO when asserv elevator can failed } */ + /* cylinder asserv status */ + asserv_status_e cylinder_status = asserv_last_cmd_ack() + ? asserv_arm_cmd_status() : none; + if (cylinder_status == success) + { + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_move_done); + } /* send event if elevator received an order */ if(elvt_order) FSM_HANDLE_EVENT (&elevator_fsm, @@ -309,6 +319,16 @@ main_loop (void) if(!IO_GET (CONTACT_ELEVATOR_DOOR)) FSM_HANDLE_EVENT (&elevator_fsm, ELEVATOR_EVENT_doors_opened); + /* bridge ready */ + if(nb_puck_fb) + 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()) { @@ -323,15 +343,9 @@ main_loop (void) INIT_EVENT_jack_inserted_into_bot); FSM_HANDLE_EVENT (&elevator_fsm, ELEVATOR_EVENT_jack_inserted_into_bot); + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_jack_inserted_into_bot); } - /* - FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ? - TOP_EVENT_jack_removed_from_bot : - TOP_EVENT_jack_inserted_into_bot); - FSM_HANDLE_EVENT (&init_fsm, switch_get_jack () ? - INIT_EVENT_jack_removed_from_bot : - INIT_EVENT_jack_inserted_into_bot); - */ /* Settings acknowledge */ /* if (main_top_generate_settings_ack_event) @@ -438,6 +452,14 @@ main_loop (void) FSM_HANDLE_EVENT (&filterbridge_fsm, FILTERBRIDGE_EVENT_no_puck_on_pos2); } + /* test cylinder sensor */ + /* if(!IO_GET (CON */ + if(cylinder_puck1_emulation) + { + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_new_puck); + } + /* FIXME to be delete */ if(jack_emulation) { -- cgit v1.2.3