summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-14 13:06:29 +0200
committerNicolas Schodet2010-05-14 13:06:29 +0200
commitcfc72a8fea2d8ce7f442f7f7612e1b50eb5664fc (patch)
tree7e631ce7a2b8d107ce0ab0c5ccfd80d645a69f3f
parente916ca27604842a8cd25c2158a941b9c3fda83b8 (diff)
digital/io/src: add blocking handlingeurobot-2010-3
-rw-r--r--digital/io/src/ai_loader_cb.c49
-rw-r--r--digital/io/src/ai_top_cb.c48
-rw-r--r--digital/io/src/loader.fsm8
-rw-r--r--digital/io/src/top.fsm12
4 files changed, 117 insertions, 0 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index b556c9eb..36bd96fe 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -364,6 +364,18 @@ ai__LOADER_ERROR_DOWNING_OPEN__clamp_succeed (void)
}
/*
+ * LOADER_ERROR_DOWNING_OPEN =clamp_failed=>
+ * => LOADER_ERROR
+ * post loader_errored event
+ */
+fsm_branch_t
+ai__LOADER_ERROR_DOWNING_OPEN__clamp_failed (void)
+{
+ main_post_event (AI_EVENT_loader_errored);
+ return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_failed);
+}
+
+/*
* LOADER_ERROR_UPING =elevator_succeed=>
* => LOADER_ERROR_UPING_OPEN
* open clamp
@@ -401,6 +413,18 @@ ai__LOADER_ERROR_UPING_OPEN__clamp_succeed (void)
}
/*
+ * LOADER_ERROR_UPING_OPEN =clamp_failed=>
+ * => LOADER_ERROR
+ * post loader_errored event
+ */
+fsm_branch_t
+ai__LOADER_ERROR_UPING_OPEN__clamp_failed (void)
+{
+ main_post_event (AI_EVENT_loader_errored);
+ return ai_next (LOADER_ERROR_UPING_OPEN, clamp_failed);
+}
+
+/*
* LOADER_LOAD_CLOSING =clamp_succeed=>
* full => LOADER_LOAD_UPING
* move up
@@ -514,6 +538,18 @@ ai__LOADER_LOAD_UNLOADING_OPEN__clamp_succeed (void)
}
/*
+ * LOADER_LOAD_UNLOADING_OPEN =clamp_failed=>
+ * => LOADER_ERROR
+ * post loader_errored event
+ */
+fsm_branch_t
+ai__LOADER_LOAD_UNLOADING_OPEN__clamp_failed (void)
+{
+ main_post_event (AI_EVENT_loader_errored);
+ return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_failed);
+}
+
+/*
* LOADER_LOAD_EMPTY_OPEN =clamp_succeed=>
* down => LOADER_DOWN
* up => LOADER_UPING
@@ -532,3 +568,16 @@ ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void)
return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down);
}
+/*
+ * LOADER_LOAD_EMPTY_OPEN =clamp_failed=>
+ * => LOADER_ERROR
+ * post loader_errored event
+ */
+fsm_branch_t
+ai__LOADER_LOAD_EMPTY_OPEN__clamp_failed (void)
+{
+ main_post_event (AI_EVENT_loader_errored);
+ return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_failed);
+}
+
+
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
index 2041ab7b..65895afe 100644
--- a/digital/io/src/ai_top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -190,6 +190,18 @@ ai__UNLOAD_LOADER_UP__loader_uped (void)
}
/*
+ * UNLOAD_LOADER_UP =loader_errored=>
+ * => UNLOAD_FACE_BIN
+ * turn toward bin
+ */
+fsm_branch_t
+ai__UNLOAD_LOADER_UP__loader_errored (void)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return ai_next (UNLOAD_LOADER_UP, loader_errored);
+}
+
+/*
* UNLOAD_FACE_BIN =bot_move_succeed=>
* => UNLOAD_BACK_BIN
* go backward to bin
@@ -202,6 +214,42 @@ ai__UNLOAD_FACE_BIN__bot_move_succeed (void)
}
/*
+ * UNLOAD_FACE_BIN =bot_move_failed=>
+ * => UNLOAD_FACE_BIN_UNBLOCK
+ * move backward
+ */
+fsm_branch_t
+ai__UNLOAD_FACE_BIN__bot_move_failed (void)
+{
+ asserv_move_linearly (-40);
+ return ai_next (UNLOAD_FACE_BIN, bot_move_failed);
+}
+
+/*
+ * UNLOAD_FACE_BIN_UNBLOCK =bot_move_succeed=>
+ * => UNLOAD_FACE_BIN
+ * turn toward bin
+ */
+fsm_branch_t
+ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_succeed (void)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_succeed);
+}
+
+/*
+ * UNLOAD_FACE_BIN_UNBLOCK =bot_move_failed=>
+ * => UNLOAD_FACE_BIN
+ * turn toward bin
+ */
+fsm_branch_t
+ai__UNLOAD_FACE_BIN_UNBLOCK__bot_move_failed (void)
+{
+ asserv_goto_angle (PG_A_DEG (90));
+ return ai_next (UNLOAD_FACE_BIN_UNBLOCK, bot_move_failed);
+}
+
+/*
* UNLOAD_BACK_BIN =bot_move_succeed=>
* => UNLOAD_UNLOAD
* unload
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index fb37dc81..7cc192b3 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -149,6 +149,8 @@ LOADER_ERROR_DOWNING:
LOADER_ERROR_DOWNING_OPEN:
clamp_succeed -> LOADER_DOWN
post loader_downed event
+ clamp_failed -> LOADER_ERROR
+ post loader_errored event
LOADER_ERROR_UPING:
elevator_succeed -> LOADER_ERROR_UPING_OPEN
@@ -159,6 +161,8 @@ LOADER_ERROR_UPING:
LOADER_ERROR_UPING_OPEN:
clamp_succeed -> LOADER_UP
post loader_uped event
+ clamp_failed -> LOADER_ERROR
+ post loader_errored event
LOADER_DOWNING:
elevator_succeed -> LOADER_DOWN
@@ -205,9 +209,13 @@ LOADER_LOAD_UNLOADING_OPEN:
move down
clamp_succeed: up -> LOADER_UPING
move up
+ clamp_failed -> LOADER_ERROR
+ post loader_errored event
LOADER_LOAD_EMPTY_OPEN:
clamp_succeed: down -> LOADER_DOWN
clamp_succeed: up -> LOADER_UPING
move up
+ clamp_failed -> LOADER_ERROR
+ post loader_errored event
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
index c33923f6..4bebcdaa 100644
--- a/digital/io/src/top.fsm
+++ b/digital/io/src/top.fsm
@@ -19,6 +19,8 @@ States:
move loader to up position
UNLOAD_FACE_BIN
orient robot to unload angle
+ UNLOAD_FACE_BIN_UNBLOCK
+ move backward to unblock
UNLOAD_BACK_BIN
move back to unload bin
UNLOAD_UNLOAD[timeout=225]
@@ -97,10 +99,20 @@ UNLOAD:
UNLOAD_LOADER_UP:
loader_uped -> UNLOAD_FACE_BIN
turn toward bin
+ loader_errored -> UNLOAD_FACE_BIN
+ turn toward bin
UNLOAD_FACE_BIN:
bot_move_succeed -> UNLOAD_BACK_BIN
go backward to bin
+ bot_move_failed -> UNLOAD_FACE_BIN_UNBLOCK
+ move backward
+
+UNLOAD_FACE_BIN_UNBLOCK:
+ bot_move_succeed -> UNLOAD_FACE_BIN
+ turn toward bin
+ bot_move_failed -> UNLOAD_FACE_BIN
+ turn toward bin
UNLOAD_BACK_BIN:
bot_move_succeed -> UNLOAD_UNLOAD