X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_db.c;h=dfc56938a381459f44af2be808f6cca848c8a65a;hp=d6af32e188d316083ed71f4a6980fc9fabaecd4e;hb=HEAD;hpb=a3ec2f79a4ed7b2148ede881e0685ddb6f141d82

diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c
index d6af32e..dfc5693 100644
--- a/projects/microb2010/mainboard/strat_db.c
+++ b/projects/microb2010/mainboard/strat_db.c
@@ -29,6 +29,7 @@
 
 #include <aversive.h>
 #include <aversive/pgmspace.h>
+#include <aversive/error.h>
 
 #include <ax12.h>
 #include <uart.h>
@@ -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,49 @@ 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 = COLOR_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(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp)
+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;
@@ -168,7 +207,7 @@ int8_t xycoord_to_ijcoord(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp)
 	x += (STEP_CORN_X/2);
 	i = x / STEP_CORN_X;
 
-	y = COLOR_Y(y);
+	y = COLOR_Y(y); /* Y depends on color */
 	y -= OFFSET_CORN_Y;
 	if ((i & 1) == 1) {
 		j = y / STEP_CORN_Y;
@@ -200,8 +239,7 @@ int8_t xycoord_to_ijcoord(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp)
 
 /******** 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 ++) {
@@ -212,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)
 {
@@ -464,6 +510,9 @@ void strat_db_init(void)
 			/* default type */
 			wp->type = WP_TYPE_WAYPOINT;
 
+			/* */
+			wp->time_removed = -1;
+
 			/* mark dangerous points */
 			if (i == 0 || i == (WAYPOINTS_NBX-1))
 				wp->dangerous = 1;
@@ -483,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;
 			}
 
@@ -532,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();
 }