From 5536ee0fb112ee462fdcceb327ffa5ac5bac9ba3 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Thu, 21 May 2009 04:39:13 +0200 Subject: * digital/io/src: - fix elevator fsm: using timeout to control doors - add forgotten event handler for cylinder (no_close_order) - fix elevator fsm again: elevator positions --- digital/io/src/cylinder_cb.c | 1 + digital/io/src/elevator.fsm | 14 ++++++------- digital/io/src/elevator_cb.c | 47 +++++++++++++++++++++----------------------- digital/io/src/main.c | 8 +++++++- 4 files changed, 36 insertions(+), 34 deletions(-) (limited to 'digital') diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c index f820f633..5694d95d 100644 --- a/digital/io/src/cylinder_cb.c +++ b/digital/io/src/cylinder_cb.c @@ -155,6 +155,7 @@ cylinder__TURN_PLUS_1_AND_OFO__move_done (void) { ++top_total_puck_taken; ++top_puck_inside_bot; + ++cylinder_nb_puck; } if(top_puck_inside_bot < 4) { diff --git a/digital/io/src/elevator.fsm b/digital/io/src/elevator.fsm index 3bb6ed37..d9388690 100644 --- a/digital/io/src/elevator.fsm +++ b/digital/io/src/elevator.fsm @@ -21,13 +21,13 @@ States: ready to open doors at your command LAND_ELEVATOR land elevator to the ground - MINI_CLOSE + MINI_CLOSE[timeout=45] try to unblock pucks - OPEN_DOORS + OPEN_DOORS[timeout=675] releasing pucks WAIT_FOR_CLOSE_ORDER ready to close doors at your command - CLOSE_DOORS + CLOSE_DOORS[timeout=675] closing the elevator's doors @@ -46,8 +46,6 @@ Events: we received a new order doors_opened doors are opened - door_move_finished - doors move is done state_timeout IDLE: @@ -98,11 +96,11 @@ LAND_ELEVATOR: OPEN_DOORS: doors_opened -> WAIT_FOR_CLOSE_ORDER wait for close order - door_move_finished -> MINI_CLOSE + state_timeout -> MINI_CLOSE try to unblock doors MINI_CLOSE: - door_move_finished -> OPEN_DOORS + state_timeout -> OPEN_DOORS try to release pucks again WAIT_FOR_CLOSE_ORDER: @@ -110,5 +108,5 @@ WAIT_FOR_CLOSE_ORDER: closing doors CLOSE_DOORS: - door_move_finished -> GO_TO_POS_X + state_timeout -> GO_TO_POS_X pucks are released and elevator is ready to make a new column diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index 471c46b9..c37ef69c 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -48,12 +48,9 @@ uint16_t posy[3] = 65 * ELEVATOR_MM_TO_STEP }; -/* nb puck on the elevator */ -uint8_t nb_puck_elvt = 0; - /* increase/decrease of pos y */ -#define MAJ_POSY 100 -#define MIN_POSY 50 +#define MAJ_POSY (10 * ELEVATOR_MM_TO_STEP) +#define MIN_POSY (5 * ELEVATOR_MM_TO_STEP) /* time limit */ /* TODO to be define */ @@ -112,7 +109,7 @@ elevator__GO_TO_POS_X__in_position (void) /* FIXME: this sucks, look at elevator.fsm for a real fix. */ elvt_is_ready = 1; /* move to first position. */ - asserv_move_elevator_absolute(posx[nb_puck_elvt], + asserv_move_elevator_absolute(posx[elvt_nb_puck], ASSERV_ELVT_SPEED_DEFAULT); return elevator_next (GO_TO_POS_X, in_position); } @@ -132,13 +129,13 @@ elevator__WAIT_A_PUCK__new_puck (void) elvt_is_ready = 0; elvt_new_puck = 0; // TODO time_ok - if(++nb_puck_elvt < 4) + if(elvt_nb_puck < 4) { /* && ((chrono_remaining_time() - OK_TIME_LIMIT > 0) || nb_puck_fb != 0)) */ - asserv_move_elevator_absolute(posx[nb_puck_elvt], + asserv_move_elevator_absolute(posx[elvt_nb_puck], ASSERV_ELVT_SPEED_DEFAULT); return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks); } @@ -167,7 +164,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 + 1] + 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); @@ -192,7 +189,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 + 1] - 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); @@ -211,15 +208,15 @@ elevator__LAND_ELEVATOR__in_position (void) } /* - * MINI_CLOSE =door_move_finished=> + * MINI_CLOSE =state_timeout=> * => OPEN_DOORS * try to release pucks again */ fsm_branch_t -elevator__MINI_CLOSE__door_move_finished (void) +elevator__MINI_CLOSE__state_timeout (void) { pwm_set(OPEN_DOOR_PWM, 0); - return elevator_next (MINI_CLOSE, door_move_finished); + return elevator_next (MINI_CLOSE, state_timeout); } /* @@ -230,21 +227,22 @@ elevator__MINI_CLOSE__door_move_finished (void) fsm_branch_t elevator__OPEN_DOORS__doors_opened (void) { - nb_puck_elvt = 0; + elvt_nb_puck = 0; top_puck_inside_bot = 0; + pwm_set(0,0); return elevator_next (OPEN_DOORS, doors_opened); } /* - * OPEN_DOORS =door_move_finished=> + * OPEN_DOORS =state_timeout=> * => MINI_CLOSE * try to unblock doors */ fsm_branch_t -elevator__OPEN_DOORS__door_move_finished (void) +elevator__OPEN_DOORS__state_timeout (void) { - pwm_set(CLOSE_DOOR_PWM, TIME_LIGHT_DOORS_PWM); - return elevator_next (OPEN_DOORS, door_move_finished); + pwm_set(CLOSE_DOOR_PWM,0); + return elevator_next (OPEN_DOORS, state_timeout); } /* @@ -255,22 +253,21 @@ elevator__OPEN_DOORS__door_move_finished (void) fsm_branch_t elevator__WAIT_FOR_CLOSE_ORDER__order_received (void) { - pwm_set(CLOSE_DOOR_PWM, TIME_DOORS_PWM); + pwm_set(CLOSE_DOOR_PWM, 0); elvt_order = 0; return elevator_next (WAIT_FOR_CLOSE_ORDER, order_received); } /* - * CLOSE_DOORS =door_move_finished=> + * CLOSE_DOORS =state_timeout=> * => GO_TO_POS_X * pucks are released and elevator is ready to make a new column */ fsm_branch_t -elevator__CLOSE_DOORS__door_move_finished (void) +elevator__CLOSE_DOORS__state_timeout (void) { - asserv_move_elevator_absolute(posx[nb_puck_elvt], + pwm_set(0,0); + asserv_move_elevator_absolute(posx[elvt_nb_puck], ASSERV_ELVT_SPEED_DEFAULT); - return elevator_next (CLOSE_DOORS, door_move_finished); + return elevator_next (CLOSE_DOORS, state_timeout); } - - diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 0f31221a..20717222 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -55,6 +55,7 @@ #include "filterbridge.h" #include "cylinder.h" #include "init.h" +#include "top.h" #include "io.h" @@ -244,14 +245,19 @@ main_event_to_fsm (void) FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_bridge_ready); /* bot empty */ - if(fb_nb_puck + elvt_nb_puck + cylinder_nb_puck < 4) + if(top_puck_inside_bot < 4) FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_bot_not_full); /* cylinder_close_order */ if(cylinder_close_order) + { FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_close_order); + } + else + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_no_close_order); /* cylinder_flush_order */ if(cylinder_flush_order) -- cgit v1.2.3