summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-12 19:01:20 +0200
committerNicolas Schodet2010-05-12 19:01:20 +0200
commit8f70e520c9c4ce504f8368346d8b85df27932ebe (patch)
treeb2ae9f58601ee00bced921758fca704937dd457c /digital
parent1ff9a0a61718a52f60a770a40aaf0915f77cb707 (diff)
digital/io/src: homologation initial sequence
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_top_cb.c97
-rw-r--r--digital/io/src/bot.h6
-rw-r--r--digital/io/src/top.fsm50
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
+