summaryrefslogtreecommitdiff
path: root/digital/io
diff options
context:
space:
mode:
authorNicolas Schodet2010-04-27 00:17:36 +0200
committerNicolas Schodet2010-04-27 00:17:36 +0200
commit3de803b964643fbc8480fd3c0133df77e324ef27 (patch)
tree92a56a0a6fde1393112ccc2cd3d4e54f3e4dcaa9 /digital/io
parent9eb335408b60d4e7efadbbd65268ef2b3a5bb328 (diff)
digital/io/src: use A* algorithm to nagivate between elements
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/Makefile5
-rw-r--r--digital/io/src/avrconfig.h6
-rw-r--r--digital/io/src/food.c153
-rw-r--r--digital/io/src/food.h49
-rw-r--r--digital/io/src/path.c462
-rw-r--r--digital/io/src/path.h78
6 files changed, 751 insertions, 2 deletions
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index 8140c0c5..424df2cb 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -5,9 +5,10 @@ PROGS = io
# Sources to compile.
io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c pwm.c \
switch.avr.c chrono.c main_timer.avr.c servo_pos.c \
- simu.host.c contact.c usdist.c radar.c
+ simu.host.c contact.c usdist.c radar.c \
+ path.c food.c
# Modules needed for IO.
-MODULES = proto uart twi utils adc math/fixed math/geometry path \
+MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \
trace flash spi
# Configuration file.
CONFIGFILE = avrconfig.h
diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h
index b0afb529..9b79b4e1 100644
--- a/digital/io/src/avrconfig.h
+++ b/digital/io/src/avrconfig.h
@@ -99,6 +99,12 @@
/** Number of points per obstacle. */
#define AC_PATH_OBSTACLES_POINTS_NB 8
+/* 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 - io/ai board. */
/** TWI address of the io board. */
#define AC_IO_TWI_ADDRESS 2
diff --git a/digital/io/src/food.c b/digital/io/src/food.c
new file mode 100644
index 00000000..62c79f3e
--- /dev/null
+++ b/digital/io/src/food.c
@@ -0,0 +1,153 @@
+/* food.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 "defs.h"
+#include "food.h"
+
+#include "modules/utils/utils.h"
+
+/** Maximum distance from a line segment to food center so that the robot is
+ * able to pick a food. */
+#define FOOD_NEAR_MM (310 / 2 - 50 - 30)
+
+/** Minimum distance from a line segment to food center so that the robot do
+ * not touch the food. */
+#define FOOD_FAR_MM (450 / 2 - 10)
+
+/** Food type. */
+enum food_type_t
+{
+ FOOD_TYPE_TOMATO,
+ FOOD_TYPE_CORN,
+};
+
+/** Food information. */
+struct food_t
+{
+ /** Whether this food is still here or not. */
+ uint8_t valid;
+ /** Food position. */
+ vect_t pos;
+ /** Food type. Corn can be good or bad. */
+ enum food_type_t type;
+};
+
+/** Information gathered on food. */
+struct food_t food_table[] =
+{
+ /* {{{ */
+ /* First half. */
+ { 1, { 150, 128 + 5 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 150, 128 + 4 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 150, 128 + 3 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 150, 128 + 2 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 150, 128 + 1 * 250 }, FOOD_TYPE_CORN },
+
+ { 1, { 150 + 1 * 450, 128 + 4 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 150 + 1 * 450, 128 + 3 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 150 + 1 * 450, 128 + 2 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 150 + 1 * 450, 128 + 1 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 150 + 1 * 450, 128 + 0 * 250 }, FOOD_TYPE_CORN },
+
+ { 1, { 150 + 2 * 450, 128 + 3 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 150 + 2 * 450, 128 + 2 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 150 + 2 * 450, 128 + 1 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 150 + 2 * 450, 128 + 0 * 250 }, FOOD_TYPE_TOMATO },
+
+ /* Second half. */
+ { 1, { 3000 - 150, 128 + 5 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 3000 - 150, 128 + 4 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 3000 - 150, 128 + 3 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 3000 - 150, 128 + 2 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 3000 - 150, 128 + 1 * 250 }, FOOD_TYPE_CORN },
+
+ { 1, { 3000 - 150 - 1 * 450, 128 + 4 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 3000 - 150 - 1 * 450, 128 + 3 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 3000 - 150 - 1 * 450, 128 + 2 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 3000 - 150 - 1 * 450, 128 + 1 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 3000 - 150 - 1 * 450, 128 + 0 * 250 }, FOOD_TYPE_CORN },
+
+ { 1, { 3000 - 150 - 2 * 450, 128 + 3 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 3000 - 150 - 2 * 450, 128 + 2 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 3000 - 150 - 2 * 450, 128 + 1 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 3000 - 150 - 2 * 450, 128 + 0 * 250 }, FOOD_TYPE_TOMATO },
+
+ /* Center. */
+ { 1, { 3000 / 2, 128 + 3 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 3000 / 2, 128 + 2 * 250 }, FOOD_TYPE_CORN },
+ { 1, { 3000 / 2, 128 + 1 * 250 }, FOOD_TYPE_TOMATO },
+ { 1, { 3000 / 2, 128 + 0 * 250 }, FOOD_TYPE_CORN },
+ /* }}} */
+};
+
+uint8_t
+food_blocking (uint8_t food)
+{
+ assert (food < UTILS_COUNT (food_table) || food == 0xff);
+ return food != 0xff && food_table[food].type == FOOD_TYPE_CORN;
+}
+
+uint8_t
+food_blocking_path (vect_t a, vect_t b, int16_t ab)
+{
+ uint8_t i;
+ /* For each obstacle, try to find an intersection. */
+ for (i = 0; i < UTILS_COUNT (food_table); i++)
+ {
+ if (food_table[i].valid)
+ {
+ /* Compute square of distance to obstacle, see
+ * distance_segment_point in modules/math/geometry for the method
+ * explanation. */
+ int32_t absq = (int32_t) ab * ab;
+ vect_t vab = b; vect_sub (&vab, &a);
+ vect_t vao = food_table[i].pos; vect_sub (&vao, &a);
+ int32_t dp = vect_dot_product (&vab, &vao);
+ int32_t dsq;
+ if (dp < 0)
+ {
+ dsq = vect_dot_product (&vao, &vao);
+ }
+ else if (dp > absq)
+ {
+ vect_t vbo = food_table[i].pos; vect_sub (&vbo, &b);
+ dsq = vect_dot_product (&vbo, &vbo);
+ }
+ else
+ {
+ vect_t vabn = vab; vect_normal (&vabn);
+ dsq = vect_dot_product (&vabn, &vao) / ab;
+ dsq *= dsq;
+ }
+ /* Compare with square of authorised distance. */
+ if (dsq < (int32_t) FOOD_FAR_MM * FOOD_FAR_MM
+ && (food_table[i].type != FOOD_TYPE_TOMATO
+ || dsq > (int32_t) FOOD_NEAR_MM * FOOD_NEAR_MM))
+ return 1;
+ }
+ }
+ return 0;
+}
+
diff --git a/digital/io/src/food.h b/digital/io/src/food.h
new file mode 100644
index 00000000..a4d6b64d
--- /dev/null
+++ b/digital/io/src/food.h
@@ -0,0 +1,49 @@
+#ifndef food_h
+#define food_h
+/* food.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.
+ *
+ * }}} */
+
+/**
+ * Tools for playing element for Eurobot 2010.
+ *
+ * Elements are oranges, tomatoes and corns. We only handle tomatoes and
+ * corns.
+ */
+
+/** Return whether a food is blocking robot movements.
+ * - food: considered food, or 0xff.
+ * 0xff is accepted as a convenience for the user, it always return false. */
+uint8_t
+food_blocking (uint8_t food);
+
+/** Return whether a food is blocking a line segment.
+ * - a: line segment first point.
+ * - b: line segment second point.
+ * - ab: line segment length.
+ * - returns: 1 if the path should not be used. */
+uint8_t
+food_blocking_path (vect_t a, vect_t b, int16_t ab);
+
+#endif /* food_h */
diff --git a/digital/io/src/path.c b/digital/io/src/path.c
new file mode 100644
index 00000000..7e7ffca5
--- /dev/null
+++ b/digital/io/src/path.c
@@ -0,0 +1,462 @@
+/* path.c - Path finding for Eurobot 2010. */
+/* 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 "defs.h"
+#include "path.h"
+
+#include "food.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.
+ *
+ * The grid is composed of 13 columns of 5 node each. They are numbered by
+ * column. See eurobot/2010/table for drawings of the grid. Even columns are
+ * aligned with playing elements, while odd columns are connecting nodes
+ * placed on the middle of segments connecting playing elements. Therefore,
+ * odd columns have a offset of 125 mm, and that is the reason why code should
+ * handle odd and even columns differently.
+ *
+ * All those tricks are used to reduce the number of nodes.
+ */
+
+/** Number of possible obstacles. */
+#define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB
+
+/** Number of nodes in a column. */
+#define PATH_COLUMN_NODES_NB 5
+
+/** Number of columns. */
+#define PATH_COLUMNS_NB 13
+
+/** Number of nodes in the grid. */
+#define PATH_GRID_NODES_NB (PATH_COLUMNS_NB * PATH_COLUMN_NODES_NB)
+
+/** Number of nodes in search graph, last two nodes are destination and source
+ * nodes. */
+#define PATH_NODES_NB (PATH_GRID_NODES_NB + 2)
+
+/** Index of destination node. */
+#define PATH_DST_NODE_INDEX PATH_GRID_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;
+ /** If this node can carry a corn, this is the index of the carried corn
+ * in the food table, else, 0xff. */
+ uint8_t carry_corn;
+};
+
+/** Context. */
+struct path_t
+{
+ /** List of obstacles. */
+ struct path_obstacle_t obstacles[PATH_OBSTACLES_NB];
+ /** 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_GRID_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_GRID_NODES_NB] = {
+ /* {{{ */
+ { 1, 0 }, /* 0 column 0. */
+ { 1, 0xff }, /* 1 */
+ { 1, 2 }, /* 2 */
+ { 1, 0xff }, /* 3 */
+ { 1, 4 }, /* 4 */
+ { 1, 0xff }, /* 5 column 1. */
+ { 1, 0xff }, /* 6 */
+ { 1, 0xff }, /* 7 */
+ { 1, 0xff }, /* 8 */
+ { 1, 0xff }, /* 9 */
+ { 1, 0xff }, /* 10 column 2. */
+ { 1, 5 }, /* 11 */
+ { 1, 0xff }, /* 12 */
+ { 1, 7 }, /* 13 */
+ { 1, 0xff }, /* 14 */
+ { 1, 0xff }, /* 15 column 3. */
+ { 1, 0xff }, /* 16 */
+ { 1, 0xff }, /* 17 */
+ { 1, 0xff }, /* 18 */
+ { 1, 0xff }, /* 19 */
+ { 0, 0xff }, /* 20 column 4. */
+ { 1, 0xff }, /* 21 */
+ { 1, 10 }, /* 22 */
+ { 1, 0xff }, /* 23 */
+ { 1, 12 }, /* 24 */
+ { 1, 0xff }, /* 25 column 5. */
+ { 1, 0xff }, /* 26 */
+ { 1, 0xff }, /* 27 */
+ { 1, 0xff }, /* 28 */
+ { 1, 0xff }, /* 29 */
+ { 0, 0xff }, /* 30 column 6. */
+ { 1, 0xff }, /* 31 */
+ { 1, 0xff }, /* 32 */
+ { 1, 29 }, /* 33 */
+ { 1, 0xff }, /* 34 */
+ { 1, 0xff }, /* 35 column 7. */
+ { 1, 0xff }, /* 36 */
+ { 1, 0xff }, /* 37 */
+ { 1, 0xff }, /* 38 */
+ { 1, 0xff }, /* 39 */
+ { 0, 0xff }, /* 40 column 8. */
+ { 1, 0xff }, /* 41 */
+ { 1, 24 }, /* 42 */
+ { 1, 0xff }, /* 43 */
+ { 1, 26 }, /* 44 */
+ { 1, 0xff }, /* 45 column 9. */
+ { 1, 0xff }, /* 46 */
+ { 1, 0xff }, /* 47 */
+ { 1, 0xff }, /* 48 */
+ { 1, 0xff }, /* 49 */
+ { 1, 0xff }, /* 50 column 10. */
+ { 1, 19 }, /* 51 */
+ { 1, 0xff }, /* 52 */
+ { 1, 21 }, /* 53 */
+ { 1, 0xff }, /* 54 */
+ { 1, 0xff }, /* 55 column 11. */
+ { 1, 0xff }, /* 56 */
+ { 1, 0xff }, /* 57 */
+ { 1, 0xff }, /* 58 */
+ { 1, 0xff }, /* 59 */
+ { 1, 14 }, /* 60 column 12. */
+ { 1, 0xff }, /* 61 */
+ { 1, 16 }, /* 62 */
+ { 1, 0xff }, /* 63 */
+ { 1, 18 }, /* 64 */
+ /* }}} */
+};
+
+/** Compute position of a node. */
+static void
+path_pos (uint8_t node, vect_t *pos)
+{
+ assert (node < PATH_NODES_NB);
+ if (node < PATH_GRID_NODES_NB)
+ {
+ uint8_t col = node / PATH_COLUMN_NODES_NB;
+ uint8_t line = node - col * PATH_COLUMN_NODES_NB;
+ pos->x = 150 + col * 450 / 2;
+ pos->y = 128 + PATH_COLUMN_NODES_NB * 250
+ - (col % 2 ? 250 / 2 : 0)
+ - line * 250;
+ }
+ else
+ {
+ *pos = path.endpoints[node - PATH_GRID_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;
+ path_pos (a, &va);
+ path_pos (b, &vb);
+ /* Test for a blocking obstacle. */
+ for (i = 0; i < PATH_OBSTACLES_NB; i++)
+ {
+ if (path.obstacles[i].valid)
+ {
+ uint16_t d = distance_segment_point (&va, &vb,
+ &path.obstacles[i].c);
+ if (d < path.obstacles[i].r)
+ return 1;
+ }
+ }
+ /* Test for a blocking food. */
+ int16_t d = distance_point_point (&va, &vb);
+ *dp = d;
+ if (d == 0)
+ return 0;
+ return food_blocking_path (va, vb, d);
+}
+
+/** Update the cache of blocked nodes. */
+static void
+path_blocked_update (void)
+{
+ uint8_t i, j;
+ for (i = 0; i < PATH_GRID_NODES_NB; i++)
+ {
+ uint8_t valid = 1;
+ /* First, gather information from tables. */
+ if (!path_nodes[i].usable
+ || food_blocking (path_nodes[i].carry_corn))
+ 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 (int16_t border_xmin, int16_t border_ymin,
+ int16_t border_xmax, int16_t border_ymax)
+{
+ /* Border are ignored as only the grid is used, nothing else to do. */
+}
+
+void
+path_endpoints (vect_t s, vect_t d)
+{
+ path.endpoints[0] = d;
+ path.endpoints[1] = s;
+}
+
+void
+path_escape (uint8_t factor)
+{
+ /* TODO */
+}
+
+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)
+ {
+ /* TODO: remove useless nodes. */
+ assert (path.get != PATH_DST_NODE_INDEX);
+ uint8_t next = path.astar_nodes[path.get].prev;
+ path.get = next;
+ path_pos (next, p);
+ return 1;
+ }
+ else
+ return 0;
+}
+
+/** Neighbors callback for nodes in grid. */
+static uint8_t
+path_astar_neighbor_callback_grid (uint8_t node,
+ struct astar_neighbor_t *neighbors)
+{
+ uint8_t neighbors_nb = 0;
+ uint8_t i;
+ /* Add neighbors in all 6 directions. */
+ static const struct {
+ /** Column offset of this neighbor. */
+ int8_t column_offset;
+ /** Line offset of this neighbor. */
+ int8_t line_offset;
+ /** Extra line offset for odd columns. */
+ int8_t odd_line_offset;
+ /** Distance to this neighbor. */
+ uint16_t weight;
+ } star_n[] = {
+ { 0, -1, 0, 250 }, /* N */
+ { -1, -1, 1, 514 / 2 }, /* NW */
+ { -1, 0, 1, 514 / 2 }, /* SW */
+ { 0, 1, 0, 250 }, /* S */
+ { 1, 0, 1, 514 / 2 }, /* SE */
+ { 1, -1, 1, 514 / 2 }, /* NE */
+ };
+ uint8_t col = node / PATH_COLUMN_NODES_NB;
+ uint8_t line = node - col * PATH_COLUMN_NODES_NB;
+ uint8_t odd = col % 2;
+ for (i = 0; i < UTILS_COUNT (star_n); i++)
+ {
+ int8_t new_col = col + star_n[i].column_offset;
+ int8_t new_line = line + star_n[i].line_offset
+ + (odd ? star_n[i].odd_line_offset : 0);
+ int8_t new_node = new_col * PATH_COLUMN_NODES_NB + new_line;
+ if (new_col >= 0 && new_col < PATH_COLUMNS_NB
+ && new_line >= 0 && new_line < PATH_COLUMN_NODES_NB
+ && path.valid[new_node])
+ {
+ neighbors[neighbors_nb].node = new_node;
+ neighbors[neighbors_nb].weight = star_n[i].weight + 1;
+ neighbors_nb++;
+ }
+ }
+ /* Check if direct path OK. */
+ int16_t d;
+ 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 grid. */
+ for (i = 0; i < PATH_GRID_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_GRID_NODES_NB)
+ return path_astar_neighbor_callback_grid (node, neighbors);
+ else
+ return path_astar_neighbor_callback_endpoints (node, neighbors);
+}
+
+uint16_t
+path_astar_heuristic_callback (uint8_t node)
+{
+ /* TODO: a better and faster heuristic can be found, considering that
+ * movement is only allowed on the grid. */
+ vect_t pos;
+ path_pos (node, &pos);
+ return distance_point_point (&pos, &path.endpoints[0]);
+}
diff --git a/digital/io/src/path.h b/digital/io/src/path.h
new file mode 100644
index 00000000..0292c992
--- /dev/null
+++ b/digital/io/src/path.h
@@ -0,0 +1,78 @@
+#ifndef path_h
+#define path_h
+/* path.h - Path finding for Eurobot 2010. */
+/* 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.
+ *
+ * }}} */
+
+/** This implement a interface similar to the path module, but adapted for the
+ * special grid of Eurobot 2010. 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 (int16_t border_xmin, int16_t border_ymin,
+ int16_t border_xmax, int16_t border_ymax);
+
+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 */