summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/cylinder.fsm57
-rw-r--r--digital/io/src/cylinder.h2
-rw-r--r--digital/io/src/cylinder_cb.c104
-rw-r--r--digital/io/src/elevator_cb.c8
-rw-r--r--digital/io/src/main.c6
5 files changed, 28 insertions, 149 deletions
diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm
index 0d6f5e23..09b381c3 100644
--- a/digital/io/src/cylinder.fsm
+++ b/digital/io/src/cylinder.fsm
@@ -13,20 +13,8 @@ States:
initializing cylinder to a good position (open)
WAIT_A_PUCK
waiting a puck enter in cylinder
- TURN_PLUS_3
- turn cylinder to 3 positions after
- WAIT_FOR_BRIDGE_READY
- waiting for the bridge is ready
- TURN_PLUS_1
- 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
- waiting for pucks release from the lift
- TURN_PLUS_1_AGAIN
- open the puck entrance
+ TURN_PLUS_4
+ turn cylinder 4 position after
Events:
start
@@ -37,10 +25,6 @@ Events:
there is a puck in position 1
move_done
cylinder move finished
- bridge_ready
- bridge ready to get a new puck
- bot_empty
- all pucks released
state_timeout
IDLE:
@@ -60,38 +44,9 @@ INIT_POS:
the cylinder is ready to get pucks
WAIT_A_PUCK:
- new_puck -> TURN_PLUS_3
- turn cylinder 3 position after
+ new_puck -> TURN_PLUS_4
+ turn cylinder 4 position after
-TURN_PLUS_3:
- move_done -> WAIT_FOR_BRIDGE_READY
- bridge full, waiting for Bison Fute's clearance
-
-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_2
- bridge clear, an another puck is in the cylinder
-
-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
- bot full, eject hypothetical puck and close the cylinder
-
-TURN_PLUS_1:
- move_done: ok_for_other_puck -> WAIT_A_PUCK
- cylinder empty and ready for new puck
- move_done: not_ok_for_other_puck -> TURN_MINUS_1
- bot full, closing cylinder
-
-TURN_MINUS_1:
- move_done -> WAIT_FOR_PUCKS_RELEASE
- cylinder close, you shall not pass (but try again later)
-
-WAIT_FOR_PUCKS_RELEASE:
- bot_empty -> TURN_PLUS_1_AGAIN
-
-TURN_PLUS_1_AGAIN:
+TURN_PLUS_4:
move_done -> WAIT_A_PUCK
- cylinder ready
+ wait a puck again
diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h
index d7162353..b095f5a2 100644
--- a/digital/io/src/cylinder.h
+++ b/digital/io/src/cylinder.h
@@ -34,6 +34,6 @@ extern uint8_t of_state;
extern uint8_t nb_puck_cylinder;
/* mecanical offset */
-#define CYLINDER_OFFSET -0x43e
+#define CYLINDER_OFFSET 0x43e
#endif /* cylinder_h */
diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c
index 2e3159ea..461230ee 100644
--- a/digital/io/src/cylinder_cb.c
+++ b/digital/io/src/cylinder_cb.c
@@ -79,114 +79,28 @@ cylinder__INIT_POS__move_done (void)
/*
* WAIT_A_PUCK =new_puck=>
- * => TURN_PLUS_3
- * turn cylinder 3 position after
+ * => TURN_PLUS_4
+ * turn cylinder 4 position after
*/
fsm_branch_t
cylinder__WAIT_A_PUCK__new_puck (void)
{
- ++nb_puck_cylinder;
- asserv_move_arm(3*60*ASSERV_ARM_STEP_BY_DEGREE,
- ASSERV_ARM_SPEED_DEFAULT);
- return cylinder_next (WAIT_A_PUCK, new_puck);
-}
-
-/*
- * TURN_PLUS_3 =move_done=>
- * => WAIT_FOR_BRIDGE_READY
- * bridge full, waiting for Bison Fute's clearance
- */
-fsm_branch_t
-cylinder__TURN_PLUS_3__move_done (void)
-{
- return cylinder_next (TURN_PLUS_3, move_done);
-}
-
-/*
- * WAIT_FOR_BRIDGE_READY =bridge_ready=>
- * no_puck_bo => TURN_PLUS_1
- * bridge clear, and no other puck on cylinder
- * puck_bo => TURN_PLUS_2
- * bridge clear, an another puck is in the cylinder
- */
-fsm_branch_t
-cylinder__WAIT_FOR_BRIDGE_READY__bridge_ready (void)
-{
+ /* ++nb_puck_cylinder; */
++nb_puck_fb;
- asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
- ASSERV_ARM_SPEED_DEFAULT);
- if(!of_state)
- {
- --nb_puck_cylinder;
- return cylinder_next_branch (WAIT_FOR_BRIDGE_READY, bridge_ready, no_puck_bo);
- }
- return cylinder_next_branch (WAIT_FOR_BRIDGE_READY, bridge_ready, puck_bo);
-}
-
-/*
- * TURN_PLUS_1 =move_done=>
- * ok_for_other_puck => WAIT_A_PUCK
- * cylinder empty and ready for new puck
- * not_ok_for_other_puck => TURN_MINUS_1
- * bot full, closing cylinder
- */
-fsm_branch_t
-cylinder__TURN_PLUS_1__move_done (void)
-{
- if(nb_puck_fb < 4)
- return cylinder_next_branch (TURN_PLUS_1, move_done, ok_for_other_puck);
- asserv_move_arm(-1*60*ASSERV_ARM_STEP_BY_DEGREE,
+ asserv_move_arm(4*60*ASSERV_ARM_STEP_BY_DEGREE,
ASSERV_ARM_SPEED_DEFAULT);
- return cylinder_next_branch (TURN_PLUS_1, move_done, not_ok_for_other_puck);
-}
-
-/*
- * 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_2__move_done (void)
-{
- if(nb_puck_fb < 4)
- 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);
-}
-
-/*
- * TURN_MINUS_1 =move_done=>
- * => WAIT_FOR_PUCKS_RELEASE
- * cylinder close, you shall not pass (but try again later)
- */
-fsm_branch_t
-cylinder__TURN_MINUS_1__move_done (void)
-{
- return cylinder_next (TURN_MINUS_1, move_done);
-}
-
-/*
- * WAIT_FOR_PUCKS_RELEASE =bot_empty=>
- * => TURN_PLUS_1_AGAIN
- */
-fsm_branch_t
-cylinder__WAIT_FOR_PUCKS_RELEASE__bot_empty (void)
-{
- asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
- ASSERV_ARM_SPEED_DEFAULT);
- return cylinder_next (WAIT_FOR_PUCKS_RELEASE, bot_empty);
+ return cylinder_next (WAIT_A_PUCK, new_puck);
}
/*
- * TURN_PLUS_1_AGAIN =move_done=>
+ * TURN_PLUS_4 =move_done=>
* => WAIT_A_PUCK
- * cylinder ready
+ * wait a puck again
*/
fsm_branch_t
-cylinder__TURN_PLUS_1_AGAIN__move_done (void)
+cylinder__TURN_PLUS_4__move_done (void)
{
- return cylinder_next (TURN_PLUS_1_AGAIN, move_done);
+ return cylinder_next (TURN_PLUS_4, move_done);
}
diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c
index 5ef5be28..0a3ea967 100644
--- a/digital/io/src/elevator_cb.c
+++ b/digital/io/src/elevator_cb.c
@@ -113,10 +113,16 @@ elevator__WAIT_A_PUCK__new_puck (void)
elevator_is_ready = 0;
elvt_new_puck = 0;
// TODO time_ok
- if(nb_puck_elvt < 4 &&
+ if(nb_puck_elvt < 4)
+ {
+/* &&
((chrono_remaining_time() - OK_TIME_LIMIT > 0)
|| nb_puck_fb != 0))
+ */
+ asserv_move_elevator_absolute(posx[nb_puck_in_elvt],
+ ASSERV_ELVT_SPEED_DEFAULT);
return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks);
+ }
else
return elevator_next_branch (WAIT_A_PUCK, new_puck, not_ok_for_other_pucks);
}
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 2e2c4f60..980448c4 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -213,7 +213,7 @@ main_event_to_fsm (void)
}
/* Check cylinder status */
- if (arm_status == success)
+ if (arm_status != none)
{
FSM_HANDLE_EVENT (&cylinder_fsm,
CYLINDER_EVENT_move_done);
@@ -240,13 +240,17 @@ main_event_to_fsm (void)
FSM_HANDLE_EVENT (&elevator_fsm,
ELEVATOR_EVENT_doors_opened);
/* bridge ready */
+ /*
if(nb_puck_fb < 2)
FSM_HANDLE_EVENT (&cylinder_fsm,
CYLINDER_EVENT_bridge_ready);
+ */
/* bot empty */
+ /*
if(!nb_puck_fb && !nb_puck_in_elvt && !nb_puck_cylinder)
FSM_HANDLE_EVENT (&cylinder_fsm,
CYLINDER_EVENT_bot_empty);
+ */
/* Jack */
if(switch_get_jack())