}
/* 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
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
y_rel = sin(RAD(opp_a)) * (double)opp_d;
/* opponent too far */
- if (opp_d > 500)
+ if (opp_d > 650)
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 > 325 || opp_a < 35)) &&
+ (mainboard.speed_d > 0 && opp_d < 650 && (opp_a > 340 || opp_a < 20))) {
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);
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 < 215 && opp_a > 145)) &&
+ (mainboard.speed_d < 0 && opp_d < 650 && (opp_a < 200 && opp_a > 160))) {
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();