From f78d43b5767db1ef7e39c871202851fc5d5360c9 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 4 May 2012 02:24:26 +0200 Subject: digital/io-hub/src/guybrush: first unload implementation --- digital/io-hub/src/guybrush/strat.c | 23 +++++++++++-- digital/io-hub/src/guybrush/strat.h | 2 ++ digital/io-hub/src/guybrush/top.c | 69 +++++++++++++++++++++++++++++++++---- 3 files changed, 85 insertions(+), 9 deletions(-) (limited to 'digital/io-hub/src') diff --git a/digital/io-hub/src/guybrush/strat.c b/digital/io-hub/src/guybrush/strat.c index fa1908e5..6ed38d01 100644 --- a/digital/io-hub/src/guybrush/strat.c +++ b/digital/io-hub/src/guybrush/strat.c @@ -79,6 +79,8 @@ struct strat_t uint8_t last_decision; /** Place of last decision. */ uint8_t last_place; + /** Robot content estimation. */ + uint8_t load; /** Places information. */ struct strat_place_dyn_t place[STRAT_PLACE_NB]; }; @@ -97,6 +99,13 @@ uint8_t strat_decision (vect_t *pos) { uint8_t i; + if (strat.load > 1) + { + strat.last_decision = STRAT_DECISION_UNLOAD; + pos->x = PG_X (BOT_SIZE_RADIUS + 30); + pos->y = PG_Y (PG_LENGTH / 2); + return strat.last_decision; + } for (i = 0; i < STRAT_PLACE_NB; i++) { if (strat.place[i].valid) @@ -114,7 +123,17 @@ strat_decision (vect_t *pos) void strat_success (void) { - assert (strat.last_decision != (uint8_t) -1); - strat.place[strat.last_place].valid = 0; + switch (strat.last_decision) + { + case STRAT_DECISION_TOTEM: + strat.place[strat.last_place].valid = 0; + strat.load++; + break; + case STRAT_DECISION_UNLOAD: + strat.load = 0; + break; + default: + assert (0); + } } diff --git a/digital/io-hub/src/guybrush/strat.h b/digital/io-hub/src/guybrush/strat.h index 33839a75..8ecf14b4 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 unload everything in our boat. */ + STRAT_DECISION_UNLOAD, }; /** Initialise strategy once color is known. */ diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c index fd979e85..439ac1a6 100644 --- a/digital/io-hub/src/guybrush/top.c +++ b/digital/io-hub/src/guybrush/top.c @@ -38,6 +38,8 @@ #include "strat.h" #include "path.h" +#include "output_defs.h" + /* * Here is the top FSM. This FSM is suppose to give life to the robot with an * impression of intelligence... Well... @@ -48,6 +50,7 @@ FSM_INIT FSM_STATES ( /* Initial state. */ TOP_START, + /* Going to a collect position above or below a totem. */ TOP_TOTEM_GOING, /* Approaching a totem. */ @@ -55,7 +58,12 @@ FSM_STATES ( /* Pushing until full contact. */ TOP_TOTEM_PUSHING, /* Going back after totem has been emptied. */ - TOP_TOTEM_GOING_BACK) + TOP_TOTEM_GOING_BACK, + + /* Going to an unload position. */ + TOP_UNLOAD_GOING, + /* Unloading, waiting for elements to fall. */ + TOP_UNLOADING) FSM_START_WITH (TOP_START) @@ -80,6 +88,16 @@ top_go_totem (void) move_start (pos, 0); } +/** Go unload. */ +static void +top_go_unload (void) +{ + position_t pos; + pos.v = top.decision_pos; + pos.a = PG_A_DEG (70); + 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 @@ -91,19 +109,36 @@ top_decision (void) case STRAT_DECISION_TOTEM: top_go_totem (); break; + case STRAT_DECISION_UNLOAD: + top_go_unload (); + break; default: assert (0); } return decision; } -FSM_TRANS (TOP_START, init_start_round, TOP_TOTEM_GOING) +#define RETURN_TOP_DECISION_SWITCH(state, event) do { \ + switch (top_decision ()) \ + { \ + default: assert (0); \ + case STRAT_DECISION_TOTEM: \ + return FSM_NEXT (state, event, totem); \ + case STRAT_DECISION_UNLOAD: \ + return FSM_NEXT (state, event, unload); \ + } \ +} while (0) + +FSM_TRANS (TOP_START, init_start_round, + totem, TOP_TOTEM_GOING, + unload, TOP_UNLOAD_GOING) { strat_init (); - top_decision (); - return FSM_NEXT (TOP_START, init_start_round); + RETURN_TOP_DECISION_SWITCH (TOP_START, init_start_round); } +/** TOTEM */ + FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_APPROACHING) { asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 30); @@ -124,9 +159,29 @@ FSM_TRANS (TOP_TOTEM_PUSHING, robot_move_success, TOP_TOTEM_GOING_BACK) return FSM_NEXT (TOP_TOTEM_PUSHING, robot_move_success); } -FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, TOP_TOTEM_GOING) +FSM_TRANS (TOP_TOTEM_GOING_BACK, move_success, + totem, TOP_TOTEM_GOING, + unload, TOP_UNLOAD_GOING) +{ + RETURN_TOP_DECISION_SWITCH (TOP_TOTEM_GOING_BACK, move_success); +} + +/** UNLOAD */ + +FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING) { - top_decision (); - return FSM_NEXT (TOP_TOTEM_GOING_BACK, move_success); + IO_SET (OUTPUT_DOOR_OPEN); + IO_CLR (OUTPUT_DOOR_CLOSE); + return FSM_NEXT (TOP_UNLOAD_GOING, move_success); +} + +FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, + totem, TOP_TOTEM_GOING, + unload, TOP_UNLOAD_GOING) +{ + strat_success (); + IO_CLR (OUTPUT_DOOR_OPEN); + IO_SET (OUTPUT_DOOR_CLOSE); + RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING, TOP_UNLOADING_TIMEOUT); } -- cgit v1.2.3