From: zer0 <zer0@carbon.local>
Date: Sun, 2 May 2010 22:43:48 +0000 (+0200)
Subject: circuit strat, first version
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=17aadc4c8c3e60c2b5e6bbba91c8542849addbd7;p=aversive.git

circuit strat, first version
---

diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index fc51e8e..52b6b92 100644
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -870,6 +870,8 @@ static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
 	printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
 	printf_P(PSTR("status = %x\r\n"), ballboard.status);
 	printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
+	printf_P(PSTR("lcob = %d\r\n"), ballboard.lcob);
+	printf_P(PSTR("rcob = %d\r\n"), ballboard.rcob);
 #endif
 }
 
diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c
index b6a12c8..ce76583 100755
--- a/projects/microb2010/mainboard/main.c
+++ b/projects/microb2010/mainboard/main.c
@@ -68,8 +68,10 @@
 #include "cs.h"
 #include "strat_base.h"
 #include "strat_db.h"
+#include "strat_avoid.h"
 #include "i2c_protocol.h"
 
+
 /* 0 means "programmed"
  * ---- with 16 Mhz quartz
  * CKSEL 3-0 : 0111
@@ -297,6 +299,7 @@ int main(void)
  	gen.logs[0] = E_USER_STRAT;
  	gen.log_level = 5;
 	strat_db_init();
+	test_strat_avoid();
 
 	/* strat-related event */
 	scheduler_add_periodical_event_priority(strat_event, NULL,
diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 5ae3797..7ea7b88 100644
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -61,6 +61,7 @@
 #include "strat_base.h"
 #include "strat_corn.h"
 #include "strat_utils.h"
+#include "strat_avoid.h"
 #include "sensor.h"
 #include "actuator.h"
 
@@ -85,7 +86,7 @@ void strat_preinit(void)
 	mainboard.flags =  DO_ENCODERS | DO_CS | DO_RS |
 		DO_POS | DO_BD | DO_POWER;
 
-	//i2c_cobboard_mode_init();
+	strat_db_init();
 	strat_conf_dump(__FUNCTION__);
 	strat_db_dump(__FUNCTION__);
 }
@@ -119,7 +120,6 @@ void strat_init(void)
 
 	/* we consider that the color is correctly set */
 	strat_running = 1;
-	strat_db_init();
 	strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
 	time_reset();
 	interrupt_traj_reset();
@@ -165,6 +165,7 @@ void strat_event(void *dummy)
 	if (strat_running == 0)
 		return;
 
+	/* read sensors from ballboard */
 	IRQ_LOCK(flags);
 	lcob = ballboard.lcob;
 	ballboard.lcob = I2C_COB_NONE;
@@ -172,6 +173,12 @@ void strat_event(void *dummy)
 	ballboard.rcob = I2C_COB_NONE;
 	IRQ_UNLOCK(flags);
 
+/* 	if (lcob != I2C_COB_NONE) */
+/* 		DEBUG(E_USER_STRAT, "XXX lcob %s", */
+/* 		      lcob == I2C_COB_WHITE ? "white" : "black"); */
+/* 	if (rcob != I2C_COB_NONE) */
+/* 		DEBUG(E_USER_STRAT, "XXX rcob %s", */
+/* 		      rcob == I2C_COB_WHITE ? "white" : "black"); */
 	/* XXX take opponent position into account */
 
 #ifdef HOST_VERSION
@@ -202,31 +209,47 @@ void strat_event(void *dummy)
 	}
 
 	/* control the cobboard mode for left spickle */
-	if (get_cob_count() >= 5 || want_pack) {
-		if (strat_db.corn_table[lidx]->corn.color != I2C_COB_WHITE &&
-		    strat_db.corn_table[lidx]->corn.color != I2C_COB_BLACK)
-			i2c_cobboard_pack(I2C_LEFT_SIDE);
+	if (lcob_near) {
+		if (get_cob_count() >= 5 || want_pack) {
+			if (strat_db.corn_table[lidx]->corn.color != I2C_COB_WHITE &&
+			    strat_db.corn_table[lidx]->corn.color != I2C_COB_BLACK)
+				i2c_cobboard_pack(I2C_LEFT_SIDE);
+		}
+		else {
+			if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
+				i2c_cobboard_autoharvest(I2C_LEFT_SIDE);
+			else if (strat_db.corn_table[lidx]->corn.color == I2C_COB_BLACK)
+				i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
+			else
+				i2c_cobboard_deploy(I2C_LEFT_SIDE);
+		}
 	}
 	else {
-		if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
-			i2c_cobboard_autoharvest(I2C_LEFT_SIDE);
-		else if (strat_db.corn_table[lidx]->corn.color == I2C_COB_BLACK)
-			i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
+		if (get_cob_count() >= 5 || want_pack)
+			i2c_cobboard_pack(I2C_LEFT_SIDE);
 		else
 			i2c_cobboard_deploy(I2C_LEFT_SIDE);
 	}
 
 	/* control the cobboard mode for right spickle */
-	if (get_cob_count() >= 5 || want_pack) {
-		if (strat_db.corn_table[lidx]->corn.color != I2C_COB_WHITE &&
-		    strat_db.corn_table[lidx]->corn.color != I2C_COB_BLACK)
-			i2c_cobboard_pack(I2C_RIGHT_SIDE);
+	if (rcob_near) {
+		if (get_cob_count() >= 5 || want_pack) {
+			if (strat_db.corn_table[ridx]->corn.color != I2C_COB_WHITE &&
+			    strat_db.corn_table[ridx]->corn.color != I2C_COB_BLACK)
+				i2c_cobboard_pack(I2C_RIGHT_SIDE);
+		}
+		else {
+			if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE)
+				i2c_cobboard_autoharvest(I2C_RIGHT_SIDE);
+			else if (strat_db.corn_table[ridx]->corn.color == I2C_COB_BLACK)
+				i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
+			else
+				i2c_cobboard_deploy(I2C_RIGHT_SIDE);
+		}
 	}
 	else {
-		if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
-			i2c_cobboard_autoharvest(I2C_RIGHT_SIDE);
-		else if (strat_db.corn_table[lidx]->corn.color == I2C_COB_BLACK)
-			i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
+		if (get_cob_count() >= 5 || want_pack)
+			i2c_cobboard_pack(I2C_RIGHT_SIDE);
 		else
 			i2c_cobboard_deploy(I2C_RIGHT_SIDE);
 	}
@@ -248,32 +271,37 @@ static uint8_t strat_eject(void)
 	//XXX return vals
 	strat_set_speed(600, SPEED_ANGLE_SLOW);
 
+	want_pack = 1;
 	trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
 	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+	/* ball ejection */
+	i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
 	trajectory_a_abs(&mainboard.traj, COLOR_A(70));
 	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 
 	DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
 	strat_hardstop();
-
-	/* ball ejection */
-	trajectory_a_abs(&mainboard.traj, COLOR_A(90));
-	i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
 	time_wait_ms(2000);
 
+
 	/* half turn */
+	strat_event_disable();
 	i2c_cobboard_pack(I2C_LEFT_SIDE);
 	i2c_cobboard_pack(I2C_RIGHT_SIDE);
 	trajectory_a_rel(&mainboard.traj, COLOR_A(180));
 	err = wait_traj_end(END_INTR|END_TRAJ);
 
 	/* cob ejection */
-	trajectory_d_rel(&mainboard.traj, -100);
+	trajectory_d_rel(&mainboard.traj, -70);
 	err = wait_traj_end(END_INTR|END_TRAJ);
-	strat_running = ;
+
 	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
+	strat_db_dump(__FUNCTION__);
 	time_wait_ms(2000);
 
+	strat_event_enable();
+	want_pack = 0;
 	return 0;
 }
 
@@ -301,11 +329,11 @@ static uint8_t strat_beginning(void)
 
 #if 1
  l1:
-	DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+	DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
 	if (get_cob_count() >= 5)
 		strat_set_speed(600, SPEED_ANGLE_FAST);
 
-	err = line2line(LINE_UP, 0, LINE_R_DOWN, 2);
+	err = line2line(0, LINE_UP, 2, LINE_R_DOWN);
 	if (!TRAJ_SUCCESS(err)) {
 		strat_hardstop();
 		time_wait_ms(2000);
@@ -313,11 +341,11 @@ static uint8_t strat_beginning(void)
 	}
 
  l2:
-	DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+	DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
 	if (get_cob_count() >= 5)
 		strat_set_speed(600, SPEED_ANGLE_FAST);
 
-	err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2);
+	err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP);
 	if (!TRAJ_SUCCESS(err)) {
 		strat_hardstop();
 		time_wait_ms(2000);
@@ -325,16 +353,22 @@ static uint8_t strat_beginning(void)
 	}
 
 #else
-	strat_set_speed(600, SPEED_ANGLE_FAST);
-	err = line2line(LINE_UP, 0, LINE_R_DOWN, 3);
-	err = line2line(LINE_R_DOWN, 3, LINE_R_UP, 2);
-	err = line2line(LINE_R_UP, 2, LINE_R_DOWN, 2);
-	err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 3);
-	err = line2line(LINE_R_UP, 3, LINE_UP, 5);
-	err = line2line(LINE_UP, 5, LINE_L_DOWN, 2);
-	err = line2line(LINE_L_DOWN, 2, LINE_L_UP, 1);
-	err = line2line(LINE_L_UP, 1, LINE_L_DOWN, 1);
-	err = line2line(LINE_L_DOWN, 1, LINE_DOWN, 0);
+/* 	strat_set_speed(600, SPEED_ANGLE_FAST); */
+/* 	err = line2line(0, LINE_UP, 3, LINE_R_DOWN); */
+/* 	err = line2line(3, LINE_R_DOWN, 2, LINE_R_UP); */
+/* 	err = line2line(2, LINE_R_UP, 2, LINE_R_DOWN); */
+/* 	err = line2line(2, LINE_R_DOWN, 3, LINE_R_UP); */
+/* 	err = line2line(3, LINE_R_UP, 5, LINE_UP); */
+/* 	err = line2line(5, LINE_UP, 2, LINE_L_DOWN); */
+/* 	err = line2line(2, LINE_L_DOWN, 1, LINE_L_UP); */
+/* 	err = line2line(1, LINE_L_UP, 1, LINE_L_DOWN); */
+/* 	err = line2line(1, LINE_L_DOWN, 0, LINE_DOWN); */
+
+	strat_set_speed(300, SPEED_ANGLE_FAST);
+	err = line2line(0, LINE_UP, 0, LINE_R_UP);
+	err = line2line(0, LINE_R_UP, 1, LINE_DOWN);
+	err = line2line(1, LINE_DOWN, 1, LINE_L_DOWN);
+	err = line2line(1, LINE_L_DOWN, 0, LINE_DOWN);
 	wait_ms(500);
 	strat_hardstop();
 	return END_TRAJ;
@@ -344,12 +378,13 @@ static uint8_t strat_beginning(void)
 
 	strat_set_speed(250, SPEED_ANGLE_FAST);
 
+#if 0
  l4:
-	DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+	DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
 	if (get_cob_count() >= 5)
 		strat_set_speed(600, SPEED_ANGLE_FAST);
 
-	err = line2line(LINE_DOWN, 5, LINE_L_UP, 2);
+	err = line2line(5, LINE_DOWN, 2, LINE_L_UP);
 	if (!TRAJ_SUCCESS(err)) {
 		strat_hardstop();
 		time_wait_ms(2000);
@@ -357,20 +392,23 @@ static uint8_t strat_beginning(void)
 	}
 
  l5:
-	DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+	DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
 	if (get_cob_count() >= 5)
 		strat_set_speed(600, SPEED_ANGLE_FAST);
 
-	err = line2line(LINE_L_UP, 2, LINE_R_UP, 2);
+	err = line2line(2, LINE_L_UP, 2, LINE_R_UP);
 	if (!TRAJ_SUCCESS(err)) {
 		strat_hardstop();
 		time_wait_ms(2000);
 		goto l5;
 	}
 
-	DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+	DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
 	if (get_cob_count() >= 5)
 		strat_set_speed(600, SPEED_ANGLE_FAST);
+#else
+	strat_harvest_circuit();
+#endif
 
 	WAIT_COND_OR_TRAJ_END(distance_from_robot(2625, COLOR_Y(1847)) < 100,
 			      TRAJ_FLAGS_STD);
diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c
index 87bcf9f..cd54e6c 100644
--- a/projects/microb2010/mainboard/strat_avoid.c
+++ b/projects/microb2010/mainboard/strat_avoid.c
@@ -28,6 +28,7 @@
 
 #include <aversive.h>
 #include <aversive/pgmspace.h>
+#include <aversive/error.h>
 
 #include <ax12.h>
 #include <uart.h>
@@ -61,322 +62,576 @@
 #include "strat_db.h"
 #include "strat_base.h"
 #include "strat_corn.h"
+#include "strat_avoid.h"
 #include "strat_utils.h"
 #include "sensor.h"
 #include "actuator.h"
 
-/* XXX TODO
-static
-const
-change x,y -> i,j to avoid confusion with coords
-could be optimized in mem space: it is not needed to store the x,y coord,
-   we can process it from idx. however it will be less optimized for speed
-
-*/
-
-/* XXX enum possible ? else just rename */
-#define START      0
-#define UP         1
-#define UP_RIGHT   2
-#define DOWN_RIGHT 3
-#define DOWN       4
-#define DOWN_LEFT  5
-#define UP_LEFT    6
-#define END        7
-
-struct djpoint {
-	uint16_t weight;
-	struct djpoint *parent;
-
-	uint8_t parent_pos:3;
-	uint8_t updated:1;
-	uint8_t todo:1;
-	uint8_t reserved:3;
-};
-
-/* database for dijkstra */
-static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
-
-/* return index from neigh pointer */
-#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
 
-void dump(void)
-{
-	int8_t i, j;
-	struct djpoint *pt;
-	struct waypoint_db *wp;
-
-	printf_P(PSTR("         "));
-	for (i=0; i<WAYPOINTS_NBX; i++) {
-		printf_P(PSTR(" %2d "), i);
-	}
-	printf_P(PSTR("\r\n"));
-
-	for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
-		printf_P(PSTR("%3d   "), j/2);
-
-		if ((j&1) == 0)
-			printf_P(PSTR("    "));
-
-		for (i=0; i<WAYPOINTS_NBX; i++) {
-			pt = &djpoints[i][j/2];
-			wp = &strat_db.wp_table[i][j/2];
+const struct wp_coord circuit1[] = {
+	{ .i = 11, .j = 6, .end = 0, },
+	{ .i = 10, .j = 6, .end = 0, },
+	{ .i = 9, .j = 5, .end = 0, },
+	{ .i = 8, .j = 5, .end = 0, },
+	{ .i = 7, .j = 4, .end = 0, },
+	{ .i = 6, .j = 4, .end = 0, },
+	{ .i = 5, .j = 4, .end = 0, },
+	{ .i = 4, .j = 5, .end = 0, },
+	{ .i = 3, .j = 5, .end = 0, },
+	{ .i = 2, .j = 6, .end = 0, },
+	{ .i = 1, .j = 6, .end = 0, },
+	{ .i = 1, .j = 5, .end = 0, },
+	{ .i = 1, .j = 4, .end = 0, },
+	{ .i = 1, .j = 3, .end = 0, },
+	{ .i = 1, .j = 2, .end = 0, },
+	{ .i = 1, .j = 1, .end = 0, },
+	{ .i = 2, .j = 2, .end = 0, },
+	{ .i = 3, .j = 2, .end = 0, },
+	{ .i = 4, .j = 3, .end = 0, },
+	{ .i = 5, .j = 3, .end = 0, },
+	{ .i = 6, .j = 4, .end = 0, },
+	{ .i = 7, .j = 3, .end = 0, },
+	{ .i = 8, .j = 3, .end = 0, },
+	{ .i = 9, .j = 2, .end = 0, },
+	{ .i = 10, .j = 2, .end = 0, },
+	{ .i = 11, .j = 1, .end = 0, },
+	{ .i = 11, .j = 2, .end = 0, },
+	{ .i = 11, .j = 3, .end = 0, },
+	{ .i = 11, .j = 4, .end = 0, },
+	{ .i = 11, .j = 5, .end = 0, },
+	{ .i = 11, .j = 6, .end = 1, },
+};
 
-			if (((i+j) & 1) == 0)
-				continue;
+const struct wp_coord circuit2[] = {
+	{ .i = 11, .j = 6, .end = 0, },
+	{ .i = 10, .j = 6, .end = 0, },
+	{ .i = 9, .j = 5, .end = 0, },
+	{ .i = 9, .j = 4, .end = 0, },
+	{ .i = 9, .j = 3, .end = 0, },
+	{ .i = 10, .j = 4, .end = 0, },
+	{ .i = 11, .j = 4, .end = 0, },
+	{ .i = 11, .j = 5, .end = 0, },
+	{ .i = 11, .j = 6, .end = 1, },
+};
 
-			if (wp->type == WP_TYPE_OBSTACLE)
-				printf_P(PSTR("     X  "));
-			else if (wp->dangerous)
-				printf_P(PSTR("     D  "));
-			else if (wp->type == WP_TYPE_CORN &&
-				 wp->corn.color == I2C_COB_WHITE)
-				printf_P(PSTR("     W  "));
-			else if (wp->type == WP_TYPE_CORN &&
-				 wp->corn.color == I2C_COB_BLACK)
-				printf_P(PSTR("     B  "));
-			else if (wp->type == WP_TYPE_CORN &&
-				 wp->corn.color == I2C_COB_UNKNOWN)
-				printf_P(PSTR("     U  "));
- 			else if (wp->type == WP_TYPE_WAYPOINT ||
-				 wp->type == WP_TYPE_TOMATO)
-				printf_P(PSTR(" %5d  "), pt->weight);
- 			else
- 				printf_P(PSTR("     ?  "));
-		}
-		printf_P(PSTR("\r\n"));
-	}
-}
+/* list of all possible circuits */
+const struct wp_coord *circuits[] = {
+	circuit1,
+	circuit2,
+	NULL,
+};
 
+/* symetric neighbor position */
 static inline uint8_t opposite_position(uint8_t pos)
 {
 	pos += 3;
-	if (pos > UP_LEFT)
+	if (pos > LINE_L_UP)
 		pos -= 6;
 	return pos;
 }
 
-/* is point reachable by the robot ? */
-static uint8_t is_reachable(uint8_t i, uint8_t j)
+static uint8_t cc;
+uint8_t xget_cob_count(void)
 {
-	struct waypoint_db *wp;
-
-	wp = &strat_db.wp_table[i][j];
-	if (wp->type == WP_TYPE_WAYPOINT)
-		return 1;
-	if (wp->type == WP_TYPE_TOMATO)
-		return 1;
-	if (wp->type == WP_TYPE_CORN &&
-	    wp->present == 0)
-		return 1;
-	return 0;
+	return cc;
 }
 
-/* return coord of the entry in the table from the pointer */
-static void djpoint2ij(struct djpoint *pt, uint8_t *i, uint8_t *j)
+static uint8_t bc;
+uint8_t xget_ball_count(void)
 {
-	int8_t idx = PT2IDX(pt);
-	*i = idx / WAYPOINTS_NBY;
-	*j = idx % WAYPOINTS_NBY;
+	return bc;
 }
 
-/* get the neighbour of the point at specified position */
-static struct djpoint *get_neigh(struct djpoint *pt,
-				 uint8_t position)
+static uint32_t ts;
+uint8_t xtime_get_s(void)
 {
-	uint8_t i,j;
-	struct djpoint *neigh;
+	return ts;
+}
 
-	djpoint2ij(pt, &i, &j);
 
+/* get the neighbour of the point at specified position, return -1 if
+ * there is no neighbor */
+static int8_t get_neigh(uint8_t i, uint8_t j,
+			uint8_t *ni, uint8_t *nj,
+			uint8_t position)
+{
 	switch (position) {
-	case UP:
+	case LINE_UP:
 		j++;
 		break;
-	case UP_RIGHT:
+	case LINE_R_UP:
 		if (!(i & 1)) j++;
 		i++;
 		break;
-	case DOWN_RIGHT:
+	case LINE_R_DOWN:
 		if (i & 1) j--;
 		i++;
 		break;
-	case DOWN:
+	case LINE_DOWN:
 		j--;
 		break;
-	case DOWN_LEFT:
+	case LINE_L_DOWN:
 		if (i & 1) j--;
 		i--;
 		break;
-	case UP_LEFT:
+	case LINE_L_UP:
 		if (!(i & 1)) j++;
 		i--;
 		break;
 	default:
-		return NULL;
+		return -1;
 	}
 	if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
-		return NULL;
+		return -1;
 
-	if (is_reachable(i, j) == 0)
-		return NULL;
+	*ni = i;
+	*nj = j;
+	return 0;
+}
 
-	neigh = &djpoints[i][j];
-	return neigh;
+static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
+{
+	switch (dir) {
+	case LINE_UP:
+	case LINE_DOWN:
+		return i/2;
+	case LINE_R_UP:
+	case LINE_L_DOWN:
+		i &= 0xfe;
+		j -= i/2;
+		return (5-j)/2;
+	case LINE_R_DOWN:
+	case LINE_L_UP:
+		i &= 0xfe;
+		j += i/2;
+		return (11-j)/2;
+	default:
+		return -1;
+	}
 }
 
-static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
+static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j,
+		       uint8_t i, uint8_t j)
 {
-	struct djpoint *neigh = NULL;
-	uint8_t pos = *position + 1;
+	int8_t diff_i, diff_j;
+
+	diff_i = i - prev_i;
+	diff_j = j - prev_j;
+
+	if (diff_i == 0 && diff_j == 1)
+		return LINE_UP;
+	if (diff_i == 0 && diff_j == -1)
+		return LINE_DOWN;
+
+	if ((prev_i & 1) == 0) {
+		if (diff_i == 1 && diff_j == 0)
+			return LINE_R_UP;
+		if (diff_i == 1 && diff_j == -1)
+			return LINE_R_DOWN;
+		if (diff_i == -1 && diff_j == 0)
+			return LINE_L_UP;
+		if (diff_i == -1 && diff_j == -1)
+			return LINE_L_DOWN;
+	}
+	else {
+		if (diff_i == 1 && diff_j == 1)
+			return LINE_R_UP;
+		if (diff_i == 1 && diff_j == 0)
+			return LINE_R_DOWN;
+		if (diff_i == -1 && diff_j == 1)
+			return LINE_L_UP;
+		if (diff_i == -1 && diff_j == 0)
+			return LINE_L_DOWN;
+	}
 
-	for (pos = *position + 1; pos < END; pos++) {
-		neigh = get_neigh(pt, pos);
-		if (neigh != NULL)
+	/* invalid value */
+	return 0xFF;
+}
+
+
+int8_t get_path(const struct wp_coord *circuit,
+		uint8_t starti, uint8_t startj, uint8_t faceA,
+		struct wp_line *circuit_wpline)
+{
+	const struct wp_coord *curcircuit;
+	uint8_t prev_i, prev_j;
+	uint8_t dir, prev_dir = 0xFF;
+	uint8_t found = 0, i = 0, j = 0;
+	uint8_t linenum;
+	int8_t step = faceA ? 1 : -1;
+	int8_t skipfirst=0;
+	int8_t path_len = 0;
+
+	printf("face: %d\n", faceA);
+	if ( !faceA && circuit->i == 11 && circuit->j == 6)
+		skipfirst=1;
+
+	/* check that the point is present in the circuit */
+	for (curcircuit = circuit + skipfirst; curcircuit->end == 0; curcircuit ++) {
+		if (curcircuit->i == starti && curcircuit->j == startj) {
+			found = 1;
 			break;
+		}
 	}
 
-	*position = pos;
-	return neigh;
-}
+	if ( !faceA && curcircuit->i == 11 && curcircuit->j == 6)
+		found = 1;
+	if (found == 0)
+		return -1;
 
-/* browse all points */
-#define POINT_FOREACH(cur)						\
-	for (cur = &djpoints[0][0];					\
-	     cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];		\
-	     cur ++)
+	/* XXX len must be >= 1 */
+	/* XXX start = 11,6 */
 
-/* XXX comment */
-#define NEIGH_FOREACH(neigh, pos, point)			\
-	for (pos = START, neigh = get_next_neigh(point, &pos);	\
-	     neigh;						\
-	     neigh = get_next_neigh(point, &pos))
+	prev_i = starti;
+	prev_j = startj;
 
-int dijkstra_init(void)
-{
-	return 0;
+	curcircuit = curcircuit;
+	while (1) {
+		if (faceA && curcircuit->end)
+			break;
+		else if (!faceA && curcircuit == circuit)
+			break;
+		i = curcircuit->i;
+		j = curcircuit->j;
+
+		dir = get_dir(prev_i, prev_j, i, j);
+
+		if (prev_dir != dir) {
+			linenum = get_line_num(prev_i, prev_j, dir);
+			/* printf_P(PSTR("COIN %d, %d, dir=%d linenum=%d\r\n"), */
+			/* 				 prev_i, prev_j, dir, linenum); */
+			circuit_wpline[path_len].line_num = linenum;
+			circuit_wpline[path_len].dir = dir;
+			path_len++;
+		}
+		prev_dir = dir;
+		prev_i = i;
+		prev_j = j;
+		curcircuit += step;
+	}
+
+/* 	printf_P(PSTR("COIN %d, %d\r\n"), curcircuit->i, curcircuit->j); */
+
+	return path_len; /* XXX */
 }
 
-/* return distance between p1 and p2 */
-static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
+int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved,
+		  uint16_t tomato_retrieved, uint8_t len)
 {
-	int16_t x1, y1, x2, y2;
-	double vx, vy;
-	uint8_t i, j;
+	int16_t score = 0;
+	uint8_t i;
+	uint32_t mask = 1;
+	uint8_t n;
+
+	/* score with corn */
+	n = xget_cob_count() * 2;
+	for (i = 0; i<CORN_NB; i++) {
+		if (n >= 10)
+			break;
+		if (wcorn_retrieved & mask) {
+			score += 250;
+			n += 2;
+		}
+		if (n >= 10)
+			break;
+		if (ucorn_retrieved & mask) {
+			score += 125;
+			n += 1;
+		}
+		mask <<= 1UL;
+	}
 
-	djpoint2ij(p1, &i, &j);
-	ijcoord_to_xycoord(i, j, &x1, &y1);
+	printf("get score: cob %d \n", n);
+	/* score with tomato */
+	n = xget_ball_count();
+	mask = 1;
+	for (i = 0; i<TOMATO_NB; i++) {
+		if (n >= 4)
+			break;
+		if (tomato_retrieved & mask) {
+			score += 150;
+			n += 1;
+		}
+		mask <<= 1UL;
+	}
 
-	djpoint2ij(p2, &i, &j);
-	ijcoord_to_xycoord(i, j, &x2, &y2);
+	printf("get score: ball %d \n", n);
+	/* malus for long circuits */
+	score -= (len * 20);
+
+	/* double malus for long circuits if we don't have much
+	 * time */
+#define WP_SPEED 1
+	if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) {
+		uint16_t extra;
+		extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s());
+		score -= (200 * extra);
+	}
 
-	vx = x2 - x1;
-	vy = y2 - y1;
-	return sqrt(vx * vx + vy * vy);
+	/* XXX use direction of robot */
+
+	return score;
 }
 
-void dijkstra_process_neighs(struct djpoint *pt)
+/* i,j: starting position */
+int8_t browse_one_circuit(const struct wp_coord *circuit,
+			  uint8_t starti, uint8_t startj,
+			  int16_t *scoreA, int16_t *scoreB)
 {
- 	struct djpoint *neigh;
-	uint8_t pos, parent_pos;
-	uint16_t weight;
-	uint8_t i,j;
-
-	djpoint2ij(pt, &i, &j);
-	printf_P(PSTR("at %d %d:\r\n"), i, j);
-
-	NEIGH_FOREACH(neigh, pos, pt) {
-		weight = pt->weight + dist(pt, neigh);
-		parent_pos = opposite_position(pos);
-
-		/* bonus if we keep the same direction */
-		if (parent_pos == pt->parent_pos ||
-		    pt->parent_pos == END)
-			weight = weight - 1;
-
-		printf_P(PSTR("  pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos,
-		       pt->parent_pos, parent_pos,
-		       neigh->weight, weight);
-		if (neigh->weight == 0 || weight < neigh->weight) {
-			djpoint2ij(neigh, &i, &j);
-			//printf_P(PSTR("    %d,%d updated\r\n"), i, j);
-			neigh->weight = weight;
-			neigh->parent_pos = parent_pos;
-			neigh->updated = 1;
+	const struct wp_coord *curcircuit;
+	uint32_t wcorn_retrieved = 0; /* bit mask */
+	uint32_t ucorn_retrieved = 0; /* bit mask */
+	uint16_t tomato_retrieved = 0; /* bit mask */
+	uint8_t len = 0, found = 0, i, j;
+	uint8_t ni = 0, nj = 0, pos, color;
+
+	/* check that the point is present in the circuit */
+	for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++) {
+		if (curcircuit->i == starti && curcircuit->j == startj) {
+			found = 1;
+			break;
 		}
 	}
-}
 
-int dijkstra(struct djpoint *start)
-{
- 	struct djpoint *cur;
-	uint8_t todolist = 1;
+	if (found == 0)
+		return -1;
 
-	start->todo = 1;
+	for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++, len ++) {
+		i = curcircuit->i;
+		j = curcircuit->j;
 
-	while (todolist) {
-		printf_P(PSTR("\r\n"));
-		dump();
-		/* process all neighbours of todo list */
-		POINT_FOREACH(cur) {
-			if (!cur->todo)
-				continue;
-			dijkstra_process_neighs(cur);
-			cur->todo = 0;
+/* 		printf("cur:%d,%d %x %x %x %d\n", i, j, */
+/* 		       wcorn_retrieved, ucorn_retrieved, tomato_retrieved, len); */
+
+		/* face A completed */
+		if (i == starti && j == startj) {
+			*scoreA = get_score(wcorn_retrieved, ucorn_retrieved,
+					   tomato_retrieved, len);
+			wcorn_retrieved = 0; /* bit mask */
+			ucorn_retrieved = 0; /* bit mask */
+			tomato_retrieved = 0; /* bit mask */
+			len = 0;
+		}
+
+		/* is there a tomato ? */
+		if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
+		    strat_db.wp_table[i][j].present) {
+			tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
 		}
 
-		/* convert updated list in todo list */
-		todolist = 0;
-		POINT_FOREACH(cur) {
-			if (!cur->updated)
+		/* browse all neighbours to see if there is cobs */
+		for (pos = LINE_UP; pos <= LINE_R_DOWN; pos++) {
+			if (get_neigh(i, j, &ni, &nj, pos) < 0)
 				continue;
-			todolist = 1;
-			cur->todo = 1;
-			cur->updated = 0;
+
+			/* is there a corn cob ? */
+			if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
+			    strat_db.wp_table[ni][nj].present) {
+				color = strat_db.wp_table[ni][nj].corn.color;
+				if (color == I2C_COB_WHITE)
+					wcorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx);
+				else if (color == I2C_COB_UNKNOWN)
+					ucorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx);
+			}
 		}
-	}
-	return 0; /* XXX */
-}
+	};
 
-/* init waypoints position */
-void init_djpoints(void)
-{
-	int8_t i, j;
-	struct djpoint *pt;
+	*scoreB = get_score(wcorn_retrieved, ucorn_retrieved,
+			    tomato_retrieved, len);
+	if (circuit->i == starti && circuit->j == startj)
+		*scoreA = *scoreB;
 
-	for (i=0; i<WAYPOINTS_NBX; i++) {
+	return 0;
+}
 
-		for (j=0; j<WAYPOINTS_NBY; j++) {
-			pt = &djpoints[i][j];
-			pt->parent_pos = END;
-			pt->updated = 0;
-			pt->todo = 0;
-			pt->weight = 0;
+/* i,j starting position */
+int8_t browse_circuits(uint8_t i, uint8_t j,
+		       const struct wp_coord **selected_circuit,
+		       int8_t *selected_face)
+{
+	const struct wp_coord **circuit;
+	int16_t scoreA, scoreB;
+	int16_t selected_score = 0x8000; /* ~-int_max */
+	int8_t found = -1;
+
+	*selected_face = 0;
+	*selected_circuit = circuits[0] ;
+	for (circuit = &circuits[0]; *circuit; circuit++) {
+		if (browse_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
+			continue;
+		found = 0;
+		printf_P(PSTR("Circuit: %d %d\r\n"), scoreA, scoreB);
+		if (scoreA > selected_score) {
+			*selected_circuit = *circuit;
+			selected_score = scoreA;
+			*selected_face = 0;
 		}
+		if (scoreB > selected_score) {
+			*selected_circuit = *circuit;
+			selected_score = scoreB;
+			*selected_face = 1;
+		}
+	}
+	return found;
+}
+
+static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
+{
+	int8_t i;
+	if (len <= 0)
+		return;
+	for (i = 0; i < len; i ++) {
+		printf(PSTR("linenum %d dir %d\r\n"), circuit_wpline[i].line_num,
+		       circuit_wpline[i].dir);
 	}
+
 }
 
-int get_path(struct djpoint *start, struct djpoint *end)
+uint8_t strat_harvest_circuit(void)
 {
- 	struct djpoint *cur;
-	uint8_t prev_direction = 0;
-	int8_t idx;
+	const struct wp_coord *selected_circuit;
+	int8_t selected_face;
+	struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
+	int8_t len;
+	uint8_t i, j, idx;
 	int16_t x, y;
+	uint8_t linenum, prev_linenum;
+	uint8_t dir, prev_dir;
+	uint8_t err;
+
+	x = position_get_x_s16(&mainboard.pos);
+	y = position_get_y_s16(&mainboard.pos);
 
-	for (cur = start;
-	     cur != NULL && cur->parent_pos != END && cur != end;
-	     cur = get_neigh(cur, cur->parent_pos)) {
-		if (prev_direction != cur->parent_pos) {
-			idx = PT2IDX(cur);
-			corn_idx_to_xycoord(idx, &x, &y);
-			printf_P(PSTR("%d %d (%d)\r\n"),
-				 x, y, cur->parent_pos);
+	if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
+		DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
+		      __FUNCTION__, x, y);
+		return END_ERROR;
+	}
+
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	if (len < 0) {
+		DEBUG(E_USER_STRAT, "%s(): cannot find a path",
+		      __FUNCTION__);
+		return END_ERROR;
+	}
+
+	dump_circuit_wp(circuit_wpline, len);
+
+	prev_linenum = circuit_wpline[0].line_num;
+	prev_dir = circuit_wpline[0].dir;
+	for (idx = 1; idx < len; idx ++) {
+	retry:
+		if (get_cob_count() >= 5)
+			strat_set_speed(600, SPEED_ANGLE_FAST);
+
+		linenum = circuit_wpline[idx].line_num;
+		dir = circuit_wpline[idx].dir;
+
+		/* XXX basic opponent management */
+		DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
+		      __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
+		err = line2line(prev_linenum, prev_dir, linenum, dir);
+		if (!TRAJ_SUCCESS(err)) {
+			strat_hardstop();
+			time_wait_ms(2000);
+			goto retry;
 		}
-		prev_direction = cur->parent_pos;
+
+		prev_linenum = linenum;
+		prev_dir = dir;
 	}
-	idx = PT2IDX(end);
-	corn_idx_to_xycoord(idx, &x, &y);
-	printf_P(PSTR("%d %d\r\n"), x, y);
 
-	return 0; /* XXX */
+	return END_TRAJ; // XXX
+}
+
+void test_strat_avoid(void)
+{
+	uint8_t i, j;
+	const struct wp_coord *selected_circuit;
+	int8_t selected_face;
+	struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
+	int8_t ret;
+
+	i = 1; j = 1;
+	printf("========= i=%d, j=%d\r\n", i, j);
+
+	ts = 0; bc = 0; cc = 0;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	ts = 0; bc = 3; cc = 0;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	ts = 0; bc = 4; cc = 0;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	ts = 0; bc = 3; cc = 5;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	ts = 0; bc = 4; cc = 5;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	ts = 80; bc = 0; cc = 0;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	i = 4; j = 3;
+	printf("========= i=%d, j=%d\r\n", i, j);
+
+	ts = 0; bc = 0; cc = 0;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	ts = 0; bc = 3; cc = 0;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	ts = 80; bc = 0; cc = 0;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	i = 11; j = 6;
+	printf("========= i=%d, j=%d\r\n", i, j);
+
+	ts = 0; bc = 0; cc = 0;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	ts = 0; bc = 3; cc = 0;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
+	ts = 80; bc = 0; cc = 0;
+	printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+	browse_circuits(i, j, &selected_circuit, &selected_face);
+	ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+	dump_circuit_wp(circuit_wpline, ret);
+
 }
diff --git a/projects/microb2010/mainboard/strat_avoid.h b/projects/microb2010/mainboard/strat_avoid.h
new file mode 100644
index 0000000..4afe032
--- /dev/null
+++ b/projects/microb2010/mainboard/strat_avoid.h
@@ -0,0 +1,47 @@
+/*
+ *  Copyright Droids, Microb Technology (2010)
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ *  Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
+ *
+ *  Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#ifndef _STRAT_AVOID_H_
+#define _STRAT_AVOID_H_
+#define MAX_CIRCUIT_WPLINE 15
+
+struct wp_coord {
+	uint8_t i:4;
+	uint8_t j:3;
+	uint8_t end:1;
+};
+
+struct wp_line {
+	uint8_t line_num:4;
+	uint8_t dir:4;
+};
+
+int8_t browse_circuits(uint8_t i, uint8_t j,
+		       const struct wp_coord **selected_circuit,
+		       int8_t *selected_face);
+
+/* harvest on the best circuit */
+uint8_t strat_harvest_circuit(void);
+
+void test_strat_avoid(void);
+
+#endif
diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index f8cafbc..8f8503f 100644
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -131,8 +131,8 @@ static uint8_t handle_spickles(void)
 #endif
 }
 
-uint8_t line2line(uint8_t dir1, uint8_t num1,
-		  uint8_t dir2, uint8_t num2)
+uint8_t line2line(uint8_t num1, uint8_t dir1,
+		  uint8_t num2, uint8_t dir2)
 {
 	double line1_a_rad, line1_a_deg, line2_a_rad;
 	double diff_a_deg, diff_a_deg_abs, beta_deg;
@@ -142,6 +142,7 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
 	point_t p;
 	uint8_t err;
 	uint16_t a_speed, d_speed;
+	int8_t ret;
 
 	/* convert to 2 points */
 	num2line(&l1, dir1, num1);
@@ -197,10 +198,13 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
 	}
 
 	/* XXX check return value !! */
-	trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
-			   line1_a_deg, 150., diff_a_deg, beta_deg,
-			   radius, xy_norm(l1.p1.x, l1.p1.y,
-					   p.x, p.y));
+	ret = trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
+				 line1_a_deg, 150., diff_a_deg, beta_deg,
+				 radius, xy_norm(l1.p1.x, l1.p1.y,
+						 p.x, p.y));
+	if (ret < 0)
+		DEBUG(E_USER_STRAT, "clitoid failed");
+
 	/* disabled */
 	if (0) {
 		err = 0;
diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h
index ac806d3..8b8342f 100644
--- a/projects/microb2010/mainboard/strat_corn.h
+++ b/projects/microb2010/mainboard/strat_corn.h
@@ -36,5 +36,5 @@ int8_t corn_is_near(uint8_t *corn_idx, uint8_t side);
 
 void num2line(struct line_2pts *l, uint8_t dir, uint8_t num);
 
-uint8_t line2line(uint8_t dir1, uint8_t num1,
-		  uint8_t dir2, uint8_t num2);
+uint8_t line2line(uint8_t num1, uint8_t dir1,
+		  uint8_t num2, uint8_t dir2);
diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c
index 2d38967..d2835fe 100644
--- a/projects/microb2010/mainboard/strat_db.c
+++ b/projects/microb2010/mainboard/strat_db.c
@@ -153,6 +153,48 @@ int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *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. */
@@ -192,31 +234,44 @@ int8_t corn_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y)
 /* 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);
-		if (d < CORN_MARGIN && (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;
@@ -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;
 			}
 		}
diff --git a/projects/microb2010/mainboard/strat_db.h b/projects/microb2010/mainboard/strat_db.h
index 1b5314f..b770b43 100644
--- a/projects/microb2010/mainboard/strat_db.h
+++ b/projects/microb2010/mainboard/strat_db.h
@@ -33,7 +33,7 @@ struct corn_db {
 };
 
 struct tomato_db {
-	/* nothing for now */
+	uint8_t idx;
 };
 
 /* structure describing the status of a waypoint */
@@ -87,6 +87,11 @@ struct strat_db {
 /* global structure storing the database */
 extern struct strat_db strat_db;
 
+/* 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);
+
 /* convert i,j coords to x,y coords */
 int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y);
 
diff --git a/projects/microb2010/tests/oa/graph.py b/projects/microb2010/tests/oa/graph.py
index 49d3649..48e9cc3 100644
--- a/projects/microb2010/tests/oa/graph.py
+++ b/projects/microb2010/tests/oa/graph.py
@@ -166,6 +166,40 @@ def build_path(ptlist):
     x, y = zip(*poly.vertices)
     return x,y
 
+def nearest_corn(x, y):
+    OFFX=150
+    OFFY=222
+    STEPX=450
+    STEPY=500
+
+    x -= OFFX
+    x += STEPX/2
+    x /= STEPX
+
+    y -= OFFY
+    y += STEPY/2
+    if (x & 1) == 1:
+        y -= STEPY/2
+    y /= STEPY
+
+    i = (x * 2)
+    j = (y * 2) + (x & 1)
+    if i >= WAYPOINTS_NBX:
+        return None
+    if j >= WAYPOINTS_NBY:
+        return None
+
+    if (i & 1) == 0:
+        y = OFFSET_CORN_Y
+    else:
+        y = OFFSET_CORN_Y + STEP_CORN_Y/2
+    y += (j * STEP_CORN_Y)
+
+    x = OFFSET_CORN_X + (i * STEP_CORN_X)
+
+    return x, y
+
+
 def build_area(ax):
     # area
     x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
@@ -180,6 +214,8 @@ def build_area(ax):
     x,y = build_poly([(740,0), (740,500), (2260,500), (2260,0)])
     ax.plot(x, y, 'g--')
 
+    """
+    # courbes a 2 balles
     x,y = build_path([(375,0), (375,2050)])
     ax.plot(x, y, 'r-')
 
@@ -202,6 +238,21 @@ def build_area(ax):
                         alpha=0.5, facecolor=(1.,0.,0.))
     ax.add_collection(p)
 
+    """
+
+    """
+    #courbes a 2 balles 50
+    for xx in range(0, 3000, 100):
+        print xx
+        for yy in range(0, 2100, 100):
+            pouet = nearest_corn(xx, yy)
+            if pouet == None:
+                continue
+            xxx, yyy = pouet
+            x,y = build_path([(xx,yy), (xxx,yyy)])
+            ax.plot(x, y, 'r-')
+    """
+
    # limit
     #x,y = build_poly([(250,250), (2750,250), (2750,1850), (250,1850)])
     #ax.plot(x, y, 'g--')