From d51d7ef789a776741cb4712646c47c7f37b005c2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 14:02:55 +0200 Subject: digital/io/src: count elements to unload --- digital/io/src/ai_loader_cb.c | 3 +++ digital/io/src/ai_top_cb.c | 25 +++++++++++++++++++++++++ digital/io/src/loader.c | 2 ++ digital/io/src/loader.fsm | 1 + digital/io/src/loader.h | 3 +++ digital/io/src/top.c | 26 +++++++++++++++++++------- digital/io/src/top.fsm | 6 ++++++ 7 files changed, 59 insertions(+), 7 deletions(-) (limited to 'digital/io/src') 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 @@ -233,6 +233,30 @@ ai__FIRST_GO_END_OF_LINE_UNBLOCKING__loader_errored (void) return ai_next (FIRST_GO_END_OF_LINE_UNBLOCKING, loader_errored); } +/* + * 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 @@ -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 -- cgit v1.2.3