summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src
diff options
context:
space:
mode:
authorNicolas Haller2009-05-20 23:19:28 +0200
committerNicolas Haller2009-05-20 23:19:28 +0200
commit1ccdcfb7c74143586c451b6777a9bc27ad0023d9 (patch)
treec22c77ce06c93ea189a111c1f885cdd11d126c89 /digital/io/src
parentdb059513199e3658722877c899c1257c4898384b (diff)
* digital/io/src:
- new cylinder FSM again again - setting top_puck_inside_bot on elevator FSM
Diffstat (limited to 'digital/io/src')
-rw-r--r--digital/io/src/asserv.c9
-rw-r--r--digital/io/src/asserv.h9
-rw-r--r--digital/io/src/cylinder.fsm82
-rw-r--r--digital/io/src/cylinder.h2
-rw-r--r--digital/io/src/cylinder_cb.c185
-rw-r--r--digital/io/src/elevator_cb.c6
-rw-r--r--digital/io/src/filterbridge.c2
7 files changed, 166 insertions, 129 deletions
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index d8fc2ea0..913ba412 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -340,6 +340,15 @@ asserv_arm_cmd_status (void)
return none;
}
+/* Is the optical fence see a puck? */
+uint8_t
+asserv_arm_of_status(void)
+{
+ if(asserv_status.input_port & _BV (5))
+ return 1;
+ return 0;
+}
+
/* Is last elevator class command has successfully ended? */
asserv_status_e
asserv_elevator_cmd_status (void)
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index 06be268b..09fb88f4 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -124,6 +124,15 @@ asserv_status_e
asserv_arm_cmd_status (void);
/**
+ *
+ * Is the optical fence see a puck?
+ * This function get the arm optical fence status
+ * @return the status of the arm optical fence
+ */
+uint8_t
+asserv_arm_of_status(void);
+
+/**
* Is last elevator class command has successfully ended?
* This function is used to know the status of the last elevator command. It
* looks at the status register.
diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm
index 3f883058..aefe1170 100644
--- a/digital/io/src/cylinder.fsm
+++ b/digital/io/src/cylinder.fsm
@@ -13,16 +13,12 @@ States:
initializing cylinder to a good position (open)
WAIT_A_PUCK
waiting a puck enter in cylinder
- TURN_PLUS_1
- turn 1 position after when we see a new puck
- TURN_MINUS_1
- turn 1 position before when the new puck dont really exist
WAIT_BRIDGE_READY
waiting fb is ready to accept new puck
- TURN_PLUS_1_AGAIN
- turn 1 position after to open cylinder after OF test
- TURN_MINUS_1_CLOSE
- turn 1 position before to close the cylinder (bot full/close_order)
+ TURN_PLUS_1_AND_OFO
+ turn 1+of_offset position after when we see a new puck
+ TURN_PLUS_1_AND_MINUS_OFO
+ turn 1-of_offset position after to be ready to get a new puck
WAIT_BOT_NOT_FULL
wait the bot to drop pucks
WAIT_CLEAR_ORDER
@@ -33,8 +29,10 @@ States:
open cylinder to prepare cyliner flush
WAIT_BRIDGE_READY_FLUSH
we wait fb is ready during the flush
- TURN_PLUS_2_FLUSH
+ TURN_PLUS_3_FLUSH
drop 1 puck to the fb
+ TURN_PLUS_1_CLOSE
+ close the cylinder
Events:
start
@@ -75,63 +73,55 @@ INIT_POS:
the cylinder is ready to get pucks
WAIT_A_PUCK:
- new_puck -> TURN_PLUS_1
- turn cylinder 1 position after
- close_order -> TURN_MINUS_1_CLOSE
+ new_puck -> WAIT_BRIDGE_READY
+ look if the bridge is ready before move
+ close_order -> TURN_PLUS_1_CLOSE
we close cylinder as requested
flush_order -> WAIT_BRIDGE_READY_FLUSH
flush all pucks to the bridge
-TURN_PLUS_1:
- move_done: of_no_puck -> TURN_MINUS_1
- There is no puck, go backward
- move_done: of_puck -> WAIT_BRIDGE_READY
- There is a real puck, check if fb is ready to get puck
-
-TURN_MINUS_1:
- move_done -> WAIT_A_PUCK
- wait a puck again
-
WAIT_BRIDGE_READY:
- bridge_ready -> TURN_PLUS_1_AGAIN
- open cylinder after the puck existence confirmation
+ bridge_ready -> TURN_PLUS_1_AND_OFO
+ open the cylinder with the puck or not.
-TURN_PLUS_1_AGAIN:
- move_done: bot_not_full -> WAIT_A_PUCK
- ready for a new puck
- move_done: bot_full -> TURN_MINUS_1_CLOSE
- bot full we close the cylinder
+TURN_PLUS_1_AND_OFO:
+ move_done: bot_not_full -> TURN_PLUS_1_AND_MINUS_OFO
+ open the cylinder to wait a new puck
+ move_done: bot_full -> WAIT_BOT_NOT_FULL
+ bot full, waiting for pucks teleportation
-TURN_MINUS_1_CLOSE:
- move_done -> WAIT_BOT_NOT_FULL
- we wait the bot is not full to reopen cylinder
+TURN_PLUS_1_AND_MINUS_OFO:
+ move_done -> WAIT_A_PUCK
+ ready for other pucks
WAIT_BOT_NOT_FULL:
bot_not_full -> WAIT_CLEAR_ORDER
- first condition to reopen bot is reached
+ the bot is not full, we go testing the other close condition
flush_order -> TURN_PLUS_1_FLUSH
- flush gordon
+ flush order received, go open the cylinder
WAIT_CLEAR_ORDER:
no_close_order -> TURN_PLUS_1_OPEN
- last condition to reopen bot reached, opening...
+ no close order, we reopen cylinder to get other pucks
flush_order -> TURN_PLUS_1_FLUSH
- flush gordon
+ flush order received, go open the cylinder
TURN_PLUS_1_OPEN:
move_done -> WAIT_A_PUCK
- we wait a puck
+ cylinder ready to get other pucks
TURN_PLUS_1_FLUSH:
move_done -> WAIT_BRIDGE_READY_FLUSH
- cylinder open and ready to flush
+ we wait the bridge before moving
WAIT_BRIDGE_READY_FLUSH:
- bridge_ready -> TURN_PLUS_2_FLUSH
- put a puck on the fb
-
-TURN_PLUS_2_FLUSH:
- move_done: cylinder_empty -> TURN_MINUS_1_CLOSE
- flush finished
- move_done: cylinder_not_empty -> WAIT_BRIDGE_READY_FLUSH
- flush again
+ bridge_ready -> TURN_PLUS_3_FLUSH
+ bridge is ready, flush gordon
+
+TURN_PLUS_3_FLUSH:
+ move_done -> WAIT_BOT_NOT_FULL
+ cylinder flushed, we test the 2 close conditions before reopen cylinder
+
+TURN_PLUS_1_CLOSE:
+ move_done -> WAIT_BOT_NOT_FULL
+ close order executed, test the 2 close conditions before reopen cylinder
diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h
index ad6d88b3..817b16ab 100644
--- a/digital/io/src/cylinder.h
+++ b/digital/io/src/cylinder.h
@@ -45,4 +45,6 @@ extern uint8_t cylinder_flush_order;
/* mecanical offset */
#define CYLINDER_OFFSET 0x43e
+/* mecanical offset for of probe */
+#define CYLINDER_OF_OFFSET 0.7
#endif /* cylinder_h */
diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c
index e01b9875..1b4cc28b 100644
--- a/digital/io/src/cylinder_cb.c
+++ b/digital/io/src/cylinder_cb.c
@@ -29,7 +29,13 @@
#include "cylinder.h"
#include "filterbridge.h"
#include "elevator.h"
+#include "top.h"
+/* locales variables */
+/* is there a puck on pos 2 or 3 */
+uint8_t puck_on_cylinder = 0;
+/* is the cylinder are in of_offset mode */
+uint8_t of_offset_enabled = 0;
/*
* IDLE =start=>
@@ -80,20 +86,18 @@ cylinder__INIT_POS__move_done (void)
/*
* WAIT_A_PUCK =new_puck=>
- * => TURN_PLUS_1
- * turn cylinder 1 position after
+ * => WAIT_BRIDGE_READY
+ * look if the bridge is ready before move
*/
fsm_branch_t
cylinder__WAIT_A_PUCK__new_puck (void)
{
- asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
- ASSERV_ARM_SPEED_DEFAULT);
return cylinder_next (WAIT_A_PUCK, new_puck);
}
/*
* WAIT_A_PUCK =close_order=>
- * => TURN_MINUS_1_CLOSE
+ * => TURN_PLUS_1_CLOSE
* we close cylinder as requested
*/
fsm_branch_t
@@ -116,84 +120,67 @@ cylinder__WAIT_A_PUCK__flush_order (void)
}
/*
- * TURN_PLUS_1 =move_done=>
- * of_no_puck => TURN_MINUS_1
- * There is no puck, go backward
- * of_puck => WAIT_BRIDGE_READY
- * There is a real puck, check if fb is ready to get puck
- */
-fsm_branch_t
-cylinder__TURN_PLUS_1__move_done (void)
-{
- if(cylinder_puck_on_of)
- {
- ++cylinder_nb_puck;
- return cylinder_next_branch (TURN_PLUS_1, move_done, of_puck);
- }
- return cylinder_next_branch (TURN_PLUS_1, move_done, of_no_puck);
-}
-
-/*
- * TURN_MINUS_1 =move_done=>
- * => WAIT_A_PUCK
- * wait a puck again
- */
-fsm_branch_t
-cylinder__TURN_MINUS_1__move_done (void)
-{
- return cylinder_next (TURN_MINUS_1, move_done);
-}
-
-/*
* WAIT_BRIDGE_READY =bridge_ready=>
- * => TURN_PLUS_1_AGAIN
- * open cylinder after the puck existence confirmation
+ * => TURN_PLUS_1_AND_OFO
+ * open the cylinder with the puck or not.
*/
fsm_branch_t
cylinder__WAIT_BRIDGE_READY__bridge_ready (void)
{
- asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
+ of_offset_enabled = 1;
+ asserv_move_arm((1+CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE,
ASSERV_ARM_SPEED_DEFAULT);
return cylinder_next (WAIT_BRIDGE_READY, bridge_ready);
}
/*
- * TURN_PLUS_1_AGAIN =move_done=>
- * bot_not_full => WAIT_A_PUCK
- * ready for a new puck
- * bot_full => TURN_MINUS_1_CLOSE
- * bot full we close the cylinder
+ * TURN_PLUS_1_AND_OFO =move_done=>
+ * bot_not_full => TURN_PLUS_1_AND_MINUS_OFO
+ * open the cylinder to wait a new puck
+ * bot_full => WAIT_BOT_NOT_FULL
+ * bot full, waiting for pucks teleportation
*/
fsm_branch_t
-cylinder__TURN_PLUS_1_AGAIN__move_done (void)
+cylinder__TURN_PLUS_1_AND_OFO__move_done (void)
{
- if(cylinder_nb_puck == 2)
+ /* we verify if we drop a puck to the bridge */
+ if(puck_on_cylinder)
{
--cylinder_nb_puck;
++fb_nb_puck;
}
- if(fb_nb_puck+ cylinder_nb_puck + elvt_nb_puck < 4)
- return cylinder_next_branch (TURN_PLUS_1_AGAIN, move_done, bot_not_full);
- asserv_move_arm(-1*60*ASSERV_ARM_STEP_BY_DEGREE,
- ASSERV_ARM_SPEED_DEFAULT);
- return cylinder_next_branch (TURN_PLUS_1_AGAIN, move_done, bot_full);
+ /* We probe the OF to see if we have a new puck */
+ puck_on_cylinder = asserv_arm_of_status();
+ if(puck_on_cylinder)
+ {
+ ++top_total_puck_taken;
+ ++top_puck_inside_bot;
+ }
+ if(top_puck_inside_bot < 4)
+ {
+ of_offset_enabled = 0;
+ asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
+ return cylinder_next_branch (TURN_PLUS_1_AND_OFO, move_done, bot_not_full);
+ }
+ return cylinder_next_branch (TURN_PLUS_1_AND_OFO, move_done, bot_full);
}
/*
- * TURN_MINUS_1_CLOSE =move_done=>
- * => WAIT_BOT_NOT_FULL
- * we wait the bot is not full to reopen cylinder
+ * TURN_PLUS_1_AND_MINUS_OFO =move_done=>
+ * => WAIT_A_PUCK
+ * ready for other pucks
*/
fsm_branch_t
-cylinder__TURN_MINUS_1_CLOSE__move_done (void)
+cylinder__TURN_PLUS_1_AND_MINUS_OFO__move_done (void)
{
- return cylinder_next (TURN_MINUS_1_CLOSE, move_done);
+ return cylinder_next (TURN_PLUS_1_AND_MINUS_OFO, move_done);
}
/*
* WAIT_BOT_NOT_FULL =bot_not_full=>
* => WAIT_CLEAR_ORDER
- * first condition to reopen bot is reached
+ * the bot is not full, we go testing the other close condition
*/
fsm_branch_t
cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void)
@@ -204,46 +191,67 @@ cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void)
/*
* WAIT_BOT_NOT_FULL =flush_order=>
* => TURN_PLUS_1_FLUSH
- * flush gordon
+ * flush order received, go open the cylinder
*/
fsm_branch_t
cylinder__WAIT_BOT_NOT_FULL__flush_order (void)
{
- asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
- ASSERV_ARM_SPEED_DEFAULT);
+ if(of_offset_enabled)
+ {
+ asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
+ of_offset_enabled = 0;
+ }
+ else
+ asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
return cylinder_next (WAIT_BOT_NOT_FULL, flush_order);
}
/*
* WAIT_CLEAR_ORDER =no_close_order=>
* => TURN_PLUS_1_OPEN
- * last condition to reopen bot reached, opening...
+ * no close order, we reopen cylinder to get other pucks
*/
fsm_branch_t
cylinder__WAIT_CLEAR_ORDER__no_close_order (void)
{
- asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
- ASSERV_ARM_SPEED_DEFAULT);
+ if(of_offset_enabled)
+ {
+ asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
+ of_offset_enabled = 0;
+ }
+ else
+ asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
return cylinder_next (WAIT_CLEAR_ORDER, no_close_order);
}
/*
* WAIT_CLEAR_ORDER =flush_order=>
* => TURN_PLUS_1_FLUSH
- * flush gordon
+ * flush order received, go open the cylinder
*/
fsm_branch_t
cylinder__WAIT_CLEAR_ORDER__flush_order (void)
{
- asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
- ASSERV_ARM_SPEED_DEFAULT);
+ if(of_offset_enabled)
+ {
+ asserv_move_arm((1-CYLINDER_OF_OFFSET)*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
+ of_offset_enabled = 0;
+ }
+ else
+ asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
return cylinder_next (WAIT_CLEAR_ORDER, flush_order);
}
/*
* TURN_PLUS_1_OPEN =move_done=>
* => WAIT_A_PUCK
- * we wait a puck
+ * cylinder ready to get other pucks
*/
fsm_branch_t
cylinder__TURN_PLUS_1_OPEN__move_done (void)
@@ -254,7 +262,7 @@ cylinder__TURN_PLUS_1_OPEN__move_done (void)
/*
* TURN_PLUS_1_FLUSH =move_done=>
* => WAIT_BRIDGE_READY_FLUSH
- * cylinder open and ready to flush
+ * we wait the bridge before moving
*/
fsm_branch_t
cylinder__TURN_PLUS_1_FLUSH__move_done (void)
@@ -264,30 +272,47 @@ cylinder__TURN_PLUS_1_FLUSH__move_done (void)
/*
* WAIT_BRIDGE_READY_FLUSH =bridge_ready=>
- * => TURN_PLUS_2_FLUSH
- * put a puck on the fb
+ * => TURN_PLUS_3_FLUSH
+ * bridge is ready, flush gordon
*/
fsm_branch_t
cylinder__WAIT_BRIDGE_READY_FLUSH__bridge_ready (void)
{
+ asserv_move_arm(3*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
return cylinder_next (WAIT_BRIDGE_READY_FLUSH, bridge_ready);
}
/*
- * TURN_PLUS_2_FLUSH =move_done=>
- * cylinder_empty => TURN_MINUS_1_CLOSE
- * flush finished
- * cylinder_not_empty => WAIT_BRIDGE_READY_FLUSH
- * flush again
+ * TURN_PLUS_3_FLUSH =move_done=>
+ * => WAIT_BOT_NOT_FULL
+ * cylinder flushed, we test the 2 close conditions before reopen cylinder
*/
fsm_branch_t
-cylinder__TURN_PLUS_2_FLUSH__move_done (void)
+cylinder__TURN_PLUS_3_FLUSH__move_done (void)
{
- --cylinder_nb_puck;
- ++fb_nb_puck;
- if(!cylinder_nb_puck)
- return cylinder_next_branch (TURN_PLUS_2_FLUSH, move_done, cylinder_empty);
- return cylinder_next_branch (TURN_PLUS_2_FLUSH, move_done, cylinder_not_empty);
+ if(puck_on_cylinder)
+ {
+ --cylinder_nb_puck;
+ ++fb_nb_puck;
+ puck_on_cylinder = 0;
+ }
+ return cylinder_next (TURN_PLUS_3_FLUSH, move_done);
}
-
+/*
+ * TURN_PLUS_1_CLOSE =move_done=>
+ * => WAIT_BOT_NOT_FULL
+ * close order executed, test the 2 close conditions before reopen cylinder
+ */
+fsm_branch_t
+cylinder__TURN_PLUS_1_CLOSE__move_done (void)
+{
+ if(puck_on_cylinder)
+ {
+ --cylinder_nb_puck;
+ ++fb_nb_puck;
+ puck_on_cylinder = 0;
+ }
+ return cylinder_next (TURN_PLUS_1_CLOSE, move_done);
+}
diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c
index f6e538a4..0c32f922 100644
--- a/digital/io/src/elevator_cb.c
+++ b/digital/io/src/elevator_cb.c
@@ -30,6 +30,7 @@
#include "pwm.h"
#include "chrono.h"
#include "filterbridge.h"
+#include "top.h"
/* Positions when waiting a puck*/
uint16_t posx[4] =
@@ -166,7 +167,7 @@ elevator__WAIT_A_PUCK__time_up (void)
fsm_branch_t
elevator__WAIT_POS_ORDER__order_received (void)
{
- asserv_move_elevator_absolute(posy[elvt_order] + MAJ_POSY,
+ asserv_move_elevator_absolute(posy[elvt_order + 1] + MAJ_POSY,
ASSERV_ELVT_SPEED_DEFAULT);
elvt_order = 0;
return elevator_next (WAIT_POS_ORDER, order_received);
@@ -191,7 +192,7 @@ elevator__GO_TO_POS_Y__in_position (void)
fsm_branch_t
elevator__WAIT_FOR_RELEASE_ORDER__order_received (void)
{
- asserv_move_elevator_absolute(posy[elvt_order] - MIN_POSY,
+ asserv_move_elevator_absolute(posy[elvt_order + 1] - MIN_POSY,
ASSERV_ELVT_SPEED_DEFAULT);
elvt_order = 0;
return elevator_next (WAIT_FOR_RELEASE_ORDER, order_received);
@@ -230,6 +231,7 @@ fsm_branch_t
elevator__OPEN_DOORS__doors_opened (void)
{
nb_puck_elvt = 0;
+ top_puck_inside_bot = 0;
return elevator_next (OPEN_DOORS, doors_opened);
}
diff --git a/digital/io/src/filterbridge.c b/digital/io/src/filterbridge.c
index c318c124..64fcbbfc 100644
--- a/digital/io/src/filterbridge.c
+++ b/digital/io/src/filterbridge.c
@@ -24,7 +24,7 @@
* }}} */
#include "filterbridge.h"
-uint8_t fb_nb_puck;
+uint8_t fb_nb_puck = 0;
/* +AutoDec */
/* -AutoDec */