X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_db.c;h=dfc56938a381459f44af2be808f6cca848c8a65a;hp=18db6ea1f2c576d3da816931228e25f0cc8cf914;hb=HEAD;hpb=d776dc0dab6dd3ddf92041f1ad42e521847ea9cf diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c index 18db6ea..dfc5693 100644 --- a/projects/microb2010/mainboard/strat_db.c +++ b/projects/microb2010/mainboard/strat_db.c @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -60,6 +61,7 @@ #include "main.h" #include "strat.h" #include "strat_base.h" +#include "strat_avoid.h" #include "strat_corn.h" #include "strat_db.h" #include "strat_utils.h" @@ -87,7 +89,9 @@ static const uint8_t corn_sym[] = { 8, 9, 6, 7, 3, 4, 5, 0, 1, 2 }; -#if 0 /* XXX maybe useless */ +#ifdef HOST_VERSION +#define SIDE_CONF 0 +#define CENTER_CONF 0 /* the 10 possible configurations for corn on the side */ static const uint8_t corn_side_confs[9][2] = { { 1, 4 }, @@ -149,14 +153,93 @@ 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); + if (i&1) + *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y + STEP_CORN_Y/2); + else + *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y); + return 0; +} + +/* return the nearest waypoint (any type) */ +int8_t xycoord_to_ijcoord(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp) +{ + int16_t x, y; + uint8_t i, j; + + x = *xp; + y = *yp; + + x -= OFFSET_CORN_X; + x += (STEP_CORN_X/2); + i = x / STEP_CORN_X; + + y = COLOR_Y(y); /* Y depends on color */ + y -= OFFSET_CORN_Y; + y += STEP_CORN_Y/2; + + if ((i & 1) == 1) + y -= STEP_CORN_Y/2; + j = y / STEP_CORN_Y; + + if (ijcoord_to_xycoord(i, j, &x, &y) < 0) + return -1; + + *xp = x; + *yp = y; + *ip = i; + *jp = j; + + return 0; +} + +/* return the nearest waypoint that is not a corn: xp and yp contains + * the input and output, and ip, jp are only outputs. return 0 on + * success. */ +int8_t xycoord_to_ijcoord_not_corn(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp) +{ + int16_t x, y; + uint8_t i, j; + + x = *xp; + y = *yp; + + x -= OFFSET_CORN_X; + x += (STEP_CORN_X/2); + i = x / STEP_CORN_X; + + y = COLOR_Y(y); /* Y depends on color */ + y -= OFFSET_CORN_Y; + if ((i & 1) == 1) { + j = y / STEP_CORN_Y; + } + else if ((i & 3) == 0) { + j = y / (STEP_CORN_Y*2); + j = j*2 + 1; + } + else { + y += (STEP_CORN_Y); + j = y / (STEP_CORN_Y*2); + j = j*2; + } + + if (ijcoord_to_xycoord(i, j, &x, &y) < 0) + return -1; + + if (strat_db.wp_table[i][j].type != WP_TYPE_WAYPOINT && + strat_db.wp_table[i][j].type != WP_TYPE_TOMATO) + return -1; + + *xp = x; + *yp = y; + *ip = i; + *jp = j; + 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) +static int8_t early_ijcoord_to_corn_idx(uint8_t i, uint8_t j) { uint8_t n; for (n = 0; n < CORN_NB; n ++) { @@ -167,6 +250,14 @@ int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j) return -1; } +/* return the index of a corn given its i,j coords. */ +int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j) +{ + if (strat_db.wp_table[i][j].type != WP_TYPE_CORN) + return -1; + return strat_db.wp_table[i][j].corn.idx; +} + /* 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) { @@ -188,35 +279,48 @@ int8_t corn_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y) return 0; } +#define CORN_MARGIN 200 /* 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) +struct waypoint_db *xycoord_to_corn_idx(int16_t *xp, int16_t *yp) { - 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; + int16_t x, y; + uint8_t i, j; + double d; - /* 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) + x = *xp; + y = *yp; + + x -= OFFSET_CORN_X; + x += STEP_CORN_X; + x /= (STEP_CORN_X*2); + + y = COLOR_Y(y); + y -= OFFSET_CORN_Y; + y += STEP_CORN_Y; + if ((x & 1) == 1) + y -= STEP_CORN_Y; + y /= (STEP_CORN_Y*2); + + i = (x * 2); + j = (y * 2) + (x & 1); + + if (ijcoord_to_xycoord(i, j, &x, &y) < 0) + return NULL; + + if (strat_db.wp_table[i][j].type != WP_TYPE_CORN) + return NULL; + + d = xy_norm(*xp, *yp, x, y); + + if (d > CORN_MARGIN) return NULL; - *x = x_corn_min; - *y = y_corn_min; + *xp = x; + *yp = y; - return strat_db.corn_table[idx]; + return &strat_db.wp_table[i][j]; } /* return true if 'idx' is in group */ @@ -286,11 +390,11 @@ void corn_set_color(struct waypoint_db *wp, uint8_t color) { uint8_t symidx; + if (wp->corn.color != I2C_COB_UNKNOWN) + return; wp->corn.color = color; if (color == I2C_COB_UNKNOWN) return; - if (wp->corn.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; @@ -341,6 +445,7 @@ int8_t tomato_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y) return 0; } +#define TOMATO_MARGIN 200 /* 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 */ @@ -355,8 +460,7 @@ struct waypoint_db *xycoord_to_tomato_idx(int16_t *x, int16_t *y) 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)) { + if (d < TOMATO_MARGIN && (d_min == 0 || d < d_min)) { d_min = d; idx = n; x_tomato_min = x_tomato; @@ -382,7 +486,7 @@ void strat_db_init(void) int8_t idx; int8_t i, j; - memset(&strat_db, 0, sizeof(strat_db)); + memset(&strat_db.wp_table, 0, sizeof(strat_db.wp_table)); /* corn table */ for (i=0; itype = WP_TYPE_WAYPOINT; + /* */ + wp->time_removed = -1; + /* mark dangerous points */ if (i == 0 || i == (WAYPOINTS_NBX-1)) wp->dangerous = 1; - if ((i&1) == 0 && j == (WAYPOINTS_NBY-1)) + if ((i & 1) == 0 && j == (WAYPOINTS_NBY-1)) wp->dangerous = 1; - /* too close of border, unreachable wp */ + /* on border, unreachable wp */ if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { wp->type = WP_TYPE_OBSTACLE; continue; @@ -425,12 +532,26 @@ void strat_db_init(void) } /* corn */ - idx = ijcoord_to_corn_idx(i, j); + idx = early_ijcoord_to_corn_idx(i, j); if (idx >= 0) { wp->type = WP_TYPE_CORN; wp->present = 1; wp->corn.idx = idx; +#ifdef HOST_VERSION + if (idx == corn_side_confs[SIDE_CONF][0] || + idx == corn_side_confs[SIDE_CONF][1] || + corn_get_sym_idx(idx) == corn_side_confs[SIDE_CONF][0] || + corn_get_sym_idx(idx) == corn_side_confs[SIDE_CONF][1] || + idx == corn_center_confs[CENTER_CONF][0] || + idx == corn_center_confs[CENTER_CONF][1] || + corn_get_sym_idx(idx) == corn_center_confs[CENTER_CONF][0] || + corn_get_sym_idx(idx) == corn_center_confs[CENTER_CONF][1]) + wp->corn.color = I2C_COB_BLACK; + else + wp->corn.color = I2C_COB_WHITE; +#else wp->corn.color = I2C_COB_UNKNOWN; +#endif continue; } @@ -439,6 +560,7 @@ void strat_db_init(void) if (idx >= 0) { wp->type = WP_TYPE_TOMATO; wp->present = 1; + wp->tomato.idx = idx; continue; } } @@ -473,4 +595,7 @@ void strat_db_dump(const char *caller) printf_P(PSTR("tomato%d: present=%d opp=%d\r\n"), i, wp->present, wp->opp_visited); } + + /* fill circuit infos */ + strat_avoid_init(); }