From ad7f48df6ea99619996bb3b36f26fd6db4592230 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 26 Apr 2008 08:08:58 +0200 Subject: * digital/io/src: - added path finding algorithm. --- digital/io/src/Makefile | 3 + digital/io/src/path.c | 337 ++++++++++++++++++++++++++++++++++++++++++++ digital/io/src/path.h | 43 ++++++ digital/io/src/playground.h | 5 + digital/io/src/test_path.c | 49 +++++++ 5 files changed, 437 insertions(+) create mode 100644 digital/io/src/path.c create mode 100644 digital/io/src/path.h create mode 100644 digital/io/src/test_path.c (limited to 'digital') diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 1899bc6f..480d49f2 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -1,5 +1,6 @@ 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,7 +9,9 @@ io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.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 +test_path_SOURCES = test_path.c path.c MODULES = proto uart twi utils adc math/fixed +test_path_MODULES = math/fixed CONFIGFILE = avrconfig.h # atmega8, atmega8535, atmega128... AVR_MCU = atmega128 diff --git a/digital/io/src/path.c b/digital/io/src/path.c new file mode 100644 index 00000000..f4fb1b86 --- /dev/null +++ b/digital/io/src/path.c @@ -0,0 +1,337 @@ +/* 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 "playground.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; +}; + +/** 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; +}; + +/** Path finding context. */ +struct path_t +{ + /** 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 (void) +{ + /** 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 > PG_BORDER_DISTANCE + && path.points[p].y > PG_BORDER_DISTANCE + && path.points[p].x < PG_WIDTH - PG_BORDER_DISTANCE + && path.points[p].y < PG_HEIGHT - PG_BORDER_DISTANCE) + /* 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); + /* 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; + } + } + 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 (); +} + +/** 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 new file mode 100644 index 00000000..de70a14f --- /dev/null +++ b/digital/io/src/path.h @@ -0,0 +1,43 @@ +#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. + * + * }}} */ + +void +path_init (void); + +void +path_endpoints (int16_t sx, int16_t sy, int16_t dx, int16_t dy); + +void +path_obstacle (uint8_t i, int16_t x, int16_t y, uint16_t r, uint16_t valid); + +void +path_update (void); + +uint8_t +path_get_next (uint16_t *x, uint16_t *y); + +#endif /* path_h */ diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h index 66acd843..4a3be101 100644 --- a/digital/io/src/playground.h +++ b/digital/io/src/playground.h @@ -46,6 +46,11 @@ */ #define PG_HEIGHT 2100 +/** + * The distance from table border for movements. + */ +#define PG_BORDER_DISTANCE 250 + /** * Considering there is a symmetry axis on X, this macro will compute the * value for on the X axis depending on the color. diff --git a/digital/io/src/test_path.c b/digital/io/src/test_path.c new file mode 100644 index 00000000..0ec2beb6 --- /dev/null +++ b/digital/io/src/test_path.c @@ -0,0 +1,49 @@ +/* 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 + +void +path_print_graph (void); + +int +main (void) +{ + path_init (); + 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; +} + -- cgit v1.2.3