From ce3a53dcc14f98b120917bf30ec72ab5be72110a Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Sat, 9 May 2009 10:42:15 +0200 Subject: * digital/io/src: - first implementation for elevator FSM (more work needed) --- digital/io/src/asserv.c | 2 +- digital/io/src/asserv.h | 4 +- digital/io/src/elevator.c | 6 ++ digital/io/src/elevator.fsm | 46 +++++++++--- digital/io/src/elevator.h | 24 +++++++ digital/io/src/elevator_cb.c | 164 +++++++++++++++++++++++++++++++++---------- 6 files changed, 196 insertions(+), 50 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index bc973ec4..2bb6b515 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -671,7 +671,7 @@ asserv_arm_zero_position (void) void asserv_elevator_zero_position (void) { - asserv_twi_buffer_param[0] = ASSERV_ELVT_SPEED_DEFAULT; + asserv_twi_buffer_param[0] = -0x10; asserv_twi_send_command ('C', 1); } diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index 17b396da..f2e29886 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -38,8 +38,8 @@ * ignored. */ -#define ASSERV_ARM_SPEED_DEFAULT 0x16 -#define ASSERV_ELVT_SPEED_DEFAULT 0x30 +#define ASSERV_ARM_SPEED_DEFAULT 0x18 +#define ASSERV_ELVT_SPEED_DEFAULT 0x46 /** * Initialize the asserv control module. diff --git a/digital/io/src/elevator.c b/digital/io/src/elevator.c index 3a4a81ee..c0eef588 100644 --- a/digital/io/src/elevator.c +++ b/digital/io/src/elevator.c @@ -29,6 +29,12 @@ * State of the elevator */ uint8_t elevator_is_ready; +uint8_t nb_puck_in_elvt = 0; +/** + * elevator orders + */ +uint8_t elvt_order; + /* +AutoDec */ /* -AutoDec */ diff --git a/digital/io/src/elevator.fsm b/digital/io/src/elevator.fsm index b35ce04e..c57bedb8 100644 --- a/digital/io/src/elevator.fsm +++ b/digital/io/src/elevator.fsm @@ -5,6 +5,10 @@ elevator States: IDLE waiting for the beginning of the round + WAIT_JACK_IN + waiting for the jack + INIT + init the elevator GO_TO_POS_X go to a position to be ready to get a new puck WAIT_A_PUCK @@ -12,9 +16,13 @@ States: WAIT_POS_ORDER waiting for position order (depending where we want to drop our column) GO_TO_POS_Y - go to a position to be ready to release pucks + go to a position + 1cm to be ready to release pucks WAIT_FOR_RELEASE_ORDER ready to open doors at your command + LAND_ELEVATOR + land elevator to the ground + MINI_CLOSE + try to unblock pucks OPEN_DOORS releasing pucks WAIT_FOR_CLOSE_ORDER @@ -23,8 +31,12 @@ States: closing the elevator's doors Events: - started - round had begun! + start + io power on! + jack_inserted_into_bot + jack into the bot + init_done + init done in_position elevator is in position new_puck @@ -35,12 +47,20 @@ Events: we received a new order doors_opened doors are opened - doors_closed - doors are closed + door_move_finished + doors move is done state_timeout IDLE: - started -> GO_TO_POS_X + start -> WAIT_JACK_IN + waiting for jack + +WAIT_JACK_IN: + jack_inserted_into_bot -> INIT + make initializations + +INIT: + init_done -> GO_TO_POS_X match begin, we're going to be ready to get a new puck GO_TO_POS_X: @@ -66,17 +86,27 @@ GO_TO_POS_Y: ready to release pucks at altitude Y WAIT_FOR_RELEASE_ORDER: - order_received -> OPEN_DOORS + order_received -> LAND_ELEVATOR + make the elevator touch the target zone + +LAND_ELEVATOR: + in_position -> OPEN_DOORS release pucks to the target position (I hope) OPEN_DOORS: doors_opened -> WAIT_FOR_CLOSE_ORDER wait for close order + door_move_finished -> MINI_CLOSE + try to unblock doors + +MINI_CLOSE: + door_move_finished -> OPEN_DOORS + try to release pucks again WAIT_FOR_CLOSE_ORDER: order_received -> CLOSE_DOORS closing doors CLOSE_DOORS: - doors_closed -> GO_TO_POS_X + door_move_finished -> GO_TO_POS_X pucks are released and elevator is ready to make a new column diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h index 0684559c..9e31f488 100644 --- a/digital/io/src/elevator.h +++ b/digital/io/src/elevator.h @@ -30,4 +30,28 @@ */ extern uint8_t elevator_is_ready; +/** + * nb puck in the elevator + */ +extern uint8_t nb_puck_in_elvt; + +/** + * elevator orders + */ +extern uint8_t elvt_order; +/** + * conversion stop/millimeter + */ + +#define STEP_BY_MM 72.34 + +/** + * pwm constant for elevator doors + */ + +#define OPEN_DOOR_PWM 200 +#define CLOSE_DOOR_PWM -200 +#define TIME_DOORS_PWM 0xcb +#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 d6cf1005..025f947c 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -25,38 +25,65 @@ #include "common.h" #include "fsm.h" #include "elevator_cb.h" +#include "elevator.h" +#include "asserv.h" +#include "pwm.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}; + +/* increase/decrease of pos y */ +#define MAJ_POSY 100 +#define MIN_POSY 50 /* - * OPEN_DOORS =doors_opened=> - * => WAIT_FOR_CLOSE_ORDER - * wait for close order + * IDLE =start=> + * => WAIT_JACK_IN + * waiting for jack */ fsm_branch_t -elevator__OPEN_DOORS__doors_opened (void) +elevator__IDLE__start (void) { - return elevator_next (OPEN_DOORS, doors_opened); + return elevator_next (IDLE, start); } /* - * WAIT_FOR_CLOSE_ORDER =order_received=> - * => CLOSE_DOORS - * closing doors + * WAIT_JACK_IN =jack_inserted_into_bot=> + * => INIT + * make initializations */ fsm_branch_t -elevator__WAIT_FOR_CLOSE_ORDER__order_received (void) +elevator__WAIT_JACK_IN__jack_inserted_into_bot (void) { - return elevator_next (WAIT_FOR_CLOSE_ORDER, order_received); + return elevator_next (WAIT_JACK_IN, jack_inserted_into_bot); } /* - * WAIT_A_PUCK =time_up=> - * => WAIT_POS_ORDER - * no more time to wait a new puck + * INIT =init_done=> + * => GO_TO_POS_X + * match begin, we're going to be ready to get a new puck */ fsm_branch_t -elevator__WAIT_A_PUCK__time_up (void) +elevator__INIT__init_done (void) { - return elevator_next (WAIT_A_PUCK, time_up); + asserv_move_elevator_absolute(posx[nb_puck_in_elvt], + ASSERV_ELVT_SPEED_DEFAULT); + elevator_is_ready = 0; + return elevator_next (INIT, init_done); +} + +/* + * GO_TO_POS_X =in_position=> + * => WAIT_A_PUCK + * in position and ready to get a new puck + */ +fsm_branch_t +elevator__GO_TO_POS_X__in_position (void) +{ + elevator_is_ready = 1; + return elevator_next (GO_TO_POS_X, in_position); } /* @@ -71,8 +98,36 @@ elevator__WAIT_A_PUCK__time_up (void) fsm_branch_t elevator__WAIT_A_PUCK__new_puck (void) { - return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks); - return elevator_next_branch (WAIT_A_PUCK, new_puck, not_ok_for_other_pucks); + // TODO time_ok + //if(nb_puck_in_elvt < 4 && (time_ok || !fb-empty)) + 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); +} + +/* + * WAIT_A_PUCK =time_up=> + * => WAIT_POS_ORDER + * no more time to wait a new puck + */ +fsm_branch_t +elevator__WAIT_A_PUCK__time_up (void) +{ + return elevator_next (WAIT_A_PUCK, time_up); +} + +/* + * WAIT_POS_ORDER =order_received=> + * => GO_TO_POS_Y + * go to position Y + */ +fsm_branch_t +elevator__WAIT_POS_ORDER__order_received (void) +{ + asserv_move_elevator_absolute(posy[elvt_order] + MAJ_POSY, + ASSERV_ELVT_SPEED_DEFAULT); + elvt_order = 0; + return elevator_next (WAIT_POS_ORDER, order_received); } /* @@ -87,58 +142,89 @@ elevator__GO_TO_POS_Y__in_position (void) } /* - * GO_TO_POS_X =in_position=> - * => WAIT_A_PUCK - * in position and ready to get a new puck + * WAIT_FOR_RELEASE_ORDER =order_received=> + * => LAND_ELEVATOR + * make the elevator touch the target zone */ fsm_branch_t -elevator__GO_TO_POS_X__in_position (void) +elevator__WAIT_FOR_RELEASE_ORDER__order_received (void) { - return elevator_next (GO_TO_POS_X, in_position); + asserv_move_elevator_absolute(posy[elvt_order] - MIN_POSY, + ASSERV_ELVT_SPEED_DEFAULT); + return elevator_next (WAIT_FOR_RELEASE_ORDER, order_received); } /* - * IDLE =started=> - * => GO_TO_POS_X - * match begin, we're going to be ready to get a new puck + * LAND_ELEVATOR =in_position=> + * => OPEN_DOORS + * release pucks to the target position (I hope) */ fsm_branch_t -elevator__IDLE__started (void) +elevator__LAND_ELEVATOR__in_position (void) { - return elevator_next (IDLE, started); + pwm_set(OPEN_DOOR_PWM, TIME_DOORS_PWM); + return elevator_next (LAND_ELEVATOR, in_position); } /* - * WAIT_FOR_RELEASE_ORDER =order_received=> + * MINI_CLOSE =door_move_finished=> * => OPEN_DOORS - * release pucks to the target position (I hope) + * try to release pucks again */ fsm_branch_t -elevator__WAIT_FOR_RELEASE_ORDER__order_received (void) +elevator__MINI_CLOSE__door_move_finished (void) { - return elevator_next (WAIT_FOR_RELEASE_ORDER, order_received); + pwm_set(OPEN_DOOR_PWM, TIME_DOORS_PWM); + return elevator_next (MINI_CLOSE, door_move_finished); } /* - * WAIT_POS_ORDER =order_received=> - * => GO_TO_POS_Y - * go to position Y + * OPEN_DOORS =doors_opened=> + * => WAIT_FOR_CLOSE_ORDER + * wait for close order */ fsm_branch_t -elevator__WAIT_POS_ORDER__order_received (void) +elevator__OPEN_DOORS__doors_opened (void) { - return elevator_next (WAIT_POS_ORDER, order_received); + nb_puck_in_elvt = 0; + return elevator_next (OPEN_DOORS, doors_opened); +} + +/* + * OPEN_DOORS =door_move_finished=> + * => MINI_CLOSE + * try to unblock doors + */ +fsm_branch_t +elevator__OPEN_DOORS__door_move_finished (void) +{ + pwm_set(CLOSE_DOOR_PWM, TIME_LIGHT_DOORS_PWM); + return elevator_next (OPEN_DOORS, door_move_finished); +} + +/* + * WAIT_FOR_CLOSE_ORDER =order_received=> + * => CLOSE_DOORS + * closing doors + */ +fsm_branch_t +elevator__WAIT_FOR_CLOSE_ORDER__order_received (void) +{ + pwm_set(CLOSE_DOOR_PWM, TIME_DOORS_PWM); + return elevator_next (WAIT_FOR_CLOSE_ORDER, order_received); } /* - * CLOSE_DOORS =doors_closed=> + * CLOSE_DOORS =door_move_finished=> * => GO_TO_POS_X * pucks are released and elevator is ready to make a new column */ fsm_branch_t -elevator__CLOSE_DOORS__doors_closed (void) +elevator__CLOSE_DOORS__door_move_finished (void) { - return elevator_next (CLOSE_DOORS, doors_closed); + asserv_move_elevator_absolute(posx[nb_puck_in_elvt], + ASSERV_ELVT_SPEED_DEFAULT); + return elevator_next (CLOSE_DOORS, door_move_finished); } -- cgit v1.2.3