projects/microb2010/mainboard/strat_corn.c

   1 /*
   2  *  Copyright Droids, Microb Technology (2010)
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  17  *
  18  *  Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
  19  *
  20  *  Olivier MATZ <zer0@droids-corp.org>
  21  */
  22
  23
  24 #include <stdio.h>
  25 #include <stdlib.h>
  26 #include <stdint.h>
  27 #include <math.h>
  28
  29 #include <aversive.h>
  30 #include <aversive/pgmspace.h>
  31
  32 #include <ax12.h>
  33 #include <uart.h>
  34 #include <pwm_ng.h>
  35 #include <clock_time.h>
  36 #include <spi.h>
  37
  38 #include <pid.h>
  39 #include <quadramp.h>
  40 #include <control_system_manager.h>
  41 #include <trajectory_manager.h>
  42 #include <trajectory_manager_utils.h>
  43 #include <trajectory_manager_core.h>
  44 #include <vect_base.h>
  45 #include <lines.h>
  46 #include <polygon.h>
  47 #include <obstacle_avoidance.h>
  48 #include <blocking_detection_manager.h>
  49 #include <robot_system.h>
  50 #include <position_manager.h>
  51
  52 #include <diagnostic.h>
  53
  54 #include <rdline.h>
  55 #include <parse.h>
  56
  57 #include "../common/i2c_commands.h"
  58 #include "i2c_protocol.h"
  59 #include "main.h"
  60 #include "strat.h"
  61 #include "strat_base.h"
  62 #include "strat_corn.h"
  63 #include "strat_utils.h"
  64 #include "sensor.h"
  65 #include "actuator.h"
  66
  67 /* XXX TODO
  68 static
  69 const
  70 change x,y -> i,j to avoid confusion with coords
  71 could be optimized in mem space: it is not needed to store the x,y coord,
  72    we can process it from idx. however it will be less optimized for speed
  73
  74 */
  75
  76 #define OFFSET_CORN_X 150
  77 #define OFFSET_CORN_Y 222
  78 #define STEP_CORN_X 225
  79 #define STEP_CORN_Y 250
  80
  81 #define CORN_NB 18
  82
  83 #define WAYPOINTS_NBX 13
  84 #define WAYPOINTS_NBY 8
  85
  86 /* enum is better */
  87 #define TYPE_WAYPOINT   0
  88 #define TYPE_DANGEROUS  1
  89 #define TYPE_WHITE_CORN 2
  90 #define TYPE_BLACK_CORN 3
  91 #define TYPE_OBSTACLE   4
  92 #define TYPE_UNKNOWN    5
  93
  94 /* XXX enum possible ? else just rename */
  95 #define START      0
  96 #define UP         1
  97 #define UP_RIGHT   2
  98 #define DOWN_RIGHT 3
  99 #define DOWN       4
 100 #define DOWN_LEFT  5
 101 #define UP_LEFT    6
 102 #define END        7
 103
 104 struct point {
 105         int32_t x;
 106         int32_t y;
 107 };
 108
 109 struct djpoint {
 110         struct point pos;
 111         uint16_t weight;
 112         struct djpoint *parent;
 113
 114         uint8_t type:3;
 115         uint8_t parent_pos:3;
 116         uint8_t updated:1;
 117         uint8_t todo:1;
 118 };
 119
 120 uint8_t corn_table[CORN_NB];
 121
 122 const uint8_t corn_coord_i[CORN_NB] = {
 123         0, 0, 0, 2, 2, 2, 4, 4, 6,
 124         6, 8, 8, 10, 10, 10, 12, 12, 12,
 125 };
 126
 127 const uint8_t corn_coord_j[CORN_NB] = {
 128         2, 4, 6, 3, 5, 7, 4, 6, 5,
 129         7, 4, 6, 3, 5, 7, 2, 4, 6,
 130 };
 131
 132 static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
 133
 134 /* table to find the symetric idx */
 135 uint8_t corn_sym[] = {
 136         15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2
 137 };
 138
 139 uint8_t corn_side_confs[9][2] = {
 140         { 1, 4 },
 141         { 0, 4 },
 142         { 2, 4 },
 143         { 2, 3 },
 144         { 0, 3 },
 145         { 1, 3 },
 146         { 1, 6 },
 147         { 0, 6 },
 148         { 2, 6 },
 149 };
 150 uint8_t corn_center_confs[4][2] = {
 151         { 5, 8 },
 152         { 7, 8 },
 153         { 5, 9 },
 154         { 7, 8 },
 155 };
 156
 157
 158 /* return index from neigh pointer */
 159 #define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
 160
 161 void dump(void)
 162 {
 163         int8_t i, j;
 164         struct djpoint *pt;
 165
 166         printf_P(PSTR("         "));
 167         for (i=0; i<WAYPOINTS_NBX; i++) {
 168                 printf_P(PSTR(" %2d "), i);
 169         }
 170         printf_P(PSTR("\r\n"));
 171
 172         for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
 173                 printf_P(PSTR("%3d   "), j/2);
 174
 175                 if ((j&1) == 0)
 176                         printf_P(PSTR("    "));
 177
 178                 for (i=0; i<WAYPOINTS_NBX; i++) {
 179                         pt = &djpoints[i][j/2];
 180
 181                         if (((i+j) & 1) == 0)
 182                                 continue;
 183
 184                         if (pt->type == TYPE_OBSTACLE)
 185                                 printf_P(PSTR("     X  "));
 186                         else if (pt->type == TYPE_DANGEROUS)
 187                                 printf_P(PSTR("     D  "));
 188                         else if (pt->type == TYPE_WHITE_CORN)
 189                                 printf_P(PSTR("     W  "));
 190                         else if (pt->type == TYPE_BLACK_CORN)
 191                                 printf_P(PSTR("     B  "));
 192                         else if (pt->type == TYPE_WAYPOINT)
 193                                 printf_P(PSTR(" %5d  "), pt->weight);
 194                         else
 195                                 printf_P(PSTR("     ?  "));
 196                 }
 197                 printf_P(PSTR("\r\n"));
 198         }
 199 }
 200
 201 static inline uint8_t opposite_position(uint8_t pos)
 202 {
 203         pos += 3;
 204         if (pos > UP_LEFT)
 205                 pos -= 6;
 206         return pos;
 207 }
 208
 209 /* return coord of the entry in the table from the pointer */
 210 static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
 211 {
 212         int8_t idx = PT2IDX(pt);
 213         *x = idx / WAYPOINTS_NBY;
 214         *y = idx % WAYPOINTS_NBY;
 215 }
 216
 217 /* get the neighbour of the point at specified position */
 218 static struct djpoint *get_neigh(struct djpoint *pt,
 219                                  uint8_t position)
 220 {
 221         int8_t i,j;
 222         struct djpoint *neigh;
 223
 224         djpoint2ij(pt, &i, &j);
 225
 226         switch (position) {
 227         case UP:
 228                 j++;
 229                 break;
 230         case UP_RIGHT:
 231                 if (!(i & 1)) j++;
 232                 i++;
 233                 break;
 234         case DOWN_RIGHT:
 235                 if (i & 1) j--;
 236                 i++;
 237                 break;
 238         case DOWN:
 239                 j--;
 240                 break;
 241         case DOWN_LEFT:
 242                 if (i & 1) j--;
 243                 i--;
 244                 break;
 245         case UP_LEFT:
 246                 if (!(i & 1)) j++;
 247                 i--;
 248                 break;
 249         default:
 250                 return NULL;
 251         }
 252         if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
 253                 return NULL;
 254
 255         neigh = &djpoints[i][j];
 256
 257         if (neigh->type != TYPE_WAYPOINT)
 258                 return NULL;
 259
 260         return neigh;
 261 }
 262
 263 static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
 264 {
 265         struct djpoint *neigh = NULL;
 266         uint8_t pos = *position + 1;
 267
 268         for (pos = *position + 1; pos < END; pos++) {
 269                 neigh = get_neigh(pt, pos);
 270                 if (neigh != NULL)
 271                         break;
 272         }
 273
 274         *position = pos;
 275         return neigh;
 276 }
 277
 278 /* browse all points */
 279 #define POINT_FOREACH(cur)                                              \
 280         for (cur = &djpoints[0][0];                                     \
 281              cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];             \
 282              cur ++)
 283
 284 /* XXX comment */
 285 #define NEIGH_FOREACH(neigh, pos, point)                        \
 286         for (pos = START, neigh = get_next_neigh(point, &pos);  \
 287              neigh;                                             \
 288              neigh = get_next_neigh(point, &pos))
 289
 290 int dijkstra_init(void)
 291 {
 292         return 0;
 293 }
 294
 295 static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
 296 {
 297         double vx, vy;
 298         vx = p2->pos.x - p1->pos.x;
 299         vy = p2->pos.y - p1->pos.y;
 300         return sqrt(vx * vx + vy * vy);
 301 }
 302
 303 void dijkstra_process_neighs(struct djpoint *pt)
 304 {
 305         struct djpoint *neigh;
 306         uint8_t pos, parent_pos;
 307         uint16_t weight;
 308         int8_t i,j;
 309
 310         djpoint2ij(pt, &i, &j);
 311         printf_P(PSTR("at %d %d:\r\n"), i, j);
 312
 313         NEIGH_FOREACH(neigh, pos, pt) {
 314                 weight = pt->weight + dist(pt, neigh);
 315                 parent_pos = opposite_position(pos);
 316
 317                 /* bonus if we keep the same direction */
 318                 if (parent_pos == pt->parent_pos ||
 319                     pt->parent_pos == END)
 320                         weight = weight - 1;
 321
 322                 printf_P(PSTR("  pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos,
 323                        pt->parent_pos, parent_pos,
 324                        neigh->weight, weight);
 325                 if (neigh->weight == 0 || weight < neigh->weight) {
 326                         djpoint2ij(neigh, &i, &j);
 327                         //printf_P(PSTR("    %d,%d updated\r\n"), i, j);
 328                         neigh->weight = weight;
 329                         neigh->parent_pos = parent_pos;
 330                         neigh->updated = 1;
 331                 }
 332         }
 333 }
 334
 335 int dijkstra(struct djpoint *start)
 336 {
 337         struct djpoint *cur;
 338         uint8_t todolist = 1;
 339
 340         start->todo = 1;
 341
 342         while (todolist) {
 343                 printf_P(PSTR("\r\n"));
 344                 dump();
 345                 /* process all neighbours of todo list */
 346                 POINT_FOREACH(cur) {
 347                         if (!cur->todo)
 348                                 continue;
 349                         dijkstra_process_neighs(cur);
 350                         cur->todo = 0;
 351                 }
 352
 353                 /* convert updated list in todo list */
 354                 todolist = 0;
 355                 POINT_FOREACH(cur) {
 356                         if (!cur->updated)
 357                                 continue;
 358                         todolist = 1;
 359                         cur->todo = 1;
 360                         cur->updated = 0;
 361                 }
 362         }
 363         return 0; /* XXX */
 364 }
 365
 366 int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j)
 367 {
 368         uint8_t n;
 369         for (n = 0; n < CORN_NB; n ++) {
 370                 if (i == corn_coord_i[n] &&
 371                     j == corn_coord_j[n])
 372                         return n;
 373         }
 374         return -1;
 375 }
 376
 377 int8_t corn_idx_to_coordij(uint8_t idx, uint8_t *i, uint8_t *j)
 378 {
 379         if (idx >= CORN_NB)
 380                 return -1;
 381         *i = corn_coord_i[idx];
 382         *j = corn_coord_j[idx];
 383         return 0;
 384 }
 385
 386 /* return the index of the closest corn at these coordinates. If the
 387  * corn is really too far (~20cm), return -1. The x and y pointer are
 388  * updated with the real position of the corn */
 389 int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y)
 390 {
 391         uint8_t idx = -1, n, i, j;
 392         int16_t d, x_corn, y_corn;
 393         int16_t x_corn_min = 0, y_corn_min = 0;
 394         int16_t d_min = 0;
 395
 396         for (n = 0; n < CORN_NB; n ++) {
 397                 corn_idx_to_coordij(n, &i, &j);
 398                 x_corn = (OFFSET_CORN_X + i*STEP_CORN_X);
 399                 y_corn = (OFFSET_CORN_Y + j*STEP_CORN_Y);
 400                 d = xy_norm(x_corn, y_corn, *x, *y);
 401                 if (d < 200 && (d_min == 0 || d < d_min)) {
 402                         d_min = d;
 403                         idx = n;
 404                         x_corn_min = x_corn;
 405                         y_corn_min = y_corn;
 406                 }
 407         }
 408         if (d_min == 0)
 409                 return -1;
 410
 411         *x = x_corn_min;
 412         *y = y_corn_min;
 413
 414         return idx;
 415 }
 416
 417 int8_t corn_get_sym(int8_t i)
 418 {
 419         if (i >= CORN_NB)
 420                 return -1;
 421         return corn_sym[i];
 422 }
 423
 424 void init_corn_table(int8_t conf_side, int8_t conf_center)
 425 {
 426         int8_t sym, i;
 427
 428         /* before match */
 429         if (conf_side == -1) {
 430                 for (i=0; i<CORN_NB; i++)
 431                         corn_table[i] = TYPE_UNKNOWN;
 432                 return;
 433         }
 434
 435         /* for test */
 436         printf_P(PSTR("confs = %d, %d\r\n"), conf_side, conf_center);
 437         for (i=0; i<CORN_NB; i++) {
 438                 if (i == corn_side_confs[conf_side][0] ||
 439                     i == corn_side_confs[conf_side][1]) {
 440                         corn_table[i] = TYPE_BLACK_CORN;
 441                         continue;
 442                 }
 443                 sym = corn_get_sym(i);
 444                 if (sym == corn_side_confs[conf_side][0] ||
 445                     sym == corn_side_confs[conf_side][1]) {
 446                         corn_table[i] = TYPE_BLACK_CORN;
 447                         continue;
 448                 }
 449                 if (i == corn_center_confs[conf_center][0] ||
 450                     i == corn_center_confs[conf_center][1]) {
 451                         corn_table[i] = TYPE_BLACK_CORN;
 452                         continue;
 453                 }
 454                 sym = corn_get_sym(i);
 455                 if (sym == corn_center_confs[conf_center][0] ||
 456                     sym == corn_center_confs[conf_center][1]) {
 457                         corn_table[i] = TYPE_BLACK_CORN;
 458                         continue;
 459                 }
 460                 corn_table[i] = TYPE_WHITE_CORN;
 461         }
 462 }
 463
 464 /* init waypoints position */
 465 void init_waypoints(void)
 466 {
 467         int8_t i, j;
 468         int32_t x, y;
 469         struct djpoint *pt;
 470
 471         x = OFFSET_CORN_X;
 472         for (i=0; i<WAYPOINTS_NBX; i++) {
 473
 474                 if (i & 1)
 475                         y = OFFSET_CORN_Y - STEP_CORN_Y/2;
 476                 else
 477                         y = OFFSET_CORN_Y;
 478
 479                 for (j=0; j<WAYPOINTS_NBY; j++) {
 480                         pt = &djpoints[i][j];
 481
 482                         pt->pos.x = x;
 483                         pt->pos.y = y;
 484
 485                         pt->type = TYPE_WAYPOINT;
 486                         pt->parent_pos = END;
 487                         pt->updated = 0;
 488                         pt->todo = 0;
 489
 490                         y += STEP_CORN_Y;
 491                 }
 492
 493                 x += STEP_CORN_X;
 494         }
 495 }
 496
 497 /* update the type and weight of waypoints, before starting
 498  * dijkstra */
 499 void update_waypoints(void)
 500 {
 501         int8_t i, j, c;
 502         struct djpoint *pt;
 503
 504         for (i=0; i<WAYPOINTS_NBX; i++) {
 505
 506                 for (j=0; j<WAYPOINTS_NBY; j++) {
 507                         pt = &djpoints[i][j];
 508
 509                         pt->weight = 0;
 510
 511                         /* corn */
 512                         c = ijcoord_to_corn_idx(i, j);
 513                         if (c >= 0) {
 514                                 pt->type = corn_table[c];
 515                                 continue;
 516                         }
 517                         /* too close of border */
 518                         if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
 519                                 pt->type = TYPE_OBSTACLE;
 520                                 continue;
 521                         }
 522                         /* hill */
 523                         if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
 524                                 pt->type = TYPE_OBSTACLE;
 525                                 continue;
 526                         }
 527                         /* dangerous points */
 528                         if (i == 0 || i == (WAYPOINTS_NBX-1)) {
 529                                 pt->type = TYPE_DANGEROUS;
 530                                 continue;
 531                         }
 532                         if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
 533                                 pt->type = TYPE_DANGEROUS;
 534                                 continue;
 535                         }
 536                         pt->type = TYPE_WAYPOINT;
 537                 }
 538         }
 539 }
 540
 541 int get_path(struct djpoint *start, struct djpoint *end)
 542 {
 543         struct djpoint *cur;
 544         uint8_t prev_direction = 0;
 545
 546         for (cur=start;
 547              cur != NULL && cur->parent_pos != END && cur != end;
 548              cur = get_neigh(cur, cur->parent_pos)) {
 549                 if (prev_direction != cur->parent_pos) {
 550                         printf_P(PSTR("%d %d (%d)\r\n"),
 551                                  cur->pos.x, cur->pos.y,
 552                                  cur->parent_pos);
 553                 }
 554                 prev_direction = cur->parent_pos;
 555         }
 556         printf_P(PSTR("%d %d\r\n"), end->pos.x, end->pos.y);
 557
 558         return 0; /* XXX */
 559 }
 560
 561 /* return 1 if there is a corn near, and fill the index ptr */
 562 static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
 563 {
 564 #define SENSOR_CORN_DIST  225
 565 #define SENSOR_CORN_ANGLE 90
 566         double x = position_get_x_double(&mainboard.pos);
 567         double y = position_get_y_double(&mainboard.pos);
 568         double a_rad = position_get_a_rad_double(&mainboard.pos);
 569         double x_corn, y_corn;
 570         int16_t x_corn_int, y_corn_int;
 571
 572         if (side == I2C_LEFT_SIDE) {
 573                 x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
 574                 y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
 575         }
 576         else {
 577                 x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
 578                 y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
 579         }
 580         x_corn_int = x_corn;
 581         y_corn_int = y_corn;
 582
 583         *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
 584         if (*corn_idx < 0)
 585                 return 0;
 586         return 1;
 587 }
 588
 589 /*
 590  * - send the correct commands to the spickles
 591  * - return 1 if we need to stop (cobboard is stucked)
 592 */
 593 static uint8_t handle_spickles(void)
 594 {
 595         int8_t corn_idx;
 596
 597         return 0;
 598
 599         if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
 600                 i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
 601         else {
 602                 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
 603                         i2c_cobboard_mode_harvest(I2C_LEFT_SIDE);
 604                 else
 605                         i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
 606         }
 607 /*      printf("%d %d\n", corn_idx, corn_table[corn_idx]); */
 608 /*      time_wait_ms(100); */
 609
 610         if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE))
 611                 i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE);
 612         else {
 613                 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
 614                         i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE);
 615                 else
 616                         i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
 617         }
 618
 619         return 0;
 620 }
 621
 622 uint8_t line2line(uint8_t dir1, uint8_t num1,
 623                   uint8_t dir2, uint8_t num2)
 624 {
 625         double line1_a_rad, line1_a_deg, line2_a_rad;
 626         double diff_a_deg, diff_a_deg_abs, beta_deg;
 627         double radius;
 628         struct line_2pts l1, l2;
 629         line_t ll1, ll2;
 630         point_t p;
 631         uint8_t err;
 632
 633         /* convert to 2 points */
 634         num2line(&l1, dir1, num1);
 635         num2line(&l2, dir2, num2);
 636
 637         printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
 638                  l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
 639         printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
 640                  l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
 641
 642         /* convert to line eq and find intersection */
 643         pts2line(&l1.p1, &l1.p2, &ll1);
 644         pts2line(&l2.p1, &l2.p2, &ll2);
 645         intersect_line(&ll1, &ll2, &p);
 646
 647         line1_a_rad = atan2(l1.p2.y - l1.p1.y,
 648                             l1.p2.x - l1.p1.x);
 649         line1_a_deg = DEG(line1_a_rad);
 650         line2_a_rad = atan2(l2.p2.y - l2.p1.y,
 651                             l2.p2.x - l2.p1.x);
 652         diff_a_deg = DEG(line2_a_rad - line1_a_rad);
 653         diff_a_deg_abs = fabs(diff_a_deg);
 654
 655         if (diff_a_deg_abs < 70.) {
 656                 radius = 200;
 657                 if (diff_a_deg > 0)
 658                         beta_deg = 40;
 659                 else
 660                         beta_deg = -40;
 661         }
 662         else if (diff_a_deg_abs < 100.) {
 663                 radius = 100;
 664                 if (diff_a_deg > 0)
 665                         beta_deg = 40;
 666                 else
 667                         beta_deg = -40;
 668         }
 669         else {
 670                 radius = 120;
 671                 if (diff_a_deg > 0)
 672                         beta_deg = 60;
 673                 else
 674                         beta_deg = -60;
 675         }
 676
 677         trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
 678                            line1_a_deg, 150., diff_a_deg, beta_deg,
 679                            radius, xy_norm(l1.p1.x, l1.p1.y,
 680                                            p.x, p.y));
 681         err = 0;
 682         while (err == 0) {
 683                 err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
 684                 if (err == 0) {
 685                         /* cobboard is stucked */
 686                         trajectory_hardstop(&mainboard.traj);
 687                         return err; /* XXX do something */
 688                 }
 689                 err = test_traj_end(0xFF);
 690         }
 691         return err;
 692 }
 693
 694 void num2line(struct line_2pts *l, uint8_t dir, uint8_t num)
 695 {
 696         float n = num;
 697
 698         switch (dir) {
 699
 700         case LINE_UP:
 701                 l->p1.x = n * 450 + 375;
 702                 l->p1.y = COLOR_Y(0);
 703                 l->p2.x = n * 450 + 375;
 704                 l->p2.y = COLOR_Y(2100);
 705                 break;
 706         case LINE_DOWN:
 707                 l->p1.x = n * 450 + 375;
 708                 l->p1.y = COLOR_Y(2100);
 709                 l->p2.x = n * 450 + 375;
 710                 l->p2.y = COLOR_Y(0);
 711                 break;
 712         case LINE_R_UP:
 713                 l->p1.x = 150;
 714                 l->p1.y = COLOR_Y(-n * 500 + 1472);
 715                 l->p2.x = 2850;
 716                 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
 717                 break;
 718         case LINE_L_DOWN:
 719                 l->p1.x = 2850;
 720                 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
 721                 l->p2.x = 150;
 722                 l->p2.y = COLOR_Y(-n * 500 + 1472);
 723                 break;
 724         case LINE_L_UP:
 725                 l->p1.x = 2850;
 726                 l->p1.y = COLOR_Y(-n * 500 + 1472);
 727                 l->p2.x = 150;
 728                 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
 729                 break;
 730         case LINE_R_DOWN:
 731                 l->p1.x = 150;
 732                 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
 733                 l->p2.x = 2850;
 734                 l->p2.y = COLOR_Y(-n * 500 + 1472);
 735                 break;
 736         default:
 737                 break;
 738         }
 739 }
 740
 741
 742 #if 0
 743 int main(void)
 744 {
 745         struct djpoint *start;
 746         struct djpoint *end;
 747
 748         start = &djpoints[1][1];
 749         end = &djpoints[12][1];
 750
 751         init_corn_table(0, 0);
 752         init_waypoints();
 753         update_waypoints();
 754
 755         dijkstra(end);
 756         dump();
 757
 758         get_path(start, end);
 759
 760         return 0;
 761 }
 762 #endif