projects/microb2010/mainboard/strat_avoid.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 #include <aversive/error.h>
  32
  33 #include <ax12.h>
  34 #include <uart.h>
  35 #include <pwm_ng.h>
  36 #include <clock_time.h>
  37 #include <spi.h>
  38
  39 #include <pid.h>
  40 #include <quadramp.h>
  41 #include <control_system_manager.h>
  42 #include <trajectory_manager.h>
  43 #include <trajectory_manager_utils.h>
  44 #include <trajectory_manager_core.h>
  45 #include <vect_base.h>
  46 #include <lines.h>
  47 #include <polygon.h>
  48 #include <obstacle_avoidance.h>
  49 #include <blocking_detection_manager.h>
  50 #include <robot_system.h>
  51 #include <position_manager.h>
  52
  53 #include <diagnostic.h>
  54
  55 #include <rdline.h>
  56 #include <parse.h>
  57
  58 #include "../common/i2c_commands.h"
  59 #include "i2c_protocol.h"
  60 #include "main.h"
  61 #include "strat.h"
  62 #include "strat_db.h"
  63 #include "strat_base.h"
  64 #include "strat_corn.h"
  65 #include "strat_avoid.h"
  66 #include "strat_utils.h"
  67 #include "sensor.h"
  68 #include "actuator.h"
  69
  70 //#define VERBOSE
  71
  72 #ifdef VERBOSE
  73 #define DPR(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__)
  74 #else
  75 #define DPR(args...) do {} while (0)
  76 #endif
  77
  78 struct circuit {
  79         const char *name;
  80         uint8_t len;
  81         const struct wp_coord *path;
  82 };
  83
  84 const struct wp_coord butterfly_tab[] = {
  85         { .i = 11, .j = 6, },
  86         { .i = 10, .j = 6, },
  87         { .i = 9, .j = 5, },
  88         { .i = 8, .j = 5, },
  89         { .i = 7, .j = 4, },
  90         { .i = 6, .j = 4, },
  91         { .i = 5, .j = 4, },
  92         { .i = 4, .j = 5, },
  93         { .i = 3, .j = 5, },
  94         { .i = 2, .j = 6, },
  95         { .i = 1, .j = 6, },
  96         { .i = 1, .j = 5, },
  97         { .i = 1, .j = 4, },
  98         { .i = 1, .j = 3, },
  99         { .i = 1, .j = 2, },
 100         { .i = 1, .j = 1, },
 101         { .i = 2, .j = 2, },
 102         { .i = 3, .j = 2, },
 103         { .i = 4, .j = 3, },
 104         { .i = 5, .j = 3, },
 105         { .i = 6, .j = 4, },
 106         { .i = 7, .j = 3, },
 107         { .i = 8, .j = 3, },
 108         { .i = 9, .j = 2, },
 109         { .i = 10, .j = 2, },
 110         { .i = 11, .j = 1, },
 111         { .i = 11, .j = 2, },
 112         { .i = 11, .j = 3, },
 113         { .i = 11, .j = 4, },
 114         { .i = 11, .j = 5, },
 115         { .i = 11, .j = 6, },
 116 };
 117
 118 const struct circuit butterfly = {
 119         .name = "butterfly",
 120         .len = sizeof(butterfly_tab)/sizeof(struct wp_coord),
 121         .path = butterfly_tab,
 122 };
 123
 124 const struct wp_coord small_tab[] = {
 125         { .i = 11, .j = 6, },
 126         { .i = 10, .j = 6, },
 127         { .i = 9, .j = 5, },
 128         { .i = 9, .j = 4, },
 129         { .i = 9, .j = 3, },
 130         { .i = 10, .j = 4, },
 131         { .i = 11, .j = 4, },
 132         { .i = 11, .j = 5, },
 133         { .i = 11, .j = 6, },
 134 };
 135
 136 const struct circuit small = {
 137         .name = "small",
 138         .len = sizeof(small)/sizeof(struct wp_coord),
 139         .path = small_tab,
 140 };
 141
 142 /* list of all possible circuits */
 143 const struct circuit *circuits[] = {
 144         &butterfly,
 145         &small,
 146         NULL,
 147 };
 148
 149 /* symetric neighbor position */
 150 static inline uint8_t opposite_position(uint8_t pos)
 151 {
 152         pos += 3;
 153         if (pos > LINE_L_UP)
 154                 pos -= 6;
 155         return pos;
 156 }
 157
 158 #ifdef HOST_VERSION
 159 //#define TEST_STRAT_AVOID
 160 #endif
 161
 162 #ifdef TEST_STRAT_AVOID
 163 static uint8_t cc;
 164 uint8_t xget_cob_count(void)
 165 {
 166         return cc;
 167 }
 168
 169 static uint8_t bc;
 170 uint8_t xget_ball_count(void)
 171 {
 172         return bc;
 173 }
 174
 175 static uint32_t ts;
 176 uint8_t xtime_get_s(void)
 177 {
 178         return ts;
 179 }
 180 #else
 181 #define xget_cob_count() get_cob_count()
 182 #define xget_ball_count() get_ball_count()
 183 #define xtime_get_s() time_get_s()
 184 #endif
 185
 186 /* return true if turn is at 60 deg */
 187 uint8_t is_60deg(uint8_t dir1, uint8_t dir2)
 188 {
 189         int8_t turn;
 190
 191         turn = dir2-dir1;
 192         if (turn < 0)
 193                 turn += 6;
 194         if (turn == 1)
 195                 return 1;
 196         if (turn == 5)
 197                 return 1;
 198         return 0;
 199 }
 200
 201 /* return true if turn is at 60 deg */
 202 uint8_t is_120deg(uint8_t dir1, uint8_t dir2)
 203 {
 204         int8_t turn;
 205
 206         turn = dir2-dir1;
 207         if (turn < 0)
 208                 turn += 6;
 209         if (turn == 2)
 210                 return 1;
 211         if (turn == 4)
 212                 return 1;
 213         return 0;
 214 }
 215
 216 /* get the neighbour of the point at specified dir, return -1 if
 217  * there is no neighbor */
 218 int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj,
 219                  uint8_t dir)
 220 {
 221         switch (dir) {
 222         case LINE_UP:
 223                 j++;
 224                 break;
 225         case LINE_R_UP:
 226                 if ((i & 1)) j++;
 227                 i++;
 228                 break;
 229         case LINE_R_DOWN:
 230                 if (!(i & 1)) j--;
 231                 i++;
 232                 break;
 233         case LINE_DOWN:
 234                 j--;
 235                 break;
 236         case LINE_L_DOWN:
 237                 if (!(i & 1)) j--;
 238                 i--;
 239                 break;
 240         case LINE_L_UP:
 241                 if ((i & 1)) j++;
 242                 i--;
 243                 break;
 244         default:
 245                 return -1;
 246         }
 247         if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
 248                 return -1;
 249
 250         *ni = i;
 251         *nj = j;
 252         return 0;
 253 }
 254
 255 static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
 256 {
 257         switch (dir) {
 258         case LINE_UP:
 259         case LINE_DOWN:
 260                 return i/2;
 261         case LINE_R_UP:
 262         case LINE_L_DOWN:
 263                 i &= 0xfe;
 264                 j -= i/2;
 265                 return (5-j)/2;
 266         case LINE_R_DOWN:
 267         case LINE_L_UP:
 268                 i &= 0xfe;
 269                 j += i/2;
 270                 return (11-j)/2;
 271         default:
 272                 return -1;
 273         }
 274 }
 275
 276 static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j,
 277                        uint8_t i, uint8_t j)
 278 {
 279         int8_t diff_i, diff_j;
 280
 281         diff_i = i - prev_i;
 282         diff_j = j - prev_j;
 283
 284         if (diff_i == 0 && diff_j == 1)
 285                 return LINE_UP;
 286         if (diff_i == 0 && diff_j == -1)
 287                 return LINE_DOWN;
 288
 289         if ((prev_i & 1) == 0) {
 290                 if (diff_i == 1 && diff_j == 0)
 291                         return LINE_R_UP;
 292                 if (diff_i == 1 && diff_j == -1)
 293                         return LINE_R_DOWN;
 294                 if (diff_i == -1 && diff_j == 0)
 295                         return LINE_L_UP;
 296                 if (diff_i == -1 && diff_j == -1)
 297                         return LINE_L_DOWN;
 298         }
 299         else {
 300                 if (diff_i == 1 && diff_j == 1)
 301                         return LINE_R_UP;
 302                 if (diff_i == 1 && diff_j == 0)
 303                         return LINE_R_DOWN;
 304                 if (diff_i == -1 && diff_j == 1)
 305                         return LINE_L_UP;
 306                 if (diff_i == -1 && diff_j == 0)
 307                         return LINE_L_DOWN;
 308         }
 309
 310         /* invalid value */
 311         return 0xFF;
 312 }
 313
 314 /* return true if a waypoint belongs to a line */
 315 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
 316 {
 317         uint8_t ln;
 318         ln = get_line_num(i, j, dir);
 319         if (ln == linenum)
 320                 return 1;
 321         return 0;
 322 }
 323
 324 /* count the number of non-black corns which are neighbors of
 325  * specified cob */
 326 uint8_t corn_count_neigh(uint8_t i, uint8_t j)
 327 {
 328         uint8_t dir, n = 0;
 329         uint8_t ni, nj;
 330
 331         for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
 332                 if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
 333                         continue;
 334
 335                 /* is there a corn cob ? */
 336                 if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
 337                     strat_db.wp_table[ni][nj].present &&
 338                     strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK)
 339                         n ++;
 340         }
 341
 342         return n;
 343 }
 344
 345
 346 /* fill circuit_wpline table with waypoints from circuit starting at
 347  * i,j and using selected face */
 348 static int8_t get_path(const struct circuit *circuit,
 349                        uint8_t starti, uint8_t startj, uint8_t faceA,
 350                        struct wp_line *circuit_wpline)
 351 {
 352         const struct wp_coord *curcircuit;
 353         const struct wp_coord *start;
 354         const struct wp_coord *end;
 355         uint8_t prev_i, prev_j;
 356         uint8_t dir, prev_dir = 0xFF;
 357         uint8_t found = 0, i = 0, j = 0;
 358         uint8_t linenum;
 359         int8_t step = faceA ? 1 : -1;
 360         int8_t path_len = 0;
 361
 362         /* start and end of circuit */
 363         if (faceA) {
 364                 start = &circuit->path[0];
 365                 end = start + circuit->len - 1;
 366         }
 367         else {
 368                 end = &circuit->path[0];
 369                 start = end + circuit->len - 1;
 370         }
 371
 372         DPR("face: %s %d\r\n", circuit->name, faceA);
 373
 374         /* check that the point is present in the circuit */
 375         for (curcircuit = start; curcircuit != end; curcircuit += step) {
 376                 if (curcircuit->i == starti && curcircuit->j == startj) {
 377                         found = 1;
 378                         break;
 379                 }
 380         }
 381         if (found == 0)
 382                 return -1;
 383
 384
 385         /* browse the circuit from starti, startj in the specified
 386          * direction, and fill the table when direction changes */
 387         prev_i = starti;
 388         prev_j = startj;
 389         for ( ; curcircuit != end;
 390               curcircuit += step, prev_dir = dir, prev_i = i, prev_j = j) {
 391
 392                 i = curcircuit->i;
 393                 j = curcircuit->j;
 394
 395                 dir = get_dir(prev_i, prev_j, i, j);
 396
 397                 if (prev_dir != dir) {
 398                         linenum = get_line_num(prev_i, prev_j, dir);
 399                         circuit_wpline[path_len].line_num = linenum;
 400                         circuit_wpline[path_len].dir = dir;
 401                         path_len++;
 402                 }
 403         }
 404
 405         return path_len;
 406 }
 407
 408 /* process score from retrieved objects number, and circuit len */
 409 static int16_t get_score(uint32_t wcorn_retrieved,
 410                          uint32_t ucorn_retrieved,
 411                          uint16_t tomato_retrieved,
 412                          uint8_t len, uint8_t opp_on_path)
 413 {
 414         int16_t score = 0;
 415         uint8_t i;
 416         uint32_t mask = 1;
 417         uint8_t n;
 418
 419         /* score with corn */
 420         n = xget_cob_count() * 2;
 421         for (i = 0; i<CORN_NB; i++) {
 422                 if (n >= 10)
 423                         break;
 424                 if (wcorn_retrieved & mask) {
 425                         score += 250;
 426                         n += 2;
 427                 }
 428                 if (n >= 10)
 429                         break;
 430                 if (ucorn_retrieved & mask) {
 431                         score += 125;
 432                         n += 1;
 433                 }
 434                 mask <<= 1UL;
 435         }
 436
 437         DPR("get score: cob %d (->%d)\r\n", n, n/2);
 438
 439         /* score with tomato */
 440         n = xget_ball_count();
 441         mask = 1;
 442         for (i = 0; i<TOMATO_NB; i++) {
 443                 if (n >= 4)
 444                         break;
 445                 if (tomato_retrieved & mask) {
 446                         score += 150;
 447                         n += 1;
 448                 }
 449                 mask <<= 1UL;
 450         }
 451
 452         DPR("get score: ball %d\r\n", n);
 453
 454         /* malus for long circuits */
 455         score -= (len * 20);
 456         DPR("malus for length: %d\r\n", len * 20);
 457
 458         /* double malus for long circuits if we don't have much
 459          * time */
 460 #define WP_SPEED 1
 461         if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) {
 462                 int32_t extra;
 463                 extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s());
 464                 extra = (200 * extra);
 465                 if (extra < 0) /* should not happen */
 466                         extra = 0;
 467                 if (extra > 10000)
 468                         extra = 10000;
 469                 score -= extra;
 470                 DPR("malus for length + time: %d\r\n", extra);
 471         }
 472
 473         /* malus if there is opponent on the path */
 474         if (opp_on_path) {
 475                 DPR("malus for opponent: 1000\r\n");
 476                 score -= 2000;
 477         }
 478
 479         return score;
 480 }
 481
 482 /* return the corn type of specified coords: I2C_COB_WHITE,
 483  * I2C_COB_UNKNOWN, or I2C_COB_NONE if it is black or not present */
 484 static uint8_t get_corn_type(uint8_t i, uint8_t j)
 485 {
 486         uint8_t color;
 487         /* is there a corn cob ? */
 488         if (strat_db.wp_table[i][j].type == WP_TYPE_CORN &&
 489             strat_db.wp_table[i][j].present) {
 490                 color = strat_db.wp_table[i][j].corn.color;
 491                 if (color == I2C_COB_WHITE)
 492                         return I2C_COB_WHITE;
 493                 else if (color == I2C_COB_UNKNOWN)
 494                         return I2C_COB_UNKNOWN;
 495         }
 496         return I2C_COB_NONE;
 497 }
 498
 499 /* i,j: starting position */
 500 static int8_t evaluate_one_face(const struct circuit *circuit,
 501                                 uint8_t starti, uint8_t startj,
 502                                 uint8_t faceA, int16_t *score)
 503 {
 504         const struct wp_coord *curcircuit;
 505         const struct wp_coord *start;
 506         const struct wp_coord *end;
 507         uint32_t wcorn_retrieved = 0; /* bit mask */
 508         uint32_t ucorn_retrieved = 0; /* bit mask */
 509         uint16_t tomato_retrieved = 0; /* bit mask */
 510         uint8_t opponent_on_path = 0;
 511         uint8_t len = 0, found = 0;
 512         uint8_t i, j, prev_i, prev_j;
 513         uint8_t ni = 0, nj = 0;
 514         uint8_t dir, color, idx;
 515         int8_t step = faceA ? 1 : -1;
 516         int16_t x, y, d;
 517         int16_t oppx, oppy;
 518
 519         *score = 0x8000; /* -int_max */
 520
 521         /* start and end of circuit */
 522         if (faceA) {
 523                 start = &circuit->path[0];
 524                 end = start + circuit->len - 1;
 525         }
 526         else {
 527                 end = &circuit->path[0];
 528                 start = end + circuit->len - 1;
 529         }
 530
 531         DPR("%s() face: %s %d\r\n", __FUNCTION__, circuit->name, faceA);
 532
 533         /* check that the point is present in the circuit */
 534         for (curcircuit = start; curcircuit != end; curcircuit += step) {
 535                 if (curcircuit->i == starti && curcircuit->j == startj) {
 536                         found = 1;
 537                         break;
 538                 }
 539         }
 540         if (found == 0)
 541                 return -1;
 542
 543         /* get opponent coords */
 544         if (get_opponent_xy(&oppx, &oppy) < 0)
 545                 oppx = I2C_OPPONENT_NOT_THERE;
 546
 547         /* silent the compiler */
 548         prev_i = 0xff;
 549         prev_j = 0xff;
 550
 551         /* browse all points and calculate the score */
 552         for (curcircuit = start;
 553              curcircuit != end;
 554              curcircuit += step, len ++, prev_i = i, prev_j = j) {
 555                 i = curcircuit->i;
 556                 j = curcircuit->j;
 557
 558                 /* is opponent near the point ? */
 559                 ijcoord_to_xycoord(i, j, &x, &y);
 560                 if (oppx != I2C_OPPONENT_NOT_THERE) {
 561                         d = distance_between(oppx, oppy, x, y);
 562                         if (d < 600)
 563                                 opponent_on_path = 1;
 564                 }
 565
 566                 /* don't try to look cobs/tomato for first point */
 567                 if (curcircuit == start)
 568                         continue;
 569
 570                 /* get current direction, we wil check cobs behind us
 571                  * on left and right */
 572                 dir = get_dir(prev_i, prev_j, i, j);
 573
 574                 DPR("%d %d -> %d %d  (%d)\n", prev_i, prev_j, i, j, dir);
 575
 576                 /* is there a tomato ? */
 577                 if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
 578                     strat_db.wp_table[i][j].present) {
 579                         DPR("  TOMATO\n");
 580                         tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
 581                 }
 582
 583                 /* behind left */
 584                 if (wp_get_neigh(i, j, &ni, &nj, (dir + 2) % 6) == 0) {
 585                         color = get_corn_type(ni, nj);
 586                         idx = strat_db.wp_table[ni][nj].corn.idx;
 587                         if (color == I2C_COB_WHITE) {
 588                                 DPR("  LEFT WCORN (%d)\n", idx);
 589                                 wcorn_retrieved |= (1UL << idx);
 590                         }
 591                         else if (color == I2C_COB_UNKNOWN) {
 592                                 DPR("  LEFT UCORN (%d)\n", idx);
 593                                 ucorn_retrieved |= (1UL << idx);
 594                         }
 595                 }
 596
 597                 /* behind right */
 598                 if (wp_get_neigh(i, j, &ni, &nj, (dir + 4) % 6) == 0) {
 599                         color = get_corn_type(ni, nj);
 600                         idx = strat_db.wp_table[ni][nj].corn.idx;
 601                         if (color == I2C_COB_WHITE) {
 602                                 DPR("  RIGHT WCORN (%d)\n", idx);
 603                                 wcorn_retrieved |= (1UL << idx);
 604                         }
 605                         else if (color == I2C_COB_UNKNOWN) {
 606                                 DPR("  RIGHT UCORN (%d)\n", idx);
 607                                 ucorn_retrieved |= (1UL << idx);
 608                         }
 609                 }
 610
 611                 /* prev_i, prev_j, len and curcircuit are updated in
 612                  * for (;;) */
 613         }
 614
 615         /* write score and exit */
 616         *score = get_score(wcorn_retrieved, ucorn_retrieved,
 617                            tomato_retrieved, len, opponent_on_path);
 618         return 0;
 619 }
 620
 621 /* i,j: starting position */
 622 static int8_t evaluate_one_circuit(const struct circuit *circuit,
 623                                    uint8_t starti, uint8_t startj,
 624                                    int16_t *scoreA, int16_t *scoreB)
 625 {
 626         if (evaluate_one_face(circuit, starti, startj, 1, scoreA) < 0)
 627                 return -1;
 628
 629         /* we are on eject point, scoreB is the same */
 630         if (starti == 11 && startj == 6) {
 631                 *scoreB = *scoreA;
 632                 return 0;
 633         }
 634
 635         if (evaluate_one_face(circuit, starti, startj, 0, scoreB) < 0)
 636                 return -1;
 637         return 0;
 638 }
 639
 640 /* i,j starting position */
 641 int8_t find_best_circuit(uint8_t i, uint8_t j,
 642                        const struct circuit **selected_circuit,
 643                        int8_t *selected_face)
 644 {
 645         const struct circuit **circuit;
 646         int16_t scoreA, scoreB;
 647         int16_t selected_score = 0x8000; /* ~-int_max */
 648         int8_t found = -1;
 649
 650         *selected_face = 0;
 651         *selected_circuit = circuits[0] ;
 652         for (circuit = &circuits[0]; *circuit; circuit++) {
 653                 if (evaluate_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
 654                         continue;
 655                 found = 0;
 656                 DEBUG(E_USER_STRAT, "Scores for %s are: faceA=%d, faceB=%d",
 657                       (*circuit)->name, scoreA, scoreB);
 658                 if (scoreA > selected_score) {
 659                         *selected_circuit = *circuit;
 660                         selected_score = scoreA;
 661                         *selected_face = 0;
 662                 }
 663                 if (scoreB > selected_score) {
 664                         *selected_circuit = *circuit;
 665                         selected_score = scoreB;
 666                         *selected_face = 1;
 667                 }
 668         }
 669
 670         if (found == -1)
 671                 DEBUG(E_USER_STRAT, "no circuit found");
 672         else
 673                 DEBUG(E_USER_STRAT, "circuit found: %s, %s",
 674                       (*selected_circuit)->name,
 675                       (*selected_face) ? "faceA":"faceB");
 676         return found;
 677 }
 678
 679 static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
 680 {
 681         int8_t i;
 682         if (len <= 0)
 683                 return;
 684         for (i = 0; i < len; i ++) {
 685                 DEBUG(E_USER_STRAT, "linenum %d dir %d",
 686                       circuit_wpline[i].line_num,
 687                        circuit_wpline[i].dir);
 688         }
 689
 690 }
 691
 692 /* choose a circuit, then harvest on this circuit */
 693 uint8_t strat_harvest_circuit(void)
 694 {
 695         const struct circuit *selected_circuit;
 696         int8_t selected_face;
 697         struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
 698         int8_t len;
 699         uint8_t i, j, idx;
 700         int16_t x, y;
 701         uint8_t linenum, prev_linenum;
 702         uint8_t dir, prev_dir;
 703         uint8_t err;
 704
 705         x = position_get_x_s16(&mainboard.pos);
 706         y = position_get_y_s16(&mainboard.pos);
 707
 708         if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
 709                 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
 710                       __FUNCTION__, x, y);
 711                 return END_ERROR;
 712         }
 713
 714         if (find_best_circuit(i, j, &selected_circuit, &selected_face) < 0) {
 715                 DEBUG(E_USER_STRAT, "%s(): cannot find a good circuit",
 716                       __FUNCTION__);
 717                 return END_ERROR;
 718         }
 719
 720         len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 721         if (len < 0) {
 722                 DEBUG(E_USER_STRAT, "%s(): cannot find a path",
 723                       __FUNCTION__);
 724                 return END_ERROR;
 725         }
 726
 727         dump_circuit_wp(circuit_wpline, len);
 728
 729         prev_linenum = circuit_wpline[0].line_num;
 730         prev_dir = circuit_wpline[0].dir;
 731         for (idx = 1; idx < len; idx ++) {
 732         retry:
 733                 linenum = circuit_wpline[idx].line_num;
 734                 dir = circuit_wpline[idx].dir;
 735
 736                 /* XXX basic opponent management */
 737                 DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
 738                       __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
 739                 err = line2line(prev_linenum, prev_dir, linenum, dir,
 740                                 TRAJ_FLAGS_NO_NEAR);
 741                 if (!TRAJ_SUCCESS(err)) {
 742                         strat_hardstop();
 743                         time_wait_ms(2000);
 744                         goto retry;
 745                 }
 746
 747                 prev_linenum = linenum;
 748                 prev_dir = dir;
 749         }
 750
 751         return END_TRAJ; // XXX
 752 }
 753
 754 void test_strat_avoid(void)
 755 {
 756 #ifdef TEST_STRAT_AVOID
 757         uint8_t i, j;
 758         const struct circuit *selected_circuit;
 759         int8_t selected_face;
 760         struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
 761         int8_t ret;
 762
 763         i = 1; j = 1;
 764         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 765
 766         ts = 0; bc = 0; cc = 0;
 767         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 768         find_best_circuit(i, j, &selected_circuit, &selected_face);
 769         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 770         dump_circuit_wp(circuit_wpline, ret);
 771
 772         ts = 0; bc = 3; cc = 0;
 773         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 774         find_best_circuit(i, j, &selected_circuit, &selected_face);
 775         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 776         dump_circuit_wp(circuit_wpline, ret);
 777
 778         ts = 0; bc = 4; cc = 0;
 779         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 780         find_best_circuit(i, j, &selected_circuit, &selected_face);
 781         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 782         dump_circuit_wp(circuit_wpline, ret);
 783
 784         ts = 0; bc = 3; cc = 5;
 785         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 786         find_best_circuit(i, j, &selected_circuit, &selected_face);
 787         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 788         dump_circuit_wp(circuit_wpline, ret);
 789
 790         ts = 0; bc = 4; cc = 5;
 791         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 792         find_best_circuit(i, j, &selected_circuit, &selected_face);
 793         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 794         dump_circuit_wp(circuit_wpline, ret);
 795
 796         ts = 80; bc = 0; cc = 0;
 797         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 798         find_best_circuit(i, j, &selected_circuit, &selected_face);
 799         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 800         dump_circuit_wp(circuit_wpline, ret);
 801
 802         i = 4; j = 3;
 803         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 804
 805         ts = 0; bc = 0; cc = 0;
 806         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 807         find_best_circuit(i, j, &selected_circuit, &selected_face);
 808         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 809         dump_circuit_wp(circuit_wpline, ret);
 810
 811         ts = 0; bc = 3; cc = 0;
 812         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 813         find_best_circuit(i, j, &selected_circuit, &selected_face);
 814         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 815         dump_circuit_wp(circuit_wpline, ret);
 816
 817         ts = 80; bc = 0; cc = 0;
 818         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 819         find_best_circuit(i, j, &selected_circuit, &selected_face);
 820         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 821         dump_circuit_wp(circuit_wpline, ret);
 822
 823         i = 11; j = 6;
 824         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 825
 826         ts = 0; bc = 0; cc = 0;
 827         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 828         find_best_circuit(i, j, &selected_circuit, &selected_face);
 829         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 830         dump_circuit_wp(circuit_wpline, ret);
 831
 832         ts = 0; bc = 3; cc = 0;
 833         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 834         find_best_circuit(i, j, &selected_circuit, &selected_face);
 835         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 836         dump_circuit_wp(circuit_wpline, ret);
 837
 838         ts = 80; bc = 0; cc = 0;
 839         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 840         find_best_circuit(i, j, &selected_circuit, &selected_face);
 841         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 842         dump_circuit_wp(circuit_wpline, ret);
 843 #endif
 844 }