summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_top_cb.c48
-rw-r--r--digital/io/src/top.c16
-rw-r--r--digital/io/src/top.fsm22
-rw-r--r--digital/io/src/top.h5
4 files changed, 87 insertions, 4 deletions
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
index 2104c5c7..c1b2a264 100644
--- a/digital/io/src/ai_top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -259,7 +259,7 @@ ai__UNLOAD_FACE_BIN__bot_move_succeed (void)
/*
* UNLOAD_BACK_BIN =bot_move_succeed=>
- * => IDLE
+ * => UNLOAD_UNLOAD
* unload
*/
fsm_branch_t
@@ -271,7 +271,7 @@ ai__UNLOAD_BACK_BIN__bot_move_succeed (void)
/*
* UNLOAD_BACK_BIN =bot_move_failed=>
- * => IDLE
+ * => UNLOAD_UNLOAD
* unload
*/
fsm_branch_t
@@ -281,3 +281,47 @@ ai__UNLOAD_BACK_BIN__bot_move_failed (void)
return ai_next (UNLOAD_BACK_BIN, bot_move_failed);
}
+/*
+ * UNLOAD_UNLOAD =state_timeout=>
+ * => COLLECT
+ * close gate
+ * loader down
+ * choose best food to collect
+ */
+fsm_branch_t
+ai__UNLOAD_UNLOAD__state_timeout (void)
+{
+ asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
+ loader_down ();
+ top_collect (1);
+ return ai_next (UNLOAD_UNLOAD, state_timeout);
+}
+
+/*
+ * COLLECT =move_fsm_succeed=>
+ * unload => UNLOAD
+ * collect => COLLECT
+ */
+fsm_branch_t
+ai__COLLECT__move_fsm_succeed (void)
+{
+ if (top_collect (0))
+ return ai_next_branch (COLLECT, move_fsm_succeed, collect);
+ else
+ return ai_next_branch (COLLECT, move_fsm_succeed, unload);
+}
+
+/*
+ * COLLECT =move_fsm_failed=>
+ * unload => UNLOAD
+ * collect => COLLECT
+ */
+fsm_branch_t
+ai__COLLECT__move_fsm_failed (void)
+{
+ if (top_collect (0))
+ return ai_next_branch (COLLECT, move_fsm_failed, collect);
+ else
+ return ai_next_branch (COLLECT, move_fsm_failed, unload);
+}
+
diff --git a/digital/io/src/top.c b/digital/io/src/top.c
index caf64de1..f36fcd26 100644
--- a/digital/io/src/top.c
+++ b/digital/io/src/top.c
@@ -23,10 +23,26 @@
*
* }}} */
#include "common.h"
+#include "defs.h"
#include "top.h"
+#include "food.h"
+#include "asserv.h"
void
top_init (void)
{
}
+uint8_t
+top_collect (uint8_t force)
+{
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ uint8_t food = food_best (robot_position);
+ if (food == 0xff)
+ return 0;
+ vect_t food_v;
+ food_pos (food, &food_v);
+ move_start_noangle (food_v, 0);
+}
+
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
index 9a6b176c..76a067a8 100644
--- a/digital/io/src/top.fsm
+++ b/digital/io/src/top.fsm
@@ -17,12 +17,18 @@ States:
unblock problem with loader, move loader up
FIRST_GO_END_OF_LINE_UNBLOCKING
unblock problem with loader, go backward
+ UNLOAD
+ go to unloading position
UNLOAD_LOADER_UP
move loader to up position
UNLOAD_FACE_BIN
orient robot to unload angle
UNLOAD_BACK_BIN
move back to unload bin
+ UNLOAD_UNLOAD[timeout=225]
+ unloading, wait
+ COLLECT
+ collecting elements
Events:
start
@@ -109,8 +115,20 @@ UNLOAD_FACE_BIN:
go backward to bin
UNLOAD_BACK_BIN:
- bot_move_succeed -> IDLE
+ bot_move_succeed -> UNLOAD_UNLOAD
unload
- bot_move_failed -> IDLE
+ bot_move_failed -> UNLOAD_UNLOAD
unload
+UNLOAD_UNLOAD:
+ state_timeout -> COLLECT
+ close gate
+ loader down
+ choose best food to collect
+
+COLLECT:
+ move_fsm_succeed: unload -> UNLOAD
+ move_fsm_succeed: collect -> COLLECT
+ move_fsm_failed: unload -> UNLOAD
+ move_fsm_failed: collect -> COLLECT
+
diff --git a/digital/io/src/top.h b/digital/io/src/top.h
index 71155342..7595a24c 100644
--- a/digital/io/src/top.h
+++ b/digital/io/src/top.h
@@ -31,4 +31,9 @@
void
top_init (void);
+/** Go collect food unless no time left unless forced, return non zero if
+ * collecting. */
+uint8_t
+top_collect (uint8_t force);
+
#endif /* top_h */