X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_corn.c;h=8d3f839cd5e9bb93220485eb4c42e1d419bcca68;hp=3694bcdab01e70367ac9c2ac3a52358e7ed776d6;hb=38196781643cba8e1d681aafb28cdadc51fe20ec;hpb=4e7801883ed4076cb14b63a0571467747894c0f8

diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index 3694bcd..8d3f839 100644
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -27,6 +27,7 @@
 #include <math.h>
 
 #include <aversive.h>
+#include <aversive/error.h>
 #include <aversive/pgmspace.h>
 
 #include <ax12.h>
@@ -65,9 +66,8 @@
 #include "sensor.h"
 #include "actuator.h"
 
-#if 0
 /* return 1 if there is a corn near, and fill the index ptr */
-uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
+int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
 {
 #define SENSOR_CORN_DIST  225
 #define SENSOR_CORN_ANGLE 90
@@ -90,11 +90,11 @@ uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
 	y_corn_int = y_corn;
 
 	wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
-	if (wp->corn.idx < 0)
+	if (wp == NULL)
 		return 0;
+	*corn_idx = wp->corn.idx;
 	return 1;
 }
-#endif
 
 /*
  * - send the correct commands to the spickles
@@ -140,15 +140,16 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
 	line_t ll1, ll2;
 	point_t p;
 	uint8_t err;
+	uint16_t a_speed, d_speed;
 
 	/* convert to 2 points */
 	num2line(&l1, dir1, num1);
 	num2line(&l2, dir2, num2);
 
-	printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
-		 l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
-	printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
-		 l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
+	DEBUG(E_USER_STRAT, "line1: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
+	      l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
+	DEBUG(E_USER_STRAT, "line2: (%2.2f, %2.2f) -> (%2.2f, %2.2f)",
+	      l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
 
 	/* convert to line eq and find intersection */
 	pts2line(&l1.p1, &l1.p2, &ll1);
@@ -161,8 +162,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 +195,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) {
@@ -202,7 +214,17 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
 		}
 	}
 
-	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	err = WAIT_COND_OR_TRAJ_END(get_cob_count() == 5, 0xFF);
+	strat_get_speed(&d_speed, &a_speed);
+
+	/* XXX 600 -> cste */
+	/* XXX does not work, do better */
+/* 	if (err == 0 && d_speed < 600 && */
+/* 	    mainboard.traj.state == RUNNING_CLITOID_LINE) */
+/* 		strat_set_speed(600, SPEED_ANGLE_FAST); */
+
+	err = wait_traj_end(0xFF);
+
 	return err;
 }