Skip to content

Commit

Permalink
Merge pull request #187 from EZ-Robotics/bug/tracking-inconsistencies-2
Browse files Browse the repository at this point in the history 
Fixed tracking inconsistencies
  • Loading branch information
@ssejrog
ssejrog authored Nov 26, 2024
2 parents 2e6a5bf + a784716 commit 4db598a
Show file tree
Hide file tree
Showing 12 changed files with 445 additions and 253 deletions.
4 changes: 2 additions & 2 deletions EZ-Template-Example-Project/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,9 @@ ez::Drive chassis(
{1, 2, 3}, // Left Chassis Ports (negative port will reverse it!)
{-4, -5, -6}, // Right Chassis Ports (negative port will reverse it!)

7, // IMU Port
7, // IMU port
4.125, // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
343); // Wheel RPM
343); // Wheel RPM = cartridge * (motor gear / wheel gear)

// Are you using tracking wheels? Comment out which ones you're using here!
// `2.75` is the wheel diameter
Expand Down
57 changes: 45 additions & 12 deletions include/EZ-Template/drive/drive.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,22 +88,22 @@ class Drive {
/**
* Left vertical tracking wheel.
*/
tracking_wheel* odom_left_tracker;
tracking_wheel* odom_tracker_left;

/**
* Right vertical tracking wheel.
*/
tracking_wheel* odom_right_tracker;
tracking_wheel* odom_tracker_right;

/**
* Front horizontal tracking wheel.
*/
tracking_wheel* odom_front_tracker;
tracking_wheel* odom_tracker_front;

/**
* Back horizontal tracking wheel.
*/
tracking_wheel* odom_back_tracker;
tracking_wheel* odom_tracker_back;

/**
* PID objects.
Expand All @@ -118,7 +118,9 @@ class Drive {
PID forward_swingPID;
PID backward_swingPID;
PID xyPID;
PID aPID;
PID current_a_odomPID;
PID boomerangPID;
PID odom_angularPID;
PID internal_leftPID;
PID internal_rightPID;

Expand Down Expand Up @@ -2737,6 +2739,26 @@ class Drive {
*/
int pid_turn_min_get();

/**
* @brief Set the heading pid constants object
*
* @param p kP
* @param i kI
* @param d kD
* @param p_start_i start_I
*/
void pid_odom_angular_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);

/**
* @brief Set the heading pid constants object
*
* @param p kP
* @param i kI
* @param d kD
* @param p_start_i start_I
*/
void pid_odom_boomerang_constants_set(double p, double i = 0.0, double d = 0.0, double p_start_i = 0.0);

/**
* Set's constants for odom driving exit conditions.
*
Expand Down Expand Up @@ -3041,11 +3063,17 @@ class Drive {
std::vector<const_and_name> pid_tuner_pids = {
{"Drive Forward PID Constants", &forward_drivePID.constants},
{"Drive Backward PID Constants", &backward_drivePID.constants},
{"Odom Angular PID Constants", &odom_angularPID.constants},
{"Boomerang Angular PID Constants", &boomerangPID.constants},
{"Heading PID Constants", &headingPID.constants},
{"Turn PID Constants", &turnPID.constants},
{"Swing Forward PID Constants", &forward_swingPID.constants},
{"Swing Backward PID Constants", &backward_swingPID.constants}};

bool odom_use_left = true;
double odom_ime_track_width_left = 0.0;
double odom_ime_track_width_right = 0.0;

private:
// odom privates
std::vector<odom> pp_movements;
Expand Down Expand Up @@ -3075,7 +3103,7 @@ class Drive {
bool odom_turn_bias_enabled();
void odom_turn_bias_set(bool set);
double angle_rad = 0.0;
double track_width = 0.0;
double global_track_width = 0.0;
bool odometry_enabled = true;
pose odom_target = {0, 0, 0};
pose odom_current = {0, 0, 0};
Expand All @@ -3093,8 +3121,13 @@ class Drive {
double max_boomerang_distance = 12.0;
double odom_turn_bias_amount = 1.375;
drive_directions current_drive_direction = fwd;
double h_last = 0, v_last = 0;
double last_theta = 0;
double h_last = 0.0, t_last = 0.0, l_last = 0.0, r_last = 0.0;
pose l_pose{0.0, 0.0, 0.0};
pose r_pose{0.0, 0.0, 0.0};
pose central_pose{0.0, 0.0, 0.0};
std::pair<float, float> decide_vert_sensor(ez::tracking_wheel* tracker, bool is_tracker_enabled, float ime = 0.0, float ime_track = 0.0);
pose solve_xy_vert(float p_track_width, float current_t, float delta_vert, float delta_t);
pose solve_xy_horiz(float p_track_width, float current_t, float delta_horiz, float delta_t);
bool was_last_pp_mode_boomerang = false;
bool global_forward_drive_slew_enabled = false;
bool global_backward_drive_slew_enabled = false;
Expand All @@ -3110,10 +3143,10 @@ class Drive {
std::vector<odom> set_odoms_direction(std::vector<odom> inputs);
odom set_odom_direction(odom input);
pose flip_pose(pose input);
bool odom_left_tracker_enabled = false;
bool odom_right_tracker_enabled = false;
bool odom_front_tracker_enabled = false;
bool odom_back_tracker_enabled = false;
bool odom_tracker_left_enabled = false;
bool odom_tracker_right_enabled = false;
bool odom_tracker_front_enabled = false;
bool odom_tracker_back_enabled = false;

double chain_target_start = 0.0;
double chain_sensor_start = 0.0;
Expand Down
64 changes: 34 additions & 30 deletions src/EZ-Template/drive/drive.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,10 +195,12 @@ void Drive::drive_defaults_set() {
std::cout << std::setprecision(2);

// PID Constants
pid_heading_constants_set(7, 0, 45, 0);
pid_drive_constants_set(20, 0, 100, 0);
pid_turn_constants_set(3, 0.05, 20, 15);
pid_swing_constants_set(6, 0, 65);
pid_drive_constants_set(20.0, 0.0, 100.0);
pid_heading_constants_set(11.0, 0.0, 20.0);
pid_turn_constants_set(3.0, 0.05, 20.0, 15.0);
pid_swing_constants_set(6.0, 0.0, 65.0);
pid_odom_angular_constants_set(5.0, 0.0, 60.0);
pid_odom_boomerang_constants_set(3.5, 0.0, 35.0);
pid_turn_min_set(30);
pid_swing_min_set(30);

Expand All @@ -211,8 +213,8 @@ void Drive::drive_defaults_set() {
pid_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
pid_swing_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms);
pid_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms);
pid_odom_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms);
pid_odom_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms);
pid_odom_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms);
pid_odom_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms);

pid_turn_chain_constant_set(3_deg);
pid_swing_chain_constant_set(5_deg);
Expand All @@ -235,7 +237,7 @@ void Drive::drive_defaults_set() {

double Drive::drive_tick_per_inch() {
if (is_tracker == ODOM_TRACKER)
return odom_right_tracker->ticks_per_inch();
return odom_tracker_right->ticks_per_inch();

CIRCUMFERENCE = WHEEL_DIAMETER * M_PI;

Expand Down Expand Up @@ -294,10 +296,16 @@ int Drive::drive_current_limit_get() {

// Motor telemetry
void Drive::drive_sensor_reset() {
v_last = 0;
h_last = 0;
h_last = 0.0;
l_last = 0.0;
r_last = 0.0;
t_last = 0.0;
left_motors.front().tare_position();
right_motors.front().tare_position();
if (odom_tracker_left_enabled) odom_tracker_left->reset();
if (odom_tracker_right_enabled) odom_tracker_right->reset();
if (odom_tracker_front_enabled) odom_tracker_front->reset();
if (odom_tracker_back_enabled) odom_tracker_back->reset();
if (is_tracker == DRIVE_ADI_ENCODER) {
left_tracker.reset();
right_tracker.reset();
Expand All @@ -307,10 +315,6 @@ void Drive::drive_sensor_reset() {
right_rotation.reset_position();
return;
}
if (odom_left_tracker_enabled) odom_left_tracker->reset();
if (odom_right_tracker_enabled) odom_right_tracker->reset();
if (odom_front_tracker_enabled) odom_front_tracker->reset();
if (odom_back_tracker_enabled) odom_back_tracker->reset();
}

int Drive::drive_sensor_right_raw() {
Expand All @@ -319,12 +323,12 @@ int Drive::drive_sensor_right_raw() {
else if (is_tracker == DRIVE_ROTATION)
return right_rotation.get_position();
else if (is_tracker == ODOM_TRACKER)
return odom_right_tracker->get_raw();
return odom_tracker_right->get_raw();
return right_motors.front().get_position();
}
double Drive::drive_sensor_right() {
if (is_tracker == ODOM_TRACKER)
return odom_right_tracker->get();
return odom_tracker_right->get();
return drive_sensor_right_raw() / drive_tick_per_inch();
}
int Drive::drive_velocity_right() { return right_motors.front().get_actual_velocity(); }
Expand All @@ -337,12 +341,12 @@ int Drive::drive_sensor_left_raw() {
else if (is_tracker == DRIVE_ROTATION)
return left_rotation.get_position();
else if (is_tracker == ODOM_TRACKER)
return odom_left_tracker->get_raw();
return odom_tracker_left->get_raw();
return left_motors.front().get_position();
}
double Drive::drive_sensor_left() {
if (is_tracker == ODOM_TRACKER)
return odom_left_tracker->get();
return odom_tracker_left->get();
return drive_sensor_left_raw() / drive_tick_per_inch();
}
int Drive::drive_velocity_left() { return left_motors.front().get_actual_velocity(); }
Expand All @@ -352,7 +356,7 @@ bool Drive::drive_current_left_over() { return left_motors.front().is_over_curre
void Drive::drive_imu_reset(double new_heading) {
imu.set_rotation(new_heading);
angle_rad = util::to_rad(new_heading);
last_theta = angle_rad;
t_last = angle_rad;
}
double Drive::drive_imu_get() { return imu.get_rotation() * IMU_SCALER; }
double Drive::drive_imu_accel_get() { return imu.get_accel().x + imu.get_accel().y; }
Expand Down Expand Up @@ -450,40 +454,40 @@ void Drive::initialize() {
void Drive::odom_tracker_left_set(tracking_wheel* input) {
if (input == nullptr) return;

odom_left_tracker = input;
odom_left_tracker_enabled = true;
odom_tracker_left = input;
odom_tracker_left_enabled = true;

// Assume the user input a positive number and set it to a negative number
odom_left_tracker->distance_to_center_flip_set(true);
odom_tracker_left->distance_to_center_flip_set(true);

// If the user has input a left and right tracking wheel,
// the tracking wheels become the new sensors always
if (odom_right_tracker_enabled)
if (odom_tracker_right_enabled)
is_tracker = ODOM_TRACKER;
}
void Drive::odom_tracker_right_set(tracking_wheel* input) {
if (input == nullptr) return;

odom_right_tracker = input;
odom_right_tracker_enabled = true;
odom_tracker_right = input;
odom_tracker_right_enabled = true;

// If the user has input a left and right tracking wheel,
// the tracking wheels become the new sensors always
if (odom_left_tracker_enabled)
if (odom_tracker_left_enabled)
is_tracker = ODOM_TRACKER;
}
void Drive::odom_tracker_front_set(tracking_wheel* input) {
if (input == nullptr) return;

odom_front_tracker = input;
odom_front_tracker_enabled = true;
odom_tracker_front = input;
odom_tracker_front_enabled = true;
}
void Drive::odom_tracker_back_set(tracking_wheel* input) {
if (input == nullptr) return;

odom_back_tracker = input;
odom_back_tracker_enabled = true;
odom_tracker_back = input;
odom_tracker_back_enabled = true;

// Set the center distance to be negative
odom_back_tracker->distance_to_center_flip_set(true);
odom_tracker_back->distance_to_center_flip_set(true);
}
40 changes: 20 additions & 20 deletions src/EZ-Template/drive/exit_conditions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,8 +82,8 @@ void Drive::pid_odom_drive_exit_condition_set(okapi::QTime p_small_exit_time, ok
}

void Drive::pid_odom_turn_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout, bool use_imu) {
aPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
aPID.velocity_sensor_secondary_toggle_set(use_imu);
current_a_odomPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
current_a_odomPID.velocity_sensor_secondary_toggle_set(use_imu);
}

void Drive::pid_odom_turn_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu) {
Expand Down Expand Up @@ -129,12 +129,12 @@ void Drive::pid_wait() {
if (mode == PURE_PURSUIT) {
while (pp_index != pp_movements.size() - 1) {
xyPID.velocity_sensor_secondary_set(drive_imu_accel_get());
aPID.velocity_sensor_secondary_set(drive_imu_accel_get());
current_a_odomPID.velocity_sensor_secondary_set(drive_imu_accel_get());
xy_exit = xy_exit != RUNNING ? xy_exit : xyPID.exit_condition({left_motors[0], right_motors[0]});
a_exit = a_exit != RUNNING ? a_exit : aPID.exit_condition({left_motors[0], right_motors[0]});
a_exit = a_exit != RUNNING ? a_exit : current_a_odomPID.exit_condition({left_motors[0], right_motors[0]});

if ((xy_exit == mA_EXIT || xy_exit == VELOCITY_EXIT) && (a_exit == mA_EXIT || a_exit == VELOCITY_EXIT)) {
if (print_toggle) std::cout << " XY: " << exit_to_string(xy_exit) << " Exited early, error: " << xyPID.error << ". Angle: " << exit_to_string(a_exit) << " Exited early, error: " << aPID.error << ".\n";
if (print_toggle) std::cout << " XY: " << exit_to_string(xy_exit) << " Exited early, error: " << xyPID.error << ". Angle: " << exit_to_string(a_exit) << " Exited early, error: " << current_a_odomPID.error << ".\n";
break;
}

Expand All @@ -145,12 +145,12 @@ void Drive::pid_wait() {
// When we're at the last point in PP / we're just going to point
while (xy_exit == RUNNING || a_exit == RUNNING) {
xyPID.velocity_sensor_secondary_set(drive_imu_accel_get());
aPID.velocity_sensor_secondary_set(drive_imu_accel_get());
current_a_odomPID.velocity_sensor_secondary_set(drive_imu_accel_get());
xy_exit = xy_exit != RUNNING ? xy_exit : xyPID.exit_condition({left_motors[0], right_motors[0]});
a_exit = a_exit != RUNNING ? a_exit : aPID.exit_condition({left_motors[0], right_motors[0]});
a_exit = a_exit != RUNNING ? a_exit : current_a_odomPID.exit_condition({left_motors[0], right_motors[0]});
pros::delay(util::DELAY_TIME);
}
if (print_toggle) std::cout << " XY: " << exit_to_string(xy_exit) << " Exit, error: " << xyPID.error << ". Angle: " << exit_to_string(a_exit) << " Exit, error: " << aPID.error << ".\n";
if (print_toggle) std::cout << " XY: " << exit_to_string(xy_exit) << " Exit, error: " << xyPID.error << ". Angle: " << exit_to_string(a_exit) << " Exit, error: " << current_a_odomPID.error << ".\n";

if (xy_exit == mA_EXIT || xy_exit == VELOCITY_EXIT || a_exit == mA_EXIT || a_exit == VELOCITY_EXIT) {
interfered = true;
Expand Down Expand Up @@ -355,30 +355,30 @@ void Drive::pid_wait_until(double target) {
void Drive::pid_wait_until_point(pose target) {
pros::delay(10);

int xy_sgn = util::sgn(is_past_target(target, odom_current));
int xy_sgn = util::sgn(is_past_target(target, odom_pose_get()));

exit_output xy_exit = RUNNING;
exit_output a_exit = RUNNING;

while (true) {
xyPID.velocity_sensor_secondary_set(drive_imu_accel_get());
aPID.velocity_sensor_secondary_set(drive_imu_accel_get());
current_a_odomPID.velocity_sensor_secondary_set(drive_imu_accel_get());
xy_exit = xy_exit != RUNNING ? xy_exit : xyPID.exit_condition({left_motors[0], right_motors[0]});
a_exit = a_exit != RUNNING ? a_exit : aPID.exit_condition({left_motors[0], right_motors[0]});
a_exit = a_exit != RUNNING ? a_exit : current_a_odomPID.exit_condition({left_motors[0], right_motors[0]});

if (xy_exit != RUNNING && a_exit != RUNNING) {
if (print_toggle) {
std::cout << " XY: " << exit_to_string(xy_exit) << " Wait Until Exit Failsafe, triggered at (" << odom_current.x << ", " << odom_current.y << ") instead of (" << target.x << ", " << target.y << ")\n";
std::cout << " XY: " << exit_to_string(xy_exit) << " Wait Until Exit Failsafe, triggered at (" << odom_x_get() << ", " << odom_y_get() << ") instead of (" << target.x << ", " << target.y << ")\n";
xyPID.timers_reset();
aPID.timers_reset();
current_a_odomPID.timers_reset();
}
return;
}

if (util::sgn((is_past_target(target, odom_current))) != xy_sgn) {
if (print_toggle) printf(" XY Wait Until Exit Success, triggered at (%.2f, %.2f). Target: (%.2f, %.2f)\n", odom_current.x, odom_current.y, target.x, target.y);
if (util::sgn((is_past_target(target, odom_pose_get()))) != xy_sgn) {
if (print_toggle) printf(" XY Wait Until Exit Success, triggered at (%.2f, %.2f). Target: (%.2f, %.2f)\n", odom_x_get(), odom_y_get(), target.x, target.y);
xyPID.timers_reset();
aPID.timers_reset();
current_a_odomPID.timers_reset();
return;
}

Expand All @@ -403,15 +403,15 @@ void Drive::pid_wait_until_index(int index) {
exit_output a_exit = RUNNING;
while (pp_index < injected_pp_index[index]) {
xyPID.velocity_sensor_secondary_set(drive_imu_accel_get());
aPID.velocity_sensor_secondary_set(drive_imu_accel_get());
current_a_odomPID.velocity_sensor_secondary_set(drive_imu_accel_get());
xy_exit = xy_exit != RUNNING ? xy_exit : xyPID.exit_condition({left_motors[0], right_motors[0]});
a_exit = a_exit != RUNNING ? a_exit : aPID.exit_condition({left_motors[0], right_motors[0]});
a_exit = a_exit != RUNNING ? a_exit : current_a_odomPID.exit_condition({left_motors[0], right_motors[0]});

if (xy_exit != RUNNING && a_exit != RUNNING) {
if (print_toggle) {
std::cout << " XY: " << exit_to_string(xy_exit) << " Wait Until Exit Failsafe, triggered at (" << odom_current.x << ", " << odom_current.y << ") instead of (" << pp_movements[index].target.x << ", " << pp_movements[index].target.y << ")\n";
std::cout << " XY: " << exit_to_string(xy_exit) << " Wait Until Exit Failsafe, triggered at (" << odom_x_get() << ", " << odom_y_get() << ") instead of (" << pp_movements[index].target.x << ", " << pp_movements[index].target.y << ")\n";
xyPID.timers_reset();
aPID.timers_reset();
current_a_odomPID.timers_reset();
}
break;
}
Expand Down
Loading

0 comments on commit 4db598a

Please sign in to comment.