From 78204f6bcba74ab5c5505ac06c3384e193177718 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Fri, 15 May 2009 00:36:58 +0200 Subject: * digital/io/src: - work on elevator fsm --- digital/io/src/elevator.c | 6 +++++- digital/io/src/elevator.h | 6 ++++++ digital/io/src/elevator_cb.c | 9 ++++----- digital/io/src/filterbridge_cb.c | 1 + digital/io/src/main.c | 34 ++++++++++++++++++++++++++++++++++ 5 files changed, 50 insertions(+), 6 deletions(-) diff --git a/digital/io/src/elevator.c b/digital/io/src/elevator.c index c0eef588..d89ff7a1 100644 --- a/digital/io/src/elevator.c +++ b/digital/io/src/elevator.c @@ -28,12 +28,16 @@ /** * State of the elevator */ -uint8_t elevator_is_ready; +uint8_t elevator_is_ready = 0; uint8_t nb_puck_in_elvt = 0; /** * elevator orders */ uint8_t elvt_order; +/** + * new_puck information (from filterbridge) + */ +uint8_t elvt_new_puck = 0; /* +AutoDec */ diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h index 9e31f488..69970e4c 100644 --- a/digital/io/src/elevator.h +++ b/digital/io/src/elevator.h @@ -35,10 +35,16 @@ extern uint8_t elevator_is_ready; */ extern uint8_t nb_puck_in_elvt; +/** + * new_puck information (from filterbridge) + */ +extern uint8_t elvt_new_puck; + /** * elevator orders */ extern uint8_t elvt_order; + /** * conversion stop/millimeter */ diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index f1184cc8..d20496dc 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -32,16 +32,13 @@ #include "filterbridge.h" /* Positions when waiting a puck*/ -uint16_t posx[4] = {0,0,0,0}; +uint16_t posx[4] = {0,25,55,85}; /* Positions when we go to a target zone */ -uint16_t posy[3] = {0,0,0}; +uint16_t posy[3] = {125,95,65}; /* nb puck on the elevator */ uint8_t nb_puck_elvt = 0; -/* ready flag */ -uint8_t elevator_is_ready = 0; - /* increase/decrease of pos y */ #define MAJ_POSY 100 #define MIN_POSY 50 @@ -113,6 +110,7 @@ elevator__WAIT_A_PUCK__new_puck (void) { elevator_is_ready = 0; ++nb_puck_in_elvt; + elvt_new_puck = 0; // TODO time_ok if(nb_puck_elvt < 4 && ((chrono_remaining_time() - OK_TIME_LIMIT > 0) @@ -123,6 +121,7 @@ elevator__WAIT_A_PUCK__new_puck (void) } /* + * TODO time_up * WAIT_A_PUCK =time_up=> * => WAIT_POS_ORDER * no more time to wait a new puck diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c index 00848d57..dd210e91 100644 --- a/digital/io/src/filterbridge_cb.c +++ b/digital/io/src/filterbridge_cb.c @@ -114,6 +114,7 @@ filterbridge__OPEN_DOOR__state_timeout (void) fsm_branch_t filterbridge__PUSH_PUCK__no_puck_on_pos2 (void) { + elvt_new_puck = 1; servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE); servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE); return filterbridge_next (PUSH_PUCK, no_puck_on_pos2); diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 32be6ac0..df111afc 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -48,6 +48,7 @@ #include "pwm.h" #include "playground.h" #include "contact.h" +#include "elevator.h" #include "io.h" @@ -155,6 +156,7 @@ main_init (void) //fsm_handle_event (&top_fsm, TOP_EVENT_start); //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); */ @@ -277,6 +279,36 @@ main_loop (void) FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_failed); } + /* elevator asserv status */ + asserv_status_e elevator_status = asserv_last_cmd_ack() + ? asserv_elevator_cmd_status() : none; + if (elevator_status == success) + { + /* TODO le init init juste la position 0 du robal */ + FSM_HANDLE_EVENT (&elevator_fsm, + ELEVATOR_EVENT_init_done); + FSM_HANDLE_EVENT (&elevator_fsm, + ELEVATOR_EVENT_in_position); + } + /*else if (elevator_status == failed) + { + TODO when asserv elevator can failed + } + */ + /* send event if elevator received an order */ + if(elvt_order) + FSM_HANDLE_EVENT (&elevator_fsm, + ELEVATOR_EVENT_order_received); + + + /* elevator new puck (set by filterbridge) */ + if(elvt_new_puck) + FSM_HANDLE_EVENT (&elevator_fsm, + ELEVATOR_EVENT_new_puck); + /* elvt door switch */ + if(!IO_GET (CONTACT_ELEVATOR_DOOR)) + FSM_HANDLE_EVENT (&elevator_fsm, + ELEVATOR_EVENT_doors_opened); /* Jack */ if(switch_get_jack()) { @@ -289,6 +321,8 @@ main_loop (void) TOP_EVENT_jack_inserted_into_bot); FSM_HANDLE_EVENT (&init_fsm, INIT_EVENT_jack_inserted_into_bot); + FSM_HANDLE_EVENT (&elevator_fsm, + ELEVATOR_EVENT_jack_inserted_into_bot); } /* FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ? -- cgit v1.2.3