oranges and enhance strats
authorzer0 <zer0@carbon.local>
Mon, 10 May 2010 16:09:37 +0000 (18:09 +0200)
committerzer0 <zer0@carbon.local>
Mon, 10 May 2010 16:09:37 +0000 (18:09 +0200)
20 files changed:
projects/microb2010/ballboard/actuator.c
projects/microb2010/ballboard/actuator.h
projects/microb2010/ballboard/commands_ballboard.c
projects/microb2010/ballboard/cs.c
projects/microb2010/ballboard/main.c
projects/microb2010/ballboard/state.c
projects/microb2010/cobboard/commands_cobboard.c
projects/microb2010/cobboard/shovel.c
projects/microb2010/cobboard/shovel.h
projects/microb2010/cobboard/state.c
projects/microb2010/common/i2c_commands.h
projects/microb2010/mainboard/beacon.c
projects/microb2010/mainboard/commands_mainboard.c
projects/microb2010/mainboard/commands_traj.c
projects/microb2010/mainboard/display.py
projects/microb2010/mainboard/strat.c
projects/microb2010/mainboard/strat.h
projects/microb2010/mainboard/strat_avoid.c
projects/microb2010/mainboard/strat_base.c
projects/microb2010/mainboard/strat_utils.h

index d745fa0..0dc89ff 100644 (file)
 #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;
 }
index 039cb1d..bd93ddc 100644 (file)
@@ -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);
index ba570a4..70ae084 100644 (file)
@@ -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";
index ed5c0ff..6bfd518 100644 (file)
@@ -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,
index 925109f..e737ba3 100755 (executable)
@@ -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"));
 
index a322d17..e777e49 100644 (file)
@@ -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;
                }
index 3f37356..9f8b562 100644 (file)
@@ -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";
index 42f734c..1f38e1a 100644 (file)
@@ -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)
index 3b01e77..0019d2d 100644 (file)
@@ -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);
index d9e6533..2d65208 100644 (file)
@@ -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 */
index 5e4a449..312a18e 100644 (file)
@@ -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;
index c661613..ea26951 100644 (file)
@@ -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;
index b43f05b..8a97000 100644 (file)
@@ -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);
index 0dff9ed..09a59b7 100644 (file)
@@ -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);
index 476a5ba..787c1c2 100644 (file)
@@ -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:
index 19c6883..3e29f3a 100644 (file)
@@ -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 */
@@ -547,6 +550,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)
 {
 #define CLITOID_TOMATO_RADIUS 100.
@@ -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 */
                /**********************/
index 3305f1f..2fa2e5b 100644 (file)
 #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);
 
index 2658b46..de85e30 100644 (file)
@@ -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;
 }
 
index acded68..8716486 100644 (file)
@@ -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;
 }
 
index f314b0e..2414c04 100644 (file)
@@ -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);