From c82623b6b12153e697e2226a56ce44773875a3d0 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 6 May 2012 16:01:02 +0200 Subject: digital/io-hub/src/guybrush: push bottle --- digital/io-hub/src/guybrush/playground_2012.h | 6 +++ digital/io-hub/src/guybrush/strat.c | 23 ++++++++++- digital/io-hub/src/guybrush/strat.h | 2 + digital/io-hub/src/guybrush/top.c | 57 +++++++++++++++++++++++++++ 4 files changed, 87 insertions(+), 1 deletion(-) (limited to 'digital') diff --git a/digital/io-hub/src/guybrush/playground_2012.h b/digital/io-hub/src/guybrush/playground_2012.h index 79017d3b..1bb68a88 100644 --- a/digital/io-hub/src/guybrush/playground_2012.h +++ b/digital/io-hub/src/guybrush/playground_2012.h @@ -53,4 +53,10 @@ #define PG_CAPTAIN_ROOM_WIDTH_MM 400 #define PG_CAPTAIN_ROOM_LENGTH_MM 500 +/** Bottles, align on path grid. */ +#define PG_BOTTLE0_X (PG_TOTEM_LEFT_X - PATH_TOTEM_CLEAR_MM) +#define PG_BOTTLE1_X PG_TOTEM_LEFT_X +#define PG_BOTTLE2_X PG_TOTEM_RIGHT_X +#define PG_BOTTLE3_X (PG_TOTEM_RIGHT_X + PATH_TOTEM_CLEAR_MM) + #endif /* playground_2012_h */ diff --git a/digital/io-hub/src/guybrush/strat.c b/digital/io-hub/src/guybrush/strat.c index 6ed38d01..a59fb4e1 100644 --- a/digital/io-hub/src/guybrush/strat.c +++ b/digital/io-hub/src/guybrush/strat.c @@ -40,6 +40,11 @@ enum STRAT_PLACE_TOTEM1, STRAT_PLACE_TOTEM2, STRAT_PLACE_TOTEM3, + /** Message in a bottle. */ + STRAT_PLACE_BOTTLE0, + STRAT_PLACE_BOTTLE1, + STRAT_PLACE_BOTTLE2, + STRAT_PLACE_BOTTLE3, /** Number of places, should be last. */ STRAT_PLACE_NB }; @@ -63,6 +68,10 @@ static const struct strat_place_t strat_place[STRAT_PLACE_NB] = { PG_LENGTH / 2 - PATH_TOTEM_CLEAR_MM }, 100, STRAT_DECISION_TOTEM }, { { PG_WIDTH / 2 + PG_TOTEM_X_OFFSET_MM, PG_LENGTH / 2 - PATH_TOTEM_CLEAR_MM }, 100, STRAT_DECISION_TOTEM }, + { { PG_BOTTLE0_X, BOT_SIZE_RADIUS + 70 }, 100, STRAT_DECISION_BOTTLE }, + { { PG_BOTTLE1_X, BOT_SIZE_RADIUS + 70 }, 100, STRAT_DECISION_BOTTLE }, + { { PG_BOTTLE2_X, BOT_SIZE_RADIUS + 70 }, 100, STRAT_DECISION_BOTTLE }, + { { PG_BOTTLE3_X, BOT_SIZE_RADIUS + 70 }, 100, STRAT_DECISION_BOTTLE }, }; /** Place dynamic information. */ @@ -93,6 +102,16 @@ strat_init (void) strat.last_decision = -1; for (i = 0; i < STRAT_PLACE_NB; i++) strat.place[i].valid = 1; + if (team_color) + { + strat.place[STRAT_PLACE_BOTTLE1].valid = 0; + strat.place[STRAT_PLACE_BOTTLE3].valid = 0; + } + else + { + strat.place[STRAT_PLACE_BOTTLE0].valid = 0; + strat.place[STRAT_PLACE_BOTTLE2].valid = 0; + } } uint8_t @@ -126,8 +145,10 @@ strat_success (void) switch (strat.last_decision) { case STRAT_DECISION_TOTEM: - strat.place[strat.last_place].valid = 0; strat.load++; + /* no break; */ + case STRAT_DECISION_BOTTLE: + strat.place[strat.last_place].valid = 0; break; case STRAT_DECISION_UNLOAD: strat.load = 0; diff --git a/digital/io-hub/src/guybrush/strat.h b/digital/io-hub/src/guybrush/strat.h index 8ecf14b4..53654bba 100644 --- a/digital/io-hub/src/guybrush/strat.h +++ b/digital/io-hub/src/guybrush/strat.h @@ -30,6 +30,8 @@ enum { /** Go collect items on a totem side. */ STRAT_DECISION_TOTEM, + /** Go push a bottle button. */ + STRAT_DECISION_BOTTLE, /** Go unload everything in our boat. */ STRAT_DECISION_UNLOAD, }; diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c index 439ac1a6..5de7dc85 100644 --- a/digital/io-hub/src/guybrush/top.c +++ b/digital/io-hub/src/guybrush/top.c @@ -60,6 +60,15 @@ FSM_STATES ( /* Going back after totem has been emptied. */ TOP_TOTEM_GOING_BACK, + /* Going to push a bottle. */ + TOP_BOTTLE_GOING, + /* Approaching the button. */ + TOP_BOTTLE_APPROACHING, + /* Push the button. */ + TOP_BOTTLE_PUSHING, + /* Going back after a bottle has been pushed. */ + TOP_BOTTLE_GOING_BACK, + /* Going to an unload position. */ TOP_UNLOAD_GOING, /* Unloading, waiting for elements to fall. */ @@ -88,6 +97,16 @@ top_go_totem (void) move_start (pos, 0); } +/** Go push a bottle button. */ +static void +top_go_bottle (void) +{ + position_t pos; + pos.v = top.decision_pos; + pos.a = POSITION_A_DEG (90); + move_start (pos, 0); +} + /** Go unload. */ static void top_go_unload (void) @@ -109,6 +128,9 @@ top_decision (void) case STRAT_DECISION_TOTEM: top_go_totem (); break; + case STRAT_DECISION_BOTTLE: + top_go_bottle (); + break; case STRAT_DECISION_UNLOAD: top_go_unload (); break; @@ -124,6 +146,8 @@ top_decision (void) default: assert (0); \ case STRAT_DECISION_TOTEM: \ return FSM_NEXT (state, event, totem); \ + case STRAT_DECISION_BOTTLE: \ + return FSM_NEXT (state, event, bottle); \ case STRAT_DECISION_UNLOAD: \ return FSM_NEXT (state, event, unload); \ } \ @@ -131,6 +155,7 @@ top_decision (void) FSM_TRANS (TOP_START, init_start_round, totem, TOP_TOTEM_GOING, + bottle, TOP_BOTTLE_GOING, unload, TOP_UNLOAD_GOING) { strat_init (); @@ -161,11 +186,42 @@ FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_success, TOP_TOTEM_GOING_BACK) FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, totem, TOP_TOTEM_GOING, + bottle, TOP_BOTTLE_GOING, unload, TOP_UNLOAD_GOING) { RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_GOING_BACK, move_success); } +/** BOTTLE */ + +FSM_TRANS (TOP_BOTTLE_GOING, move_success, TOP_BOTTLE_APPROACHING) +{ + asserv_move_linearly (-(BOT_SIZE_RADIUS + 70 - BOT_SIZE_BACK - 22 - 30)); + return FSM_NEXT (TOP_BOTTLE_GOING, move_success); +} + +FSM_TRANS (TOP_BOTTLE_APPROACHING, robot_move_success, TOP_BOTTLE_PUSHING) +{ + asserv_push_the_wall (ASSERV_BACKWARD, -1, -1, -1); + return FSM_NEXT (TOP_BOTTLE_APPROACHING, robot_move_success); +} + +FSM_TRANS (TOP_BOTTLE_PUSHING, robot_move_success, TOP_BOTTLE_GOING_BACK) +{ + asserv_stop_motor (); + strat_success (); + move_start_noangle (top.decision_pos, 0, 0); + return FSM_NEXT (TOP_BOTTLE_PUSHING, robot_move_success); +} + +FSM_TRANS (TOP_BOTTLE_GOING_BACK, move_success, + totem, TOP_TOTEM_GOING, + bottle, TOP_BOTTLE_GOING, + unload, TOP_UNLOAD_GOING) +{ + RETURN_TOP_DECISION_SWITCH (TOP_BOTTLE_GOING_BACK, move_success); +} + /** UNLOAD */ FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING) @@ -177,6 +233,7 @@ FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING) FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, totem, TOP_TOTEM_GOING, + bottle, TOP_BOTTLE_GOING, unload, TOP_UNLOAD_GOING) { strat_success (); -- cgit v1.2.3