Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed tracking inconsistencies #187

Merged
merged 3 commits into from
Nov 26, 2024
Merged

Fixed tracking inconsistencies #187

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f6f8eed
Fixed tracking inconsistencies #177
ssejrog Nov 26, 2024
2b92c73
Trackers work & renaming
ssejrog Nov 26, 2024
a784716
Cleaning everything up
ssejrog Nov 26, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions EZ-Template-Example-Project/src/main.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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