X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;fp=projects%2Fmicrob2010%2Fmainboard%2Fstrat.c;h=ce3092031eafc7e43373e8517247a5720c8b5855;hp=4955bbaea0e0e6a0efc14b7bd366b5003155cea8;hb=81e4a9146e5c5ae192e66eb3f68076cd2a7abd0d;hpb=c20f7eb8460dd04e42f95d799f17d9b60b5ee8e4

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();