summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Haller2009-05-09 05:05:39 +0200
committerNicolas Haller2009-05-09 05:05:39 +0200
commita367d0c7148a6de0c16aa8f9dabd597be01d3584 (patch)
treeb81ccb315894510e286f38cf305be35e368bc54b
parent7d174ea9c2cbecfa548fa4423d432073c5fa8043 (diff)
* digital/io/src:
- fix init FSM (fix angle values) - implement filterbridge FSM
-rw-r--r--digital/io/src/aquajim.h6
-rw-r--r--digital/io/src/contact.c3
-rw-r--r--digital/io/src/filterbridge.fsm10
-rw-r--r--digital/io/src/filterbridge_cb.c17
-rw-r--r--digital/io/src/init_cb.c5
-rw-r--r--digital/io/src/main.c27
-rw-r--r--digital/io/src/simu.host.h2
7 files changed, 59 insertions, 11 deletions
diff --git a/digital/io/src/aquajim.h b/digital/io/src/aquajim.h
index 53d06567..a64fc4cd 100644
--- a/digital/io/src/aquajim.h
+++ b/digital/io/src/aquajim.h
@@ -45,4 +45,10 @@
#define SERVO_DOOR_OPEN 0
#define SERVO_DOOR_CLOSE 1
+/**
+ * How to compute a angle for giboulee?
+ * One degree is 65536 / 360
+ */
+#define BOT_ANGLE_DEGREE (65536 / 360)
+
#endif /* aquajim_h */
diff --git a/digital/io/src/contact.c b/digital/io/src/contact.c
index a9b408a3..3a26e962 100644
--- a/digital/io/src/contact.c
+++ b/digital/io/src/contact.c
@@ -22,14 +22,13 @@
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* }}} */
-#include "common.h"
+
#include "contact.h"
#include "io.h"
void
contact_init (void)
{
- /* Pull-up. */
IO_SET (CONTACT_FILTER_BRIDGE_PUCK);
IO_SET (CONTACT_ELEVATOR_DOOR);
}
diff --git a/digital/io/src/filterbridge.fsm b/digital/io/src/filterbridge.fsm
index 2a87ab05..5722f5e2 100644
--- a/digital/io/src/filterbridge.fsm
+++ b/digital/io/src/filterbridge.fsm
@@ -11,6 +11,8 @@ States:
close access to the lift (and wait 1 sec)
WAIT_A_PUCK
waiting for a new puck
+ MARCEL_WAIT[timeout=45]
+ waiting the puck is entirely in pos2
OPEN_DOOR[timeout=225]
release puck to the lift and block a new puck on pos1
PUSH_PUCK
@@ -43,8 +45,12 @@ CLOSE_DOOR:
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
+ puck_on_pos2: lift_ready -> MARCEL_WAIT
+ wait the puck is entirely on pos2
+
+MARCEL_WAIT:
+ state_timeout -> OPEN_DOOR
+ lift ready, we deliver
OPEN_DOOR:
state_timeout -> PUSH_PUCK
diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c
index 12210416..00848d57 100644
--- a/digital/io/src/filterbridge_cb.c
+++ b/digital/io/src/filterbridge_cb.c
@@ -68,15 +68,14 @@ filterbridge__CLOSE_DOOR__state_timeout (void)
* 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
+ * lift_ready => MARCEL_WAIT
+ * wait the puck is entirely on pos2
*/
fsm_branch_t
filterbridge__WAIT_A_PUCK__puck_on_pos2 (void)
{
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
@@ -84,6 +83,18 @@ filterbridge__WAIT_A_PUCK__puck_on_pos2 (void)
}
/*
+ * MARCEL_WAIT =state_timeout=>
+ * => OPEN_DOOR
+ * lift ready, we deliver
+ */
+fsm_branch_t
+filterbridge__MARCEL_WAIT__state_timeout (void)
+{
+ servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_OPEN);
+ return filterbridge_next (MARCEL_WAIT, state_timeout);
+}
+
+/*
* OPEN_DOOR =state_timeout=>
* => PUSH_PUCK
* we push the puck to the lift
diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c
index f05a5fef..70a964c1 100644
--- a/digital/io/src/init_cb.c
+++ b/digital/io/src/init_cb.c
@@ -29,6 +29,7 @@
#include "init.h"
#include "playground.h"
#include "main.h"
+#include "aquajim.h"
/*
* GOTO_THE_WALL_AGAIN =move_done=>
@@ -86,7 +87,7 @@ init__TURN_90_DEGREES_CCW__move_done (void)
fsm_branch_t
init__GO_BACKWARD__move_done (void)
{
- asserv_move_angularly(90);
+ asserv_move_angularly(90*BOT_ANGLE_DEGREE);
return init_next (GO_BACKWARD, move_done);
}
@@ -136,7 +137,7 @@ init__WAIT_2_SEC__init_tempo_ended (void)
fsm_branch_t
init__GO_BACKWARD_AGAIN__move_done (void)
{
- asserv_move_angularly(180);
+ asserv_move_angularly(180*BOT_ANGLE_DEGREE);
return init_next (GO_BACKWARD_AGAIN, move_done);
}
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 33f6c838..32be6ac0 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -89,6 +89,8 @@ uint8_t main_always_stop_for_obstacle = 1;
*/
uint16_t main_move_wait_cycle;
+/* FIXME to be delete */
+uint8_t jack_emulation = 0;
/**
* The same for init FSM
*/
@@ -150,8 +152,8 @@ main_init (void)
fsm_init(&elevator_fsm);
fsm_init(&filterbridge_fsm);
/* 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 (&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);
@@ -391,6 +393,24 @@ main_loop (void)
--main_init_wait_cycle;
if(!main_init_wait_cycle)
FSM_HANDLE_EVENT (&init_fsm, INIT_EVENT_init_tempo_ended);
+ /* test filterbridge sensor */
+ if(!IO_GET (CONTACT_FILTER_BRIDGE_PUCK))
+ {
+ FSM_HANDLE_EVENT (&filterbridge_fsm,
+ FILTERBRIDGE_EVENT_puck_on_pos2);
+ }
+ else
+ {
+ FSM_HANDLE_EVENT (&filterbridge_fsm,
+ FILTERBRIDGE_EVENT_no_puck_on_pos2);
+ }
+ /* FIXME to be delete */
+ if(jack_emulation)
+ {
+ FSM_HANDLE_EVENT (&filterbridge_fsm,
+ FILTERBRIDGE_EVENT_jack_inserted_into_bot);
+ }
+
}
}
@@ -406,6 +426,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
#define c(cmd, size) (cmd << 8 | size)
switch (c (cmd, size))
{
+ case c ('j', 0):
+ jack_emulation = 1;
+ break;
case c ('z', 0):
/* Reset */
utils_reset ();
diff --git a/digital/io/src/simu.host.h b/digital/io/src/simu.host.h
index 214fce3d..f314db87 100644
--- a/digital/io/src/simu.host.h
+++ b/digital/io/src/simu.host.h
@@ -25,6 +25,8 @@
*
* }}} */
+#include "common.h"
+
#ifdef HOST
extern uint8_t PORTC, PINC;