X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_corn.c;fp=projects%2Fmicrob2010%2Fmainboard%2Fstrat_corn.c;h=31afd436463d3639cd7dc2804c0d0946eef2ad54;hp=3694bcdab01e70367ac9c2ac3a52358e7ed776d6;hb=e442b9c066de9b55eef70fdf9993fc3f6b8259e8;hpb=4f7bc6a1332aa9cd2ac3cf8f34bf990203cb03e7

diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index 3694bcd..31afd43 100644
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -161,8 +161,17 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
 	line2_a_rad = atan2(l2.p2.y - l2.p1.y,
 			    l2.p2.x - l2.p1.x);
 	diff_a_deg = DEG(line2_a_rad - line1_a_rad);
+	if (diff_a_deg < -180) {
+		diff_a_deg += 360;
+	}
+	else if (diff_a_deg > 180) {
+		diff_a_deg -= 360;
+	}
 	diff_a_deg_abs = fabs(diff_a_deg);
 
+/* 	printf_P(PSTR("diff_a_deg=%2.2f\r\n"), diff_a_deg_abs); */
+/* 	printf_P(PSTR("inter=%2.2f,%2.2f\r\n"), p.x, p.y); */
+
 	if (diff_a_deg_abs < 70.) {
 		radius = 200;
 		if (diff_a_deg > 0)
@@ -185,10 +194,12 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
 			beta_deg = -60;
 	}
 
+	/* XXX check return value !! */
 	trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
 			   line1_a_deg, 150., diff_a_deg, beta_deg,
 			   radius, xy_norm(l1.p1.x, l1.p1.y,
 					   p.x, p.y));
+	/* disabled */
 	if (0) {
 		err = 0;
 		while (err == 0) {