summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-30 10:57:35 +0200
committerNicolas Schodet2010-05-30 10:57:35 +0200
commit9107c6f474f508fce88045c9b041269797c776b1 (patch)
treeac3c96963cfc010be10f12e1d08c4ef0b243c133
parent78e07f956ad17ec0c781bd6c2caf36f953a5ed51 (diff)
digital/io/src: pick corn before sensing them
-rw-r--r--digital/io/src/ai_top_cb.c40
-rw-r--r--digital/io/src/top.fsm20
2 files changed, 35 insertions, 25 deletions
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
index 8c781d8d..ec87b6cb 100644
--- a/digital/io/src/ai_top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -295,8 +295,7 @@ ai__UNLOAD_UNLOAD__state_timeout (void)
* unload => UNLOAD
* collect => COLLECT
* slow_motion => COLLECT_SLOW_MOTION
- * slow down
- * move to food
+ * post loader_element event
*/
fsm_branch_t
ai__COLLECT__move_fsm_succeed (void)
@@ -304,8 +303,7 @@ ai__COLLECT__move_fsm_succeed (void)
int16_t slow_motion = food_slow_motion (top_food);
if (slow_motion)
{
- asserv_set_speed (BOT_MOVE_SNAYLE);
- asserv_move_linearly (slow_motion);
+ fsm_handle_event (&ai_fsm, AI_EVENT_loader_element);
return ai_next_branch (COLLECT, move_fsm_succeed, slow_motion);
}
else if (top_collect (0))
@@ -333,47 +331,59 @@ ai__COLLECT__move_fsm_failed (void)
}
/*
- * COLLECT_SLOW_MOTION =bot_move_succeed=>
+ * COLLECT_SLOW_MOTION =loader_downed=>
* unload => UNLOAD
* collect => COLLECT
- * speed up
* collect
*/
fsm_branch_t
-ai__COLLECT_SLOW_MOTION__bot_move_succeed (void)
+ai__COLLECT_SLOW_MOTION__loader_downed (void)
{
if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, bot_move_succeed, collect);
+ return ai_next_branch (COLLECT_SLOW_MOTION, loader_downed, collect);
else
- return ai_next_branch (COLLECT_SLOW_MOTION, bot_move_succeed, unload);
+ return ai_next_branch (COLLECT_SLOW_MOTION, loader_downed, unload);
}
/*
- * COLLECT_SLOW_MOTION =bot_move_failed=>
+ * COLLECT_SLOW_MOTION =loader_errored=>
* unload => UNLOAD
* collect => COLLECT
* same as above
*/
fsm_branch_t
-ai__COLLECT_SLOW_MOTION__bot_move_failed (void)
+ai__COLLECT_SLOW_MOTION__loader_errored (void)
{
if (top_collect (0))
- return ai_next_branch (COLLECT_SLOW_MOTION, bot_move_failed, collect);
+ return ai_next_branch (COLLECT_SLOW_MOTION, loader_errored, collect);
else
- return ai_next_branch (COLLECT_SLOW_MOTION, bot_move_failed, unload);
+ return ai_next_branch (COLLECT_SLOW_MOTION, loader_errored, unload);
+}
+
+/*
+ * COLLECT_SLOW_MOTION =state_timeout=>
+ * unload => UNLOAD
+ * collect => COLLECT
+ * same as above
+ */
+fsm_branch_t
+ai__COLLECT_SLOW_MOTION__state_timeout (void)
+{
+ if (top_collect (0))
+ return ai_next_branch (COLLECT_SLOW_MOTION, state_timeout, collect);
+ else
+ return ai_next_branch (COLLECT_SLOW_MOTION, state_timeout, unload);
}
/*
* COLLECT_SLOW_MOTION =loader_black=>
* => COLLECT_BLACK
- * speed up
* move backward
* mark as black
*/
fsm_branch_t
ai__COLLECT_SLOW_MOTION__loader_black (void)
{
- asserv_set_speed (BOT_MOVE_SLOW);
asserv_move_linearly (-90);
food_black (top_food);
return ai_next (COLLECT_SLOW_MOTION, loader_black);
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
index faa049f5..c268bd66 100644
--- a/digital/io/src/top.fsm
+++ b/digital/io/src/top.fsm
@@ -27,8 +27,8 @@ States:
unloading, wait
COLLECT
collecting elements
- COLLECT_SLOW_MOTION
- slowly move toward element to collect
+ COLLECT_SLOW_MOTION[timeout=450]
+ picking element before bumper contact
COLLECT_BLACK
moving backward after trying to collect a black corn
@@ -133,22 +133,22 @@ COLLECT:
move_fsm_succeed: unload -> UNLOAD
move_fsm_succeed: collect -> COLLECT
move_fsm_succeed: slow_motion -> COLLECT_SLOW_MOTION
- slow down
- move to food
+ post loader_element event
move_fsm_failed: unload -> UNLOAD
move_fsm_failed: collect -> COLLECT
cancel this element
COLLECT_SLOW_MOTION:
- bot_move_succeed: unload -> UNLOAD
- bot_move_succeed: collect -> COLLECT
- speed up
+ loader_downed: unload -> UNLOAD
+ loader_downed: collect -> COLLECT
collect
- bot_move_failed: unload -> UNLOAD
- bot_move_failed: collect -> COLLECT
+ loader_errored: unload -> UNLOAD
+ loader_errored: collect -> COLLECT
+ same as above
+ state_timeout: unload -> UNLOAD
+ state_timeout: collect -> COLLECT
same as above
loader_black -> COLLECT_BLACK
- speed up
move backward
mark as black