summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Haller2009-05-19 01:32:30 +0200
committerNicolas Haller2009-05-19 01:32:30 +0200
commit23f4603cf160b07376f10b705a84d1cb823e2c46 (patch)
treea7b40d015e810fdcfc49c22fe4e0b831ba799dbc /digital
parentf416af1eeed92e9508744544e39983af82479965 (diff)
* digital/io/src:
- fixes on cylinder fsm(init offset, grafcet fix)
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/cylinder.fsm20
-rw-r--r--digital/io/src/cylinder.h3
-rw-r--r--digital/io/src/cylinder_cb.c31
-rw-r--r--digital/io/src/main.c12
4 files changed, 50 insertions, 16 deletions
diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm
index 2fcf06c4..0d6f5e23 100644
--- a/digital/io/src/cylinder.fsm
+++ b/digital/io/src/cylinder.fsm
@@ -7,7 +7,9 @@ States:
waiting for init
WAIT_FOR_JACK_IN
waiting for jack
- INIT
+ RESET_POS
+ reset the cylinder position
+ INIT_POS
initializing cylinder to a good position (open)
WAIT_A_PUCK
waiting a puck enter in cylinder
@@ -17,8 +19,8 @@ States:
waiting for the bridge is ready
TURN_PLUS_1
turn cylinder to 1 position after
- TURN_PLUS_1_LOOP
- turn cylinder to 1 position after
+ TURN_PLUS_2
+ turn cylinder to 2 position after
TURN_MINUS_1
turn cylinder to 1 position before to prevent new puck enter in the robot
WAIT_FOR_PUCKS_RELEASE
@@ -46,10 +48,14 @@ IDLE:
we wait the jack before moving anything
WAIT_FOR_JACK_IN:
- jack_inserted_into_bot -> INIT
+ jack_inserted_into_bot -> RESET_POS
we init the cylinder position
-INIT:
+RESET_POS:
+ move_done -> INIT_POS
+ move the cylinder to open it
+
+INIT_POS:
move_done -> WAIT_A_PUCK
the cylinder is ready to get pucks
@@ -64,10 +70,10 @@ TURN_PLUS_3:
WAIT_FOR_BRIDGE_READY:
bridge_ready: no_puck_bo -> TURN_PLUS_1
bridge clear, and no other puck on cylinder
- bridge_ready: puck_bo -> TURN_PLUS_1_LOOP
+ bridge_ready: puck_bo -> TURN_PLUS_2
bridge clear, an another puck is in the cylinder
-TURN_PLUS_1_LOOP:
+TURN_PLUS_2:
move_done: ok_for_other_puck -> WAIT_FOR_BRIDGE_READY
we test bo again
move_done: not_ok_for_other_puck -> WAIT_FOR_PUCKS_RELEASE
diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h
index f6d7ff58..d7162353 100644
--- a/digital/io/src/cylinder.h
+++ b/digital/io/src/cylinder.h
@@ -33,4 +33,7 @@ extern uint8_t of_state;
/* nb puck in cylinder */
extern uint8_t nb_puck_cylinder;
+/* mecanical offset */
+#define CYLINDER_OFFSET -0x43e
+
#endif /* cylinder_h */
diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c
index fa207959..2e3159ea 100644
--- a/digital/io/src/cylinder_cb.c
+++ b/digital/io/src/cylinder_cb.c
@@ -43,7 +43,7 @@ cylinder__IDLE__start (void)
/*
* WAIT_FOR_JACK_IN =jack_inserted_into_bot=>
- * => INIT
+ * => RESET_POS
* we init the cylinder position
*/
fsm_branch_t
@@ -54,14 +54,27 @@ cylinder__WAIT_FOR_JACK_IN__jack_inserted_into_bot (void)
}
/*
- * INIT =move_done=>
+ * RESET_POS =move_done=>
+ * => INIT_POS
+ * move the cylinder to open it
+ */
+fsm_branch_t
+cylinder__RESET_POS__move_done (void)
+{
+ asserv_move_arm(CYLINDER_OFFSET,
+ ASSERV_ARM_SPEED_DEFAULT);
+ return cylinder_next (RESET_POS, move_done);
+}
+
+/*
+ * INIT_POS =move_done=>
* => WAIT_A_PUCK
* the cylinder is ready to get pucks
*/
fsm_branch_t
-cylinder__INIT__move_done (void)
+cylinder__INIT_POS__move_done (void)
{
- return cylinder_next (INIT, move_done);
+ return cylinder_next (INIT_POS, move_done);
}
/*
@@ -93,7 +106,7 @@ cylinder__TURN_PLUS_3__move_done (void)
* WAIT_FOR_BRIDGE_READY =bridge_ready=>
* no_puck_bo => TURN_PLUS_1
* bridge clear, and no other puck on cylinder
- * puck_bo => TURN_PLUS_1_LOOP
+ * puck_bo => TURN_PLUS_2
* bridge clear, an another puck is in the cylinder
*/
fsm_branch_t
@@ -128,18 +141,18 @@ cylinder__TURN_PLUS_1__move_done (void)
}
/*
- * TURN_PLUS_1_LOOP =move_done=>
+ * TURN_PLUS_2 =move_done=>
* ok_for_other_puck => WAIT_FOR_BRIDGE_READY
* we test bo again
* not_ok_for_other_puck => WAIT_FOR_PUCKS_RELEASE
* bot full, eject hypothetical puck and close the cylinder
*/
fsm_branch_t
-cylinder__TURN_PLUS_1_LOOP__move_done (void)
+cylinder__TURN_PLUS_2__move_done (void)
{
if(nb_puck_fb < 4)
- return cylinder_next_branch (TURN_PLUS_1_LOOP, move_done, ok_for_other_puck);
- return cylinder_next_branch (TURN_PLUS_1_LOOP, move_done, not_ok_for_other_puck);
+ return cylinder_next_branch (TURN_PLUS_2, move_done, ok_for_other_puck);
+ return cylinder_next_branch (TURN_PLUS_2, move_done, not_ok_for_other_puck);
}
/*
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index e4455844..9219c633 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -214,6 +214,18 @@ main_event_to_fsm (void)
{
/* TODO: */
}
+
+ /* Check cylinder status */
+ if (arm_status == success)
+ {
+ FSM_HANDLE_EVENT (&cylinder_fsm,
+ CYLINDER_EVENT_move_done);
+ }
+ else if (elevator_status == failure)
+ {
+ /* TODO: */
+ }
+
/* FIXME: use general setting ack. */
/* send event if elevator received an order */
if(elvt_order)