From: zer0 <zer0@carbon.local>
Date: Thu, 13 May 2010 13:39:23 +0000 (+0200)
Subject: reliability
X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=5573d7668c4fe0cde51b566d086582cf3d9c7e4b

reliability
---

diff --git a/projects/microb2010/ballboard/i2c_protocol.c b/projects/microb2010/ballboard/i2c_protocol.c
index 8f0242d..71ac516 100644
--- a/projects/microb2010/ballboard/i2c_protocol.c
+++ b/projects/microb2010/ballboard/i2c_protocol.c
@@ -161,6 +161,10 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 			if (size != sizeof (*cmd))
 				goto error;
 
+			beacon.robot_x = cmd->x;
+			beacon.robot_y = cmd->y;
+			beacon.robot_angle = cmd->a;
+
 			i2c_send_status();
 			break;
 		}
diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c
index bf97946..85b2ac1 100644
--- a/projects/microb2010/cobboard/actuator.c
+++ b/projects/microb2010/cobboard/actuator.c
@@ -45,7 +45,7 @@
 #include "main.h"
 #include "actuator.h"
 
-#define COBROLLER_SPEED 1000
+#define COBROLLER_SPEED 666
 
 #define SERVO_DOOR_OPEN 300
 #define SERVO_DOOR_CLOSED 525
diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c
index 1f38e1a..8714367 100644
--- a/projects/microb2010/cobboard/shovel.c
+++ b/projects/microb2010/cobboard/shovel.c
@@ -45,7 +45,7 @@
 
 #define SHOVEL_DOWN 100
 #define SHOVEL_MID  4500
-#define SHOVEL_UP   11000
+#define SHOVEL_UP   11300
 #define SHOVEL_KICKSTAND_UP   12800
 #define SHOVEL_KICKSTAND_DOWN 10000
 
diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
index 9882ba6..1168100 100644
--- a/projects/microb2010/common/i2c_commands.h
+++ b/projects/microb2010/common/i2c_commands.h
@@ -150,6 +150,11 @@ struct i2c_ans_cobboard_status {
 
 struct i2c_req_ballboard_status {
 	struct i2c_cmd_hdr hdr;
+
+	/* position sent by mainboard */
+	int16_t x;
+	int16_t y;
+	int16_t a;
 };
 
 #define I2C_ANS_BALLBOARD_STATUS 0x83
diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c
index 0700469..fbb28d9 100644
--- a/projects/microb2010/mainboard/cs.c
+++ b/projects/microb2010/mainboard/cs.c
@@ -212,9 +212,9 @@ void microb_cs_init(void)
 				 RIGHT_ENCODER, IMP_COEF * 1.);
 #else
 	rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
-				LEFT_ENCODER, IMP_COEF * -1.011718);
+				LEFT_ENCODER, IMP_COEF * -1.012729);
 	rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
-				 RIGHT_ENCODER, IMP_COEF * 1.012695);
+				 RIGHT_ENCODER, IMP_COEF * 1.01370769);
 #endif
 	/* rs will use external encoders */
 	rs_set_flags(&mainboard.rs, RS_USE_EXT);
diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index 2c27410..49b707b 100644
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -379,6 +379,10 @@ static int8_t i2c_req_ballboard_status(void)
 	struct i2c_req_ballboard_status buf;
 
 	buf.hdr.cmd = I2C_REQ_BALLBOARD_STATUS;
+	/* robot position */
+	buf.x = position_get_x_s16(&mainboard.pos);
+	buf.y = position_get_y_s16(&mainboard.pos);
+	buf.a = position_get_a_deg_s16(&mainboard.pos);
 	return i2c_send(I2C_BALLBOARD_ADDR, (uint8_t*)&buf,
 			sizeof(buf), I2C_CTRL_GENERIC);
 }
diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h
index 2fe7199..abe92f2 100755
--- a/projects/microb2010/mainboard/main.h
+++ b/projects/microb2010/mainboard/main.h
@@ -76,7 +76,7 @@
 #define MATCH_TIME 89
 
 /* decrease track to decrease angle */
-#define EXT_TRACK_MM 304.61875
+#define EXT_TRACK_MM 305.1097108
 #define VIRTUAL_TRACK_MM EXT_TRACK_MM
 
 #define ROBOT_HALF_LENGTH_FRONT 130
diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index df8ee0e..bf4b731 100644
--- a/projects/microb2010/mainboard/robotsim.c
+++ b/projects/microb2010/mainboard/robotsim.c
@@ -196,6 +196,27 @@ robotsim_i2c(uint8_t addr, uint8_t *buf, uint8_t size)
 	return 0;
 }
 
+static void beacon_update(void)
+{
+       uint8_t flags;
+       int16_t oppx, oppy;
+       double oppa, oppd;
+
+       IRQ_LOCK(flags);
+       if (ballboard.opponent_x == I2C_OPPONENT_NOT_THERE) {
+               IRQ_UNLOCK(flags);
+               return;
+       }
+       oppx = ballboard.opponent_x;
+       oppy = ballboard.opponent_y;
+       abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
+       ballboard.opponent_a = DEG(oppa);
+       if (ballboard.opponent_a < 0)
+               ballboard.opponent_a += 360;
+       ballboard.opponent_d = oppd;
+       IRQ_UNLOCK(flags);
+}
+
 /* must be called periodically */
 void robotsim_update(void)
 {
@@ -220,6 +241,8 @@ void robotsim_update(void)
 	int oppx, oppy;
 	double oppa, oppd;
 
+	beacon_update();
+
 	/* time shift the command */
 	l_pwm_shift[i] = l_pwm;
 	r_pwm_shift[i] = r_pwm;
@@ -245,12 +268,12 @@ void robotsim_update(void)
 		if (sscanf(cmd, "opp %d %d", &oppx, &oppy) == 2) {
 			abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
 			IRQ_LOCK(flags);
-			beaconboard.oppx = oppx;
-			beaconboard.oppy = oppy;
-			beaconboard.oppa = DEG(oppa);
-			if (beaconboard.oppa < 0)
-				beaconboard.oppa += 360;
-			beaconboard.oppd = oppd;
+			ballboard.opponent_x = oppx;
+			ballboard.opponent_y = oppy;
+			ballboard.opponent_a = DEG(oppa);
+			if (ballboard.opponent_a < 0)
+				ballboard.opponent_a += 360;
+			ballboard.opponent_d = oppd;
 			IRQ_UNLOCK(flags);
 		}
 	}
diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index 023db36..0b14384 100644
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -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,24 +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 > 650)
+	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_d < 500 && (opp_a > 325 || opp_a < 35)) &&
-	    (mainboard.speed_d > 0 && opp_d < 650 && (opp_a > 340 || opp_a < 20))) {
+	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);
@@ -427,14 +466,22 @@ uint8_t strat_obstacle(void)
 		return 1;
 	}
 	/* opponent is behind us */
-	if ((mainboard.speed_d < 0 && opp_d < 500 && (opp_a < 215 && opp_a > 145)) &&
-	    (mainboard.speed_d < 0 && opp_d < 650 && (opp_a < 200 && opp_a > 160))) {
+	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;
 }