summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/cylinder.c6
-rw-r--r--digital/io/src/cylinder.fsm51
-rw-r--r--digital/io/src/cylinder.h6
-rw-r--r--digital/io/src/cylinder_cb.c126
-rw-r--r--digital/io/src/main.c7
5 files changed, 196 insertions, 0 deletions
diff --git a/digital/io/src/cylinder.c b/digital/io/src/cylinder.c
index 4dd0e590..62904f4a 100644
--- a/digital/io/src/cylinder.c
+++ b/digital/io/src/cylinder.c
@@ -38,4 +38,10 @@ uint8_t cylinder_close_order = 0;
/* flush puck on the cylinder */
uint8_t cylinder_flush_order = 0;
+/* distributor mode switch */
+uint8_t cylinder_distributor_mode = 0;
+
+/* distributor fucked */
+uint8_t cylinder_distributor_fucked = 0;
+
/* cylinder order */
diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm
index aefe1170..4b2fd49f 100644
--- a/digital/io/src/cylinder.fsm
+++ b/digital/io/src/cylinder.fsm
@@ -33,6 +33,19 @@ States:
drop 1 puck to the fb
TURN_PLUS_1_CLOSE
close the cylinder
+ WAIT_DISTRIB_FUCKED
+ waiting for the bot are really on the distributor
+ WAIT_BRIDGE_READY_DISTRIB
+ waiting for the bridge
+ TURN_PLUS_1_AND_OFO_DISTRIB
+ get a puck on distrib
+ TURN_PLUS_1_AND_MINUS_OFO_DISTRIB
+ we neutralize the offset before return in normal mode
+ TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP
+ blah
+ PROBE_OF
+ we probe the OF
+
Events:
start
@@ -53,6 +66,14 @@ Events:
the bot dont have 4 pucks
flush_order
we receive a flush order
+ approching_distributor
+ we go in distributor mode
+ distrib_fucked
+ we are really in the distributor
+ of_puck
+ we see a puck to the of
+ of_no_puck
+ we don't see a puck in of
state_timeout
useless here
@@ -79,6 +100,32 @@ WAIT_A_PUCK:
we close cylinder as requested
flush_order -> WAIT_BRIDGE_READY_FLUSH
flush all pucks to the bridge
+ approching_distributor -> WAIT_DISTRIB_FUCKED
+ We wait a distributor
+
+WAIT_DISTRIB_FUCKED:
+ distrib_fucked -> WAIT_BRIDGE_READY_DISTRIB
+ We are on a distributor
+
+WAIT_BRIDGE_READY_DISTRIB:
+ bridge_ready -> TURN_PLUS_1_AND_OFO_DISTRIB
+ we turn to check the of
+
+TURN_PLUS_1_AND_OFO_DISTRIB:
+ move_done -> PROBE_OF
+ We probe the puck
+
+PROBE_OF:
+ of_puck: bot_full -> WAIT_BOT_NOT_FULL
+ return to normal mode with a bot full of puck
+ of_puck: bot_not_full -> TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP
+ we get a puck and we are hungry yet, go eat an another puck
+ of_no_puck -> TURN_PLUS_1_AND_MINUS_OFO_DISTRIB
+ Distributor empty, go elsewhere
+
+TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP:
+ move_done -> WAIT_BRIDGE_READY_DISTRIB
+ ready to get another puck
WAIT_BRIDGE_READY:
bridge_ready -> TURN_PLUS_1_AND_OFO
@@ -90,6 +137,10 @@ TURN_PLUS_1_AND_OFO:
move_done: bot_full -> WAIT_BOT_NOT_FULL
bot full, waiting for pucks teleportation
+TURN_PLUS_1_AND_MINUS_OFO_DISTRIB:
+ move_done -> WAIT_A_PUCK
+ bridge empty, return to normal mode
+
TURN_PLUS_1_AND_MINUS_OFO:
move_done -> WAIT_A_PUCK
ready for other pucks
diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h
index 817b16ab..78a65e7f 100644
--- a/digital/io/src/cylinder.h
+++ b/digital/io/src/cylinder.h
@@ -42,6 +42,12 @@ extern uint8_t cylinder_close_order;
/* flush puck on the cylinder */
extern uint8_t cylinder_flush_order;
+/* distributor mode switch */
+extern uint8_t cylinder_distributor_mode;
+
+/* distributor fucked */
+extern uint8_t cylinder_distributor_fucked;
+
/* mecanical offset */
#define CYLINDER_OFFSET 0x43e
diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c
index 5694d95d..d2c31f6a 100644
--- a/digital/io/src/cylinder_cb.c
+++ b/digital/io/src/cylinder_cb.c
@@ -92,6 +92,8 @@ cylinder__INIT_POS__move_done (void)
fsm_branch_t
cylinder__WAIT_A_PUCK__new_puck (void)
{
+ cylinder_distributor_mode = 0;
+ cylinder_distributor_fucked = 0;
return cylinder_next (WAIT_A_PUCK, new_puck);
}
@@ -103,6 +105,8 @@ cylinder__WAIT_A_PUCK__new_puck (void)
fsm_branch_t
cylinder__WAIT_A_PUCK__close_order (void)
{
+ cylinder_distributor_mode = 0;
+ cylinder_distributor_fucked = 0;
asserv_move_arm(-1*60*ASSERV_ARM_STEP_BY_DEGREE,
ASSERV_ARM_SPEED_DEFAULT);
return cylinder_next (WAIT_A_PUCK, close_order);
@@ -116,10 +120,25 @@ cylinder__WAIT_A_PUCK__close_order (void)
fsm_branch_t
cylinder__WAIT_A_PUCK__flush_order (void)
{
+ cylinder_distributor_mode = 0;
+ cylinder_distributor_fucked = 0;
return cylinder_next (WAIT_A_PUCK, flush_order);
}
/*
+ * WAIT_A_PUCK =approching_distributor=>
+ * => WAIT_DISTRIB_FUCKED
+ * We wait a distributor
+ */
+fsm_branch_t
+cylinder__WAIT_A_PUCK__approching_distributor (void)
+{
+ cylinder_distributor_mode = 0;
+ cylinder_distributor_fucked = 0;
+ return cylinder_next (WAIT_A_PUCK, approching_distributor);
+}
+
+/*
* WAIT_BRIDGE_READY =bridge_ready=>
* => TURN_PLUS_1_AND_OFO
* open the cylinder with the puck or not.
@@ -186,6 +205,8 @@ cylinder__TURN_PLUS_1_AND_MINUS_OFO__move_done (void)
fsm_branch_t
cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void)
{
+ cylinder_distributor_mode = 0;
+ cylinder_distributor_fucked = 0;
return cylinder_next (WAIT_BOT_NOT_FULL, bot_not_full);
}
@@ -197,6 +218,8 @@ cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void)
fsm_branch_t
cylinder__WAIT_BOT_NOT_FULL__flush_order (void)
{
+ cylinder_distributor_mode = 0;
+ cylinder_distributor_fucked = 0;
if(of_offset_enabled)
{
asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE,
@@ -318,3 +341,106 @@ cylinder__TURN_PLUS_1_CLOSE__move_done (void)
}
return cylinder_next (TURN_PLUS_1_CLOSE, move_done);
}
+
+/*
+ * WAIT_DISTRIB_FUCKED =distrib_fucked=>
+ * => WAIT_BRIDGE_READY_DISTRIB
+ * We are on a distributor
+ */
+fsm_branch_t
+cylinder__WAIT_DISTRIB_FUCKED__distrib_fucked (void)
+{
+ return cylinder_next (WAIT_DISTRIB_FUCKED, distrib_fucked);
+}
+
+/*
+ * WAIT_BRIDGE_READY_DISTRIB =bridge_ready=>
+ * => TURN_PLUS_1_AND_OFO_DISTRIB
+ * we turn to check the of
+ */
+fsm_branch_t
+cylinder__WAIT_BRIDGE_READY_DISTRIB__bridge_ready (void)
+{
+ asserv_move_arm((1+CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
+ of_offset_enabled = 1;
+ return cylinder_next (WAIT_BRIDGE_READY_DISTRIB, bridge_ready);
+}
+
+/*
+ * TURN_PLUS_1_AND_OFO_DISTRIB =move_done=>
+ * => PROBE_OF
+ * We probe the puck
+ */
+fsm_branch_t
+cylinder__TURN_PLUS_1_AND_OFO_DISTRIB__move_done (void)
+{
+ if(puck_on_cylinder)
+ {
+ --cylinder_nb_puck;
+ ++fb_nb_puck;
+ puck_on_cylinder = 0;
+ }
+ return cylinder_next (TURN_PLUS_1_AND_OFO_DISTRIB, move_done);
+}
+
+/*
+ * TURN_PLUS_1_AND_MINUS_OFO_DISTRIB =move_done=>
+ * => WAIT_A_PUCK
+ * bridge empty, return to normal mode
+ */
+fsm_branch_t
+cylinder__TURN_PLUS_1_AND_MINUS_OFO_DISTRIB__move_done (void)
+{
+ return cylinder_next (TURN_PLUS_1_AND_MINUS_OFO_DISTRIB, move_done);
+}
+
+/*
+ * TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP =move_done=>
+ * => WAIT_BRIDGE_READY_DISTRIB
+ * ready to get another puck
+ */
+fsm_branch_t
+cylinder__TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP__move_done (void)
+{
+ return cylinder_next (TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP, move_done);
+}
+
+/*
+ * PROBE_OF =of_puck=>
+ * bot_full => WAIT_BOT_NOT_FULL
+ * return to normal mode with a bot full of puck
+ * bot_not_full => TURN_PLUS_1_AND_MINUS_OFO_DISTRIB_LOOP
+ * we get a puck and we are hungry yet, go eat an another puck
+ */
+fsm_branch_t
+cylinder__PROBE_OF__of_puck (void)
+{
+ /* we have a new puck on cylinder */
+ puck_on_cylinder =1;
+ ++cylinder_nb_puck;
+ ++top_total_puck_taken;
+ ++top_puck_inside_bot;
+ if(top_puck_inside_bot < 4)
+ {
+ asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
+ of_offset_enabled = 0;
+ return cylinder_next_branch (PROBE_OF, of_puck, bot_not_full);
+ }
+ return cylinder_next_branch (PROBE_OF, of_puck, bot_full);
+}
+
+/*
+ * PROBE_OF =of_no_puck=>
+ * => TURN_PLUS_1_AND_MINUS_OFO_DISTRIB
+ * Distributor empty, go elsewhere
+ */
+fsm_branch_t
+cylinder__PROBE_OF__of_no_puck (void)
+{
+ asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
+ of_offset_enabled = 0;
+ return cylinder_next (PROBE_OF, of_no_puck);
+}
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index d7c8a870..75b318dd 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -244,6 +244,13 @@ main_event_to_fsm (void)
FSM_HANDLE_EVENT (&elevator_fsm,
ELEVATOR_EVENT_order_bypass);
+ if(cylinder_distributor_mode)
+ FSM_HANDLE_EVENT (&cylinder_fsm,
+ CYLINDER_EVENT_approching_distributor);
+
+ if(cylinder_distributor_fucked)
+ FSM_HANDLE_EVENT (&cylinder_fsm,
+ CYLINDER_EVENT_distrib_fucked);
/* bot empty */
if(top_puck_inside_bot < 4)
FSM_HANDLE_EVENT (&cylinder_fsm,