X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_base.c;h=0b14384f840dc78a1acbd83b88548274333d1661;hp=871648666119706f8bd6aff0dd3aad538279b473;hb=5573d7668c4fe0cde51b566d086582cf3d9c7e4b;hpb=28da5858ac871c626153f47566e968ecb05ff52b

diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index 8716486..0b14384 100644
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -143,18 +143,18 @@ uint8_t strat_goto_xy_force(int16_t x, int16_t y)
 }
 
 /* reset position */
-void strat_reset_pos(int16_t x, int16_t y, int16_t a)
+void strat_reset_pos(int16_t x, int16_t y, double a)
 {
-	int16_t posx = position_get_x_s16(&mainboard.pos);
-	int16_t posy = position_get_y_s16(&mainboard.pos);
-	int16_t posa = position_get_a_deg_s16(&mainboard.pos);
+	double posx = position_get_x_double(&mainboard.pos);
+	double posy = position_get_y_double(&mainboard.pos);
+	double posa = position_get_a_rad_double(&mainboard.pos);
 
 	if (x == DO_NOT_SET_POS)
 		x = posx;
 	if (y == DO_NOT_SET_POS)
 		y = posy;
 	if (a == DO_NOT_SET_POS)
-		a = posa;
+		a = DEG(posa);
 
 	/* some issues with blocking on simulator */
 #ifdef HOST_VERSION
@@ -217,7 +217,7 @@ uint8_t strat_calib(int16_t dist, uint8_t flags)
 	trajectory_d_rel(&mainboard.traj, dist);
 	pid_set_maximums(&mainboard.distance.pid, max_in, max_i, 4095);
 	pid_set_gains(&mainboard.angle.pid, 1, 0, 0);
-	time_wait_ms(300);
+	time_wait_ms(500);
 	strat_hardstop();
 
 #ifdef HOST_VERSION
@@ -382,6 +382,8 @@ void strat_start(void)
 	strat_exit();
 }
 
+uint8_t xxxdebug = 0;
+
 /* return true if we have to brake due to an obstacle */
 uint8_t strat_obstacle(void)
 {
@@ -389,12 +391,25 @@ uint8_t strat_obstacle(void)
 	int16_t opp_x, opp_y, opp_d, opp_a;
 
 	/* too slow */
-	if (ABS(mainboard.speed_d) < 150)
+	if (ABS(mainboard.speed_d) < 0) {
+		if (xxxdebug != 1) {
+			DEBUG(E_USER_STRAT, "XXX too slow");
+			xxxdebug = 1;
+		}
 		return 0;
+	}
 
 	/* no opponent detected */
-	if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a))
+	if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a)) {
+		if (xxxdebug != 2) {
+			DEBUG(E_USER_STRAT, "XXX no opp");
+			DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+			      "x=%d y=%d (speed_d=%d)",
+			      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
+			xxxdebug = 2;
+		}
 		return 0;
+	}
 
 	/* save obstacle position */
 	opponent_obstacle.x = opp_x;
@@ -402,23 +417,48 @@ uint8_t strat_obstacle(void)
 	opponent_obstacle.d = opp_d;
 	opponent_obstacle.a = opp_a;
 
-	if (!is_in_area(opp_x, opp_y, 250))
+	if (!is_in_area(opp_x, opp_y, 250)) {
+		if (xxxdebug != 3) {
+			DEBUG(E_USER_STRAT, "XXX not in area");
+			DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+			      "x=%d y=%d (speed_d=%d)",
+			      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
+			xxxdebug = 3;
+		}
 		return 0;
+	}
 
 	/* sensor are temporarily disabled */
-	if (sensor_obstacle_is_disabled())
+	if (sensor_obstacle_is_disabled()) {
+		if (xxxdebug != 4) {
+			DEBUG(E_USER_STRAT, "XXX disabled");
+			DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+			      "x=%d y=%d (speed_d=%d)",
+			      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
+			xxxdebug = 4;
+		}
 		return 0;
+	}
 
 	/* relative position */
 	x_rel = cos(RAD(opp_a)) * (double)opp_d;
 	y_rel = sin(RAD(opp_a)) * (double)opp_d;
 
 	/* opponent too far */
-	if (opp_d > 500)
+	if (opp_d > 650) {
+		if (xxxdebug != 5) {
+			DEBUG(E_USER_STRAT, "XXX too far");
+			DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+			      "x=%d y=%d (speed_d=%d)",
+			      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
+			xxxdebug = 5;
+		}
 		return 0;
+	}
 
 	/* opponent is in front of us */
-	if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) {
+	if ((mainboard.speed_d >= 0 && opp_d < 500 && (opp_a > 315 || opp_a < 45)) ||
+	    (mainboard.speed_d >= 0 && opp_d < 650 && (opp_a > 330 || opp_a < 30))) {
 		DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d "
 		      "xrel=%d yrel=%d (speed_d=%d)",
 		      opp_d, opp_a, x_rel, y_rel, mainboard.speed_d);
@@ -426,13 +466,22 @@ uint8_t strat_obstacle(void)
 		return 1;
 	}
 	/* opponent is behind us */
-	if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) {
+	if ((mainboard.speed_d < 0 && opp_d < 500 && (opp_a < 225 && opp_a > 135)) ||
+	    (mainboard.speed_d < 0 && opp_d < 650 && (opp_a < 210 && opp_a > 150))) {
 		DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d",
 		      opp_d, opp_a, x_rel, y_rel);
 		sensor_obstacle_disable();
 		return 1;
 	}
 
+	if (xxxdebug != 6) {
+		DEBUG(E_USER_STRAT, "XXX not in cone");
+		DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+		      "x=%d y=%d (speed_d=%d)",
+		      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
+		xxxdebug = 6;
+	}
+
 	return 0;
 }