From: zer0 <zer0@carbon.local>
Date: Thu, 15 Apr 2010 22:15:22 +0000 (+0200)
Subject: 20100416
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=b022f257a5ee568737e1a684a83d0154397fffcb;p=aversive.git

20100416
---

diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h
index cde4608..9f10c35 100755
--- a/projects/microb2010/ballboard/main.h
+++ b/projects/microb2010/ballboard/main.h
@@ -30,13 +30,13 @@
 #define LED1_OFF() 	cbi(PORTJ, 2)
 #define LED1_TOGGLE() 	LED_TOGGLE(PORTJ, 2)
 
-#define LED2_ON() 	sbi(PORTL, 7)
-#define LED2_OFF() 	cbi(PORTL, 7)
-#define LED2_TOGGLE() 	LED_TOGGLE(PORTL, 7)
+#define LED2_ON() 	sbi(PORTJ, 3)
+#define LED2_OFF() 	cbi(PORTJ, 3)
+#define LED2_TOGGLE() 	LED_TOGGLE(PORTJ, 3)
 
-#define LED3_ON() 	sbi(PORTJ, 3)
-#define LED3_OFF() 	cbi(PORTJ, 3)
-#define LED3_TOGGLE() 	LED_TOGGLE(PORTJ, 3)
+#define LED3_ON() 	sbi(PORTL, 7)
+#define LED3_OFF() 	cbi(PORTL, 7)
+#define LED3_TOGGLE() 	LED_TOGGLE(PORTL, 7)
 
 #define LED4_ON() 	sbi(PORTL, 6)
 #define LED4_OFF() 	cbi(PORTL, 6)
diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c
index 1b6e91f..8dfe8bb 100644
--- a/projects/microb2010/cobboard/actuator.c
+++ b/projects/microb2010/cobboard/actuator.c
@@ -49,8 +49,8 @@
 //#define COBROLLER_SPEED 400
 
 #define SERVO_DOOR_OPEN 260
-#define SERVO_DOOR_CLOSED 510
-#define SERVO_DOOR_BLOCK 510
+#define SERVO_DOOR_CLOSED 500
+#define SERVO_DOOR_BLOCK 500
 
 #define SERVO_CARRY_L_OPEN 295
 #define SERVO_CARRY_L_CLOSED 400 // 510
diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c
index a718970..0ed2f1a 100644
--- a/projects/microb2010/cobboard/cs.c
+++ b/projects/microb2010/cobboard/cs.c
@@ -102,8 +102,8 @@ static void do_cs(__attribute__((unused)) void *dummy)
 
 		/* urgent case: stop power on blocking */
 		if (cobboard.flags & DO_ERRBLOCKING) {
-			if (bd_get(&cobboard.left_spickle.bd) ||
-			    bd_get(&cobboard.right_spickle.bd) ||
+			if (/* bd_get(&cobboard.left_spickle.bd) || */
+			    /* bd_get(&cobboard.right_spickle.bd) || */
 			    bd_get(&cobboard.shovel.bd)) {
 				printf_P(PSTR("MOTOR BLOCKED STOP ALL\r\n"));
 				cobboard.flags &= ~(DO_POWER | DO_ERRBLOCKING);
@@ -149,8 +149,8 @@ void microb_cs_init(void)
 	/* ---- CS left_spickle */
 	/* PID */
 	pid_init(&cobboard.left_spickle.pid);
-	pid_set_gains(&cobboard.left_spickle.pid, 300, 10, 1500);
-	pid_set_maximums(&cobboard.left_spickle.pid, 0, 10000, 2400); /* max is 12 V */
+	pid_set_gains(&cobboard.left_spickle.pid, 400, 10, 1500);
+	pid_set_maximums(&cobboard.left_spickle.pid, 0, 25000, 4095);
 	pid_set_out_shift(&cobboard.left_spickle.pid, 10);
 	pid_set_derivate_filter(&cobboard.left_spickle.pid, 4);
 
@@ -169,8 +169,8 @@ void microb_cs_init(void)
 	/* ---- CS right_spickle */
 	/* PID */
 	pid_init(&cobboard.right_spickle.pid);
-	pid_set_gains(&cobboard.right_spickle.pid, 300, 10, 1500);
-	pid_set_maximums(&cobboard.right_spickle.pid, 0, 10000, 2400); /* max is 12 V */
+	pid_set_gains(&cobboard.right_spickle.pid, 400, 10, 1500);
+	pid_set_maximums(&cobboard.right_spickle.pid, 0, 25000, 4095);
 	pid_set_out_shift(&cobboard.right_spickle.pid, 10);
 	pid_set_derivate_filter(&cobboard.right_spickle.pid, 4);
 
diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c
index 8d86b49..9b2a9e7 100644
--- a/projects/microb2010/cobboard/i2c_protocol.c
+++ b/projects/microb2010/cobboard/i2c_protocol.c
@@ -99,17 +99,11 @@ static void i2c_send_status(void)
 	ans.right_cobroller_speed = cobboard.right_cobroller_speed;
 
 	ans.cob_count = state_get_cob_count();
-;
+
 	i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
 		 sizeof(ans), I2C_CTRL_GENERIC);
 }
 
-static int8_t i2c_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
-{
-	state_set_mode(cmd->mode);
-	return 0;
-}
-
 void i2c_recvevent(uint8_t * buf, int8_t size)
 {
 	void *void_cmd = buf;
@@ -136,6 +130,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 			break;
 		}
 
+#if 0
 	case I2C_CMD_COBBOARD_SET_MODE:
 		{
 			struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
@@ -144,6 +139,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 			i2c_set_mode(cmd);
 			break;
 		}
+#endif
 
 	case I2C_CMD_GENERIC_SET_COLOR:
 		{
@@ -164,16 +160,18 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 			break;
 		}
 #endif
-		
+
 	/* Add other commands here ...*/
 
 
 	case I2C_REQ_COBBOARD_STATUS:
 		{
-			//struct i2c_req_cobboard_status *cmd = void_cmd;
+			struct i2c_req_cobboard_status *cmd = void_cmd;
 			if (size != sizeof (struct i2c_req_cobboard_status))
 				goto error;
-			
+
+			/* mode is in req */
+			state_set_mode(cmd->mode);
 			i2c_send_status();
 			break;
 		}
diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c
index 1f4065b..d21e7b7 100755
--- a/projects/microb2010/cobboard/main.c
+++ b/projects/microb2010/cobboard/main.c
@@ -207,8 +207,8 @@ int main(void)
 	PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10,
 				 TIMER4_PRESCALER_DIV_1);
 
-	PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED |
-		      PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4);
+	PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED,
+		      &PORTD, 4);
 	PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED |
 		      PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5);
 	PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
@@ -271,6 +271,9 @@ int main(void)
  	gen.log_level = 5;
 	cobboard.flags |= DO_CS;
 
+	spickle_pack(I2C_LEFT_SIDE);
+	spickle_pack(I2C_RIGHT_SIDE);
+
  	state_machine();
 	cmdline_interact();
 
diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h
index 699a5c1..0c12fdc 100755
--- a/projects/microb2010/cobboard/main.h
+++ b/projects/microb2010/cobboard/main.h
@@ -30,13 +30,13 @@
 #define LED1_OFF() 	cbi(PORTJ, 2)
 #define LED1_TOGGLE() 	LED_TOGGLE(PORTJ, 2)
 
-#define LED2_ON() 	sbi(PORTL, 7)
-#define LED2_OFF() 	cbi(PORTL, 7)
-#define LED2_TOGGLE() 	LED_TOGGLE(PORTL, 7)
+#define LED2_ON() 	sbi(PORTJ, 3)
+#define LED2_OFF() 	cbi(PORTJ, 3)
+#define LED2_TOGGLE() 	LED_TOGGLE(PORTJ, 3)
 
-#define LED3_ON() 	sbi(PORTJ, 3)
-#define LED3_OFF() 	cbi(PORTJ, 3)
-#define LED3_TOGGLE() 	LED_TOGGLE(PORTJ, 3)
+#define LED3_ON() 	sbi(PORTL, 7)
+#define LED3_OFF() 	cbi(PORTL, 7)
+#define LED3_TOGGLE() 	LED_TOGGLE(PORTL, 7)
 
 #define LED4_ON() 	sbi(PORTL, 6)
 #define LED4_OFF() 	cbi(PORTL, 6)
diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c
index 5bbf43f..c471033 100644
--- a/projects/microb2010/cobboard/sensor.c
+++ b/projects/microb2010/cobboard/sensor.c
@@ -1,7 +1,7 @@
-/*  
+/*
  *  Copyright Droids Corporation (2009)
  *  Olivier MATZ <zer0@droids-corp.org>
- * 
+ *
  *  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
@@ -38,6 +38,7 @@
 #include <parse.h>
 #include <rdline.h>
 
+#include "../common/i2c_commands.h"
 #include "main.h"
 #include "sensor.h"
 
@@ -75,7 +76,7 @@ int16_t rii_strong(struct adc_infos *adc, int16_t val)
 #define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x )
 
 /* define which ADC to poll, see in sensor.h */
-static struct adc_infos adc_infos[ADC_MAX] = { 
+static struct adc_infos adc_infos[ADC_MAX] = {
 	[ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium },
 	[ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium },
 	[ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium },
@@ -91,7 +92,7 @@ static struct adc_infos adc_infos[ADC_MAX] = {
 static void adc_event(int16_t result);
 
 /* called every 10 ms, see init below */
-static void do_adc(__attribute__((unused)) void *dummy) 
+static void do_adc(__attribute__((unused)) void *dummy)
 {
 	/* launch first conversion */
 	adc_launch(adc_infos[0].config);
@@ -146,10 +147,10 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = {
 	[S_CAP2] =      { 10, 0, 3, 7, 0, 0 }, /* 1 */
 	[S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */
 	[S_CAP4] =      { 1, 0, 0, 1, 0, 0 }, /* 3 */
-	[S_LCOB] =      { 1, 0, 0, 1, 0, 0 }, /* 4 */
-	[S_LEFT] =      { 5, 0, 4, 1, 0, 0 }, /* 5 */
-	[S_RIGHT] =     { 5, 0, 4, 1, 0, 1 }, /* 6 */
-	[S_RCOB] =      { 1, 0, 0, 1, 0, 0 }, /* 7 */
+	[S_LCOB] =      { 1, 0, 0, 1, 0, 1 }, /* 4 */
+	[S_LEFT] =      { 5, 0, 4, 1, 0, 0 }, /* 5 */ /////// not used
+	[S_RIGHT] =     { 5, 0, 4, 1, 0, 1 }, /* 6 */ /////// not used
+	[S_RCOB] =      { 1, 0, 0, 1, 0, 1 }, /* 7 */
 	[S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */
 	[S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */
 	[S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */
@@ -163,7 +164,7 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = {
 /* value of filtered sensors */
 static uint16_t sensor_filtered = 0;
 
-/* sensor mapping : 
+/* sensor mapping :
  * 0-3:  PORTK 2->5 (cap1 -> cap4) (adc10 -> adc13)
  * 4-5:  PORTL 0->1 (cap5 -> cap6)
  * 6-7:  PORTE 3->4 (cap7 -> cap8)
@@ -218,7 +219,7 @@ static void do_boolean_sensors(__attribute__((unused)) void *dummy)
 			if (sensor_filter[i].cpt <= sensor_filter[i].thres_off)
 				sensor_filter[i].prev = 0;
 		}
-		
+
 		if (sensor_filter[i].prev && !sensor_filter[i].invert) {
 			tmp |= (1UL << i);
 		}
@@ -231,6 +232,51 @@ static void do_boolean_sensors(__attribute__((unused)) void *dummy)
 	IRQ_UNLOCK(flags);
 }
 
+static uint8_t lcob_cpt = 0, rcob_cpt = 0;
+uint8_t cob_falling_edge(uint8_t side)
+{
+	uint8_t flags;
+
+	if (side == I2C_LEFT_SIDE) {
+		IRQ_LOCK(flags);
+		if (lcob_cpt == 0) {
+			IRQ_UNLOCK(flags);
+			return 0;
+		}
+		lcob_cpt = 0;
+		IRQ_UNLOCK(flags);
+		return 1;
+	}
+	else {
+		IRQ_LOCK(flags);
+		if (rcob_cpt == 0) {
+			IRQ_UNLOCK(flags);
+			return 0;
+		}
+		rcob_cpt = 0;
+		IRQ_UNLOCK(flags);
+		return 1;
+	}
+}
+
+static void cob_edge_manage(void)
+{
+	static uint8_t lprev, rprev;
+	uint8_t l, r;
+	l = sensor_get(S_LCOB);
+	r = sensor_get(S_RCOB);
+	/* falling edge */
+	if (lprev != 0 && l == 0)
+		lcob_cpt = 10;
+	if (rprev != 0 && r == 0)
+		rcob_cpt = 10;
+	if (lcob_cpt > 0)
+		lcob_cpt --;
+	if (rcob_cpt > 0)
+		rcob_cpt --;
+	lprev = l;
+	rprev = r;
+}
 
 
 /************ global sensor init */
@@ -240,6 +286,7 @@ static void do_sensors(__attribute__((unused)) void *dummy)
 {
 	do_adc(NULL);
 	do_boolean_sensors(NULL);
+	cob_edge_manage();
 }
 
 void sensor_init(void)
@@ -247,8 +294,8 @@ void sensor_init(void)
 	adc_init();
 	adc_register_event(adc_event);
 	/* CS EVENT */
-	scheduler_add_periodical_event_priority(do_sensors, NULL, 
-						10000L / SCHEDULER_UNIT, 
+	scheduler_add_periodical_event_priority(do_sensors, NULL,
+						10000L / SCHEDULER_UNIT,
 						ADC_PRIO);
 }
 
diff --git a/projects/microb2010/cobboard/sensor.h b/projects/microb2010/cobboard/sensor.h
index 0ba6b0c..c24af73 100644
--- a/projects/microb2010/cobboard/sensor.h
+++ b/projects/microb2010/cobboard/sensor.h
@@ -54,3 +54,5 @@ int16_t sensor_get_adc(uint8_t i);
 /* get filtered values of boolean sensors */
 uint16_t sensor_get_all(void);
 uint8_t sensor_get(uint8_t i);
+
+uint8_t cob_falling_edge(uint8_t side);
diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index e1d9a7b..dcee125 100644
--- a/projects/microb2010/cobboard/spickle.c
+++ b/projects/microb2010/cobboard/spickle.c
@@ -60,23 +60,23 @@ struct spickle_params {
 };
 
 static struct spickle_params spickle = {
-	.k1 = 500,
+	.k1 = 1000,
 	.k2 = 20,
 	.csb = {
 		&cobboard.left_spickle,
 		&cobboard.right_spickle,
 	},
 	.pos_deployed = {
-		40000, /* left */
-		-40000, /* right */
+		0, /* left */
+		0, /* right */
 	},
 	.pos_mid = {
-		20000, /* left */
-		-20000, /* right */
+		25000, /* left */
+		26000, /* right */
 	},
 	.pos_packed = {
-		0, /* left */
-		0, /* right */
+		53000, /* left */
+		54500, /* right */
 	},
 };
 
@@ -84,9 +84,9 @@ static struct spickle_params spickle = {
 static void spickle_autopos(void)
 {
 	printf_P(PSTR("spickle autopos..."));
-	pwm_ng_set(LEFT_SPICKLE_PWM, -500);
-	pwm_ng_set(RIGHT_SPICKLE_PWM, 500);
-	wait_ms(1000);
+	pwm_ng_set(LEFT_SPICKLE_PWM, -700);
+	pwm_ng_set(RIGHT_SPICKLE_PWM, -700);
+	wait_ms(2500);
 	pwm_ng_set(LEFT_SPICKLE_PWM, 0);
 	pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
 	encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0);
diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c
index a16675d..389411e 100644
--- a/projects/microb2010/cobboard/state.c
+++ b/projects/microb2010/cobboard/state.c
@@ -86,25 +86,6 @@ static void state_debug_wait_key_pressed(void)
 	while(!cmdline_keypressed());
 }
 
-/* return true if cob is present */
-static uint8_t state_cob_present(uint8_t side)
-{
-	if (side == I2C_LEFT_SIDE)
-		return sensor_get(S_LCOB);
-	else
-		return sensor_get(S_RCOB);
-}
-
-/* return the detected color of the cob (only valid if present) */
-static uint8_t state_cob_color(uint8_t side)
-{
-	/* XXX no color sensor for now */
-	if (side == I2C_LEFT_SIDE)
-		return I2C_COB_WHITE;
-	else
-		return I2C_COB_WHITE;
-}
-
 /* return true if the cob is correctly inside */
 static uint8_t state_cob_inside(void)
 {
@@ -186,11 +167,7 @@ static void state_do_harvest(uint8_t side)
 		state_status = I2C_COBBOARD_STATUS_RBUSY;
 
 	/* if there is no cob, return */
-	if (state_cob_present(side))
-		return;
-
-	/* if it is black, nothing to do */
-	if (state_cob_color(side) == I2C_COB_BLACK)
+	if (cob_falling_edge(side) == 0)
 		return;
 
 	STMCH_DEBUG("start");
@@ -330,6 +307,7 @@ void state_machine(void)
 			state_do_eject();
 		}
 	}
+	state_status = I2C_COBBOARD_STATUS_READY;
 }
 
 void state_init(void)
diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
index d6794af..4b8aea5 100644
--- a/projects/microb2010/common/i2c_commands.h
+++ b/projects/microb2010/common/i2c_commands.h
@@ -68,6 +68,7 @@ struct i2c_cmd_generic_color {
 
 #define I2C_CMD_COBBOARD_SET_MODE 0x02
 
+/* XXX disabled, use memory sync instead */
 struct i2c_cmd_cobboard_set_mode {
 	struct i2c_cmd_hdr hdr;
 
@@ -77,7 +78,7 @@ struct i2c_cmd_cobboard_set_mode {
 #define I2C_COBBOARD_MODE_R_HARVEST    0x08 /* auto harvest withe cobs */
 #define I2C_COBBOARD_MODE_EJECT        0x10 /* eject cobs */
 #define I2C_COBBOARD_MODE_INIT         0x20 /* init state machine */
-	uint8_t mode;
+	//uint8_t mode;
 };
 
 #define I2C_CMD_BALLBOARD_SET_MODE 0x10
@@ -105,7 +106,7 @@ struct i2c_cmd_ballboard_set_mode {
 
 struct i2c_req_cobboard_status {
 	struct i2c_cmd_hdr hdr;
-	int16_t sickle_left1_current;
+	uint8_t mode;
 };
 
 #define I2C_ANS_COBBOARD_STATUS 0x81
diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile
index 141943f..33b5c52 100755
--- a/projects/microb2010/mainboard/Makefile
+++ b/projects/microb2010/mainboard/Makefile
@@ -6,7 +6,7 @@ AVERSIVE_DIR = ../../..
 SRC  = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c
 SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
 SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
-SRC += strat_utils.c strat_base.c strat.c
+SRC += strat_utils.c strat_base.c strat.c strat_corn.c
 ifeq ($(H),1)
 SRC += robotsim.c
 endif
diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index 8a79c35..7c42411 100644
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -66,6 +66,7 @@
 #include "strat.h"
 #include "strat_utils.h"
 #include "strat_base.h"
+#include "strat_corn.h"
 #include "i2c_protocol.h"
 #include "actuator.h"
 
@@ -1202,6 +1203,65 @@ static void reverse_line(struct line_2pts *l)
 }
 #endif
 
+/* return 1 if there is a corn near, and fill the index ptr */
+static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
+{
+#define SENSOR_CORN_DIST  225
+#define SENSOR_CORN_ANGLE 90
+	double x = position_get_x_double(&mainboard.pos);
+	double y = position_get_y_double(&mainboard.pos);
+	double a_rad = position_get_a_rad_double(&mainboard.pos);
+	double x_corn, y_corn;
+	int16_t x_corn_int, y_corn_int;
+
+	if (side == I2C_LEFT_SIDE) {
+		x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+		y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+	}
+	else {
+		x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+		y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+	}
+	x_corn_int = x_corn;
+	y_corn_int = y_corn;
+
+	*corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
+	if (*corn_idx < 0)
+		return 0;
+	return 1;
+}
+
+/*
+ * - send the correct commands to the spickles
+ * - return 1 if we need to stop (cobboard is stucked)
+*/
+static uint8_t handle_spickles(void)
+{
+	int8_t corn_idx;
+
+	if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
+		i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
+	else {
+		if (corn_table[corn_idx] == TYPE_WHITE_CORN)
+			i2c_cobboard_mode_harvest(I2C_LEFT_SIDE);
+		else
+			i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
+	}
+/* 	printf("%d %d\n", corn_idx, corn_table[corn_idx]); */
+/* 	time_wait_ms(100); */
+
+	if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE))
+		i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE);
+	else {
+		if (corn_table[corn_idx] == TYPE_WHITE_CORN)
+			i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE);
+		else
+			i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
+	}
+
+	return 0;
+}
+
 static void line2line(uint8_t dir1, uint8_t num1,
 		      uint8_t dir2, uint8_t num2)
 {
@@ -1211,6 +1271,7 @@ static void line2line(uint8_t dir1, uint8_t num1,
 	struct line_2pts l1, l2;
 	line_t ll1, ll2;
 	point_t p;
+	uint8_t err;
 
 	/* convert to 2 points */
 	num2line(&l1, dir1, num1);
@@ -1255,11 +1316,22 @@ static void line2line(uint8_t dir1, uint8_t num1,
 		else
 			beta_deg = -60;
 	}
+
 	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));
-	wait_traj_end(0xFF);
+	err = 0;
+	while (err == 0) {
+		err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
+		if (err == 0) {
+			/* cobboard is stucked */
+			trajectory_hardstop(&mainboard.traj);
+			return; /* XXX do something */
+		}
+		err = test_traj_end(0xFF);
+	}
+	return;
 }
 
 /* function called when cmd_test is parsed successfully */
@@ -1269,11 +1341,14 @@ static void cmd_test_parsed(void *parsed_result, void *data)
 	strat_reset_pos(298.48, 309.21, 70.02);
 	mainboard.angle.on = 1;
 	mainboard.distance.on = 1;
+	strat_set_speed(250, SPEED_ANGLE_FAST);
 #endif
+	init_corn_table(0, 0);
 	time_wait_ms(100);
 
 	line2line(LINE_UP, 0, LINE_R_DOWN, 2);
-	line2line(LINE_R_DOWN, 2, LINE_R_UP, 1);
+	line2line(LINE_R_DOWN, 2, LINE_R_UP, 2);
+	line2line(LINE_R_UP, 2, LINE_UP, 5);
 
 	trajectory_hardstop(&mainboard.traj);
 }
diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index edcf494..eb63f03 100644
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -258,14 +258,14 @@ def set_robot():
     robot.axis = axis
     robot.size = (250, 320, ROBOT_HEIGHT)
 
-    lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a-90)*math.pi/180),
-                    robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a-90)*math.pi/180),
+    lspickle.pos = (robot_x - AREA_X/2 + (robot_lspickle*60) * math.cos((robot_a+90)*math.pi/180),
+                    robot_y - AREA_Y/2 + (robot_lspickle*60) * math.sin((robot_a+90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     lspickle.axis = axis
     lspickle.size = (20, 320, 5)
 
-    rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a+90)*math.pi/180),
-                    robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a+90)*math.pi/180),
+    rspickle.pos = (robot_x - AREA_X/2 + (robot_rspickle*60) * math.cos((robot_a-90)*math.pi/180),
+                    robot_y - AREA_Y/2 + (robot_rspickle*60) * math.sin((robot_a-90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     rspickle.axis = axis
     rspickle.size = (20, 320, 5)
@@ -297,7 +297,8 @@ def silent_mkfifo(f):
     except:
         pass
 
-init_corn_table(random.randint(0,8), random.randint(0,3))
+#init_corn_table(random.randint(0,8), random.randint(0,3))
+init_corn_table(0, 0)
 waypoints = init_waypoints()
 toggle_obj_disp()
 
diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index a4aa028..aed92c7 100644
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -272,7 +272,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 			goto error;
 
 		/* status */
-		cobboard.mode = ans->mode;
+		//cobboard.mode = ans->mode;
 		cobboard.status = ans->status;
 		cobboard.left_cobroller_speed = ans->left_cobroller_speed;
 		cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed);
@@ -354,6 +354,7 @@ static int8_t i2c_req_cobboard_status(void)
 	int8_t err;
 
 	buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS;
+	buf.mode = cobboard.mode;
 	err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf,
 			sizeof(buf), I2C_CTRL_GENERIC);
 
@@ -390,20 +391,38 @@ int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
 	return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
 }
 
-int8_t i2c_cobboard_mode_eject(void)
+static int8_t i2c_cobboard_set_mode(uint8_t mode)
 {
+#ifdef HOST_VERSION
+	return robotsim_i2c_cobboard_set_mode(mode);
+#else
+	cobboard.mode = mode;
+	return 0;
+#endif
+
+#if 0 /* old */
 	struct i2c_cmd_cobboard_set_mode buf;
 	buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
 	buf.mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT;
 	return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+#endif
+}
+
+int8_t i2c_cobboard_mode_eject(void)
+{
+	/* XXXXXXXXX bad bad bad */
+	uint8_t mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT;
+	i2c_cobboard_set_mode(mode);
+	time_wait_ms(500);
+	mode = cobboard.mode & (~I2C_COBBOARD_MODE_EJECT);
+	i2c_cobboard_set_mode(mode);
+	return 0;
 }
 
 int8_t i2c_cobboard_mode_harvest(uint8_t side)
 {
-	struct i2c_cmd_cobboard_set_mode buf;
 	uint8_t mode = cobboard.mode;
 
-	buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
 	if (side == I2C_LEFT_SIDE) {
 		mode |= I2C_COBBOARD_MODE_L_DEPLOY;
 		mode |= I2C_COBBOARD_MODE_L_HARVEST;
@@ -412,16 +431,13 @@ int8_t i2c_cobboard_mode_harvest(uint8_t side)
 		mode |= I2C_COBBOARD_MODE_R_DEPLOY;
 		mode |= I2C_COBBOARD_MODE_R_HARVEST;
 	}
-	buf.mode = mode;
-	return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+	return i2c_cobboard_set_mode(mode);
 }
 
 int8_t i2c_cobboard_mode_deploy(uint8_t side)
 {
-	struct i2c_cmd_cobboard_set_mode buf;
 	uint8_t mode = cobboard.mode;
 
-	buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
 	if (side == I2C_LEFT_SIDE) {
 		mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST);
 		mode |= I2C_COBBOARD_MODE_L_DEPLOY;
@@ -430,30 +446,23 @@ int8_t i2c_cobboard_mode_deploy(uint8_t side)
 		mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST);
 		mode |= I2C_COBBOARD_MODE_R_DEPLOY;
 	}
-	buf.mode = mode;
-	return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+	return i2c_cobboard_set_mode(mode);
 }
 
 int8_t i2c_cobboard_mode_pack(uint8_t side)
 {
-	struct i2c_cmd_cobboard_set_mode buf;
 	uint8_t mode = cobboard.mode;
 
-	buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
 	if (side == I2C_LEFT_SIDE)
 		mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST);
 	else
 		mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST);
-	buf.mode = mode;
-	return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+	return i2c_cobboard_set_mode(mode);
 }
 
 int8_t i2c_cobboard_mode_init(void)
 {
-	struct i2c_cmd_cobboard_set_mode buf;
-	buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
-	buf.mode = I2C_COBBOARD_MODE_INIT;
-	return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+	return i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT);
 }
 
 int8_t i2c_ballboard_set_mode(uint8_t mode)
diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h
index 6e3f8ad..21c0065 100755
--- a/projects/microb2010/mainboard/main.h
+++ b/projects/microb2010/mainboard/main.h
@@ -53,13 +53,13 @@
 #define LED1_OFF() 	cbi(PORTJ, 2)
 #define LED1_TOGGLE() 	LED_TOGGLE(PORTJ, 2)
 
-#define LED2_ON() 	sbi(PORTL, 7)
-#define LED2_OFF() 	cbi(PORTL, 7)
-#define LED2_TOGGLE() 	LED_TOGGLE(PORTL, 7)
+#define LED2_ON() 	sbi(PORTJ, 3)
+#define LED2_OFF() 	cbi(PORTJ, 3)
+#define LED2_TOGGLE() 	LED_TOGGLE(PORTJ, 3)
 
-#define LED3_ON() 	sbi(PORTJ, 3)
-#define LED3_OFF() 	cbi(PORTJ, 3)
-#define LED3_TOGGLE() 	LED_TOGGLE(PORTJ, 3)
+#define LED3_ON() 	sbi(PORTL, 7)
+#define LED3_OFF() 	cbi(PORTL, 7)
+#define LED3_TOGGLE() 	LED_TOGGLE(PORTL, 7)
 
 #define LED4_ON() 	sbi(PORTL, 6)
 #define LED4_OFF() 	cbi(PORTL, 6)
diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index 5fc2e37..a0fa5a0 100644
--- a/projects/microb2010/mainboard/robotsim.c
+++ b/projects/microb2010/mainboard/robotsim.c
@@ -91,14 +91,17 @@ robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd)
 	return 0;
 }
 
-static int8_t
-robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
+int8_t
+robotsim_i2c_cobboard_set_mode(uint8_t mode)
 {
 	char buf[BUFSIZ];
 	int len;
 
-	cobboard.mode = cmd->mode;
-	len = snprintf(buf, sizeof(buf), "cobboard=%d\n", cmd->mode);
+	if (cobboard.mode == mode)
+		return 0;
+
+	cobboard.mode = mode;
+	len = snprintf(buf, sizeof(buf), "cobboard=%d\n", mode);
 	hostsim_lock();
 	write(fdw, buf, len);
 	hostsim_unlock();
@@ -127,15 +130,17 @@ robotsim_i2c_ballboard(uint8_t addr, uint8_t *buf, uint8_t size)
 static int8_t
 robotsim_i2c_cobboard(uint8_t addr, uint8_t *buf, uint8_t size)
 {
-	void *void_cmd = buf;
+	//	void *void_cmd = buf;
 
 	switch (buf[0]) {
+#if 0 /* deleted */
 	case I2C_CMD_COBBOARD_SET_MODE:
 		{
 			struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
 			robotsim_i2c_cobboard_set_mode(cmd);
 			break;
 		}
+#endif
 	default:
 		break;
 	}
diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h
index a9316b9..8246500 100644
--- a/projects/microb2010/mainboard/robotsim.h
+++ b/projects/microb2010/mainboard/robotsim.h
@@ -25,3 +25,4 @@ void robotsim_pwm(void *arg, int32_t val);
 int32_t robotsim_encoder_get(void *arg);
 int robotsim_init(void);
 void robotsim_dump(void);
+int8_t robotsim_i2c_cobboard_set_mode(uint8_t mode);
diff --git a/projects/microb2010/mainboard/sensor.c b/projects/microb2010/mainboard/sensor.c
index a6f85de..473ed76 100644
--- a/projects/microb2010/mainboard/sensor.c
+++ b/projects/microb2010/mainboard/sensor.c
@@ -158,14 +158,14 @@ struct sensor_filter {
  * CAP 1,5,6,7,8
  */
 static struct sensor_filter sensor_filter[SENSOR_MAX] = {
-	[S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 4 */
+	[S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */
 	[S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */
-	[S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */
-	[S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */
-	[S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 0 */
-	[S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */
-	[S_DISP_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 6 */
-	[S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */
+	[S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 },    /* 2 */
+	[S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 },   /* 3 */
+	[S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 },  /* 4 */
+	[S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 },  /* 5 */
+	[S_RCOB_WHITE] = { 1, 0, 0, 1, 0, 0 }, /* 6 */
+	[S_LCOB_WHITE] = { 1, 0, 0, 1, 0, 0 }, /* 7 */
 	[S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */
 	[S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */
 	[S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */
diff --git a/projects/microb2010/mainboard/sensor.h b/projects/microb2010/mainboard/sensor.h
index 0df5567..0ab7a3b 100644
--- a/projects/microb2010/mainboard/sensor.h
+++ b/projects/microb2010/mainboard/sensor.h
@@ -36,8 +36,8 @@
 #define S_COLUMN_LEFT  3
 #define S_START_SWITCH 4
 #define S_DISP_LEFT    5
-#define S_DISP_RIGHT   6
-#define S_CAP8         7
+#define S_RCOB_WHITE   6
+#define S_LCOB_WHITE   7
 #define S_RESERVED1    8
 #define S_RESERVED2    9
 #define S_RESERVED3   10
diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 97d159c..1a525e5 100644
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -58,6 +58,7 @@
 #include "main.h"
 #include "strat.h"
 #include "strat_base.h"
+#include "strat_corn.h"
 #include "strat_utils.h"
 #include "sensor.h"
 #include "actuator.h"
@@ -138,12 +139,14 @@ void strat_dump_infos(const char *caller)
  * here */
 void strat_reset_infos(void)
 {
+	init_corn_table(-1, -1);
 }
 
 /* call it just before launching the strat */
 void strat_init(void)
 {
 	/* XXX init rollers, .. */
+
 	strat_reset_infos();
 
 	/* we consider that the color is correctly set */
diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index d852396..0a31073 100644
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -45,20 +45,21 @@
 /*
  *
  *
- *
- *
- *     +------------------------------------+
- *     |      			            |
- *     |      			            |
- *     |      			            |
- *     |      			            | 
- *  y  |      			            |
- *     |      			            |  
- *     |------			      ------| 
- *     |     |			      |     | 
- *     |     |			      |     | 
- *     +-----+------------------------+-----+
- *                       x
+ *           vertical lines
+ *            O     1     2     3     4     5
+ * 2100 +-----|-----|-----|-----|-----|-----|-----+
+ *      |        o           o           o        |
+ *      |  o           o           o           o  |   diag
+ *      |        o           o           o        |   lines
+ *     0/  o           o           o           o  \0
+ *  y   |        o                       o        |
+ *     1/  o                                   o  \1
+ *      |                                         |
+ *     2/------                             ------\2
+ *      |     |                             |     |
+ *      |     |                             |     |
+ *   0  +-----+-----------------------------+-----+
+ *     0                  x                      3000
  */
 
 /* useful traj flags */
@@ -70,8 +71,8 @@
 #define TRAJ_FLAGS_SMALL_DIST (END_TRAJ|END_BLOCKING|END_INTR)
 
 /* default acc */
-#define ACC_DIST  5.
-#define ACC_ANGLE 5.
+#define ACC_DIST  10.
+#define ACC_ANGLE 10.
 
 /* default speeds */
 #define SPEED_DIST_FAST 2500.
diff --git a/projects/microb2010/mainboard/strat_base.h b/projects/microb2010/mainboard/strat_base.h
index 6ff0666..d8b61e5 100644
--- a/projects/microb2010/mainboard/strat_base.h
+++ b/projects/microb2010/mainboard/strat_base.h
@@ -26,8 +26,9 @@
 #define END_OBSTACLE   8 /* There is an obstacle in front of us */
 #define END_ERROR     16 /* Cannot do the command */
 #define END_INTR      32 /* interrupted by user */
-#define END_TIMER     64 /* we don't a lot of time */
-#define END_RESERVED 128 /* reserved */
+#define END_TIMER     64 /* we don't have a lot of time */
+#define END_RESERVED 128 /* reserved... be careful, sometimes error is
+			    coded on a int8_t */
 
 /* only valid after a END_OBSTACLE */
 struct opponent_obstacle {
diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
new file mode 100644
index 0000000..72029b3
--- /dev/null
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -0,0 +1,548 @@
+/*
+ *  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>
+ */
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <math.h>
+
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+
+#include <vect_base.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
+
+*/
+
+#define OFFSET_CORN_X 150
+#define OFFSET_CORN_Y 222
+#define STEP_CORN_X 225
+#define STEP_CORN_Y 250
+
+#define CORN_NB 18
+
+#define WAYPOINTS_NBX 13
+#define WAYPOINTS_NBY 8
+
+/* enum is better */
+#define TYPE_WAYPOINT   0
+#define TYPE_DANGEROUS  1
+#define TYPE_WHITE_CORN 2
+#define TYPE_BLACK_CORN 3
+#define TYPE_OBSTACLE   4
+#define TYPE_UNKNOWN    5
+
+/* 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 point {
+	int32_t x;
+	int32_t y;
+};
+
+struct djpoint {
+	struct point pos;
+	uint16_t weight;
+	struct djpoint *parent;
+
+	uint8_t type:3;
+	uint8_t parent_pos:3;
+	uint8_t updated:1;
+	uint8_t todo:1;
+};
+
+uint8_t corn_table[CORN_NB];
+
+const uint8_t corn_coord_i[CORN_NB] = {
+	0, 0, 0, 2, 2, 2, 4, 4, 6,
+	6, 8, 8, 10, 10, 10, 12, 12, 12,
+};
+
+const uint8_t corn_coord_j[CORN_NB] = {
+	2, 4, 6, 3, 5, 7, 4, 6, 5,
+	7, 4, 6, 3, 5, 7, 2, 4, 6,
+};
+
+static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
+
+/* table to find the symetric idx */
+uint8_t corn_sym[] = {
+	15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2
+};
+
+uint8_t corn_side_confs[9][2] = {
+	{ 1, 4 },
+	{ 0, 4 },
+	{ 2, 4 },
+	{ 2, 3 },
+	{ 0, 3 },
+	{ 1, 3 },
+	{ 1, 6 },
+	{ 0, 6 },
+	{ 2, 6 },
+};
+uint8_t corn_center_confs[4][2] = {
+	{ 5, 8 },
+	{ 7, 8 },
+	{ 5, 9 },
+	{ 7, 8 },
+};
+
+
+/* return index from neigh pointer */
+#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
+
+void dump(void)
+{
+	int8_t i, j;
+	struct djpoint *pt;
+
+	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];
+
+			if (((i+j) & 1) == 0)
+				continue;
+
+			if (pt->type == TYPE_OBSTACLE)
+				printf_P(PSTR("     X  "));
+			else if (pt->type == TYPE_DANGEROUS)
+				printf_P(PSTR("     D  "));
+			else if (pt->type == TYPE_WHITE_CORN)
+				printf_P(PSTR("     W  "));
+			else if (pt->type == TYPE_BLACK_CORN)
+				printf_P(PSTR("     B  "));
+ 			else if (pt->type == TYPE_WAYPOINT)
+				printf_P(PSTR(" %5d  "), pt->weight);
+ 			else
+ 				printf_P(PSTR("     ?  "));
+		}
+		printf_P(PSTR("\r\n"));
+	}
+}
+
+static inline uint8_t opposite_position(uint8_t pos)
+{
+	pos += 3;
+	if (pos > UP_LEFT)
+		pos -= 6;
+	return pos;
+}
+
+/* return coord of the entry in the table from the pointer */
+static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
+{
+	int8_t idx = PT2IDX(pt);
+	*x = idx / WAYPOINTS_NBY;
+	*y = idx % WAYPOINTS_NBY;
+}
+
+/* get the neighbour of the point at specified position */
+static struct djpoint *get_neigh(struct djpoint *pt,
+				 uint8_t position)
+{
+	int8_t i,j;
+	struct djpoint *neigh;
+
+	djpoint2ij(pt, &i, &j);
+
+	switch (position) {
+	case UP:
+		j++;
+		break;
+	case UP_RIGHT:
+		if (!(i & 1)) j++;
+		i++;
+		break;
+	case DOWN_RIGHT:
+		if (i & 1) j--;
+		i++;
+		break;
+	case DOWN:
+		j--;
+		break;
+	case DOWN_LEFT:
+		if (i & 1) j--;
+		i--;
+		break;
+	case UP_LEFT:
+		if (!(i & 1)) j++;
+		i--;
+		break;
+	default:
+		return NULL;
+	}
+	if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
+		return NULL;
+
+	neigh = &djpoints[i][j];
+
+	if (neigh->type != TYPE_WAYPOINT)
+		return NULL;
+
+	return neigh;
+}
+
+static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
+{
+	struct djpoint *neigh = NULL;
+	uint8_t pos = *position + 1;
+
+	for (pos = *position + 1; pos < END; pos++) {
+		neigh = get_neigh(pt, pos);
+		if (neigh != NULL)
+			break;
+	}
+
+	*position = pos;
+	return neigh;
+}
+
+/* browse all points */
+#define POINT_FOREACH(cur)						\
+	for (cur = &djpoints[0][0];					\
+	     cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];		\
+	     cur ++)
+
+/* XXX comment */
+#define NEIGH_FOREACH(neigh, pos, point)			\
+	for (pos = START, neigh = get_next_neigh(point, &pos);	\
+	     neigh;						\
+	     neigh = get_next_neigh(point, &pos))
+
+int dijkstra_init(void)
+{
+	return 0;
+}
+
+static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
+{
+	double vx, vy;
+	vx = p2->pos.x - p1->pos.x;
+	vy = p2->pos.y - p1->pos.y;
+	return sqrt(vx * vx + vy * vy);
+}
+
+void dijkstra_process_neighs(struct djpoint *pt)
+{
+ 	struct djpoint *neigh;
+	uint8_t pos, parent_pos;
+	uint16_t weight;
+	int8_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;
+		}
+	}
+}
+
+int dijkstra(struct djpoint *start)
+{
+ 	struct djpoint *cur;
+	uint8_t todolist = 1;
+
+	start->todo = 1;
+
+	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;
+		}
+
+		/* convert updated list in todo list */
+		todolist = 0;
+		POINT_FOREACH(cur) {
+			if (!cur->updated)
+				continue;
+			todolist = 1;
+			cur->todo = 1;
+			cur->updated = 0;
+		}
+	}
+	return 0; /* XXX */
+}
+
+int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j)
+{
+	uint8_t n;
+	for (n = 0; n < CORN_NB; n ++) {
+		if (i == corn_coord_i[n] &&
+		    j == corn_coord_j[n])
+			return n;
+	}
+	return -1;
+}
+
+int8_t corn_idx_to_coordij(uint8_t idx, uint8_t *i, uint8_t *j)
+{
+	if (idx >= CORN_NB)
+		return -1;
+	*i = corn_coord_i[idx];
+	*j = corn_coord_j[idx];
+	return 0;
+}
+
+/* return the index of the closest corn at these coordinates. If the
+ * corn is really too far (~20cm), return -1. The x and y pointer are
+ * updated with the real position of the corn */
+int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y)
+{
+	uint8_t idx = -1, n, i, j;
+	int16_t d, x_corn, y_corn;
+	int16_t x_corn_min = 0, y_corn_min = 0;
+	int16_t d_min = 0;
+
+	for (n = 0; n < CORN_NB; n ++) {
+		corn_idx_to_coordij(n, &i, &j);
+		x_corn = (OFFSET_CORN_X + i*STEP_CORN_X);
+		y_corn = (OFFSET_CORN_Y + j*STEP_CORN_Y);
+		d = xy_norm(x_corn, y_corn, *x, *y);
+		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)
+		return -1;
+
+	*x = x_corn_min;
+	*y = y_corn_min;
+
+	return idx;
+}
+
+int8_t corn_get_sym(int8_t i)
+{
+	if (i >= CORN_NB)
+		return -1;
+	return corn_sym[i];
+}
+
+void init_corn_table(int8_t conf_side, int8_t conf_center)
+{
+	int8_t sym, i;
+
+	/* before match */
+	if (conf_side == -1) {
+		for (i=0; i<CORN_NB; i++)
+			corn_table[i] = TYPE_UNKNOWN;
+		return;
+	}
+
+	/* for test */
+	printf_P(PSTR("confs = %d, %d\r\n"), conf_side, conf_center);
+	for (i=0; i<CORN_NB; i++) {
+		if (i == corn_side_confs[conf_side][0] ||
+		    i == corn_side_confs[conf_side][1]) {
+			corn_table[i] = TYPE_BLACK_CORN;
+			continue;
+		}
+		sym = corn_get_sym(i);
+		if (sym == corn_side_confs[conf_side][0] ||
+		    sym == corn_side_confs[conf_side][1]) {
+			corn_table[i] = TYPE_BLACK_CORN;
+			continue;
+		}
+		if (i == corn_center_confs[conf_center][0] ||
+		    i == corn_center_confs[conf_center][1]) {
+			corn_table[i] = TYPE_BLACK_CORN;
+			continue;
+		}
+		sym = corn_get_sym(i);
+		if (sym == corn_center_confs[conf_center][0] ||
+		    sym == corn_center_confs[conf_center][1]) {
+			corn_table[i] = TYPE_BLACK_CORN;
+			continue;
+		}
+		corn_table[i] = TYPE_WHITE_CORN;
+	}
+}
+
+/* init waypoints position */
+void init_waypoints(void)
+{
+	int8_t i, j;
+	int32_t x, y;
+	struct djpoint *pt;
+
+	x = OFFSET_CORN_X;
+	for (i=0; i<WAYPOINTS_NBX; i++) {
+
+		if (i & 1)
+			y = OFFSET_CORN_Y - STEP_CORN_Y/2;
+		else
+			y = OFFSET_CORN_Y;
+
+		for (j=0; j<WAYPOINTS_NBY; j++) {
+			pt = &djpoints[i][j];
+
+			pt->pos.x = x;
+			pt->pos.y = y;
+
+			pt->type = TYPE_WAYPOINT;
+			pt->parent_pos = END;
+			pt->updated = 0;
+			pt->todo = 0;
+
+			y += STEP_CORN_Y;
+		}
+
+		x += STEP_CORN_X;
+	}
+}
+
+/* update the type and weight of waypoints, before starting
+ * dijkstra */
+void update_waypoints(void)
+{
+	int8_t i, j, c;
+	struct djpoint *pt;
+
+	for (i=0; i<WAYPOINTS_NBX; i++) {
+
+		for (j=0; j<WAYPOINTS_NBY; j++) {
+			pt = &djpoints[i][j];
+
+			pt->weight = 0;
+
+			/* corn */
+			c = ijcoord_to_corn_idx(i, j);
+			if (c >= 0) {
+				pt->type = corn_table[c];
+				continue;
+			}
+			/* too close of border */
+			if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
+				pt->type = TYPE_OBSTACLE;
+				continue;
+			}
+			/* hill */
+			if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
+				pt->type = TYPE_OBSTACLE;
+				continue;
+			}
+			/* dangerous points */
+			if (i == 0 || i == (WAYPOINTS_NBX-1)) {
+				pt->type = TYPE_DANGEROUS;
+				continue;
+			}
+			if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
+				pt->type = TYPE_DANGEROUS;
+				continue;
+			}
+			pt->type = TYPE_WAYPOINT;
+		}
+	}
+}
+
+int get_path(struct djpoint *start, struct djpoint *end)
+{
+ 	struct djpoint *cur;
+	uint8_t prev_direction = 0;
+
+	for (cur=start;
+	     cur != NULL && cur->parent_pos != END && cur != end;
+	     cur = get_neigh(cur, cur->parent_pos)) {
+		if (prev_direction != cur->parent_pos) {
+			printf_P(PSTR("%d %d (%d)\r\n"),
+				 cur->pos.x, cur->pos.y,
+				 cur->parent_pos);
+		}
+		prev_direction = cur->parent_pos;
+	}
+	printf_P(PSTR("%d %d\r\n"), end->pos.x, end->pos.y);
+
+	return 0; /* XXX */
+}
+
+#if 0
+int main(void)
+{
+ 	struct djpoint *start;
+ 	struct djpoint *end;
+
+	start = &djpoints[1][1];
+	end = &djpoints[12][1];
+
+	init_corn_table(0, 0);
+	init_waypoints();
+	update_waypoints();
+
+	dijkstra(end);
+	dump();
+
+	get_path(start, end);
+
+	return 0;
+}
+#endif
diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h
new file mode 100644
index 0000000..8ab2398
--- /dev/null
+++ b/projects/microb2010/mainboard/strat_corn.h
@@ -0,0 +1,36 @@
+/*
+ *  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>
+ */
+
+#define CORN_NB 18
+
+/* enum is better */
+#define TYPE_WAYPOINT 0
+#define TYPE_DANGEROUS 1
+#define TYPE_WHITE_CORN 2
+#define TYPE_BLACK_CORN 3
+#define TYPE_OBSTACLE 4
+
+extern uint8_t corn_table[CORN_NB];
+
+int8_t ijcoord_to_corn_idx(int8_t i, int8_t j);
+int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y);
+void init_corn_table(int8_t conf_side, int8_t conf_center);