From c912f3edd803e4d8e6664e360590a0a538fbc7e5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 26 Mar 2012 00:57:00 +0200 Subject: digital/io-hub/src/guybrush: initial guybrush almost empty program --- digital/io-hub/src/guybrush/Makefile | 39 ++ digital/io-hub/src/guybrush/README | 24 ++ digital/io-hub/src/guybrush/avrconfig.h | 135 +++++++ digital/io-hub/src/guybrush/bot.h | 58 +++ digital/io-hub/src/guybrush/contact_defs.h | 35 ++ digital/io-hub/src/guybrush/init_defs.h | 50 +++ digital/io-hub/src/guybrush/main.c | 286 ++++++++++++++ digital/io-hub/src/guybrush/main.h | 31 ++ digital/io-hub/src/guybrush/move.c | 536 ++++++++++++++++++++++++++ digital/io-hub/src/guybrush/move.h | 63 +++ digital/io-hub/src/guybrush/path.c | 380 ++++++++++++++++++ digital/io-hub/src/guybrush/path.h | 78 ++++ digital/io-hub/src/guybrush/playground_2012.h | 32 ++ digital/io-hub/src/guybrush/radar_defs.c | 46 +++ digital/io-hub/src/guybrush/radar_defs.h | 41 ++ digital/io-hub/src/guybrush/simu.host.c | 107 +++++ digital/io-hub/src/guybrush/simu.host.h | 46 +++ digital/io-hub/src/guybrush/top.c | 59 +++ 18 files changed, 2046 insertions(+) create mode 100644 digital/io-hub/src/guybrush/Makefile create mode 100644 digital/io-hub/src/guybrush/README create mode 100644 digital/io-hub/src/guybrush/avrconfig.h create mode 100644 digital/io-hub/src/guybrush/bot.h create mode 100644 digital/io-hub/src/guybrush/contact_defs.h create mode 100644 digital/io-hub/src/guybrush/init_defs.h create mode 100644 digital/io-hub/src/guybrush/main.c create mode 100644 digital/io-hub/src/guybrush/main.h create mode 100644 digital/io-hub/src/guybrush/move.c create mode 100644 digital/io-hub/src/guybrush/move.h create mode 100644 digital/io-hub/src/guybrush/path.c create mode 100644 digital/io-hub/src/guybrush/path.h create mode 100644 digital/io-hub/src/guybrush/playground_2012.h create mode 100644 digital/io-hub/src/guybrush/radar_defs.c create mode 100644 digital/io-hub/src/guybrush/radar_defs.h create mode 100644 digital/io-hub/src/guybrush/simu.host.c create mode 100644 digital/io-hub/src/guybrush/simu.host.h create mode 100644 digital/io-hub/src/guybrush/top.c (limited to 'digital/io-hub/src/guybrush') diff --git a/digital/io-hub/src/guybrush/Makefile b/digital/io-hub/src/guybrush/Makefile new file mode 100644 index 00000000..535fba17 --- /dev/null +++ b/digital/io-hub/src/guybrush/Makefile @@ -0,0 +1,39 @@ +# Base directory of AVR. +BASE = ../../../avr +# Name of the program to build. +PROGS = io_hub +# Sources to compile. +io_hub_SOURCES = main.c top.c \ + radar_defs.c radar.c path.c move.c \ + init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ + contact.avr.c contact.host.c \ + twi_master.c asserv.c mimot.c \ + chrono.c timer.avr.c simu.host.c +# Modules needed for IO. +MODULES = proto uart twi utils \ + adc devices/usdist \ + math/fixed math/geometry path/astar +AI_MODULES = twi_master common utils fsm move +# Configuration file. +CONFIGFILE = avrconfig.h +AVR_MCU = at90usb1287 +# Optimize for speed. +OPTIMIZE = -O2 +HOST_LIBS = -lm + +vpath %.c ../common +vpath %.h ../common +vpath %.c $(AI_MODULES:%=../../../ai/src/%) +vpath %.h $(AI_MODULES:%=../../../ai/src/%) +INCLUDES += -I. -I../common $(AI_MODULES:%=-I../../../ai/src/%) + +EXTRA_CLEAN_FILES = fsm_AI_gen.h fsm_AI_gen.avr.c + +include $(BASE)/make/Makefile.gen + +# FSM generation. +obj/main.avr.o: fsm_AI_gen.h +fsm_AI_gen.avr.c: fsm_AI_gen.h +fsm_AI_gen.h: io_hub.host + ./$< --gen + mv fsm_AI_gen.c fsm_AI_gen.avr.c diff --git a/digital/io-hub/src/guybrush/README b/digital/io-hub/src/guybrush/README new file mode 100644 index 00000000..d03aff14 --- /dev/null +++ b/digital/io-hub/src/guybrush/README @@ -0,0 +1,24 @@ +guybrush - Eurobot 2012 AI. + +AI & IO for the APBTeam Robot Guybrush. + + +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. diff --git a/digital/io-hub/src/guybrush/avrconfig.h b/digital/io-hub/src/guybrush/avrconfig.h new file mode 100644 index 00000000..ab581210 --- /dev/null +++ b/digital/io-hub/src/guybrush/avrconfig.h @@ -0,0 +1,135 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.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. + * + * }}} */ + +/* global */ +/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, + * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#define AC_FREQ 16000000 + +/* uart - UART module. */ +/** Select hardware uart for primary uart: 0, 1 or -1 to disable. */ +#define AC_UART0_PORT 1 +/** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800, + * 115200, 230400, 250000, 500000, 1000000. */ +#define AC_UART0_BAUDRATE 38400 +/** Send mode: + * - POLLING: no interrupts. + * - RING: interrupts, ring buffer. */ +#define AC_UART0_SEND_MODE RING +/** Recv mode, same as send mode. */ +#define AC_UART0_RECV_MODE RING +/** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */ +#define AC_UART0_CHAR_SIZE 8 +/** Parity : ODD, EVEN, NONE. */ +#define AC_UART0_PARITY EVEN +/** Stop bits : 1, 2. */ +#define AC_UART0_STOP_BITS 1 +/** Send buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_SEND_BUFFER_SIZE 32 +/** Recv buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_RECV_BUFFER_SIZE 32 +/** If the send buffer is full when putc: + * - DROP: drop the new byte. + * - WAIT: wait until there is room in the send buffer. */ +#define AC_UART0_SEND_BUFFER_FULL WAIT +/** In HOST compilation: + * - STDIO: use stdin/out. + * - PTS: use pseudo terminal. */ +#define AC_UART0_HOST_DRIVER STDIO +/** Same thing for secondary port. */ +#define AC_UART1_PORT -1 +#define AC_UART1_BAUDRATE 115200 +#define AC_UART1_SEND_MODE RING +#define AC_UART1_RECV_MODE RING +#define AC_UART1_CHAR_SIZE 8 +#define AC_UART1_PARITY EVEN +#define AC_UART1_STOP_BITS 1 +#define AC_UART1_SEND_BUFFER_SIZE 32 +#define AC_UART1_RECV_BUFFER_SIZE 32 +#define AC_UART1_SEND_BUFFER_FULL WAIT +#define AC_UART1_HOST_DRIVER PTS + +/* proto - Protocol module. */ +/** Maximum argument size. */ +#define AC_PROTO_ARGS_MAX_SIZE 12 +/** Callback function name. */ +#define AC_PROTO_CALLBACK proto_callback +/** Putchar function name. */ +#define AC_PROTO_PUTC uart0_putc +/** Support for quote parameter. */ +#define AC_PROTO_QUOTE 1 + +/* twi - TWI module. */ +/** Driver to implement TWI: HARD, SOFT, or USI. */ +#define AC_TWI_DRIVER HARD +/** Do not use interrupts. */ +#define AC_TWI_NO_INTERRUPT 0 +/** TWI frequency, should really be 100 kHz. */ +#define AC_TWI_FREQ 100000 +/** Enable slave part. */ +#define AC_TWI_SLAVE_ENABLE 0 +/** Enable master part. */ +#define AC_TWI_MASTER_ENABLE 1 +/** Master transfer completion callback, optionally defined by the user, called + * at end of master transfer. */ +#undef AC_TWI_MASTER_DONE +/** Use internal pull up. */ +#define AC_TWI_PULL_UP 0 + +/* usdist - Analog US distance sensor. */ +/** Number of sensors. */ +#define AC_USDIST_NB 4 +/** Measuring period, in number of update call. */ +#define AC_USDIST_PERIOD 1 +/** List of space separated sensor definition, see usdist.h. */ +#define AC_USDIST_SENSORS \ + USDIST_SENSOR (0, A, 0) \ + USDIST_SENSOR (1, A, 1) \ + USDIST_SENSOR (2, A, 2) \ + USDIST_SENSOR (3, A, 3) + +/* path - Path finding module. */ +/** Report path found for debug. */ +#define AC_PATH_REPORT defined (HOST) +/** Report function name. */ +#define AC_PATH_REPORT_CALLBACK simu_send_path +/** Number of possible obstacles. */ +#define AC_PATH_OBSTACLES_NB 2 + +/* astar - A* path finding module. */ +/** Neighbor callback. */ +#define AC_ASTAR_NEIGHBOR_CALLBACK path_astar_neighbor_callback +/** Heuristic callback. */ +#define AC_ASTAR_HEURISTIC_CALLBACK path_astar_heuristic_callback + +/* io-hub - io/ai board. */ +/** TWI address of the io board. */ +#define AC_IO_TWI_ADDRESS 10 +/** PWM setting. */ +#undef AC_IOHUB_PWM + +#endif /* avrconfig_h */ diff --git a/digital/io-hub/src/guybrush/bot.h b/digital/io-hub/src/guybrush/bot.h new file mode 100644 index 00000000..65b4cb09 --- /dev/null +++ b/digital/io-hub/src/guybrush/bot.h @@ -0,0 +1,58 @@ +#ifndef bot_h +#define bot_h +/* bot.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. + * + * }}} */ + +/** Robot specific defines. */ + +/** Scaling factor, millimeter per step. */ +#ifdef HOST +# define BOT_SCALE 0.0395840674352314 +#else +# define BOT_SCALE 0.0317975134344 +#endif + +/** Distance from the robot axis to the front. */ +#define BOT_SIZE_FRONT 150 +/** Distance from the robot axis to the back. */ +#define BOT_SIZE_BACK 150 +/** Distance from the robot axis to the side. */ +#define BOT_SIZE_SIDE 190 + +/** Distance between the front contact point and the robot center. */ +#define BOT_FRONT_CONTACT_DIST_MM 150 +/** Angle error at the front contact point. */ +#define BOT_FRONT_CONTACT_ANGLE_ERROR_DEG 0 + +/** Speed used for initialisation. */ +#ifdef HOST +# define BOT_SPEED_INIT 0x20, 0x20, 0x20, 0x20 +#else +# define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10 +#endif +/** Normal cruise speed. */ +#define BOT_SPEED_NORMAL 0x50, 0x60, 0x20, 0x20 + +#endif /* bot_h */ diff --git a/digital/io-hub/src/guybrush/contact_defs.h b/digital/io-hub/src/guybrush/contact_defs.h new file mode 100644 index 00000000..55773d78 --- /dev/null +++ b/digital/io-hub/src/guybrush/contact_defs.h @@ -0,0 +1,35 @@ +#ifndef contact_defs_h +#define contact_defs_h +/* contact_defs.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. + * + * }}} */ + +#define CONTACT_COLOR E, 3 +#define CONTACT_JACK E, 5 +#define CONTACT_STRAT E, 1 + +#define CONTACT_LIST \ + CONTACT (CONTACT_STRAT) + +#endif /* contact_defs_h */ diff --git a/digital/io-hub/src/guybrush/init_defs.h b/digital/io-hub/src/guybrush/init_defs.h new file mode 100644 index 00000000..128d694e --- /dev/null +++ b/digital/io-hub/src/guybrush/init_defs.h @@ -0,0 +1,50 @@ +#ifndef init_defs_h +#define init_defs_h +/* init_defs.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 "playground_2012.h" +#include "bot.h" + +/** Parameters to push the first wall. */ +#define INIT_FIRST_WALL_PUSH \ + 0, PG_X (BOT_FRONT_CONTACT_DIST_MM), 200, \ + PG_A_DEG (180 + BOT_FRONT_CONTACT_ANGLE_ERROR_DEG) +/** Parameters to go away from the first wall. */ +#define INIT_FIRST_WALL_AWAY -500 +/** Parameter to face the second wall. */ +#define INIT_SECOND_WALL_ANGLE PG_A_DEG (90) +/** Parameters to push the second wall. */ +#define INIT_SECOND_WALL_PUSH \ + 0, -1, PG_Y (PG_LENGTH - BOT_FRONT_CONTACT_DIST_MM), -1 +/** Parameters to go away from the second wall. */ +#define INIT_SECOND_WALL_AWAY -(200 - BOT_FRONT_CONTACT_DIST_MM) +/** Parameter to face the start position. */ +#define INIT_START_POSITION_ANGLE PG_A_DEG (0) +/** Start position. */ +#define INIT_START_POSITION \ + PG_X (200), PG_Y (PG_LENGTH - 200), PG_A_DEG (0), ASSERV_BACKWARD + +#endif /* init_defs_h */ diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c new file mode 100644 index 00000000..92f9c33b --- /dev/null +++ b/digital/io-hub/src/guybrush/main.c @@ -0,0 +1,286 @@ +/* main.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 "modules/utils/utils.h" +#include "modules/uart/uart.h" +#include "modules/proto/proto.h" + +#include "modules/devices/usdist/usdist.h" + +#include "timer.h" +#include "chrono.h" +#include "simu.host.h" + +#include "asserv.h" +#include "mimot.h" +#include "twi_master.h" + +#include "contact.h" +#include "radar.h" + +#define FSM_NAME AI +#include "fsm.h" +#ifdef HOST +# include +#endif +#include "fsm_queue.h" + +#include "path.h" +#include "move.h" + +#include "bot.h" + +#include "io.h" + +#ifndef HOST +# include +#endif + +/** Our color. */ +enum team_color_e team_color; + +/** Obstacles positions, updated using radar module. */ +vect_t main_obstacles_pos[2]; + +/** Number of obstacles in main_obstacles_pos. */ +uint8_t main_obstacles_nb; + +/** Asserv stats counters. */ +static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; + +/** Contact stats counters. */ +static uint8_t main_stats_contact_, main_stats_contact_cpt_; + +/** US sensors stats counters. */ +static uint8_t main_stats_usdist_, main_stats_usdist_cpt_; + +/** Main initialisation. */ +static void +main_init (void) +{ +#ifndef HOST + /* Disable watchdog (enabled in bootloader). */ + MCUSR &= ~(1 << WDRF); + wdt_disable (); +#endif + /* Serial port */ + uart0_init (); + /* Enable interrupts */ + sei (); + /* Main timer */ + timer_init (); + timer_wait (); + /* TWI communications */ + asserv_init (); + mimot_init (); + twi_master_init (); + /* IO modules. */ + contact_init (); + usdist_init (); + /* AI modules. */ + path_init (); + /* Initialization done. */ + proto_send0 ('z'); +} + +/** Main events management. */ +void +main_event_to_fsm (void) +{ + /* If an event is handled, stop generating any other event, because a + * transition may have invalidated the current robot state. */ +#define FSM_HANDLE_E(fsm, event) \ + do { if (FSM_HANDLE (fsm, event)) return; } while (0) +#define FSM_HANDLE_VAR_E(fsm, event) \ + do { if (FSM_HANDLE_VAR (fsm, event)) return; } while (0) +#define FSM_HANDLE_TIMEOUT_E(fsm) \ + do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0) + /* Update FSM timeouts. */ + FSM_HANDLE_TIMEOUT_E (AI); + /* Motor status. */ + asserv_status_e robot_move_status, mimot_motor0_status, + mimot_motor1_status; + robot_move_status = asserv_move_cmd_status (); + mimot_motor0_status = mimot_motor0_cmd_status (); + mimot_motor1_status = mimot_motor1_cmd_status (); + if (robot_move_status == success) + FSM_HANDLE_E (AI, robot_move_success); + else if (robot_move_status == failure) + FSM_HANDLE_E (AI, robot_move_failure); + /* Jack. */ + if (!contact_get_jack ()) + FSM_HANDLE_E (AI, jack_inserted); + else + FSM_HANDLE_E (AI, jack_removed); + /* Events from the event queue. */ + if (fsm_queue_poll ()) + { + /* We must post the event at the end of this block because if it is + * handled, the function will return and every instruction after will + * never be executed. */ + uint8_t save_event = fsm_queue_pop_event (); + /* Post the event */ + FSM_HANDLE_VAR_E (AI, save_event); + } + /* Check obstables. */ + if (move_check_obstacles ()) + return; +} + +/** Main (and infinite) loop. */ +static void +main_loop (void) +{ + while (1) + { + /* Wait until next cycle. */ + timer_wait (); + /* Update chrono. */ + chrono_update (); + /* Is match over? */ + if (chrono_is_match_over ()) + { + /* End it and block here indefinitely. */ + chrono_end_match (42); + return; + } + /* Handle commands from UART. */ + while (uart0_poll ()) + proto_accept (uart0_getc ()); + /* Update IO modules. */ + contact_update (); + if (usdist_update ()) + { + position_t robot_pos; + asserv_get_position (&robot_pos); + main_obstacles_nb = radar_update (&robot_pos, main_obstacles_pos); + move_obstacles_update (); + simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0); + } + /* Update AI modules. */ + path_decay (); + /* Only manage events if slaves are synchronised. */ + if (twi_master_sync ()) + main_event_to_fsm (); + /* Send stats if requested. */ + if (main_stats_asserv_ && !--main_stats_asserv_cpt_) + { + /* Get current position */ + position_t cur_pos; + asserv_get_position (&cur_pos); + /* Send stats */ + proto_send3w ('A', cur_pos.v.x, cur_pos.v.y, cur_pos.a); + /* Reset stats counter */ + main_stats_asserv_cpt_ = main_stats_asserv_; + } + if (main_stats_contact_ && !--main_stats_contact_cpt_) + { + proto_send1d ('P', contact_all () | (uint32_t) mimot_get_input () << 24); + main_stats_contact_cpt_ = main_stats_contact_; + } + if (main_stats_usdist_ && !--main_stats_usdist_cpt_) + { + proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2], + usdist_mm[3]); + main_stats_usdist_cpt_ = main_stats_usdist_; + } + } +} + +/** Handle received commands. */ +void +proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) +{ +#define c(cmd, size) (cmd << 8 | size) + switch (c (cmd, size)) + { + case c ('z', 0): + /* Reset */ + utils_reset (); + break; + case c ('j', 0): + /* Simulate jack insertion. */ + fsm_queue_post_event (FSM_EVENT (AI, jack_inserted)); + break; + case c ('m', 5): + /* Go to position. + * - 2w: x, y. + * - 1b: backward. */ + { + vect_t position = { v8_to_v16 (args[0], args[1]), + v8_to_v16 (args[2], args[3]) }; + move_start_noangle (position, args[4], 0); + } + break; + case c ('w', 0): + /* Disable all motor control. */ + mimot_motor0_free (); + mimot_motor1_free (); + asserv_free_motor (); + break; + /* Stats commands. + * - b: interval between stats. */ + case c ('A', 1): + /* Position stats. */ + main_stats_asserv_ = main_stats_asserv_cpt_ = args[0]; + break; + case c ('P', 1): + /* Contact stats. */ + main_stats_contact_ = main_stats_contact_cpt_ = args[0]; + break; + case c ('U', 1): + /* US sensors stats. */ + main_stats_usdist_ = main_stats_usdist_cpt_ = args[0]; + break; + default: + /* Unknown commands */ + proto_send0 ('?'); + return; + } + /* When no error, acknowledge commands */ + proto_send (cmd, size, args); +#undef c +} + +int +main (int argc, char **argv) +{ +#ifdef HOST + /* Produce AVR's FSM headers. */ + int i; + if (argc > 1) + for (i = 1; i < argc; i++) + if (strcmp (argv[i], "--gen") == 0) + { + FSM_GENERATE (AVR, 0); + return 0; + } +#endif + avr_init (argc, argv); + main_init (); + main_loop (); + return 0; +} diff --git a/digital/io-hub/src/guybrush/main.h b/digital/io-hub/src/guybrush/main.h new file mode 100644 index 00000000..617096ff --- /dev/null +++ b/digital/io-hub/src/guybrush/main.h @@ -0,0 +1,31 @@ +#ifndef main_h +#define main_h +/* main.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. + * + * }}} */ + +extern vect_t main_obstacles_pos[2]; +extern uint8_t main_obstacles_nb; + +#endif /* main_h */ diff --git a/digital/io-hub/src/guybrush/move.c b/digital/io-hub/src/guybrush/move.c new file mode 100644 index 00000000..3c4c7f46 --- /dev/null +++ b/digital/io-hub/src/guybrush/move.c @@ -0,0 +1,536 @@ +/* move.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 "move.h" + +#include "main.h" +#include "asserv.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" + +#include "radar.h" +#include "path.h" + +#include "modules/utils/utils.h" + +#include + +/** Move context. */ +struct move_t +{ + /** Final position. */ + position_t final; + /** Use angle consign for final point. */ + uint8_t with_angle; + /** Next step. */ + vect_t step; + /** Next step angle. */ + uint16_t step_angle; + /** Next step with_angle. */ + uint8_t step_with_angle; + /** Next step backward. */ + uint8_t step_backward; + /** Non zero means this is a tricky move, slow down, and minimize + * turns. */ + uint8_t slow; + /** Backward direction allowed flag. */ + uint8_t backward_movement_allowed; + /** Try again counter. */ + uint8_t try_again_counter; + /** Dirty fix to know this is the final move. */ + uint8_t final_move; + /** Distance to remove from path. */ + int16_t shorten; +}; + +/* Global context. */ +struct move_t move_data; + +void +move_start (position_t position, uint8_t backward) +{ + /* Set parameters. */ + move_data.final = position; + move_data.with_angle = 1; + move_data.backward_movement_allowed = backward; + move_data.final_move = 0; + move_data.shorten = 0; + /* Reset try counter. */ + move_data.try_again_counter = 3; + /* Start the FSM. */ + fsm_queue_post_event (FSM_EVENT (AI, move_start)); +} + +void +move_start_noangle (vect_t position, uint8_t backward, int16_t shorten) +{ + /* Set parameters. */ + move_data.final.v = position; + move_data.with_angle = 0; + move_data.backward_movement_allowed = backward; + move_data.final_move = 0; + move_data.shorten = shorten; + /* Reset try counter. */ + move_data.try_again_counter = 3; + /* Start the FSM. */ + fsm_queue_post_event (FSM_EVENT (AI, move_start)); +} + +void +move_stop (void) +{ + /* Stop the FSM. */ + fsm_queue_post_event (FSM_EVENT (AI, move_stop)); +} + +void +move_obstacles_update (void) +{ + uint8_t i; + for (i = 0; i < main_obstacles_nb; i++) + path_obstacle (i, main_obstacles_pos[i], MOVE_OBSTACLE_RADIUS, 0, + MOVE_OBSTACLE_VALIDITY); +} + +uint8_t +move_check_obstacles (void) +{ + if (FSM_CAN_HANDLE (AI, obstacle_in_front)) + { + position_t robot_pos; + asserv_get_position (&robot_pos); + if (radar_blocking (&robot_pos.v, &move_data.step, main_obstacles_pos, + main_obstacles_nb)) + if (FSM_HANDLE (AI, obstacle_in_front)) + return 1; + } + return 0; +} + +FSM_STATES ( + /* Waiting for the start order. */ + MOVE_IDLE, + /* Rotating towards next point. */ + MOVE_ROTATING, + /* Moving to a position (intermediate or final). */ + MOVE_MOVING, + /* Moving backward to go away from what is blocking the bot. */ + MOVE_MOVING_BACKWARD_TO_TURN_FREELY, + /* Waiting for obstacle to disappear. */ + MOVE_WAIT_FOR_CLEAR_PATH) + +FSM_EVENTS ( + /* Report from asserv after a successful move command. */ + robot_move_success, + /* Report from asserv after a failed move command. */ + robot_move_failure, + /* Initialize the FSM and start the movement directly. */ + move_start, + /* Stop movement. */ + move_stop, + /* Movement success. */ + move_success, + /* Movement failure. */ + move_failure, + /* The bot has seen something (front is the same when going backward). */ + obstacle_in_front) + +FSM_START_WITH (MOVE_IDLE) + +/** Go to current step, low level function. */ +static void +move_go (void) +{ + vect_t dst = move_data.step; + /* Modify final point if requested. */ + if (move_data.final_move && move_data.shorten) + { + /* Compute a vector from destination to robot with lenght + * 'shorten'. */ + position_t robot_position; + asserv_get_position (&robot_position); + vect_t v = robot_position.v; + vect_sub (&v, &move_data.step); + int16_t d = vect_norm (&v); + if (d > move_data.shorten) + { + vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten); + vect_translate (&dst, &v); + } + } + if (move_data.step_with_angle) + asserv_goto_xya (dst.x, dst.y, move_data.step_angle, + move_data.step_backward); + else + asserv_goto (dst.x, dst.y, move_data.step_backward); +} + +/** Go or rotate toward position, returns 1 for linear move, 2 for angular + * move. */ +static uint8_t +move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle, + uint8_t backward) +{ + position_t robot_position; + asserv_get_position (&robot_position); + uint16_t robot_angle = robot_position.a; + if (backward & ASSERV_BACKWARD) + robot_angle += 0x8000; + /* Remember step. */ + move_data.step = dst; + move_data.step_angle = angle; + move_data.step_with_angle = with_angle; + move_data.step_backward = backward; + /* Compute angle to destination. */ + vect_t v = dst; vect_sub (&v, &robot_position.v); + uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI)); + if (backward & ASSERV_BACKWARD) + dst_angle += 0x8000; + int16_t diff_angle = dst_angle - robot_angle; + if ((backward & ASSERV_REVERT_OK) + && (diff_angle > 0x4000 || diff_angle < -0x4000)) + dst_angle += 0x8000; + int16_t diff = dst_angle - robot_angle; + /* Move or rotate. */ + if (UTILS_ABS (diff) < 0x1000) + { + move_go (); + return 1; + } + else + { + asserv_goto_angle (dst_angle); + return 2; + } +} + +/** Go to next position computed by path module, to be called by + * move_path_init and move_path_next. Returns 1 for linear move, 2 for angular + * move. */ +static uint8_t +move_go_to_next (vect_t dst) +{ + uint8_t r; + /* If it is not the last position. */ + if (dst.x != move_data.final.v.x || dst.y != move_data.final.v.y) + { + /* Not final position. */ + move_data.final_move = 0; + /* Goto without angle. */ + r = move_go_or_rotate (dst, 0, 0, move_data.backward_movement_allowed + | (move_data.slow ? ASSERV_REVERT_OK : 0)); + } + else + { + /* Final position. */ + move_data.final_move = 1; + /* Goto with angle if requested. */ + r = move_go_or_rotate (dst, move_data.final.a, move_data.with_angle, + move_data.backward_movement_allowed); + } + /* Next time, do not use slow. */ + move_data.slow = 0; + return r; +} + +/** Update and go to first position, return non zero if a path is found, 1 for + * linear move, 2 for angular move. */ +static uint8_t +move_path_init (void) +{ + uint8_t found; + vect_t dst; + /* Get the current position */ + position_t current_pos; + asserv_get_position (¤t_pos); + /* Give the current position of the bot to the path module */ + path_endpoints (current_pos.v, move_data.final.v); + /* Update the path module */ + move_data.slow = 0; + path_update (); + found = path_get_next (&dst); + /* If not found, try to escape. */ + if (!found) + { + move_data.slow = 1; + path_escape (8); + path_update (); + found = path_get_next (&dst); + } + /* If found, go. */ + if (found) + { + return move_go_to_next (dst); + } + else + { + /* Error, not final move. */ + move_data.final_move = 0; + return 0; + } +} + +/** Go to next position in path. Returns 1 for linear move, 2 for angular + * move. */ +static uint8_t +move_path_next (void) +{ + vect_t dst; + path_get_next (&dst); + return move_go_to_next (dst); +} + +FSM_TRANS (MOVE_IDLE, move_start, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found, MOVE_IDLE) +{ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_IDLE, move_start, path_found_rotate); + else + return FSM_NEXT (MOVE_IDLE, move_start, path_found); + } + else + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_IDLE, move_start, no_path_found); + } +} + +FSM_TRANS (MOVE_ROTATING, + robot_move_success, + MOVE_MOVING) +{ + move_go (); + return FSM_NEXT (MOVE_ROTATING, robot_move_success); +} + +FSM_TRANS (MOVE_ROTATING, + robot_move_failure, + MOVE_MOVING) +{ + move_go (); + return FSM_NEXT (MOVE_ROTATING, robot_move_failure); +} + +FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1250, + MOVE_MOVING) +{ + move_go (); + return FSM_NEXT_TIMEOUT (MOVE_ROTATING); +} + +FSM_TRANS (MOVE_ROTATING, move_stop, MOVE_IDLE) +{ + asserv_stop_motor (); + return FSM_NEXT (MOVE_ROTATING, move_stop); +} + +FSM_TRANS (MOVE_MOVING, robot_move_success, + done, MOVE_IDLE, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found, MOVE_IDLE) +{ + if (move_data.final_move) + { + fsm_queue_post_event (FSM_EVENT (AI, move_success)); + return FSM_NEXT (MOVE_MOVING, robot_move_success, done); + } + else + { + uint8_t next = move_path_next (); + if (next == 2) + return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found); + } + //return FSM_NEXT (MOVE_MOVING, robot_move_success, no_path_found); +} + +static void +move_moving_backward_to_turn_freely (void) +{ + move_data.final_move = 0; + /* Assume there is an obstacle in front of the robot. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + vect_t obstacle_pos; + int16_t dist = asserv_get_last_moving_direction () == DIRECTION_FORWARD + ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS + : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS); + vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a); + vect_translate (&obstacle_pos, &robot_pos.v); + path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0, + MOVE_OBSTACLE_VALIDITY); + /* Move backward to turn freely. */ + asserv_move_linearly (asserv_get_last_moving_direction () + == DIRECTION_FORWARD ? -300 : 300); +} + +FSM_TRANS (MOVE_MOVING, + robot_move_failure, + MOVE_MOVING_BACKWARD_TO_TURN_FREELY) +{ + move_moving_backward_to_turn_freely (); + return FSM_NEXT (MOVE_MOVING, robot_move_failure); +} + +FSM_TRANS_TIMEOUT (MOVE_MOVING, 2500, + MOVE_MOVING_BACKWARD_TO_TURN_FREELY) +{ + move_moving_backward_to_turn_freely (); + return FSM_NEXT_TIMEOUT (MOVE_MOVING); +} + +FSM_TRANS (MOVE_MOVING, obstacle_in_front, + tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + tryout, MOVE_IDLE) +{ + move_data.final_move = 0; + asserv_stop_motor (); + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout); + } + else + return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain); +} + +FSM_TRANS (MOVE_MOVING, move_stop, MOVE_IDLE) +{ + asserv_stop_motor (); + return FSM_NEXT (MOVE_MOVING, move_stop); +} + +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, + tryout, MOVE_IDLE, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found, MOVE_IDLE) +{ + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, tryout); + } + else + { + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found); + } + else + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, no_path_found); + } + } +} + +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, + tryout, MOVE_IDLE, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + no_path_found_tryout, MOVE_IDLE) +{ + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, tryout); + } + else + { + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found); + } + else + { + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryout); + } + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryagain); + } + } +} + +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop, MOVE_IDLE) +{ + asserv_stop_motor (); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop); +} + +FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + no_path_found_tryout, MOVE_IDLE) +{ + /* Try to move. */ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found_rotate); + else + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found); + } + else + { + /* Error, no new position, should we try again? */ + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryout); + } + else + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryagain); + } +} + +FSM_TRANS (MOVE_WAIT_FOR_CLEAR_PATH, move_stop, MOVE_IDLE) +{ + return FSM_NEXT (MOVE_WAIT_FOR_CLEAR_PATH, move_stop); +} + diff --git a/digital/io-hub/src/guybrush/move.h b/digital/io-hub/src/guybrush/move.h new file mode 100644 index 00000000..dcb7cb88 --- /dev/null +++ b/digital/io-hub/src/guybrush/move.h @@ -0,0 +1,63 @@ +#ifndef move_h +#define move_h +/* move.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" +#include "bot.h" + +/** Real radius of an obstacle. */ +#define MOVE_REAL_OBSTACLE_RADIUS 150 + +/** Obstacle radius for the path module. + * It corresponds to the real radius of the obstacle plus the distance you + * want to add to avoid it. */ +#define MOVE_OBSTACLE_RADIUS (MOVE_REAL_OBSTACLE_RADIUS \ + + RADAR_CLEARANCE_MM \ + + BOT_SIZE_SIDE) + +/** Obstacle validity time (in term of number of cycles). */ +#define MOVE_OBSTACLE_VALIDITY (3 * 225) + +/** Go to a position, see asserv.h for backward argument. */ +void +move_start (position_t position, uint8_t backward); + +/** Go to a position, with no angle consign. */ +void +move_start_noangle (vect_t position, uint8_t backward, int16_t shorten); + +/** Stop movement. */ +void +move_stop (void); + +/** To be called when obstacles positions are computed. */ +void +move_obstacles_update (void); + +/** Check for blocking obstacles, return non zero if an event is handled. */ +uint8_t +move_check_obstacles (void); + +#endif /* move_h */ diff --git a/digital/io-hub/src/guybrush/path.c b/digital/io-hub/src/guybrush/path.c new file mode 100644 index 00000000..bd9668c5 --- /dev/null +++ b/digital/io-hub/src/guybrush/path.c @@ -0,0 +1,380 @@ +/* path.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 "defs.h" +#include "path.h" +#include "bot.h" +#include "playground_2012.h" + +#include "modules/path/astar/astar.h" +#include "modules/utils/utils.h" +#include "modules/math/geometry/distance.h" + +#define PATH_DEBUG 0 + +#if PATH_DEBUG +#include "debug.host.h" +#endif + +/** + * This year, due to the large number of obstacles, a grid like structure is + * used for path finding on the playground. The A* algorithm is used to find + * path along nodes. + * + * TODO: this is a placeholder for the future algorithm. + */ + +/** Number of possible obstacles. */ +#define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB + +/** Number of fixed nodes. */ +#define PATH_FIXED_NODES_NB (1) + +/** Number of nodes in search graph, last two nodes are destination and source + * nodes. */ +#define PATH_NODES_NB (PATH_FIXED_NODES_NB + 2) + +/** Index of destination node. */ +#define PATH_DST_NODE_INDEX PATH_FIXED_NODES_NB + +/** Index of source node. */ +#define PATH_SRC_NODE_INDEX (PATH_DST_NODE_INDEX + 1) + +/** Information on a node. */ +struct path_node_t +{ + /** Whether this node can be used. */ + uint8_t usable; +}; + +/** Context. */ +struct path_t +{ + /** List of obstacles. */ + struct path_obstacle_t obstacles[PATH_OBSTACLES_NB]; + /** Escape factor, 0 if none. */ + uint8_t escape_factor; + /** List of nodes used for A*. */ + struct astar_node_t astar_nodes[PATH_NODES_NB]; + /** Cache of whether a node is blocked. */ + uint8_t valid[PATH_FIXED_NODES_NB]; + /** Position of end points. */ + vect_t endpoints[2]; + /** Whether the last update was a success. */ + uint8_t found; + /** Which node to look at for next step. */ + uint8_t get; +}; +static struct path_t path; + +/** Static information on nodes. */ +static const struct path_node_t path_nodes[PATH_FIXED_NODES_NB] = { + /* {{{ */ + { 1 }, + /* }}} */ +}; + +/** Compute position of a node. */ +static void +path_pos (uint8_t node, vect_t *pos) +{ + assert (node < PATH_NODES_NB); + if (node < PATH_FIXED_NODES_NB) + { + /* Temporary nonsense node. */ + pos->x = PG_WIDTH / 2; + pos->y = PG_LENGTH / 2; + } + else + { + *pos = path.endpoints[node - PATH_FIXED_NODES_NB]; + } +} + +/** Return 1 if the direct path between a and b nodes is blocked, also compute + * distance. */ +static uint8_t +path_blocking (uint8_t a, uint8_t b, int16_t *dp) +{ + uint8_t i; + vect_t va; + vect_t vb; + uint8_t escape_factor = 0; + uint8_t factor = 1; + uint8_t blocking = 0; + if (a == PATH_SRC_NODE_INDEX || b == PATH_SRC_NODE_INDEX) + escape_factor = path.escape_factor; + path_pos (a, &va); + path_pos (b, &vb); + /* Test for a blocking obstacle. */ + for (i = 0; i < PATH_OBSTACLES_NB && !blocking; i++) + { + if (path.obstacles[i].valid) + { + uint16_t d = distance_segment_point (&va, &vb, + &path.obstacles[i].c); + if (d < path.obstacles[i].r) + blocking = 1; + } + } + /* Compute distance. */ + int16_t d = distance_point_point (&va, &vb); + if (d == 0) + { + *dp = 0; + return 0; + } + /* Handle escaping. */ + /* TODO: do not escape through a totem! */ + if (blocking) + { + if (escape_factor) + { + *dp = d * escape_factor; + return 0; + } + else + return 1; + } + /* No blocking. */ + *dp = d * factor; + return 0; +} + +/** Update the cache of blocked nodes. */ +static void +path_blocked_update (void) +{ + uint8_t i, j; + for (i = 0; i < PATH_FIXED_NODES_NB; i++) + { + uint8_t valid = 1; + /* First, gather information from tables. */ + if (!path_nodes[i].usable) + valid = 0; + else + { + vect_t pos; + path_pos (i, &pos); + /* Then, test for obstacles. */ + for (j = 0; j < PATH_OBSTACLES_NB; j++) + { + if (path.obstacles[j].valid) + { + vect_t v = pos; vect_sub (&v, &path.obstacles[j].c); + uint32_t dsq = vect_dot_product (&v, &v); + uint32_t r = path.obstacles[j].r; + if (dsq <= r * r) + { + valid = 0; + break; + } + } + } + } + /* Update cache. */ + path.valid[i] = valid; + } +} + +void +path_init (void) +{ +} + +void +path_endpoints (vect_t s, vect_t d) +{ + path.endpoints[0] = d; + path.endpoints[1] = s; +} + +void +path_escape (uint8_t factor) +{ + path.escape_factor = factor; +} + +void +path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor, + uint16_t valid) +{ + assert (i < AC_PATH_OBSTACLES_NB); + assert (factor == 0); + path.obstacles[i].c = c; + path.obstacles[i].r = r; + path.obstacles[i].valid = valid; +} + +void +path_decay (void) +{ + uint8_t i; + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid + && path.obstacles[i].valid != PATH_OBSTACLE_VALID_ALWAYS) + path.obstacles[i].valid--; + } +} + +void +path_update (void) +{ + path_blocked_update (); + path.found = astar (path.astar_nodes, PATH_NODES_NB, PATH_DST_NODE_INDEX, + PATH_SRC_NODE_INDEX); + path.get = PATH_SRC_NODE_INDEX; +#if AC_PATH_REPORT + if (path.found) + { + uint8_t n, len = 0; + vect_t points[PATH_NODES_NB]; + for (n = path.get; n != PATH_DST_NODE_INDEX; n = path.astar_nodes[n].prev) + path_pos (n, &points[len++]); + path_pos (n, &points[len++]); + AC_PATH_REPORT_CALLBACK (points, len, path.obstacles, + PATH_OBSTACLES_NB); + } +#endif +} + +uint8_t +path_get_next (vect_t *p) +{ + if (path.found) + { + assert (path.get != PATH_DST_NODE_INDEX); + uint8_t prev = path.get; + vect_t pp; + path_pos (prev, &pp); + uint8_t next = path.astar_nodes[path.get].prev; + path.get = next; + path_pos (next, p); + while (next != 0xff) + { + /* Try to remove useless points. */ + uint8_t next = path.astar_nodes[path.get].prev; + if (next == 0xff || next == PATH_DST_NODE_INDEX) + break; + vect_t np; + path_pos (next, &np); + vect_t vnp = np; vect_sub (&vnp, &pp); + vect_t vp = *p; vect_sub (&vp, &pp); + if (vect_normal_dot_product (&vp, &vnp) == 0) + { + path.get = next; + *p = np; + } + else + break; + } + return 1; + } + else + return 0; +} + +/** Neighbors callback for fixed nodes. */ +static uint8_t +path_astar_neighbor_callback_fixed (uint8_t node, + struct astar_neighbor_t *neighbors) +{ + uint8_t neighbors_nb = 0; + int16_t d; + /* Check if direct path OK. */ + if (!path_blocking (node, PATH_SRC_NODE_INDEX, &d)) + { + /* Add this neighbor. */ + neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } +#if PATH_DEBUG + for (i = 0; i < neighbors_nb; i++) + DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight); +#endif + return neighbors_nb; +} + +/** Neighbors callback for endpoints. */ +static uint8_t +path_astar_neighbor_callback_endpoints (uint8_t node, + struct astar_neighbor_t *neighbors) +{ + uint8_t neighbors_nb = 0; + uint8_t i; + assert (node == PATH_DST_NODE_INDEX); + /* Select neighbors in the fixed nodes. */ + for (i = 0; i < PATH_FIXED_NODES_NB; i++) + { + /* Discard blocking nodes. */ + if (!path.valid[i]) + continue; + /* Check if there is an obstacle along the path. */ + int16_t d; + if (path_blocking (PATH_DST_NODE_INDEX, i, &d)) + continue; + /* Add this neighbor. */ + neighbors[neighbors_nb].node = i; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } + /* Check if direct path OK. */ + int16_t d; + if (!path_blocking (PATH_DST_NODE_INDEX, PATH_SRC_NODE_INDEX, &d)) + { + /* Add this neighbor. */ + neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } +#if PATH_DEBUG + for (i = 0; i < neighbors_nb; i++) + DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight); +#endif + return neighbors_nb; +} + +uint8_t +path_astar_neighbor_callback (uint8_t node, + struct astar_neighbor_t *neighbors) +{ +#if PATH_DEBUG + DPRINTF ("neighbor %d\n", node); +#endif + if (node < PATH_FIXED_NODES_NB) + return path_astar_neighbor_callback_fixed (node, neighbors); + else + return path_astar_neighbor_callback_endpoints (node, neighbors); +} + +uint16_t +path_astar_heuristic_callback (uint8_t node) +{ + vect_t pos; + path_pos (node, &pos); + return distance_point_point (&pos, &path.endpoints[0]); +} diff --git a/digital/io-hub/src/guybrush/path.h b/digital/io-hub/src/guybrush/path.h new file mode 100644 index 00000000..35f5cd69 --- /dev/null +++ b/digital/io-hub/src/guybrush/path.h @@ -0,0 +1,78 @@ +#ifndef path_h +#define path_h +/* path.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" + +/** This implement a interface similar to the path module, but adapted for the + * special grid of Eurobot 2012. See real path modules for interface comments. */ + +/** Infinite validity for an obstacle. */ +#define PATH_OBSTACLE_VALID_ALWAYS 0xffff + +/** Obstacle. */ +struct path_obstacle_t +{ + /** Center. */ + vect_t c; + /** Radius. */ + uint16_t r; + /** Validity counter, when this is zero, the obstacle is ignored. */ + uint16_t valid; +}; + +void +path_init (void); + +void +path_endpoints (vect_t s, vect_t d); + +void +path_escape (uint8_t factor); + +void +path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor, + uint16_t valid); + +void +path_decay (void); + +void +path_update (void); + +uint8_t +path_get_next (vect_t *p); + +#if AC_PATH_REPORT + +/** Report computed path. */ +void +AC_PATH_REPORT_CALLBACK (vect_t *points, uint8_t len, + struct path_obstacle_t *obstacles, + uint8_t obstacles_nb); + +#endif + +#endif /* path_h */ diff --git a/digital/io-hub/src/guybrush/playground_2012.h b/digital/io-hub/src/guybrush/playground_2012.h new file mode 100644 index 00000000..474b28c2 --- /dev/null +++ b/digital/io-hub/src/guybrush/playground_2012.h @@ -0,0 +1,32 @@ +#ifndef playground_2012_h +#define playground_2012_h +/* playground_2012.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. + * + * }}} */ + +#define EUROBOT 2012 + +#include "playground.h" + +#endif /* playground_2012_h */ diff --git a/digital/io-hub/src/guybrush/radar_defs.c b/digital/io-hub/src/guybrush/radar_defs.c new file mode 100644 index 00000000..8fd626d6 --- /dev/null +++ b/digital/io-hub/src/guybrush/radar_defs.c @@ -0,0 +1,46 @@ +/* radar_defs.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 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 "radar.h" + +#include "modules/devices/usdist/usdist.h" +#include "playground_2012.h" + +/** Define radar configuration. */ +struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = { + { &usdist_mm[0], { 20, 20 }, G_ANGLE_UF016_DEG (10) }, + { &usdist_mm[1], { 20, -20 }, G_ANGLE_UF016_DEG (-10) }, + { &usdist_mm[2], { -20, -20 }, G_ANGLE_UF016_DEG (180 + 10) }, + { &usdist_mm[3], { -20, 20 }, G_ANGLE_UF016_DEG (180 - 10) }, +}; + +/** Define exclusion area (considered as invalid point). */ +uint8_t +radar_valid (vect_t p) +{ + return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM + && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM; +} + diff --git a/digital/io-hub/src/guybrush/radar_defs.h b/digital/io-hub/src/guybrush/radar_defs.h new file mode 100644 index 00000000..5f6a2a73 --- /dev/null +++ b/digital/io-hub/src/guybrush/radar_defs.h @@ -0,0 +1,41 @@ +#ifndef radar_defs_h +#define radar_defs_h +/* radar_defs.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 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. + * + * }}} */ + +#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 +#define RADAR_OBSTACLE_RADIUS_MM 150 +#define RADAR_STOP_MM 350 +#define RADAR_CLEARANCE_MM 100 +#define RADAR_EPSILON_MM 70 + +#define RADAR_SENSOR_NB 4 + +#define RADAR_SENSOR_FRONT_FIRST 0 +#define RADAR_SENSOR_FRONT_NB 2 +#define RADAR_SENSOR_BACK_FIRST 2 +#define RADAR_SENSOR_BACK_NB 2 + +#endif /* radar_defs_h */ diff --git a/digital/io-hub/src/guybrush/simu.host.c b/digital/io-hub/src/guybrush/simu.host.c new file mode 100644 index 00000000..6e523228 --- /dev/null +++ b/digital/io-hub/src/guybrush/simu.host.c @@ -0,0 +1,107 @@ +/* simu.host.c - Host simulation. */ +/* 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 "simu.host.h" + +#include "modules/utils/utils.h" +#include "modules/host/host.h" +#include "modules/host/mex.h" +#include "modules/adc/adc.h" +#include "modules/path/path.h" +#include "io.h" + +/** AVR registers. */ +uint8_t PORTA, DDRA, PINA, PINE, PINF; + +/** Message types. */ +uint8_t simu_mex_pos_report; +uint8_t simu_mex_path; + +static void +simu_adc_handle (void *user, mex_msg_t *msg) +{ + uint8_t index; + uint16_t value; + mex_msg_pop (msg, "BH", &index, &value); + adc_values[index] = value; +} + +/** Initialise simulation. */ +void +simu_init (void) +{ + const char *mex_instance; + mex_node_connect (); + mex_instance = host_get_instance ("io-hub0", 0); + uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance); + mex_node_register (mtype, simu_adc_handle, 0); + simu_mex_pos_report = mex_node_reservef ("%s:pos-report", mex_instance); + simu_mex_path = mex_node_reservef ("%s:path", mex_instance); +} + +/** Make a simulation step. */ +void +simu_step (void) +{ +} + +void +timer_init (void) +{ + simu_init (); +} + +uint8_t +timer_wait (void) +{ + mex_node_wait_date (mex_node_date () + 4); + simu_step (); + return 0; +} + +/** Send computed path. */ +void +simu_send_path (vect_t *points, uint8_t len, + struct path_obstacle_t *obstacles, uint8_t obstacles_nb) +{ + int i; + mex_msg_t *m; + m = mex_msg_new (simu_mex_path); + for (i = 0; i < len; i++) + mex_msg_push (m, "hh", points[i].x, points[i].y); + mex_node_send (m); +} + +void +simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id) +{ + mex_msg_t *m; + m = mex_msg_new (simu_mex_pos_report); + mex_msg_push (m, "b", id); + for (; pos_nb; pos++, pos_nb--) + mex_msg_push (m, "hh", pos->x, pos->y); + mex_node_send (m); +} + diff --git a/digital/io-hub/src/guybrush/simu.host.h b/digital/io-hub/src/guybrush/simu.host.h new file mode 100644 index 00000000..a419e857 --- /dev/null +++ b/digital/io-hub/src/guybrush/simu.host.h @@ -0,0 +1,46 @@ +#ifndef simu_host_h +#define simu_host_h +/* simu.host.h - Host simulation. */ +/* 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" + +#ifdef HOST + +extern uint8_t PORTA, DDRA, PINA, PINE, PINF; + +/** Send general purpose positions to indicate computation results. + * - pos: array of positions to report. + * - pos_nb: number of elements in the array. + * - id: identifier so that several unrelated positions could be reported. */ +void +simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id); + +#else /* !defined (HOST) */ + +#define simu_send_pos_report(pos, pos_nb, id) ((void) 0) + +#endif /* !defined (HOST) */ + +#endif /* simu_host_h */ diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c new file mode 100644 index 00000000..15c3854d --- /dev/null +++ b/digital/io-hub/src/guybrush/top.c @@ -0,0 +1,59 @@ +/* top.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 "io.h" + +#include "playground_2012.h" +#include "asserv.h" + +#define FSM_NAME AI +#include "fsm.h" + +#include "move.h" +#include "chrono.h" +#include "contact.h" + +/* + * Here is the top FSM. This FSM is suppose to give life to the robot with an + * impression of intelligence... Well... + */ + +FSM_INIT + +FSM_STATES ( + /* Initial state. */ + TOP_START) + +FSM_START_WITH (TOP_START) + +/** Top context. */ +struct top_t +{ +}; + +/** Global context. */ +struct top_t top_global; +#define ctx top_global + -- cgit v1.2.3