X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_db.c;h=2d3896793bcfe80201a281b9747898d2204bfd80;hp=57c20bab93ee116e44341e98b198a6db6fc51447;hb=96748eb468a27a784f148c89db593372357d9e2d;hpb=06813a0566192d90e1ad8c891dfb7cc55dedd525

diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c
index 57c20ba..2d38967 100644
--- a/projects/microb2010/mainboard/strat_db.c
+++ b/projects/microb2010/mainboard/strat_db.c
@@ -149,7 +149,7 @@ 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;
 }
 
@@ -188,6 +188,7 @@ 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 */
@@ -202,8 +203,7 @@ struct waypoint_db *xycoord_to_corn_idx(int16_t *x, int16_t *y)
 	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)) {
+		if (d < CORN_MARGIN && (d_min == 0 || d < d_min)) {
 			d_min = d;
 			idx = n;
 			x_corn_min = x_corn;
@@ -289,6 +289,8 @@ void corn_set_color(struct waypoint_db *wp, uint8_t color)
 	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;
@@ -339,6 +341,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 */
@@ -353,8 +356,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;