summaryrefslogtreecommitdiff
path: root/digital/io
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-14 03:23:08 +0200
committerNicolas Schodet2010-05-14 03:23:08 +0200
commitdf32c7a59f59166a22eff4f2a76ec6244c64d721 (patch)
tree89d6340f6c7affc2e6d29632ed9b0b17eb74ef4a /digital/io
parent6ed5d46331c2765ec8c090faa4bc823fef2dd5b1 (diff)
digital/io/src: handle loader blocking
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/ai_move_cb.c86
-rw-r--r--digital/io/src/move.fsm26
-rw-r--r--digital/io/src/top.fsm2
3 files changed, 113 insertions, 1 deletions
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
index e37b56e6..5544eaf6 100644
--- a/digital/io/src/ai_move_cb.c
+++ b/digital/io/src/ai_move_cb.c
@@ -47,6 +47,8 @@
#include <math.h>
+#define MOVE_LOADER_UNBLOCKING_DISTANCE 70
+
/** Go or rotate toward position, returns 1 for linear move, 2 for angular
* move. */
static uint8_t
@@ -212,6 +214,20 @@ ai__MOVE_ROTATING__bot_move_succeed (void)
}
/*
+ * MOVE_ROTATING =loader_errored=>
+ * => MOVE_LOADER_UNBLOCKING_UPING
+ * move backward
+ * loader up
+ */
+fsm_branch_t
+ai__MOVE_ROTATING__loader_errored (void)
+{
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return ai_next (MOVE_ROTATING, loader_errored);
+}
+
+/*
* MOVE_MOVING =bot_move_succeed=>
* done => MOVE_IDLE
* post success event.
@@ -283,6 +299,20 @@ ai__MOVE_MOVING__obstacle_in_front (void)
}
/*
+ * MOVE_MOVING =loader_errored=>
+ * => MOVE_LOADER_UNBLOCKING_UPING
+ * move backward
+ * loader up
+ */
+fsm_branch_t
+ai__MOVE_MOVING__loader_errored (void)
+{
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return ai_next (MOVE_MOVING, loader_errored);
+}
+
+/*
* MOVE_MOVING_BACKWARD_TO_TURN_FREELY =bot_move_succeed=>
* path_found_rotate => MOVE_ROTATING
* rotate towards next position.
@@ -371,4 +401,60 @@ ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void)
}
}
+/*
+ * MOVE_LOADER_UNBLOCKING_UPING =bot_move_succeed=>
+ * => MOVE_LOADER_UNBLOCKING_DOWNING
+ * loader down
+ */
+fsm_branch_t
+ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_succeed (void)
+{
+ loader_down ();
+ return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed);
+}
+
+/*
+ * MOVE_LOADER_UNBLOCKING_UPING =bot_move_failed=>
+ * => MOVE_LOADER_UNBLOCKING_DOWNING
+ * loader down
+ */
+fsm_branch_t
+ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_failed (void)
+{
+ loader_down ();
+ return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed);
+}
+
+/*
+ * MOVE_LOADER_UNBLOCKING_DOWNING =loader_downed=>
+ * rotate => MOVE_ROTATING
+ * repeat last rotate
+ * move => MOVE_MOVING
+ * repeat last move
+ */
+fsm_branch_t
+ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_downed (void)
+{
+ if (move_go_or_rotate (move_data.step, move_data.step_angle,
+ move_data.step_with_angle,
+ move_data.step_backward) == 2)
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, rotate);
+ else
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, move);
+}
+
+/*
+ * MOVE_LOADER_UNBLOCKING_DOWNING =loader_errored=>
+ * => MOVE_LOADER_UNBLOCKING_UPING
+ * move backward
+ * loader up
+ */
+fsm_branch_t
+ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_errored (void)
+{
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return ai_next (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored);
+}
+
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
index 20334686..2812f686 100644
--- a/digital/io/src/move.fsm
+++ b/digital/io/src/move.fsm
@@ -16,6 +16,10 @@ States:
moving backward to go away from what is blocking the bot.
MOVE_WAIT_FOR_CLEAR_PATH [timeout=255]
waiting for obstacle to disappear.
+ MOVE_LOADER_UNBLOCKING_UPING
+ moving loader up and move backward to unblock loader.
+ MOVE_LOADER_UNBLOCKING_DOWNING
+ moving loader down.
Events:
move_start
@@ -34,6 +38,9 @@ MOVE_IDLE:
MOVE_ROTATING:
bot_move_succeed -> MOVE_MOVING
move to next position.
+ loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
+ move backward
+ loader up
MOVE_MOVING:
bot_move_succeed: done -> MOVE_IDLE
@@ -50,6 +57,9 @@ MOVE_MOVING:
obstacle_in_front -> MOVE_WAIT_FOR_CLEAR_PATH
reset final_move.
stop the bot.
+ loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
+ move backward
+ loader up
MOVE_MOVING_BACKWARD_TO_TURN_FREELY:
bot_move_succeed: path_found_rotate -> MOVE_ROTATING
@@ -74,3 +84,19 @@ MOVE_WAIT_FOR_CLEAR_PATH:
decrement counter.
state_timeout: no_path_found_and_no_try_again -> MOVE_IDLE
post failure.
+
+MOVE_LOADER_UNBLOCKING_UPING:
+ bot_move_succeed -> MOVE_LOADER_UNBLOCKING_DOWNING
+ loader down
+ bot_move_failed -> MOVE_LOADER_UNBLOCKING_DOWNING
+ loader down
+
+MOVE_LOADER_UNBLOCKING_DOWNING:
+ loader_downed: rotate -> MOVE_ROTATING
+ repeat last rotate
+ loader_downed: move -> MOVE_MOVING
+ repeat last move
+ loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
+ move backward
+ loader up
+
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
index 46434b7f..a105e87d 100644
--- a/digital/io/src/top.fsm
+++ b/digital/io/src/top.fsm
@@ -47,8 +47,8 @@ Events:
last command sent to the asserv board has been acquitted.
import init.fsm
-import move.fsm
import loader.fsm
+import move.fsm
IDLE:
start -> WAIT_INIT_TO_FINISH