summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/ai_move_cb.c
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/src/ai_move_cb.c
parent6ed5d46331c2765ec8c090faa4bc823fef2dd5b1 (diff)
digital/io/src: handle loader blocking
Diffstat (limited to 'digital/io/src/ai_move_cb.c')
-rw-r--r--digital/io/src/ai_move_cb.c86
1 files changed, 86 insertions, 0 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);
+}
+