summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/guybrush/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-02 21:31:39 +0200
committerNicolas Schodet2012-05-07 16:31:46 +0200
commit45af370d9be47e5d180ed172e780a36924519987 (patch)
tree3e634f7964c816cd46007add302109786af78b7e /digital/io-hub/src/guybrush/top.c
parent7432f74c696b7b84c1fbfb2c71a209863d5c1631 (diff)
digital/io-hub/src/guybrush: first dumb top FSM
Diffstat (limited to 'digital/io-hub/src/guybrush/top.c')
-rw-r--r--digital/io-hub/src/guybrush/top.c79
1 files changed, 76 insertions, 3 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 15c3854d..fd979e85 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -35,6 +35,9 @@
#include "chrono.h"
#include "contact.h"
+#include "strat.h"
+#include "path.h"
+
/*
* Here is the top FSM. This FSM is suppose to give life to the robot with an
* impression of intelligence... Well...
@@ -44,16 +47,86 @@ FSM_INIT
FSM_STATES (
/* Initial state. */
- TOP_START)
+ TOP_START,
+ /* Going to a collect position above or below a totem. */
+ TOP_TOTEM_GOING,
+ /* Approaching a totem. */
+ TOP_TOTEM_APPROACHING,
+ /* Pushing until full contact. */
+ TOP_TOTEM_PUSHING,
+ /* Going back after totem has been emptied. */
+ TOP_TOTEM_GOING_BACK)
FSM_START_WITH (TOP_START)
/** Top context. */
struct top_t
{
+ /** Decision position. */
+ vect_t decision_pos;
};
/** Global context. */
-struct top_t top_global;
-#define ctx top_global
+struct top_t top;
+
+/** Go collect a totem. */
+static void
+top_go_totem (void)
+{
+ position_t pos;
+ pos.v = top.decision_pos;
+ pos.a = pos.v.y > PG_LENGTH / 2 ? POSITION_A_DEG (-90)
+ : POSITION_A_DEG (90);
+ move_start (pos, 0);
+}
+
+/** Call strat to make a decision, apply it and return the decision to go to
+ * the next state. */
+static uint8_t
+top_decision (void)
+{
+ uint8_t decision = strat_decision (&top.decision_pos);
+ switch (decision)
+ {
+ case STRAT_DECISION_TOTEM:
+ top_go_totem ();
+ break;
+ default:
+ assert (0);
+ }
+ return decision;
+}
+
+FSM_TRANS (TOP_START, init_start_round, TOP_TOTEM_GOING)
+{
+ strat_init ();
+ top_decision ();
+ return FSM_NEXT (TOP_START, init_start_round);
+}
+
+FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_APPROACHING)
+{
+ asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 30);
+ return FSM_NEXT (TOP_TOTEM_GOING, move_success);
+}
+
+FSM_TRANS (TOP_TOTEM_APPROACHING, robot_move_success, TOP_TOTEM_PUSHING)
+{
+ asserv_push_the_wall (0, -1, -1, -1);
+ return FSM_NEXT (TOP_TOTEM_APPROACHING, robot_move_success);
+}
+
+FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_success, TOP_TOTEM_GOING_BACK)
+{
+ asserv_stop_motor ();
+ strat_success ();
+ move_start_noangle (top.decision_pos, ASSERV_BACKWARD, 0);
+ return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_success);
+}
+
+FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, TOP_TOTEM_GOING)
+{
+ top_decision ();
+ return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_success);
+}