From 72f865523b14953d13a207599068ba68e9df0b70 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 17 Jun 2008 22:57:24 +0200 Subject: * digital/avr/modules/path, digital/io/src: - moved path finding to a separated module. --- digital/avr/modules/path/Makefile.module | 1 + digital/avr/modules/path/README | 25 +++ digital/avr/modules/path/avrconfig.h | 38 ++++ digital/avr/modules/path/path.c | 351 ++++++++++++++++++++++++++++++ digital/avr/modules/path/path.h | 83 +++++++ digital/avr/modules/path/test/Makefile | 12 + digital/avr/modules/path/test/avrconfig.h | 39 ++++ digital/avr/modules/path/test/test_path.c | 52 +++++ digital/io/src/Makefile | 8 +- digital/io/src/avrconfig.h | 10 + digital/io/src/main.c | 2 +- digital/io/src/move_cb.c | 2 +- digital/io/src/path.c | 348 ----------------------------- digital/io/src/path.h | 65 ------ digital/io/src/simu.host.c | 1 + digital/io/src/simu.host.h | 7 - digital/io/src/test_path.c | 56 ----- 17 files changed, 616 insertions(+), 484 deletions(-) create mode 100644 digital/avr/modules/path/Makefile.module create mode 100644 digital/avr/modules/path/README create mode 100644 digital/avr/modules/path/avrconfig.h create mode 100644 digital/avr/modules/path/path.c create mode 100644 digital/avr/modules/path/path.h create mode 100644 digital/avr/modules/path/test/Makefile create mode 100644 digital/avr/modules/path/test/avrconfig.h create mode 100644 digital/avr/modules/path/test/test_path.c delete mode 100644 digital/io/src/path.c delete mode 100644 digital/io/src/path.h delete mode 100644 digital/io/src/test_path.c diff --git a/digital/avr/modules/path/Makefile.module b/digital/avr/modules/path/Makefile.module new file mode 100644 index 00000000..9d537233 --- /dev/null +++ b/digital/avr/modules/path/Makefile.module @@ -0,0 +1 @@ +path_SOURCES = path.c diff --git a/digital/avr/modules/path/README b/digital/avr/modules/path/README new file mode 100644 index 00000000..618e4b79 --- /dev/null +++ b/digital/avr/modules/path/README @@ -0,0 +1,25 @@ +avr.path - Path finding module. + +Path finding among obstacles represented as circles. See modules README for +more details about AVR modules. + + +Copyright (C) 2008 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/avr/modules/path/avrconfig.h b/digital/avr/modules/path/avrconfig.h new file mode 100644 index 00000000..9f25d772 --- /dev/null +++ b/digital/avr/modules/path/avrconfig.h @@ -0,0 +1,38 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - Path module configuration template. */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +/* path - Path finding module. */ +/** Report path found for debug. */ +#define AC_PATH_REPORT defined (HOST) +/** Report function name. */ +#define AC_PATH_REPORT_CALLBACK path_report +/** Number of possible obstacles. */ +#define AC_PATH_OBSTACLES_NB 2 +/** Number of points per obstacle. */ +#define AC_PATH_OBSTACLES_POINTS_NB 8 + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/path/path.c b/digital/avr/modules/path/path.c new file mode 100644 index 00000000..30cc03f8 --- /dev/null +++ b/digital/avr/modules/path/path.c @@ -0,0 +1,351 @@ +/* path.c - Find a path between obstables. */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 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/math/fixed/fixed.h" +#include "modules/utils/utils.h" + +#include "path.h" + +#ifdef HOST +# include +#endif + +/** Number of possible obstacles. */ +#define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB + +/** Number of points per obstacle. */ +#define PATH_OBSTACLES_POINTS_NB AC_PATH_OBSTACLES_POINTS_NB + +/** Angle between obstacles points. */ +#define PATH_OBSTACLES_POINTS_ANGLE ((1L << 24) / PATH_OBSTACLES_POINTS_NB) + +/** Number of points. */ +#define PATH_POINTS_NB (2 + PATH_OBSTACLES_NB * PATH_OBSTACLES_POINTS_NB) + +/** Point with extra data for Dijkstra algorithm. */ +struct path_point_t +{ + /** Coordinates. */ + int16_t x, y; + /** Weight. */ + uint16_t w; + /** Next point (preceding in the usual Dijkstra meaning). */ + uint8_t next; + /** Is this point taken yet? */ + uint8_t taken; +}; + +/** Path finding context. */ +struct path_t +{ + /** Borders, any point outside borders is eliminated. */ + int16_t border_xmin, border_ymin, border_xmax, border_ymax; + /** Complex number used to position obstacles points. + * The obstacles points are placed evenly on a circle around the center + * point. Complex multiplication is used to make a point rotate around + * the center point. */ + uint32_t rot_a, rot_b; + /** Margin to take into account approximation of the circle. */ + uint32_t margin; + /** List of obstacles. */ + struct path_obstacle_t obstacles[PATH_OBSTACLES_NB]; + /** List of points. First two points are the destination and source + * points. Then comes the obstacles points. */ + struct path_point_t points[PATH_POINTS_NB]; + /** Number of points. */ + uint8_t points_nb; + /** Connection matrix, value is arc weight, 0xffff for no connection. */ + uint16_t arcs[PATH_POINTS_NB][PATH_POINTS_NB]; +}; + +/** Global context. */ +struct path_t path; + +/** Initialise path finder. */ +void +path_init (int16_t border_xmin, int16_t border_ymin, + int16_t border_xmax, int16_t border_ymax) +{ + /** Save borders. */ + path.border_xmin = border_xmin; + path.border_ymin = border_ymin; + path.border_xmax = border_xmax; + path.border_ymax = border_ymax; + /** Complex number of modulus 1 and rotation angle. */ + path.rot_a = fixed_cos_f824 (PATH_OBSTACLES_POINTS_ANGLE); + path.rot_b = fixed_sin_f824 (PATH_OBSTACLES_POINTS_ANGLE); + path.margin = + fixed_div_f824 (0x11L << 20, + fixed_cos_f824 (PATH_OBSTACLES_POINTS_ANGLE / 2)); +} + +/** Compute points from obstacles. */ +static void +path_compute_points (void) +{ + /* Do not touch first two points, they are destination and source. */ + uint8_t i, j, p = 2; + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid) + { + /* Compute obstacle points positions around a circle. */ + uint32_t x, y, nx; + x = path.obstacles[i].r; y = 0; + x = fixed_mul_f824 (x, path.margin); + path.points[p].x = path.obstacles[i].x + (uint16_t) x; + path.points[p].y = path.obstacles[i].y; + p++; + for (j = 0; j < PATH_OBSTACLES_POINTS_NB - 1; j++) + { + nx = fixed_mul_f824 (x, path.rot_a) + - fixed_mul_f824 (y, path.rot_b); + y = fixed_mul_f824 (y, path.rot_a) + + fixed_mul_f824 (x, path.rot_b); + x = nx; + path.points[p].x = path.obstacles[i].x + (uint16_t) x; + path.points[p].y = path.obstacles[i].y + (uint16_t) y; + /* Check it is in playground. */ + if (path.points[p].x >= path.border_xmin + && path.points[p].y >= path.border_ymin + && path.points[p].x < path.border_xmax + && path.points[p].y < path.border_ymax) + /* Accept point. */ + p++; + } + } + } + path.points_nb = p; +} + +/** Compute and return distance between two points if there is no obstacle + * between them. Return 0xffff if there is an obstacle. */ +static uint16_t +path_compute_weight (uint8_t a, uint8_t b) +{ + int32_t dx, dy; + int16_t ab, d, m, f; + uint8_t i; + /* Compute distance. */ + dx = path.points[b].x - path.points[a].x; + dy = path.points[b].y - path.points[a].y; + ab = fixed_sqrt_ui32 (dx * dx + dy * dy); + if (ab == 0) + return 0; + /* Is there an intersection with a circle. */ + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid) + { + /* Compute distance to center. + * Use a scalar product between a segment perpendicular vector and + * the vector from one segment end to obstacle center. */ + int32_t acx, acy; + acx = path.obstacles[i].x - path.points[a].x; + acy = path.obstacles[i].y - path.points[a].y; + d = (acx * dy - acy * dx) / ab; + d = UTILS_ABS (d); + /* If out of the circle, no problem, else, more tests. */ + if ((uint16_t) d <= path.obstacles[i].r) + { + /* If the line intersect the circle, the segment intersect the + * circle if it as one point on each side of an intersection. */ + m = (acx * dx + acy * dy) / ab; + f = fixed_sqrt_ui32 (path.obstacles[i].r * path.obstacles[i].r + - d * d); + if (!((m - f > 0 && m + f > 0 && m - f > ab && m + f > ab) + || (m - f < 0 && m + f < 0 && m - f < ab && m + f < ab))) + return 0xffff; + } + } + } + return ab; +} + +/** Fill the arc matrix. */ +static void +path_compute_arcs (void) +{ + uint8_t i, j; + for (i = 0; i < path.points_nb; i++) + { + path.arcs[i][i] = 0xffff; + for (j = 0; j < i; j++) + { + path.arcs[i][j] = path_compute_weight (i, j); + path.arcs[j][i] = path.arcs[i][j]; + } + } +} + +/** Apply the Dijkstra algorithm. */ +static void +path_dijkstra (void) +{ + uint8_t i, u; + uint16_t d, wmin; + /** Initialise each points. */ + for (i = 0; i < path.points_nb; i++) + { + path.points[i].w = 0xffff; + path.points[i].next = 0xff; + path.points[i].taken = 0; + } + /** Start with the destination point. */ + u = 0; + path.points[u].w = 0; + path.points[u].taken = 1; + do + { + /* Relax every connected points. */ + for (i = 0; i < path.points_nb; i++) + { + if (path.arcs[i][u] != 0xffff) + { + d = path.points[u].w + path.arcs[i][u]; + if (d < path.points[i].w) + { + path.points[i].w = d; + path.points[i].next = u; + } + } + } + /* Find the next best point. */ + wmin = 0xffff; + u = 0xff; + for (i = 0; i < path.points_nb; i++) + { + if (!path.points[i].taken && path.points[i].w < wmin) + { + u = i; + wmin = path.points[u].w; + } + } + if (u != 0xff) + path.points[u].taken = 1; + /* Until the source point is found, or no solution. */ + } while (u != 1 && u != 0xff); +} + +/** Setup end points (source and destination coordinates). */ +void +path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy) +{ + path.points[0].x = dx; + path.points[0].y = dy; + path.points[1].x = sx; + path.points[1].y = sy; +} + +/** Set up an obstacle at given position with the given radius and validity + * period. */ +void +path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint16_t valid) +{ + path.obstacles[i].x = x; + path.obstacles[i].y = y; + path.obstacles[i].r = r; + path.obstacles[i].valid = valid; +} + +/** Slowly make the obstacles disappear. */ +void +path_decay (void) +{ + uint8_t i; + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid && path.obstacles[i].valid != 0xffff) + path.obstacles[i].valid--; + } +} + +/** Compute shortest path. */ +void +path_update (void) +{ + path_compute_points (); + path_compute_arcs (); + path_dijkstra (); +#if AC_PATH_REPORT + uint8_t len, i; + uint16_t points[PATH_POINTS_NB * 2]; + len = 0; + for (i = 1; i != 0xff; i = path.points[i].next) + { + points[len++] = path.points[i].x; + points[len++] = path.points[i].y; + } + AC_PATH_REPORT_CALLBACK (points, len, path.obstacles, PATH_OBSTACLES_NB); +#endif /* AC_PATH_REPORT */ +} + +/** Retrieve first path point coordinates. Return 0 on failure. */ +uint8_t +path_get_next (uint16_t *x, uint16_t *y) +{ + uint8_t next; + next = path.points[1].next; + if (next == 0xff) + return 0; + else + { + *x = path.points[next].x; + *y = path.points[next].y; + return 1; + } +} + +#ifdef HOST + +#include + +/** Output graph in Graphviz format. */ +void +path_print_graph (void) +{ + int i, j; + printf ("graph map {\n overlap = scale\n sep = 0.5\n"); + for (i = 0; i < path.points_nb; i++) + { + printf (" %d [ shape = Mrecord, label = \"{%d|{%d|%d}}\", " + "pos = \"%d,%d\" ]\n", i, i, path.points[i].x, + path.points[i].y, path.points[i].x, path.points[i].y); + for (j = 0; j < i; j++) + { + if (path.arcs[i][j] != 0xffff) + { + printf (" %d -- %d [ label = \"%d\" ]\n", i, j, + path.arcs[i][j]); + } + } + } + printf ("}\n// Path:\n"); + for (i = 1; i != 0xff; i = path.points[i].next) + printf ("// %d, %d\n", path.points[i].x, path.points[i].y); +} + +#endif diff --git a/digital/avr/modules/path/path.h b/digital/avr/modules/path/path.h new file mode 100644 index 00000000..8255bf65 --- /dev/null +++ b/digital/avr/modules/path/path.h @@ -0,0 +1,83 @@ +#ifndef path_h +#define path_h +/* path.h */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +/** Obstacle. */ +struct path_obstacle_t +{ + /** Center. */ + int16_t x, y; + /** Radius. */ + uint16_t r; + /** Validity counter, when this is zero, the obstacle is ignored. */ + uint16_t valid; +}; + +/** Initialise path finder. */ +void +path_init (int16_t border_xmin, int16_t border_ymin, + int16_t border_xmax, int16_t border_ymax); + +/** Setup end points (source and destination coordinates). */ +void +path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy); + +/** Set up an obstacle at given position with the given radius and validity + * period. */ +void +path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint16_t valid); + +/** Slowly make the obstacles disappear. */ +void +path_decay (void); + +/** Compute shortest path. */ +void +path_update (void); + +/** Retrieve first path point coordinates. Return 0 on failure. */ +uint8_t +path_get_next (uint16_t *x, uint16_t *y); + +#if AC_PATH_REPORT + +/** Report computed path. */ +void +AC_PATH_REPORT_CALLBACK (uint16_t *points, uint8_t len, + struct path_obstacle_t *obstacles, + uint8_t obstacles_nb); + +#endif + +#ifdef HOST + +/** Output graph in Graphviz format. */ +void +path_print_graph (void); + +#endif + +#endif /* path_h */ diff --git a/digital/avr/modules/path/test/Makefile b/digital/avr/modules/path/test/Makefile new file mode 100644 index 00000000..698ee538 --- /dev/null +++ b/digital/avr/modules/path/test/Makefile @@ -0,0 +1,12 @@ +BASE = ../../.. +HOST_PROGS = test_path +test_path_SOURCES = test_path.c +MODULES = path math/fixed utils +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 + +include $(BASE)/make/Makefile.gen diff --git a/digital/avr/modules/path/test/avrconfig.h b/digital/avr/modules/path/test/avrconfig.h new file mode 100644 index 00000000..476fee86 --- /dev/null +++ b/digital/avr/modules/path/test/avrconfig.h @@ -0,0 +1,39 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - Path module configuration template. */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 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. + * + * }}} */ + +/* path - Path finding module. */ +/** Report path found for debug. */ +#define AC_PATH_REPORT defined (HOST) +/** Report function name. */ +#define AC_PATH_REPORT_CALLBACK path_report +/** Number of possible obstacles. */ +#define AC_PATH_OBSTACLES_NB 2 +/** Number of points per obstacle. */ +#define AC_PATH_OBSTACLES_POINTS_NB 8 + + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/path/test/test_path.c b/digital/avr/modules/path/test/test_path.c new file mode 100644 index 00000000..8b88e5d7 --- /dev/null +++ b/digital/avr/modules/path/test/test_path.c @@ -0,0 +1,52 @@ +/* test_path.c */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 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/path/path.h" + +#include + +int +main (void) +{ + path_init (0, 0, 3000, 2100); + path_endpoints (300, 1000, 1200, 1000); + path_obstacle (0, 600, 930, 100, 1); + path_obstacle (1, 900, 1070, 100, 1); + path_update (); + path_print_graph (); + uint16_t x, y; + if (path_get_next (&x, &y)) + printf ("// Next point: %d, %d\n", x, y); + else + printf ("// Failure\n"); + return 0; +} + +void +path_report (uint16_t *points, uint8_t len, + struct path_obstacle_t *obstacles, uint8_t obstacles_nb) +{ +} + diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index e230bc89..f668777f 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -1,6 +1,5 @@ BASE = ../../avr PROGS = io -HOST_PROGS = test_path io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c \ switch.avr.c chrono.c \ simu.host.c \ @@ -8,11 +7,8 @@ io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c \ getsamples.c getsamples_fsm.c getsamples_cb.c \ gutter_fsm.c gutter_cb.c gutter.c \ move.c move_fsm.c move_cb.c \ - top.c top_fsm.c top_cb.c \ - path.c -test_path_SOURCES = test_path.c path.c -MODULES = proto uart twi utils adc math/fixed -test_path_MODULES = math/fixed + top.c top_fsm.c top_cb.c +MODULES = proto uart twi utils adc math/fixed path CONFIGFILE = avrconfig.h # atmega8, atmega8535, atmega128... AVR_MCU = atmega128 diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h index f24fae76..fe2208a3 100644 --- a/digital/io/src/avrconfig.h +++ b/digital/io/src/avrconfig.h @@ -89,6 +89,16 @@ /** Activate slave part. */ #define AC_TWI_SLAVE_ENABLE 0 +/* 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 +/** Number of points per obstacle. */ +#define AC_PATH_OBSTACLES_POINTS_NB 8 + /* io - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 2 diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 05fb68b5..3203675d 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -27,6 +27,7 @@ #include "modules/uart/uart.h" #include "modules/proto/proto.h" #include "modules/utils/utils.h" +#include "modules/path/path.h" /* AVR include, non HOST */ #ifndef HOST @@ -46,7 +47,6 @@ #include "chrono.h" /* chrono_end_match */ #include "gutter.h" /* gutter_generate_wait_finished_event */ #include "sharp.h" /* sharp module */ -#include "path.h" /* path module */ #include "playground.h" #include "io.h" diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index a70e61ab..ec5f6715 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -26,7 +26,6 @@ #include "fsm.h" #include "move_cb.h" -#include "path.h" #include "asserv.h" #include "playground.h" #include "move.h" @@ -34,6 +33,7 @@ #include "main.h" /* main_post_event_for_top_fsm */ #include "modules/math/fixed/fixed.h" /* fixed_* */ +#include "modules/path/path.h" #include "debug.host.h" diff --git a/digital/io/src/path.c b/digital/io/src/path.c deleted file mode 100644 index 0cd8e331..00000000 --- a/digital/io/src/path.c +++ /dev/null @@ -1,348 +0,0 @@ -/* path.c - Find a path between obstables. */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 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 "path.h" -#include "simu.host.h" - -#include "modules/math/fixed/fixed.h" -#include "modules/utils/utils.h" - -/** Number of possible obstacles. */ -#define PATH_OBSTACLES_NB 2 - -/** Number of points per obstacle. */ -#define PATH_OBSTACLES_POINTS_NB 8 - -/** Angle between obstacles points. */ -#define PATH_OBSTACLES_POINTS_ANGLE ((1L << 24) / PATH_OBSTACLES_POINTS_NB) - -/** Number of points. */ -#define PATH_POINTS_NB (2 + PATH_OBSTACLES_NB * PATH_OBSTACLES_POINTS_NB) - -/** Point with extra data for Dijkstra algorithm. */ -struct path_point_t -{ - /** Coordinates. */ - int16_t x, y; - /** Weight. */ - uint16_t w; - /** Next point (preceding in the usual Dijkstra meaning). */ - uint8_t next; - /** Is this point taken yet? */ - uint8_t taken; -}; - -/** Path finding context. */ -struct path_t -{ - /** Borders, any point outside borders is eliminated. */ - int16_t border_xmin, border_ymin, border_xmax, border_ymax; - /** Complex number used to position obstacles points. - * The obstacles points are placed evenly on a circle around the center - * point. Complex multiplication is used to make a point rotate around - * the center point. */ - uint32_t rot_a, rot_b; - /** Margin to take into account approximation of the circle. */ - uint32_t margin; - /** List of obstacles. */ - struct path_obstacle_t obstacles[PATH_OBSTACLES_NB]; - /** List of points. First two points are the destination and source - * points. Then comes the obstacles points. */ - struct path_point_t points[PATH_POINTS_NB]; - /** Number of points. */ - uint8_t points_nb; - /** Connection matrix, value is arc weight, 0xffff for no connection. */ - uint16_t arcs[PATH_POINTS_NB][PATH_POINTS_NB]; -}; - -/** Global context. */ -struct path_t path; - -/** Initialise path finder. */ -void -path_init (int16_t border_xmin, int16_t border_ymin, - int16_t border_xmax, int16_t border_ymax) -{ - /** Save borders. */ - path.border_xmin = border_xmin; - path.border_ymin = border_ymin; - path.border_xmax = border_xmax; - path.border_ymax = border_ymax; - /** Complex number of modulus 1 and rotation angle. */ - path.rot_a = fixed_cos_f824 (PATH_OBSTACLES_POINTS_ANGLE); - path.rot_b = fixed_sin_f824 (PATH_OBSTACLES_POINTS_ANGLE); - path.margin = - fixed_div_f824 (0x11L << 20, - fixed_cos_f824 (PATH_OBSTACLES_POINTS_ANGLE / 2)); -} - -/** Compute points from obstacles. */ -static void -path_compute_points (void) -{ - /* Do not touch first two points, they are destination and source. */ - uint8_t i, j, p = 2; - for (i = 0; i < PATH_OBSTACLES_NB; i++) - { - if (path.obstacles[i].valid) - { - /* Compute obstacle points positions around a circle. */ - uint32_t x, y, nx; - x = path.obstacles[i].r; y = 0; - x = fixed_mul_f824 (x, path.margin); - path.points[p].x = path.obstacles[i].x + (uint16_t) x; - path.points[p].y = path.obstacles[i].y; - p++; - for (j = 0; j < PATH_OBSTACLES_POINTS_NB - 1; j++) - { - nx = fixed_mul_f824 (x, path.rot_a) - - fixed_mul_f824 (y, path.rot_b); - y = fixed_mul_f824 (y, path.rot_a) - + fixed_mul_f824 (x, path.rot_b); - x = nx; - path.points[p].x = path.obstacles[i].x + (uint16_t) x; - path.points[p].y = path.obstacles[i].y + (uint16_t) y; - /* Check it is in playground. */ - if (path.points[p].x >= path.border_xmin - && path.points[p].y >= path.border_ymin - && path.points[p].x < path.border_xmax - && path.points[p].y < path.border_ymax) - /* Accept point. */ - p++; - } - } - } - path.points_nb = p; -} - -/** Compute and return distance between two points if there is no obstacle - * between them. Return 0xffff if there is an obstacle. */ -static uint16_t -path_compute_weight (uint8_t a, uint8_t b) -{ - int32_t dx, dy; - int16_t ab, d, m, f; - uint8_t i; - /* Compute distance. */ - dx = path.points[b].x - path.points[a].x; - dy = path.points[b].y - path.points[a].y; - ab = fixed_sqrt_ui32 (dx * dx + dy * dy); - if (ab == 0) - return 0; - /* Is there an intersection with a circle. */ - for (i = 0; i < PATH_OBSTACLES_NB; i++) - { - if (path.obstacles[i].valid) - { - /* Compute distance to center. - * Use a scalar product between a segment perpendicular vector and - * the vector from one segment end to obstacle center. */ - int32_t acx, acy; - acx = path.obstacles[i].x - path.points[a].x; - acy = path.obstacles[i].y - path.points[a].y; - d = (acx * dy - acy * dx) / ab; - d = UTILS_ABS (d); - /* If out of the circle, no problem, else, more tests. */ - if ((uint16_t) d <= path.obstacles[i].r) - { - /* If the line intersect the circle, the segment intersect the - * circle if it as one point on each side of an intersection. */ - m = (acx * dx + acy * dy) / ab; - f = fixed_sqrt_ui32 (path.obstacles[i].r * path.obstacles[i].r - - d * d); - if (!((m - f > 0 && m + f > 0 && m - f > ab && m + f > ab) - || (m - f < 0 && m + f < 0 && m - f < ab && m + f < ab))) - return 0xffff; - } - } - } - return ab; -} - -/** Fill the arc matrix. */ -static void -path_compute_arcs (void) -{ - uint8_t i, j; - for (i = 0; i < path.points_nb; i++) - { - path.arcs[i][i] = 0xffff; - for (j = 0; j < i; j++) - { - path.arcs[i][j] = path_compute_weight (i, j); - path.arcs[j][i] = path.arcs[i][j]; - } - } -} - -/** Apply the Dijkstra algorithm. */ -static void -path_dijkstra (void) -{ - uint8_t i, u; - uint16_t d, wmin; - /** Initialise each points. */ - for (i = 0; i < path.points_nb; i++) - { - path.points[i].w = 0xffff; - path.points[i].next = 0xff; - path.points[i].taken = 0; - } - /** Start with the destination point. */ - u = 0; - path.points[u].w = 0; - path.points[u].taken = 1; - do - { - /* Relax every connected points. */ - for (i = 0; i < path.points_nb; i++) - { - if (path.arcs[i][u] != 0xffff) - { - d = path.points[u].w + path.arcs[i][u]; - if (d < path.points[i].w) - { - path.points[i].w = d; - path.points[i].next = u; - } - } - } - /* Find the next best point. */ - wmin = 0xffff; - u = 0xff; - for (i = 0; i < path.points_nb; i++) - { - if (!path.points[i].taken && path.points[i].w < wmin) - { - u = i; - wmin = path.points[u].w; - } - } - if (u != 0xff) - path.points[u].taken = 1; - /* Until the source point is found, or no solution. */ - } while (u != 1 && u != 0xff); -} - -/** Setup end points (source and destination coordinates). */ -void -path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy) -{ - path.points[0].x = dx; - path.points[0].y = dy; - path.points[1].x = sx; - path.points[1].y = sy; -} - -/** Set up an obstacle at given position with the given radius and validity - * period. */ -void -path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint16_t valid) -{ - path.obstacles[i].x = x; - path.obstacles[i].y = y; - path.obstacles[i].r = r; - path.obstacles[i].valid = valid; -} - -/** Slowly make the obstacles disappear. */ -void -path_decay (void) -{ - uint8_t i; - for (i = 0; i < PATH_OBSTACLES_NB; i++) - { - if (path.obstacles[i].valid && path.obstacles[i].valid != 0xffff) - path.obstacles[i].valid--; - } -} - -/** Compute shortest path. */ -void -path_update (void) -{ - path_compute_points (); - path_compute_arcs (); - path_dijkstra (); -#if defined (HOST) - uint8_t len, i; - uint16_t points[PATH_POINTS_NB * 2]; - len = 0; - for (i = 1; i != 0xff; i = path.points[i].next) - { - points[len++] = path.points[i].x; - points[len++] = path.points[i].y; - } - simu_send_path (points, len, path.obstacles, PATH_OBSTACLES_NB); -#endif -} - -/** Retrieve first path point coordinates. Return 0 on failure. */ -uint8_t -path_get_next (uint16_t *x, uint16_t *y) -{ - uint8_t next; - next = path.points[1].next; - if (next == 0xff) - return 0; - else - { - *x = path.points[next].x; - *y = path.points[next].y; - return 1; - } -} - -#ifdef HOST - -#include - -/** Output graph in Graphviz format. */ -void -path_print_graph (void) -{ - int i, j; - printf ("graph map {\n overlap = scale\n sep = 0.5\n"); - for (i = 0; i < path.points_nb; i++) - { - printf (" %d [ shape = Mrecord, label = \"{%d|{%d|%d}}\", " - "pos = \"%d,%d\" ]\n", i, i, path.points[i].x, - path.points[i].y, path.points[i].x, path.points[i].y); - for (j = 0; j < i; j++) - { - if (path.arcs[i][j] != 0xffff) - { - printf (" %d -- %d [ label = \"%d\" ]\n", i, j, - path.arcs[i][j]); - } - } - } - printf ("}\n// Path:\n"); - for (i = 1; i != 0xff; i = path.points[i].next) - printf ("// %d, %d\n", path.points[i].x, path.points[i].y); -} - -#endif diff --git a/digital/io/src/path.h b/digital/io/src/path.h deleted file mode 100644 index a314fcec..00000000 --- a/digital/io/src/path.h +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef path_h -#define path_h -/* path.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 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. - * - * }}} */ - -/** Obstacle. */ -struct path_obstacle_t -{ - /** Center. */ - int16_t x, y; - /** Radius. */ - uint16_t r; - /** Validity counter, when this is zero, the obstacle is ignored. */ - uint16_t valid; -}; - -/** Initialise path finder. */ -void -path_init (int16_t border_xmin, int16_t border_ymin, - int16_t border_xmax, int16_t border_ymax); - -/** Setup end points (source and destination coordinates). */ -void -path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy); - -/** Set up an obstacle at given position with the given radius and validity - * period. */ -void -path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint16_t valid); - -/** Slowly make the obstacles disappear. */ -void -path_decay (void); - -/** Compute shortest path. */ -void -path_update (void); - -/** Retrieve first path point coordinates. Return 0 on failure. */ -uint8_t -path_get_next (uint16_t *x, uint16_t *y); - -#endif /* path_h */ diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c index cce7711b..99870e74 100644 --- a/digital/io/src/simu.host.c +++ b/digital/io/src/simu.host.c @@ -32,6 +32,7 @@ #include "modules/host/host.h" #include "modules/host/mex.h" #include "modules/adc/adc.h" +#include "modules/path/path.h" enum { diff --git a/digital/io/src/simu.host.h b/digital/io/src/simu.host.h index 8e0243d1..bbd5c218 100644 --- a/digital/io/src/simu.host.h +++ b/digital/io/src/simu.host.h @@ -27,8 +27,6 @@ #ifdef HOST -#include "path.h" - /** Hooked, initialise the host simulation. */ void main_timer_init (void); @@ -53,11 +51,6 @@ switch_get_color (void); uint8_t switch_get_jack (void); -/** Send computed path. */ -void -simu_send_path (uint16_t *points, uint8_t len, - struct path_obstacle_t *obstacles, uint8_t obstacles_nb); - #endif /* defined (HOST) */ #endif /* simu_host_h */ diff --git a/digital/io/src/test_path.c b/digital/io/src/test_path.c deleted file mode 100644 index 1ec8c533..00000000 --- a/digital/io/src/test_path.c +++ /dev/null @@ -1,56 +0,0 @@ -/* test_path.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 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 "path.h" -#include "simu.host.h" - -#include - -void -path_print_graph (void); - -int -main (void) -{ - path_init (0, 0, 3000, 2100); - path_endpoints (300, 1000, 1200, 1000); - path_obstacle (0, 600, 930, 100, 1); - path_obstacle (1, 900, 1070, 100, 1); - path_update (); - path_print_graph (); - uint16_t x, y; - if (path_get_next (&x, &y)) - printf ("// Next point: %d, %d\n", x, y); - else - printf ("// Failure\n"); - return 0; -} - -void -simu_send_path (uint16_t *points, uint8_t len, - struct path_obstacle_t *obstacles, uint8_t obstacles_nb) -{ -} - -- cgit v1.2.3