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 that is not a corn: xp and yp contains
 164  * the input and output, and ip, jp are only outputs. return 0 on
 165  * success. */
 166 int8_t xycoord_to_ijcoord(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp)
 167 {
 168         int16_t x, y;
 169         uint8_t i, j;
 170
 171         x = *xp;
 172         y = *yp;
 173
 174         x -= OFFSET_CORN_X;
 175         x += (STEP_CORN_X/2);
 176         i = x / STEP_CORN_X;
 177
 178         y = COLOR_Y(y); /* Y depends on color */
 179         y -= OFFSET_CORN_Y;
 180         if ((i & 1) == 1) {
 181                 j = y / STEP_CORN_Y;
 182         }
 183         else if ((i & 3) == 0) {
 184                 j = y / (STEP_CORN_Y*2);
 185                 j = j*2 + 1;
 186         }
 187         else {
 188                 y += (STEP_CORN_Y);
 189                 j = y / (STEP_CORN_Y*2);
 190                 j = j*2;
 191         }
 192
 193         if (ijcoord_to_xycoord(i, j, &x, &y) < 0)
 194                 return -1;
 195
 196         if (strat_db.wp_table[i][j].type != WP_TYPE_WAYPOINT &&
 197             strat_db.wp_table[i][j].type != WP_TYPE_TOMATO)
 198                 return -1;
 199
 200         *xp = x;
 201         *yp = y;
 202         *ip = i;
 203         *jp = j;
 204
 205         return 0;
 206 }
 207
 208 /******** CORN */
 209
 210 static int8_t early_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 index of a corn given its i,j coords. */
 222 int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j)
 223 {
 224         if (strat_db.wp_table[i][j].type != WP_TYPE_CORN)
 225                 return -1;
 226         return strat_db.wp_table[i][j].corn.idx;
 227 }
 228
 229 /* return the i,j coords of a corn given its index */
 230 int8_t corn_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j)
 231 {
 232         if (idx >= CORN_NB)
 233                 return -1;
 234         *i = corn_coord_i[idx];
 235         *j = corn_coord_j[idx];
 236         return 0;
 237 }
 238
 239 /* return the index of a corn given its x,y coords. */
 240 int8_t corn_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y)
 241 {
 242         uint8_t i, j;
 243         if (corn_idx_to_ijcoord(idx, &i, &j) < 0)
 244                 return -1;
 245         if (ijcoord_to_xycoord(i, j, x, y) < 0)
 246                 return -1;
 247         return 0;
 248 }
 249
 250 #define CORN_MARGIN 200
 251 /* return the index of the closest corn at these coordinates. If the
 252  * corn is really too far (~20cm), return NULL. The x and y pointer are
 253  * updated with the real position of the corn */
 254 struct waypoint_db *xycoord_to_corn_idx(int16_t *xp, int16_t *yp)
 255 {
 256         int16_t x, y;
 257         uint8_t i, j;
 258         double d;
 259
 260         x = *xp;
 261         y = *yp;
 262
 263         x -= OFFSET_CORN_X;
 264         x += STEP_CORN_X;
 265         x /= (STEP_CORN_X*2);
 266
 267         y = COLOR_Y(y);
 268         y -= OFFSET_CORN_Y;
 269         y += STEP_CORN_Y;
 270         if ((x & 1) == 1)
 271                 y -= STEP_CORN_Y;
 272         y /= (STEP_CORN_Y*2);
 273
 274         i = (x * 2);
 275         j = (y * 2) + (x & 1);
 276
 277         if (ijcoord_to_xycoord(i, j, &x, &y) < 0)
 278                 return NULL;
 279
 280         if (strat_db.wp_table[i][j].type != WP_TYPE_CORN)
 281                 return NULL;
 282
 283         d = xy_norm(*xp, *yp, x, y);
 284
 285         if (d > CORN_MARGIN)
 286                 return NULL;
 287
 288         *xp = x;
 289         *yp = y;
 290
 291         return &strat_db.wp_table[i][j];
 292 }
 293
 294 /* return true if 'idx' is in group */
 295 static uint8_t is_in_group(const int8_t *group, uint8_t idx)
 296 {
 297         const int8_t *pidx;
 298         for (pidx = group; *pidx != -1; pidx++) {
 299                 if (*pidx == idx) {
 300                         return 1;
 301                 }
 302         }
 303         return 0;
 304 }
 305
 306 /* return the number of cob of that color in the group */
 307 static uint8_t count_in_group(const int8_t *group, uint8_t color)
 308 {
 309         const int8_t *pidx;
 310         struct waypoint_db *wp;
 311         uint8_t count = 0;
 312
 313         for (pidx = &group[0]; *pidx != -1; pidx++) {
 314                 wp = strat_db.corn_table[*pidx];
 315                 if (wp->corn.color == color)
 316                         count ++;
 317         }
 318         return count;
 319 }
 320
 321 /* set all unkown cobs to specified color */
 322 static void set_unknown_in_group(const int8_t *group, uint8_t color)
 323 {
 324         const int8_t *pidx;
 325         struct waypoint_db *wp;
 326
 327         for (pidx = &group[0]; *pidx != -1; pidx++) {
 328                 wp = strat_db.corn_table[*pidx];
 329                 if (wp->corn.color == I2C_COB_UNKNOWN)
 330                         wp->corn.color = color;
 331         }
 332 }
 333
 334 /* depending on which cob is set (and its color), set the color of
 335  * other cobs */
 336 static void corn_deduct_other(uint8_t idx, uint8_t color)
 337 {
 338         const int8_t **pgroup;
 339
 340         for (pgroup = &corn_groups[0]; *pgroup; pgroup++) {
 341                 if (!is_in_group(*pgroup, idx))
 342                         continue;
 343                 if (color == I2C_COB_BLACK) {
 344                         set_unknown_in_group(*pgroup, I2C_COB_WHITE);
 345                 }
 346                 else if (color == I2C_COB_WHITE) {
 347                         if (count_in_group(*pgroup, I2C_COB_UNKNOWN) == 1)
 348                                 set_unknown_in_group(*pgroup, I2C_COB_BLACK);
 349                 }
 350         }
 351 }
 352
 353 /* set color of a corn
 354  * type is I2C_COB_BLACK, I2C_COB_WHITE, I2C_COB_UNKNOWN
 355  * it will update the symetric corn if != UNKOWN
 356  * it will also deduct color of some other cobs */
 357 void corn_set_color(struct waypoint_db *wp, uint8_t color)
 358 {
 359         uint8_t symidx;
 360
 361         if (wp->corn.color != I2C_COB_UNKNOWN)
 362                 return;
 363         wp->corn.color = color;
 364         if (color == I2C_COB_UNKNOWN)
 365                 return;
 366         corn_deduct_other(wp->corn.idx, color);
 367         symidx = corn_get_sym_idx(wp->corn.idx);
 368         strat_db.corn_table[symidx]->corn.color = color;
 369         corn_deduct_other(symidx, color);
 370 }
 371
 372
 373 /* return the idx of the symetric corn */
 374 int8_t corn_get_sym_idx(int8_t i)
 375 {
 376         if (i >= CORN_NB)
 377                 return -1;
 378         return corn_sym[i];
 379 }
 380
 381 /*********** TOMATO */
 382
 383 /* return the index of a tomato given its i,j coords. */
 384 int8_t ijcoord_to_tomato_idx(uint8_t i, uint8_t j)
 385 {
 386         uint8_t n;
 387         for (n = 0; n < TOMATO_NB; n ++) {
 388                 if (i == tomato_coord_i[n] &&
 389                     j == tomato_coord_j[n])
 390                         return n;
 391         }
 392         return -1;
 393 }
 394
 395 /* return the i,j coords of a tomato given its index */
 396 int8_t tomato_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j)
 397 {
 398         if (idx >= TOMATO_NB)
 399                 return -1;
 400         *i = tomato_coord_i[idx];
 401         *j = tomato_coord_j[idx];
 402         return 0;
 403 }
 404
 405 /* return the index of a tomato given its x,y coords. */
 406 int8_t tomato_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y)
 407 {
 408         uint8_t i, j;
 409         if (tomato_idx_to_ijcoord(idx, &i, &j) < 0)
 410                 return -1;
 411         if (ijcoord_to_xycoord(i, j, x, y) < 0)
 412                 return -1;
 413         return 0;
 414 }
 415
 416 #define TOMATO_MARGIN 200
 417 /* return the index of the closest tomato at these coordinates. If the
 418  * tomato is really too far (~20cm), return NULL. The x and y pointer are
 419  * updated with the real position of the tomato */
 420 struct waypoint_db *xycoord_to_tomato_idx(int16_t *x, int16_t *y)
 421 {
 422         uint8_t idx = -1, n;
 423         int16_t d, x_tomato, y_tomato;
 424         int16_t x_tomato_min = 0, y_tomato_min = 0;
 425         int16_t d_min = 0;
 426
 427         /* XXX does it work when we are blue ? */
 428         for (n = 0; n < TOMATO_NB; n ++) {
 429                 tomato_idx_to_xycoord(n, &x_tomato, &y_tomato);
 430                 d = xy_norm(x_tomato, y_tomato, *x, *y);
 431                 if (d < TOMATO_MARGIN && (d_min == 0 || d < d_min)) {
 432                         d_min = d;
 433                         idx = n;
 434                         x_tomato_min = x_tomato;
 435                         y_tomato_min = y_tomato;
 436                 }
 437         }
 438         if (d_min == 0)
 439                 return NULL;
 440
 441         *x = x_tomato_min;
 442         *y = y_tomato_min;
 443
 444         return strat_db.tomato_table[idx];
 445 }
 446
 447 /*
 448  * Init internal database. The initialization is done with UNKNOWN
 449  * corn with all objects present
 450  */
 451 void strat_db_init(void)
 452 {
 453         struct waypoint_db *wp;
 454         int8_t idx;
 455         int8_t i, j;
 456
 457         memset(&strat_db.wp_table, 0, sizeof(strat_db.wp_table));
 458
 459         /* corn table */
 460         for (i=0; i<CORN_NB; i++) {
 461                 strat_db.corn_table[i] =
 462                         &strat_db.wp_table[corn_coord_i[i]][corn_coord_j[i]];
 463         }
 464         /* tomato table */
 465         for (i=0; i<TOMATO_NB; i++) {
 466                 strat_db.tomato_table[i] =
 467                         &strat_db.wp_table[tomato_coord_i[i]][tomato_coord_j[i]];
 468         }
 469
 470         strat_db.our_oranges_count = 6;
 471         strat_db.opp_oranges_count = 6;
 472
 473         for (i=0; i<WAYPOINTS_NBX; i++) {
 474
 475                 for (j=0; j<WAYPOINTS_NBY; j++) {
 476                         wp = &strat_db.wp_table[i][j];
 477
 478                         /* default type */
 479                         wp->type = WP_TYPE_WAYPOINT;
 480
 481                         /* */
 482                         wp->time_removed = -1;
 483
 484                         /* mark dangerous points */
 485                         if (i == 0 || i == (WAYPOINTS_NBX-1))
 486                                 wp->dangerous = 1;
 487                         if ((i & 1) == 0 && j == (WAYPOINTS_NBY-1))
 488                                 wp->dangerous = 1;
 489
 490                         /* on border, unreachable wp */
 491                         if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
 492                                 wp->type = WP_TYPE_OBSTACLE;
 493                                 continue;
 494                         }
 495
 496                         /* hill */
 497                         if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
 498                                 wp->type = WP_TYPE_OBSTACLE;
 499                                 continue;
 500                         }
 501
 502                         /* corn */
 503                         idx = early_ijcoord_to_corn_idx(i, j);
 504                         if (idx >= 0) {
 505                                 wp->type = WP_TYPE_CORN;
 506                                 wp->present = 1;
 507                                 wp->corn.idx = idx;
 508 #ifdef HOST_VERSION
 509                                 if (idx == corn_side_confs[SIDE_CONF][0] ||
 510                                     idx == corn_side_confs[SIDE_CONF][1] ||
 511                                     corn_get_sym_idx(idx) == corn_side_confs[SIDE_CONF][0] ||
 512                                     corn_get_sym_idx(idx) == corn_side_confs[SIDE_CONF][1] ||
 513                                     idx == corn_center_confs[CENTER_CONF][0] ||
 514                                     idx == corn_center_confs[CENTER_CONF][1] ||
 515                                     corn_get_sym_idx(idx) == corn_center_confs[CENTER_CONF][0] ||
 516                                     corn_get_sym_idx(idx) == corn_center_confs[CENTER_CONF][1])
 517                                         wp->corn.color = I2C_COB_BLACK;
 518                                 else
 519                                         wp->corn.color = I2C_COB_WHITE;
 520 #else
 521                                 wp->corn.color = I2C_COB_UNKNOWN;
 522 #endif
 523                                 continue;
 524                         }
 525
 526                         /* tomato */
 527                         idx = ijcoord_to_tomato_idx(i, j);
 528                         if (idx >= 0) {
 529                                 wp->type = WP_TYPE_TOMATO;
 530                                 wp->present = 1;
 531                                 wp->tomato.idx = idx;
 532                                 continue;
 533                         }
 534                 }
 535         }
 536 }
 537
 538 /* dump infos about area and objects */
 539 void strat_db_dump(const char *caller)
 540 {
 541         uint8_t i;
 542         struct waypoint_db *wp;
 543
 544         if (strat_db.dump_enabled == 0)
 545                 return;
 546
 547         printf_P(PSTR("DB dump from <%s>\r\n"), caller);
 548         for (i=0; i<CORN_NB; i++) {
 549                 wp = strat_db.corn_table[i];
 550                 printf_P(PSTR("corn%d: present=%d opp=%d "),
 551                          i, wp->present, wp->opp_visited);
 552                 if (wp->corn.color == I2C_COB_UNKNOWN)
 553                         printf_P(PSTR("unknown"));
 554                 else if (wp->corn.color == I2C_COB_BLACK)
 555                         printf_P(PSTR("black"));
 556                 else if (wp->corn.color == I2C_COB_WHITE)
 557                         printf_P(PSTR("white"));
 558                 printf_P(PSTR("\r\n"));
 559         }
 560
 561         for (i=0; i<TOMATO_NB; i++) {
 562                 wp = strat_db.tomato_table[i];
 563                 printf_P(PSTR("tomato%d: present=%d opp=%d\r\n"),
 564                          i, wp->present, wp->opp_visited);
 565         }
 566
 567         /* fill circuit infos */
 568         strat_avoid_init();
 569 }