From 9f98a7f973d4e8b36cd74d44b3e3d3f9ce7a1903 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Wed, 13 May 2009 00:37:03 +0200 Subject: * digital/io/src: - elevator FSM implemented (but not tested) --- digital/io/src/elevator_cb.c | 25 ++++++++++++++++++++++--- digital/io/src/filterbridge.c | 2 ++ digital/io/src/filterbridge.h | 4 ++++ 3 files changed, 28 insertions(+), 3 deletions(-) (limited to 'digital') diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index 025f947c..f1184cc8 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -28,16 +28,28 @@ #include "elevator.h" #include "asserv.h" #include "pwm.h" +#include "chrono.h" +#include "filterbridge.h" /* Positions when waiting a puck*/ uint16_t posx[4] = {0,0,0,0}; /* Positions when we go to a target zone */ uint16_t posy[3] = {0,0,0}; +/* 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 +/* time limit */ +/* TODO to be define */ +#define OK_TIME_LIMIT 20 + /* * IDLE =start=> * => WAIT_JACK_IN @@ -57,6 +69,7 @@ elevator__IDLE__start (void) fsm_branch_t elevator__WAIT_JACK_IN__jack_inserted_into_bot (void) { + asserv_elevator_zero_position(); return elevator_next (WAIT_JACK_IN, jack_inserted_into_bot); } @@ -68,9 +81,9 @@ elevator__WAIT_JACK_IN__jack_inserted_into_bot (void) fsm_branch_t elevator__INIT__init_done (void) { + elevator_is_ready = 0; asserv_move_elevator_absolute(posx[nb_puck_in_elvt], ASSERV_ELVT_SPEED_DEFAULT); - elevator_is_ready = 0; return elevator_next (INIT, init_done); } @@ -98,10 +111,14 @@ elevator__GO_TO_POS_X__in_position (void) fsm_branch_t elevator__WAIT_A_PUCK__new_puck (void) { + elevator_is_ready = 0; + ++nb_puck_in_elvt; // TODO time_ok - //if(nb_puck_in_elvt < 4 && (time_ok || !fb-empty)) + if(nb_puck_elvt < 4 && + ((chrono_remaining_time() - OK_TIME_LIMIT > 0) + || nb_puck_pf != 0)) return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks); - //else + else return elevator_next_branch (WAIT_A_PUCK, new_puck, not_ok_for_other_pucks); } @@ -113,6 +130,7 @@ elevator__WAIT_A_PUCK__new_puck (void) fsm_branch_t elevator__WAIT_A_PUCK__time_up (void) { + elevator_is_ready = 0; return elevator_next (WAIT_A_PUCK, time_up); } @@ -186,6 +204,7 @@ elevator__MINI_CLOSE__door_move_finished (void) fsm_branch_t elevator__OPEN_DOORS__doors_opened (void) { + nb_puck_elvt = 0; nb_puck_in_elvt = 0; return elevator_next (OPEN_DOORS, doors_opened); } diff --git a/digital/io/src/filterbridge.c b/digital/io/src/filterbridge.c index f74a1a58..efe4ec5b 100644 --- a/digital/io/src/filterbridge.c +++ b/digital/io/src/filterbridge.c @@ -24,6 +24,8 @@ * }}} */ #include "filterbridge.h" +uint8_t nb_puck_pf; + /* +AutoDec */ /* -AutoDec */ diff --git a/digital/io/src/filterbridge.h b/digital/io/src/filterbridge.h index 1876bd86..1853a496 100644 --- a/digital/io/src/filterbridge.h +++ b/digital/io/src/filterbridge.h @@ -25,4 +25,8 @@ * * }}} */ +#include "common.h" + +extern uint8_t nb_puck_pf; + #endif // filterbridge_h -- cgit v1.2.3