* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
void strat_reset_pos(int16_t x, int16_t y, int16_t a)
{
int16_t posx = position_get_x_s16(&mainboard.pos);
void strat_reset_pos(int16_t x, int16_t y, int16_t a)
{
int16_t posx = position_get_x_s16(&mainboard.pos);
int32_t p = pid_get_gain_P(&mainboard.angle.pid);
int32_t i = pid_get_gain_I(&mainboard.angle.pid);
int32_t d = pid_get_gain_D(&mainboard.angle.pid);
int32_t p = pid_get_gain_P(&mainboard.angle.pid);
int32_t i = pid_get_gain_I(&mainboard.angle.pid);
int32_t d = pid_get_gain_D(&mainboard.angle.pid);
- pid_set_gains(&mainboard.angle.pid, 150, 0, 2000);
+ pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000);
+ pid_set_gains(&mainboard.angle.pid, 200, 0, 2000);
trajectory_d_rel(&mainboard.traj, dist);
err = wait_traj_end(flags);
pid_set_gains(&mainboard.angle.pid, p, i, d);
trajectory_d_rel(&mainboard.traj, dist);
err = wait_traj_end(flags);
pid_set_gains(&mainboard.angle.pid, p, i, d);
/* if start sw plugged */
if (!sensor_get(S_START_SWITCH)) {
printf_P(PSTR("Ready, unplug start switch to start\r\n"));
/* if start sw plugged */
if (!sensor_get(S_START_SWITCH)) {
printf_P(PSTR("Ready, unplug start switch to start\r\n"));
/* opponent is in front of us */
if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) {
DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d "
/* opponent is in front of us */
if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) {
DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d "
opp_d, opp_a, x_rel, y_rel, mainboard.speed_d);
sensor_obstacle_disable();
return 1;
}
/* opponent is behind us */
if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) {
opp_d, opp_a, x_rel, y_rel, mainboard.speed_d);
sensor_obstacle_disable();
return 1;
}
/* opponent is behind us */
if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) {
- DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d",
+ 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;
opp_d, opp_a, x_rel, y_rel);
sensor_obstacle_disable();
return 1;
/* we are near the destination point (depends on current
* speed) AND the robot is in the area bounding box. */
if (why & END_NEAR) {
/* we are near the destination point (depends on current
* speed) AND the robot is in the area bounding box. */
if (why & END_NEAR) {
if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) &&
is_in_boundingbox(&robot_pt))
return END_NEAR;
}
if (trajectory_in_window(&mainboard.traj, d_near, RAD(5.0)) &&
is_in_boundingbox(&robot_pt))
return END_NEAR;
}
line, opp_x, opp_y, opp_d, opp_a);
}
else {
line, opp_x, opp_y, opp_d, opp_a);
}
else {