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>
32 #include <vect_base.h>
37 change x,y -> i,j to avoid confusion with coords
38 could be optimized in mem space: it is not needed to store the x,y coord,
39 we can process it from idx. however it will be less optimized for speed
43 #define OFFSET_CORN_X 150
44 #define OFFSET_CORN_Y 222
45 #define STEP_CORN_X 225
46 #define STEP_CORN_Y 250
50 #define WAYPOINTS_NBX 13
51 #define WAYPOINTS_NBY 8
54 #define TYPE_WAYPOINT 0
55 #define TYPE_DANGEROUS 1
56 #define TYPE_WHITE_CORN 2
57 #define TYPE_BLACK_CORN 3
58 #define TYPE_OBSTACLE 4
59 #define TYPE_UNKNOWN 5
61 /* XXX enum possible ? else just rename */
79 struct djpoint *parent;
87 uint8_t corn_table[CORN_NB];
89 const uint8_t corn_coord_i[CORN_NB] = {
90 0, 0, 0, 2, 2, 2, 4, 4, 6,
91 6, 8, 8, 10, 10, 10, 12, 12, 12,
94 const uint8_t corn_coord_j[CORN_NB] = {
95 2, 4, 6, 3, 5, 7, 4, 6, 5,
96 7, 4, 6, 3, 5, 7, 2, 4, 6,
99 static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
101 /* table to find the symetric idx */
102 uint8_t corn_sym[] = {
103 15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2
106 uint8_t corn_side_confs[9][2] = {
117 uint8_t corn_center_confs[4][2] = {
125 /* return index from neigh pointer */
126 #define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
134 for (i=0; i<WAYPOINTS_NBX; i++) {
135 printf_P(PSTR(" %2d "), i);
137 printf_P(PSTR("\r\n"));
139 for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
140 printf_P(PSTR("%3d "), j/2);
145 for (i=0; i<WAYPOINTS_NBX; i++) {
146 pt = &djpoints[i][j/2];
148 if (((i+j) & 1) == 0)
151 if (pt->type == TYPE_OBSTACLE)
152 printf_P(PSTR(" X "));
153 else if (pt->type == TYPE_DANGEROUS)
154 printf_P(PSTR(" D "));
155 else if (pt->type == TYPE_WHITE_CORN)
156 printf_P(PSTR(" W "));
157 else if (pt->type == TYPE_BLACK_CORN)
158 printf_P(PSTR(" B "));
159 else if (pt->type == TYPE_WAYPOINT)
160 printf_P(PSTR(" %5d "), pt->weight);
162 printf_P(PSTR(" ? "));
164 printf_P(PSTR("\r\n"));
168 static inline uint8_t opposite_position(uint8_t pos)
176 /* return coord of the entry in the table from the pointer */
177 static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
179 int8_t idx = PT2IDX(pt);
180 *x = idx / WAYPOINTS_NBY;
181 *y = idx % WAYPOINTS_NBY;
184 /* get the neighbour of the point at specified position */
185 static struct djpoint *get_neigh(struct djpoint *pt,
189 struct djpoint *neigh;
191 djpoint2ij(pt, &i, &j);
219 if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
222 neigh = &djpoints[i][j];
224 if (neigh->type != TYPE_WAYPOINT)
230 static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
232 struct djpoint *neigh = NULL;
233 uint8_t pos = *position + 1;
235 for (pos = *position + 1; pos < END; pos++) {
236 neigh = get_neigh(pt, pos);
245 /* browse all points */
246 #define POINT_FOREACH(cur) \
247 for (cur = &djpoints[0][0]; \
248 cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \
252 #define NEIGH_FOREACH(neigh, pos, point) \
253 for (pos = START, neigh = get_next_neigh(point, &pos); \
255 neigh = get_next_neigh(point, &pos))
257 int dijkstra_init(void)
262 static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
265 vx = p2->pos.x - p1->pos.x;
266 vy = p2->pos.y - p1->pos.y;
267 return sqrt(vx * vx + vy * vy);
270 void dijkstra_process_neighs(struct djpoint *pt)
272 struct djpoint *neigh;
273 uint8_t pos, parent_pos;
277 djpoint2ij(pt, &i, &j);
278 printf_P(PSTR("at %d %d:\r\n"), i, j);
280 NEIGH_FOREACH(neigh, pos, pt) {
281 weight = pt->weight + dist(pt, neigh);
282 parent_pos = opposite_position(pos);
284 /* bonus if we keep the same direction */
285 if (parent_pos == pt->parent_pos ||
286 pt->parent_pos == END)
289 printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos,
290 pt->parent_pos, parent_pos,
291 neigh->weight, weight);
292 if (neigh->weight == 0 || weight < neigh->weight) {
293 djpoint2ij(neigh, &i, &j);
294 //printf_P(PSTR(" %d,%d updated\r\n"), i, j);
295 neigh->weight = weight;
296 neigh->parent_pos = parent_pos;
302 int dijkstra(struct djpoint *start)
305 uint8_t todolist = 1;
310 printf_P(PSTR("\r\n"));
312 /* process all neighbours of todo list */
316 dijkstra_process_neighs(cur);
320 /* convert updated list in todo list */
333 int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j)
336 for (n = 0; n < CORN_NB; n ++) {
337 if (i == corn_coord_i[n] &&
338 j == corn_coord_j[n])
344 int8_t corn_idx_to_coordij(uint8_t idx, uint8_t *i, uint8_t *j)
348 *i = corn_coord_i[idx];
349 *j = corn_coord_j[idx];
353 /* return the index of the closest corn at these coordinates. If the
354 * corn is really too far (~20cm), return -1. The x and y pointer are
355 * updated with the real position of the corn */
356 int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y)
358 uint8_t idx = -1, n, i, j;
359 int16_t d, x_corn, y_corn;
360 int16_t x_corn_min = 0, y_corn_min = 0;
363 for (n = 0; n < CORN_NB; n ++) {
364 corn_idx_to_coordij(n, &i, &j);
365 x_corn = (OFFSET_CORN_X + i*STEP_CORN_X);
366 y_corn = (OFFSET_CORN_Y + j*STEP_CORN_Y);
367 d = xy_norm(x_corn, y_corn, *x, *y);
368 if (d < 200 && (d_min == 0 || d < d_min)) {
384 int8_t corn_get_sym(int8_t i)
391 void init_corn_table(int8_t conf_side, int8_t conf_center)
396 if (conf_side == -1) {
397 for (i=0; i<CORN_NB; i++)
398 corn_table[i] = TYPE_UNKNOWN;
403 printf_P(PSTR("confs = %d, %d\r\n"), conf_side, conf_center);
404 for (i=0; i<CORN_NB; i++) {
405 if (i == corn_side_confs[conf_side][0] ||
406 i == corn_side_confs[conf_side][1]) {
407 corn_table[i] = TYPE_BLACK_CORN;
410 sym = corn_get_sym(i);
411 if (sym == corn_side_confs[conf_side][0] ||
412 sym == corn_side_confs[conf_side][1]) {
413 corn_table[i] = TYPE_BLACK_CORN;
416 if (i == corn_center_confs[conf_center][0] ||
417 i == corn_center_confs[conf_center][1]) {
418 corn_table[i] = TYPE_BLACK_CORN;
421 sym = corn_get_sym(i);
422 if (sym == corn_center_confs[conf_center][0] ||
423 sym == corn_center_confs[conf_center][1]) {
424 corn_table[i] = TYPE_BLACK_CORN;
427 corn_table[i] = TYPE_WHITE_CORN;
431 /* init waypoints position */
432 void init_waypoints(void)
439 for (i=0; i<WAYPOINTS_NBX; i++) {
442 y = OFFSET_CORN_Y - STEP_CORN_Y/2;
446 for (j=0; j<WAYPOINTS_NBY; j++) {
447 pt = &djpoints[i][j];
452 pt->type = TYPE_WAYPOINT;
453 pt->parent_pos = END;
464 /* update the type and weight of waypoints, before starting
466 void update_waypoints(void)
471 for (i=0; i<WAYPOINTS_NBX; i++) {
473 for (j=0; j<WAYPOINTS_NBY; j++) {
474 pt = &djpoints[i][j];
479 c = ijcoord_to_corn_idx(i, j);
481 pt->type = corn_table[c];
484 /* too close of border */
485 if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
486 pt->type = TYPE_OBSTACLE;
490 if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
491 pt->type = TYPE_OBSTACLE;
494 /* dangerous points */
495 if (i == 0 || i == (WAYPOINTS_NBX-1)) {
496 pt->type = TYPE_DANGEROUS;
499 if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
500 pt->type = TYPE_DANGEROUS;
503 pt->type = TYPE_WAYPOINT;
508 int get_path(struct djpoint *start, struct djpoint *end)
511 uint8_t prev_direction = 0;
514 cur != NULL && cur->parent_pos != END && cur != end;
515 cur = get_neigh(cur, cur->parent_pos)) {
516 if (prev_direction != cur->parent_pos) {
517 printf_P(PSTR("%d %d (%d)\r\n"),
518 cur->pos.x, cur->pos.y,
521 prev_direction = cur->parent_pos;
523 printf_P(PSTR("%d %d\r\n"), end->pos.x, end->pos.y);
531 struct djpoint *start;
534 start = &djpoints[1][1];
535 end = &djpoints[12][1];
537 init_corn_table(0, 0);
544 get_path(start, end);