From a14c2c58d9132b444e7db5e2278d84f585444148 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Fri, 22 May 2009 02:51:46 +0200 Subject: * digital/io/src: - fix of_probe on distributor mode (cylinder fsm) - improve pucks detection between fb and elevator --- digital/io/src/elevator.fsm | 15 ++++++++++++++- digital/io/src/elevator_cb.c | 43 ++++++++++++++++++++++++++++++++++++++++--- digital/io/src/main.c | 11 ++++++++++- digital/io/src/top.fsm | 2 +- 4 files changed, 65 insertions(+), 6 deletions(-) diff --git a/digital/io/src/elevator.fsm b/digital/io/src/elevator.fsm index cc815fb6..c844f88c 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_BRIDGE_EMPTY[timeout=1200] + waiting bridge empty WAIT_POS_ORDER waiting for position order (depending where we want to drop our column) GO_TO_POS_Y @@ -42,6 +44,8 @@ Events: we received a new puck from FB (filter bridge, not facebook) order_bypass Dont wait to have 4 pucks but we wait we don't have puck before bridge + bridge_empty + the bridge is empty order_received we received a new order doors_opened @@ -74,9 +78,18 @@ 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 - order_bypass -> GO_TO_POS_Y + order_bypass -> WAIT_BRIDGE_EMPTY elevator filling has been shut, get ready to drop pucks +WAIT_BRIDGE_EMPTY: + bridge_empty -> GO_TO_POS_Y + bridge is empty, we are happy, we drop the column + state_timeout -> GO_TO_POS_Y + we suppose bridge is empty and we said it to the fsm + new_puck -> WAIT_A_PUCK + We have a new puck, elevator must move, we go to WAIT_A_PUCK + + WAIT_POS_ORDER: order_received -> GO_TO_POS_Y go to position Y diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index d47d286f..c0cf26c3 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -145,17 +145,54 @@ elevator__WAIT_A_PUCK__new_puck (void) /* * WAIT_A_PUCK =order_bypass=> - * => GO_TO_POS_Y + * => WAIT_BRIDGE_EMPTY * elevator filling has been shut, get ready to drop pucks */ fsm_branch_t elevator__WAIT_A_PUCK__order_bypass (void) { - elvt_new_puck = 0; + return elevator_next (WAIT_A_PUCK, order_bypass); +} + +/* + * WAIT_BRIDGE_EMPTY =bridge_empty=> + * => GO_TO_POS_Y + * bridge is empty, we are happy, we drop the column + */ +fsm_branch_t +elevator__WAIT_BRIDGE_EMPTY__bridge_empty (void) +{ elvt_is_ready = 0; asserv_move_elevator_absolute(posy[elvt_order - 1] - MAJ_POSY, ASSERV_ELVT_SPEED_DEFAULT); - return elevator_next (WAIT_A_PUCK, order_bypass); + return elevator_next (WAIT_BRIDGE_EMPTY, bridge_empty); +} + +/* + * WAIT_BRIDGE_EMPTY =state_timeout=> + * => GO_TO_POS_Y + * we suppose bridge is empty and we said it to the fsm + */ +fsm_branch_t +elevator__WAIT_BRIDGE_EMPTY__state_timeout (void) +{ + elvt_is_ready = 0; + /* XXX bridge is empty but he don't know it yet */ + fb_nb_puck = 0; + asserv_move_elevator_absolute(posy[elvt_order - 1] - MAJ_POSY, + ASSERV_ELVT_SPEED_DEFAULT); + return elevator_next (WAIT_BRIDGE_EMPTY, state_timeout); +} + +/* + * WAIT_BRIDGE_EMPTY =new_puck=> + * => WAIT_A_PUCK + * We have a new puck, elevator must move, we go to WAIT_A_PUCK + */ +fsm_branch_t +elevator__WAIT_BRIDGE_EMPTY__new_puck (void) +{ + return elevator_next (WAIT_BRIDGE_EMPTY, new_puck); } /* diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 869d124e..23d70b5a 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -217,6 +217,15 @@ main_event_to_fsm (void) CYLINDER_EVENT_move_done); } + /* check of status */ + if (asserv_arm_of_status()) + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_of_puck); + else + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_of_no_puck); + + /* FIXME: use general setting ack. */ /* send event if elevator received an order */ if (elvt_order) @@ -240,7 +249,7 @@ main_event_to_fsm (void) if(fb_nb_puck < 2) FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_bridge_ready); - if(!fb_nb_puck && elvt_order) + if(elvt_order) FSM_HANDLE_EVENT (&elevator_fsm, ELEVATOR_EVENT_order_bypass); diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 8cee0840..b48fb7be 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -19,7 +19,7 @@ States: when going to unload area. FUCK_UNLOAD_AREA when going backward in position for unload. - UNLOAD_PUCKS [timeout=1125] + UNLOAD_PUCKS [timeout=112500] when unloading all pucks from the bot. ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD [timeout=1125] wait for the elevator to be ready for a bot movement. -- cgit v1.2.3