From 81e4a9146e5c5ae192e66eb3f68076cd2a7abd0d Mon Sep 17 00:00:00 2001
From: zer0 <zer0@carbon.local>
Date: Tue, 11 May 2010 00:17:39 +0200
Subject: [PATCH] another beginning

---
 projects/microb2010/mainboard/commands_traj.c |  4 ++-
 projects/microb2010/mainboard/strat.c         | 27 ++++++++++++++++++-
 projects/microb2010/mainboard/strat.h         |  1 +
 3 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
index 54c0ccc..0bd04b5 100644
--- 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 4955bba..ce30920 100644
--- 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 b4592c5..e8572fc 100644
--- 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;
 };
 
-- 
2.39.5