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