summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorJérémy Dufour2008-04-30 01:37:18 +0200
committerJérémy Dufour2008-04-30 01:37:18 +0200
commitd1970dba0d168fdf9b89f3f7446ea2300fac9d30 (patch)
tree39b64caa69fb334a0b82fbfe2f3b165ad9a9e7b8 /digital
parent2005d00e2962ab2f4a036b1692ba96e283843a6b (diff)
* digital/io/src
- integrate sharp to generate obstacle event for move FSM.
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/asserv.c14
-rw-r--r--digital/io/src/asserv.h10
-rw-r--r--digital/io/src/main.c22
3 files changed, 46 insertions, 0 deletions
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index 4b3a5795..2505da88 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -307,6 +307,20 @@ asserv_get_arm_position (void)
return asserv_status.arm_position;
}
+/* Are we moving forward/backward? */
+uint8_t
+asserv_get_moving_direction (void)
+{
+ /* Foward move? */
+ if (asserv_status.status & _BV (4))
+ return 1;
+ /* Backward move? */
+ if (asserv_status.status & _BV (5))
+ return 2;
+ /* Not moving */
+ return 0;
+}
+
/* Reset the asserv board. */
void
asserv_reset (void)
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index 686cddca..ebadf188 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -140,6 +140,16 @@ uint16_t
asserv_get_arm_position (void);
/**
+ * Are we moving forward/backward?
+ * @return
+ * - 0 we are not moving;
+ * - 1 we are moving forward;
+ * - 2 we are moving backward.
+ */
+uint8_t
+asserv_get_moving_direction (void);
+
+/**
* Reset the asserv board.
* Other class command.
*/
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 46b171c6..6a709d0c 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -243,6 +243,28 @@ main_loop (void)
/* Post the event */
FSM_HANDLE_EVENT (&top_fsm, save_event);
}
+ /* Sharps event for move FSM */
+ uint8_t moving_direction = asserv_get_moving_direction ();
+ if (moving_direction)
+ {
+ if (moving_direction == 1)
+ {
+ /* Front only */
+ if (sharp_get_interpreted (SHARP_FRONT_LEFT) ||
+ sharp_get_interpreted (SHARP_FRONT_RIGHT))
+ /* Generate an event for move FSM */
+ FSM_HANDLE_EVENT (&move_fsm,
+ MOVE_EVENT_bot_move_obstacle);
+ }
+ else
+ {
+ /* Back only */
+ if (sharp_get_interpreted (SHARP_BACK_LEFT) ||
+ sharp_get_interpreted (SHARP_BACK_RIGHT))
+ /* Generate an event for move FSM */
+ FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_obstacle);
+ }
+ }
/* TODO: Check other sensors */
}