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