From 99a771f6a8d6cfd2c574ba330e9c2c8f875231aa Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 12:15:15 +0200 Subject: digital/io/src: add collect after homologation move --- digital/io/src/ai_top_cb.c | 48 ++++++++++++++++++++++++++++++++++++++++++++-- digital/io/src/top.c | 16 ++++++++++++++++ digital/io/src/top.fsm | 22 +++++++++++++++++++-- digital/io/src/top.h | 5 +++++ 4 files changed, 87 insertions(+), 4 deletions(-) (limited to 'digital') 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 */ -- cgit v1.2.3