#include "i2c_protocol.h"
#include "main.h"
#include "strat.h"
+#include "strat_db.h"
#include "strat_base.h"
#include "strat_corn.h"
#include "strat_utils.h"
*/
-#define OFFSET_CORN_X 150
-#define OFFSET_CORN_Y 222
-#define STEP_CORN_X 225
-#define STEP_CORN_Y 250
-
-#define CORN_NB 18
-
-#define WAYPOINTS_NBX 13
-#define WAYPOINTS_NBY 8
-
-/* enum is better */
-#define TYPE_WAYPOINT 0
-#define TYPE_DANGEROUS 1
-#define TYPE_WHITE_CORN 2
-#define TYPE_BLACK_CORN 3
-#define TYPE_OBSTACLE 4
-#define TYPE_UNKNOWN 5
-
/* XXX enum possible ? else just rename */
#define START 0
#define UP 1
uint16_t weight;
struct djpoint *parent;
- uint8_t type:3;
uint8_t parent_pos:3;
uint8_t updated:1;
uint8_t todo:1;
+ uint8_t reserved:3;
};
-uint8_t corn_table[CORN_NB];
-
+/* database for dijkstra */
static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
/* return index from neigh pointer */
{
int8_t i, j;
struct djpoint *pt;
+ struct waypoint_db *wp;
printf_P(PSTR(" "));
for (i=0; i<WAYPOINTS_NBX; i++) {
for (i=0; i<WAYPOINTS_NBX; i++) {
pt = &djpoints[i][j/2];
+ wp = &strat_db.wp_table[i][j/2];
if (((i+j) & 1) == 0)
continue;
- if (pt->type == TYPE_OBSTACLE)
+ if (wp->type == WP_TYPE_OBSTACLE)
printf_P(PSTR(" X "));
- else if (pt->type == TYPE_DANGEROUS)
+ else if (wp->dangerous)
printf_P(PSTR(" D "));
- else if (pt->type == TYPE_WHITE_CORN)
+ else if (wp->type == WP_TYPE_CORN &&
+ wp->corn.color == I2C_COB_WHITE)
printf_P(PSTR(" W "));
- else if (pt->type == TYPE_BLACK_CORN)
+ else if (wp->type == WP_TYPE_CORN &&
+ wp->corn.color == I2C_COB_BLACK)
printf_P(PSTR(" B "));
- else if (pt->type == TYPE_WAYPOINT)
+ else if (wp->type == WP_TYPE_CORN &&
+ wp->corn.color == I2C_COB_UNKNOWN)
+ printf_P(PSTR(" U "));
+ else if (wp->type == WP_TYPE_WAYPOINT ||
+ wp->type == WP_TYPE_TOMATO)
printf_P(PSTR(" %5d "), pt->weight);
else
printf_P(PSTR(" ? "));
return pos;
}
+/* is point reachable by the robot ? */
+static uint8_t is_reachable(uint8_t i, uint8_t j)
+{
+ struct waypoint_db *wp;
+
+ wp = &strat_db.wp_table[i][j];
+ if (wp->type == WP_TYPE_WAYPOINT)
+ return 1;
+ if (wp->type == WP_TYPE_TOMATO)
+ return 1;
+ if (wp->type == WP_TYPE_CORN &&
+ wp->present == 0)
+ return 1;
+ return 0;
+}
+
/* return coord of the entry in the table from the pointer */
-static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
+static void djpoint2ij(struct djpoint *pt, uint8_t *i, uint8_t *j)
{
int8_t idx = PT2IDX(pt);
- *x = idx / WAYPOINTS_NBY;
- *y = idx % WAYPOINTS_NBY;
+ *i = idx / WAYPOINTS_NBY;
+ *j = idx % WAYPOINTS_NBY;
}
/* get the neighbour of the point at specified position */
static struct djpoint *get_neigh(struct djpoint *pt,
uint8_t position)
{
- int8_t i,j;
+ uint8_t i,j;
struct djpoint *neigh;
djpoint2ij(pt, &i, &j);
if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
return NULL;
- neigh = &djpoints[i][j];
-
- if (neigh->type != TYPE_WAYPOINT)
+ if (is_reachable(i, j) == 0)
return NULL;
+ neigh = &djpoints[i][j];
return neigh;
}
return 0;
}
+/* return distance between p1 and p2 */
static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
{
+ int16_t x1, y1, x2, y2;
double vx, vy;
- vx = p2->pos.x - p1->pos.x;
- vy = p2->pos.y - p1->pos.y;
+ uint8_t i, j;
+
+ djpoint2ij(p1, &i, &j);
+ ijcoord_to_xycoord(i, j, &x1, &y1);
+
+ djpoint2ij(p2, &i, &j);
+ ijcoord_to_xycoord(i, j, &x2, &y2);
+
+ vx = x2 - x1;
+ vy = y2 - y1;
return sqrt(vx * vx + vy * vy);
}
struct djpoint *neigh;
uint8_t pos, parent_pos;
uint16_t weight;
- int8_t i,j;
+ uint8_t i,j;
djpoint2ij(pt, &i, &j);
printf_P(PSTR("at %d %d:\r\n"), i, j);
for (j=0; j<WAYPOINTS_NBY; j++) {
pt = &djpoints[i][j];
-
- pt->type = TYPE_WAYPOINT;
pt->parent_pos = END;
pt->updated = 0;
pt->todo = 0;
- }
- }
-}
-
-/* update the type and weight of waypoints, before starting
- * dijkstra */
-void update_djpoints(void)
-{
- int8_t i, j, c;
- struct djpoint *pt;
-
- for (i=0; i<WAYPOINTS_NBX; i++) {
-
- for (j=0; j<WAYPOINTS_NBY; j++) {
- pt = &djpoints[i][j];
-
pt->weight = 0;
-
- /* corn */
- c = ijcoord_to_corn_idx(i, j);
- if (c >= 0) {
- pt->type = corn_table[c];
- continue;
- }
- /* too close of border */
- if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
- pt->type = TYPE_OBSTACLE;
- continue;
- }
- /* hill */
- if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
- pt->type = TYPE_OBSTACLE;
- continue;
- }
- /* dangerous points */
- if (i == 0 || i == (WAYPOINTS_NBX-1)) {
- pt->type = TYPE_DANGEROUS;
- continue;
- }
- if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
- pt->type = TYPE_DANGEROUS;
- continue;
- }
- pt->type = TYPE_WAYPOINT;
}
}
}
cur = get_neigh(cur, cur->parent_pos)) {
if (prev_direction != cur->parent_pos) {
idx = PT2IDX(cur);
- corn_idx_to_coordxy(idx, &x, &y);
+ corn_idx_to_xycoord(idx, &x, &y);
printf_P(PSTR("%d %d (%d)\r\n"),
x, y, cur->parent_pos);
}
prev_direction = cur->parent_pos;
}
idx = PT2IDX(end);
- corn_idx_to_coordxy(idx, &x, &y);
+ corn_idx_to_xycoord(idx, &x, &y);
printf_P(PSTR("%d %d\r\n"), x, y);
return 0; /* XXX */
--- /dev/null
+/*
+ * Copyright Droids, Microb Technology (2010)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <math.h>
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <clock_time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
+#include <trajectory_manager_core.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <diagnostic.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "../common/i2c_commands.h"
+#include "i2c_protocol.h"
+#include "main.h"
+#include "strat.h"
+#include "strat_base.h"
+#include "strat_corn.h"
+#include "strat_db.h"
+#include "strat_utils.h"
+#include "sensor.h"
+#include "actuator.h"
+
+/* status of objects on area */
+struct strat_db strat_db;
+
+/* given an index, give the i coord */
+static const uint8_t corn_coord_i[CORN_NB] = {
+ 0, 0, 0, 2, 2, 2, 4, 4, 6,
+ 6, 8, 8, 10, 10, 10, 12, 12, 12,
+};
+
+/* given an index, give the j coord */
+static const uint8_t corn_coord_j[CORN_NB] = {
+ 2, 4, 6, 3, 5, 7, 4, 6, 5,
+ 7, 4, 6, 3, 5, 7, 2, 4, 6,
+};
+
+/* table to find the symetric idx */
+static const uint8_t corn_sym[] = {
+ 15, 16, 17, 12, 13, 14, 10, 11,
+ 8, 9, 6, 7, 3, 4, 5, 0, 1, 2
+};
+
+#if 0 /* XXX maybe useless */
+/* the 10 possible configurations for corn on the side */
+static const uint8_t corn_side_confs[9][2] = {
+ { 1, 4 },
+ { 0, 4 },
+ { 2, 4 },
+ { 2, 3 },
+ { 0, 3 },
+ { 1, 3 },
+ { 1, 6 },
+ { 0, 6 },
+ { 2, 6 },
+};
+
+/* the 4 possible configurations for corn on center */
+static const uint8_t corn_center_confs[4][2] = {
+ { 5, 8 },
+ { 7, 8 },
+ { 5, 9 },
+ { 7, 8 },
+};
+#endif
+
+/* in these groups, only one black cob */
+static const int8_t corn_group1[] = { 0, 1, 2, -1, };
+static const int8_t corn_group2[] = { 3, 4, 6, -1, };
+static const int8_t corn_group3[] = { 5, 7, -1, };
+static const int8_t corn_group4[] = { 8, 9, -1, };
+static const int8_t corn_group5[] = { 11, 14, -1, };
+static const int8_t corn_group6[] = { 10, 12, 13, -1, };
+static const int8_t corn_group7[] = { 15, 16, 17, -1, };
+
+static const int8_t *corn_groups[] = {
+ corn_group1,
+ corn_group2,
+ corn_group3,
+ corn_group4,
+ corn_group5,
+ corn_group6,
+ corn_group7,
+ NULL,
+};
+
+/* given an index, give the i coord */
+static const uint8_t tomato_coord_i[TOMATO_NB] = {
+ 0, 0, 2, 2, 4, 4, 6, 6,
+ 8, 8, 10, 10, 12, 12,
+};
+
+/* given an index, give the j coord */
+static const uint8_t tomato_coord_j[TOMATO_NB] = {
+ 3, 5, 4, 6, 5, 7, 4, 6, 5, 7, 4, 6, 3, 5,
+};
+
+/******** Generic waypoint */
+
+/* return the xy coords of a waypoint given its ij coords. */
+int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y)
+{
+ if (i >= WAYPOINTS_NBX && j >= WAYPOINTS_NBY)
+ return -1;
+ *x = (OFFSET_CORN_X + i*STEP_CORN_X);
+ *y = (OFFSET_CORN_Y + j*STEP_CORN_Y);
+ return 0;
+}
+
+/******** CORN */
+
+/* return the index of a corn given its i,j coords. */
+int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j)
+{
+ uint8_t n;
+ for (n = 0; n < CORN_NB; n ++) {
+ if (i == corn_coord_i[n] &&
+ j == corn_coord_j[n])
+ return n;
+ }
+ return -1;
+}
+
+/* return the i,j coords of a corn given its index */
+int8_t corn_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j)
+{
+ if (idx >= CORN_NB)
+ return -1;
+ *i = corn_coord_i[idx];
+ *j = corn_coord_j[idx];
+ return 0;
+}
+
+/* return the index of a corn given its x,y coords. */
+int8_t corn_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y)
+{
+ uint8_t i, j;
+ if (corn_idx_to_ijcoord(idx, &i, &j) < 0)
+ return -1;
+ if (ijcoord_to_xycoord(i, j, x, y) < 0)
+ return -1;
+ return 0;
+}
+
+/* return the index of the closest corn at these coordinates. If the
+ * corn is really too far (~20cm), return NULL. The x and y pointer are
+ * updated with the real position of the corn */
+struct waypoint_db *xycoord_to_corn_idx(int16_t *x, int16_t *y)
+{
+ uint8_t idx = -1, n;
+ int16_t d, x_corn, y_corn;
+ int16_t x_corn_min = 0, y_corn_min = 0;
+ int16_t d_min = 0;
+
+ /* XXX does it work when we are blue ? */
+ for (n = 0; n < CORN_NB; n ++) {
+ corn_idx_to_xycoord(n, &x_corn, &y_corn);
+ d = xy_norm(x_corn, y_corn, *x, *y);
+ /* XXX 200 -> constant */
+ if (d < 200 && (d_min == 0 || d < d_min)) {
+ d_min = d;
+ idx = n;
+ x_corn_min = x_corn;
+ y_corn_min = y_corn;
+ }
+ }
+ if (d_min == 0)
+ return NULL;
+
+ *x = x_corn_min;
+ *y = y_corn_min;
+
+ return strat_db.corn_table[idx];
+}
+
+/* return true if 'idx' is in group */
+static uint8_t is_in_group(const int8_t *group, uint8_t idx)
+{
+ const int8_t *pidx;
+ for (pidx = group; *pidx != -1; pidx++) {
+ if (*pidx == idx) {
+ return 1;
+ }
+ }
+ return 0;
+}
+
+/* return the number of cob of that color in the group */
+static uint8_t count_in_group(const int8_t *group, uint8_t color)
+{
+ const int8_t *pidx;
+ struct waypoint_db *wp;
+ uint8_t count = 0;
+
+ for (pidx = &group[0]; *pidx != -1; pidx++) {
+ wp = strat_db.corn_table[*pidx];
+ if (wp->corn.color == color)
+ count ++;
+ }
+ return count;
+}
+
+/* set all unkown cobs to specified color */
+static void set_unknown_in_group(const int8_t *group, uint8_t color)
+{
+ const int8_t *pidx;
+ struct waypoint_db *wp;
+
+ for (pidx = &group[0]; *pidx != -1; pidx++) {
+ wp = strat_db.corn_table[*pidx];
+ if (wp->corn.color == I2C_COB_UNKNOWN)
+ wp->corn.color = color;
+ }
+}
+
+/* depending on which cob is set (and its color), set the color of
+ * other cobs */
+static void corn_deduct_other(uint8_t idx, uint8_t color)
+{
+ const int8_t **pgroup;
+
+ for (pgroup = &corn_groups[0]; *pgroup; pgroup++) {
+ if (!is_in_group(*pgroup, idx))
+ continue;
+ if (color == I2C_COB_BLACK) {
+ set_unknown_in_group(*pgroup, I2C_COB_WHITE);
+ }
+ else if (color == I2C_COB_WHITE) {
+ if (count_in_group(*pgroup, I2C_COB_UNKNOWN) == 1)
+ set_unknown_in_group(*pgroup, I2C_COB_BLACK);
+ }
+ }
+}
+
+/* set color of a corn
+ * type is I2C_COB_BLACK, I2C_COB_WHITE, I2C_COB_UNKNOWN
+ * it will update the symetric corn if != UNKOWN
+ * it will also deduct color of some other cobs */
+void corn_set_color(struct waypoint_db *wp, uint8_t color)
+{
+ uint8_t symidx;
+
+ wp->corn.color = color;
+ if (color == I2C_COB_UNKNOWN)
+ return;
+ corn_deduct_other(wp->corn.idx, color);
+ symidx = corn_get_sym_idx(wp->corn.idx);
+ strat_db.corn_table[symidx]->corn.color = color;
+ corn_deduct_other(symidx, color);
+}
+
+
+/* return the idx of the symetric corn */
+int8_t corn_get_sym_idx(int8_t i)
+{
+ if (i >= CORN_NB)
+ return -1;
+ return corn_sym[i];
+}
+
+/*********** TOMATO */
+
+/* return the index of a tomato given its i,j coords. */
+int8_t ijcoord_to_tomato_idx(uint8_t i, uint8_t j)
+{
+ uint8_t n;
+ for (n = 0; n < TOMATO_NB; n ++) {
+ if (i == tomato_coord_i[n] &&
+ j == tomato_coord_j[n])
+ return n;
+ }
+ return -1;
+}
+
+/* return the i,j coords of a tomato given its index */
+int8_t tomato_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j)
+{
+ if (idx >= TOMATO_NB)
+ return -1;
+ *i = tomato_coord_i[idx];
+ *j = tomato_coord_j[idx];
+ return 0;
+}
+
+/* return the index of a tomato given its x,y coords. */
+int8_t tomato_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y)
+{
+ uint8_t i, j;
+ if (tomato_idx_to_ijcoord(idx, &i, &j) < 0)
+ return -1;
+ if (ijcoord_to_xycoord(i, j, x, y) < 0)
+ return -1;
+ return 0;
+}
+
+/* return the index of the closest tomato at these coordinates. If the
+ * tomato is really too far (~20cm), return NULL. The x and y pointer are
+ * updated with the real position of the tomato */
+struct waypoint_db *xycoord_to_tomato_idx(int16_t *x, int16_t *y)
+{
+ uint8_t idx = -1, n;
+ int16_t d, x_tomato, y_tomato;
+ int16_t x_tomato_min = 0, y_tomato_min = 0;
+ int16_t d_min = 0;
+
+ /* XXX does it work when we are blue ? */
+ for (n = 0; n < TOMATO_NB; n ++) {
+ tomato_idx_to_xycoord(n, &x_tomato, &y_tomato);
+ d = xy_norm(x_tomato, y_tomato, *x, *y);
+ /* XXX 200 -> constant */
+ if (d < 200 && (d_min == 0 || d < d_min)) {
+ d_min = d;
+ idx = n;
+ x_tomato_min = x_tomato;
+ y_tomato_min = y_tomato;
+ }
+ }
+ if (d_min == 0)
+ return NULL;
+
+ *x = x_tomato_min;
+ *y = y_tomato_min;
+
+ return strat_db.tomato_table[idx];
+}
+
+/*
+ * Init internal database. The initialization is done with UNKNOWN
+ * corn with all objects present
+ */
+void strat_db_init(void)
+{
+ struct waypoint_db *wp;
+ int8_t idx;
+ int8_t i, j;
+
+ memset(&strat_db, 0, sizeof(strat_db));
+
+ /* corn table */
+ for (i=0; i<CORN_NB; i++) {
+ strat_db.corn_table[i] =
+ &strat_db.wp_table[corn_coord_i[i]][corn_coord_j[i]];
+ }
+ /* tomato table */
+ for (i=0; i<TOMATO_NB; i++) {
+ strat_db.tomato_table[i] =
+ &strat_db.wp_table[tomato_coord_i[i]][tomato_coord_j[i]];
+ }
+
+ strat_db.our_oranges_count = 6;
+ strat_db.opp_oranges_count = 6;
+
+ for (i=0; i<WAYPOINTS_NBX; i++) {
+
+ for (j=0; j<WAYPOINTS_NBY; j++) {
+ wp = &strat_db.wp_table[i][j];
+
+ /* default type */
+ wp->type = WP_TYPE_WAYPOINT;
+
+ /* mark dangerous points */
+ if (i == 0 || i == (WAYPOINTS_NBX-1))
+ wp->dangerous = 1;
+ if ((i&1) == 0 && j == (WAYPOINTS_NBY-1))
+ wp->dangerous = 1;
+
+ /* too close of border, unreachable wp */
+ if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
+ wp->type = WP_TYPE_OBSTACLE;
+ continue;
+ }
+
+ /* hill */
+ if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
+ wp->type = WP_TYPE_OBSTACLE;
+ continue;
+ }
+
+ /* corn */
+ idx = ijcoord_to_corn_idx(i, j);
+ if (idx >= 0) {
+ wp->type = WP_TYPE_CORN;
+ wp->present = 1;
+ wp->corn.idx = idx;
+ wp->corn.color = I2C_COB_UNKNOWN;
+ continue;
+ }
+
+ /* tomato */
+ idx = ijcoord_to_tomato_idx(i, j);
+ if (idx >= 0) {
+ wp->type = WP_TYPE_TOMATO;
+ wp->present = 1;
+ continue;
+ }
+ }
+ }
+}
+
+/* dump infos about area and objects */
+void strat_db_dump(const char *caller)
+{
+ uint8_t i;
+ struct waypoint_db *wp;
+
+ if (strat_db.dump_enabled == 0)
+ return;
+
+ printf_P(PSTR("DB dump from <%s>\r\n"), caller);
+ for (i=0; i<CORN_NB; i++) {
+ wp = strat_db.corn_table[i];
+ printf_P(PSTR("corn%d: present=%d opp=%d "),
+ i, wp->present, wp->opp_visited);
+ if (wp->corn.color == I2C_COB_UNKNOWN)
+ printf_P(PSTR("unknown"));
+ else if (wp->corn.color == I2C_COB_BLACK)
+ printf_P(PSTR("black"));
+ else if (wp->corn.color == I2C_COB_WHITE)
+ printf_P(PSTR("white"));
+ printf_P(PSTR("\r\n"));
+ }
+
+ for (i=0; i<TOMATO_NB; i++) {
+ wp = strat_db.tomato_table[i];
+ printf_P(PSTR("tomato%d: present=%d opp=%d\r\n"),
+ i, wp->present, wp->opp_visited);
+ }
+}
--- /dev/null
+/*
+ * Copyright Droids, Microb Technology (2010)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#define WAYPOINTS_NBX 13
+#define WAYPOINTS_NBY 8
+
+struct corn_db {
+ /* I2C_COB_UNKNOWN, I2C_COB_WHITE, I2C_COB_BLACK */
+ uint8_t color:2;
+ uint8_t reserved:6;
+
+ /* index in corn table */
+ uint8_t idx;
+};
+
+struct tomato_db {
+ /* nothing for now */
+};
+
+/* structure describing the status of a waypoint */
+struct waypoint_db {
+ /* type of the waypoint */
+#define WP_TYPE_WAYPOINT 0 /* no object on it */
+#define WP_TYPE_OBSTACLE 1 /* cannot reach this point */
+#define WP_TYPE_TOMATO 2 /* place for a tomato */
+#define WP_TYPE_CORN 3 /* place for a corn */
+ uint8_t type:2;
+
+ /* true if point is near the border */
+ uint8_t dangerous:1;
+
+ /* true if element is present */
+ uint8_t present:1;
+
+ /* visited by opponent */
+ uint8_t opp_visited:1;
+
+ uint8_t reserved:3;
+
+ /* absolute position of the waypoint */
+/* int16_t x; */
+/* int16_t y; */
+
+ union {
+ struct corn_db corn;
+ struct tomato_db tomato;
+ };
+};
+
+/* database reflecting the status of objects on area */
+struct strat_db {
+ uint8_t dump_enabled;
+
+ /* table of waypoints */
+ struct waypoint_db wp_table[WAYPOINTS_NBX][WAYPOINTS_NBY];
+
+ /* corn_table: pointers to waypoints */
+ struct waypoint_db *corn_table[CORN_NB];
+
+ /* tomato_table: pointers to waypoints */
+ struct waypoint_db *tomato_table[TOMATO_NB];
+
+ /* number of oranges remaining */
+ uint8_t our_oranges_count;
+ uint8_t opp_oranges_count;
+};
+
+/* global structure storing the database */
+extern struct strat_db strat_db;
+
+/* convert i,j coords to x,y coords */
+int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y);
+
+/* return the index of a corn given its i,j coords. */
+int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j);
+
+/* return the i,j coords of a corn given its index */
+int8_t corn_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j);
+
+/* return the index of a corn given its x,y coords. */
+int8_t corn_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y);
+
+/* return the index of the closest corn at these coordinates. If the
+ * corn is really too far (~20cm), return NULL. The x and y pointer are
+ * updated with the real position of the corn */
+struct waypoint_db *xycoord_to_corn_idx(int16_t *x, int16_t *y);
+
+/* set color of a corn
+ * type is I2C_COB_BLACK, I2C_COB_WHITE, I2C_COB_UNKNOWN
+ * it will update the symetric corn if != UNKOWN
+ * it will also deduct color of some other cobs */
+void corn_set_color(struct waypoint_db *wp, uint8_t color);
+
+/* return the idx of the symetric corn */
+int8_t corn_get_sym_idx(int8_t i);
+
+/*********** TOMATO */
+
+/* return the index of a tomato given its i,j coords. */
+int8_t ijcoord_to_tomato_idx(uint8_t i, uint8_t j);
+
+/* return the i,j coords of a tomato given its index */
+int8_t tomato_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j);
+
+/* return the index of a tomato given its x,y coords. */
+int8_t tomato_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y);
+
+/* return the index of the closest tomato at these coordinates. If the
+ * tomato is really too far (~20cm), return NULL. The x and y pointer are
+ * updated with the real position of the tomato */
+struct waypoint_db *xycoord_to_tomato_idx(int16_t *x, int16_t *y);
+
+/*
+ * Init internal database. The initialization is done with UNKNOWN
+ * corn with all objects present
+ */
+void strat_db_init(void);
+
+/* dump infos about area and objects */
+void strat_db_dump(const char *caller);
git.droids-corp.org / aversive.git / commitdiff