From 00daa402978eb67ffcff2b6751425c26ce55c716 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 1 Apr 2010 23:57:49 +0200 Subject: digital/io/src: add very basic top behaviour to test move FSM --- digital/io/src/ai_top_cb.c | 42 ++++++++++++++++++++++++++++++++++++++++-- digital/io/src/top.fsm | 16 ++++++++++++++-- 2 files changed, 54 insertions(+), 4 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index 5fbe703f..9c47e5ca 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -51,12 +51,50 @@ ai__IDLE__start (void) /* * WAIT_INIT_TO_FINISH =init_match_is_started=> - * => WAIT_INIT_TO_FINISH - * the match start, place something interresting here + * => GO_FAR + * the match start + * go to a far point */ fsm_branch_t ai__WAIT_INIT_TO_FINISH__init_match_is_started (void) { + asserv_position_t pos; + pos.x = PG_X_VALUE_COMPUTING (2000); + pos.y = 1000; + pos.a = PG_A_VALUE_COMPUTING (0); + 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 + */ +fsm_branch_t +ai__GO_FAR__move_fsm_succeed (void) +{ + asserv_position_t pos; + pos.x = PG_X_VALUE_COMPUTING (1000); + pos.y = 1000; + pos.a = PG_A_VALUE_COMPUTING (0); + move_start (pos, 0); + return ai_next (GO_FAR, move_fsm_succeed); +} + +/* + * GO_NEAR =move_fsm_succeed=> + * => GO_FAR + * restart + */ +fsm_branch_t +ai__GO_NEAR__move_fsm_succeed (void) +{ + asserv_position_t pos; + pos.x = PG_X_VALUE_COMPUTING (2000); + pos.y = 1000; + pos.a = PG_A_VALUE_COMPUTING (0); + move_start (pos, 0); + return ai_next (GO_NEAR, move_fsm_succeed); +} + diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index dde79a51..492f286d 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -7,6 +7,10 @@ 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. Events: start @@ -34,6 +38,14 @@ IDLE: nothing to do. WAIT_INIT_TO_FINISH: - init_match_is_started -> . - the match start, place something interresting here + init_match_is_started -> GO_FAR + the match start + go to a far point +GO_FAR: + move_fsm_succeed -> GO_NEAR + go to a near point + +GO_NEAR: + move_fsm_succeed -> GO_FAR + restart -- cgit v1.2.3