X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_base.c;h=90762a635d72a7066f8b0011eeaa04e4517fa3f6;hp=a6724ad2033bfdfd19b073c0a15e8f0d16a0b5c7;hb=HEAD;hpb=78150017ab8c5615af414df706a0525fe7c262ae

diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index a6724ad..90762a6 100644
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -59,10 +59,14 @@
 #include "strat.h"
 #include "sensor.h"
 #include "i2c_protocol.h"
+#include "robotsim.h"
 
 /* true if we want to interrupt a trajectory */
 static uint8_t traj_intr=0;
 
+/* to debug state of strat_obstacle() */
+static uint8_t strat_obstacle_debug = 0;
+
 /* filled when a END_OBSTACLE is returned */
 struct opponent_obstacle opponent_obstacle;
 
@@ -103,6 +107,8 @@ const char *get_err(uint8_t err)
 
 void strat_hardstop(void)
 {
+	DEBUG(E_USER_STRAT, "strat_hardstop");
+
 	trajectory_hardstop(&mainboard.traj);
 	pid_reset(&mainboard.angle.pid);
 	pid_reset(&mainboard.distance.pid);
@@ -141,19 +147,38 @@ 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
+	if (x == ROBOT_HALF_LENGTH_REAR)
+		x = ROBOT_HALF_LENGTH_REAR + 10;
+	if (x == AREA_X - ROBOT_HALF_LENGTH_REAR)
+		x = AREA_X - ROBOT_HALF_LENGTH_REAR - 10;
+	if (y == ROBOT_HALF_LENGTH_REAR)
+		y = ROBOT_HALF_LENGTH_REAR + 10;
+	if (y == AREA_Y - ROBOT_HALF_LENGTH_REAR)
+		y = AREA_Y - ROBOT_HALF_LENGTH_REAR - 10;
+	if (x == ROBOT_HALF_LENGTH_FRONT)
+		x = ROBOT_HALF_LENGTH_FRONT + 10;
+	if (x == AREA_X - ROBOT_HALF_LENGTH_FRONT)
+		x = AREA_X - ROBOT_HALF_LENGTH_FRONT - 10;
+	if (y == ROBOT_HALF_LENGTH_FRONT)
+		y = ROBOT_HALF_LENGTH_FRONT + 10;
+	if (y == AREA_Y - ROBOT_HALF_LENGTH_FRONT)
+		y = AREA_Y - ROBOT_HALF_LENGTH_FRONT - 10;
+#endif
 	DEBUG(E_USER_STRAT, "reset pos (%s%s%s)",
 	      x == DO_NOT_SET_POS ? "" : "x",
 	      y == DO_NOT_SET_POS ? "" : "y",
@@ -174,14 +199,45 @@ uint8_t strat_calib(int16_t dist, uint8_t flags)
 	int32_t max_in = pid_get_max_in(&mainboard.angle.pid);
 	int32_t max_i = pid_get_max_I(&mainboard.angle.pid);
 	int32_t max_out = pid_get_max_out(&mainboard.angle.pid);
+	uint32_t i_thres = mainboard.distance.bd.i_thres;
+	int32_t k1 = mainboard.distance.bd.k1;
+	int32_t k2 = mainboard.distance.bd.k2;
+	uint16_t cpt_thres = mainboard.distance.bd.cpt_thres;
+
 	uint8_t err;
 
+	/* go with a lower angle pid, and with a sensible blocking
+	 * detection */
+	bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 400000, 20);
 	pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000);
-	pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
+	pid_set_gains(&mainboard.angle.pid, 500, 10, 4000);
 	trajectory_d_rel(&mainboard.traj, dist);
 	err = wait_traj_end(flags);
+	if (err != END_BLOCKING)
+		goto fail;
+
+	/* we detected a blocking, reset bd, remove angle pid and
+	 * continue */
+	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(500);
+	strat_hardstop();
+
+#ifdef HOST_VERSION
+	/* issue with block on simulator */
+	if (dist > 0)
+		trajectory_d_rel(&mainboard.traj, -10);
+	else
+		trajectory_d_rel(&mainboard.traj, 10);
+	wait_traj_end(END_TRAJ);
+#endif
+
+ fail:
 	pid_set_gains(&mainboard.angle.pid, p, i, d);
 	pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out);
+	bd_set_current_thresholds(&mainboard.distance.bd, k1, k2, i_thres, cpt_thres);
+
 	return err;
 }
 
@@ -209,6 +265,11 @@ void strat_set_speed(uint16_t d, uint16_t a)
 	IRQ_UNLOCK(flags);
 }
 
+void strat_set_acc(double d, double a)
+{
+	trajectory_set_acc(&mainboard.traj, d, a);
+}
+
 void strat_get_speed(uint16_t *d, uint16_t *a)
 {
 	uint8_t flags;
@@ -218,6 +279,15 @@ void strat_get_speed(uint16_t *d, uint16_t *a)
 	IRQ_UNLOCK(flags);
 }
 
+void strat_get_acc(double *d, double *a)
+{
+	uint8_t flags;
+	IRQ_LOCK(flags);
+	*d = mainboard.traj.d_acc;
+	*a = mainboard.traj.a_acc;
+	IRQ_UNLOCK(flags);
+}
+
 void strat_limit_speed_enable(void)
 {
 	strat_limit_speed_enabled = 1;
@@ -228,7 +298,7 @@ void strat_limit_speed_disable(void)
 	strat_limit_speed_enabled = 0;
 }
 
-/* called periodically */
+/* called periodically (note: disabled in 2010) */
 void strat_limit_speed(void)
 {
 	uint16_t lim_d = 0, lim_a = 0;
@@ -278,13 +348,15 @@ void strat_limit_speed(void)
 /* start the strat */
 void strat_start(void)
 {
-	int8_t i;
 	uint8_t err;
 
 	strat_preinit();
 
+#ifndef HOST_VERSION
 	/* if start sw not plugged */
 	if (sensor_get(S_START_SWITCH)) {
+		int8_t i;
+
 		printf_P(PSTR("No start switch, press a key or plug it\r\n"));
 
 		/* while start sw not plugged */
@@ -306,6 +378,7 @@ void strat_start(void)
 		/* while start sw plugged */
 		while (!sensor_get(S_START_SWITCH));
 	}
+#endif
 
 	strat_init();
 	err = strat_main();
@@ -313,6 +386,7 @@ void strat_start(void)
 	strat_exit();
 }
 
+
 /* return true if we have to brake due to an obstacle */
 uint8_t strat_obstacle(void)
 {
@@ -320,13 +394,22 @@ 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) < 100) {
+		if (strat_obstacle_debug != 1) {
+			DEBUG(E_USER_STRAT, "disable opp, too slow");
+			strat_obstacle_debug = 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) < 0) {
+		if (strat_obstacle_debug != 2) {
+			DEBUG(E_USER_STRAT, "no opponent found");
+			strat_obstacle_debug = 2;
+		}
 		return 0;
+	}
 
 	/* save obstacle position */
 	opponent_obstacle.x = opp_x;
@@ -334,20 +417,47 @@ 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 (strat_obstacle_debug != 3) {
+			DEBUG(E_USER_STRAT,
+			      "opponent not in area : d=%d, a=%d "
+			      "x=%d y=%d (speed_d=%d)",
+			      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
+			strat_obstacle_debug = 3;
+		}
+		return 0;
+	}
+
 	/* sensor are temporarily disabled */
-	if (sensor_obstacle_is_disabled())
+	if (sensor_obstacle_is_disabled()) {
+		if (strat_obstacle_debug != 4) {
+			DEBUG(E_USER_STRAT,
+			      "sensor are disabled: opponent d=%d, a=%d "
+			      "x=%d y=%d (speed_d=%d)",
+			      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
+			strat_obstacle_debug = 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 > 600)
+	if (opp_d > 650) {
+		if (strat_obstacle_debug != 5) {
+			DEBUG(E_USER_STRAT, "opponent too far d=%d, a=%d "
+			      "x=%d y=%d (speed_d=%d)",
+			      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
+			strat_obstacle_debug = 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 < 600 && (opp_a > 315 || opp_a < 45)) ||
+	    (mainboard.speed_d >= 0 && opp_d < 800 && (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);
@@ -355,13 +465,21 @@ 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 < 600 && (opp_a < 225 && opp_a > 135)) ||
+	    (mainboard.speed_d < 0 && opp_d < 800 && (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 (strat_obstacle_debug != 6) {
+		DEBUG(E_USER_STRAT, "opponent not in cone d=%d, a=%d "
+		      "x=%d y=%d (speed_d=%d)",
+		      opp_d, opp_a, opp_x, opp_y, mainboard.speed_d);
+		strat_obstacle_debug = 6;
+	}
+
 	return 0;
 }
 
@@ -429,6 +547,12 @@ uint8_t test_traj_end(uint8_t why)
 		return END_OBSTACLE;
 	}
 
+#ifdef HOST_VERSION
+	if (robotsim_blocking) {
+		robotsim_blocking = 0;
+		return END_BLOCKING;
+	}
+#endif
 	return 0;
 }