summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/ai_loader_cb.c3
-rw-r--r--digital/io/src/ai_top_cb.c25
-rw-r--r--digital/io/src/loader.c2
-rw-r--r--digital/io/src/loader.fsm1
-rw-r--r--digital/io/src/loader.h3
-rw-r--r--digital/io/src/top.c26
-rw-r--r--digital/io/src/top.fsm6
7 files changed, 59 insertions, 7 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index 2960cdd9..f486bbc6 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -30,6 +30,7 @@
#include "bot.h"
#include "main.h"
#include "food.h"
+#include "loader.h"
/*
* LOADER_IDLE =start=>
@@ -351,6 +352,7 @@ ai__LOADER_ERROR_UPING_OPEN__clamp_succeed (void)
* LOADER_LOAD_CLOSING =clamp_succeed=>
* full => LOADER_LOAD_UPING
* move up
+ * count one element
* empty => LOADER_LOAD_EMPTY_OPEN
* open clamp
*/
@@ -368,6 +370,7 @@ ai__LOADER_LOAD_CLOSING__clamp_succeed (void)
position_t robot_position;
asserv_get_position (&robot_position);
food_taken (robot_position);
+ loader_elements++;
return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full);
}
else
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
index c1b2a264..173df388 100644
--- a/digital/io/src/ai_top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -234,6 +234,30 @@ ai__FIRST_GO_END_OF_LINE_UNBLOCKING__loader_errored (void)
}
/*
+ * UNLOAD =move_fsm_succeed=>
+ * => UNLOAD_LOADER_UP
+ * move loader up
+ */
+fsm_branch_t
+ai__UNLOAD__move_fsm_succeed (void)
+{
+ loader_up ();
+ return ai_next (UNLOAD, move_fsm_succeed);
+}
+
+/*
+ * UNLOAD =move_fsm_failed=>
+ * => UNLOAD
+ * retry
+ */
+fsm_branch_t
+ai__UNLOAD__move_fsm_failed (void)
+{
+ move_start_noangle (PG_VECT (2625, 253), 0);
+ return ai_next (UNLOAD, move_fsm_failed);
+}
+
+/*
* UNLOAD_LOADER_UP =loader_uped=>
* => UNLOAD_FACE_BIN
* turn toward bin
@@ -291,6 +315,7 @@ ai__UNLOAD_BACK_BIN__bot_move_failed (void)
fsm_branch_t
ai__UNLOAD_UNLOAD__state_timeout (void)
{
+ loader_elements = 0;
asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED);
loader_down ();
top_collect (1);
diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c
index 019ec41b..179cf3b8 100644
--- a/digital/io/src/loader.c
+++ b/digital/io/src/loader.c
@@ -27,6 +27,8 @@
#include "fsm.h"
+uint8_t loader_elements;
+
void
loader_up (void)
{
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index cc636a97..92c0d839 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -171,6 +171,7 @@ LOADER_UPING:
LOADER_LOAD_CLOSING:
clamp_succeed: full -> LOADER_LOAD_UPING
move up
+ count one element
clamp_succeed: empty -> LOADER_LOAD_EMPTY_OPEN
open clamp
diff --git a/digital/io/src/loader.h b/digital/io/src/loader.h
index 6ca7459f..b8150891 100644
--- a/digital/io/src/loader.h
+++ b/digital/io/src/loader.h
@@ -25,6 +25,9 @@
*
* }}} */
+/** Number of loaded elements. */
+extern uint8_t loader_elements;
+
/** Move loader up, in a rest position. */
void
loader_up (void);
diff --git a/digital/io/src/top.c b/digital/io/src/top.c
index f36fcd26..0fa451cd 100644
--- a/digital/io/src/top.c
+++ b/digital/io/src/top.c
@@ -27,6 +27,9 @@
#include "top.h"
#include "food.h"
#include "asserv.h"
+#include "loader.h"
+#include "move.h"
+#include "playground.h"
void
top_init (void)
@@ -36,13 +39,22 @@ 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)
+ if (loader_elements < 4 || 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);
+ return 1;
+ }
+ else
+ {
+ move_start_noangle (PG_VECT (2625, 253), 0);
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 76a067a8..9cb3a044 100644
--- a/digital/io/src/top.fsm
+++ b/digital/io/src/top.fsm
@@ -106,6 +106,12 @@ FIRST_GO_END_OF_LINE_UNBLOCKING:
loader_errored -> .
unblock again
+UNLOAD:
+ move_fsm_succeed -> UNLOAD_LOADER_UP
+ move loader up
+ move_fsm_failed -> .
+ retry
+
UNLOAD_LOADER_UP:
loader_uped -> UNLOAD_FACE_BIN
turn toward bin