From b699727efceb661be65fbd22e975565b800c628e Mon Sep 17 00:00:00 2001
From: zer0 <zer0@carbon.local>
Date: Sun, 9 May 2010 16:25:35 +0200
Subject: [PATCH] update and reliabilize strats

---
 projects/microb2010/mainboard/commands.c      |   2 -
 .../microb2010/mainboard/commands_mainboard.c |  72 ----
 projects/microb2010/mainboard/commands_traj.c | 270 +-------------
 projects/microb2010/mainboard/display.py      |   4 +-
 projects/microb2010/mainboard/i2c_protocol.c  |   4 +-
 projects/microb2010/mainboard/main.c          |   1 -
 projects/microb2010/mainboard/robotsim.c      |  16 +-
 projects/microb2010/mainboard/strat.c         | 350 ++++++++++++++++--
 projects/microb2010/mainboard/strat.h         |   7 +-
 projects/microb2010/mainboard/strat_avoid.c   | 177 +++++++--
 projects/microb2010/mainboard/strat_avoid.h   |   2 +-
 projects/microb2010/mainboard/strat_base.c    |  11 +-
 projects/microb2010/mainboard/strat_base.h    |   1 +
 projects/microb2010/mainboard/strat_corn.c    |   8 +-
 projects/microb2010/mainboard/strat_db.c      |  25 +-
 projects/microb2010/mainboard/strat_db.h      |  11 +-
 projects/microb2010/mainboard/strat_utils.c   |  11 +
 projects/microb2010/mainboard/strat_utils.h   |   7 +
 18 files changed, 579 insertions(+), 400 deletions(-)

diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c
index 50be5fa..a49cb89 100644
--- a/projects/microb2010/mainboard/commands.c
+++ b/projects/microb2010/mainboard/commands.c
@@ -83,7 +83,6 @@ extern parse_pgm_inst_t cmd_servo_balls;
 extern parse_pgm_inst_t cmd_clitoid;
 extern parse_pgm_inst_t cmd_time_monitor;
 extern parse_pgm_inst_t cmd_strat_event;
-extern parse_pgm_inst_t cmd_climb;
 extern parse_pgm_inst_t cmd_sleep;
 extern parse_pgm_inst_t cmd_test;
 
@@ -179,7 +178,6 @@ parse_pgm_ctx_t main_ctx[] = {
 	(parse_pgm_inst_t *)&cmd_clitoid,
 	(parse_pgm_inst_t *)&cmd_time_monitor,
 	(parse_pgm_inst_t *)&cmd_strat_event,
-	(parse_pgm_inst_t *)&cmd_climb,
 	(parse_pgm_inst_t *)&cmd_sleep,
 	(parse_pgm_inst_t *)&cmd_test,
 
diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index 713fedf..b43f05b 100644
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -1298,75 +1298,3 @@ parse_pgm_inst_t cmd_test = {
 		NULL,
 	},
 };
-
-
-
-
-/**********************************************************/
-/* Climb */
-
-/* this structure is filled when cmd_climb is parsed successfully */
-struct cmd_climb_result {
-	fixed_string_t arg0;
-	int32_t radius;
-	int32_t dist;
-};
-
-
-//#define DIM_BALLS_DOWN 620
-//#define DIM_BALLS_UP 700
-
-#define DIM_BALLS_DOWN 530
-#define DIM_BALLS_UP 700
-
-/* function called when cmd_climb is parsed successfully */
-static void cmd_climb_parsed(void *parsed_result, void *data)
-{
-	uint8_t err;
-
-	printf_P(PSTR("starting Clifenger\r\n"));
-	strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW );
-
-	trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
-	err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-	trajectory_a_abs(&mainboard.traj, 0);
-	err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
-
-
-	trajectory_goto_xy_abs(&mainboard.traj, 1250, 250);
-	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_DOWN, 0xFF);
-	printf_P(PSTR("ball_pack\r\n"));
-	support_balls_pack();
-	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_UP, 0xFF);
-	printf_P(PSTR("ball_deploy\r\n"));
-	support_balls_deploy();
-	err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
-	/* reach top, go down */
-	trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
-	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_UP, 0xFF);
-	printf_P(PSTR("ball_pack\r\n"));
-	support_balls_pack();
-	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_DOWN, 0xFF);
-	printf_P(PSTR("ball_deploy\r\n"));
-	support_balls_deploy();
-
-	err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-}
-
-prog_char str_climb_arg0[] = "climb";
-parse_pgm_token_string_t cmd_climb_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_climb_result, arg0, str_climb_arg0);
-parse_pgm_token_num_t cmd_climb_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, radius, INT32);
-parse_pgm_token_num_t cmd_climb_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, dist, INT32);
-
-prog_char help_climb[] = "Climb function";
-parse_pgm_inst_t cmd_climb = {
-	.f = cmd_climb_parsed,  /* function to call */
-	.data = NULL,      /* 2nd arg of func */
-	.help_str = help_climb,
-	.tokens = {        /* token list, NULL terminated */
-		(prog_void *)&cmd_climb_arg0,
-		NULL,
-	},
-};
diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
index 1898bd8..0dff9ed 100644
--- a/projects/microb2010/mainboard/commands_traj.c
+++ b/projects/microb2010/mainboard/commands_traj.c
@@ -1042,24 +1042,8 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
 	else
 		on = 0;
 
-#if 0
-	if (!strcmp_P(res->arg1, PSTR("one_temple_on_disc")))
-		bit = STRAT_CONF_ONLY_ONE_ON_DISC;
-	else if (!strcmp_P(res->arg1, PSTR("bypass_static2")))
-		bit = STRAT_CONF_BYPASS_STATIC2;
-	else if (!strcmp_P(res->arg1, PSTR("take_one_lintel")))
-		bit = STRAT_CONF_TAKE_ONE_LINTEL;
-	else if (!strcmp_P(res->arg1, PSTR("skip_when_check_fail")))
-		bit = STRAT_CONF_TAKE_ONE_LINTEL;
-	else if (!strcmp_P(res->arg1, PSTR("store_static2")))
-		bit = STRAT_CONF_STORE_STATIC2;
-	else if (!strcmp_P(res->arg1, PSTR("big3_temple")))
-		bit = STRAT_CONF_BIG_3_TEMPLE;
-	else if (!strcmp_P(res->arg1, PSTR("early_opp_scan")))
-		bit = STRAT_CONF_EARLY_SCAN;
-	else if (!strcmp_P(res->arg1, PSTR("push_opp_cols")))
-		bit = STRAT_CONF_PUSH_OPP_COLS;
-#endif
+	if (!strcmp_P(res->arg1, PSTR("our_orange")))
+		bit = STRAT_CONF_OUR_ORANGE;
 
 	if (on)
 		strat_conf.flags |= bit;
@@ -1072,7 +1056,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
 
 prog_char str_strat_conf2_arg0[] = "strat_conf";
 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
-prog_char str_strat_conf2_arg1[] = "faux";
+prog_char str_strat_conf2_arg1[] = "our_orange";
 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
 prog_char str_strat_conf2_arg2[] = "on#off";
 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);
@@ -1104,43 +1088,25 @@ struct cmd_strat_conf3_result {
 /* function called when cmd_strat_conf3 is parsed successfully */
 static void cmd_strat_conf3_parsed(void *parsed_result, void *data)
 {
-#if 0
 	struct cmd_strat_conf3_result *res = parsed_result;
 
-	if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
-		if (res->arg2 > 90)
-			res->arg2 = 90;
-		strat_conf.scan_opp_min_time = res->arg2;
-	}
-	else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
+	if (!strcmp_P(res->arg1, PSTR("orphan_tomato"))) {
 		if (res->arg2 > 90)
 			res->arg2 = 90;
-		strat_conf.delay_between_opp_scan = res->arg2;
+		strat_conf.orphan_tomato = res->arg2;
 	}
-	else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
+	else if (!strcmp_P(res->arg1, PSTR("opp_orange"))) {
 		if (res->arg2 > 90)
 			res->arg2 = 90;
-		strat_conf.scan_our_min_time = res->arg2;
+		strat_conf.opp_orange = res->arg2;
 	}
-	else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
-		if (res->arg2 > 90)
-			res->arg2 = 90;
-		strat_conf.delay_between_our_scan = res->arg2;
-	}
-	else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
-		strat_conf.wait_opponent = res->arg2;
-	}
-	else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
-		strat_conf.lintel_min_time = res->arg2;
-	}
-#endif
 	strat_conf.dump_enabled = 1;
 	strat_conf_dump(__FUNCTION__);
 }
 
 prog_char str_strat_conf3_arg0[] = "strat_conf";
 parse_pgm_token_string_t cmd_strat_conf3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg0, str_strat_conf3_arg0);
-prog_char str_strat_conf3_arg1[] = "faux2";
+prog_char str_strat_conf3_arg1[] = "orphan_tomato#opp_orange";
 parse_pgm_token_string_t cmd_strat_conf3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf3_result, arg1, str_strat_conf3_arg1);
 parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT16);
 
@@ -1161,221 +1127,10 @@ parse_pgm_inst_t cmd_strat_conf3 = {
 /**********************************************************/
 /* Subtraj */
 
-//////////////////////
-
-// 500 -- 5
-// 400 -- 3
-#define TEST_SPEED 400
-#define TEST_ACC 3
-
-static void line2line(double line1x1, double line1y1,
-		      double line1x2, double line1y2,
-		      double line2x1, double line2y1,
-		      double line2x2, double line2y2,
-		      double radius, double dist)
-{
-	uint8_t err;
-	double speed_d, speed_a;
-	double distance, angle;
-	double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
-	double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
-
-	printf_P(PSTR("%s()\r\n"), __FUNCTION__);
-
-	strat_set_speed(TEST_SPEED, TEST_SPEED);
-	quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
-
-	circle_get_da_speed_from_radius(&mainboard.traj, radius,
-					&speed_d, &speed_a);
-	trajectory_line_abs(&mainboard.traj,
-			    line1x1, line1y1,
-			    line1x2, line1y2, 150.);
-	err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
-				    dist, TRAJ_FLAGS_NO_NEAR);
-	/* circle */
-	strat_set_speed(speed_d, speed_a);
-	angle = line2_angle - line1_angle;
-	distance = angle * radius;
-	if (distance < 0)
-		distance = -distance;
-	angle = simple_modulo_2pi(angle);
-	angle = DEG(angle);
-	printf_P(PSTR("(%d,%d,%d) "),
-		 position_get_x_s16(&mainboard.pos),
-		 position_get_y_s16(&mainboard.pos),
-		 position_get_a_deg_s16(&mainboard.pos));
-	printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
-		 distance, angle);
-
-	/* take some margin on dist to avoid deceleration */
-	trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
-
-	/* circle exit condition */
-	err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
-				    TRAJ_FLAGS_NO_NEAR);
-
-	strat_set_speed(500, 500);
-	printf_P(PSTR("(%d,%d,%d) "),
-		 position_get_x_s16(&mainboard.pos),
-		 position_get_y_s16(&mainboard.pos),
-		 position_get_a_deg_s16(&mainboard.pos));
-	printf_P(PSTR("line\r\n"));
-	trajectory_line_abs(&mainboard.traj,
-			    line2x1, line2y1,
-			    line2x2, line2y2, 150.);
-}
-
-static void halfturn(double line1x1, double line1y1,
-		     double line1x2, double line1y2,
-		     double line2x1, double line2y1,
-		     double line2x2, double line2y2,
-		     double radius, double dist, double dir)
-{
-	uint8_t err;
-	double speed_d, speed_a;
-	double distance, angle;
-
-	printf_P(PSTR("%s()\r\n"), __FUNCTION__);
-
-	strat_set_speed(TEST_SPEED, TEST_SPEED);
-	quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
-
-	circle_get_da_speed_from_radius(&mainboard.traj, radius,
-					&speed_d, &speed_a);
-	trajectory_line_abs(&mainboard.traj,
-			    line1x1, line1y1,
-			    line1x2, line1y2, 150.);
-	err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
-				    dist, TRAJ_FLAGS_NO_NEAR);
-	/* circle */
-	strat_set_speed(speed_d, speed_a);
-	angle = dir * M_PI/2.;
-	distance = angle * radius;
-	if (distance < 0)
-		distance = -distance;
-	angle = simple_modulo_2pi(angle);
-	angle = DEG(angle);
-
-	/* take some margin on dist to avoid deceleration */
-	DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
-	      distance, angle);
-	trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
-
-	/* circle exit condition */
-	err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
-				    TRAJ_FLAGS_NO_NEAR);
-
-	DEBUG(E_USER_STRAT, "miniline");
-	err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
-				    dist, TRAJ_FLAGS_NO_NEAR);
-	DEBUG(E_USER_STRAT, "circle2");
-	/* take some margin on dist to avoid deceleration */
-	trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
-
-	err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
-				    TRAJ_FLAGS_NO_NEAR);
-
-	strat_set_speed(500, 500);
-	DEBUG(E_USER_STRAT, "line");
-	trajectory_line_abs(&mainboard.traj,
-			    line2x1, line2y1,
-			    line2x2, line2y2, 150.);
-}
-
 
 /* function called when cmd_test is parsed successfully */
 static void subtraj_test(void)
 {
-#ifdef HOST_VERSION
-	strat_reset_pos(400, 400, 90);
-	mainboard.angle.on = 1;
-	mainboard.distance.on = 1;
-#endif
-	printf_P(PSTR("%s()\r\n"), __FUNCTION__);
-	while (!cmdline_keypressed()) {
-		/****** PASS1 */
-
-#define DIST_HARD_TURN   260
-#define RADIUS_HARD_TURN 100
-#define DIST_EASY_TURN   190
-#define RADIUS_EASY_TURN 190
-#define DIST_HALF_TURN   225
-#define RADIUS_HALF_TURN 130
-
-		/* hard turn */
-		line2line(375, 597, 375, 1847,
-			  375, 1847, 1050, 1472,
-			  RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-		/* easy left and easy right !*/
-		line2line(825, 1596, 1050, 1472,
-			  1050, 1472, 1500, 1722,
-			  RADIUS_EASY_TURN, DIST_EASY_TURN);
-		line2line(1050, 1472, 1500, 1722,
-			  1500, 1722, 2175, 1347,
-			  RADIUS_EASY_TURN, DIST_EASY_TURN);
-		line2line(1500, 1722, 2175, 1347,
-			  2175, 1347, 2175, 847,
-			  RADIUS_EASY_TURN, DIST_EASY_TURN);
-
-		/* half turns */
-		halfturn(2175, 1347, 2175, 722,
-			 2625, 722, 2625, 1597,
-			 RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
-		halfturn(2625, 847, 2625, 1722,
-			  2175, 1722, 2175, 1097,
-			  RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
-
-		/* easy turns */
-		line2line(2175, 1597, 2175, 1097,
-			  2175, 1097, 1500, 722,
-			  RADIUS_EASY_TURN, DIST_EASY_TURN);
-		line2line(2175, 1097, 1500, 722,
-			  1500, 722, 1050, 972,
-			  RADIUS_EASY_TURN, DIST_EASY_TURN);
-		line2line(1500, 722, 1050, 972,
-			  1050, 972, 375, 597,
-			  RADIUS_EASY_TURN, DIST_EASY_TURN);
-
-		/* hard turn */
-		line2line(1050, 972, 375, 597,
-			  375, 597, 375, 1097,
-			  RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-		/****** PASS2 */
-
-		/* easy turn */
-		line2line(375, 597, 375, 1097,
-			  375, 1097, 1050, 1472,
-			  RADIUS_EASY_TURN, DIST_EASY_TURN);
-
-		/* hard turn */
-		line2line(375, 1097, 1050, 1472,
-			  1050, 1472, 375, 1847,
-			  RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-		/* hard turn */
-		line2line(1050, 1472, 375, 1847,
-			  375, 1847, 375, 1347,
-			  RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-		/* easy turn */
-		line2line(375, 1847, 375, 1347,
-			  375, 1347, 1050, 972,
-			  RADIUS_EASY_TURN, DIST_EASY_TURN);
-
-		/* hard turn */
-		line2line(375, 1347, 1050, 972,
-			  1050, 972, 375, 597,
-			  RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-		/* hard turn */
-		line2line(1050, 972, 375, 597,
-			  375, 597, 375, 1847,
-			  RADIUS_HARD_TURN, DIST_HARD_TURN);
-
-	}
-	trajectory_hardstop(&mainboard.traj);
 }
 
 
@@ -1396,13 +1151,20 @@ static void cmd_subtraj_parsed(void *parsed_result, void *data)
 
 	if (!strcmp_P(res->arg1, PSTR("test")))
 		subtraj_test();
+	else if (!strcmp_P(res->arg1, PSTR("orange")))
+		subtraj_test();
+	else if (!strcmp_P(res->arg1, PSTR("tomato"))) {
+		position_set(&mainboard.pos, 2625,
+			     COLOR_Y(1847), COLOR_A(0.00));
+		get_orphan_tomatoes();
+	}
 
 	trajectory_hardstop(&mainboard.traj);
 }
 
 prog_char str_subtraj_arg0[] = "subtraj";
 parse_pgm_token_string_t cmd_subtraj_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg0, str_subtraj_arg0);
-prog_char str_subtraj_arg1[] = "faux";
+prog_char str_subtraj_arg1[] = "test#orange#tomato";
 parse_pgm_token_string_t cmd_subtraj_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_subtraj_result, arg1, str_subtraj_arg1);
 parse_pgm_token_num_t cmd_subtraj_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg2, INT32);
 parse_pgm_token_num_t cmd_subtraj_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_subtraj_result, arg3, INT32);
diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index 7445e81..476a5ba 100644
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -305,7 +305,7 @@ def set_robot():
     lspickle.axis = axis
     lspickle.size = (20, ROBOT_WIDTH, 5)
     if robot_lspickle_autoharvest:
-        lspickle.color = (1, 0, 0)
+        lspickle.color = (0.2, 0.2, 1)
     else:
         lspickle.color = (0.4, 0.4, 0.4)
 
@@ -315,7 +315,7 @@ def set_robot():
     rspickle.axis = axis
     rspickle.size = (20, ROBOT_WIDTH, 5)
     if robot_rspickle_autoharvest:
-        rspickle.color = (1, 0, 0)
+        rspickle.color = (0.2, 0.2, 1)
     else:
         rspickle.color = (0.4, 0.4, 0.4)
 
diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index 22970c0..442cf3a 100644
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -404,13 +404,11 @@ int8_t i2c_cobboard_set_mode(uint8_t mode)
 {
 #ifdef HOST_VERSION
 	cobboard.mode = mode;
-	return 0;
-#else
+#endif
 	struct i2c_cmd_cobboard_set_mode buf;
 	buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
 	buf.mode = mode;
 	return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-#endif
 }
 
 static int8_t i2c_cobboard_set_spickle(uint8_t side, uint8_t flags)
diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c
index d7ac3d4..7690838 100755
--- a/projects/microb2010/mainboard/main.c
+++ b/projects/microb2010/mainboard/main.c
@@ -321,7 +321,6 @@ int main(void)
 	sei();
 
 	strat_db_init();
-	test_strat_avoid();
 
 	printf_P(PSTR("\r\n"));
 	printf_P(PSTR("Respect et robustesse.\r\n"));
diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index 1b103d8..df8ee0e 100644
--- a/projects/microb2010/mainboard/robotsim.c
+++ b/projects/microb2010/mainboard/robotsim.c
@@ -105,12 +105,22 @@ robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd)
 
 	ballboard.mode = cmd->mode;
 	len = snprintf(buf, sizeof(buf), "ballboard=%d\n", cmd->mode);
+	if (cmd->mode == I2C_BALLBOARD_MODE_EJECT)
+		ballboard.ball_count = 0;
 	hostsim_lock();
 	write(fdw, buf, len);
 	hostsim_unlock();
 	return 0;
 }
 
+static int8_t
+robotsim_i2c_cobboard_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
+{
+	if (cmd->mode == I2C_COBBOARD_MODE_EJECT)
+		cobboard.cob_count = 0;
+	return 0;
+}
+
 int8_t
 robotsim_i2c_cobboard_set_spickles(uint8_t side, uint8_t flags)
 {
@@ -159,17 +169,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/strat.c b/projects/microb2010/mainboard/strat.c
index 9b58196..19c6883 100644
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -40,6 +40,8 @@
 #include <quadramp.h>
 #include <control_system_manager.h>
 #include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
+#include <trajectory_manager_core.h>
 #include <vect_base.h>
 #include <lines.h>
 #include <polygon.h>
@@ -72,7 +74,12 @@ static volatile uint8_t strat_running = 0;
 volatile uint8_t strat_want_pack = 0;
 volatile uint8_t strat_lpack60 = 0;
 volatile uint8_t strat_rpack60 = 0;
-struct strat_conf strat_conf;
+struct strat_conf strat_conf = {
+	.dump_enabled = 0,
+	.opp_orange = 90,
+	.orphan_tomato = 50,
+	.flags = 0,
+};
 
 /*************************************************************/
 
@@ -99,7 +106,10 @@ void strat_conf_dump(const char *caller)
 		return;
 
 	printf_P(PSTR("-- conf --\r\n"));
-
+	printf_P(PSTR("opp_orange = %d\n"), strat_conf.opp_orange);
+	printf_P(PSTR("orphan_tomato = %d\n"), strat_conf.orphan_tomato);
+	printf_P(PSTR("our_orange = %s\n"),
+		 (strat_conf.flags & STRAT_CONF_OUR_ORANGE) ? "y":"n");
 }
 
 void strat_event_enable(void)
@@ -160,6 +170,25 @@ static void check_tomato(void)
 {
 	int16_t x, y;
 	uint8_t i, j;
+	static uint8_t prev_check_time;
+	uint8_t cur_time;
+
+	/* check present tomatoes once per second */
+	cur_time = time_get_s();
+	if (cur_time != prev_check_time) {
+		uint8_t k;
+
+		for (k = 0; k < TOMATO_NB; k++) {
+			if (strat_db.tomato_table[k]->time_removed != -1 &&
+			    strat_db.tomato_table[k]->time_removed + 2 == time_get_s()) {
+#ifdef HOST_VERSION
+				printf("remove tomato %d\n", k);
+#endif
+				strat_db.tomato_table[k]->present = 0;
+			}
+		}
+		prev_check_time = cur_time;
+	}
 
 	x = position_get_x_s16(&mainboard.pos);
 	y = position_get_y_s16(&mainboard.pos);
@@ -170,7 +199,15 @@ static void check_tomato(void)
 	if (strat_db.wp_table[i][j].type != WP_TYPE_TOMATO)
 		return;
 
+	if (strat_db.wp_table[i][j].present == 0)
+		return;
+
+	strat_db.wp_table[i][j].time_removed = time_get_s();
 	strat_db.wp_table[i][j].present = 0;
+#ifdef HOST_VERSION
+	ballboard.ball_count ++;
+	printf("add ball %d,%d\n", i, j);
+#endif
 }
 
 /* mark corn as not present and give correct commands to the cobboard
@@ -181,6 +218,8 @@ static void check_corn(void)
 	int8_t lcob_near, rcob_near;
 	uint8_t lcob, rcob;
 	uint8_t lidx, ridx;
+	static uint8_t prev_check_time;
+	uint8_t cur_time;
 
 	/* read sensors from ballboard */
 	IRQ_LOCK(flags);
@@ -190,20 +229,24 @@ static void check_corn(void)
 	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 */
 
+	/* check present cobs once per second */
+	cur_time = time_get_s();
+	if (cur_time != prev_check_time) {
+		uint8_t i;
+
+		for (i = 0; i < CORN_NB; i++) {
+			if (strat_db.corn_table[i]->time_removed != -1 &&
+			    strat_db.corn_table[i]->time_removed + 2 == time_get_s()) {
 #ifdef HOST_VERSION
-	if (time_get_s() == 15)
-		cobboard.cob_count = 5;
-	if (time_get_s() == 16)
-		cobboard.cob_count = 0;
+				printf("remove cob %d\n", i);
 #endif
+				strat_db.corn_table[i]->present = 0;
+			}
+		}
+		prev_check_time = cur_time;
+	}
 
 	/* detect cob on left side */
 	lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE);
@@ -232,8 +275,17 @@ static void check_corn(void)
 			/* deploy spickle and harvest white ones */
 			if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) {
 				i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE);
-				if (cobboard.status == I2C_COBBOARD_STATUS_LBUSY)
-					strat_db.corn_table[lidx]->present = 0;
+				if (strat_db.corn_table[lidx]->time_removed == -1
+#ifndef HOST_VERSION
+				    && cobboard.status == I2C_COBBOARD_STATUS_LBUSY
+#endif
+				    ) {
+					strat_db.corn_table[lidx]->time_removed = time_get_s();
+#ifdef HOST_VERSION
+					cobboard.cob_count ++;
+					printf("add cob %d\n", lidx);
+#endif
+				}
 			}
 			else
 				i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
@@ -256,8 +308,17 @@ static void check_corn(void)
 			/* deploy spickle and harvest white ones */
 			if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) {
 				i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE);
-				if (cobboard.status == I2C_COBBOARD_STATUS_RBUSY)
-					strat_db.corn_table[ridx]->present = 0;
+				if (strat_db.corn_table[ridx]->time_removed == -1
+#ifndef HOST_VERSION
+				    && cobboard.status == I2C_COBBOARD_STATUS_RBUSY
+#endif
+				    ) {
+					strat_db.corn_table[ridx]->time_removed = time_get_s();
+#ifdef HOST_VERSION
+					cobboard.cob_count ++;
+					printf("add cob %d\n", ridx);
+#endif
+				}
 			}
 			else
 				i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
@@ -348,7 +409,8 @@ static uint8_t strat_eject(void)
 {
 	uint8_t err;
 
-	DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+	DEBUG(E_USER_STRAT, "%s() cob_count=%d ball_count=%d",
+	      __FUNCTION__, get_cob_count(), get_ball_count());
 
 	/* check that we are called from an eject line */
 	if (!robot_is_on_eject_line()) {
@@ -403,10 +465,16 @@ static uint8_t strat_eject(void)
 
 	if (get_cob_count() > 0) {
 		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
-		strat_db_dump(__FUNCTION__);
 		time_wait_ms(2000);
 	}
 
+	/* cob ejection */
+	trajectory_d_rel(&mainboard.traj, 70);
+	err = wait_traj_end(END_INTR|END_TRAJ);
+	if (!TRAJ_SUCCESS(err))
+		goto fail;
+
+	strat_db_dump(__FUNCTION__);
 	err = END_TRAJ;
 
  fail:
@@ -415,16 +483,18 @@ static uint8_t strat_eject(void)
 	return err;
 }
 
-static uint8_t strat_beginning(void)
+static uint8_t strat_beginning(uint8_t do_initturn)
 {
 	uint8_t err;
 
 	strat_set_acc(ACC_DIST, ACC_ANGLE);
-	strat_set_speed(600, 60); /* OK */
 
-	trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
-	err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
-				    TRAJ_FLAGS_STD);
+	if (do_initturn) {
+		strat_set_speed(600, 60); /* OK */
+		trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
+		err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+					    TRAJ_FLAGS_STD);
+	}
 
 	strat_set_acc(ACC_DIST, ACC_ANGLE);
 	strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
@@ -476,25 +546,251 @@ static uint8_t need_more_elements(void)
 }
 #endif
 
-uint8_t strat_main(void)
+/* get tomatoes near our goals (12,5 and 12,3) */
+uint8_t get_orphan_tomatoes(void)
+{
+#define CLITOID_TOMATO_RADIUS 100.
+#define TOMATO_BACK_X 2760
+#define TOMATO_BACK_LEN 200
+
+	int16_t x, y, a;
+	uint8_t i, j;
+	uint8_t err;
+	int8_t ret;
+
+	DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+	/* only go if both tomatoes are present */
+	if (!strat_db.wp_table[12][5].present ||
+	    !strat_db.wp_table[12][3].present) {
+		return END_TRAJ;
+	}
+
+	x = position_get_x_s16(&mainboard.pos);
+	y = position_get_y_s16(&mainboard.pos);
+
+	if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
+		return END_ERROR;
+
+	/* not on eject point */
+	if (i != 11 || j != 6)
+		return END_ERROR;
+
+	strat_want_pack = 1;
+	strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
+
+	/* turn in the correct direction */
+	trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+	/* clitoid to turn and take the first ball */
+	ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
+				 COLOR_A(-90), 150., COLOR_A(90), 0,
+				 CLITOID_TOMATO_RADIUS, 3*125);
+	if (ret < 0) {
+		err = END_ERROR;
+		goto fail;
+	}
+
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	if (!TRAJ_SUCCESS(err))
+		goto fail;
+
+	strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
+	err = strat_calib(300, END_TRAJ|END_BLOCKING);
+	a = position_get_a_deg_s16(&mainboard.pos);
+	if (ABS(a) < 10)
+		strat_reset_pos(AREA_X - ROBOT_HALF_LENGTH_FRONT,
+				DO_NOT_SET_POS,
+				COLOR_A(0));
+
+	strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
+	trajectory_d_rel(&mainboard.traj, -250);
+	err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
+				    TRAJ_FLAGS_NO_NEAR);
+#ifdef HOST_VERSION /* simulator sees 2 blockings */
+	if (err == END_BLOCKING) {
+		trajectory_d_rel(&mainboard.traj, -250);
+		err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
+					    TRAJ_FLAGS_NO_NEAR);
+	}
+#endif
+
+	if (err != 0 && !TRAJ_SUCCESS(err))
+		goto fail;
+
+	trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+	/* next ball */
+
+	/* clitoid to turn and take the first ball */
+	strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
+	ret = trajectory_clitoid(&mainboard.traj, 2625, COLOR_Y(1847),
+				 COLOR_A(-90), 150., COLOR_A(90), 0,
+				 CLITOID_TOMATO_RADIUS, 7*125);
+	if (ret < 0) {
+		err = END_ERROR;
+		goto fail;
+	}
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	if (!TRAJ_SUCCESS(err))
+		goto fail;
+
+	strat_hardstop();
+	strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_FAST);
+	trajectory_d_rel(&mainboard.traj, -250);
+	err = WAIT_COND_OR_TRAJ_END(!x_is_more_than(TOMATO_BACK_X),
+				    TRAJ_FLAGS_NO_NEAR);
+	trajectory_d_a_rel(&mainboard.traj, -TOMATO_BACK_LEN, COLOR_A(-90));
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+fail:
+	strat_want_pack = 0;
+	return err;
+}
+
+/* get oranges, must be called near game area */
+uint8_t run_to_the_hills(uint8_t orange_color)
 {
+#define HILL_LEN 500
+#define HILL_POSY_YELLOW 300
+#define HILL_POSY_BLUE 200
+#define HILL_POSX_BALLS_DOWN 900
+#define HILL_START_POSX 580
+
+	double aa, ad;
+	uint16_t sa, sd;
 	uint8_t err;
+	uint8_t our_color = get_color();
+	int16_t startx, starty;
+
+	strat_want_pack = 1;
+	strat_get_acc(&ad, &aa);
+	strat_get_speed(&sd, &sa);
+
+	DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+	strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+	if (orange_color == I2C_COLOR_YELLOW)
+		starty = HILL_POSY_YELLOW;
+	else
+		starty = HILL_POSY_BLUE;
+	if (orange_color == our_color)
+		startx = HILL_START_POSX;
+	else
+		startx = AREA_X - HILL_START_POSX;
+	trajectory_goto_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
+	err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+	/* turn to the hills */
+	if (orange_color == our_color)
+		trajectory_a_abs(&mainboard.traj, COLOR_A(0));
+	else
+		trajectory_a_abs(&mainboard.traj, COLOR_A(180));
+	err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+	strat_set_acc(3, 3);
+	strat_set_speed(300, 500);
+	bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 40);
+	support_balls_pack();
+
+	/* here it is difficult to handle return values, because we
+	 * are on the hill */
+	trajectory_d_rel(&mainboard.traj, HILL_LEN);
+	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) > HILL_POSX_BALLS_DOWN,
+				    TRAJ_FLAGS_NO_NEAR);
+	printf_P(PSTR("deploy\r\n"));
+	support_balls_deploy();
+	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND);
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+	time_wait_ms(2000);
+
+	/* reach top, go down */
+	trajectory_d_rel(&mainboard.traj, -HILL_LEN);
+
+	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < HILL_POSX_BALLS_DOWN,
+				    TRAJ_FLAGS_NO_NEAR);
+	printf_P(PSTR("pack\r\n"));
+	support_balls_pack();
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	printf_P(PSTR("deploy\r\n"));
+	support_balls_deploy();
+
+	/* calibrate position on the wall */
+	strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+	strat_set_acc(ad, aa);
+	trajectory_goto_xy_abs(&mainboard.traj, 150, COLOR_Y(starty));
+	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 300,
+				    TRAJ_FLAGS_NO_NEAR);
+	strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+	err = strat_calib(-300, TRAJ_FLAGS_NO_NEAR);
+	if (orange_color == our_color)
+		position_set(&mainboard.pos, ROBOT_HALF_LENGTH_REAR,
+			     DO_NOT_SET_POS, COLOR_A(0));
+	else
+		position_set(&mainboard.pos, AREA_X - ROBOT_HALF_LENGTH_REAR,
+			     DO_NOT_SET_POS, COLOR_A(180));
+	strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+	trajectory_goto_xy_abs(&mainboard.traj, 150, COLOR_Y(starty));
+	err = wait_traj_end(TRAJ_FLAGS_STD);
+
+	/* revert acceleration and speed */
+	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+	strat_want_pack = 0;
+	strat_set_speed(sd, sa);
+	bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
+	support_balls_deploy();
+	return err;
+}
+
+uint8_t strat_main(void)
+{
+	uint8_t err, do_initturn = 1;
+
+	/* get oranges */
+	if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) {
+		err = run_to_the_hills(get_color());
+		do_initturn = 0;
+	}
 
 	/* harvest the first cobs + balls */
-	err = strat_beginning();
+	err = strat_beginning(do_initturn);
+
 	if (!TRAJ_SUCCESS(err))
 		strat_unblock();
 	else
 		err = strat_eject();
 
+	/* choose circuit, and harvest on it */
 	while (1) {
 
+		DEBUG(E_USER_STRAT, "start main loop");
+
+		/* if it's time to get tomatoes, do it */
+		if (time_get_s() > strat_conf.orphan_tomato) {
+			err = get_orphan_tomatoes();
+			if (err == END_ERROR) {
+				DEBUG(E_USER_STRAT,
+				      "get_orphan_tomatoes returned END_ERROR");
+			}
+			else if (err == END_TIMER) {
+				DEBUG(E_USER_STRAT, "End of time");
+				strat_exit();
+				break;
+			}
+			else if (!TRAJ_SUCCESS(err)) {
+				/* don't retry these tomatoes if it failed */
+				strat_conf.orphan_tomato = 90;
+				strat_unblock();
+			}
+		}
+
 		/**********************/
 		/* harvest on circuit */
 		/**********************/
 
-		DEBUG(E_USER_STRAT, "start main loop");
-
 		err = strat_harvest_circuit();
 		if (err == END_TIMER) {
 			DEBUG(E_USER_STRAT, "End of time");
diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index 2be7651..3305f1f 100644
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -150,7 +150,10 @@
 struct strat_conf {
 	uint8_t dump_enabled;
 
-#define STRAT_CONF_XXX   0x01
+	uint8_t opp_orange;
+	uint8_t orphan_tomato;
+
+#define STRAT_CONF_OUR_ORANGE   0x01
 	uint8_t flags;
 };
 
@@ -171,5 +174,7 @@ uint8_t strat_main(void);
 void strat_event(void *dummy);
 void strat_event_enable(void);
 void strat_event_disable(void);
+uint8_t run_to_the_hills(uint8_t orange_color);
+uint8_t get_orphan_tomatoes(void);
 
 #endif
diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c
index 5211c4f..2658b46 100644
--- a/projects/microb2010/mainboard/strat_avoid.c
+++ b/projects/microb2010/mainboard/strat_avoid.c
@@ -315,6 +315,39 @@ const struct circuit letter_v_circuit = {
 	.path = letter_v_tab,
 };
 
+const struct wp_coord sperma_tab[] = {
+	{ .i = 11, .j = 6, },
+	{ .i = 10, .j = 6, },
+	{ .i = 9, .j = 5, },
+	{ .i = 8, .j = 5, },
+	{ .i = 7, .j = 5, },
+	{ .i = 6, .j = 6, },
+	{ .i = 5, .j = 5, },
+	{ .i = 4, .j = 5, },
+	{ .i = 3, .j = 4, },
+	{ .i = 2, .j = 4, },
+	{ .i = 1, .j = 3, },
+	{ .i = 1, .j = 4, },
+	{ .i = 1, .j = 5, },
+	{ .i = 1, .j = 6, },
+	{ .i = 2, .j = 6, },
+	{ .i = 3, .j = 5, },
+	{ .i = 4, .j = 5, },
+	{ .i = 5, .j = 5, },
+	{ .i = 6, .j = 6, },
+	{ .i = 7, .j = 5, },
+	{ .i = 8, .j = 5, },
+	{ .i = 9, .j = 5, },
+	{ .i = 10, .j = 6, },
+	{ .i = 11, .j = 6, },
+};
+
+const struct circuit sperma_circuit = {
+	.name = "sperma",
+	.len = sizeof(sperma_tab)/sizeof(struct wp_coord),
+	.path = sperma_tab,
+};
+
 /* list of all possible circuits */
 const struct circuit *circuits[] = {
 	&butterfly_circuit,
@@ -325,6 +358,7 @@ const struct circuit *circuits[] = {
 	&asym_butterfly_circuit,
 	&big_h_lambda_circuit,
 	&letter_v_circuit,
+	&sperma_circuit,
 	NULL,
 };
 
@@ -434,19 +468,33 @@ int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj,
 	return 0;
 }
 
-static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
+static int8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
 {
+	uint8_t mod;
+
 	switch (dir) {
 	case LINE_UP:
 	case LINE_DOWN:
+		if ((i & 1) == 0)
+			return -1;
 		return i/2;
 	case LINE_R_UP:
 	case LINE_L_DOWN:
+		mod = i & 3;
+		if ((mod == 0 || mod == 1) && ((j & 1) == 0))
+			return -1;
+		if ((mod == 2 || mod == 3) && ((j & 1) == 1))
+			return -1;
 		i &= 0xfe;
 		j -= i/2;
 		return (5-j)/2;
 	case LINE_R_DOWN:
 	case LINE_L_UP:
+		mod = i & 3;
+		if ((mod == 0 || mod == 3) && ((j & 1) == 0))
+			return -1;
+		if ((mod == 1 || mod == 2) && ((j & 1) == 1))
+			return -1;
 		i &= 0xfe;
 		j += i/2;
 		return (11-j)/2;
@@ -493,11 +541,34 @@ static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j,
 	return 0xFF;
 }
 
+/* return approximative angle of line */
+int16_t linedir2angle(uint8_t dir)
+{
+	switch (dir) {
+	case LINE_UP:
+		return COLOR_A(90);
+	case LINE_DOWN:
+		return COLOR_A(-90);
+	case LINE_R_UP:
+		return COLOR_A(30);
+	case LINE_R_DOWN:
+		return COLOR_A(-90);
+	case LINE_L_UP:
+		return COLOR_A(150);
+	case LINE_L_DOWN:
+		return COLOR_A(-150);
+	default:
+		return 0;
+	}
+}
+
 /* return true if a waypoint belongs to a line */
 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
 {
-	uint8_t ln;
+	int8_t ln;
 	ln = get_line_num(i, j, dir);
+	if (ln == -1)
+		return 0;
 	if (ln == linenum)
 		return 1;
 	return 0;
@@ -514,10 +585,11 @@ uint8_t corn_count_neigh(uint8_t i, uint8_t j)
 		if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
 			continue;
 
-		/* is there a corn cob ? */
+		/* is there a corn cob removed for more than 2 secs ? */
 		if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
-		    strat_db.wp_table[ni][nj].present &&
-		    strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK)
+		    strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK &&
+		    (strat_db.wp_table[ni][nj].present ||
+		     strat_db.wp_table[ni][nj].time_removed + 2 > time_get_s()))
 			n ++;
 	}
 
@@ -656,8 +728,8 @@ static int16_t get_score(uint32_t wcorn_retrieved,
 
 	/* malus if there is opponent on the path */
 	if (opp_on_path) {
-		DPR("malus for opponent: 1000\r\n");
-		score -= 2000;
+		DPR("malus for opponent: %d\r\n", (500 * opp_on_path));
+		score -= (500 * opp_on_path);
 	}
 
 	return score;
@@ -697,7 +769,8 @@ static int8_t evaluate_one_face(const struct circuit *circuit,
 	uint8_t ni = 0, nj = 0;
 	uint8_t dir, color, idx;
 	int8_t step = faceA ? 1 : -1;
-	int16_t x, y, d, prev_d = 0;
+	int16_t x, y;
+	int32_t d, prev_d = 0;
 	int16_t oppx, oppy;
 
 	*score = 0x8000; /* -int_max */
@@ -744,11 +817,13 @@ static int8_t evaluate_one_face(const struct circuit *circuit,
 		/* is opponent near the point ? */
 		ijcoord_to_xycoord(i, j, &x, &y);
 		if (oppx != I2C_OPPONENT_NOT_THERE) {
-			d = distance_between(oppx, oppy, x, y);
+			d = quad_distance_between(oppx, oppy, x, y);
 			DPR("%s(): opp at %d mm (ij=%d,%d opp=%d,%d pos=%d,%d)\r\n",
 			    __FUNCTION__, d, i, j, oppx, oppy, x, y);
-			if (d < 600 && d < prev_d)
-				opponent_on_path = 1;
+			if (d < (250L*250L) && d < prev_d)
+				opponent_on_path += 3;
+			else if (d < (500L*500L) && d < prev_d)
+				opponent_on_path ++;
 			prev_d = d;
 		}
 
@@ -865,7 +940,7 @@ static int8_t find_best_circuit(uint8_t i, uint8_t j,
 	return found;
 }
 
-static void test_all_circuits(void)
+static void init_all_circuits(void)
 {
 	const struct circuit **circuit;
 	const struct wp_coord *cur;
@@ -887,6 +962,8 @@ static void test_all_circuits(void)
 			i = cur->i;
 			j = cur->j;
 
+			strat_db.wp_table[i][j].on_circuit = 1;
+
 			dir = get_dir(prev_i, prev_j, i, j);
 			if (dir == 0xFF)
 				printf_P("Bad circuit %s %d %d\r\n", (*circuit)->name, i, j);
@@ -922,6 +999,9 @@ uint8_t strat_harvest_circuit(void)
 	uint8_t err;
 
 	strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
+	strat_want_pack = 1;
+
+	printf("PACK PACK\n");
 
 	x = position_get_x_s16(&mainboard.pos);
 	y = position_get_y_s16(&mainboard.pos);
@@ -929,20 +1009,23 @@ uint8_t strat_harvest_circuit(void)
 	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;
+		err = END_ERROR;
+		goto fail;
 	}
 
 	if (find_best_circuit(i, j, &selected_circuit, &selected_face) < 0) {
 		DEBUG(E_USER_STRAT, "%s(): cannot find a good circuit",
 		      __FUNCTION__);
-		return END_ERROR;
+		err = END_ERROR;
+		goto fail;
 	}
 
 	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;
+		err = END_ERROR;
+		goto fail;
 	}
 
 	dump_circuit_wp(circuit_wpline, len);
@@ -950,6 +1033,15 @@ uint8_t strat_harvest_circuit(void)
 	prev_linenum = circuit_wpline[0].line_num;
 	prev_dir = circuit_wpline[0].dir;
 
+	/* fix orientation first */
+	trajectory_a_abs(&mainboard.traj, linedir2angle(prev_dir));
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	if (!TRAJ_SUCCESS(err))
+		goto fail;
+
+	strat_want_pack = 0;
+	printf("UNPACK UNPACK\n");
+
 	/* do all lines of circuit */
 	for (idx = 1; idx < len; idx ++) {
 		linenum = circuit_wpline[idx].line_num;
@@ -960,22 +1052,40 @@ uint8_t strat_harvest_circuit(void)
 		err = line2line(prev_linenum, prev_dir, linenum, dir,
 				TRAJ_FLAGS_NO_NEAR);
 		if (!TRAJ_SUCCESS(err))
-			return err;
+			goto fail;
 
 		prev_linenum = linenum;
 		prev_dir = dir;
 	}
+	err = END_TRAJ;
 
-	return END_TRAJ;
+ fail:
+	strat_want_pack = 0;
+	return err;
 }
 
+/* list of waypoints when we are not on a circuit */
+const struct xy_point unblock_pts[] = {
+	{ .x = 375, .y = 597 },  /* 1,1 */
+	{ .x = 2625, .y = 597 }, /* 11,1 */
+	{ .x = 1500, .y = 722 }, /* 6,2 */
+	{ .x = 375, .y = 1097 }, /* 1,3 */
+	{ .x = 375, .y = 1597 }, /* 1,5 */
+	{ .x = 2625, .y = 1097 }, /* 11,3 */
+	{ .x = 2625, .y = 1597 }, /* 11,5 */
+	{ .x = 1500, .y = 1722 }, /* 6,6 */
+};
+
+
 /* try to unblock in any situation */
 uint8_t strat_unblock(void)
 {
 	int16_t x, y;
-	uint8_t i, j;
+	uint8_t i, j, k;
 	uint16_t old_dspeed, old_aspeed;
 	uint8_t err;
+	uint16_t d_min = 0xFFFF, d;
+	const struct xy_point *pt;
 
 	DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
 
@@ -986,11 +1096,28 @@ uint8_t strat_unblock(void)
 	strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 	x = position_get_x_s16(&mainboard.pos);
 	y = position_get_y_s16(&mainboard.pos);
-	if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
-		/* aie... go to center... but it's really a bad
-		 * idea */
-		x = CENTER_X;
-		y = CENTER_Y;
+
+	if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
+		x = -1;
+	else if (strat_db.wp_table[i][j].on_circuit == 0)
+		x = -1;
+
+	/* find the nearest unblock point */
+	if (x == -1) {
+		/* position may have been modified */
+		x = position_get_x_s16(&mainboard.pos);
+		y = position_get_y_s16(&mainboard.pos);
+
+		/* browse all points and find the nearest */
+		for (k = 0; k < sizeof(unblock_pts)/sizeof(*unblock_pts); k++) {
+			pt = &unblock_pts[k];
+			d = distance_between(x, y, pt->x, COLOR_Y(pt->y));
+			if (d < d_min) {
+				d_min = d;
+				x = pt->x;
+				y = COLOR_Y(pt->y);
+			}
+		}
 	}
 
 	/* XXX if opponent is too close, go back, or wait ? */
@@ -1009,9 +1136,9 @@ uint8_t strat_unblock(void)
 	return END_TRAJ;
 }
 
-void test_strat_avoid(void)
+void strat_avoid_init(void)
 {
-	test_all_circuits();
+	init_all_circuits();
 
 #ifdef TEST_STRAT_AVOID
 	uint8_t i, j;
diff --git a/projects/microb2010/mainboard/strat_avoid.h b/projects/microb2010/mainboard/strat_avoid.h
index 56de996..f729741 100644
--- a/projects/microb2010/mainboard/strat_avoid.h
+++ b/projects/microb2010/mainboard/strat_avoid.h
@@ -60,6 +60,6 @@ uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir);
 uint8_t is_60deg(uint8_t dir1, uint8_t dir2);
 uint8_t is_120deg(uint8_t dir1, uint8_t dir2);
 
-void test_strat_avoid(void);
+void strat_avoid_init(void);
 
 #endif
diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index 1a89f4d..acded68 100644
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -225,6 +225,15 @@ void strat_get_speed(uint16_t *d, uint16_t *a)
 	IRQ_UNLOCK(flags);
 }
 
+void strat_get_acc(double *d, double *a)
+{
+	uint8_t flags;
+	IRQ_LOCK(flags);
+	*d = mainboard.traj.d_acc;
+	*a = mainboard.traj.a_acc;
+	IRQ_UNLOCK(flags);
+}
+
 void strat_limit_speed_enable(void)
 {
 	strat_limit_speed_enabled = 1;
@@ -355,7 +364,7 @@ uint8_t strat_obstacle(void)
 	y_rel = sin(RAD(opp_a)) * (double)opp_d;
 
 	/* opponent too far */
-	if (opp_d > 600)
+	if (opp_d > 500)
 		return 0;
 
 	/* opponent is in front of us */
diff --git a/projects/microb2010/mainboard/strat_base.h b/projects/microb2010/mainboard/strat_base.h
index 908a2f6..bcabf2b 100644
--- a/projects/microb2010/mainboard/strat_base.h
+++ b/projects/microb2010/mainboard/strat_base.h
@@ -67,6 +67,7 @@ uint8_t strat_obstacle(void);
 void strat_set_speed(uint16_t d, uint16_t a);
 void strat_get_speed(uint16_t *d, uint16_t *a);
 void strat_set_acc(double d, double a);
+void strat_get_acc(double *d, double *a);
 
 /* when user type ctrl-c we can interrupt traj */
 void interrupt_traj(void);
diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index 77c9a51..e64871e 100644
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -171,10 +171,12 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
 	if (corn_count_neigh(i, j) == 2)
 		return 1;
 
-	/* we are on intersection, let's go slow... but as we enter in
-	 * the curve-part of the clitoid, we should not go there */
+
+	/* we are on intersection, keep the same speed... but as we
+	 * enter in the curve-part of the clitoid, we should not go
+	 * there */
 	if (wp_belongs_to_line(i, j, num2, dir2))
-		return 0;
+		return clitoid_slow;
 
 	/* we can ge fast if it's a 60deg angle and if we checked the
 	 * current point */
diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c
index 9fe4329..c3608d6 100644
--- a/projects/microb2010/mainboard/strat_db.c
+++ b/projects/microb2010/mainboard/strat_db.c
@@ -60,6 +60,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 +88,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 },
@@ -467,6 +470,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;
@@ -491,7 +497,21 @@ void strat_db_init(void)
 				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;
 			}
 
@@ -535,4 +555,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();
 }
diff --git a/projects/microb2010/mainboard/strat_db.h b/projects/microb2010/mainboard/strat_db.h
index b770b43..61ee6bf 100644
--- a/projects/microb2010/mainboard/strat_db.h
+++ b/projects/microb2010/mainboard/strat_db.h
@@ -54,16 +54,19 @@ struct waypoint_db {
 	/* visited by opponent */
 	uint8_t opp_visited:1;
 
-	uint8_t reserved:3;
+	/* true if the wp is on a circuit */
+	uint8_t on_circuit:1;
 
-	/* absolute position of the waypoint */
-/* 	int16_t x; */
-/* 	int16_t y; */
+	uint8_t reserved:2;
 
 	union {
 		struct corn_db corn;
 		struct tomato_db tomato;
 	};
+
+	/* to monitor time when corn/ball was removed */
+	/* not optimal... but... we have enough ram */
+	int8_t time_removed;
 };
 
 /* database reflecting the status of objects on area */
diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c
index 1f19904..35803ac 100644
--- a/projects/microb2010/mainboard/strat_utils.c
+++ b/projects/microb2010/mainboard/strat_utils.c
@@ -68,6 +68,17 @@ int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
 	return sqrt(x+y);
 }
 
+/* return the distance between two points */
+int32_t quad_distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2)
+{
+	int32_t x,y;
+	x = (x2-x1);
+	x = x*x;
+	y = (y2-y1);
+	y = y*y;
+	return x+y;
+}
+
 /* return the distance to a point in the area */
 int16_t distance_from_robot(int16_t x, int16_t y)
 {
diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h
index 49b58b0..f314b0e 100644
--- a/projects/microb2010/mainboard/strat_utils.h
+++ b/projects/microb2010/mainboard/strat_utils.h
@@ -33,10 +33,17 @@ struct xy_point {
 		while ( (! (cond)) && (__err == 0)) {			\
 			__err = test_traj_end(mask);			\
 		}							\
+		if (!__err)						\
+			DEBUG(E_USER_STRAT, "cond is true at line %d",	\
+			      __LINE__);				\
+		else							\
+			DEBUG(E_USER_STRAT, "got %s at line %d",	\
+			      get_err(__err), __LINE__);		\
 		__err;							\
 	})								\
 
 int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
+int32_t quad_distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
 int16_t distance_from_robot(int16_t x, int16_t y);
 int16_t simple_modulo_360(int16_t a);
 int16_t angle_abs_to_rel(int16_t a_abs);
-- 
2.39.5