From b0b1b6e4f1c80ef2e1c0e438d9512c6c42d81e37 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Tue, 19 May 2009 17:59:03 +0200 Subject: * digital/io/src: - fix timing and PWD for doors movement, - fix elevator positions, - correct initialization procedure that was sending an asserv command (go to first position) without waiting for the previous one to be finished (find zero position). - consider doors opened only when we have contact (not after a certain time). --- digital/io/src/elevator.fsm | 9 ++++++--- digital/io/src/elevator.h | 6 +++--- digital/io/src/elevator_cb.c | 37 ++++++++++++++++++++++--------------- 3 files changed, 31 insertions(+), 21 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/elevator.fsm b/digital/io/src/elevator.fsm index 0e4b3ab5..3bb6ed37 100644 --- a/digital/io/src/elevator.fsm +++ b/digital/io/src/elevator.fsm @@ -56,15 +56,18 @@ IDLE: WAIT_JACK_IN: jack_inserted_into_bot -> INIT - make initializations (elevator zero and open doors) + open doors. + find the zero of the elevator. INIT: doors_opened -> GO_TO_POS_X - make initializations (close doors) + doors opened, close doors. GO_TO_POS_X: in_position -> WAIT_A_PUCK - in position and ready to get a new puck +# FIXME, we should go to a WAIT_FIRST_POS because we are not really ready to +# load some pucks (but this will not happen...). + elevator in position zero, move to first position. WAIT_A_PUCK: new_puck: ok_for_other_pucks -> GO_TO_POS_X diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h index 4ac1027c..49cbd184 100644 --- a/digital/io/src/elevator.h +++ b/digital/io/src/elevator.h @@ -55,9 +55,9 @@ extern uint8_t elvt_order; * pwm constant for elevator doors */ -#define OPEN_DOOR_PWM 200 -#define CLOSE_DOOR_PWM -200 -#define TIME_DOORS_PWM 0xcb +#define OPEN_DOOR_PWM 0x200 +#define CLOSE_DOOR_PWM -0x200 +#define TIME_DOORS_PWM 0xB0 #define TIME_LIGHT_DOORS_PWM 0xaa #endif // elevator_h diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index 840dc314..fe73a717 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -34,10 +34,10 @@ /* Positions when waiting a puck*/ uint16_t posx[4] = { - 0 * ELEVATOR_MM_TO_STEP, - 25 * ELEVATOR_MM_TO_STEP, - 55 * ELEVATOR_MM_TO_STEP, - 85 * ELEVATOR_MM_TO_STEP + 3 * ELEVATOR_MM_TO_STEP, + 10 * ELEVATOR_MM_TO_STEP, + 40 * ELEVATOR_MM_TO_STEP, + 70 * ELEVATOR_MM_TO_STEP }; /* Positions when we go to a target zone */ uint16_t posy[3] = @@ -72,40 +72,47 @@ elevator__IDLE__start (void) /* * WAIT_JACK_IN =jack_inserted_into_bot=> * => INIT - * make initializations (elevator zero and open doors) + * open doors. + * find the zero of the elevator. */ fsm_branch_t elevator__WAIT_JACK_IN__jack_inserted_into_bot (void) { - pwm_set(OPEN_DOOR_PWM, 0); - asserv_elevator_zero_position(); + /* Open doors. */ + pwm_set (OPEN_DOOR_PWM, 0); + /* Find the zero of the elevator. */ + asserv_elevator_zero_position (); return elevator_next (WAIT_JACK_IN, jack_inserted_into_bot); } /* * INIT =doors_opened=> * => GO_TO_POS_X - * make initializations (close doors) + * doors opened, close doors. */ fsm_branch_t elevator__INIT__doors_opened (void) { + /* FIXME: why this is here? */ elevator_is_ready = 0; - pwm_set(CLOSE_DOOR_PWM, TIME_DOORS_PWM); - asserv_move_elevator_absolute(posx[nb_puck_in_elvt], - ASSERV_ELVT_SPEED_DEFAULT); + /* Close the door. */ + pwm_set (CLOSE_DOOR_PWM, TIME_DOORS_PWM); return elevator_next (INIT, doors_opened); } /* * GO_TO_POS_X =in_position=> * => WAIT_A_PUCK - * in position and ready to get a new puck + * elevator in position zero, move to first position. */ fsm_branch_t elevator__GO_TO_POS_X__in_position (void) { + /* FIXME: this sucks, look at elevator.fsm for a real fix. */ elevator_is_ready = 1; + /* move to first position. */ + asserv_move_elevator_absolute(posx[nb_puck_in_elvt], + ASSERV_ELVT_SPEED_DEFAULT); return elevator_next (GO_TO_POS_X, in_position); } @@ -124,7 +131,7 @@ elevator__WAIT_A_PUCK__new_puck (void) elevator_is_ready = 0; elvt_new_puck = 0; // TODO time_ok - if(nb_puck_elvt < 4) + if(++nb_puck_elvt < 4) { /* && ((chrono_remaining_time() - OK_TIME_LIMIT > 0) @@ -198,7 +205,7 @@ elevator__WAIT_FOR_RELEASE_ORDER__order_received (void) fsm_branch_t elevator__LAND_ELEVATOR__in_position (void) { - pwm_set(OPEN_DOOR_PWM, TIME_DOORS_PWM); + pwm_set(OPEN_DOOR_PWM, 0); return elevator_next (LAND_ELEVATOR, in_position); } @@ -210,7 +217,7 @@ elevator__LAND_ELEVATOR__in_position (void) fsm_branch_t elevator__MINI_CLOSE__door_move_finished (void) { - pwm_set(OPEN_DOOR_PWM, TIME_DOORS_PWM); + pwm_set(OPEN_DOOR_PWM, 0); return elevator_next (MINI_CLOSE, door_move_finished); } -- cgit v1.2.3