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/Makefile | 2 +- digital/io-hub/src/guybrush/path.c | 7 -- digital/io-hub/src/guybrush/path.h | 7 ++ digital/io-hub/src/guybrush/strat.c | 120 +++++++++++++++++++++++++++++++++++ digital/io-hub/src/guybrush/strat.h | 47 ++++++++++++++ digital/io-hub/src/guybrush/top.c | 79 ++++++++++++++++++++++- 6 files changed, 251 insertions(+), 11 deletions(-) create mode 100644 digital/io-hub/src/guybrush/strat.c create mode 100644 digital/io-hub/src/guybrush/strat.h diff --git a/digital/io-hub/src/guybrush/Makefile b/digital/io-hub/src/guybrush/Makefile index 9099dc20..ecb7ff28 100644 --- a/digital/io-hub/src/guybrush/Makefile +++ b/digital/io-hub/src/guybrush/Makefile @@ -3,7 +3,7 @@ BASE = ../../../avr # Name of the program to build. PROGS = io_hub # Sources to compile. -io_hub_SOURCES = main.c top.c \ +io_hub_SOURCES = main.c top.c strat.c \ radar_defs.c radar.c path.c move.c \ pressure.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ diff --git a/digital/io-hub/src/guybrush/path.c b/digital/io-hub/src/guybrush/path.c index 177f7d8c..735982ba 100644 --- a/digital/io-hub/src/guybrush/path.c +++ b/digital/io-hub/src/guybrush/path.c @@ -45,10 +45,6 @@ * path along nodes. */ -/** Clearance between obstacle and robot center, only used for grid - * construction. */ -#define PATH_GRID_CLEARANCE_MM (70 + BOT_SIZE_RADIUS) - /** Number of possible obstacles. */ #define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB @@ -132,9 +128,6 @@ static const struct path_node_t path_nodes[PATH_FIXED_NODES_NB] = { /* }}} */ }; -/** Shortcut. */ -#define PATH_TOTEM_CLEAR_MM (PG_TOTEM_WIDTH_MM / 2 + PATH_GRID_CLEARANCE_MM) - /** X position of columns. */ static const uint16_t path_nodes_x[PATH_COLUMNS_NB] = { PG_TOTEM_LEFT_X - PATH_TOTEM_CLEAR_MM, diff --git a/digital/io-hub/src/guybrush/path.h b/digital/io-hub/src/guybrush/path.h index 35f5cd69..53c32a15 100644 --- a/digital/io-hub/src/guybrush/path.h +++ b/digital/io-hub/src/guybrush/path.h @@ -32,6 +32,13 @@ /** Infinite validity for an obstacle. */ #define PATH_OBSTACLE_VALID_ALWAYS 0xffff +/** Clearance between obstacle and robot center, only used for grid + * construction. */ +#define PATH_GRID_CLEARANCE_MM (70 + BOT_SIZE_RADIUS) + +/** Shortcut. */ +#define PATH_TOTEM_CLEAR_MM (PG_TOTEM_WIDTH_MM / 2 + PATH_GRID_CLEARANCE_MM) + /** Obstacle. */ struct path_obstacle_t { diff --git a/digital/io-hub/src/guybrush/strat.c b/digital/io-hub/src/guybrush/strat.c new file mode 100644 index 00000000..fa1908e5 --- /dev/null +++ b/digital/io-hub/src/guybrush/strat.c @@ -0,0 +1,120 @@ +/* strat.c */ +/* guybrush - Eurobot 2012 AI. {{{ + * + * Copyright (C) 2012 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "strat.h" + +#include "playground_2012.h" +#include "bot.h" +#include "path.h" + +/* + * This file implements strategic decisions. + */ + +enum +{ + /** The four collecting places for totems. */ + STRAT_PLACE_TOTEM0, + STRAT_PLACE_TOTEM1, + STRAT_PLACE_TOTEM2, + STRAT_PLACE_TOTEM3, + /** Number of places, should be last. */ + STRAT_PLACE_NB +}; + +/** Place static information. */ +struct strat_place_t +{ + /** Collect position. */ + vect_t pos; + /** Static score. */ + uint8_t score; + /** Decision code. */ + uint8_t decision; +}; +static const struct strat_place_t strat_place[STRAT_PLACE_NB] = { + { { PG_WIDTH / 2 - PG_TOTEM_X_OFFSET_MM, + 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_WIDTH / 2 - PG_TOTEM_X_OFFSET_MM, + 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 }, +}; + +/** Place dynamic information. */ +struct strat_place_dyn_t +{ + /** Valid (not collected yet). */ + uint8_t valid; +}; + +/** Strat context. */ +struct strat_t +{ + /** Last decision. */ + uint8_t last_decision; + /** Place of last decision. */ + uint8_t last_place; + /** Places information. */ + struct strat_place_dyn_t place[STRAT_PLACE_NB]; +}; +static struct strat_t strat; + +void +strat_init (void) +{ + uint8_t i; + strat.last_decision = -1; + for (i = 0; i < STRAT_PLACE_NB; i++) + strat.place[i].valid = 1; +} + +uint8_t +strat_decision (vect_t *pos) +{ + uint8_t i; + for (i = 0; i < STRAT_PLACE_NB; i++) + { + if (strat.place[i].valid) + { + *pos = strat_place[i].pos; + strat.last_decision = strat_place[i].decision; + strat.last_place = i; + return strat.last_decision; + } + } + /* Nothing yet, crash. */ + return -1; +} + +void +strat_success (void) +{ + assert (strat.last_decision != (uint8_t) -1); + strat.place[strat.last_place].valid = 0; +} + diff --git a/digital/io-hub/src/guybrush/strat.h b/digital/io-hub/src/guybrush/strat.h new file mode 100644 index 00000000..33839a75 --- /dev/null +++ b/digital/io-hub/src/guybrush/strat.h @@ -0,0 +1,47 @@ +#ifndef strat_h +#define strat_h +/* strat.h */ +/* guybrush - Eurobot 2012 AI. {{{ + * + * Copyright (C) 2012 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "defs.h" + +enum +{ + /** Go collect items on a totem side. */ + STRAT_DECISION_TOTEM, +}; + +/** Initialise strategy once color is known. */ +void +strat_init (void); + +/** Take a decision, return its code. */ +uint8_t +strat_decision (vect_t *pos); + +/** Report a success of last decision. */ +void +strat_success (void); + +#endif /* strat_h */ 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