2 * Copyright Droids, Microb Technology (2010)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
30 #include <aversive/pgmspace.h>
35 #include <clock_time.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <trajectory_manager_utils.h>
43 #include <trajectory_manager_core.h>
44 #include <vect_base.h>
47 #include <obstacle_avoidance.h>
48 #include <blocking_detection_manager.h>
49 #include <robot_system.h>
50 #include <position_manager.h>
52 #include <diagnostic.h>
57 #include "../common/i2c_commands.h"
58 #include "i2c_protocol.h"
61 #include "strat_base.h"
62 #include "strat_corn.h"
63 #include "strat_utils.h"
70 change x,y -> i,j to avoid confusion with coords
71 could be optimized in mem space: it is not needed to store the x,y coord,
72 we can process it from idx. however it will be less optimized for speed
76 #define OFFSET_CORN_X 150
77 #define OFFSET_CORN_Y 222
78 #define STEP_CORN_X 225
79 #define STEP_CORN_Y 250
83 #define WAYPOINTS_NBX 13
84 #define WAYPOINTS_NBY 8
87 #define TYPE_WAYPOINT 0
88 #define TYPE_DANGEROUS 1
89 #define TYPE_WHITE_CORN 2
90 #define TYPE_BLACK_CORN 3
91 #define TYPE_OBSTACLE 4
92 #define TYPE_UNKNOWN 5
94 /* XXX enum possible ? else just rename */
106 struct djpoint *parent;
109 uint8_t parent_pos:3;
114 uint8_t corn_table[CORN_NB];
116 static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
118 /* return index from neigh pointer */
119 #define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
127 for (i=0; i<WAYPOINTS_NBX; i++) {
128 printf_P(PSTR(" %2d "), i);
130 printf_P(PSTR("\r\n"));
132 for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
133 printf_P(PSTR("%3d "), j/2);
138 for (i=0; i<WAYPOINTS_NBX; i++) {
139 pt = &djpoints[i][j/2];
141 if (((i+j) & 1) == 0)
144 if (pt->type == TYPE_OBSTACLE)
145 printf_P(PSTR(" X "));
146 else if (pt->type == TYPE_DANGEROUS)
147 printf_P(PSTR(" D "));
148 else if (pt->type == TYPE_WHITE_CORN)
149 printf_P(PSTR(" W "));
150 else if (pt->type == TYPE_BLACK_CORN)
151 printf_P(PSTR(" B "));
152 else if (pt->type == TYPE_WAYPOINT)
153 printf_P(PSTR(" %5d "), pt->weight);
155 printf_P(PSTR(" ? "));
157 printf_P(PSTR("\r\n"));
161 static inline uint8_t opposite_position(uint8_t pos)
169 /* return coord of the entry in the table from the pointer */
170 static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
172 int8_t idx = PT2IDX(pt);
173 *x = idx / WAYPOINTS_NBY;
174 *y = idx % WAYPOINTS_NBY;
177 /* get the neighbour of the point at specified position */
178 static struct djpoint *get_neigh(struct djpoint *pt,
182 struct djpoint *neigh;
184 djpoint2ij(pt, &i, &j);
212 if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
215 neigh = &djpoints[i][j];
217 if (neigh->type != TYPE_WAYPOINT)
223 static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
225 struct djpoint *neigh = NULL;
226 uint8_t pos = *position + 1;
228 for (pos = *position + 1; pos < END; pos++) {
229 neigh = get_neigh(pt, pos);
238 /* browse all points */
239 #define POINT_FOREACH(cur) \
240 for (cur = &djpoints[0][0]; \
241 cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \
245 #define NEIGH_FOREACH(neigh, pos, point) \
246 for (pos = START, neigh = get_next_neigh(point, &pos); \
248 neigh = get_next_neigh(point, &pos))
250 int dijkstra_init(void)
255 static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
258 vx = p2->pos.x - p1->pos.x;
259 vy = p2->pos.y - p1->pos.y;
260 return sqrt(vx * vx + vy * vy);
263 void dijkstra_process_neighs(struct djpoint *pt)
265 struct djpoint *neigh;
266 uint8_t pos, parent_pos;
270 djpoint2ij(pt, &i, &j);
271 printf_P(PSTR("at %d %d:\r\n"), i, j);
273 NEIGH_FOREACH(neigh, pos, pt) {
274 weight = pt->weight + dist(pt, neigh);
275 parent_pos = opposite_position(pos);
277 /* bonus if we keep the same direction */
278 if (parent_pos == pt->parent_pos ||
279 pt->parent_pos == END)
282 printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos,
283 pt->parent_pos, parent_pos,
284 neigh->weight, weight);
285 if (neigh->weight == 0 || weight < neigh->weight) {
286 djpoint2ij(neigh, &i, &j);
287 //printf_P(PSTR(" %d,%d updated\r\n"), i, j);
288 neigh->weight = weight;
289 neigh->parent_pos = parent_pos;
295 int dijkstra(struct djpoint *start)
298 uint8_t todolist = 1;
303 printf_P(PSTR("\r\n"));
305 /* process all neighbours of todo list */
309 dijkstra_process_neighs(cur);
313 /* convert updated list in todo list */
326 /* init waypoints position */
327 void init_djpoints(void)
332 for (i=0; i<WAYPOINTS_NBX; i++) {
334 for (j=0; j<WAYPOINTS_NBY; j++) {
335 pt = &djpoints[i][j];
337 pt->type = TYPE_WAYPOINT;
338 pt->parent_pos = END;
345 /* update the type and weight of waypoints, before starting
347 void update_djpoints(void)
352 for (i=0; i<WAYPOINTS_NBX; i++) {
354 for (j=0; j<WAYPOINTS_NBY; j++) {
355 pt = &djpoints[i][j];
360 c = ijcoord_to_corn_idx(i, j);
362 pt->type = corn_table[c];
365 /* too close of border */
366 if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
367 pt->type = TYPE_OBSTACLE;
371 if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
372 pt->type = TYPE_OBSTACLE;
375 /* dangerous points */
376 if (i == 0 || i == (WAYPOINTS_NBX-1)) {
377 pt->type = TYPE_DANGEROUS;
380 if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
381 pt->type = TYPE_DANGEROUS;
384 pt->type = TYPE_WAYPOINT;
389 int get_path(struct djpoint *start, struct djpoint *end)
392 uint8_t prev_direction = 0;
397 cur != NULL && cur->parent_pos != END && cur != end;
398 cur = get_neigh(cur, cur->parent_pos)) {
399 if (prev_direction != cur->parent_pos) {
401 corn_idx_to_coordxy(idx, &x, &y);
402 printf_P(PSTR("%d %d (%d)\r\n"),
403 x, y, cur->parent_pos);
405 prev_direction = cur->parent_pos;
408 corn_idx_to_coordxy(idx, &x, &y);
409 printf_P(PSTR("%d %d\r\n"), x, y);