From 1f89ecb260b2915db08412c315d697c1eb7dd6d6 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Thu, 21 May 2009 06:20:34 +0200 Subject: * digital/io/src: - add dropping pucks command before we have 4 pucks in elevator - fix top_puck_inside_bot assignation on elevator fsm --- digital/io/src/elevator.fsm | 14 ++++++++++---- digital/io/src/elevator_cb.c | 33 +++++++++++++++++++++++---------- digital/io/src/filterbridge.c | 1 + digital/io/src/filterbridge.h | 3 +++ digital/io/src/filterbridge_cb.c | 3 +++ digital/io/src/main.c | 4 ++++ 6 files changed, 44 insertions(+), 14 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/elevator.fsm b/digital/io/src/elevator.fsm index d9388690..8befad77 100644 --- a/digital/io/src/elevator.fsm +++ b/digital/io/src/elevator.fsm @@ -13,6 +13,8 @@ States: go to a position to be ready to get a new puck WAIT_A_PUCK waiting for a new puck (hurry up filter bridge!) + WAIT_FB_IDLE + waiting for the fb is idle WAIT_POS_ORDER waiting for position order (depending where we want to drop our column) GO_TO_POS_Y @@ -40,8 +42,8 @@ Events: elevator is in position new_puck we received a new puck from FB (filter bridge, not facebook) - time_up - complex event (!time_ok . fb_empty) + fb_idle + filterbridge idle order_received we received a new order doors_opened @@ -74,8 +76,12 @@ WAIT_A_PUCK: new_puck: not_ok_for_other_pucks -> WAIT_POS_ORDER incrementing nb_puck var no more time to wait a new puck - time_up -> WAIT_POS_ORDER - no more time to wait a new puck + order_received -> WAIT_FB_IDLE + elevator filling has been shut, get ready to drop pucks + +WAIT_FB_IDLE: + fb_idle -> GO_TO_POS_Y + execute order WAIT_POS_ORDER: order_received -> GO_TO_POS_Y diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index c37ef69c..caef50c2 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -144,16 +144,29 @@ 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 + * WAIT_A_PUCK =order_received=> + * => WAIT_FB_IDLE + * elevator filling has been shut, get ready to drop pucks */ fsm_branch_t -elevator__WAIT_A_PUCK__time_up (void) +elevator__WAIT_A_PUCK__order_received (void) { elvt_is_ready = 0; - return elevator_next (WAIT_A_PUCK, time_up); + return elevator_next (WAIT_A_PUCK, order_received); +} + +/* + * WAIT_FB_IDLE =fb_idle=> + * => GO_TO_POS_Y + * execute order + */ +fsm_branch_t +elevator__WAIT_FB_IDLE__fb_idle (void) +{ + elvt_new_puck = 0; + asserv_move_elevator_absolute(posy[elvt_order - 1] - MAJ_POSY, + ASSERV_ELVT_SPEED_DEFAULT); + return elevator_next (WAIT_FB_IDLE, fb_idle); } /* @@ -166,7 +179,6 @@ elevator__WAIT_POS_ORDER__order_received (void) { 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); } @@ -178,6 +190,7 @@ elevator__WAIT_POS_ORDER__order_received (void) fsm_branch_t elevator__GO_TO_POS_Y__in_position (void) { + elvt_order = 0; return elevator_next (GO_TO_POS_Y, in_position); } @@ -191,7 +204,6 @@ elevator__WAIT_FOR_RELEASE_ORDER__order_received (void) { 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); } @@ -203,6 +215,7 @@ elevator__WAIT_FOR_RELEASE_ORDER__order_received (void) fsm_branch_t elevator__LAND_ELEVATOR__in_position (void) { + elvt_order = 0; pwm_set(OPEN_DOOR_PWM, 0); return elevator_next (LAND_ELEVATOR, in_position); } @@ -227,8 +240,8 @@ elevator__MINI_CLOSE__state_timeout (void) fsm_branch_t elevator__OPEN_DOORS__doors_opened (void) { + top_puck_inside_bot -= elvt_nb_puck; elvt_nb_puck = 0; - top_puck_inside_bot = 0; pwm_set(0,0); return elevator_next (OPEN_DOORS, doors_opened); } @@ -254,7 +267,6 @@ fsm_branch_t elevator__WAIT_FOR_CLOSE_ORDER__order_received (void) { pwm_set(CLOSE_DOOR_PWM, 0); - elvt_order = 0; return elevator_next (WAIT_FOR_CLOSE_ORDER, order_received); } @@ -266,6 +278,7 @@ elevator__WAIT_FOR_CLOSE_ORDER__order_received (void) fsm_branch_t elevator__CLOSE_DOORS__state_timeout (void) { + elvt_order = 0; pwm_set(0,0); asserv_move_elevator_absolute(posx[elvt_nb_puck], ASSERV_ELVT_SPEED_DEFAULT); diff --git a/digital/io/src/filterbridge.c b/digital/io/src/filterbridge.c index 64fcbbfc..f61cd27d 100644 --- a/digital/io/src/filterbridge.c +++ b/digital/io/src/filterbridge.c @@ -25,6 +25,7 @@ #include "filterbridge.h" uint8_t fb_nb_puck = 0; +uint8_t fb_work_in_progress = 0; /* +AutoDec */ /* -AutoDec */ diff --git a/digital/io/src/filterbridge.h b/digital/io/src/filterbridge.h index 235d29ee..49f3819c 100644 --- a/digital/io/src/filterbridge.h +++ b/digital/io/src/filterbridge.h @@ -29,4 +29,7 @@ extern uint8_t fb_nb_puck; +/* Is servo moving ? */ +extern uint8_t fb_work_in_progress; + #endif // filterbridge_h diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c index 9393a702..f8db2e2c 100644 --- a/digital/io/src/filterbridge_cb.c +++ b/digital/io/src/filterbridge_cb.c @@ -49,6 +49,7 @@ filterbridge__IDLE__start (void) fsm_branch_t filterbridge__WAIT_JACK_IN__jack_inserted_into_bot (void) { + fb_work_in_progress = 1; servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE); servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE); return filterbridge_next (WAIT_JACK_IN, jack_inserted_into_bot); @@ -62,6 +63,7 @@ filterbridge__WAIT_JACK_IN__jack_inserted_into_bot (void) fsm_branch_t filterbridge__CLOSE_DOOR__state_timeout (void) { + fb_work_in_progress = 0; return filterbridge_next (CLOSE_DOOR, state_timeout); } @@ -91,6 +93,7 @@ filterbridge__WAIT_A_PUCK__puck_on_pos2 (void) fsm_branch_t filterbridge__MARCEL_WAIT__state_timeout (void) { + fb_work_in_progress = 1; servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_OPEN); return filterbridge_next (MARCEL_WAIT, state_timeout); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 2c11ba89..39df3357 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -240,6 +240,10 @@ main_event_to_fsm (void) if(fb_nb_puck < 2) FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_bridge_ready); + if(!fb_work_in_progress) + FSM_HANDLE_EVENT (&elevator_fsm, + ELEVATOR_EVENT_fb_idle); + /* bot empty */ if(top_puck_inside_bot < 4) FSM_HANDLE_EVENT (&cylinder_fsm, -- cgit v1.2.3