From: zer0 <zer0@carbon.local>
Date: Fri, 14 May 2010 00:38:04 +0000 (+0200)
Subject: irrational unblocking, low speed obstacle bug and spicke pack when y too big
X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=e6e4582229cd4ed8ffe73bdfbae79d236129df3b

irrational unblocking, low speed obstacle bug and spicke pack when y too big
---

diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index 583f9a8..ff5bb02 100644
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -418,6 +418,8 @@ while True:
                 fw.write("l")
             elif k == "r":
                 fw.write("r")
+            elif k == "b":
+                fw.write("b")
             elif k == "c":
                 robot_trail_list = []
             elif k == "x":
diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index bf4b731..6971155 100644
--- a/projects/microb2010/mainboard/robotsim.c
+++ b/projects/microb2010/mainboard/robotsim.c
@@ -54,6 +54,8 @@
 #include "strat_utils.h"
 #include "main.h"
 
+uint8_t robotsim_blocking = 0;
+
 static int32_t l_pwm, r_pwm;
 static int32_t l_enc, r_enc;
 
@@ -264,6 +266,8 @@ void robotsim_update(void)
 		pertl = 1;
 	else if (cmd[0] == 'r')
 		pertr = 1;
+	else if (cmd[0] == 'b')
+		robotsim_blocking = 1;
 	if (cmd[0] == 'o') {
 		if (sscanf(cmd, "opp %d %d", &oppx, &oppy) == 2) {
 			abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h
index 7c1eccf..491f2b3 100644
--- a/projects/microb2010/mainboard/robotsim.h
+++ b/projects/microb2010/mainboard/robotsim.h
@@ -19,6 +19,8 @@
  *
  */
 
+extern uint8_t robotsim_blocking;
+
 int8_t robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size);
 void robotsim_update(void);
 void robotsim_pwm(void *arg, int32_t val);
diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 7f7b9d9..41e7e8b 100644
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -234,6 +234,9 @@ static void check_corn(void)
 	static uint8_t prev_check_time;
 	uint8_t cur_time;
 	uint8_t need_lpack, need_rpack;
+	int16_t l_xspickle, l_yspickle;
+	int16_t r_xspickle, r_yspickle;
+	uint8_t l_y_too_high_pack = 0, r_y_too_high_pack = 0;
 
 	/* read sensors from ballboard */
 	IRQ_LOCK(flags);
@@ -263,26 +266,33 @@ static void check_corn(void)
 	}
 
 	/* detect cob on left side */
-	lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE);
+	lcob_near = corn_is_near(&lidx, I2C_LEFT_SIDE,
+				 &l_xspickle, &l_yspickle);
 	if (lcob_near && lcob != I2C_COB_NONE) {
 		if (strat_db.corn_table[lidx]->corn.color == I2C_COB_UNKNOWN)
 			DEBUG(E_USER_STRAT, "lcob %s %d",
 			      lcob == I2C_COB_WHITE ? "white" : "black", lidx);
 		corn_set_color(strat_db.corn_table[lidx], lcob);
 	}
+	if (!__y_is_more_than(l_yspickle, 600))
+		l_y_too_high_pack = 1;
 
 	/* detect cob on right side */
-	rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE);
+	rcob_near = corn_is_near(&ridx, I2C_RIGHT_SIDE,
+				 &r_xspickle, &r_yspickle);
 	if (rcob_near && rcob != I2C_COB_NONE) {
 		if (strat_db.corn_table[ridx]->corn.color == I2C_COB_UNKNOWN)
 			DEBUG(E_USER_STRAT, "rcob %s %d",
 			      rcob == I2C_COB_WHITE ? "white" : "black", ridx);
 		corn_set_color(strat_db.corn_table[ridx], rcob);
 	}
+	if (!__y_is_more_than(r_yspickle, 600))
+		r_y_too_high_pack = 1;
 
 	/* control the cobboard mode for left spickle */
 	need_lpack = get_cob_count() >= 5 || strat_want_pack ||
-		strat_lpack60 || strat_opponent_lpack;
+		strat_lpack60 || strat_opponent_lpack || l_y_too_high_pack;
+
 	if (lcob_near && strat_db.corn_table[lidx]->present) {
 		if (need_lpack) {
 			/* nothing  */
@@ -319,7 +329,7 @@ static void check_corn(void)
 
 	/* control the cobboard mode for right spickle */
 	need_rpack = get_cob_count() >= 5 || strat_want_pack ||
-		strat_rpack60 || strat_opponent_rpack;
+		strat_rpack60 || strat_opponent_rpack || r_y_too_high_pack;
 	if (rcob_near && strat_db.corn_table[ridx]->present) {
 		if (need_rpack) {
 			/* nothing */
diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c
index 73f97f7..37483ce 100644
--- a/projects/microb2010/mainboard/strat_avoid.c
+++ b/projects/microb2010/mainboard/strat_avoid.c
@@ -552,7 +552,7 @@ int16_t linedir2angle(uint8_t dir)
 	case LINE_R_UP:
 		return COLOR_A(30);
 	case LINE_R_DOWN:
-		return COLOR_A(-90);
+		return COLOR_A(-30);
 	case LINE_L_UP:
 		return COLOR_A(150);
 	case LINE_L_DOWN:
@@ -562,6 +562,25 @@ int16_t linedir2angle(uint8_t dir)
 	}
 }
 
+int16_t get_nearest_dir_angle(int16_t a)
+{
+	uint8_t dir, min_dir = 0;
+	int16_t min_diff = 0x7FFF, diff;
+
+	for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
+		diff = abs(linedir2angle(dir) - a);
+		if (diff > 360)
+			diff -= 360;
+		if (diff > 360)
+			diff -= 360;
+		if (diff < min_diff) {
+			min_diff = diff;
+			min_dir = dir;
+		}
+	}
+	return linedir2angle(min_dir);
+}
+
 /* return true if a waypoint belongs to a line */
 uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
 {
@@ -1107,8 +1126,8 @@ const struct xy_point unblock_pts[] = {
 /* try to unblock in any situation */
 uint8_t strat_unblock(void)
 {
-	int16_t x, y, posx, posy;
-	uint8_t i, j, k;
+	int16_t x, y, posx, posy, posa;
+	uint8_t i, j, k, cpt;
 	uint16_t old_dspeed, old_aspeed;
 	uint8_t err;
 	uint16_t d_min = 0x7FFF, d;
@@ -1120,6 +1139,8 @@ uint8_t strat_unblock(void)
 	strat_get_speed(&old_dspeed, &old_aspeed);
 
 	strat_hardstop();
+	posa = position_get_a_deg_s16(&mainboard.pos);
+
 	strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
 	posx = position_get_x_s16(&mainboard.pos);
 	posy = position_get_y_s16(&mainboard.pos);
@@ -1148,10 +1169,32 @@ uint8_t strat_unblock(void)
 	DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d",
 	      __FUNCTION__, x, y);
 
-	/* XXX if opponent is too close, go back, or wait ? */
+	for (cpt = 0; cpt < 2; cpt++) {
+
+		/* go to nearest waypoint */
+		trajectory_goto_xy_abs(&mainboard.traj, x, y);
+		err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+		if (err == END_TIMER)
+			return err;
+
+		if (TRAJ_SUCCESS(err))
+			break;
+
+		if (cpt == 1)
+			break;
+
+		/* aie... do a S */
+		trajectory_d_a_rel(&mainboard.traj, 100, 20);
+		err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+		trajectory_d_a_rel(&mainboard.traj, 100, -20);
+		err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+		trajectory_d_a_rel(&mainboard.traj, -100, -20);
+		err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+		trajectory_d_a_rel(&mainboard.traj, -100, 20);
+		err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	}
 
-	/* go to nearest waypoint */
-	trajectory_goto_xy_abs(&mainboard.traj, x, y);
+	trajectory_a_abs(&mainboard.traj, get_nearest_dir_angle(posa));
 	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 	if (err == END_TIMER)
 		return err;
diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index 0b14384..5a819c9 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;
 
@@ -382,7 +386,6 @@ 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)
@@ -391,22 +394,19 @@ uint8_t strat_obstacle(void)
 	int16_t opp_x, opp_y, opp_d, opp_a;
 
 	/* too slow */
-	if (ABS(mainboard.speed_d) < 0) {
-		if (xxxdebug != 1) {
-			DEBUG(E_USER_STRAT, "XXX too slow");
-			xxxdebug = 1;
+	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 (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;
+	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;
 	}
@@ -418,24 +418,24 @@ uint8_t strat_obstacle(void)
 	opponent_obstacle.a = opp_a;
 
 	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 "
+		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);
-			xxxdebug = 3;
+			strat_obstacle_debug = 3;
 		}
 		return 0;
 	}
 
 	/* sensor are temporarily disabled */
 	if (sensor_obstacle_is_disabled()) {
-		if (xxxdebug != 4) {
-			DEBUG(E_USER_STRAT, "XXX disabled");
-			DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+		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);
-			xxxdebug = 4;
+			strat_obstacle_debug = 4;
 		}
 		return 0;
 	}
@@ -446,12 +446,11 @@ uint8_t strat_obstacle(void)
 
 	/* opponent too far */
 	if (opp_d > 650) {
-		if (xxxdebug != 5) {
-			DEBUG(E_USER_STRAT, "XXX too far");
-			DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+		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);
-			xxxdebug = 5;
+			strat_obstacle_debug = 5;
 		}
 		return 0;
 	}
@@ -474,12 +473,11 @@ uint8_t strat_obstacle(void)
 		return 1;
 	}
 
-	if (xxxdebug != 6) {
-		DEBUG(E_USER_STRAT, "XXX not in cone");
-		DEBUG(E_USER_STRAT, "opponent d=%d, a=%d "
+	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);
-		xxxdebug = 6;
+		strat_obstacle_debug = 6;
 	}
 
 	return 0;
@@ -549,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;
 }
 
diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index db672bf..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)
@@ -300,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 */
diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h
index 6f597f1..4f50cb6 100644
--- a/projects/microb2010/mainboard/strat_corn.h
+++ b/projects/microb2010/mainboard/strat_corn.h
@@ -33,7 +33,8 @@ struct line_2pts {
 };
 
 /* there is a corn near */
-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);
 
 /* go from line num1,dir1 to line num2,dir2. Uses trjectory flags
  * specified as argument and return END_xxx condition */
diff --git a/projects/microb2010/mainboard/strat_utils.c b/projects/microb2010/mainboard/strat_utils.c
index 9deba04..1b98929 100644
--- a/projects/microb2010/mainboard/strat_utils.c
+++ b/projects/microb2010/mainboard/strat_utils.c
@@ -184,11 +184,8 @@ uint8_t robot_is_in_area(int16_t margin)
 
 /* return 1 or 0 depending on which side of a line (y=cste) is the
  * robot. works in yellow or blue color. */
-uint8_t y_is_more_than(int16_t y)
+uint8_t __y_is_more_than(int16_t posy, int16_t y)
 {
-	int16_t posy;
-
-	posy = position_get_y_s16(&mainboard.pos);
 	if (mainboard.our_color == I2C_COLOR_YELLOW) {
 		if (posy > y)
 			return 1;
@@ -205,17 +202,32 @@ uint8_t y_is_more_than(int16_t y)
 
 /* return 1 or 0 depending on which side of a line (x=cste) is the
  * robot. works in yellow or blue color. */
-uint8_t x_is_more_than(int16_t x)
+uint8_t __x_is_more_than(int16_t posx, int16_t x)
 {
-	int16_t posx;
-
-	posx = position_get_x_s16(&mainboard.pos);
 	if (posx > x)
 		return 1;
 	else
 		return 0;
 }
 
+/* return 1 or 0 depending on which side of a line (y=cste) is the
+ * robot. works in yellow or blue color. */
+uint8_t y_is_more_than(int16_t y)
+{
+	int16_t posy;
+	posy = position_get_y_s16(&mainboard.pos);
+	return __y_is_more_than(posy, y);
+}
+
+/* return 1 or 0 depending on which side of a line (x=cste) is the
+ * robot. works in yellow or blue color. */
+uint8_t x_is_more_than(int16_t x)
+{
+	int16_t posx;
+	posx = position_get_x_s16(&mainboard.pos);
+	return __x_is_more_than(posx, x);
+}
+
 int16_t sin_table[] = {
 	0,
 	3211,
diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h
index 2414c04..5c7b8e9 100644
--- a/projects/microb2010/mainboard/strat_utils.h
+++ b/projects/microb2010/mainboard/strat_utils.h
@@ -78,6 +78,8 @@ uint8_t robot_is_in_area(int16_t margin);
 uint8_t robot_is_near_disc(void);
 uint8_t y_is_more_than(int16_t y);
 uint8_t x_is_more_than(int16_t x);
+uint8_t __y_is_more_than(int16_t posy, int16_t y);
+uint8_t __x_is_more_than(int16_t posx, int16_t x);
 int16_t fast_sin(int16_t deg);
 int16_t fast_cos(int16_t deg);
 uint8_t get_color(void);