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);
+ *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y);
+ 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(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 -= 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;
}
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 */
{
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;
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 */
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;
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; i<CORN_NB; i++) {
/* 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;
/* tomato */
idx = ijcoord_to_tomato_idx(i, j);
if (idx >= 0) {
+ printf("%d %d\n", i, j);
wp->type = WP_TYPE_TOMATO;
wp->present = 1;
+ wp->tomato.idx = idx;
continue;
}
}