summaryrefslogtreecommitdiff
path: root/digital/io/src/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-13 14:02:55 +0200
committerNicolas Schodet2010-05-13 14:02:55 +0200
commitd51d7ef789a776741cb4712646c47c7f37b005c2 (patch)
tree0a9009937cf3ca7042dfe09a42c436d6806c1cbd /digital/io/src/top.c
parent1701d46af3362c616d1851b17c652977c3cd0b74 (diff)
digital/io/src: count elements to unload
Diffstat (limited to 'digital/io/src/top.c')
-rw-r--r--digital/io/src/top.c26
1 files changed, 19 insertions, 7 deletions
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);
+ }
}