X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_db.c;h=d2835fe76a6022a247bbb49a367bba92d564686a;hp=18db6ea1f2c576d3da816931228e25f0cc8cf914;hb=17aadc4c8c3e60c2b5e6bbba91c8542849addbd7;hpb=d776dc0dab6dd3ddf92041f1ad42e521847ea9cf

diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c
index 18db6ea..d2835fe 100644
--- a/projects/microb2010/mainboard/strat_db.c
+++ b/projects/microb2010/mainboard/strat_db.c
@@ -149,10 +149,52 @@ 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);
+	*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 {
+		y += (STEP_CORN_Y/2);
+		y /= (STEP_CORN_Y*2);
+		j = (y * 2) + 1;
+	}
+
+	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. */
@@ -188,35 +230,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 +341,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 +396,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 +411,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 +437,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; i<CORN_NB; i++) {
@@ -409,10 +464,10 @@ void strat_db_init(void)
 			/* 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;
@@ -437,8 +492,10 @@ void strat_db_init(void)
 			/* 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;
 			}
 		}