X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_corn.c;h=8abef5bec7fed22cd8c3c191c53d9b399a52714b;hp=1f5e1d347c4868af3093b634cb242ef842498480;hb=HEAD;hpb=876789a372da2e93a816428649e511c2ad9e40be

diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index 1f5e1d3..8abef5b 100644
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -70,7 +70,8 @@
 static volatile uint8_t clitoid_slow = 0;
 
 /* return 1 if there is a corn near, and fill the index ptr */
-int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
+int8_t corn_is_near(uint8_t *corn_idx, uint8_t side,
+		    int16_t *xspickle, int16_t *yspickle)
 {
 	/* XXX to be checked */
 #define SENSOR_CORN_DIST  225
@@ -92,6 +93,8 @@ int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
 	}
 	x_corn_int = x_corn;
 	y_corn_int = y_corn;
+	*xspickle = x_corn_int;
+	*yspickle = y_corn_int;
 
 	wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
 	if (wp == NULL)
@@ -162,19 +165,25 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
 	if (get_cob_count() >= 5)
 		return 0; /* fast */
 
-	if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
+	if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) {
 		DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
 		      __FUNCTION__, x, y);
 		return 1;
 	}
 
-/* 	if (time_get_s() > 39) */
-/* 		DEBUG(E_USER_STRAT, "i,j = (%d %d), count=%d", i, j, corn_count_neigh(i, j)); */
-
 	if (corn_count_neigh(i, j) == 2)
 		return 1;
 
+
+	/* we are on intersection, keep the same speed... but as we
+	 * enter in the curve-part of the clitoid, we should not go
+	 * there */
 	if (wp_belongs_to_line(i, j, num2, dir2))
+		return clitoid_slow;
+
+	/* we can ge fast if it's a 60deg angle and if we checked the
+	 * current point */
+	if (is_60deg(dir1, dir2))
 		return 0;
 
 	/* get next point */
@@ -184,7 +193,7 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
 		return 1;
 	}
 
-	/* if (i2, j2) belongs to next line, check corn in opposition */
+	/* if (i2, j2) belongs to next line, check corns */
 	if (wp_belongs_to_line(i2, j2, num2, dir2)) {
 		if (corn_count_neigh(i2, j2) > 0)
 			return 1;
@@ -199,7 +208,7 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
 		return 1;
 	}
 
-	/* if (i3, j3) belongs to next line, check corn in opposition */
+	/* if (i3, j3) belongs to next line, check corns */
 	if (wp_belongs_to_line(i3, j3, num2, dir2)) {
 		if (corn_count_neigh(i2, j2) > 0 ||
 		    corn_count_neigh(i3, j3) > 0)
@@ -294,7 +303,7 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
 		}
 		else {
 			beta_deg = 0;
-			*pack_spickles = I2C_RIGHT_SIDE;
+			*pack_spickles = I2C_LEFT_SIDE;
 		}
 	}
 	/* double 90 deg for half turn -- not used */
@@ -316,15 +325,14 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
 
 	clitoid_slow = clitoid_select_speed(num1, dir1, num2, dir2);
 	if (clitoid_slow) {
-		DEBUG(E_USER_STRAT, "slow clito\n");
+		DEBUG(E_USER_STRAT, "slow clito");
 		strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 	}
 	else {
-		DEBUG(E_USER_STRAT, "fast clito\n");
+		DEBUG(E_USER_STRAT, "fast clito");
 		strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
 	}
 
-	/* XXX check return value !! */
 	ret = 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,
@@ -342,8 +350,10 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
 
  reprocess:
 	ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles);
-	if (ret < 0)
+	if (ret < 0) {
 		DEBUG(E_USER_STRAT, "clitoid failed");
+		return END_ERROR;
+	}
 
 	/* XXX what to do if cobboard is stucked */
 
@@ -352,8 +362,13 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
 						     num2, dir2),
 				    flags);
 
+	/* error during traj, or traj finished */
+	if (err != 0)
+		return err;
+
 	/* the speed has to change */
-	if (trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
+	if (err == 0 &&
+	    trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
 		goto reprocess;
 
 	DEBUG(E_USER_STRAT, "clitoid started err=%d pack_spickles=%d",
@@ -371,13 +386,7 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
 		err = wait_traj_end(flags);
 	}
 
-	DEBUG(E_USER_STRAT, "clitoid finished");
-
-	/* 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); */
+	DEBUG(E_USER_STRAT, "clitoid finished, err=%d", err);
 
 	strat_rpack60 = 0;
 	strat_lpack60 = 0;