projects/microb2010/mainboard/strat_corn.c

   1 /*
   2  *  Copyright Droids, Microb Technology (2010)
   3  *
   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.
   8  *
   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.
  13  *
  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
  17  *
  18  *  Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23
  24 #include <stdio.h>
  25 #include <stdlib.h>
  26 #include <stdint.h>
  27 #include <math.h>
  28
  29 #include <aversive.h>
  30 #include <aversive/pgmspace.h>
  31
  32 #include <vect_base.h>
  33
  34 /* XXX TODO
  35 static
  36 const
  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
  40
  41 */
  42
  43 #define OFFSET_CORN_X 150
  44 #define OFFSET_CORN_Y 222
  45 #define STEP_CORN_X 225
  46 #define STEP_CORN_Y 250
  47
  48 #define CORN_NB 18
  49
  50 #define WAYPOINTS_NBX 13
  51 #define WAYPOINTS_NBY 8
  52
  53 /* enum is better */
  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
  60
  61 /* XXX enum possible ? else just rename */
  62 #define START      0
  63 #define UP         1
  64 #define UP_RIGHT   2
  65 #define DOWN_RIGHT 3
  66 #define DOWN       4
  67 #define DOWN_LEFT  5
  68 #define UP_LEFT    6
  69 #define END        7
  70
  71 struct point {
  72         int32_t x;
  73         int32_t y;
  74 };
  75
  76 struct djpoint {
  77         struct point pos;
  78         uint16_t weight;
  79         struct djpoint *parent;
  80
  81         uint8_t type:3;
  82         uint8_t parent_pos:3;
  83         uint8_t updated:1;
  84         uint8_t todo:1;
  85 };
  86
  87 uint8_t corn_table[CORN_NB];
  88
  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,
  92 };
  93
  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,
  97 };
  98
  99 static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
 100
 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
 104 };
 105
 106 uint8_t corn_side_confs[9][2] = {
 107         { 1, 4 },
 108         { 0, 4 },
 109         { 2, 4 },
 110         { 2, 3 },
 111         { 0, 3 },
 112         { 1, 3 },
 113         { 1, 6 },
 114         { 0, 6 },
 115         { 2, 6 },
 116 };
 117 uint8_t corn_center_confs[4][2] = {
 118         { 5, 8 },
 119         { 7, 8 },
 120         { 5, 9 },
 121         { 7, 8 },
 122 };
 123
 124
 125 /* return index from neigh pointer */
 126 #define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
 127
 128 void dump(void)
 129 {
 130         int8_t i, j;
 131         struct djpoint *pt;
 132
 133         printf_P(PSTR("         "));
 134         for (i=0; i<WAYPOINTS_NBX; i++) {
 135                 printf_P(PSTR(" %2d "), i);
 136         }
 137         printf_P(PSTR("\r\n"));
 138
 139         for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
 140                 printf_P(PSTR("%3d   "), j/2);
 141
 142                 if ((j&1) == 0)
 143                         printf_P(PSTR("    "));
 144
 145                 for (i=0; i<WAYPOINTS_NBX; i++) {
 146                         pt = &djpoints[i][j/2];
 147
 148                         if (((i+j) & 1) == 0)
 149                                 continue;
 150
 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);
 161                         else
 162                                 printf_P(PSTR("     ?  "));
 163                 }
 164                 printf_P(PSTR("\r\n"));
 165         }
 166 }
 167
 168 static inline uint8_t opposite_position(uint8_t pos)
 169 {
 170         pos += 3;
 171         if (pos > UP_LEFT)
 172                 pos -= 6;
 173         return pos;
 174 }
 175
 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)
 178 {
 179         int8_t idx = PT2IDX(pt);
 180         *x = idx / WAYPOINTS_NBY;
 181         *y = idx % WAYPOINTS_NBY;
 182 }
 183
 184 /* get the neighbour of the point at specified position */
 185 static struct djpoint *get_neigh(struct djpoint *pt,
 186                                  uint8_t position)
 187 {
 188         int8_t i,j;
 189         struct djpoint *neigh;
 190
 191         djpoint2ij(pt, &i, &j);
 192
 193         switch (position) {
 194         case UP:
 195                 j++;
 196                 break;
 197         case UP_RIGHT:
 198                 if (!(i & 1)) j++;
 199                 i++;
 200                 break;
 201         case DOWN_RIGHT:
 202                 if (i & 1) j--;
 203                 i++;
 204                 break;
 205         case DOWN:
 206                 j--;
 207                 break;
 208         case DOWN_LEFT:
 209                 if (i & 1) j--;
 210                 i--;
 211                 break;
 212         case UP_LEFT:
 213                 if (!(i & 1)) j++;
 214                 i--;
 215                 break;
 216         default:
 217                 return NULL;
 218         }
 219         if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
 220                 return NULL;
 221
 222         neigh = &djpoints[i][j];
 223
 224         if (neigh->type != TYPE_WAYPOINT)
 225                 return NULL;
 226
 227         return neigh;
 228 }
 229
 230 static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
 231 {
 232         struct djpoint *neigh = NULL;
 233         uint8_t pos = *position + 1;
 234
 235         for (pos = *position + 1; pos < END; pos++) {
 236                 neigh = get_neigh(pt, pos);
 237                 if (neigh != NULL)
 238                         break;
 239         }
 240
 241         *position = pos;
 242         return neigh;
 243 }
 244
 245 /* browse all points */
 246 #define POINT_FOREACH(cur)                                              \
 247         for (cur = &djpoints[0][0];                                     \
 248              cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];             \
 249              cur ++)
 250
 251 /* XXX comment */
 252 #define NEIGH_FOREACH(neigh, pos, point)                        \
 253         for (pos = START, neigh = get_next_neigh(point, &pos);  \
 254              neigh;                                             \
 255              neigh = get_next_neigh(point, &pos))
 256
 257 int dijkstra_init(void)
 258 {
 259         return 0;
 260 }
 261
 262 static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
 263 {
 264         double vx, vy;
 265         vx = p2->pos.x - p1->pos.x;
 266         vy = p2->pos.y - p1->pos.y;
 267         return sqrt(vx * vx + vy * vy);
 268 }
 269
 270 void dijkstra_process_neighs(struct djpoint *pt)
 271 {
 272         struct djpoint *neigh;
 273         uint8_t pos, parent_pos;
 274         uint16_t weight;
 275         int8_t i,j;
 276
 277         djpoint2ij(pt, &i, &j);
 278         printf_P(PSTR("at %d %d:\r\n"), i, j);
 279
 280         NEIGH_FOREACH(neigh, pos, pt) {
 281                 weight = pt->weight + dist(pt, neigh);
 282                 parent_pos = opposite_position(pos);
 283
 284                 /* bonus if we keep the same direction */
 285                 if (parent_pos == pt->parent_pos ||
 286                     pt->parent_pos == END)
 287                         weight = weight - 1;
 288
 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;
 297                         neigh->updated = 1;
 298                 }
 299         }
 300 }
 301
 302 int dijkstra(struct djpoint *start)
 303 {
 304         struct djpoint *cur;
 305         uint8_t todolist = 1;
 306
 307         start->todo = 1;
 308
 309         while (todolist) {
 310                 printf_P(PSTR("\r\n"));
 311                 dump();
 312                 /* process all neighbours of todo list */
 313                 POINT_FOREACH(cur) {
 314                         if (!cur->todo)
 315                                 continue;
 316                         dijkstra_process_neighs(cur);
 317                         cur->todo = 0;
 318                 }
 319
 320                 /* convert updated list in todo list */
 321                 todolist = 0;
 322                 POINT_FOREACH(cur) {
 323                         if (!cur->updated)
 324                                 continue;
 325                         todolist = 1;
 326                         cur->todo = 1;
 327                         cur->updated = 0;
 328                 }
 329         }
 330         return 0; /* XXX */
 331 }
 332
 333 int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j)
 334 {
 335         uint8_t n;
 336         for (n = 0; n < CORN_NB; n ++) {
 337                 if (i == corn_coord_i[n] &&
 338                     j == corn_coord_j[n])
 339                         return n;
 340         }
 341         return -1;
 342 }
 343
 344 int8_t corn_idx_to_coordij(uint8_t idx, uint8_t *i, uint8_t *j)
 345 {
 346         if (idx >= CORN_NB)
 347                 return -1;
 348         *i = corn_coord_i[idx];
 349         *j = corn_coord_j[idx];
 350         return 0;
 351 }
 352
 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)
 357 {
 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;
 361         int16_t d_min = 0;
 362
 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)) {
 369                         d_min = d;
 370                         idx = n;
 371                         x_corn_min = x_corn;
 372                         y_corn_min = y_corn;
 373                 }
 374         }
 375         if (d_min == 0)
 376                 return -1;
 377
 378         *x = x_corn_min;
 379         *y = y_corn_min;
 380
 381         return idx;
 382 }
 383
 384 int8_t corn_get_sym(int8_t i)
 385 {
 386         if (i >= CORN_NB)
 387                 return -1;
 388         return corn_sym[i];
 389 }
 390
 391 void init_corn_table(int8_t conf_side, int8_t conf_center)
 392 {
 393         int8_t sym, i;
 394
 395         /* before match */
 396         if (conf_side == -1) {
 397                 for (i=0; i<CORN_NB; i++)
 398                         corn_table[i] = TYPE_UNKNOWN;
 399                 return;
 400         }
 401
 402         /* for test */
 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;
 408                         continue;
 409                 }
 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;
 414                         continue;
 415                 }
 416                 if (i == corn_center_confs[conf_center][0] ||
 417                     i == corn_center_confs[conf_center][1]) {
 418                         corn_table[i] = TYPE_BLACK_CORN;
 419                         continue;
 420                 }
 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;
 425                         continue;
 426                 }
 427                 corn_table[i] = TYPE_WHITE_CORN;
 428         }
 429 }
 430
 431 /* init waypoints position */
 432 void init_waypoints(void)
 433 {
 434         int8_t i, j;
 435         int32_t x, y;
 436         struct djpoint *pt;
 437
 438         x = OFFSET_CORN_X;
 439         for (i=0; i<WAYPOINTS_NBX; i++) {
 440
 441                 if (i & 1)
 442                         y = OFFSET_CORN_Y - STEP_CORN_Y/2;
 443                 else
 444                         y = OFFSET_CORN_Y;
 445
 446                 for (j=0; j<WAYPOINTS_NBY; j++) {
 447                         pt = &djpoints[i][j];
 448
 449                         pt->pos.x = x;
 450                         pt->pos.y = y;
 451
 452                         pt->type = TYPE_WAYPOINT;
 453                         pt->parent_pos = END;
 454                         pt->updated = 0;
 455                         pt->todo = 0;
 456
 457                         y += STEP_CORN_Y;
 458                 }
 459
 460                 x += STEP_CORN_X;
 461         }
 462 }
 463
 464 /* update the type and weight of waypoints, before starting
 465  * dijkstra */
 466 void update_waypoints(void)
 467 {
 468         int8_t i, j, c;
 469         struct djpoint *pt;
 470
 471         for (i=0; i<WAYPOINTS_NBX; i++) {
 472
 473                 for (j=0; j<WAYPOINTS_NBY; j++) {
 474                         pt = &djpoints[i][j];
 475
 476                         pt->weight = 0;
 477
 478                         /* corn */
 479                         c = ijcoord_to_corn_idx(i, j);
 480                         if (c >= 0) {
 481                                 pt->type = corn_table[c];
 482                                 continue;
 483                         }
 484                         /* too close of border */
 485                         if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
 486                                 pt->type = TYPE_OBSTACLE;
 487                                 continue;
 488                         }
 489                         /* hill */
 490                         if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
 491                                 pt->type = TYPE_OBSTACLE;
 492                                 continue;
 493                         }
 494                         /* dangerous points */
 495                         if (i == 0 || i == (WAYPOINTS_NBX-1)) {
 496                                 pt->type = TYPE_DANGEROUS;
 497                                 continue;
 498                         }
 499                         if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
 500                                 pt->type = TYPE_DANGEROUS;
 501                                 continue;
 502                         }
 503                         pt->type = TYPE_WAYPOINT;
 504                 }
 505         }
 506 }
 507
 508 int get_path(struct djpoint *start, struct djpoint *end)
 509 {
 510         struct djpoint *cur;
 511         uint8_t prev_direction = 0;
 512
 513         for (cur=start;
 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,
 519                                  cur->parent_pos);
 520                 }
 521                 prev_direction = cur->parent_pos;
 522         }
 523         printf_P(PSTR("%d %d\r\n"), end->pos.x, end->pos.y);
 524
 525         return 0; /* XXX */
 526 }
 527
 528 #if 0
 529 int main(void)
 530 {
 531         struct djpoint *start;
 532         struct djpoint *end;
 533
 534         start = &djpoints[1][1];
 535         end = &djpoints[12][1];
 536
 537         init_corn_table(0, 0);
 538         init_waypoints();
 539         update_waypoints();
 540
 541         dijkstra(end);
 542         dump();
 543
 544         get_path(start, end);
 545
 546         return 0;
 547 }
 548 #endif