summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Haller2009-05-22 02:51:46 +0200
committerNicolas Haller2009-05-22 02:51:46 +0200
commita14c2c58d9132b444e7db5e2278d84f585444148 (patch)
treeda73c42f78834a23824000e6841f33d260760b17
parenta671973979b86e1bc19d2d3adc69c691ff96255f (diff)
* digital/io/src:
- fix of_probe on distributor mode (cylinder fsm) - improve pucks detection between fb and elevator
-rw-r--r--digital/io/src/elevator.fsm15
-rw-r--r--digital/io/src/elevator_cb.c43
-rw-r--r--digital/io/src/main.c11
-rw-r--r--digital/io/src/top.fsm2
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.