summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-15 07:47:31 +0200
committerNicolas Schodet2010-05-15 07:47:31 +0200
commite93c0b1ae95cb338199c1cf7f180ac11beb5e798 (patch)
tree6c8f000ed86b02e3dbdb4b31cef9c4dd7c49ce61
parent63720f726cda3fad93c5766850ba622a3cee0be1 (diff)
digital/io/src: go unloading if next food is far away
-rw-r--r--digital/io/src/top.c9
1 files changed, 9 insertions, 0 deletions
diff --git a/digital/io/src/top.c b/digital/io/src/top.c
index 7222bcc1..250c9f30 100644
--- a/digital/io/src/top.c
+++ b/digital/io/src/top.c
@@ -32,6 +32,8 @@
#include "chrono.h"
#include "playground.h"
+#include "modules/utils/utils.h"
+
uint8_t top_food;
/** Maximum elements to load before unloading. */
@@ -59,6 +61,13 @@ top_collect (uint8_t force)
return 0;
vect_t food_v;
food_pos (top_food, &food_v);
+ /* If loaded an next ball is far away, go unload. */
+ if (loader_elements
+ && UTILS_ABS (food_v.x - robot_position.v.x) > 1000)
+ {
+ move_start_noangle (PG_VECT (2625, 253), 0, 0);
+ return 0;
+ }
move_start_noangle (food_v, 0, food_shorten (top_food));
return 1;
}