another beginning

diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
index 54c0cccd305a631fa3e4103e05808bd08dc37f7b..0bd04b50058be823f08547183ca2aee1b45b9155 100644 (file)
--- a/projects/microb2010/mainboard/commands_traj.c
+++ b/projects/microb2010/mainboard/commands_traj.c
@@ -1060,6 +1060,8 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
                bit = STRAT_CONF_OUR_ORANGE;
        else if (!strcmp_P(res->arg1, PSTR("wait_obstacle")))
                bit = STRAT_CONF_WAIT_OBSTACLE;
+       else if (!strcmp_P(res->arg1, PSTR("straight_begin")))
+               bit = STRAT_CONF_STRAIGHT_BEGIN;
 
        if (on)
                strat_conf.flags |= bit;
@@ -1072,7 +1074,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
 
 prog_char str_strat_conf2_arg0[] = "strat_conf";
 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
-prog_char str_strat_conf2_arg1[] = "our_orange#wait_obstacle";
+prog_char str_strat_conf2_arg1[] = "our_orange#wait_obstacle#straight_begin";
 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
 prog_char str_strat_conf2_arg2[] = "on#off";
 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);

diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 4955bbaea0e0e6a0efc14b7bd366b5003155cea8..ce3092031eafc7e43373e8517247a5720c8b5855 100644 (file)
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -587,6 +587,28 @@ static uint8_t strat_beginning(uint8_t do_initturn)
        return END_TRAJ;
 }
 
+static uint8_t strat_beginning2(uint8_t do_initturn)
+{
+       uint8_t err;
+
+       strat_set_acc(ACC_DIST, ACC_ANGLE);
+
+       if (do_initturn) {
+               strat_set_speed(600, 90); /* OK */
+               trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(-40));
+               err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+                                           TRAJ_FLAGS_STD);
+               if (err == 0)
+                       return END_TRAJ;
+       }
+       else {
+               trajectory_goto_forward_xy_abs(&mainboard.traj,
+                                              375, COLOR_Y(597));
+               err = wait_traj_end(TRAJ_FLAGS_STD);
+       }
+       return err;
+}
+
 /* dump state (every 5 s max) */
 #define DUMP_RATE_LIMIT(dump, last_print)              \
        do {                                            \
@@ -949,7 +971,10 @@ uint8_t strat_main(void)
        }
 
        /* harvest the first cobs + balls */
-       err = strat_beginning(do_initturn);
+       if (strat_conf.flags & STRAT_CONF_STRAIGHT_BEGIN)
+               err = strat_beginning2(do_initturn);
+       else
+               err = strat_beginning(do_initturn);
 
        if (!TRAJ_SUCCESS(err))
                strat_unblock();

diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index b4592c536d10eb89dd26af5b075322041db7fc96..e8572fca1f0ad47136ae43a5e90dabf8246cda72 100644 (file)
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -157,6 +157,7 @@ struct strat_conf {
 
 #define STRAT_CONF_OUR_ORANGE      0x01
 #define STRAT_CONF_WAIT_OBSTACLE   0x02
+#define STRAT_CONF_STRAIGHT_BEGIN  0x04
        uint8_t flags;
 };