projects/microb2010/mainboard/strat_db.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 <string.h>
  25 #include <stdio.h>
  26 #include <stdlib.h>
  27 #include <stdint.h>
  28 #include <math.h>
  29
  30 #include <aversive.h>
  31 #include <aversive/pgmspace.h>
  32 #include <aversive/error.h>
  33
  34 #include <ax12.h>
  35 #include <uart.h>
  36 #include <pwm_ng.h>
  37 #include <clock_time.h>
  38 #include <spi.h>
  39
  40 #include <pid.h>
  41 #include <quadramp.h>
  42 #include <control_system_manager.h>
  43 #include <trajectory_manager.h>
  44 #include <trajectory_manager_utils.h>
  45 #include <trajectory_manager_core.h>
  46 #include <vect_base.h>
  47 #include <lines.h>
  48 #include <polygon.h>
  49 #include <obstacle_avoidance.h>
  50 #include <blocking_detection_manager.h>
  51 #include <robot_system.h>
  52 #include <position_manager.h>
  53
  54 #include <diagnostic.h>
  55
  56 #include <rdline.h>
  57 #include <parse.h>
  58
  59 #include "../common/i2c_commands.h"
  60 #include "i2c_protocol.h"
  61 #include "main.h"
  62 #include "strat.h"
  63 #include "strat_base.h"
  64 #include "strat_avoid.h"
  65 #include "strat_corn.h"
  66 #include "strat_db.h"
  67 #include "strat_utils.h"
  68 #include "sensor.h"
  69 #include "actuator.h"
  70
  71 /* status of objects on area */
  72 struct strat_db strat_db;
  73
  74 /* given an index, give the i coord */
  75 static const uint8_t corn_coord_i[CORN_NB] = {
  76         0, 0, 0, 2, 2, 2, 4, 4, 6,
  77         6, 8, 8, 10, 10, 10, 12, 12, 12,
  78 };
  79
  80 /* given an index, give the j coord */
  81 static const uint8_t corn_coord_j[CORN_NB] = {
  82         2, 4, 6, 3, 5, 7, 4, 6, 5,
  83         7, 4, 6, 3, 5, 7, 2, 4, 6,
  84 };
  85
  86 /* table to find the symetric idx */
  87 static const uint8_t corn_sym[] = {
  88         15, 16, 17, 12, 13, 14, 10, 11,
  89         8, 9, 6, 7, 3, 4, 5, 0, 1, 2
  90 };
  91
  92 #ifdef HOST_VERSION
  93 #define SIDE_CONF 0
  94 #define CENTER_CONF 0
  95 /* the 10 possible configurations for corn on the side */
  96 static const uint8_t corn_side_confs[9][2] = {
  97         { 1, 4 },
  98         { 0, 4 },
  99         { 2, 4 },
 100         { 2, 3 },
 101         { 0, 3 },
 102         { 1, 3 },
 103         { 1, 6 },
 104         { 0, 6 },
 105         { 2, 6 },
 106 };
 107
 108 /* the 4 possible configurations for corn on center */
 109 static const uint8_t corn_center_confs[4][2] = {
 110         { 5, 8 },
 111         { 7, 8 },
 112         { 5, 9 },
 113         { 7, 8 },
 114 };
 115 #endif
 116
 117 /* in these groups, only one black cob */
 118 static const int8_t corn_group1[] = { 0, 1, 2, -1, };
 119 static const int8_t corn_group2[] = { 3, 4, 6, -1, };
 120 static const int8_t corn_group3[] = { 5, 7, -1, };
 121 static const int8_t corn_group4[] = { 8, 9, -1, };
 122 static const int8_t corn_group5[] = { 11, 14, -1, };
 123 static const int8_t corn_group6[] = { 10, 12, 13, -1, };
 124 static const int8_t corn_group7[] = { 15, 16, 17, -1, };
 125
 126 static const int8_t *corn_groups[] = {
 127         corn_group1,
 128         corn_group2,
 129         corn_group3,
 130         corn_group4,
 131         corn_group5,
 132         corn_group6,
 133         corn_group7,
 134         NULL,
 135 };
 136
 137 /* given an index, give the i coord */
 138 static const uint8_t tomato_coord_i[TOMATO_NB] = {
 139         0, 0, 2, 2, 4, 4, 6, 6,
 140         8, 8, 10, 10, 12, 12,
 141 };
 142
 143 /* given an index, give the j coord */
 144 static const uint8_t tomato_coord_j[TOMATO_NB] = {
 145         3, 5, 4, 6, 5, 7, 4, 6, 5, 7, 4, 6, 3, 5,
 146 };
 147
 148 /******** Generic waypoint */
 149
 150 /* return the xy coords of a waypoint given its ij coords. */
 151 int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y)
 152 {
 153         if (i >= WAYPOINTS_NBX && j >= WAYPOINTS_NBY)
 154                 return -1;
 155         *x = (OFFSET_CORN_X + i*STEP_CORN_X);
 156         if (i&1)
 157                 *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y + STEP_CORN_Y/2);
 158         else
 159                 *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y);
 160         return 0;
 161 }
 162
 163 /* return the nearest waypoint (any type) */
 164 int8_t xycoord_to_ijcoord(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp)
 165 {
 166         int16_t x, y;
 167         uint8_t i, j;
 168
 169         x = *xp;
 170         y = *yp;
 171
 172         x -= OFFSET_CORN_X;
 173         x += (STEP_CORN_X/2);
 174         i = x / STEP_CORN_X;
 175
 176         y = COLOR_Y(y); /* Y depends on color */
 177         y -= OFFSET_CORN_Y;
 178         y += STEP_CORN_Y/2;
 179
 180         if ((i & 1) == 1)
 181                 y -= STEP_CORN_Y/2;
 182         j = y / STEP_CORN_Y;
 183
 184         if (ijcoord_to_xycoord(i, j, &x, &y) < 0)
 185                 return -1;
 186
 187         *xp = x;
 188         *yp = y;
 189         *ip = i;
 190         *jp = j;
 191
 192         return 0;
 193 }
 194
 195 /* return the nearest waypoint that is not a corn: xp and yp contains
 196  * the input and output, and ip, jp are only outputs. return 0 on
 197  * success. */
 198 int8_t xycoord_to_ijcoord_not_corn(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp)
 199 {
 200         int16_t x, y;
 201         uint8_t i, j;
 202
 203         x = *xp;
 204         y = *yp;
 205
 206         x -= OFFSET_CORN_X;
 207         x += (STEP_CORN_X/2);
 208         i = x / STEP_CORN_X;
 209
 210         y = COLOR_Y(y); /* Y depends on color */
 211         y -= OFFSET_CORN_Y;
 212         if ((i & 1) == 1) {
 213                 j = y / STEP_CORN_Y;
 214         }
 215         else if ((i & 3) == 0) {
 216                 j = y / (STEP_CORN_Y*2);
 217                 j = j*2 + 1;
 218         }
 219         else {
 220                 y += (STEP_CORN_Y);
 221                 j = y / (STEP_CORN_Y*2);
 222                 j = j*2;
 223         }
 224
 225         if (ijcoord_to_xycoord(i, j, &x, &y) < 0)
 226                 return -1;
 227
 228         if (strat_db.wp_table[i][j].type != WP_TYPE_WAYPOINT &&
 229             strat_db.wp_table[i][j].type != WP_TYPE_TOMATO)
 230                 return -1;
 231
 232         *xp = x;
 233         *yp = y;
 234         *ip = i;
 235         *jp = j;
 236
 237         return 0;
 238 }
 239
 240 /******** CORN */
 241
 242 static int8_t early_ijcoord_to_corn_idx(uint8_t i, uint8_t j)
 243 {
 244         uint8_t n;
 245         for (n = 0; n < CORN_NB; n ++) {
 246                 if (i == corn_coord_i[n] &&
 247                     j == corn_coord_j[n])
 248                         return n;
 249         }
 250         return -1;
 251 }
 252
 253 /* return the index of a corn given its i,j coords. */
 254 int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j)
 255 {
 256         if (strat_db.wp_table[i][j].type != WP_TYPE_CORN)
 257                 return -1;
 258         return strat_db.wp_table[i][j].corn.idx;
 259 }
 260
 261 /* return the i,j coords of a corn given its index */
 262 int8_t corn_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j)
 263 {
 264         if (idx >= CORN_NB)
 265                 return -1;
 266         *i = corn_coord_i[idx];
 267         *j = corn_coord_j[idx];
 268         return 0;
 269 }
 270
 271 /* return the index of a corn given its x,y coords. */
 272 int8_t corn_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y)
 273 {
 274         uint8_t i, j;
 275         if (corn_idx_to_ijcoord(idx, &i, &j) < 0)
 276                 return -1;
 277         if (ijcoord_to_xycoord(i, j, x, y) < 0)
 278                 return -1;
 279         return 0;
 280 }
 281
 282 #define CORN_MARGIN 200
 283 /* return the index of the closest corn at these coordinates. If the
 284  * corn is really too far (~20cm), return NULL. The x and y pointer are
 285  * updated with the real position of the corn */
 286 struct waypoint_db *xycoord_to_corn_idx(int16_t *xp, int16_t *yp)
 287 {
 288         int16_t x, y;
 289         uint8_t i, j;
 290         double d;
 291
 292         x = *xp;
 293         y = *yp;
 294
 295         x -= OFFSET_CORN_X;
 296         x += STEP_CORN_X;
 297         x /= (STEP_CORN_X*2);
 298
 299         y = COLOR_Y(y);
 300         y -= OFFSET_CORN_Y;
 301         y += STEP_CORN_Y;
 302         if ((x & 1) == 1)
 303                 y -= STEP_CORN_Y;
 304         y /= (STEP_CORN_Y*2);
 305
 306         i = (x * 2);
 307         j = (y * 2) + (x & 1);
 308
 309         if (ijcoord_to_xycoord(i, j, &x, &y) < 0)
 310                 return NULL;
 311
 312         if (strat_db.wp_table[i][j].type != WP_TYPE_CORN)
 313                 return NULL;
 314
 315         d = xy_norm(*xp, *yp, x, y);
 316
 317         if (d > CORN_MARGIN)
 318                 return NULL;
 319
 320         *xp = x;
 321         *yp = y;
 322
 323         return &strat_db.wp_table[i][j];
 324 }
 325
 326 /* return true if 'idx' is in group */
 327 static uint8_t is_in_group(const int8_t *group, uint8_t idx)
 328 {
 329         const int8_t *pidx;
 330         for (pidx = group; *pidx != -1; pidx++) {
 331                 if (*pidx == idx) {
 332                         return 1;
 333                 }
 334         }
 335         return 0;
 336 }
 337
 338 /* return the number of cob of that color in the group */
 339 static uint8_t count_in_group(const int8_t *group, uint8_t color)
 340 {
 341         const int8_t *pidx;
 342         struct waypoint_db *wp;
 343         uint8_t count = 0;
 344
 345         for (pidx = &group[0]; *pidx != -1; pidx++) {
 346                 wp = strat_db.corn_table[*pidx];
 347                 if (wp->corn.color == color)
 348                         count ++;
 349         }
 350         return count;
 351 }
 352
 353 /* set all unkown cobs to specified color */
 354 static void set_unknown_in_group(const int8_t *group, uint8_t color)
 355 {
 356         const int8_t *pidx;
 357         struct waypoint_db *wp;
 358
 359         for (pidx = &group[0]; *pidx != -1; pidx++) {
 360                 wp = strat_db.corn_table[*pidx];
 361                 if (wp->corn.color == I2C_COB_UNKNOWN)
 362                         wp->corn.color = color;
 363         }
 364 }
 365
 366 /* depending on which cob is set (and its color), set the color of
 367  * other cobs */
 368 static void corn_deduct_other(uint8_t idx, uint8_t color)
 369 {
 370         const int8_t **pgroup;
 371
 372         for (pgroup = &corn_groups[0]; *pgroup; pgroup++) {
 373                 if (!is_in_group(*pgroup, idx))
 374                         continue;
 375                 if (color == I2C_COB_BLACK) {
 376                         set_unknown_in_group(*pgroup, I2C_COB_WHITE);
 377                 }
 378                 else if (color == I2C_COB_WHITE) {
 379                         if (count_in_group(*pgroup, I2C_COB_UNKNOWN) == 1)
 380                                 set_unknown_in_group(*pgroup, I2C_COB_BLACK);
 381                 }
 382         }
 383 }
 384
 385 /* set color of a corn
 386  * type is I2C_COB_BLACK, I2C_COB_WHITE, I2C_COB_UNKNOWN
 387  * it will update the symetric corn if != UNKOWN
 388  * it will also deduct color of some other cobs */
 389 void corn_set_color(struct waypoint_db *wp, uint8_t color)
 390 {
 391         uint8_t symidx;
 392
 393         if (wp->corn.color != I2C_COB_UNKNOWN)
 394                 return;
 395         wp->corn.color = color;
 396         if (color == I2C_COB_UNKNOWN)
 397                 return;
 398         corn_deduct_other(wp->corn.idx, color);
 399         symidx = corn_get_sym_idx(wp->corn.idx);
 400         strat_db.corn_table[symidx]->corn.color = color;
 401         corn_deduct_other(symidx, color);
 402 }
 403
 404
 405 /* return the idx of the symetric corn */
 406 int8_t corn_get_sym_idx(int8_t i)
 407 {
 408         if (i >= CORN_NB)
 409                 return -1;
 410         return corn_sym[i];
 411 }
 412
 413 /*********** TOMATO */
 414
 415 /* return the index of a tomato given its i,j coords. */
 416 int8_t ijcoord_to_tomato_idx(uint8_t i, uint8_t j)
 417 {
 418         uint8_t n;
 419         for (n = 0; n < TOMATO_NB; n ++) {
 420                 if (i == tomato_coord_i[n] &&
 421                     j == tomato_coord_j[n])
 422                         return n;
 423         }
 424         return -1;
 425 }
 426
 427 /* return the i,j coords of a tomato given its index */
 428 int8_t tomato_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j)
 429 {
 430         if (idx >= TOMATO_NB)
 431                 return -1;
 432         *i = tomato_coord_i[idx];
 433         *j = tomato_coord_j[idx];
 434         return 0;
 435 }
 436
 437 /* return the index of a tomato given its x,y coords. */
 438 int8_t tomato_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y)
 439 {
 440         uint8_t i, j;
 441         if (tomato_idx_to_ijcoord(idx, &i, &j) < 0)
 442                 return -1;
 443         if (ijcoord_to_xycoord(i, j, x, y) < 0)
 444                 return -1;
 445         return 0;
 446 }
 447
 448 #define TOMATO_MARGIN 200
 449 /* return the index of the closest tomato at these coordinates. If the
 450  * tomato is really too far (~20cm), return NULL. The x and y pointer are
 451  * updated with the real position of the tomato */
 452 struct waypoint_db *xycoord_to_tomato_idx(int16_t *x, int16_t *y)
 453 {
 454         uint8_t idx = -1, n;
 455         int16_t d, x_tomato, y_tomato;
 456         int16_t x_tomato_min = 0, y_tomato_min = 0;
 457         int16_t d_min = 0;
 458
 459         /* XXX does it work when we are blue ? */
 460         for (n = 0; n < TOMATO_NB; n ++) {
 461                 tomato_idx_to_xycoord(n, &x_tomato, &y_tomato);
 462                 d = xy_norm(x_tomato, y_tomato, *x, *y);
 463                 if (d < TOMATO_MARGIN && (d_min == 0 || d < d_min)) {
 464                         d_min = d;
 465                         idx = n;
 466                         x_tomato_min = x_tomato;
 467                         y_tomato_min = y_tomato;
 468                 }
 469         }
 470         if (d_min == 0)
 471                 return NULL;
 472
 473         *x = x_tomato_min;
 474         *y = y_tomato_min;
 475
 476         return strat_db.tomato_table[idx];
 477 }
 478
 479 /*
 480  * Init internal database. The initialization is done with UNKNOWN
 481  * corn with all objects present
 482  */
 483 void strat_db_init(void)
 484 {
 485         struct waypoint_db *wp;
 486         int8_t idx;
 487         int8_t i, j;
 488
 489         memset(&strat_db.wp_table, 0, sizeof(strat_db.wp_table));
 490
 491         /* corn table */
 492         for (i=0; i<CORN_NB; i++) {
 493                 strat_db.corn_table[i] =
 494                         &strat_db.wp_table[corn_coord_i[i]][corn_coord_j[i]];
 495         }
 496         /* tomato table */
 497         for (i=0; i<TOMATO_NB; i++) {
 498                 strat_db.tomato_table[i] =
 499                         &strat_db.wp_table[tomato_coord_i[i]][tomato_coord_j[i]];
 500         }
 501
 502         strat_db.our_oranges_count = 6;
 503         strat_db.opp_oranges_count = 6;
 504
 505         for (i=0; i<WAYPOINTS_NBX; i++) {
 506
 507                 for (j=0; j<WAYPOINTS_NBY; j++) {
 508                         wp = &strat_db.wp_table[i][j];
 509
 510                         /* default type */
 511                         wp->type = WP_TYPE_WAYPOINT;
 512
 513                         /* */
 514                         wp->time_removed = -1;
 515
 516                         /* mark dangerous points */
 517                         if (i == 0 || i == (WAYPOINTS_NBX-1))
 518                                 wp->dangerous = 1;
 519                         if ((i & 1) == 0 && j == (WAYPOINTS_NBY-1))
 520                                 wp->dangerous = 1;
 521
 522                         /* on border, unreachable wp */
 523                         if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
 524                                 wp->type = WP_TYPE_OBSTACLE;
 525                                 continue;
 526                         }
 527
 528                         /* hill */
 529                         if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
 530                                 wp->type = WP_TYPE_OBSTACLE;
 531                                 continue;
 532                         }
 533
 534                         /* corn */
 535                         idx = early_ijcoord_to_corn_idx(i, j);
 536                         if (idx >= 0) {
 537                                 wp->type = WP_TYPE_CORN;
 538                                 wp->present = 1;
 539                                 wp->corn.idx = idx;
 540 #ifdef HOST_VERSION
 541                                 if (idx == corn_side_confs[SIDE_CONF][0] ||
 542                                     idx == corn_side_confs[SIDE_CONF][1] ||
 543                                     corn_get_sym_idx(idx) == corn_side_confs[SIDE_CONF][0] ||
 544                                     corn_get_sym_idx(idx) == corn_side_confs[SIDE_CONF][1] ||
 545                                     idx == corn_center_confs[CENTER_CONF][0] ||
 546                                     idx == corn_center_confs[CENTER_CONF][1] ||
 547                                     corn_get_sym_idx(idx) == corn_center_confs[CENTER_CONF][0] ||
 548                                     corn_get_sym_idx(idx) == corn_center_confs[CENTER_CONF][1])
 549                                         wp->corn.color = I2C_COB_BLACK;
 550                                 else
 551                                         wp->corn.color = I2C_COB_WHITE;
 552 #else
 553                                 wp->corn.color = I2C_COB_UNKNOWN;
 554 #endif
 555                                 continue;
 556                         }
 557
 558                         /* tomato */
 559                         idx = ijcoord_to_tomato_idx(i, j);
 560                         if (idx >= 0) {
 561                                 wp->type = WP_TYPE_TOMATO;
 562                                 wp->present = 1;
 563                                 wp->tomato.idx = idx;
 564                                 continue;
 565                         }
 566                 }
 567         }
 568 }
 569
 570 /* dump infos about area and objects */
 571 void strat_db_dump(const char *caller)
 572 {
 573         uint8_t i;
 574         struct waypoint_db *wp;
 575
 576         if (strat_db.dump_enabled == 0)
 577                 return;
 578
 579         printf_P(PSTR("DB dump from <%s>\r\n"), caller);
 580         for (i=0; i<CORN_NB; i++) {
 581                 wp = strat_db.corn_table[i];
 582                 printf_P(PSTR("corn%d: present=%d opp=%d "),
 583                          i, wp->present, wp->opp_visited);
 584                 if (wp->corn.color == I2C_COB_UNKNOWN)
 585                         printf_P(PSTR("unknown"));
 586                 else if (wp->corn.color == I2C_COB_BLACK)
 587                         printf_P(PSTR("black"));
 588                 else if (wp->corn.color == I2C_COB_WHITE)
 589                         printf_P(PSTR("white"));
 590                 printf_P(PSTR("\r\n"));
 591         }
 592
 593         for (i=0; i<TOMATO_NB; i++) {
 594                 wp = strat_db.tomato_table[i];
 595                 printf_P(PSTR("tomato%d: present=%d opp=%d\r\n"),
 596                          i, wp->present, wp->opp_visited);
 597         }
 598
 599         /* fill circuit infos */
 600         strat_avoid_init();
 601 }