2 * Copyright Droids, Microb Technology (2010)
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.
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.
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
18 * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
31 #include <aversive/pgmspace.h>
32 #include <aversive/error.h>
37 #include <clock_time.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>
49 #include <obstacle_avoidance.h>
50 #include <blocking_detection_manager.h>
51 #include <robot_system.h>
52 #include <position_manager.h>
54 #include <diagnostic.h>
59 #include "../common/i2c_commands.h"
60 #include "i2c_protocol.h"
63 #include "strat_base.h"
64 #include "strat_avoid.h"
65 #include "strat_corn.h"
67 #include "strat_utils.h"
71 /* status of objects on area */
72 struct strat_db strat_db;
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,
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,
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
95 /* the 10 possible configurations for corn on the side */
96 static const uint8_t corn_side_confs[9][2] = {
108 /* the 4 possible configurations for corn on center */
109 static const uint8_t corn_center_confs[4][2] = {
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, };
126 static const int8_t *corn_groups[] = {
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,
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,
148 /******** Generic waypoint */
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)
153 if (i >= WAYPOINTS_NBX && j >= WAYPOINTS_NBY)
155 *x = (OFFSET_CORN_X + i*STEP_CORN_X);
157 *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y + STEP_CORN_Y/2);
159 *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y);
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)
173 x += (STEP_CORN_X/2);
176 y = COLOR_Y(y); /* Y depends on color */
184 if (ijcoord_to_xycoord(i, j, &x, &y) < 0)
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
198 int8_t xycoord_to_ijcoord_not_corn(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp)
207 x += (STEP_CORN_X/2);
210 y = COLOR_Y(y); /* Y depends on color */
215 else if ((i & 3) == 0) {
216 j = y / (STEP_CORN_Y*2);
221 j = y / (STEP_CORN_Y*2);
225 if (ijcoord_to_xycoord(i, j, &x, &y) < 0)
228 if (strat_db.wp_table[i][j].type != WP_TYPE_WAYPOINT &&
229 strat_db.wp_table[i][j].type != WP_TYPE_TOMATO)
242 static int8_t early_ijcoord_to_corn_idx(uint8_t i, uint8_t j)
245 for (n = 0; n < CORN_NB; n ++) {
246 if (i == corn_coord_i[n] &&
247 j == corn_coord_j[n])
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)
256 if (strat_db.wp_table[i][j].type != WP_TYPE_CORN)
258 return strat_db.wp_table[i][j].corn.idx;
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)
266 *i = corn_coord_i[idx];
267 *j = corn_coord_j[idx];
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)
275 if (corn_idx_to_ijcoord(idx, &i, &j) < 0)
277 if (ijcoord_to_xycoord(i, j, x, y) < 0)
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)
297 x /= (STEP_CORN_X*2);
304 y /= (STEP_CORN_Y*2);
307 j = (y * 2) + (x & 1);
309 if (ijcoord_to_xycoord(i, j, &x, &y) < 0)
312 if (strat_db.wp_table[i][j].type != WP_TYPE_CORN)
315 d = xy_norm(*xp, *yp, x, y);
323 return &strat_db.wp_table[i][j];
326 /* return true if 'idx' is in group */
327 static uint8_t is_in_group(const int8_t *group, uint8_t idx)
330 for (pidx = group; *pidx != -1; pidx++) {
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)
342 struct waypoint_db *wp;
345 for (pidx = &group[0]; *pidx != -1; pidx++) {
346 wp = strat_db.corn_table[*pidx];
347 if (wp->corn.color == color)
353 /* set all unkown cobs to specified color */
354 static void set_unknown_in_group(const int8_t *group, uint8_t color)
357 struct waypoint_db *wp;
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;
366 /* depending on which cob is set (and its color), set the color of
368 static void corn_deduct_other(uint8_t idx, uint8_t color)
370 const int8_t **pgroup;
372 for (pgroup = &corn_groups[0]; *pgroup; pgroup++) {
373 if (!is_in_group(*pgroup, idx))
375 if (color == I2C_COB_BLACK) {
376 set_unknown_in_group(*pgroup, I2C_COB_WHITE);
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);
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)
393 if (wp->corn.color != I2C_COB_UNKNOWN)
395 wp->corn.color = color;
396 if (color == I2C_COB_UNKNOWN)
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);
405 /* return the idx of the symetric corn */
406 int8_t corn_get_sym_idx(int8_t i)
413 /*********** TOMATO */
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)
419 for (n = 0; n < TOMATO_NB; n ++) {
420 if (i == tomato_coord_i[n] &&
421 j == tomato_coord_j[n])
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)
430 if (idx >= TOMATO_NB)
432 *i = tomato_coord_i[idx];
433 *j = tomato_coord_j[idx];
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)
441 if (tomato_idx_to_ijcoord(idx, &i, &j) < 0)
443 if (ijcoord_to_xycoord(i, j, x, y) < 0)
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)
455 int16_t d, x_tomato, y_tomato;
456 int16_t x_tomato_min = 0, y_tomato_min = 0;
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)) {
466 x_tomato_min = x_tomato;
467 y_tomato_min = y_tomato;
476 return strat_db.tomato_table[idx];
480 * Init internal database. The initialization is done with UNKNOWN
481 * corn with all objects present
483 void strat_db_init(void)
485 struct waypoint_db *wp;
489 memset(&strat_db.wp_table, 0, sizeof(strat_db.wp_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]];
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]];
502 strat_db.our_oranges_count = 6;
503 strat_db.opp_oranges_count = 6;
505 for (i=0; i<WAYPOINTS_NBX; i++) {
507 for (j=0; j<WAYPOINTS_NBY; j++) {
508 wp = &strat_db.wp_table[i][j];
511 wp->type = WP_TYPE_WAYPOINT;
514 wp->time_removed = -1;
516 /* mark dangerous points */
517 if (i == 0 || i == (WAYPOINTS_NBX-1))
519 if ((i & 1) == 0 && j == (WAYPOINTS_NBY-1))
522 /* on border, unreachable wp */
523 if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
524 wp->type = WP_TYPE_OBSTACLE;
529 if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
530 wp->type = WP_TYPE_OBSTACLE;
535 idx = early_ijcoord_to_corn_idx(i, j);
537 wp->type = WP_TYPE_CORN;
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;
551 wp->corn.color = I2C_COB_WHITE;
553 wp->corn.color = I2C_COB_UNKNOWN;
559 idx = ijcoord_to_tomato_idx(i, j);
561 wp->type = WP_TYPE_TOMATO;
563 wp->tomato.idx = idx;
570 /* dump infos about area and objects */
571 void strat_db_dump(const char *caller)
574 struct waypoint_db *wp;
576 if (strat_db.dump_enabled == 0)
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"));
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);
599 /* fill circuit infos */