summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Haller2009-05-15 21:26:39 +0200
committerNicolas Haller2009-05-15 21:26:39 +0200
commit5274f4083e08bf82f23eb67065b94ce14de11bcd (patch)
tree165468db7ae1fa83ebcc41c21bcfa53c8020a11b
parent6966b30c7f8a1630c137d92e0af723139831f5fb (diff)
* digital/io/src:
- cylinder fsm implemented (but not tested)
-rw-r--r--digital/io/src/asserv.h2
-rw-r--r--digital/io/src/cylinder.c6
-rw-r--r--digital/io/src/cylinder.fsm4
-rw-r--r--digital/io/src/cylinder.h8
-rw-r--r--digital/io/src/cylinder_cb.c148
-rw-r--r--digital/io/src/elevator_cb.c2
-rw-r--r--digital/io/src/filterbridge.c2
-rw-r--r--digital/io/src/filterbridge.h2
-rw-r--r--digital/io/src/main.c44
9 files changed, 131 insertions, 87 deletions
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index f2e29886..9240b2d3 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -41,6 +41,8 @@
#define ASSERV_ARM_SPEED_DEFAULT 0x18
#define ASSERV_ELVT_SPEED_DEFAULT 0x46
+#define ASSERV_ARM_STEP_BY_DEGREE 14.814814
+
/**
* Initialize the asserv control module.
* This functions does not initialize the asserv board, but the underling
diff --git a/digital/io/src/cylinder.c b/digital/io/src/cylinder.c
index 9520e0b9..f533e425 100644
--- a/digital/io/src/cylinder.c
+++ b/digital/io/src/cylinder.c
@@ -23,3 +23,9 @@
*
* }}} */
#include "cylinder.h"
+
+/* last state of optical fence */
+uint8_t of_state = 0;
+
+/* nb puck in cylinder */
+uint8_t nb_puck_cylinder = 0;
diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm
index a015f61a..2fcf06c4 100644
--- a/digital/io/src/cylinder.fsm
+++ b/digital/io/src/cylinder.fsm
@@ -29,7 +29,7 @@ States:
Events:
start
start event
- jack_in
+ jack_inserted_into_bot
the init signal
new_puck
there is a puck in position 1
@@ -46,7 +46,7 @@ IDLE:
we wait the jack before moving anything
WAIT_FOR_JACK_IN:
- jack_in -> INIT
+ jack_inserted_into_bot -> INIT
we init the cylinder position
INIT:
diff --git a/digital/io/src/cylinder.h b/digital/io/src/cylinder.h
index 8b9cb427..f6d7ff58 100644
--- a/digital/io/src/cylinder.h
+++ b/digital/io/src/cylinder.h
@@ -25,4 +25,12 @@
*
* }}} */
+#include "common.h"
+
+/* last state of optical fence */
+extern uint8_t of_state;
+
+/* nb puck in cylinder */
+extern uint8_t nb_puck_cylinder;
+
#endif /* cylinder_h */
diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c
index 1ff30760..fa207959 100644
--- a/digital/io/src/cylinder_cb.c
+++ b/digital/io/src/cylinder_cb.c
@@ -25,149 +25,155 @@
#include "common.h"
#include "fsm.h"
#include "cylinder_cb.h"
+#include "asserv.h"
+#include "cylinder.h"
+#include "filterbridge.h"
+
/*
- * WAIT_FOR_BRIDGE_READY =bridge_ready=>
- * => TURN_PLUS_2_AGAIN
- * bridge clear, delivering puck...
+ * IDLE =start=>
+ * => WAIT_FOR_JACK_IN
+ * we wait the jack before moving anything
*/
fsm_branch_t
-cylinder__WAIT_FOR_BRIDGE_READY__bridge_ready (void)
+cylinder__IDLE__start (void)
{
- return cylinder_next (WAIT_FOR_BRIDGE_READY, bridge_ready);
+ return cylinder_next (IDLE, start);
}
/*
- * WAIT_A_PUCK =new_puck=>
- * bridge_ready => TURN_PLUS_4
- * put puck to the bridge
- * bridge_full => TURN_PLUS_2
- * bridge full, waiting before release puck to the bridge
+ * WAIT_FOR_JACK_IN =jack_inserted_into_bot=>
+ * => INIT
+ * we init the cylinder position
*/
fsm_branch_t
-cylinder__WAIT_A_PUCK__new_puck (void)
+cylinder__WAIT_FOR_JACK_IN__jack_inserted_into_bot (void)
{
- return cylinder_next_branch (WAIT_A_PUCK, new_puck, bridge_ready);
- return cylinder_next_branch (WAIT_A_PUCK, new_puck, bridge_full);
+ asserv_arm_zero_position();
+ return cylinder_next (WAIT_FOR_JACK_IN, jack_inserted_into_bot);
}
/*
- * INIT =init_done=>
+ * INIT =move_done=>
* => WAIT_A_PUCK
* the cylinder is ready to get pucks
*/
fsm_branch_t
-cylinder__INIT__init_done (void)
+cylinder__INIT__move_done (void)
{
- return cylinder_next (INIT, init_done);
+ return cylinder_next (INIT, move_done);
}
/*
- * WAIT_FOR_PUCKS_RELEASE =lift_ready=>
- * => TURN_PLUS_1
- * robot ready for new puck, open door
- */
-fsm_branch_t
-cylinder__WAIT_FOR_PUCKS_RELEASE__lift_ready (void)
-{
- return cylinder_next (WAIT_FOR_PUCKS_RELEASE, lift_ready);
-}
-
-/*
- * IDLE =jack_in=>
- * => INIT
- * initialize robot
+ * WAIT_A_PUCK =new_puck=>
+ * => TURN_PLUS_3
+ * turn cylinder 3 position after
*/
fsm_branch_t
-cylinder__IDLE__jack_in (void)
+cylinder__WAIT_A_PUCK__new_puck (void)
{
- return cylinder_next (IDLE, jack_in);
+ ++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_2 =move_done=>
+ * TURN_PLUS_3 =move_done=>
* => WAIT_FOR_BRIDGE_READY
* bridge full, waiting for Bison Fute's clearance
*/
fsm_branch_t
-cylinder__TURN_PLUS_2__move_done (void)
+cylinder__TURN_PLUS_3__move_done (void)
{
- return cylinder_next (TURN_PLUS_2, move_done);
+ return cylinder_next (TURN_PLUS_3, move_done);
}
/*
- * PROBE_OF =probe_done=>
- * puck_detected_and_lift_full => TURN_MINUS_3
- * BAD CASE, there is an undesirable puck to the cylinder
- * puck_detected_and_lift_ready => WAIT_FOR_BRIDGE_READY
- * there is a puck, we take a shortcut...
- * cylinder_empty_and_lift_ready => WAIT_A_PUCK
- * ready for a new puck
- * cylinder_empty_and_lift_full => TURN_MINUS_1
- * stop accepting new puck
+ * WAIT_FOR_BRIDGE_READY =bridge_ready=>
+ * no_puck_bo => TURN_PLUS_1
+ * bridge clear, and no other puck on cylinder
+ * puck_bo => TURN_PLUS_1_LOOP
+ * bridge clear, an another puck is in the cylinder
*/
fsm_branch_t
-cylinder__PROBE_OF__probe_done (void)
+cylinder__WAIT_FOR_BRIDGE_READY__bridge_ready (void)
{
- return cylinder_next_branch (PROBE_OF, probe_done, puck_detected_and_lift_full);
- return cylinder_next_branch (PROBE_OF, probe_done, puck_detected_and_lift_ready);
- return cylinder_next_branch (PROBE_OF, probe_done, cylinder_empty_and_lift_ready);
- return cylinder_next_branch (PROBE_OF, probe_done, cylinder_empty_and_lift_full);
+ ++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=>
- * => WAIT_A_PUCK
- * door opened, ready for a new puck
+ * 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)
{
- return cylinder_next (TURN_PLUS_1, move_done);
+ 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_ARM_SPEED_DEFAULT);
+ return cylinder_next_branch (TURN_PLUS_1, move_done, not_ok_for_other_puck);
}
/*
- * TURN_MINUS_1 =move_done=>
- * => WAIT_FOR_PUCKS_RELEASE
- * robot entrance closed
+ * TURN_PLUS_1_LOOP =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_MINUS_1__move_done (void)
+cylinder__TURN_PLUS_1_LOOP__move_done (void)
{
- return cylinder_next (TURN_MINUS_1, move_done);
+ if(nb_puck_fb < 4)
+ return cylinder_next_branch (TURN_PLUS_1_LOOP, move_done, ok_for_other_puck);
+ return cylinder_next_branch (TURN_PLUS_1_LOOP, move_done, not_ok_for_other_puck);
}
/*
- * TURN_MINUS_3 =move_done=>
+ * TURN_MINUS_1 =move_done=>
* => WAIT_FOR_PUCKS_RELEASE
- * evil puck dropped and robot entrance closed
+ * cylinder close, you shall not pass (but try again later)
*/
fsm_branch_t
-cylinder__TURN_MINUS_3__move_done (void)
+cylinder__TURN_MINUS_1__move_done (void)
{
- return cylinder_next (TURN_MINUS_3, move_done);
+ return cylinder_next (TURN_MINUS_1, move_done);
}
/*
- * TURN_PLUS_2_AGAIN =move_done=>
- * => PROBE_OF
- * there is a puck to the cylinder?
+ * WAIT_FOR_PUCKS_RELEASE =bot_empty=>
+ * => TURN_PLUS_1_AGAIN
*/
fsm_branch_t
-cylinder__TURN_PLUS_2_AGAIN__move_done (void)
+cylinder__WAIT_FOR_PUCKS_RELEASE__bot_empty (void)
{
- return cylinder_next (TURN_PLUS_2_AGAIN, move_done);
+ asserv_move_arm(1*60*ASSERV_ARM_STEP_BY_DEGREE,
+ ASSERV_ARM_SPEED_DEFAULT);
+ return cylinder_next (WAIT_FOR_PUCKS_RELEASE, bot_empty);
}
/*
- * TURN_PLUS_4 =move_done=>
- * => PROBE_OF
- * there is a puck to the cylinder?
+ * TURN_PLUS_1_AGAIN =move_done=>
+ * => WAIT_A_PUCK
+ * cylinder ready
*/
fsm_branch_t
-cylinder__TURN_PLUS_4__move_done (void)
+cylinder__TURN_PLUS_1_AGAIN__move_done (void)
{
- return cylinder_next (TURN_PLUS_4, move_done);
+ return cylinder_next (TURN_PLUS_1_AGAIN, move_done);
}
diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c
index d20496dc..2fc029d7 100644
--- a/digital/io/src/elevator_cb.c
+++ b/digital/io/src/elevator_cb.c
@@ -114,7 +114,7 @@ elevator__WAIT_A_PUCK__new_puck (void)
// TODO time_ok
if(nb_puck_elvt < 4 &&
((chrono_remaining_time() - OK_TIME_LIMIT > 0)
- || nb_puck_pf != 0))
+ || nb_puck_fb != 0))
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/filterbridge.c b/digital/io/src/filterbridge.c
index efe4ec5b..0b5001cc 100644
--- a/digital/io/src/filterbridge.c
+++ b/digital/io/src/filterbridge.c
@@ -24,7 +24,7 @@
* }}} */
#include "filterbridge.h"
-uint8_t nb_puck_pf;
+uint8_t nb_puck_fb;
/* +AutoDec */
/* -AutoDec */
diff --git a/digital/io/src/filterbridge.h b/digital/io/src/filterbridge.h
index 1853a496..81fdcc60 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_pf;
+extern uint8_t nb_puck_fb;
#endif // filterbridge_h
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index df111afc..ea200a73 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -49,6 +49,8 @@
#include "playground.h"
#include "contact.h"
#include "elevator.h"
+#include "filterbridge.h"
+#include "cylinder.h"
#include "io.h"
@@ -92,6 +94,7 @@ uint16_t main_move_wait_cycle;
/* FIXME to be delete */
uint8_t jack_emulation = 0;
+uint8_t cylinder_puck1_emulation = 0;
/**
* The same for init FSM
*/
@@ -157,9 +160,8 @@ main_init (void)
//fsm_handle_event (&init_fsm, INIT_EVENT_start);
fsm_handle_event (&filterbridge_fsm, FILTERBRIDGE_EVENT_start);
fsm_handle_event (&elevator_fsm, ELEVATOR_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); */
+ fsm_handle_event (&cylinder_fsm, CYLINDER_EVENT_start);
+ /* fsm_handle_event (&top_fsm, TOP_EVENT_start); */
/* Sharp module */
sharp_init ();
/* PWM module */
@@ -295,6 +297,14 @@ main_loop (void)
TODO when asserv elevator can failed
}
*/
+ /* cylinder asserv status */
+ asserv_status_e cylinder_status = asserv_last_cmd_ack()
+ ? asserv_arm_cmd_status() : none;
+ if (cylinder_status == success)
+ {
+ FSM_HANDLE_EVENT (&cylinder_fsm,
+ CYLINDER_EVENT_move_done);
+ }
/* send event if elevator received an order */
if(elvt_order)
FSM_HANDLE_EVENT (&elevator_fsm,
@@ -309,6 +319,16 @@ main_loop (void)
if(!IO_GET (CONTACT_ELEVATOR_DOOR))
FSM_HANDLE_EVENT (&elevator_fsm,
ELEVATOR_EVENT_doors_opened);
+ /* bridge ready */
+ if(nb_puck_fb)
+ 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())
{
@@ -323,15 +343,9 @@ main_loop (void)
INIT_EVENT_jack_inserted_into_bot);
FSM_HANDLE_EVENT (&elevator_fsm,
ELEVATOR_EVENT_jack_inserted_into_bot);
+ FSM_HANDLE_EVENT (&cylinder_fsm,
+ CYLINDER_EVENT_jack_inserted_into_bot);
}
- /*
- FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ?
- TOP_EVENT_jack_removed_from_bot :
- TOP_EVENT_jack_inserted_into_bot);
- FSM_HANDLE_EVENT (&init_fsm, switch_get_jack () ?
- INIT_EVENT_jack_removed_from_bot :
- INIT_EVENT_jack_inserted_into_bot);
- */
/* Settings acknowledge */
/*
if (main_top_generate_settings_ack_event)
@@ -438,6 +452,14 @@ main_loop (void)
FSM_HANDLE_EVENT (&filterbridge_fsm,
FILTERBRIDGE_EVENT_no_puck_on_pos2);
}
+ /* test cylinder sensor */
+ /* if(!IO_GET (CON */
+ if(cylinder_puck1_emulation)
+ {
+ FSM_HANDLE_EVENT (&cylinder_fsm,
+ CYLINDER_EVENT_new_puck);
+ }
+
/* FIXME to be delete */
if(jack_emulation)
{