summaryrefslogtreecommitdiff
path: root/digital/io/src
diff options
context:
space:
mode:
authorNicolas Schodet2008-06-17 22:57:24 +0200
committerNicolas Schodet2008-06-17 22:57:24 +0200
commit72f865523b14953d13a207599068ba68e9df0b70 (patch)
tree8813a1213c2b0b9e89ed043d9dd6efc4cbffaec0 /digital/io/src
parent19c0b55ba097db272d96d879e2895e2654a2c91c (diff)
* digital/avr/modules/path, digital/io/src:
- moved path finding to a separated module.
Diffstat (limited to 'digital/io/src')
-rw-r--r--digital/io/src/Makefile8
-rw-r--r--digital/io/src/avrconfig.h10
-rw-r--r--digital/io/src/main.c2
-rw-r--r--digital/io/src/move_cb.c2
-rw-r--r--digital/io/src/path.c348
-rw-r--r--digital/io/src/path.h65
-rw-r--r--digital/io/src/simu.host.c1
-rw-r--r--digital/io/src/simu.host.h7
-rw-r--r--digital/io/src/test_path.c56
9 files changed, 15 insertions, 484 deletions
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 <stdio.h>
-
-/** 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 <stdio.h>
-
-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)
-{
-}
-