summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Haller2009-05-20 16:24:19 +0200
committerNicolas Haller2009-05-20 16:24:19 +0200
commit266966d7fd9e90a636948e931c8d7a6777114b91 (patch)
tree725a0abfd90f4873d3b4a9e5922d5d5c8ea49aa1
parentdff7bab8c28790986d5c16ab6f1894a5f3cd8680 (diff)
* digital/io/src:
- new cylinder fsm - fsm variable prefixed by fsm name
-rw-r--r--digital/io/src/cylinder.c14
-rw-r--r--digital/io/src/cylinder.fsm95
-rw-r--r--digital/io/src/cylinder.h13
-rw-r--r--digital/io/src/cylinder_cb.c203
-rw-r--r--digital/io/src/elevator.c13
-rw-r--r--digital/io/src/elevator.h4
-rw-r--r--digital/io/src/elevator_cb.c15
-rw-r--r--digital/io/src/filterbridge.c2
-rw-r--r--digital/io/src/filterbridge.h2
-rw-r--r--digital/io/src/filterbridge_cb.c6
-rw-r--r--digital/io/src/main.c20
11 files changed, 344 insertions, 43 deletions
diff --git a/digital/io/src/cylinder.c b/digital/io/src/cylinder.c
index f533e425..4dd0e590 100644
--- a/digital/io/src/cylinder.c
+++ b/digital/io/src/cylinder.c
@@ -25,7 +25,17 @@
#include "cylinder.h"
/* last state of optical fence */
-uint8_t of_state = 0;
+uint8_t cylinder_of_state = 0;
/* nb puck in cylinder */
-uint8_t nb_puck_cylinder = 0;
+uint8_t cylinder_nb_puck = 0;
+
+uint8_t cylinder_puck_on_of = 0;
+
+/* force cylinder to be closed */
+uint8_t cylinder_close_order = 0;
+
+/* flush puck on the cylinder */
+uint8_t cylinder_flush_order = 0;
+
+/* cylinder order */
diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm
index 09b381c3..3f883058 100644
--- a/digital/io/src/cylinder.fsm
+++ b/digital/io/src/cylinder.fsm
@@ -13,8 +13,28 @@ States:
initializing cylinder to a good position (open)
WAIT_A_PUCK
waiting a puck enter in cylinder
- TURN_PLUS_4
- turn cylinder 4 position after
+ 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)
+ WAIT_BOT_NOT_FULL
+ wait the bot to drop pucks
+ WAIT_CLEAR_ORDER
+ wait the clear order (unset close_order flags)
+ TURN_PLUS_1_OPEN
+ open cylinder after close (bot full/close_order)
+ TURN_PLUS_1_FLUSH
+ open cylinder to prepare cyliner flush
+ WAIT_BRIDGE_READY_FLUSH
+ we wait fb is ready during the flush
+ TURN_PLUS_2_FLUSH
+ drop 1 puck to the fb
Events:
start
@@ -25,7 +45,18 @@ Events:
there is a puck in position 1
move_done
cylinder move finished
+ bridge_ready
+ the fb is ready to get new pucks
+ close_order
+ we receive the order to close the cylinder
+ no_close_order
+ we can reopen the cylinder
+ bot_not_full
+ the bot dont have 4 pucks
+ flush_order
+ we receive a flush order
state_timeout
+ useless here
IDLE:
start -> WAIT_FOR_JACK_IN
@@ -44,9 +75,63 @@ INIT_POS:
the cylinder is ready to get pucks
WAIT_A_PUCK:
- new_puck -> TURN_PLUS_4
- turn cylinder 4 position after
+ new_puck -> TURN_PLUS_1
+ turn cylinder 1 position after
+ close_order -> TURN_MINUS_1_CLOSE
+ we close cylinder as requested
+ flush_order -> WAIT_BRIDGE_READY_FLUSH
+ flush all pucks to the bridge
-TURN_PLUS_4:
+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
+
+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_MINUS_1_CLOSE:
+ move_done -> WAIT_BOT_NOT_FULL
+ we wait the bot is not full to reopen cylinder
+
+WAIT_BOT_NOT_FULL:
+ bot_not_full -> WAIT_CLEAR_ORDER
+ first condition to reopen bot is reached
+ flush_order -> TURN_PLUS_1_FLUSH
+ flush gordon
+
+WAIT_CLEAR_ORDER:
+ no_close_order -> TURN_PLUS_1_OPEN
+ last condition to reopen bot reached, opening...
+ flush_order -> TURN_PLUS_1_FLUSH
+ flush gordon
+
+TURN_PLUS_1_OPEN:
+ move_done -> WAIT_A_PUCK
+ we wait a puck
+
+TURN_PLUS_1_FLUSH:
+ move_done -> WAIT_BRIDGE_READY_FLUSH
+ cylinder open and ready to flush
+
+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
diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h
index b095f5a2..ad6d88b3 100644
--- a/digital/io/src/cylinder.h
+++ b/digital/io/src/cylinder.h
@@ -28,10 +28,19 @@
#include "common.h"
/* last state of optical fence */
-extern uint8_t of_state;
+extern uint8_t cylinder_of_state;
/* nb puck in cylinder */
-extern uint8_t nb_puck_cylinder;
+extern uint8_t cylinder_nb_puck;
+
+/* of state */
+extern uint8_t cylinder_puck_on_of;
+
+/* force cylinder to be closed */
+extern uint8_t cylinder_close_order;
+
+/* flush puck on the cylinder */
+extern uint8_t cylinder_flush_order;
/* mecanical offset */
#define CYLINDER_OFFSET 0x43e
diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c
index 461230ee..e01b9875 100644
--- a/digital/io/src/cylinder_cb.c
+++ b/digital/io/src/cylinder_cb.c
@@ -28,6 +28,7 @@
#include "asserv.h"
#include "cylinder.h"
#include "filterbridge.h"
+#include "elevator.h"
/*
@@ -79,28 +80,214 @@ cylinder__INIT_POS__move_done (void)
/*
* WAIT_A_PUCK =new_puck=>
- * => TURN_PLUS_4
- * turn cylinder 4 position after
+ * => TURN_PLUS_1
+ * turn cylinder 1 position after
*/
fsm_branch_t
cylinder__WAIT_A_PUCK__new_puck (void)
{
- /* ++nb_puck_cylinder; */
- ++nb_puck_fb;
- asserv_move_arm(4*60*ASSERV_ARM_STEP_BY_DEGREE,
+ asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
ASSERV_ARM_SPEED_DEFAULT);
return cylinder_next (WAIT_A_PUCK, new_puck);
}
/*
- * TURN_PLUS_4 =move_done=>
+ * WAIT_A_PUCK =close_order=>
+ * => TURN_MINUS_1_CLOSE
+ * we close cylinder as requested
+ */
+fsm_branch_t
+cylinder__WAIT_A_PUCK__close_order (void)
+{
+ asserv_move_arm(-1*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
+ return cylinder_next (WAIT_A_PUCK, close_order);
+}
+
+/*
+ * WAIT_A_PUCK =flush_order=>
+ * => WAIT_BRIDGE_READY_FLUSH
+ * flush all pucks to the bridge
+ */
+fsm_branch_t
+cylinder__WAIT_A_PUCK__flush_order (void)
+{
+ return cylinder_next (WAIT_A_PUCK, flush_order);
+}
+
+/*
+ * 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_PLUS_4__move_done (void)
+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
+ */
+fsm_branch_t
+cylinder__WAIT_BRIDGE_READY__bridge_ready (void)
+{
+ asserv_move_arm(1*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
+ */
+fsm_branch_t
+cylinder__TURN_PLUS_1_AGAIN__move_done (void)
+{
+ if(cylinder_nb_puck == 2)
+ {
+ --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);
+}
+
+/*
+ * TURN_MINUS_1_CLOSE =move_done=>
+ * => WAIT_BOT_NOT_FULL
+ * we wait the bot is not full to reopen cylinder
+ */
+fsm_branch_t
+cylinder__TURN_MINUS_1_CLOSE__move_done (void)
+{
+ return cylinder_next (TURN_MINUS_1_CLOSE, move_done);
+}
+
+/*
+ * WAIT_BOT_NOT_FULL =bot_not_full=>
+ * => WAIT_CLEAR_ORDER
+ * first condition to reopen bot is reached
+ */
+fsm_branch_t
+cylinder__WAIT_BOT_NOT_FULL__bot_not_full (void)
+{
+ return cylinder_next (WAIT_BOT_NOT_FULL, bot_not_full);
+}
+
+/*
+ * WAIT_BOT_NOT_FULL =flush_order=>
+ * => TURN_PLUS_1_FLUSH
+ * flush gordon
+ */
+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);
+ 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...
+ */
+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);
+ return cylinder_next (WAIT_CLEAR_ORDER, no_close_order);
+}
+
+/*
+ * WAIT_CLEAR_ORDER =flush_order=>
+ * => TURN_PLUS_1_FLUSH
+ * flush gordon
+ */
+fsm_branch_t
+cylinder__WAIT_CLEAR_ORDER__flush_order (void)
+{
+ 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
+ */
+fsm_branch_t
+cylinder__TURN_PLUS_1_OPEN__move_done (void)
+{
+ return cylinder_next (TURN_PLUS_1_OPEN, move_done);
+}
+
+/*
+ * TURN_PLUS_1_FLUSH =move_done=>
+ * => WAIT_BRIDGE_READY_FLUSH
+ * cylinder open and ready to flush
+ */
+fsm_branch_t
+cylinder__TURN_PLUS_1_FLUSH__move_done (void)
+{
+ return cylinder_next (TURN_PLUS_1_FLUSH, move_done);
+}
+
+/*
+ * WAIT_BRIDGE_READY_FLUSH =bridge_ready=>
+ * => TURN_PLUS_2_FLUSH
+ * put a puck on the fb
+ */
+fsm_branch_t
+cylinder__WAIT_BRIDGE_READY_FLUSH__bridge_ready (void)
+{
+ 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
+ */
+fsm_branch_t
+cylinder__TURN_PLUS_2_FLUSH__move_done (void)
{
- return cylinder_next (TURN_PLUS_4, move_done);
+ --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);
}
diff --git a/digital/io/src/elevator.c b/digital/io/src/elevator.c
index d89ff7a1..6319501f 100644
--- a/digital/io/src/elevator.c
+++ b/digital/io/src/elevator.c
@@ -28,17 +28,22 @@
/**
* State of the elevator
*/
-uint8_t elevator_is_ready = 0;
-uint8_t nb_puck_in_elvt = 0;
+uint8_t elvt_is_ready = 0;
+
/**
- * elevator orders
+ * nb puck in the elevator
*/
-uint8_t elvt_order;
+uint8_t elvt_nb_puck = 0;
+
/**
* new_puck information (from filterbridge)
*/
uint8_t elvt_new_puck = 0;
+/**
+ * elevator orders
+ */
+uint8_t elvt_order = 0;
/* +AutoDec */
/* -AutoDec */
diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h
index 49cbd184..6d131861 100644
--- a/digital/io/src/elevator.h
+++ b/digital/io/src/elevator.h
@@ -28,12 +28,12 @@
/**
* State of the elevator
*/
-extern uint8_t elevator_is_ready;
+extern uint8_t elvt_is_ready;
/**
* nb puck in the elevator
*/
-extern uint8_t nb_puck_in_elvt;
+extern uint8_t elvt_nb_puck;
/**
* new_puck information (from filterbridge)
diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c
index fe73a717..f6e538a4 100644
--- a/digital/io/src/elevator_cb.c
+++ b/digital/io/src/elevator_cb.c
@@ -94,7 +94,7 @@ fsm_branch_t
elevator__INIT__doors_opened (void)
{
/* FIXME: why this is here? */
- elevator_is_ready = 0;
+ elvt_is_ready = 0;
/* Close the door. */
pwm_set (CLOSE_DOOR_PWM, TIME_DOORS_PWM);
return elevator_next (INIT, doors_opened);
@@ -109,9 +109,9 @@ fsm_branch_t
elevator__GO_TO_POS_X__in_position (void)
{
/* FIXME: this sucks, look at elevator.fsm for a real fix. */
- elevator_is_ready = 1;
+ elvt_is_ready = 1;
/* move to first position. */
- asserv_move_elevator_absolute(posx[nb_puck_in_elvt],
+ asserv_move_elevator_absolute(posx[nb_puck_elvt],
ASSERV_ELVT_SPEED_DEFAULT);
return elevator_next (GO_TO_POS_X, in_position);
}
@@ -128,7 +128,7 @@ elevator__GO_TO_POS_X__in_position (void)
fsm_branch_t
elevator__WAIT_A_PUCK__new_puck (void)
{
- elevator_is_ready = 0;
+ elvt_is_ready = 0;
elvt_new_puck = 0;
// TODO time_ok
if(++nb_puck_elvt < 4)
@@ -137,7 +137,7 @@ elevator__WAIT_A_PUCK__new_puck (void)
((chrono_remaining_time() - OK_TIME_LIMIT > 0)
|| nb_puck_fb != 0))
*/
- asserv_move_elevator_absolute(posx[nb_puck_in_elvt],
+ asserv_move_elevator_absolute(posx[nb_puck_elvt],
ASSERV_ELVT_SPEED_DEFAULT);
return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks);
}
@@ -154,7 +154,7 @@ elevator__WAIT_A_PUCK__new_puck (void)
fsm_branch_t
elevator__WAIT_A_PUCK__time_up (void)
{
- elevator_is_ready = 0;
+ elvt_is_ready = 0;
return elevator_next (WAIT_A_PUCK, time_up);
}
@@ -230,7 +230,6 @@ fsm_branch_t
elevator__OPEN_DOORS__doors_opened (void)
{
nb_puck_elvt = 0;
- nb_puck_in_elvt = 0;
return elevator_next (OPEN_DOORS, doors_opened);
}
@@ -266,7 +265,7 @@ elevator__WAIT_FOR_CLOSE_ORDER__order_received (void)
fsm_branch_t
elevator__CLOSE_DOORS__door_move_finished (void)
{
- asserv_move_elevator_absolute(posx[nb_puck_in_elvt],
+ asserv_move_elevator_absolute(posx[nb_puck_elvt],
ASSERV_ELVT_SPEED_DEFAULT);
return elevator_next (CLOSE_DOORS, door_move_finished);
}
diff --git a/digital/io/src/filterbridge.c b/digital/io/src/filterbridge.c
index 0b5001cc..c318c124 100644
--- a/digital/io/src/filterbridge.c
+++ b/digital/io/src/filterbridge.c
@@ -24,7 +24,7 @@
* }}} */
#include "filterbridge.h"
-uint8_t nb_puck_fb;
+uint8_t fb_nb_puck;
/* +AutoDec */
/* -AutoDec */
diff --git a/digital/io/src/filterbridge.h b/digital/io/src/filterbridge.h
index 81fdcc60..235d29ee 100644
--- a/digital/io/src/filterbridge.h
+++ b/digital/io/src/filterbridge.h
@@ -27,6 +27,6 @@
#include "common.h"
-extern uint8_t nb_puck_fb;
+extern uint8_t fb_nb_puck;
#endif // filterbridge_h
diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c
index 3f571f66..9393a702 100644
--- a/digital/io/src/filterbridge_cb.c
+++ b/digital/io/src/filterbridge_cb.c
@@ -75,7 +75,7 @@ filterbridge__CLOSE_DOOR__state_timeout (void)
fsm_branch_t
filterbridge__WAIT_A_PUCK__puck_on_pos2 (void)
{
- if(elevator_is_ready)
+ if(elvt_is_ready)
{
return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, lift_ready);
}
@@ -127,8 +127,8 @@ filterbridge__PUSH_PUCK__no_puck_on_pos2 (void)
fsm_branch_t
filterbridge__TEMPO_ELVETATOR_COMMUNICATION__state_timeout (void)
{
- ++nb_puck_in_elvt;
- --nb_puck_fb;
+ ++elvt_nb_puck;
+ --fb_nb_puck;
elvt_new_puck = 1;
servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE);
servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE);
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 7d38d1dd..c84c7ca6 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -240,17 +240,23 @@ main_event_to_fsm (void)
FSM_HANDLE_EVENT (&elevator_fsm,
ELEVATOR_EVENT_doors_opened);
/* bridge ready */
- /*
- if(nb_puck_fb < 2)
+ if(fb_nb_puck < 2)
FSM_HANDLE_EVENT (&cylinder_fsm,
CYLINDER_EVENT_bridge_ready);
- */
/* bot empty */
- /*
- if(!nb_puck_fb && !nb_puck_in_elvt && !nb_puck_cylinder)
+ if(fb_nb_puck + elvt_nb_puck + cylinder_nb_puck < 4)
FSM_HANDLE_EVENT (&cylinder_fsm,
- CYLINDER_EVENT_bot_empty);
- */
+ CYLINDER_EVENT_bot_not_full);
+
+ /* cylinder_close_order */
+ if(cylinder_close_order)
+ FSM_HANDLE_EVENT (&cylinder_fsm,
+ CYLINDER_EVENT_close_order);
+
+ /* cylinder__flush_order */
+ if(cylinder_flush_order)
+ FSM_HANDLE_EVENT (&cylinder_fsm,
+ CYLINDER_EVENT_flush_order);
/* Jack */
if(switch_get_jack())