From 8f70e520c9c4ce504f8368346d8b85df27932ebe Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 12 May 2010 19:01:20 +0200 Subject: digital/io/src: homologation initial sequence --- digital/io/src/ai_top_cb.c | 97 +++++++++++++++++++++++++++++++++------------- digital/io/src/bot.h | 6 +++ digital/io/src/top.fsm | 50 +++++++++++++++--------- 3 files changed, 108 insertions(+), 45 deletions(-) diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index 8fc1541e..2e9a824d 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -31,6 +31,7 @@ #include "move.h" #include "chrono.h" #include "playground.h" +#include "asserv.h" #include "loader.h" /** @@ -51,67 +52,109 @@ ai__IDLE__start (void) /* * WAIT_INIT_TO_FINISH =init_match_is_started=> - * => GO_FAR + * => FIRST_GO_BEGIN_OF_LINE_FAST * the match start - * go to a far point + * set fast speed + * go to first sequence begin of line */ fsm_branch_t ai__WAIT_INIT_TO_FINISH__init_match_is_started (void) { + asserv_set_speed (BOT_MOVE_FAST); position_t pos = PG_POSITION_DEG (375, 1503, -29); move_start (pos, 0); return ai_next (WAIT_INIT_TO_FINISH, init_match_is_started); } /* - * GO_FAR =move_fsm_succeed=> - * => GO_NEAR - * go to a near point + * FIRST_GO_BEGIN_OF_LINE_FAST =move_fsm_succeed=> + * => FIRST_GO_END_OF_LINE_FAST + * go to end of line + * loader down */ fsm_branch_t -ai__GO_FAR__move_fsm_succeed (void) +ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_succeed (void) { position_t pos = PG_POSITION_DEG (2625, 253, -90); move_start (pos, 0); loader_down (); - return ai_next (GO_FAR, move_fsm_succeed); + return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_succeed); } /* - * GO_FAR =move_fsm_failed=> - * => GO_NEAR - * go to a near point + * FIRST_GO_BEGIN_OF_LINE_FAST =move_fsm_failed=> + * => FIRST_GO_BEGIN_OF_LINE_FAST + * set slow speed + * retry */ fsm_branch_t -ai__GO_FAR__move_fsm_failed (void) +ai__FIRST_GO_BEGIN_OF_LINE_FAST__move_fsm_failed (void) { - ai__GO_FAR__move_fsm_succeed (); - return ai_next (GO_FAR, move_fsm_failed); + asserv_set_speed (BOT_MOVE_SLOW); + position_t pos = PG_POSITION_DEG (375, 1503, -29); + move_start (pos, 0); + return ai_next (FIRST_GO_BEGIN_OF_LINE_FAST, move_fsm_failed); } /* - * GO_NEAR =move_fsm_succeed=> - * => GO_FAR - * restart + * FIRST_GO_END_OF_LINE_FAST =in_field=> + * => FIRST_GO_END_OF_LINE_SLOW + * set slow speed */ fsm_branch_t -ai__GO_NEAR__move_fsm_succeed (void) +ai__FIRST_GO_END_OF_LINE_FAST__in_field (void) { - position_t pos = PG_POSITION_DEG (375, 1503, -29); - move_start (pos, 0); - loader_up (); - return ai_next (GO_NEAR, move_fsm_succeed); + asserv_set_speed (BOT_MOVE_SLOW); + return ai_next (FIRST_GO_END_OF_LINE_FAST, in_field); +} + +/* + * FIRST_GO_END_OF_LINE_FAST =move_fsm_succeed=> + * => IDLE + * set slow speed + */ +fsm_branch_t +ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_succeed (void) +{ + asserv_set_speed (BOT_MOVE_SLOW); + ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed (); + return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_succeed); +} + +/* + * FIRST_GO_END_OF_LINE_FAST =move_fsm_failed=> + * => FIRST_GO_END_OF_LINE_SLOW + * set slow speed + * retry + */ +fsm_branch_t +ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_failed (void) +{ + asserv_set_speed (BOT_MOVE_SLOW); + ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (); + return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed); +} + +/* + * FIRST_GO_END_OF_LINE_SLOW =move_fsm_succeed=> + * => IDLE + */ +fsm_branch_t +ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_succeed (void) +{ + return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_succeed); } /* - * GO_NEAR =move_fsm_failed=> - * => GO_FAR - * restart + * FIRST_GO_END_OF_LINE_SLOW =move_fsm_failed=> + * => FIRST_GO_END_OF_LINE_SLOW + * retry */ fsm_branch_t -ai__GO_NEAR__move_fsm_failed (void) +ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (void) { - ai__GO_NEAR__move_fsm_succeed (); - return ai_next (GO_NEAR, move_fsm_failed); + position_t pos = PG_POSITION_DEG (2625, 253, -90); + move_start (pos, 0); + return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed); } diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h index c3960099..df99aacb 100644 --- a/digital/io/src/bot.h +++ b/digital/io/src/bot.h @@ -65,6 +65,12 @@ */ #define BOT_MIN_DISTANCE_TURN_FREE 300 +/** Fast parameters. */ +#define BOT_MOVE_FAST 0x50, 0x50, 0x20, 0x20 + +/** Slow parameters. */ +#define BOT_MOVE_SLOW 0x20, 0x20, 0x10, 0x10 + /** Elevator stroke in steps. */ #define BOT_ELEVATOR_STROKE_STEP 7089 diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index c3581c54..a46929f0 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -7,10 +7,12 @@ States: waiting for the beginning of the top FSM. WAIT_INIT_TO_FINISH waiting for the end of init. - GO_FAR - testing move fsm, going to a far point. - GO_NEAR - testing move fsm, going to a near point. + FIRST_GO_BEGIN_OF_LINE_FAST + first sequence, go to start of diagonal line + FIRST_GO_END_OF_LINE_FAST + first sequence, go to end of line + FIRST_GO_END_OF_LINE_SLOW + slow down to take elements Events: start @@ -41,18 +43,30 @@ IDLE: nothing to do. WAIT_INIT_TO_FINISH: - init_match_is_started -> GO_FAR + init_match_is_started -> FIRST_GO_BEGIN_OF_LINE_FAST the match start - go to a far point - -GO_FAR: - move_fsm_succeed -> GO_NEAR - go to a near point - move_fsm_failed -> GO_NEAR - go to a near point - -GO_NEAR: - move_fsm_succeed -> GO_FAR - restart - move_fsm_failed -> GO_FAR - restart + set fast speed + go to first sequence begin of line + +FIRST_GO_BEGIN_OF_LINE_FAST: + move_fsm_succeed -> FIRST_GO_END_OF_LINE_FAST + go to end of line + loader down + move_fsm_failed -> . + set slow speed + retry + +FIRST_GO_END_OF_LINE_FAST: + in_field -> FIRST_GO_END_OF_LINE_SLOW + set slow speed + move_fsm_succeed -> IDLE + set slow speed + move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW + set slow speed + retry + +FIRST_GO_END_OF_LINE_SLOW: + move_fsm_succeed -> IDLE + move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW + retry + -- cgit v1.2.3