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
  71 const struct wp_coord circuit1[] = {
  72         { .i = 11, .j = 6, .end = 0, },
  73         { .i = 10, .j = 6, .end = 0, },
  74         { .i = 9, .j = 5, .end = 0, },
  75         { .i = 8, .j = 5, .end = 0, },
  76         { .i = 7, .j = 4, .end = 0, },
  77         { .i = 6, .j = 4, .end = 0, },
  78         { .i = 5, .j = 4, .end = 0, },
  79         { .i = 4, .j = 5, .end = 0, },
  80         { .i = 3, .j = 5, .end = 0, },
  81         { .i = 2, .j = 6, .end = 0, },
  82         { .i = 1, .j = 6, .end = 0, },
  83         { .i = 1, .j = 5, .end = 0, },
  84         { .i = 1, .j = 4, .end = 0, },
  85         { .i = 1, .j = 3, .end = 0, },
  86         { .i = 1, .j = 2, .end = 0, },
  87         { .i = 1, .j = 1, .end = 0, },
  88         { .i = 2, .j = 2, .end = 0, },
  89         { .i = 3, .j = 2, .end = 0, },
  90         { .i = 4, .j = 3, .end = 0, },
  91         { .i = 5, .j = 3, .end = 0, },
  92         { .i = 6, .j = 4, .end = 0, },
  93         { .i = 7, .j = 3, .end = 0, },
  94         { .i = 8, .j = 3, .end = 0, },
  95         { .i = 9, .j = 2, .end = 0, },
  96         { .i = 10, .j = 2, .end = 0, },
  97         { .i = 11, .j = 1, .end = 0, },
  98         { .i = 11, .j = 2, .end = 0, },
  99         { .i = 11, .j = 3, .end = 0, },
 100         { .i = 11, .j = 4, .end = 0, },
 101         { .i = 11, .j = 5, .end = 0, },
 102         { .i = 11, .j = 6, .end = 1, },
 103 };
 104
 105 const struct wp_coord circuit2[] = {
 106         { .i = 11, .j = 6, .end = 0, },
 107         { .i = 10, .j = 6, .end = 0, },
 108         { .i = 9, .j = 5, .end = 0, },
 109         { .i = 9, .j = 4, .end = 0, },
 110         { .i = 9, .j = 3, .end = 0, },
 111         { .i = 10, .j = 4, .end = 0, },
 112         { .i = 11, .j = 4, .end = 0, },
 113         { .i = 11, .j = 5, .end = 0, },
 114         { .i = 11, .j = 6, .end = 1, },
 115 };
 116
 117 /* list of all possible circuits */
 118 const struct wp_coord *circuits[] = {
 119         circuit1,
 120         circuit2,
 121         NULL,
 122 };
 123
 124 /* symetric neighbor position */
 125 static inline uint8_t opposite_position(uint8_t pos)
 126 {
 127         pos += 3;
 128         if (pos > LINE_L_UP)
 129                 pos -= 6;
 130         return pos;
 131 }
 132
 133 #if 0
 134 static uint8_t cc;
 135 uint8_t xget_cob_count(void)
 136 {
 137         return cc;
 138 }
 139
 140 static uint8_t bc;
 141 uint8_t xget_ball_count(void)
 142 {
 143         return bc;
 144 }
 145
 146 static uint32_t ts;
 147 uint8_t xtime_get_s(void)
 148 {
 149         return ts;
 150 }
 151 #else
 152 #define xget_cob_count() get_cob_count()
 153 #define xget_ball_count() get_ball_count()
 154 #define xtime_get_s() time_get_s()
 155 #endif
 156
 157 /* get the neighbour of the point at specified dir, return -1 if
 158  * there is no neighbor */
 159 int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj,
 160                  uint8_t dir)
 161 {
 162         switch (dir) {
 163         case LINE_UP:
 164                 j++;
 165                 break;
 166         case LINE_R_UP:
 167                 if ((i & 1)) j++;
 168                 i++;
 169                 break;
 170         case LINE_R_DOWN:
 171                 if (!(i & 1)) j--;
 172                 i++;
 173                 break;
 174         case LINE_DOWN:
 175                 j--;
 176                 break;
 177         case LINE_L_DOWN:
 178                 if (!(i & 1)) j--;
 179                 i--;
 180                 break;
 181         case LINE_L_UP:
 182                 if ((i & 1)) j++;
 183                 i--;
 184                 break;
 185         default:
 186                 return -1;
 187         }
 188         if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
 189                 return -1;
 190
 191         *ni = i;
 192         *nj = j;
 193         return 0;
 194 }
 195
 196 static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
 197 {
 198         switch (dir) {
 199         case LINE_UP:
 200         case LINE_DOWN:
 201                 return i/2;
 202         case LINE_R_UP:
 203         case LINE_L_DOWN:
 204                 i &= 0xfe;
 205                 j -= i/2;
 206                 return (5-j)/2;
 207         case LINE_R_DOWN:
 208         case LINE_L_UP:
 209                 i &= 0xfe;
 210                 j += i/2;
 211                 return (11-j)/2;
 212         default:
 213                 return -1;
 214         }
 215 }
 216
 217 static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j,
 218                        uint8_t i, uint8_t j)
 219 {
 220         int8_t diff_i, diff_j;
 221
 222         diff_i = i - prev_i;
 223         diff_j = j - prev_j;
 224
 225         if (diff_i == 0 && diff_j == 1)
 226                 return LINE_UP;
 227         if (diff_i == 0 && diff_j == -1)
 228                 return LINE_DOWN;
 229
 230         if ((prev_i & 1) == 0) {
 231                 if (diff_i == 1 && diff_j == 0)
 232                         return LINE_R_UP;
 233                 if (diff_i == 1 && diff_j == -1)
 234                         return LINE_R_DOWN;
 235                 if (diff_i == -1 && diff_j == 0)
 236                         return LINE_L_UP;
 237                 if (diff_i == -1 && diff_j == -1)
 238                         return LINE_L_DOWN;
 239         }
 240         else {
 241                 if (diff_i == 1 && diff_j == 1)
 242                         return LINE_R_UP;
 243                 if (diff_i == 1 && diff_j == 0)
 244                         return LINE_R_DOWN;
 245                 if (diff_i == -1 && diff_j == 1)
 246                         return LINE_L_UP;
 247                 if (diff_i == -1 && diff_j == 0)
 248                         return LINE_L_DOWN;
 249         }
 250
 251         /* invalid value */
 252         return 0xFF;
 253 }
 254
 255 /* return true if a waypoint belongs to a line */
 256 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
 257 {
 258         uint8_t ln;
 259         ln = get_line_num(i, j, dir);
 260         if (ln == linenum)
 261                 return 1;
 262         return 0;
 263 }
 264
 265 /* count the number of non-black corns which are neighbors of
 266  * specified cob */
 267 uint8_t corn_count_neigh(uint8_t i, uint8_t j)
 268 {
 269         uint8_t dir, n = 0;
 270         uint8_t ni, nj;
 271
 272         for (dir = LINE_UP; dir <= LINE_R_DOWN; dir++) {
 273                 if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
 274                         continue;
 275
 276                 //printf("i,j=%d,%d dir=%d, ni,nj=%d,%d\r\n",
 277                 //       i, j, dir, ni, nj);
 278
 279                 /* is there a corn cob ? */
 280                 if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
 281                     strat_db.wp_table[ni][nj].present &&
 282                     strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK)
 283                         n ++;
 284         }
 285
 286         return n;
 287 }
 288
 289
 290 int8_t get_path(const struct wp_coord *circuit,
 291                 uint8_t starti, uint8_t startj, uint8_t faceA,
 292                 struct wp_line *circuit_wpline)
 293 {
 294         const struct wp_coord *curcircuit;
 295         uint8_t prev_i, prev_j;
 296         uint8_t dir, prev_dir = 0xFF;
 297         uint8_t found = 0, i = 0, j = 0;
 298         uint8_t linenum;
 299         int8_t step = faceA ? 1 : -1;
 300         int8_t skipfirst=0;
 301         int8_t path_len = 0;
 302
 303         printf_P(PSTR("face: %d\r\n"), faceA);
 304         if ( !faceA && circuit->i == 11 && circuit->j == 6)
 305                 skipfirst=1;
 306
 307         /* check that the point is present in the circuit */
 308         for (curcircuit = circuit + skipfirst; curcircuit->end == 0; curcircuit ++) {
 309                 if (curcircuit->i == starti && curcircuit->j == startj) {
 310                         found = 1;
 311                         break;
 312                 }
 313         }
 314
 315         if ( !faceA && curcircuit->i == 11 && curcircuit->j == 6)
 316                 found = 1;
 317         if (found == 0)
 318                 return -1;
 319
 320         /* XXX len must be >= 1 */
 321         /* XXX start = 11,6 */
 322
 323         prev_i = starti;
 324         prev_j = startj;
 325
 326         curcircuit = curcircuit;
 327         while (1) {
 328                 if (faceA && curcircuit->end)
 329                         break;
 330                 else if (!faceA && curcircuit == circuit)
 331                         break;
 332                 i = curcircuit->i;
 333                 j = curcircuit->j;
 334
 335                 dir = get_dir(prev_i, prev_j, i, j);
 336
 337                 if (prev_dir != dir) {
 338                         linenum = get_line_num(prev_i, prev_j, dir);
 339                         /* printf_P(PSTR("COIN %d, %d, dir=%d linenum=%d\r\n"), */
 340                         /*                               prev_i, prev_j, dir, linenum); */
 341                         circuit_wpline[path_len].line_num = linenum;
 342                         circuit_wpline[path_len].dir = dir;
 343                         path_len++;
 344                 }
 345                 prev_dir = dir;
 346                 prev_i = i;
 347                 prev_j = j;
 348                 curcircuit += step;
 349         }
 350
 351 /*      printf_P(PSTR("COIN %d, %d\r\n"), curcircuit->i, curcircuit->j); */
 352
 353         return path_len; /* XXX */
 354 }
 355
 356 int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved,
 357                   uint16_t tomato_retrieved, uint8_t len)
 358 {
 359         int16_t score = 0;
 360         uint8_t i;
 361         uint32_t mask = 1;
 362         uint8_t n;
 363
 364         /* score with corn */
 365         n = xget_cob_count() * 2;
 366         for (i = 0; i<CORN_NB; i++) {
 367                 if (n >= 10)
 368                         break;
 369                 if (wcorn_retrieved & mask) {
 370                         score += 250;
 371                         n += 2;
 372                 }
 373                 if (n >= 10)
 374                         break;
 375                 if (ucorn_retrieved & mask) {
 376                         score += 125;
 377                         n += 1;
 378                 }
 379                 mask <<= 1UL;
 380         }
 381
 382         printf_P(PSTR("get score: cob %d \r\n"), n);
 383         /* score with tomato */
 384         n = xget_ball_count();
 385         mask = 1;
 386         for (i = 0; i<TOMATO_NB; i++) {
 387                 if (n >= 4)
 388                         break;
 389                 if (tomato_retrieved & mask) {
 390                         score += 150;
 391                         n += 1;
 392                 }
 393                 mask <<= 1UL;
 394         }
 395
 396         printf_P(PSTR("get score: ball %d \r\n"), n);
 397         /* malus for long circuits */
 398         score -= (len * 20);
 399
 400         /* double malus for long circuits if we don't have much
 401          * time */
 402 #define WP_SPEED 1
 403         if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) {
 404                 uint16_t extra;
 405                 extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s());
 406                 score -= (200 * extra);
 407         }
 408
 409         /* XXX use direction of robot */
 410
 411         return score;
 412 }
 413
 414 /* i,j: starting position */
 415 int8_t browse_one_circuit(const struct wp_coord *circuit,
 416                           uint8_t starti, uint8_t startj,
 417                           int16_t *scoreA, int16_t *scoreB)
 418 {
 419         const struct wp_coord *curcircuit;
 420         uint32_t wcorn_retrieved = 0; /* bit mask */
 421         uint32_t ucorn_retrieved = 0; /* bit mask */
 422         uint16_t tomato_retrieved = 0; /* bit mask */
 423         uint8_t len = 0, found = 0, i, j;
 424         uint8_t ni = 0, nj = 0, pos, color;
 425
 426         /* check that the point is present in the circuit */
 427         for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++) {
 428                 if (curcircuit->i == starti && curcircuit->j == startj) {
 429                         found = 1;
 430                         break;
 431                 }
 432         }
 433
 434         if (found == 0)
 435                 return -1;
 436
 437         for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++, len ++) {
 438                 i = curcircuit->i;
 439                 j = curcircuit->j;
 440
 441 /*              printf("cur:%d,%d %x %x %x %d\n", i, j, */
 442 /*                     wcorn_retrieved, ucorn_retrieved, tomato_retrieved, len); */
 443
 444                 /* face A completed */
 445                 if (i == starti && j == startj) {
 446                         *scoreA = get_score(wcorn_retrieved, ucorn_retrieved,
 447                                            tomato_retrieved, len);
 448                         wcorn_retrieved = 0; /* bit mask */
 449                         ucorn_retrieved = 0; /* bit mask */
 450                         tomato_retrieved = 0; /* bit mask */
 451                         len = 0;
 452                 }
 453
 454                 /* is there a tomato ? */
 455                 if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
 456                     strat_db.wp_table[i][j].present) {
 457                         tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
 458                 }
 459
 460                 /* browse all neighbours to see if there is cobs */
 461                 for (pos = LINE_UP; pos <= LINE_R_DOWN; pos++) {
 462                         if (wp_get_neigh(i, j, &ni, &nj, pos) < 0)
 463                                 continue;
 464
 465                         /* is there a corn cob ? */
 466                         if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
 467                             strat_db.wp_table[ni][nj].present) {
 468                                 color = strat_db.wp_table[ni][nj].corn.color;
 469                                 if (color == I2C_COB_WHITE)
 470                                         wcorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx);
 471                                 else if (color == I2C_COB_UNKNOWN)
 472                                         ucorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx);
 473                         }
 474                 }
 475         };
 476
 477         *scoreB = get_score(wcorn_retrieved, ucorn_retrieved,
 478                             tomato_retrieved, len);
 479         if (circuit->i == starti && circuit->j == startj)
 480                 *scoreA = *scoreB;
 481
 482         return 0;
 483 }
 484
 485 /* i,j starting position */
 486 int8_t browse_circuits(uint8_t i, uint8_t j,
 487                        const struct wp_coord **selected_circuit,
 488                        int8_t *selected_face)
 489 {
 490         const struct wp_coord **circuit;
 491         int16_t scoreA, scoreB;
 492         int16_t selected_score = 0x8000; /* ~-int_max */
 493         int8_t found = -1;
 494
 495         *selected_face = 0;
 496         *selected_circuit = circuits[0] ;
 497         for (circuit = &circuits[0]; *circuit; circuit++) {
 498                 if (browse_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
 499                         continue;
 500                 found = 0;
 501                 printf_P(PSTR("Circuit: %d %d\r\n"), scoreA, scoreB);
 502                 if (scoreA > selected_score) {
 503                         *selected_circuit = *circuit;
 504                         selected_score = scoreA;
 505                         *selected_face = 0;
 506                 }
 507                 if (scoreB > selected_score) {
 508                         *selected_circuit = *circuit;
 509                         selected_score = scoreB;
 510                         *selected_face = 1;
 511                 }
 512         }
 513         return found;
 514 }
 515
 516 static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
 517 {
 518         int8_t i;
 519         if (len <= 0)
 520                 return;
 521         for (i = 0; i < len; i ++) {
 522                 printf_P(PSTR("linenum %d dir %d\r\n"), circuit_wpline[i].line_num,
 523                        circuit_wpline[i].dir);
 524         }
 525
 526 }
 527
 528 uint8_t strat_harvest_circuit(void)
 529 {
 530         const struct wp_coord *selected_circuit;
 531         int8_t selected_face;
 532         struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
 533         int8_t len;
 534         uint8_t i, j, idx;
 535         int16_t x, y;
 536         uint8_t linenum, prev_linenum;
 537         uint8_t dir, prev_dir;
 538         uint8_t err;
 539
 540         x = position_get_x_s16(&mainboard.pos);
 541         y = position_get_y_s16(&mainboard.pos);
 542
 543         if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
 544                 DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
 545                       __FUNCTION__, x, y);
 546                 return END_ERROR;
 547         }
 548
 549         browse_circuits(i, j, &selected_circuit, &selected_face);
 550         len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 551         if (len < 0) {
 552                 DEBUG(E_USER_STRAT, "%s(): cannot find a path",
 553                       __FUNCTION__);
 554                 return END_ERROR;
 555         }
 556
 557         dump_circuit_wp(circuit_wpline, len);
 558
 559         prev_linenum = circuit_wpline[0].line_num;
 560         prev_dir = circuit_wpline[0].dir;
 561         for (idx = 1; idx < len; idx ++) {
 562         retry:
 563                 linenum = circuit_wpline[idx].line_num;
 564                 dir = circuit_wpline[idx].dir;
 565
 566                 /* XXX basic opponent management */
 567                 DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
 568                       __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
 569                 err = line2line(prev_linenum, prev_dir, linenum, dir,
 570                                 TRAJ_FLAGS_NO_NEAR);
 571                 if (!TRAJ_SUCCESS(err)) {
 572                         strat_hardstop();
 573                         time_wait_ms(2000);
 574                         goto retry;
 575                 }
 576
 577                 prev_linenum = linenum;
 578                 prev_dir = dir;
 579         }
 580
 581         return END_TRAJ; // XXX
 582 }
 583
 584 void test_strat_avoid(void)
 585 {
 586
 587         //corn_count_neigh(1, 3);
 588
 589
 590 #if 0
 591         uint8_t i, j;
 592         const struct wp_coord *selected_circuit;
 593         int8_t selected_face;
 594         struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
 595         int8_t ret;
 596
 597         i = 1; j = 1;
 598         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 599
 600         ts = 0; bc = 0; cc = 0;
 601         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 602         browse_circuits(i, j, &selected_circuit, &selected_face);
 603         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 604         dump_circuit_wp(circuit_wpline, ret);
 605
 606         ts = 0; bc = 3; cc = 0;
 607         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 608         browse_circuits(i, j, &selected_circuit, &selected_face);
 609         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 610         dump_circuit_wp(circuit_wpline, ret);
 611
 612         ts = 0; bc = 4; cc = 0;
 613         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 614         browse_circuits(i, j, &selected_circuit, &selected_face);
 615         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 616         dump_circuit_wp(circuit_wpline, ret);
 617
 618         ts = 0; bc = 3; cc = 5;
 619         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 620         browse_circuits(i, j, &selected_circuit, &selected_face);
 621         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 622         dump_circuit_wp(circuit_wpline, ret);
 623
 624         ts = 0; bc = 4; cc = 5;
 625         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 626         browse_circuits(i, j, &selected_circuit, &selected_face);
 627         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 628         dump_circuit_wp(circuit_wpline, ret);
 629
 630         ts = 80; bc = 0; cc = 0;
 631         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 632         browse_circuits(i, j, &selected_circuit, &selected_face);
 633         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 634         dump_circuit_wp(circuit_wpline, ret);
 635
 636         i = 4; j = 3;
 637         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 638
 639         ts = 0; bc = 0; cc = 0;
 640         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 641         browse_circuits(i, j, &selected_circuit, &selected_face);
 642         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 643         dump_circuit_wp(circuit_wpline, ret);
 644
 645         ts = 0; bc = 3; cc = 0;
 646         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 647         browse_circuits(i, j, &selected_circuit, &selected_face);
 648         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 649         dump_circuit_wp(circuit_wpline, ret);
 650
 651         ts = 80; bc = 0; cc = 0;
 652         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 653         browse_circuits(i, j, &selected_circuit, &selected_face);
 654         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 655         dump_circuit_wp(circuit_wpline, ret);
 656
 657         i = 11; j = 6;
 658         printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 659
 660         ts = 0; bc = 0; cc = 0;
 661         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 662         browse_circuits(i, j, &selected_circuit, &selected_face);
 663         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 664         dump_circuit_wp(circuit_wpline, ret);
 665
 666         ts = 0; bc = 3; cc = 0;
 667         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 668         browse_circuits(i, j, &selected_circuit, &selected_face);
 669         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 670         dump_circuit_wp(circuit_wpline, ret);
 671
 672         ts = 80; bc = 0; cc = 0;
 673         printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
 674         browse_circuits(i, j, &selected_circuit, &selected_face);
 675         ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
 676         dump_circuit_wp(circuit_wpline, ret);
 677 #endif
 678 }