summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Haller2009-05-09 00:10:04 +0200
committerNicolas Haller2009-05-09 00:10:04 +0200
commitb76073669f43dca5e10cc1127d0818a05859058f (patch)
tree7d454c3b946b687861e44c7d035a5c03256a560c /digital
parent1901265a3ea59267f92fe2e923184fce3d689f72 (diff)
* digital/io/src
- modification on filterbridge FSM due to meca changes - implementing filterbridge FSM - fix init FSM to add the timeout feature
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/aquajim.h11
-rw-r--r--digital/io/src/elevator.c6
-rw-r--r--digital/io/src/elevator.h5
-rw-r--r--digital/io/src/filterbridge.fsm75
-rw-r--r--digital/io/src/filterbridge_cb.c89
-rw-r--r--digital/io/src/init.fsm1
-rw-r--r--digital/io/src/main.c3
7 files changed, 101 insertions, 89 deletions
diff --git a/digital/io/src/aquajim.h b/digital/io/src/aquajim.h
index 6abc0f6b..53d06567 100644
--- a/digital/io/src/aquajim.h
+++ b/digital/io/src/aquajim.h
@@ -34,4 +34,15 @@
*/
#define MATCH_DURATION_MS 90000
+/**
+ * filterbridge servo
+ */
+
+#define SERVO_FINGER_ID 0
+#define SERVO_DOOR_ID 1
+#define SERVO_FINGER_IDLE 0
+#define SERVO_FINGER_PUSHING 1
+#define SERVO_DOOR_OPEN 0
+#define SERVO_DOOR_CLOSE 1
+
#endif /* aquajim_h */
diff --git a/digital/io/src/elevator.c b/digital/io/src/elevator.c
index 8da3b6c7..3a4a81ee 100644
--- a/digital/io/src/elevator.c
+++ b/digital/io/src/elevator.c
@@ -22,8 +22,14 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
+#include "common.h"
#include "elevator.h"
+/**
+ * State of the elevator
+ */
+uint8_t elevator_is_ready;
+
/* +AutoDec */
/* -AutoDec */
diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h
index 3e8c9065..0684559c 100644
--- a/digital/io/src/elevator.h
+++ b/digital/io/src/elevator.h
@@ -25,4 +25,9 @@
*
* }}} */
+/**
+ * State of the elevator
+ */
+extern uint8_t elevator_is_ready;
+
#endif // elevator_h
diff --git a/digital/io/src/filterbridge.fsm b/digital/io/src/filterbridge.fsm
index 97bcfa54..2a87ab05 100644
--- a/digital/io/src/filterbridge.fsm
+++ b/digital/io/src/filterbridge.fsm
@@ -5,66 +5,51 @@ filterbridge
States:
IDLE
waiting the lift is ready to accept new puck
+ WAIT_JACK_IN
+ waiting for the jack to be inserted into the bot
+ CLOSE_DOOR[timeout=225]
+ close access to the lift (and wait 1 sec)
WAIT_A_PUCK
waiting for a new puck
- WAIT_RGB_PROBE
- waiting for RGB probe the first puck
- EJECT_PUCK
- eject a bad puck
- RETURN_NORMAL_POS
- move the bridge to his normal position
- OPEN_DOOR
+ OPEN_DOOR[timeout=225]
release puck to the lift and block a new puck on pos1
- CLOSE_DOOR
- release pos1 puck and close access to the lift
+ PUSH_PUCK
+ push the puck to the lift
Events:
- lift_ready
- lift ready to accept new puck
+ start
+ robot start
+ jack_inserted_into_bot
+ jack into the bot
puck_on_pos2
a puck has been detected on bridge's position 2
no_puck_on_pos2
there is no puck on position 2
- bridge_in_position
- bridge is in his normal position
- door_opened
- puck 1 blocked and puck 2 release to the lift
- ejection_done
- the ejection procedure is completed
- color_probed
- the result of the investigation will be revealed (suspens!!)
- door_closed
- ready to test a new puck
state_timeout
+ timeout used to wait the end of servo move
IDLE:
- lift_ready -> WAIT_A_PUCK
- the lift is ready to get pucks, we can begin testing procedure
+ start -> WAIT_JACK_IN
+ we wait for the jack
-WAIT_A_PUCK:
- puck_on_pos2: too_much_puck -> EJECT_PUCK
- we have too much puck, eject the puck quickly and whistle
- puck_on_pos2: nb_puck_ok -> WAIT_RGB_PROBE
- probe color of the new puck
-
-WAIT_RGB_PROBE:
- color_probed: bad_color -> EJECT_PUCK
- eject bad puck
- color_probed: good_color -> OPEN_DOOR
- put puck to the lift
+WAIT_JACK_IN:
+ jack_inserted_into_bot -> CLOSE_DOOR
+ we init the bridge
-EJECT_PUCK:
- ejection_done -> RETURN_NORMAL_POS
- put bridge on normal position after puck ejection
+CLOSE_DOOR:
+ state_timeout -> WAIT_A_PUCK
+ we are ready, we wait a puck
-RETURN_NORMAL_POS:
- bridge_in_position -> CLOSE_DOOR
- make bridge ready to test a new puck
+WAIT_A_PUCK:
+ puck_on_pos2: lift_not_ready -> .
+ Lift not ready, we stand by
+ puck_on_pos2: lift_ready -> OPEN_DOOR
+ Lift ok, we deliver
OPEN_DOOR:
- no_puck_on_pos2 -> CLOSE_DOOR
- release puck to the lift
+ state_timeout -> PUSH_PUCK
+ we push the puck to the lift
-CLOSE_DOOR:
- door_closed -> IDLE
- filter bridge ready
+PUSH_PUCK:
+ no_puck_on_pos2 -> CLOSE_DOOR
+ the puck disappears, we close doors
diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c
index 7949f176..12210416 100644
--- a/digital/io/src/filterbridge_cb.c
+++ b/digital/io/src/filterbridge_cb.c
@@ -25,86 +25,87 @@
#include "common.h"
#include "fsm.h"
#include "filterbridge_cb.h"
+#include "elevator.h"
+#include "servo_pos.h"
+#include "aquajim.h"
/*
- * WAIT_A_PUCK =puck_on_pos2=>
- * nb_puck_ok => WAIT_RGB_PROBE
- * probe color of the new puck
- * too_much_puck => EJECT_PUCK
- * we have too much puck, eject the puck quickly and whistle
+ * IDLE =start=>
+ * => WAIT_JACK_IN
+ * we wait for the jack
*/
fsm_branch_t
-filterbridge__WAIT_A_PUCK__puck_on_pos2 (void)
+filterbridge__IDLE__start (void)
{
- return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, nb_puck_ok);
- return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, too_much_puck);
+ return filterbridge_next (IDLE, start);
}
/*
- * WAIT_RGB_PROBE =color_probed=>
- * bad_color => EJECT_PUCK
- * eject bad puck
- * good_color => OPEN_DOOR
- * put puck to the lift
+ * WAIT_JACK_IN =jack_inserted_into_bot=>
+ * => CLOSE_DOOR
+ * we init the bridge
*/
fsm_branch_t
-filterbridge__WAIT_RGB_PROBE__color_probed (void)
+filterbridge__WAIT_JACK_IN__jack_inserted_into_bot (void)
{
- return filterbridge_next_branch (WAIT_RGB_PROBE, color_probed, bad_color);
- return filterbridge_next_branch (WAIT_RGB_PROBE, color_probed, good_color);
+ servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE);
+ servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE);
+ return filterbridge_next (WAIT_JACK_IN, jack_inserted_into_bot);
}
/*
- * IDLE =lift_ready=>
+ * CLOSE_DOOR =state_timeout=>
* => WAIT_A_PUCK
- * the lift is ready to get pucks, we can begin testing procedure
+ * we are ready, we wait a puck
*/
fsm_branch_t
-filterbridge__IDLE__lift_ready (void)
+filterbridge__CLOSE_DOOR__state_timeout (void)
{
- return filterbridge_next (IDLE, lift_ready);
+ return filterbridge_next (CLOSE_DOOR, state_timeout);
}
/*
- * CLOSE_DOOR =door_closed=>
- * => IDLE
- * filter bridge ready
+ * WAIT_A_PUCK =puck_on_pos2=>
+ * lift_not_ready => WAIT_A_PUCK
+ * Lift not ready, we stand by
+ * lift_ready => OPEN_DOOR
+ * Lift ok, we deliver
*/
fsm_branch_t
-filterbridge__CLOSE_DOOR__door_closed (void)
+filterbridge__WAIT_A_PUCK__puck_on_pos2 (void)
{
- return filterbridge_next (CLOSE_DOOR, door_closed);
+ if(elevator_is_ready)
+ {
+ servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_OPEN);
+ return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, lift_ready);
+ }
+ else
+ return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, lift_not_ready);
}
/*
- * RETURN_NORMAL_POS =bridge_in_position=>
- * => CLOSE_DOOR
- * make bridge ready to test a new puck
+ * OPEN_DOOR =state_timeout=>
+ * => PUSH_PUCK
+ * we push the puck to the lift
*/
fsm_branch_t
-filterbridge__RETURN_NORMAL_POS__bridge_in_position (void)
+filterbridge__OPEN_DOOR__state_timeout (void)
{
- return filterbridge_next (RETURN_NORMAL_POS, bridge_in_position);
+ servo_pos_move_to(SERVO_FINGER_ID,SERVO_FINGER_PUSHING);
+ return filterbridge_next (OPEN_DOOR, state_timeout);
}
/*
- * OPEN_DOOR =no_puck_on_pos2=>
+ * PUSH_PUCK =no_puck_on_pos2=>
* => CLOSE_DOOR
- * release puck to the lift
+ * the puck disappears, we close doors
*/
fsm_branch_t
-filterbridge__OPEN_DOOR__no_puck_on_pos2 (void)
+filterbridge__PUSH_PUCK__no_puck_on_pos2 (void)
{
- return filterbridge_next (OPEN_DOOR, no_puck_on_pos2);
+ servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE);
+ servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE);
+ return filterbridge_next (PUSH_PUCK, no_puck_on_pos2);
}
-/*
- * EJECT_PUCK =ejection_done=>
- * => RETURN_NORMAL_POS
- * put bridge on normal position after puck ejection
- */
-fsm_branch_t
-filterbridge__EJECT_PUCK__ejection_done (void)
-{
- return filterbridge_next (EJECT_PUCK, ejection_done);
-}
+
diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm
index db69e15c..fda5247a 100644
--- a/digital/io/src/init.fsm
+++ b/digital/io/src/init.fsm
@@ -35,6 +35,7 @@ Events:
the tempo is finished
move_done
the move is finished
+ state_timeout
IDLE:
start -> WAIT_JACK_IN
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index c3f85770..4eeddc87 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -151,6 +151,7 @@ main_init (void)
/* Start all FSM (except move FSM) */
fsm_handle_event (&top_fsm, TOP_EVENT_start);
fsm_handle_event (&init_fsm, INIT_EVENT_start);
+ fsm_handle_event (&filterbridge_fsm, FILTERBRIDGE_EVENT_start);
/* fsm_handle_event (&top_fsm, TOP_EVENT_start);
fsm_handle_event (&top_fsm, TOP_EVENT_start);
fsm_handle_event (&top_fsm, TOP_EVENT_start); */
@@ -338,6 +339,8 @@ main_loop (void)
/* TODO: Check other sensors */
}
+ /* TODO: implement filterbridge events */
+
/* Send Sharps raw stats. */
if (main_stats_sharps && !--main_stats_sharps_cpt)
{