From 45af370d9be47e5d180ed172e780a36924519987 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 2 May 2012 21:31:39 +0200 Subject: digital/io-hub/src/guybrush: first dumb top FSM --- digital/io-hub/src/guybrush/top.c | 79 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 76 insertions(+), 3 deletions(-) (limited to 'digital/io-hub/src/guybrush/top.c') 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); +} -- cgit v1.2.3