From 5573d7668c4fe0cde51b566d086582cf3d9c7e4b Mon Sep 17 00:00:00 2001 From: zer0 Date: Thu, 13 May 2010 15:39:23 +0200 Subject: [PATCH] reliability --- projects/microb2010/ballboard/i2c_protocol.c | 4 ++ projects/microb2010/cobboard/actuator.c | 2 +- projects/microb2010/cobboard/shovel.c | 2 +- projects/microb2010/common/i2c_commands.h | 5 ++ projects/microb2010/mainboard/cs.c | 4 +- projects/microb2010/mainboard/i2c_protocol.c | 4 ++ projects/microb2010/mainboard/main.h | 2 +- projects/microb2010/mainboard/robotsim.c | 35 +++++++++-- projects/microb2010/mainboard/strat_base.c | 65 +++++++++++++++++--- 9 files changed, 103 insertions(+), 20 deletions(-) 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; } -- 2.39.5