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_cb.c | 43 ++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 40 insertions(+), 3 deletions(-) (limited to 'digital/io/src/elevator_cb.c') 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); } /* -- cgit v1.2.3