From: zer0 <zer0@carbon.local>
Date: Mon, 10 May 2010 16:09:37 +0000 (+0200)
Subject: oranges and enhance strats
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=28da5858ac871c626153f47566e968ecb05ff52b;p=aversive.git

oranges and enhance strats
---

diff --git a/projects/microb2010/ballboard/actuator.c b/projects/microb2010/ballboard/actuator.c
index d745fa0..0dc89ff 100644
--- a/projects/microb2010/ballboard/actuator.c
+++ b/projects/microb2010/ballboard/actuator.c
@@ -47,13 +47,10 @@
 #define ROLLER_OFF     0
 #define ROLLER_REVERSE ROLLER_SPEED
 
-#define FORKROT_DEPLOYED -50000
+#define FORKROT_DEPLOYED -55000
+#define FORKROT_MID -33000
 #define FORKROT_PACKED   0
 
-#define FORKTRANS_LEFT   0
-#define FORKTRANS_MIDDLE 500
-#define FORKTRANS_RIGHT  1000
-
 void roller_on(void)
 {
 	cs_set_consign(&ballboard.roller.cs, ROLLER_ON);
@@ -79,17 +76,18 @@ void fork_pack(void)
 	cs_set_consign(&ballboard.forkrot.cs, FORKROT_PACKED);
 }
 
-void fork_left(void)
-{
-	cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_LEFT);
-}
-
-void fork_right(void)
+void fork_mid(void)
 {
-	cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_RIGHT);
+	cs_set_consign(&ballboard.forkrot.cs, FORKROT_MID);
 }
 
-void fork_middle(void)
+void actuator_init(void)
 {
-	cs_set_consign(&ballboard.forktrans.cs, FORKTRANS_MIDDLE);
+	printf_P(PSTR("fork autopos..."));
+	pwm_ng_set(FORKROT_PWM, 400);
+	wait_ms(1000);
+	pwm_ng_set(FORKROT_PWM, 0);
+	encoders_spi_set_value(FORKROT_ENCODER, 0);
+	printf_P(PSTR("ok\r\n"));
+	ballboard.forkrot.on = 1;
 }
diff --git a/projects/microb2010/ballboard/actuator.h b/projects/microb2010/ballboard/actuator.h
index 039cb1d..bd93ddc 100644
--- a/projects/microb2010/ballboard/actuator.h
+++ b/projects/microb2010/ballboard/actuator.h
@@ -27,6 +27,6 @@ void roller_reverse(void);
 
 void fork_deploy(void);
 void fork_pack(void);
-void fork_left(void);
-void fork_right(void);
-void fork_middle(void);
+void fork_mid(void);
+
+void actuator_init(void);
diff --git a/projects/microb2010/ballboard/commands_ballboard.c b/projects/microb2010/ballboard/commands_ballboard.c
index ba570a4..70ae084 100644
--- a/projects/microb2010/ballboard/commands_ballboard.c
+++ b/projects/microb2010/ballboard/commands_ballboard.c
@@ -195,13 +195,17 @@ static void cmd_state1_parsed(void *parsed_result,
 		state_set_mode(I2C_BALLBOARD_MODE_EJECT);
 	else if (!strcmp_P(res->arg1, PSTR("harvest")))
 		state_set_mode(I2C_BALLBOARD_MODE_HARVEST);
+	else if (!strcmp_P(res->arg1, PSTR("prepare")))
+		state_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
+	else if (!strcmp_P(res->arg1, PSTR("take")))
+		state_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
 
 	/* other commands */
 }
 
 prog_char str_state1_arg0[] = "ballboard";
 parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
-prog_char str_state1_arg1[] = "init#eject#harvest#off";
+prog_char str_state1_arg1[] = "init#eject#harvest#off#prepare#take";
 parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
 
 prog_char help_state1[] = "set ballboard mode";
@@ -230,27 +234,11 @@ struct cmd_state2_result {
 static void cmd_state2_parsed(void *parsed_result,
 			      __attribute__((unused)) void *data)
 {
-	struct cmd_state2_result *res = parsed_result;
-	uint8_t mode;
-
-	if (!strcmp_P(res->arg2, PSTR("left"))) {
-		if (!strcmp_P(res->arg1, PSTR("prepare")))
-			mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
-		else if (!strcmp_P(res->arg1, PSTR("take")))
-			mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
-	}
-	else {
-		if (!strcmp_P(res->arg1, PSTR("prepare")))
-			mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
-		else if (!strcmp_P(res->arg1, PSTR("take")))
-			mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
-	}
-	//state_set_mode(mode);
 }
 
 prog_char str_state2_arg0[] = "ballboard";
 parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
-prog_char str_state2_arg1[] = "prepare#take";
+prog_char str_state2_arg1[] = "xxx";
 parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
 prog_char str_state2_arg2[] = "left#right";
 parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
@@ -391,17 +379,13 @@ static void cmd_fork_parsed(void *parsed_result,
 		fork_deploy();
 	else if (!strcmp_P(res->arg1, PSTR("pack")))
 		fork_pack();
-	else if (!strcmp_P(res->arg1, PSTR("left")))
-		fork_left();
-	else if (!strcmp_P(res->arg1, PSTR("right")))
-		fork_right();
 	else if (!strcmp_P(res->arg1, PSTR("middle")))
-		fork_middle();
+		fork_mid();
 }
 
 prog_char str_fork_arg0[] = "fork";
 parse_pgm_token_string_t cmd_fork_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg0, str_fork_arg0);
-prog_char str_fork_arg1[] = "deploy#pack#left#right#middle";
+prog_char str_fork_arg1[] = "deploy#pack#middle";
 parse_pgm_token_string_t cmd_fork_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg1, str_fork_arg1);
 
 prog_char help_fork[] = "move fork";
diff --git a/projects/microb2010/ballboard/cs.c b/projects/microb2010/ballboard/cs.c
index ed5c0ff..6bfd518 100644
--- a/projects/microb2010/ballboard/cs.c
+++ b/projects/microb2010/ballboard/cs.c
@@ -178,7 +178,7 @@ void microb_cs_init(void)
 
 	/* QUADRAMP */
 	quadramp_init(&ballboard.forkrot.qr);
-	quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 800, 800); /* set speed */
+	quadramp_set_1st_order_vars(&ballboard.forkrot.qr, 200, 800); /* set speed */
 	quadramp_set_2nd_order_vars(&ballboard.forkrot.qr, 20, 20); /* set accel */
 
 	/* CS */
@@ -197,7 +197,7 @@ void microb_cs_init(void)
 	/* set them on !! */
 	ballboard.roller.on = 1;
 	ballboard.forktrans.on = 1;
-	ballboard.forkrot.on = 1;
+	ballboard.forkrot.on = 0;
 
 
 	scheduler_add_periodical_event_priority(do_cs, NULL,
diff --git a/projects/microb2010/ballboard/main.c b/projects/microb2010/ballboard/main.c
index 925109f..e737ba3 100755
--- a/projects/microb2010/ballboard/main.c
+++ b/projects/microb2010/ballboard/main.c
@@ -212,8 +212,8 @@ int main(void)
 		      &PORTD, 4);
 	PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED,
 		      &PORTD, 5);
-	PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
-		      &PORTD, 6);
+	PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED |
+		      PWM_NG_MODE_SIGN_INVERTED, &PORTD, 6);
 	PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
 		      &PORTD, 7);
 
@@ -255,6 +255,8 @@ int main(void)
 
 	sei();
 
+	actuator_init();
+
 	printf_P(PSTR("\r\n"));
 	printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
 
diff --git a/projects/microb2010/ballboard/state.c b/projects/microb2010/ballboard/state.c
index a322d17..e777e49 100644
--- a/projects/microb2010/ballboard/state.c
+++ b/projects/microb2010/ballboard/state.c
@@ -64,10 +64,8 @@ static volatile uint8_t ball_count;
 #define OFF I2C_BALLBOARD_MODE_OFF
 #define HARVEST I2C_BALLBOARD_MODE_HARVEST
 #define EJECT I2C_BALLBOARD_MODE_EJECT
-#define PREP_L_FORK I2C_BALLBOARD_MODE_PREP_L_FORK
-#define TAKE_L_FORK I2C_BALLBOARD_MODE_TAKE_L_FORK
-#define PREP_R_FORK I2C_BALLBOARD_MODE_PREP_R_FORK
-#define TAKE_R_FORK I2C_BALLBOARD_MODE_TAKE_R_FORK
+#define PREP_FORK I2C_BALLBOARD_MODE_PREP_FORK
+#define TAKE_FORK I2C_BALLBOARD_MODE_TAKE_FORK
 
 uint8_t state_debug = 0;
 
@@ -216,6 +214,7 @@ void state_machine(void)
 
 		case INIT:
 			state_init();
+			fork_pack();
 			state_mode = OFF;
 			state_status = I2C_BALLBOARD_STATUS_F_READY;
 			break;
@@ -223,20 +222,35 @@ void state_machine(void)
 		case OFF:
 			state_status = I2C_BALLBOARD_STATUS_F_READY;
 			roller_off();
+			fork_pack();
 			break;
 
 		case HARVEST:
 			state_status = I2C_BALLBOARD_STATUS_F_READY;
+			fork_pack();
 			state_do_harvest();
 			break;
 
 		case EJECT:
 			state_status = I2C_BALLBOARD_STATUS_F_BUSY;
+			fork_pack();
 			state_do_eject();
 			state_status = I2C_BALLBOARD_STATUS_F_READY;
 			state_mode = HARVEST;
 			break;
 
+		case PREP_FORK:
+			roller_off();
+			fork_deploy();
+			break;
+
+		case TAKE_FORK:
+			roller_off();
+			fork_mid();
+			time_wait_ms(1300);
+			state_mode = OFF;
+			break;
+
 		default:
 			break;
 		}
diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c
index 3f37356..9f8b562 100644
--- a/projects/microb2010/cobboard/commands_cobboard.c
+++ b/projects/microb2010/cobboard/commands_cobboard.c
@@ -193,8 +193,10 @@ static void cmd_state1_parsed(void *parsed_result,
 		state_init();
 	else if (!strcmp_P(res->arg1, PSTR("eject")))
 		state_set_mode(I2C_COBBOARD_MODE_EJECT);
-	else if (!strcmp_P(res->arg1, PSTR("kickstand")))
-		state_set_mode(I2C_COBBOARD_MODE_KICKSTAND);
+	else if (!strcmp_P(res->arg1, PSTR("kickstand_up")))
+		state_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
+	else if (!strcmp_P(res->arg1, PSTR("kickstand_down")))
+		state_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
 	else if (!strcmp_P(res->arg1, PSTR("ignore_i2c")))
 		state_set_i2c_ignore(1);
 	else if (!strcmp_P(res->arg1, PSTR("care_i2c")))
@@ -205,7 +207,7 @@ static void cmd_state1_parsed(void *parsed_result,
 
 prog_char str_state1_arg0[] = "cobboard";
 parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
-prog_char str_state1_arg1[] = "init#eject#ignore_i2c#care_i2c#kickstand";
+prog_char str_state1_arg1[] = "init#eject#ignore_i2c#care_i2c#kickstand_up#kickstand_down";
 parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
 
 prog_char help_state1[] = "set cobboard mode";
diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c
index 42f734c..1f38e1a 100644
--- a/projects/microb2010/cobboard/shovel.c
+++ b/projects/microb2010/cobboard/shovel.c
@@ -46,7 +46,8 @@
 #define SHOVEL_DOWN 100
 #define SHOVEL_MID  4500
 #define SHOVEL_UP   11000
-#define SHOVEL_KICKSTAND 12800
+#define SHOVEL_KICKSTAND_UP   12800
+#define SHOVEL_KICKSTAND_DOWN 10000
 
 static int32_t shovel_k1 = 1000;
 static int32_t shovel_k2 = 20;
@@ -121,6 +122,7 @@ void shovel_set(void *mot, int32_t cmd)
 
 void shovel_down(void)
 {
+	shovel_current_limit_enable(0);
 	quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2500, 2500);
 	quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 50, 80);
 	cs_set_consign(&cobboard.shovel.cs, SHOVEL_DOWN);
@@ -128,6 +130,7 @@ void shovel_down(void)
 
 void shovel_mid(void)
 {
+	shovel_current_limit_enable(0);
 	quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2500, 2500);
 	quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 80, 80);
 	cs_set_consign(&cobboard.shovel.cs, SHOVEL_MID);
@@ -135,6 +138,7 @@ void shovel_mid(void)
 
 void shovel_up(void)
 {
+	shovel_current_limit_enable(0);
 	if (state_get_cob_count() <= 1)
 		quadramp_set_1st_order_vars(&cobboard.shovel.qr, 1000, 2500);
 	else
@@ -143,11 +147,22 @@ void shovel_up(void)
 	cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP);
 }
 
-void shovel_kickstand(void)
+void shovel_kickstand_up(void)
 {
+	shovel_set_current_limit_coefs(1000, 20);
+	shovel_current_limit_enable(1);
 	quadramp_set_1st_order_vars(&cobboard.shovel.qr, 200, 200);
 	quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 10, 10);
-	cs_set_consign(&cobboard.shovel.cs, SHOVEL_KICKSTAND);
+	cs_set_consign(&cobboard.shovel.cs, SHOVEL_KICKSTAND_UP);
+}
+
+void shovel_kickstand_down(void)
+{
+	shovel_set_current_limit_coefs(500, 0);
+	shovel_current_limit_enable(1);
+	quadramp_set_1st_order_vars(&cobboard.shovel.qr, 200, 200);
+	quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 10, 10);
+	cs_set_consign(&cobboard.shovel.cs, SHOVEL_KICKSTAND_DOWN);
 }
 
 uint8_t shovel_is_up(void)
diff --git a/projects/microb2010/cobboard/shovel.h b/projects/microb2010/cobboard/shovel.h
index 3b01e77..0019d2d 100644
--- a/projects/microb2010/cobboard/shovel.h
+++ b/projects/microb2010/cobboard/shovel.h
@@ -32,7 +32,8 @@ void shovel_set(void *mot, int32_t cmd);
 void shovel_down(void);
 void shovel_mid(void);
 void shovel_up(void);
-void shovel_kickstand(void);
+void shovel_kickstand_up(void);
+void shovel_kickstand_down(void);
 
 uint8_t shovel_is_up(void);
 uint8_t shovel_is_down(void);
diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c
index d9e6533..2d65208 100644
--- a/projects/microb2010/cobboard/state.c
+++ b/projects/microb2010/cobboard/state.c
@@ -66,7 +66,8 @@ static uint8_t cob_count;
 #define INIT(mode)       ((mode) == I2C_COBBOARD_MODE_INIT)
 #define HARVEST(mode)    ((mode) == I2C_COBBOARD_MODE_HARVEST)
 #define EJECT(mode)      ((mode) == I2C_COBBOARD_MODE_EJECT)
-#define KICKSTAND(mode)  ((mode) == I2C_COBBOARD_MODE_KICKSTAND)
+#define KICKSTAND_UP(mode)    ((mode) == I2C_COBBOARD_MODE_KICKSTAND_UP)
+#define KICKSTAND_DOWN(mode)  ((mode) == I2C_COBBOARD_MODE_KICKSTAND_DOWN)
 
 uint8_t state_debug = 0;
 
@@ -347,7 +348,6 @@ void state_machine(void)
 			shovel_down();
 			servo_carry_close();
 			servo_door_close();
-			shovel_current_limit_enable(0);
 
 			/* pack/deply spickles, enable/disable roller */
 			cobroller_off(I2C_LEFT_SIDE);
@@ -367,11 +367,18 @@ void state_machine(void)
 		}
 
 		/* help to climb the hill */
-		if (KICKSTAND(state_mode)) {
+		if (KICKSTAND_UP(state_mode)) {
+			state_status = I2C_COBBOARD_STATUS_KICKSTAND_UP;
 			servo_carry_open();
 			servo_door_open();
-			shovel_current_limit_enable(1);
-			shovel_kickstand();
+			shovel_kickstand_up();
+		}
+
+		if (KICKSTAND_DOWN(state_mode)) {
+			state_status = I2C_COBBOARD_STATUS_KICKSTAND_DOWN;
+			servo_carry_open();
+			servo_door_open();
+			shovel_kickstand_down();
 		}
 
 		/* eject */
diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
index 5e4a449..312a18e 100644
--- a/projects/microb2010/common/i2c_commands.h
+++ b/projects/microb2010/common/i2c_commands.h
@@ -78,7 +78,8 @@ struct i2c_cmd_cobboard_set_mode {
 #define I2C_COBBOARD_MODE_HARVEST      0x01 /* harvest mode */
 #define I2C_COBBOARD_MODE_EJECT        0x02 /* eject cobs */
 #define I2C_COBBOARD_MODE_INIT         0x03 /* init state machine */
-#define I2C_COBBOARD_MODE_KICKSTAND    0x04 /* help to climb the hill */
+#define I2C_COBBOARD_MODE_KICKSTAND_UP   0x04 /* help to climb the hill */
+#define I2C_COBBOARD_MODE_KICKSTAND_DOWN 0x05 /* help to climb the hill */
 	uint8_t mode;
 };
 
@@ -91,10 +92,8 @@ struct i2c_cmd_ballboard_set_mode {
 #define I2C_BALLBOARD_MODE_OFF         0x01
 #define I2C_BALLBOARD_MODE_HARVEST     0x02
 #define I2C_BALLBOARD_MODE_EJECT       0x03
-#define I2C_BALLBOARD_MODE_PREP_L_FORK 0x04
-#define I2C_BALLBOARD_MODE_TAKE_L_FORK 0x05
-#define I2C_BALLBOARD_MODE_PREP_R_FORK 0x06
-#define I2C_BALLBOARD_MODE_TAKE_R_FORK 0x07
+#define I2C_BALLBOARD_MODE_PREP_FORK   0x04
+#define I2C_BALLBOARD_MODE_TAKE_FORK   0x05
 	uint8_t mode;
 };
 
@@ -124,12 +123,13 @@ struct i2c_ans_cobboard_status {
 	/* mode type are defined above: I2C_COBBOARD_MODE_xxx */
 	uint8_t mode;
 
-#define I2C_COBBOARD_STATUS_READY         0x00
-#define I2C_COBBOARD_STATUS_OFF           0x01
-#define I2C_COBBOARD_STATUS_LBUSY         0x02
-#define I2C_COBBOARD_STATUS_RBUSY         0x03
-#define I2C_COBBOARD_STATUS_EJECT         0x04
-#define I2C_COBBOARD_STATUS_KICKSTAND     0x05
+#define I2C_COBBOARD_STATUS_READY          0x00
+#define I2C_COBBOARD_STATUS_OFF            0x01
+#define I2C_COBBOARD_STATUS_LBUSY          0x02
+#define I2C_COBBOARD_STATUS_RBUSY          0x03
+#define I2C_COBBOARD_STATUS_EJECT          0x04
+#define I2C_COBBOARD_STATUS_KICKSTAND_UP   0x05
+#define I2C_COBBOARD_STATUS_KICKSTAND_DOWN 0x06
 	uint8_t status;
 
 	uint8_t cob_count;
diff --git a/projects/microb2010/mainboard/beacon.c b/projects/microb2010/mainboard/beacon.c
index c661613..ea26951 100644
--- a/projects/microb2010/mainboard/beacon.c
+++ b/projects/microb2010/mainboard/beacon.c
@@ -162,8 +162,11 @@ static void beacon_event(void *dummy)
 	IRQ_LOCK(flags);
 	if (opp_age < 3)
 		opp_age ++;
-	else
+	else {
 		beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
+		IRQ_UNLOCK(flags);
+		return;
+	}
 
 	ia = opp_a;
 	id = opp_d;
diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index b43f05b..8a97000 100644
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -512,7 +512,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data)
 		}
 		else {
 #ifdef HOST_VERSION
-#define PWM_INTERACT 300
+#define PWM_INTERACT 1200
 #else
 #define PWM_INTERACT 1200
 #endif
@@ -738,13 +738,15 @@ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
 		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT);
 	else if (!strcmp_P(res->arg1, PSTR("eject")))
 		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
-	else if (!strcmp_P(res->arg1, PSTR("kickstand")))
-		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND);
+	else if (!strcmp_P(res->arg1, PSTR("kickstand_up")))
+		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
+	else if (!strcmp_P(res->arg1, PSTR("kickstand_down")))
+		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
 }
 
 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
 parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
-prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand";
+prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand_up#kickstand_down";
 parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
 
 prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
@@ -925,13 +927,17 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
 		i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
 	else if (!strcmp_P(res->arg1, PSTR("harvest")))
 		i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
+	else if (!strcmp_P(res->arg1, PSTR("prepare")))
+		i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
+	else if (!strcmp_P(res->arg1, PSTR("take")))
+		i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
 
 	/* other commands */
 }
 
 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
 parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
-prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
+prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off#take#prepare";
 parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
 
 prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
@@ -959,27 +965,11 @@ struct cmd_ballboard_setmode2_result {
 /* function called when cmd_ballboard_setmode2 is parsed successfully */
 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
 {
-	struct cmd_ballboard_setmode2_result *res = parsed_result;
-	uint8_t mode = I2C_BALLBOARD_MODE_INIT;
-
-	if (!strcmp_P(res->arg2, PSTR("left"))) {
-		if (!strcmp_P(res->arg1, PSTR("prepare")))
-			mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
-		else if (!strcmp_P(res->arg1, PSTR("take")))
-			mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
-	}
-	else {
-		if (!strcmp_P(res->arg1, PSTR("prepare")))
-			mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
-		else if (!strcmp_P(res->arg1, PSTR("take")))
-			mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
-	}
-	i2c_ballboard_set_mode(mode);
 }
 
 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
 parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
-prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
+prog_char str_ballboard_setmode2_arg1[] = "xxx";
 parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
 prog_char str_ballboard_setmode2_arg2[] = "left#right";
 parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
index 0dff9ed..09a59b7 100644
--- a/projects/microb2010/mainboard/commands_traj.c
+++ b/projects/microb2010/mainboard/commands_traj.c
@@ -885,7 +885,8 @@ static void cmd_position_parsed(void * parsed_result, void * data)
 	else if (!strcmp_P(res->arg1, PSTR("set"))) {
 		position_set(&mainboard.pos, res->arg2, res->arg3, res->arg4);
 	}
-	else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
+	else if (!strcmp_P(res->arg1, PSTR("autoset_blue")) ||
+		 !strcmp_P(res->arg1, PSTR("autoset_or_blue"))) {
 		mainboard.our_color = I2C_COLOR_BLUE;
 #ifndef HOST_VERSION
 		i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
@@ -893,7 +894,8 @@ static void cmd_position_parsed(void * parsed_result, void * data)
 #endif
 		auto_position();
 	}
-	else if (!strcmp_P(res->arg1, PSTR("autoset_yellow"))) {
+	else if (!strcmp_P(res->arg1, PSTR("autoset_yellow")) ||
+		 !strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) {
 		mainboard.our_color = I2C_COLOR_YELLOW;
 #ifndef HOST_VERSION
 		i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
@@ -902,6 +904,15 @@ static void cmd_position_parsed(void * parsed_result, void * data)
 		auto_position();
 	}
 
+	if (!strcmp_P(res->arg1, PSTR("autoset_or_blue"))) {
+		strat_conf.flags |= STRAT_CONF_OUR_ORANGE;
+		prepare_hill(I2C_COLOR_BLUE, 340);
+	}
+	else if (!strcmp_P(res->arg1, PSTR("autoset_or_yellow"))) {
+		strat_conf.flags |= STRAT_CONF_OUR_ORANGE;
+		prepare_hill(I2C_COLOR_YELLOW, 340);
+	}
+
 	/* else it's just a "show" */
 	printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"),
 		 position_get_x_double(&mainboard.pos),
@@ -911,7 +922,7 @@ static void cmd_position_parsed(void * parsed_result, void * data)
 
 prog_char str_position_arg0[] = "position";
 parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0);
-prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow";
+prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_yellow#autoset_or_blue#autoset_or_yellow";
 parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1);
 
 prog_char help_position[] = "Show/reset (x,y,a) position";
@@ -1108,7 +1119,7 @@ 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[] = "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);
+parse_pgm_token_num_t cmd_strat_conf3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_strat_conf3_result, arg2, UINT8);
 
 prog_char help_strat_conf3[] = "configure strat options";
 parse_pgm_inst_t cmd_strat_conf3 = {
@@ -1151,8 +1162,10 @@ 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("orange_yellow")))
+		run_to_the_hills(I2C_COLOR_YELLOW);
+	else if (!strcmp_P(res->arg1, PSTR("orange_blue")))
+		run_to_the_hills(I2C_COLOR_BLUE);
 	else if (!strcmp_P(res->arg1, PSTR("tomato"))) {
 		position_set(&mainboard.pos, 2625,
 			     COLOR_Y(1847), COLOR_A(0.00));
@@ -1164,7 +1177,7 @@ static void cmd_subtraj_parsed(void *parsed_result, void *data)
 
 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[] = "test#orange#tomato";
+prog_char str_subtraj_arg1[] = "test#orange_yellow#orange_blue#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 476a5ba..787c1c2 100644
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -380,7 +380,7 @@ while True:
             if not m:
                 m = re.match("cobboard=%s,%s"%(INT,INT), l)
                 if m:
-                    #print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1]))
+                    print "cobboard: %x,%x"%(int(m.groups()[0]),int(m.groups()[1]))
                     side = int(m.groups()[0])
                     flags = int(m.groups()[1])
                     if side == 0:
diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 19c6883..3e29f3a 100644
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -58,6 +58,7 @@
 #include "../common/i2c_commands.h"
 #include "i2c_protocol.h"
 #include "main.h"
+#include "cs.h"
 #include "strat.h"
 #include "strat_db.h"
 #include "strat_base.h"
@@ -179,8 +180,9 @@ static void check_tomato(void)
 		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()) {
+			if (strat_db.tomato_table[k]->present == 1 &&
+			    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
@@ -237,8 +239,9 @@ static void check_corn(void)
 		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()) {
+			if (strat_db.corn_table[i]->present == 1 &&
+			    strat_db.corn_table[i]->time_removed != -1 &&
+			    strat_db.corn_table[i]->time_removed + 2 <= time_get_s()) {
 #ifdef HOST_VERSION
 				printf("remove cob %d\n", i);
 #endif
@@ -419,7 +422,7 @@ static uint8_t strat_eject(void)
 	}
 
 	/* go to eject point */
-	trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
+	trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
 	err = WAIT_COND_OR_TRAJ_END(speedify_eject(),
 				    TRAJ_FLAGS_NO_NEAR);
 	/* err is never == 0 because speedify_eject() always return 0 */
@@ -546,6 +549,57 @@ static uint8_t need_more_elements(void)
 }
 #endif
 
+/* get tomatoes near our goals (12,5 and 12,3) */
+uint8_t get_opp_oranges(void)
+{
+	int16_t x, y;
+	uint8_t i, j;
+	uint8_t err;
+
+	DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+	/* only if oranges are present */
+	if (strat_db.opp_oranges_count == 0)
+		return END_TRAJ;
+
+	strat_db.opp_oranges_count = 0;
+	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_DIST_FAST, SPEED_ANGLE_FAST);
+
+	/* turn in the correct direction */
+	trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	if (!TRAJ_SUCCESS(err))
+		goto fail;
+
+	trajectory_goto_forward_xy_abs(&mainboard.traj, 2625, COLOR_Y(597));
+	err = wait_traj_end(TRAJ_FLAGS_STD);
+	if (!TRAJ_SUCCESS(err))
+		goto fail;
+
+	strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+	trajectory_goto_forward_xy_abs(&mainboard.traj, 2750, COLOR_Y(250));
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	if (!TRAJ_SUCCESS(err))
+		goto fail;
+
+	err = run_to_the_hills(get_opponent_color());
+
+ fail:
+	strat_want_pack = 0;
+	return err;
+}
+
 /* get tomatoes near our goals (12,5 and 12,3) */
 uint8_t get_orphan_tomatoes(void)
 {
@@ -608,13 +662,6 @@ uint8_t get_orphan_tomatoes(void)
 	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;
@@ -650,97 +697,169 @@ fail:
 	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;
+#define HILL_LEN 1000
 
-	strat_want_pack = 1;
-	strat_get_acc(&ad, &aa);
-	strat_get_speed(&sd, &sa);
+#define HILL_ANGLE 0
+#define HILL_POSY_YELLOW 310
+#define HILL_POSY_BLUE 190
 
-	DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+#define HILL_POSX_BALLS_DOWN1 830
+#define HILL_POSX_BALLS_DOWN2 920
+#define HILL_POSX_BALLS_DOWN3 730
+#define HILL_START_POSX 580
+
+uint8_t prepare_hill(uint8_t orange_color, int16_t posx)
+{
+	int16_t startx, starty;
+	uint8_t our_color = get_color();
+	uint8_t err;
 
-	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;
+		startx = posx;
 	else
-		startx = AREA_X - HILL_START_POSX;
-	trajectory_goto_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
+		startx = AREA_X - posx;
+	trajectory_goto_forward_xy_abs(&mainboard.traj, startx, COLOR_Y(starty));
 	err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+	if (!TRAJ_SUCCESS(err))
+		return err;
 
 	/* turn to the hills */
 	if (orange_color == our_color)
-		trajectory_a_abs(&mainboard.traj, COLOR_A(0));
+		trajectory_a_abs(&mainboard.traj, COLOR_A(HILL_ANGLE));
 	else
-		trajectory_a_abs(&mainboard.traj, COLOR_A(180));
+		trajectory_a_abs(&mainboard.traj, COLOR_A(-180+HILL_ANGLE));
 	err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+	if (!TRAJ_SUCCESS(err))
+		return err;
 
-	strat_set_acc(3, 3);
-	strat_set_speed(300, 500);
-	bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 40);
+	return END_TRAJ;
+}
+
+/* get oranges, must be called near game area */
+uint8_t run_to_the_hills(uint8_t orange_color)
+{
+	double aa, ad;
+	uint16_t sa, sd;
+	uint8_t err;
+	uint8_t our_color = get_color();
+	int32_t p = pid_get_gain_P(&mainboard.angle.pid);
+	int32_t i = pid_get_gain_I(&mainboard.angle.pid);
+	int32_t d = pid_get_gain_D(&mainboard.angle.pid);
+	int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
+	int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
+	int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
+
+	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);
+	err = prepare_hill(orange_color, HILL_START_POSX);
+	if (!TRAJ_SUCCESS(err))
+		return err;
+
+	strat_set_acc(5, ACC_ANGLE);
+	strat_set_speed(300, SPEED_ANGLE_SLOW);
+	bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 2000000, 80);
+	bd_set_current_thresholds(&mainboard.angle.bd, 500, 8000, 2000000, 80);
+	bd_set_speed_threshold(&mainboard.distance.bd, 10);
 	support_balls_pack();
+	i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
+
+	/* decrease angle gains */
+	pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
 
 	/* here it is difficult to handle return values, because we
 	 * are on the hill */
+	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_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("deploy\r\n"));
+	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) >
+				    HILL_POSX_BALLS_DOWN1,
+				    TRAJ_FLAGS_SMALL_DIST);
+	DEBUG(E_USER_STRAT, "deploy support balls");
 	support_balls_deploy();
-	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND);
-	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
+	trajectory_only_a_rel(&mainboard.traj, 2);
+	err = WAIT_COND_OR_TE_TO(0, 0, 2200);
 
-	time_wait_ms(2000);
+	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
+	strat_set_acc(3, 3);
+	strat_hardstop();
+	i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
+
+	time_wait_ms(1800);
 
 	/* 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"));
+	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
+				    HILL_POSX_BALLS_DOWN2,
+				    TRAJ_FLAGS_SMALL_DIST);
+	DEBUG(E_USER_STRAT, "pack support balls");
 	support_balls_pack();
-	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-	printf_P(PSTR("deploy\r\n"));
+	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) <
+				    HILL_POSX_BALLS_DOWN3,
+				    TRAJ_FLAGS_SMALL_DIST);
+	DEBUG(E_USER_STRAT, "deploy support balls");
+	strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+	strat_set_acc(ad, aa);
 	support_balls_deploy();
+	err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+
+	/* wait to be near the wall */
+	err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos) < 200,
+				    TRAJ_FLAGS_SMALL_DIST);
+
+	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+
+	/* restore BD coefs */
+	bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
+	bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 20);
+	bd_set_speed_threshold(&mainboard.distance.bd, 60);
 
 	/* 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);
+	err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST);
 	if (orange_color == our_color)
-		position_set(&mainboard.pos, ROBOT_HALF_LENGTH_REAR,
+		strat_reset_pos(ROBOT_HALF_LENGTH_REAR,
 			     DO_NOT_SET_POS, COLOR_A(0));
 	else
-		position_set(&mainboard.pos, AREA_X - ROBOT_HALF_LENGTH_REAR,
+		strat_reset_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);
+
+	trajectory_d_rel(&mainboard.traj, 250);
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	if (orange_color == I2C_COLOR_YELLOW)
+		trajectory_a_rel(&mainboard.traj, 90);
+	else
+		trajectory_a_rel(&mainboard.traj, -90);
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	time_wait_ms(200);
+
+	strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
+	err = strat_calib(-400, TRAJ_FLAGS_SMALL_DIST);
+	strat_reset_pos(DO_NOT_SET_POS,
+			COLOR_Y(ROBOT_HALF_LENGTH_REAR),
+			COLOR_A(90));
+	strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
+
+	trajectory_d_rel(&mainboard.traj, 250);
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 
 	/* revert acceleration and speed */
-	i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+	pid_set_gains(&mainboard.angle.pid, p, i, d);
+	pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
 	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;
 }
@@ -752,6 +871,7 @@ uint8_t strat_main(void)
 	/* get oranges */
 	if (strat_conf.flags & STRAT_CONF_OUR_ORANGE) {
 		err = run_to_the_hills(get_color());
+		strat_db.our_oranges_count = 0;
 		do_initturn = 0;
 	}
 
@@ -787,6 +907,25 @@ uint8_t strat_main(void)
 			}
 		}
 
+		/* if it's time to get opponent oranges, do it */
+		if (time_get_s() > strat_conf.opp_orange) {
+			err = get_opp_oranges();
+			if (err == END_ERROR) {
+				DEBUG(E_USER_STRAT,
+				      "get_opp_oranges 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 oranges if it failed */
+				strat_conf.opp_orange = 90;
+				strat_unblock();
+			}
+		}
+
 		/**********************/
 		/* harvest on circuit */
 		/**********************/
diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index 3305f1f..2fa2e5b 100644
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -135,8 +135,8 @@
 #define ACC_ANGLE 16.
 
 /* default speeds */
-#define SPEED_DIST_FAST 1500.
-#define SPEED_ANGLE_FAST 1000.
+#define SPEED_DIST_FAST 1200.
+#define SPEED_ANGLE_FAST 800.
 #define SPEED_DIST_SLOW 500.
 #define SPEED_ANGLE_SLOW 500.
 #define SPEED_DIST_VERY_SLOW 200.
@@ -174,6 +174,7 @@ uint8_t strat_main(void);
 void strat_event(void *dummy);
 void strat_event_enable(void);
 void strat_event_disable(void);
+uint8_t prepare_hill(uint8_t orange_color, int16_t posx);
 uint8_t run_to_the_hills(uint8_t orange_color);
 uint8_t get_orphan_tomatoes(void);
 
diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c
index 2658b46..de85e30 100644
--- a/projects/microb2010/mainboard/strat_avoid.c
+++ b/projects/microb2010/mainboard/strat_avoid.c
@@ -1001,8 +1001,6 @@ uint8_t strat_harvest_circuit(void)
 	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);
 
@@ -1040,7 +1038,6 @@ uint8_t strat_harvest_circuit(void)
 		goto fail;
 
 	strat_want_pack = 0;
-	printf("UNPACK UNPACK\n");
 
 	/* do all lines of circuit */
 	for (idx = 1; idx < len; idx ++) {
@@ -1080,11 +1077,11 @@ const struct xy_point unblock_pts[] = {
 /* try to unblock in any situation */
 uint8_t strat_unblock(void)
 {
-	int16_t x, y;
+	int16_t x, y, posx, posy;
 	uint8_t i, j, k;
 	uint16_t old_dspeed, old_aspeed;
 	uint8_t err;
-	uint16_t d_min = 0xFFFF, d;
+	uint16_t d_min = 0x7FFF, d;
 	const struct xy_point *pt;
 
 	DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
@@ -1094,8 +1091,10 @@ uint8_t strat_unblock(void)
 
 	strat_hardstop();
 	strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
-	x = position_get_x_s16(&mainboard.pos);
-	y = position_get_y_s16(&mainboard.pos);
+	posx = position_get_x_s16(&mainboard.pos);
+	posy = position_get_y_s16(&mainboard.pos);
+	x = posx;
+	y = posy;
 
 	if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
 		x = -1;
@@ -1104,14 +1103,11 @@ uint8_t strat_unblock(void)
 
 	/* 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));
+			d = distance_between(posx, posy, pt->x, COLOR_Y(pt->y));
 			if (d < d_min) {
 				d_min = d;
 				x = pt->x;
@@ -1119,6 +1115,8 @@ uint8_t strat_unblock(void)
 			}
 		}
 	}
+	DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d",
+	      __FUNCTION__, x, y);
 
 	/* XXX if opponent is too close, go back, or wait ? */
 
@@ -1132,7 +1130,6 @@ uint8_t strat_unblock(void)
 		return err;
 
 	strat_set_speed(old_dspeed, old_aspeed);
-	strat_want_pack = 0;
 	return END_TRAJ;
 }
 
diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index acded68..8716486 100644
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -156,6 +156,25 @@ void strat_reset_pos(int16_t x, int16_t y, int16_t a)
 	if (a == DO_NOT_SET_POS)
 		a = posa;
 
+	/* some issues with blocking on simulator */
+#ifdef HOST_VERSION
+	if (x == ROBOT_HALF_LENGTH_REAR)
+		x = ROBOT_HALF_LENGTH_REAR + 10;
+	if (x == AREA_X - ROBOT_HALF_LENGTH_REAR)
+		x = AREA_X - ROBOT_HALF_LENGTH_REAR - 10;
+	if (y == ROBOT_HALF_LENGTH_REAR)
+		y = ROBOT_HALF_LENGTH_REAR + 10;
+	if (y == AREA_Y - ROBOT_HALF_LENGTH_REAR)
+		y = AREA_Y - ROBOT_HALF_LENGTH_REAR - 10;
+	if (x == ROBOT_HALF_LENGTH_FRONT)
+		x = ROBOT_HALF_LENGTH_FRONT + 10;
+	if (x == AREA_X - ROBOT_HALF_LENGTH_FRONT)
+		x = AREA_X - ROBOT_HALF_LENGTH_FRONT - 10;
+	if (y == ROBOT_HALF_LENGTH_FRONT)
+		y = ROBOT_HALF_LENGTH_FRONT + 10;
+	if (y == AREA_Y - ROBOT_HALF_LENGTH_FRONT)
+		y = AREA_Y - ROBOT_HALF_LENGTH_FRONT - 10;
+#endif
 	DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
 	      x == DO_NOT_SET_POS ? "" : "x",
 	      y == DO_NOT_SET_POS ? "" : "y",
@@ -176,14 +195,45 @@ uint8_t strat_calib(int16_t dist, uint8_t flags)
 	int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
 	int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
 	int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
+	uint32_t i_thres = mainboard.distance.bd.i_thres;
+	int32_t k1 = mainboard.distance.bd.k1;
+	int32_t k2 = mainboard.distance.bd.k2;
+	uint16_t cpt_thres = mainboard.distance.bd.cpt_thres;
+
 	uint8_t err;
 
+	/* go with a lower angle pid, and with a sensible blocking
+	 * detection */
+	bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 400000, 20);
 	pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000);
-	pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
+	pid_set_gains(&mainboard.angle.pid, 500, 10, 4000);
 	trajectory_d_rel(&mainboard.traj, dist);
 	err = wait_traj_end(flags);
+	if (err != END_BLOCKING)
+		goto fail;
+
+	/* we detected a blocking, reset bd, remove angle pid and
+	 * continue */
+	trajectory_d_rel(&mainboard.traj, dist);
+	pid_set_maximums(&mainboard.distance.pid, max_in, max_i, 4095);
+	pid_set_gains(&mainboard.angle.pid, 1, 0, 0);
+	time_wait_ms(300);
+	strat_hardstop();
+
+#ifdef HOST_VERSION
+	/* issue with block on simulator */
+	if (dist > 0)
+		trajectory_d_rel(&mainboard.traj, -10);
+	else
+		trajectory_d_rel(&mainboard.traj, 10);
+	wait_traj_end(END_TRAJ);
+#endif
+
+ fail:
 	pid_set_gains(&mainboard.angle.pid, p, i, d);
 	pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
+	bd_set_current_thresholds(&mainboard.distance.bd, k1, k2, i_thres, cpt_thres);
+
 	return err;
 }
 
diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h
index f314b0e..2414c04 100644
--- a/projects/microb2010/mainboard/strat_utils.h
+++ b/projects/microb2010/mainboard/strat_utils.h
@@ -42,6 +42,27 @@ struct xy_point {
 		__err;							\
 	})								\
 
+#define WAIT_COND_OR_TE_TO(cond, mask, timeout)				\
+	({								\
+		microseconds __us = time_get_us2();			\
+		uint8_t __ret = 0;					\
+		while ( (! (cond)) && (__ret == 0)) {			\
+			__ret = test_traj_end(mask);			\
+			if (time_get_us2() - __us > (timeout)*1000L) {	\
+				__ret = 0;				\
+				break;					\
+			}						\
+		}							\
+		if (!__ret)						\
+			DEBUG(E_USER_STRAT, "cond / timeout at line %d", \
+			      __LINE__);				\
+		else							\
+			DEBUG(E_USER_STRAT, "got %s (%d) at line %d",	\
+			      get_err(__ret), __ret, __LINE__);		\
+									\
+		__ret;							\
+	})
+
 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);