From 06813a0566192d90e1ad8c891dfb7cc55dedd525 Mon Sep 17 00:00:00 2001 From: zer0 Date: Thu, 22 Apr 2010 23:49:57 +0200 Subject: [PATCH] strat db + avoid --- projects/microb2010/mainboard/Makefile | 2 +- .../microb2010/mainboard/commands_mainboard.c | 12 + projects/microb2010/mainboard/strat_avoid.c | 136 ++--- projects/microb2010/mainboard/strat_db.c | 474 ++++++++++++++++++ projects/microb2010/mainboard/strat_db.h | 139 +++++ 5 files changed, 679 insertions(+), 84 deletions(-) create mode 100644 projects/microb2010/mainboard/strat_db.c create mode 100644 projects/microb2010/mainboard/strat_db.h diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile index 2ebfff2..7df1dc5 100755 --- a/projects/microb2010/mainboard/Makefile +++ b/projects/microb2010/mainboard/Makefile @@ -7,7 +7,7 @@ SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c SRC += strat_utils.c strat_base.c strat.c strat_corn.c -SRC += strat_db.c +SRC += strat_db.c strat_avoid.c ifeq ($(H),1) SRC += robotsim.c endif diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index e4eee57..e552583 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -1130,6 +1130,18 @@ struct cmd_test_result { /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { + strat_db.dump_enabled = 1; + strat_db_dump(__FUNCTION__); + + corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK); + strat_db_dump(__FUNCTION__); + + corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); + corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); + corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); } prog_char str_test_arg0[] = "test"; diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 3c128bf..e661ac1 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -58,6 +58,7 @@ #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" @@ -73,24 +74,6 @@ could be optimized in mem space: it is not needed to store the x,y coord, */ -#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 @@ -105,14 +88,13 @@ struct djpoint { 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 */ @@ -122,6 +104,7 @@ void dump(void) { int8_t i, j; struct djpoint *pt; + struct waypoint_db *wp; printf_P(PSTR(" ")); for (i=0; itype == 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(" ? ")); @@ -166,19 +156,35 @@ static inline uint8_t opposite_position(uint8_t pos) 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); @@ -212,11 +218,10 @@ static struct djpoint *get_neigh(struct djpoint *pt, 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; } @@ -252,11 +257,21 @@ int dijkstra_init(void) 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); } @@ -265,7 +280,7 @@ void dijkstra_process_neighs(struct djpoint *pt) 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); @@ -333,55 +348,10 @@ void init_djpoints(void) for (j=0; jtype = 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; iweight = 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; } } } @@ -398,14 +368,14 @@ int get_path(struct djpoint *start, struct djpoint *end) 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 */ diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c new file mode 100644 index 0000000..57c20ba --- /dev/null +++ b/projects/microb2010/mainboard/strat_db.c @@ -0,0 +1,474 @@ +/* + * 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 + */ + + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#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; itype = 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; ipresent, 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; ipresent, wp->opp_visited); + } +} diff --git a/projects/microb2010/mainboard/strat_db.h b/projects/microb2010/mainboard/strat_db.h new file mode 100644 index 0000000..1b5314f --- /dev/null +++ b/projects/microb2010/mainboard/strat_db.h @@ -0,0 +1,139 @@ +/* + * 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 + */ + +#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); -- 2.39.5