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 = 3, },
  92         { .i = 4, .j = 3, },
  93         { .i = 3, .j = 2, },
  94         { .i = 2, .j = 2, },
  95         { .i = 1, .j = 1, },
  96         { .i = 1, .j = 2, },
  97         { .i = 1, .j = 3, },
  98         { .i = 1, .j = 4, },
  99         { .i = 1, .j = 5, },
 100         { .i = 1, .j = 6, },
 101         { .i = 2, .j = 6, },
 102         { .i = 3, .j = 5, },
 103         { .i = 4, .j = 5, },
 104         { .i = 5, .j = 4, },
 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_circuit = {
 119         .name = "butterfly",
 120         .len = sizeof(butterfly_tab)/sizeof(struct wp_coord),
 121         .path = butterfly_tab,
 122 };
 123
 124 const struct wp_coord losange_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 losange_circuit = {
 137         .name = "losange",
 138         .len = sizeof(losange_tab)/sizeof(struct wp_coord),
 139         .path = losange_tab,
 140 };
 141
 142 const struct wp_coord triangle_tab[] = {
 143         { .i = 11, .j = 6, },
 144         { .i = 10, .j = 6, },
 145         { .i = 9, .j = 5, },
 146         { .i = 8, .j = 5, },
 147         { .i = 7, .j = 4, },
 148         { .i = 6, .j = 4, },
 149         { .i = 7, .j = 3, },
 150         { .i = 8, .j = 3, },
 151         { .i = 9, .j = 2, },
 152         { .i = 10, .j = 2, },
 153         { .i = 11, .j = 1, },
 154         { .i = 11, .j = 2, },
 155         { .i = 11, .j = 3, },
 156         { .i = 11, .j = 4, },
 157         { .i = 11, .j = 5, },
 158         { .i = 11, .j = 6, },
 159 };
 160
 161 const struct circuit triangle_circuit = {
 162         .name = "triangle",
 163         .len = sizeof(triangle_tab)/sizeof(struct wp_coord),
 164         .path = triangle_tab,
 165 };
 166
 167 const struct wp_coord answer_d_tab[] = {
 168         { .i = 11, .j = 6, },
 169         { .i = 11, .j = 5, },
 170         { .i = 11, .j = 4, },
 171         { .i = 11, .j = 3, },
 172         { .i = 11, .j = 2, },
 173         { .i = 11, .j = 1, },
 174         { .i = 10, .j = 2, },
 175         { .i = 9, .j = 2, },
 176         { .i = 8, .j = 3, },
 177         { .i = 9, .j = 3, },
 178         { .i = 10, .j = 4, },
 179         { .i = 11, .j = 4, },
 180         { .i = 11, .j = 5, },
 181         { .i = 11, .j = 6, },
 182 };
 183
 184 const struct circuit answer_d_circuit = {
 185         .name = "answer_d",
 186         .len = sizeof(answer_d_tab)/sizeof(struct wp_coord),
 187         .path = answer_d_tab,
 188 };
 189
 190 const struct wp_coord h_lambda_tab[] = {
 191         { .i = 11, .j = 6, },
 192         { .i = 10, .j = 6, },
 193         { .i = 9, .j = 5, },
 194         { .i = 8, .j = 5, },
 195         { .i = 7, .j = 4, },
 196         { .i = 6, .j = 4, },
 197         { .i = 5, .j = 3, },
 198         { .i = 5, .j = 4, },
 199         { .i = 5, .j = 5, },
 200         { .i = 5, .j = 6, },
 201         { .i = 6, .j = 6, },
 202         { .i = 7, .j = 5, },
 203         { .i = 8, .j = 5, },
 204         { .i = 9, .j = 5, },
 205         { .i = 10, .j = 6, },
 206         { .i = 11, .j = 6, },
 207 };
 208
 209 const struct circuit h_lambda_circuit = {
 210         .name = "h_lambda",
 211         .len = sizeof(h_lambda_tab)/sizeof(struct wp_coord),
 212         .path = h_lambda_tab,
 213 };
 214
 215 const struct wp_coord asym_butterfly_tab[] = {
 216         { .i = 11, .j = 6, },
 217         { .i = 10, .j = 6, },
 218         { .i = 9, .j = 5, },
 219         { .i = 8, .j = 5, },
 220         { .i = 7, .j = 4, },
 221         { .i = 6, .j = 4, },
 222         { .i = 5, .j = 3, },
 223         { .i = 4, .j = 3, },
 224         { .i = 3, .j = 2, },
 225         { .i = 3, .j = 3, },
 226         { .i = 3, .j = 4, },
 227         { .i = 3, .j = 5, },
 228         { .i = 4, .j = 5, },
 229         { .i = 5, .j = 4, },
 230         { .i = 6, .j = 4, },
 231         { .i = 7, .j = 3, },
 232         { .i = 8, .j = 3, },
 233         { .i = 9, .j = 2, },
 234         { .i = 10, .j = 2, },
 235         { .i = 11, .j = 1, },
 236         { .i = 11, .j = 2, },
 237         { .i = 11, .j = 3, },
 238         { .i = 11, .j = 4, },
 239         { .i = 11, .j = 5, },
 240         { .i = 11, .j = 6, },
 241 };
 242
 243 const struct circuit asym_butterfly_circuit = {
 244         .name = "asym_butterfly",
 245         .len = sizeof(asym_butterfly_tab)/sizeof(struct wp_coord),
 246         .path = asym_butterfly_tab,
 247 };
 248
 249 const struct wp_coord big_h_lambda_tab[] = {
 250         { .i = 11, .j = 6, },
 251         { .i = 10, .j = 6, },
 252         { .i = 9, .j = 5, },
 253         { .i = 8, .j = 5, },
 254         { .i = 7, .j = 4, },
 255         { .i = 6, .j = 4, },
 256         { .i = 5, .j = 4, },
 257         { .i = 4, .j = 5, },
 258         { .i = 3, .j = 5, },
 259         { .i = 2, .j = 6, },
 260         { .i = 1, .j = 6, },
 261         { .i = 1, .j = 5, },
 262         { .i = 1, .j = 4, },
 263         { .i = 1, .j = 3, },
 264         { .i = 1, .j = 2, },
 265         { .i = 1, .j = 1, },
 266         { .i = 2, .j = 2, },
 267         { .i = 3, .j = 2, },
 268         { .i = 4, .j = 3, },
 269         { .i = 5, .j = 3, },
 270         { .i = 6, .j = 4, },
 271         { .i = 7, .j = 4, },
 272         { .i = 8, .j = 5, },
 273         { .i = 9, .j = 5, },
 274         { .i = 10, .j = 6, },
 275         { .i = 11, .j = 6, },
 276 };
 277
 278 const struct circuit big_h_lambda_circuit = {
 279         .name = "big_h_lambda",
 280         .len = sizeof(big_h_lambda_tab)/sizeof(struct wp_coord),
 281         .path = big_h_lambda_tab,
 282 };
 283
 284 const struct wp_coord letter_v_tab[] = {
 285         { .i = 11, .j = 6, },
 286         { .i = 10, .j = 6, },
 287         { .i = 9, .j = 5, },
 288         { .i = 8, .j = 5, },
 289         { .i = 7, .j = 4, },
 290         { .i = 6, .j = 4, },
 291         { .i = 5, .j = 4, },
 292         { .i = 4, .j = 5, },
 293         { .i = 3, .j = 5, },
 294         { .i = 2, .j = 6, },
 295         { .i = 1, .j = 6, },
 296         { .i = 1, .j = 5, },
 297         { .i = 1, .j = 4, },
 298         { .i = 2, .j = 4, },
 299         { .i = 3, .j = 3, },
 300         { .i = 4, .j = 3, },
 301         { .i = 5, .j = 2, },
 302         { .i = 6, .j = 2, },
 303         { .i = 7, .j = 2, },
 304         { .i = 8, .j = 3, },
 305         { .i = 9, .j = 3, },
 306         { .i = 10, .j = 4, },
 307         { .i = 11, .j = 4, },
 308         { .i = 11, .j = 5, },
 309         { .i = 11, .j = 6, },
 310 };
 311
 312 const struct circuit letter_v_circuit = {
 313         .name = "letter_v",
 314         .len = sizeof(letter_v_tab)/sizeof(struct wp_coord),
 315         .path = letter_v_tab,
 316 };
 317
 318 const struct wp_coord sperma_tab[] = {
 319         { .i = 11, .j = 6, },
 320         { .i = 10, .j = 6, },
 321         { .i = 9, .j = 5, },
 322         { .i = 8, .j = 5, },
 323         { .i = 7, .j = 5, },
 324         { .i = 6, .j = 6, },
 325         { .i = 5, .j = 5, },
 326         { .i = 4, .j = 5, },
 327         { .i = 3, .j = 4, },
 328         { .i = 2, .j = 4, },
 329         { .i = 1, .j = 3, },
 330         { .i = 1, .j = 4, },
 331         { .i = 1, .j = 5, },
 332         { .i = 1, .j = 6, },
 333         { .i = 2, .j = 6, },
 334         { .i = 3, .j = 5, },
 335         { .i = 4, .j = 5, },
 336         { .i = 5, .j = 5, },
 337         { .i = 6, .j = 6, },
 338         { .i = 7, .j = 5, },
 339         { .i = 8, .j = 5, },
 340         { .i = 9, .j = 5, },
 341         { .i = 10, .j = 6, },
 342         { .i = 11, .j = 6, },
 343 };
 344
 345 const struct circuit sperma_circuit = {
 346         .name = "sperma",
 347         .len = sizeof(sperma_tab)/sizeof(struct wp_coord),
 348         .path = sperma_tab,
 349 };
 350
 351 /* list of all possible circuits */
 352 const struct circuit *circuits[] = {
 353         &butterfly_circuit,
 354         &losange_circuit,
 355         &triangle_circuit,
 356         &answer_d_circuit,
 357         &h_lambda_circuit,
 358         &asym_butterfly_circuit,
 359         &big_h_lambda_circuit,
 360         &letter_v_circuit,
 361         &sperma_circuit,
 362         NULL,
 363 };
 364
 365 /* symetric neighbor position */
 366 static inline uint8_t opposite_position(uint8_t pos)
 367 {
 368         pos += 3;
 369         if (pos > LINE_L_UP)
 370                 pos -= 6;
 371         return pos;
 372 }
 373
 374 #ifdef HOST_VERSION
 375 //#define TEST_STRAT_AVOID
 376 #endif
 377
 378 #ifdef TEST_STRAT_AVOID
 379 static uint8_t cc;
 380 uint8_t xget_cob_count(void)
 381 {
 382         return cc;
 383 }
 384
 385 static uint8_t bc;
 386 uint8_t xget_ball_count(void)
 387 {
 388         return bc;
 389 }
 390
 391 static uint32_t ts;
 392 uint8_t xtime_get_s(void)
 393 {
 394         return ts;
 395 }
 396 #else
 397 #define xget_cob_count() get_cob_count()
 398 #define xget_ball_count() get_ball_count()
 399 #define xtime_get_s() time_get_s()
 400 #endif
 401
 402 /* return true if turn is at 60 deg */
 403 uint8_t is_60deg(uint8_t dir1, uint8_t dir2)
 404 {
 405         int8_t turn;
 406
 407         turn = dir2-dir1;
 408         if (turn < 0)
 409                 turn += 6;
 410         if (turn == 1)
 411                 return 1;
 412         if (turn == 5)
 413                 return 1;
 414         return 0;
 415 }
 416
 417 /* return true if turn is at 60 deg */
 418 uint8_t is_120deg(uint8_t dir1, uint8_t dir2)
 419 {
 420         int8_t turn;
 421
 422         turn = dir2-dir1;
 423         if (turn < 0)
 424                 turn += 6;
 425         if (turn == 2)
 426                 return 1;
 427         if (turn == 4)
 428                 return 1;
 429         return 0;
 430 }
 431
 432 /* get the neighbour of the point at specified dir, return -1 if
 433  * there is no neighbor */
 434 int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj,
 435                  uint8_t dir)
 436 {
 437         switch (dir) {
 438         case LINE_UP:
 439                 j++;
 440                 break;
 441         case LINE_R_UP:
 442                 if ((i & 1)) j++;
 443                 i++;
 444                 break;
 445         case LINE_R_DOWN:
 446                 if (!(i & 1)) j--;
 447                 i++;
 448                 break;
 449         case LINE_DOWN:
 450                 j--;
 451                 break;
 452         case LINE_L_DOWN:
 453                 if (!(i & 1)) j--;
 454                 i--;
 455                 break;
 456         case LINE_L_UP:
 457                 if ((i & 1)) j++;
 458                 i--;
 459                 break;
 460         default:
 461                 return -1;
 462         }
 463         if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
 464                 return -1;
 465
 466         *ni = i;
 467         *nj = j;
 468         return 0;
 469 }
 470
 471 static int8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
 472 {
 473         uint8_t mod;
 474
 475         switch (dir) {
 476         case LINE_UP:
 477         case LINE_DOWN:
 478                 if ((i & 1) == 0)
 479                         return -1;
 480                 return i/2;
 481         case LINE_R_UP:
 482         case LINE_L_DOWN:
 483                 mod = i & 3;
 484                 if ((mod == 0 || mod == 1) && ((j & 1) == 0))
 485                         return -1;
 486                 if ((mod == 2 || mod == 3) && ((j & 1) == 1))
 487                         return -1;
 488                 i &= 0xfe;
 489                 j -= i/2;
 490                 return (5-j)/2;
 491         case LINE_R_DOWN:
 492         case LINE_L_UP:
 493                 mod = i & 3;
 494                 if ((mod == 0 || mod == 3) && ((j & 1) == 0))
 495                         return -1;
 496                 if ((mod == 1 || mod == 2) && ((j & 1) == 1))
 497                         return -1;
 498                 i &= 0xfe;
 499                 j += i/2;
 500                 return (11-j)/2;
 501         default:
 502                 return -1;
 503         }
 504 }
 505
 506 static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j,
 507                        uint8_t i, uint8_t j)
 508 {
 509         int8_t diff_i, diff_j;
 510
 511         diff_i = i - prev_i;
 512         diff_j = j - prev_j;
 513
 514         if (diff_i == 0 && diff_j == 1)
 515                 return LINE_UP;
 516         if (diff_i == 0 && diff_j == -1)
 517                 return LINE_DOWN;
 518
 519         if ((prev_i & 1) == 0) {
 520                 if (diff_i == 1 && diff_j == 0)
 521                         return LINE_R_UP;
 522                 if (diff_i == 1 && diff_j == -1)
 523                         return LINE_R_DOWN;
 524                 if (diff_i == -1 && diff_j == 0)
 525                         return LINE_L_UP;
 526                 if (diff_i == -1 && diff_j == -1)
 527                         return LINE_L_DOWN;
 528         }
 529         else {
 530                 if (diff_i == 1 && diff_j == 1)
 531                         return LINE_R_UP;
 532                 if (diff_i == 1 && diff_j == 0)
 533                         return LINE_R_DOWN;
 534                 if (diff_i == -1 && diff_j == 1)
 535                         return LINE_L_UP;
 536                 if (diff_i == -1 && diff_j == 0)
 537                         return LINE_L_DOWN;
 538         }
 539
 540         /* invalid value */
 541         return 0xFF;
 542 }
 543
 544 /* return approximative angle of line */
 545 int16_t linedir2angle(uint8_t dir)
 546 {
 547         switch (dir) {
 548         case LINE_UP:
 549                 return COLOR_A(90);
 550         case LINE_DOWN:
 551                 return COLOR_A(-90);
 552         case LINE_R_UP:
 553                 return COLOR_A(30);
 554         case LINE_R_DOWN:
 555                 return COLOR_A(-30);
 556         case LINE_L_UP:
 557                 return COLOR_A(150);
 558         case LINE_L_DOWN:
 559                 return COLOR_A(-150);
 560         default:
 561                 return 0;
 562         }
 563 }
 564
 565 int16_t get_nearest_dir_angle(int16_t a)
 566 {
 567         uint8_t dir, min_dir = 0;
 568         int16_t min_diff = 0x7FFF, diff;
 569
 570         for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
 571                 diff = abs(linedir2angle(dir) - a);
 572                 if (diff > 360)
 573                         diff -= 360;
 574                 if (diff > 360)
 575                         diff -= 360;
 576                 if (diff < min_diff) {
 577                         min_diff = diff;
 578                         min_dir = dir;
 579                 }
 580         }
 581         return linedir2angle(min_dir);
 582 }
 583
 584 /* return true if a waypoint belongs to a line */
 585 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
 586 {
 587         int8_t ln;
 588         ln = get_line_num(i, j, dir);
 589         if (ln == -1)
 590                 return 0;
 591         if (ln == linenum)
 592                 return 1;
 593         return 0;
 594 }
 595
 596 /* count the number of non-black corns which are neighbors of
 597  * specified cob */
 598 uint8_t corn_count_neigh(uint8_t i, uint8_t j)
 599 {
 600         uint8_t dir, n = 0;
 601         uint8_t ni, nj;
 602
 603         for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
 604                 if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
 605                         continue;
 606
 607                 /* is there a corn cob removed for more than 2 secs ? */
 608                 if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
 609                     strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK &&
 610                     (strat_db.wp_table[ni][nj].present ||
 611                      strat_db.wp_table[ni][nj].time_removed + 2 > time_get_s()))
 612                         n ++;
 613         }
 614
 615         return n;
 616 }
 617
 618
 619 /* fill circuit_wpline table with waypoints from circuit starting at
 620  * i,j and using selected face */
 621 static int8_t get_path(const struct circuit *circuit,
 622                        uint8_t starti, uint8_t startj, uint8_t faceA,
 623                        struct wp_line *circuit_wpline)
 624 {
 625         const struct wp_coord *curcircuit;
 626         const struct wp_coord *start;
 627         const struct wp_coord *end;
 628         uint8_t prev_i, prev_j;
 629         uint8_t dir, prev_dir = 0xFF;
 630         uint8_t found = 0, i = 0, j = 0;
 631         uint8_t linenum;
 632         int8_t step = faceA ? 1 : -1;
 633         int8_t path_len = 0;
 634
 635         /* start and end of circuit */
 636         if (faceA) {
 637                 start = &circuit->path[0];
 638                 end = start + circuit->len - 1;
 639         }
 640         else {
 641                 end = &circuit->path[0];
 642                 start = end + circuit->len - 1;
 643         }
 644
 645         DPR("%s(): %s face=%d\r\n", __FUNCTION__, circuit->name, faceA);
 646
 647         /* check that the point is present in the circuit */
 648         for (curcircuit = start; curcircuit != end; curcircuit += step) {
 649                 if (curcircuit->i == starti && curcircuit->j == startj) {
 650                         found = 1;
 651                         break;
 652                 }
 653         }
 654         if (found == 0)
 655                 return -1;
 656
 657
 658         /* browse the circuit from starti, startj in the specified
 659          * direction, and fill the table when direction changes */
 660         prev_i = starti;
 661         prev_j = startj;
 662         for ( ; curcircuit != end;
 663               curcircuit += step, prev_dir = dir, prev_i = i, prev_j = j) {
 664
 665                 i = curcircuit->i;
 666                 j = curcircuit->j;
 667
 668                 dir = get_dir(prev_i, prev_j, i, j);
 669
 670                 if (prev_dir != dir) {
 671                         linenum = get_line_num(prev_i, prev_j, dir);
 672                         circuit_wpline[path_len].line_num = linenum;
 673                         circuit_wpline[path_len].dir = dir;
 674                         DPR("%s(): %d %d -> %d %d / len=%d num=%d dir=%d\r\n",
 675                             __FUNCTION__, prev_i, prev_j, i, j, path_len, linenum, dir);
 676                         path_len++;
 677                 }
 678         }
 679
 680         return path_len;
 681 }
 682
 683 /* process score from retrieved objects number, and circuit len */
 684 static int16_t get_score(uint32_t wcorn_retrieved,
 685                          uint32_t ucorn_retrieved,
 686                          uint16_t tomato_retrieved,
 687                          uint16_t utomato_retrieved,
 688                          uint8_t len, uint8_t opp_on_path)
 689 {
 690         int16_t score = 0;
 691         uint8_t i;
 692         uint32_t mask = 1;
 693         uint8_t n;
 694
 695         /* score with corn */
 696         n = xget_cob_count() * 2;
 697         for (i = 0; i<CORN_NB; i++) {
 698                 if (n >= 10)
 699                         break;
 700                 if (wcorn_retrieved & mask) {
 701                         score += 250;
 702                         n += 2;
 703                 }
 704                 if (n >= 10)
 705                         break;
 706                 if (ucorn_retrieved & mask) {
 707                         score += 125;
 708                         n += 1;
 709                 }
 710                 mask <<= 1UL;
 711         }
 712
 713         DPR("get score: cob %d (->%d)\r\n", n, n/2);
 714
 715         /* score with tomato */
 716         n = xget_ball_count();
 717         mask = 1;
 718         for (i = 0; i<TOMATO_NB; i++) {
 719                 if (n >= 4)
 720                         break;
 721                 if (tomato_retrieved & mask) {
 722                         score += 150;
 723                         n += 1;
 724                 }
 725                 mask <<= 1UL;
 726         }
 727
 728         DPR("get score: ball %d\r\n", n);
 729
 730         /* malus for long circuits */
 731         score -= (len * 20);
 732         DPR("malus for length: %d\r\n", len * 20);
 733
 734         /* double malus for long circuits if we don't have much
 735          * time */
 736 #define WP_SPEED 1
 737         if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) {
 738                 int32_t extra;
 739                 extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s());
 740                 extra = (200 * extra);
 741                 if (extra < 0) /* should not happen */
 742                         extra = 0;
 743                 if (extra > 10000)
 744                         extra = 10000;
 745                 score -= extra;
 746                 DPR("malus for length + time: %d\r\n", extra);
 747         }
 748
 749         /* malus if there is opponent on the path */
 750         if (opp_on_path) {
 751                 DPR("malus for opponent: %d\r\n", (500 * opp_on_path));
 752                 score -= (500 * opp_on_path);
 753         }
 754
 755         return score;
 756 }
 757
 758 /* return the corn type of specified coords: I2C_COB_WHITE,
 759  * I2C_COB_UNKNOWN, or I2C_COB_NONE if it is black or not present */
 760 static uint8_t get_corn_type(uint8_t i, uint8_t j)
 761 {
 762         uint8_t color;
 763         /* is there a corn cob ? */
 764         if (strat_db.wp_table[i][j].type == WP_TYPE_CORN &&
 765             strat_db.wp_table[i][j].present) {
 766                 color = strat_db.wp_table[i][j].corn.color;
 767                 if (color == I2C_COB_WHITE)
 768                         return I2C_COB_WHITE;
 769                 else if (color == I2C_COB_UNKNOWN)
 770                         return I2C_COB_UNKNOWN;
 771         }
 772         return I2C_COB_NONE;
 773 }
 774
 775 /* i,j: starting position */
 776 static int8_t evaluate_one_face(const struct circuit *circuit,
 777                                 uint8_t starti, uint8_t startj,
 778                                 uint8_t faceA, int16_t *score)
 779 {
 780         const struct wp_coord *curcircuit;
 781         const struct wp_coord *start;
 782         const struct wp_coord *end;
 783         uint32_t wcorn_retrieved = 0; /* bit mask */
 784         uint32_t ucorn_retrieved = 0; /* bit mask */
 785         uint16_t tomato_retrieved = 0; /* bit mask */
 786         uint16_t utomato_retrieved = 0; /* bit mask */
 787         uint8_t opponent_on_path = 0;
 788         uint8_t len = 0, found = 0;
 789         uint8_t i, j, prev_i, prev_j;
 790         uint8_t ni = 0, nj = 0;
 791         uint8_t dir, color, idx, visited;
 792         int8_t step = faceA ? 1 : -1;
 793         int16_t x, y;
 794         int32_t d, prev_d = 0;
 795         int16_t oppx, oppy;
 796
 797         *score = 0x8000; /* -int_max */
 798
 799         /* start and end of circuit */
 800         if (faceA) {
 801                 start = &circuit->path[0];
 802                 end = start + circuit->len - 1;
 803         }
 804         else {
 805                 end = &circuit->path[0];
 806                 start = end + circuit->len - 1;
 807         }
 808
 809         DPR("%s() face: %s %d\r\n", __FUNCTION__, circuit->name, faceA);
 810
 811         /* check that the point is present in the circuit */
 812         for (curcircuit = start; curcircuit != end; curcircuit += step) {
 813                 if (curcircuit->i == starti && curcircuit->j == startj) {
 814                         found = 1;
 815                         break;
 816                 }
 817         }
 818         if (found == 0)
 819                 return -1;
 820
 821         /* get opponent coords */
 822         if (get_opponent_xy(&oppx, &oppy) < 0)
 823                 oppx = I2C_OPPONENT_NOT_THERE;
 824         else
 825                 DPR("%s() opponent: %d, %d\r\n", __FUNCTION__, oppx, oppy);
 826
 827         /* silent the compiler */
 828         prev_i = 0xff;
 829         prev_j = 0xff;
 830
 831         /* browse all points and calculate the score */
 832         for (; /* start at starti,startj */
 833              curcircuit != end;
 834              curcircuit += step, len ++, prev_i = i, prev_j = j) {
 835                 i = curcircuit->i;
 836                 j = curcircuit->j;
 837
 838                 /* is opponent near the point ? */
 839                 ijcoord_to_xycoord(i, j, &x, &y);
 840                 if (oppx != I2C_OPPONENT_NOT_THERE) {
 841                         d = quad_distance_between(oppx, oppy, x, y);
 842                         DPR("%s(): opp at %d mm (ij=%d,%d opp=%d,%d pos=%d,%d)\r\n",
 843                             __FUNCTION__, d, i, j, oppx, oppy, x, y);
 844                         if (d < (250L*250L) && d < prev_d)
 845                                 opponent_on_path += 3;
 846                         else if (d < (500L*500L) && d < prev_d)
 847                                 opponent_on_path ++;
 848                         prev_d = d;
 849                 }
 850
 851                 /* don't try to look cobs/tomato for first point */
 852                 if (curcircuit == start)
 853                         continue;
 854
 855                 /* get current direction, we wil check cobs behind us
 856                  * on left and right */
 857                 dir = get_dir(prev_i, prev_j, i, j);
 858
 859                 DPR("%d %d -> %d %d  (%d)\n", prev_i, prev_j, i, j, dir);
 860
 861                 /* is there a tomato ? */
 862                 if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
 863                     strat_db.wp_table[i][j].present) {
 864                         if (strat_db.wp_table[i][j].opp_visited) {
 865                                 DPR("  TOMATO (opp visited)\n");
 866                                 utomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
 867                         }
 868                         else {
 869                                 DPR("  TOMATO\n");
 870                                 tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
 871                         }
 872                 }
 873
 874                 /* behind left */
 875                 if (wp_get_neigh(i, j, &ni, &nj, (dir + 2) % 6) == 0) {
 876                         color = get_corn_type(ni, nj);
 877                         idx = strat_db.wp_table[ni][nj].corn.idx;
 878                         visited = strat_db.wp_table[ni][nj].opp_visited;
 879                         if (color == I2C_COB_WHITE && !visited) {
 880                                 DPR("  LEFT WCORN (%d)\n", idx);
 881                                 wcorn_retrieved |= (1UL << idx);
 882                         }
 883                         else if (color == I2C_COB_WHITE && visited) {
 884                                 DPR("  LEFT CORN visited (%d)\n", idx);
 885                                 ucorn_retrieved |= (1UL << idx);
 886                         }
 887                         else if (color == I2C_COB_UNKNOWN) {
 888                                 DPR("  LEFT UCORN (%d)\n", idx);
 889                                 ucorn_retrieved |= (1UL << idx);
 890                         }
 891                 }
 892
 893                 /* behind right */
 894                 if (wp_get_neigh(i, j, &ni, &nj, (dir + 4) % 6) == 0) {
 895                         color = get_corn_type(ni, nj);
 896                         idx = strat_db.wp_table[ni][nj].corn.idx;
 897                         visited = strat_db.wp_table[ni][nj].opp_visited;
 898                         if (color == I2C_COB_WHITE && !visited) {
 899                                 DPR("  RIGHT WCORN (%d)\n", idx);
 900                                 wcorn_retrieved |= (1UL << idx);
 901                         }
 902                         else if (color == I2C_COB_WHITE && visited) {
 903                                 DPR("  RIGHT CORN visited (%d)\n", idx);
 904                                 ucorn_retrieved |= (1UL << idx);
 905                         }
 906                         else if (color == I2C_COB_UNKNOWN) {
 907                                 DPR("  RIGHT UCORN (%d)\n", idx);
 908                                 ucorn_retrieved |= (1UL << idx);
 909                         }
 910                 }
 911
 912                 /* prev_i, prev_j, len and curcircuit are updated in
 913                  * for (;;) */
 914         }
 915
 916         /* write score and exit */
 917         *score = get_score(wcorn_retrieved, ucorn_retrieved,
 918                            tomato_retrieved, utomato_retrieved,
 919                            len, opponent_on_path);
 920         return 0;
 921 }
 922
 923 /* i,j: starting position */
 924 static int8_t evaluate_one_circuit(const struct circuit *circuit,
 925                                    uint8_t starti, uint8_t startj,
 926                                    int16_t *scoreA, int16_t *scoreB)
 927 {
 928         if (evaluate_one_face(circuit, starti, startj, 1, scoreA) < 0)
 929                 return -1;
 930
 931         /* we are on eject point, scoreB is the same */
 932         if (starti == 11 && startj == 6) {
 933                 *scoreB = *scoreA;
 934                 return 0;
 935         }
 936
 937         if (evaluate_one_face(circuit, starti, startj, 0, scoreB) < 0)
 938                 return -1;
 939         return 0;
 940 }
 941
 942 /* i,j starting position */
 943 static int8_t find_best_circuit(uint8_t i, uint8_t j,
 944                                 const struct circuit **selected_circuit,
 945                                 int8_t *selected_face)
 946 {
 947         const struct circuit **circuit;
 948         int16_t scoreA, scoreB;
 949         int16_t selected_score = 0x8000; /* ~-int_max */
 950         int8_t found = -1;
 951
 952         *selected_face = 0;
 953         *selected_circuit = circuits[0] ;
 954         for (circuit = &circuits[0]; *circuit; circuit++) {
 955                 if (evaluate_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
 956                         continue;
 957                 found = 0;
 958                 DEBUG(E_USER_STRAT, "Scores for %s are: faceA=%d, faceB=%d",
 959                       (*circuit)->name, scoreA, scoreB);
 960                 if (scoreA > selected_score) {
 961                         *selected_circuit = *circuit;
 962                         selected_score = scoreA;
 963                         *selected_face = 1;
 964                 }
 965                 if (scoreB > selected_score) {
 966                         *selected_circuit = *circuit;
 967                         selected_score = scoreB;
 968                         *selected_face = 0;
 969                 }
 970         }
 971
 972         if (found == -1)
 973                 DEBUG(E_USER_STRAT, "no circuit found");
 974         else
 975                 DEBUG(E_USER_STRAT, "circuit found: %s, %s",
 976                       (*selected_circuit)->name,
 977                       (*selected_face) ? "faceA":"faceB");
 978         return found;
 979 }
 980
 981 static void init_all_circuits(void)
 982 {
 983         const struct circuit **circuit;
 984         const struct wp_coord *cur;
 985         const struct wp_coord *start;
 986         const struct wp_coord *end;
 987         uint8_t prev_i, prev_j, i, j, dir;
 988
 989         for (circuit = &circuits[0]; *circuit; circuit++) {
 990                 start = &(*circuit)->path[0];
 991                 end = start + (*circuit)->len - 1;
 992
 993                 prev_i = start->i;
 994                 prev_j = start->j;
 995                 start ++;
 996
 997                 for (cur = start; cur != end;
 998                      cur ++, prev_i = i, prev_j = j) {
 999
1000                         i = cur->i;
1001                         j = cur->j;
1002
1003                         strat_db.wp_table[i][j].on_circuit = 1;
1004
1005                         dir = get_dir(prev_i, prev_j, i, j);
1006                         if (dir == 0xFF)
1007                                 printf_P("Bad circuit %s %d %d\r\n", (*circuit)->name, i, j);
1008                 }
1009         }
1010 }
1011
1012
1013 static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
1014 {
1015         int8_t i;
1016         if (len <= 0)
1017                 return;
1018         for (i = 0; i < len; i ++) {
1019                 DEBUG(E_USER_STRAT, "linenum %d dir %d",
1020                       circuit_wpline[i].line_num,
1021                        circuit_wpline[i].dir);
1022         }
1023
1024 }
1025
1026 /* choose a circuit, then harvest on this circuit */
1027 uint8_t strat_harvest_circuit(void)
1028 {
1029         const struct circuit *selected_circuit;
1030         int8_t selected_face;
1031         struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
1032         int8_t len;
1033         uint8_t i, j, idx;
1034         int16_t x, y;
1035         uint8_t linenum, prev_linenum;
1036         uint8_t dir, prev_dir;
1037         uint8_t err;
1038
1039         strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
1040         strat_want_pack = 1;
1041
1042         x = position_get_x_s16(&mainboard.pos);
1043         y = position_get_y_s16(&mainboard.pos);
1044
1045         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) {
1046                 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
1047                       __FUNCTION__, x, y);
1048                 err = END_ERROR;
1049                 goto fail;
1050         }
1051
1052         if (find_best_circuit(i, j, &selected_circuit, &selected_face) < 0) {
1053                 DEBUG(E_USER_STRAT, "%s(): cannot find a good circuit",
1054                       __FUNCTION__);
1055                 err = END_ERROR;
1056                 goto fail;
1057         }
1058
1059         len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1060         if (len < 0) {
1061                 DEBUG(E_USER_STRAT, "%s(): cannot find a path",
1062                       __FUNCTION__);
1063                 err = END_ERROR;
1064                 goto fail;
1065         }
1066
1067         dump_circuit_wp(circuit_wpline, len);
1068
1069         prev_linenum = circuit_wpline[0].line_num;
1070         prev_dir = circuit_wpline[0].dir;
1071
1072         /* fix orientation first */
1073         trajectory_a_abs(&mainboard.traj, linedir2angle(prev_dir));
1074         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1075         if (!TRAJ_SUCCESS(err))
1076                 goto fail;
1077
1078         strat_want_pack = 0;
1079
1080         /* do all lines of circuit */
1081         for (idx = 1; idx < len; idx ++) {
1082         retry:
1083                 linenum = circuit_wpline[idx].line_num;
1084                 dir = circuit_wpline[idx].dir;
1085
1086                 DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
1087                       __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
1088                 err = line2line(prev_linenum, prev_dir, linenum, dir,
1089                                 TRAJ_FLAGS_NO_NEAR);
1090
1091                 /* in some cases it is better to wait that obstacle is
1092                  * gone before starting to avoid it */
1093                 if (err == END_OBSTACLE &&
1094                     strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
1095                     time_get_s() > strat_conf.prev_wait_obstacle + 5) {
1096                         strat_conf.prev_wait_obstacle = time_get_s();
1097                         time_wait_ms(2000);
1098                         goto retry;
1099                 }
1100                 if (!TRAJ_SUCCESS(err))
1101                         goto fail;
1102
1103                 prev_linenum = linenum;
1104                 prev_dir = dir;
1105         }
1106         err = END_TRAJ;
1107
1108  fail:
1109         strat_want_pack = 0;
1110         return err;
1111 }
1112
1113 /* list of waypoints when we are not on a circuit */
1114 const struct xy_point unblock_pts[] = {
1115         { .x = 375, .y = 597 },  /* 1,1 */
1116         { .x = 2625, .y = 597 }, /* 11,1 */
1117         { .x = 1500, .y = 722 }, /* 6,2 */
1118         { .x = 375, .y = 1097 }, /* 1,3 */
1119         { .x = 375, .y = 1597 }, /* 1,5 */
1120         { .x = 2625, .y = 1097 }, /* 11,3 */
1121         { .x = 2625, .y = 1597 }, /* 11,5 */
1122         { .x = 1500, .y = 1722 }, /* 6,6 */
1123 };
1124
1125
1126 /* try to unblock in any situation */
1127 uint8_t strat_unblock(void)
1128 {
1129         int16_t x, y, posx, posy, posa;
1130         uint8_t i, j, k, cpt;
1131         uint16_t old_dspeed, old_aspeed;
1132         uint8_t err;
1133         uint16_t d_min = 0x7FFF, d;
1134         const struct xy_point *pt;
1135
1136         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
1137
1138         strat_want_pack = 1;
1139         strat_get_speed(&old_dspeed, &old_aspeed);
1140
1141         strat_hardstop();
1142         posa = position_get_a_deg_s16(&mainboard.pos);
1143
1144         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
1145         posx = position_get_x_s16(&mainboard.pos);
1146         posy = position_get_y_s16(&mainboard.pos);
1147         x = posx;
1148         y = posy;
1149
1150         if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
1151                 x = -1;
1152         else if (strat_db.wp_table[i][j].on_circuit == 0)
1153                 x = -1;
1154
1155         /* find the nearest unblock point */
1156         if (x == -1) {
1157
1158                 /* browse all points and find the nearest */
1159                 for (k = 0; k < sizeof(unblock_pts)/sizeof(*unblock_pts); k++) {
1160                         pt = &unblock_pts[k];
1161                         d = distance_between(posx, posy, pt->x, COLOR_Y(pt->y));
1162                         if (d < d_min) {
1163                                 d_min = d;
1164                                 x = pt->x;
1165                                 y = COLOR_Y(pt->y);
1166                         }
1167                 }
1168         }
1169         DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d",
1170               __FUNCTION__, x, y);
1171
1172         for (cpt = 0; cpt < 2; cpt++) {
1173
1174                 /* go to nearest waypoint */
1175                 trajectory_goto_xy_abs(&mainboard.traj, x, y);
1176                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1177                 if (err == END_TIMER)
1178                         return err;
1179
1180                 if (TRAJ_SUCCESS(err))
1181                         break;
1182
1183                 if (cpt == 1)
1184                         break;
1185
1186                 /* aie... do a S */
1187                 trajectory_d_a_rel(&mainboard.traj, 100, 20);
1188                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1189                 trajectory_d_a_rel(&mainboard.traj, 100, -20);
1190                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1191                 trajectory_d_a_rel(&mainboard.traj, -100, -20);
1192                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1193                 trajectory_d_a_rel(&mainboard.traj, -100, 20);
1194                 err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1195         }
1196
1197         trajectory_a_abs(&mainboard.traj, get_nearest_dir_angle(posa));
1198         err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
1199         if (err == END_TIMER)
1200                 return err;
1201
1202         if (!TRAJ_SUCCESS(err))
1203                 return err;
1204
1205         strat_set_speed(old_dspeed, old_aspeed);
1206         return END_TRAJ;
1207 }
1208
1209 void strat_avoid_init(void)
1210 {
1211         init_all_circuits();
1212
1213 #ifdef TEST_STRAT_AVOID
1214         uint8_t i, j;
1215         const struct circuit *selected_circuit;
1216         int8_t selected_face;
1217         struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
1218         int8_t ret;
1219
1220         i = 1; j = 1;
1221         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
1222
1223         ts = 0; bc = 0; cc = 0;
1224         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1225         find_best_circuit(i, j, &selected_circuit, &selected_face);
1226         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1227         dump_circuit_wp(circuit_wpline, ret);
1228
1229         ts = 0; bc = 3; cc = 0;
1230         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1231         find_best_circuit(i, j, &selected_circuit, &selected_face);
1232         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1233         dump_circuit_wp(circuit_wpline, ret);
1234
1235         ts = 0; bc = 4; cc = 0;
1236         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1237         find_best_circuit(i, j, &selected_circuit, &selected_face);
1238         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1239         dump_circuit_wp(circuit_wpline, ret);
1240
1241         ts = 0; bc = 3; cc = 5;
1242         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1243         find_best_circuit(i, j, &selected_circuit, &selected_face);
1244         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1245         dump_circuit_wp(circuit_wpline, ret);
1246
1247         ts = 0; bc = 4; cc = 5;
1248         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1249         find_best_circuit(i, j, &selected_circuit, &selected_face);
1250         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1251         dump_circuit_wp(circuit_wpline, ret);
1252
1253         ts = 80; bc = 0; cc = 0;
1254         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1255         find_best_circuit(i, j, &selected_circuit, &selected_face);
1256         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1257         dump_circuit_wp(circuit_wpline, ret);
1258
1259         i = 4; j = 3;
1260         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
1261
1262         ts = 0; bc = 0; cc = 0;
1263         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1264         find_best_circuit(i, j, &selected_circuit, &selected_face);
1265         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1266         dump_circuit_wp(circuit_wpline, ret);
1267
1268         ts = 0; bc = 3; cc = 0;
1269         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1270         find_best_circuit(i, j, &selected_circuit, &selected_face);
1271         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1272         dump_circuit_wp(circuit_wpline, ret);
1273
1274         ts = 80; bc = 0; cc = 0;
1275         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1276         find_best_circuit(i, j, &selected_circuit, &selected_face);
1277         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1278         dump_circuit_wp(circuit_wpline, ret);
1279
1280         i = 11; j = 6;
1281         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
1282
1283         ts = 0; bc = 0; cc = 0;
1284         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1285         find_best_circuit(i, j, &selected_circuit, &selected_face);
1286         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1287         dump_circuit_wp(circuit_wpline, ret);
1288
1289         ts = 0; bc = 3; cc = 0;
1290         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1291         find_best_circuit(i, j, &selected_circuit, &selected_face);
1292         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1293         dump_circuit_wp(circuit_wpline, ret);
1294
1295         ts = 80; bc = 0; cc = 0;
1296         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
1297         find_best_circuit(i, j, &selected_circuit, &selected_face);
1298         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
1299         dump_circuit_wp(circuit_wpline, ret);
1300 #endif
1301 }