another beginning

diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
index 54c0ccc..0bd04b5 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;
                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;
 
        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_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);
 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 4955bba..ce30920 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;
 }
 
        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 {                                            \
 /* 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 */
        }
 
        /* 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();
 
        if (!TRAJ_SUCCESS(err))
                strat_unblock();

diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index b4592c5..e8572fc 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_OUR_ORANGE      0x01
 #define STRAT_CONF_WAIT_OBSTACLE   0x02

+#define STRAT_CONF_STRAIGHT_BEGIN  0x04

        uint8_t flags;
 };
 
        uint8_t flags;
 };