Skip to content

Commit

Permalink
✨Imu velocity exiting #102
Browse files Browse the repository at this point in the history 
* PID class has secondary sensor velocity support, implemented that with imu accel into exit conditions

* Made names more clear, added missing functions
  • Loading branch information
@ssejrog
ssejrog committed Jun 2, 2024
1 parent 70f7588 commit 97cc097
Show file tree
Hide file tree
Showing 5 changed files with 135 additions and 19 deletions.
58 changes: 57 additions & 1 deletion include/EZ-Template/PID.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,58 @@ class PID {
*/
exit_condition_ exit;

/**
* Updates a secondary sensor for velocity exiting. Ideal use is IMU during normal drive motions.
*
* \param secondary_sensor
* double for a secondary sensor.
*/
void velocity_sensor_secondary_set(double secondary_sensor);

/**
* Returns the updated secondary sensor for velocity exiting.
*/
double velocity_sensor_secondary_get();

/**
* Boolean for if the secondary sensor will be updated or not. True uses this sensor, false does not.
*
* \param toggle
* True uses this sensor, false does not.
*/
void velocity_sensor_secondary_toggle_set(bool toggle);

/**
* Returns the boolean for if the secondary sensor will be updated or not. True uses this sensor, false does not.
*/
bool velocity_sensor_secondary_toggle_get();

/**
* Sets the threshold that the main sensor will return 0 velocity within
*
* \param zero
* a small double
*/
void velocity_sensor_main_exit_set(double zero);

/**
* Returns the threshold that the main sensor will return 0 velocity within
*/
double velocity_sensor_main_exit_get();

/**
* Sets the threshold that the secondary sensor will return 0 velocity within
*
* \param zero
* a small double
*/
void velocity_sensor_secondary_exit_set(double zero);

/**
* Returns the threshold that the secondary sensor will return 0 velocity within
*/
double velocity_sensor_secondary_exit_get();

/**
* Iterative exit condition for PID.
*
Expand Down Expand Up @@ -206,13 +258,17 @@ class PID {
long prev_time = 0;

private:
int i = 0, j = 0, k = 0, l = 0;
double velocity_zero_main = 0.05;
double velocity_zero_secondary = 0.1;
int i = 0, j = 0, k = 0, l = 0, m = 0;
bool is_mA = false;
double second_sensor = 0.0;
void timers_reset();
std::string name;
bool name_active = false;
void exit_condition_print(ez::exit_output exit_type);
bool reset_i_sgn = true;
double raw_compute();
bool use_second_sensor = false;
};
}; // namespace ez
31 changes: 24 additions & 7 deletions include/EZ-Template/drive/drive.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -649,10 +649,15 @@ class Drive {
void drive_imu_reset(double new_heading = 0);

/**
* Returns the current imu value.
* Returns the current imu rotation value.
*/
double drive_imu_get();

/**
* Returns the current imu accel x + accel y value.
*/
double drive_imu_accel_get();

/**
* Sets a new imu scaling factor. This value is multiplied by the imu to change its output.
*
Expand Down Expand Up @@ -1110,8 +1115,10 @@ class Drive {
* Sets big_error. Timer will start when error is within this. In okapi units.
* \param p_velocity_exit_time
* Sets velocity_exit_time. Timer will start when velocity is 0. In okapi units.
* \param use_imu
* Adds the Imu for velocity calculation in conjunction with the main sensor.
*/
void pid_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QLength p_small_error, okapi::QTime p_big_exit_time, okapi::QLength p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout);
void pid_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QLength p_small_error, okapi::QTime p_big_exit_time, okapi::QLength p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu = true);

/**
* Set's constants for turn exit conditions.
Expand All @@ -1126,8 +1133,10 @@ class Drive {
* Sets big_error. Timer will start when error is within this. In okapi units.
* \param p_velocity_exit_time
* Sets velocity_exit_time. Timer will start when velocity is 0. In okapi units.
* \param use_imu
* Adds the Imu for velocity calculation in conjunction with the main sensor.
*/
void pid_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);
void pid_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 = true);

/**
* Set's constants for swing exit conditions.
Expand All @@ -1142,8 +1151,10 @@ class Drive {
* Sets big_error. Timer will start when error is within this. In okapi units.
* \param p_velocity_exit_time
* Sets velocity_exit_time. Timer will start when velocity is 0. In okapi units.
* \param use_imu
* Adds the Imu for velocity calculation in conjunction with the main sensor.
*/
void pid_swing_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);
void pid_swing_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 = true);

/**
* Set's constants for drive exit conditions.
Expand All @@ -1158,8 +1169,10 @@ class Drive {
* Sets big_error. Timer will start when error is within this.
* \param p_velocity_exit_time
* Sets velocity_exit_time. Timer will start when velocity is 0.
* \param use_imu
* Adds the Imu for velocity calculation in conjunction with the main sensor.
*/
void pid_drive_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);
void pid_drive_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 = true);

/**
* Set's constants for turn exit conditions.
Expand All @@ -1174,8 +1187,10 @@ class Drive {
* Sets big_error. Timer will start when error is within this.
* \param p_velocity_exit_time
* Sets velocity_exit_time. Timer will start when velocity is 0.
* \param use_imu
* Adds the Imu for velocity calculation in conjunction with the main sensor.
*/
void pid_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);
void pid_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 = true);

/**
* Set's constants for swing exit conditions.
Expand All @@ -1190,8 +1205,10 @@ class Drive {
* Sets big_error. Timer will start when error is within this.
* \param p_velocity_exit_time
* Sets velocity_exit_time. Timer will start when velocity is 0.
* \param use_imu
* Adds the Imu for velocity calculation in conjunction with the main sensor.
*/
void pid_swing_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);
void pid_swing_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 = true);

/**
* Returns current tick_per_inch()
Expand Down
34 changes: 32 additions & 2 deletions src/EZ-Template/PID.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,7 @@ void PID::timers_reset() {
k = 0;
j = 0;
l = 0;
m = 0;
is_mA = false;
}

Expand All @@ -113,6 +114,18 @@ void PID::exit_condition_print(ez::exit_output exit_type) {
std::cout << exit_to_string(exit_type) << " Exit.\n";
}

void PID::velocity_sensor_secondary_toggle_set(bool toggle) { use_second_sensor = toggle; }
bool PID::velocity_sensor_secondary_toggle_get() { return use_second_sensor; }

void PID::velocity_sensor_secondary_set(double secondary_sensor) { second_sensor = secondary_sensor; }
double PID::velocity_sensor_secondary_get() { return second_sensor; }

void PID::velocity_sensor_main_exit_set(double zero) { velocity_zero_main = zero; }
double PID::velocity_sensor_main_exit_get() { return velocity_zero_main; }

void PID::velocity_sensor_secondary_exit_set(double zero) { velocity_zero_secondary = zero; }
double PID::velocity_sensor_secondary_exit_get() { return velocity_zero_secondary; }

exit_output PID::exit_condition(bool print) {
// If this function is called while all exit constants are 0, print an error
if (exit.small_error == 0 && exit.small_exit_time == 0 && exit.big_error == 0 && exit.big_exit_time == 0 && exit.velocity_exit_time == 0 && exit.mA_timeout == 0) {
Expand Down Expand Up @@ -150,9 +163,9 @@ exit_output PID::exit_condition(bool print) {
}
}

// If the motor velocity is 0,the code will timeout and set interfered to true.
// If the motor velocity is 0, the code will timeout and set interfered to true.
if (exit.velocity_exit_time != 0) { // Check if this condition is enabled
if (abs(derivative) <= 0.05) {
if (abs(derivative) <= velocity_zero_main) {
k += util::DELAY_TIME;
if (k > exit.velocity_exit_time) {
timers_reset();
Expand All @@ -164,6 +177,23 @@ exit_output PID::exit_condition(bool print) {
}
}

if (!use_second_sensor)
return RUNNING;

// If the secondary sensors velocity is 0, the code will timeout and set interfered to true.
if (exit.velocity_exit_time != 0) { // Check if this condition is enabled
if (abs(second_sensor) <= velocity_zero_secondary) {
m += util::DELAY_TIME;
if (m > exit.velocity_exit_time) {
timers_reset();
if (print) exit_condition_print(VELOCITY_EXIT);
return VELOCITY_EXIT;
}
} else {
m = 0;
}
}

return RUNNING;
}

Expand Down
1 change: 1 addition & 0 deletions src/EZ-Template/drive/drive.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -274,6 +274,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); }
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; }

void Drive::drive_imu_scaler_set(double scaler) { IMU_SCALER = scaler; }
double Drive::drive_imu_scaler_get() { return IMU_SCALER; }
Expand Down
30 changes: 21 additions & 9 deletions src/EZ-Template/drive/exit_conditions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,14 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.

using namespace ez;

void Drive::pid_drive_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) {
void Drive::pid_drive_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) {
leftPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
rightPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
leftPID.velocity_sensor_secondary_toggle_set(use_imu);
rightPID.velocity_sensor_secondary_toggle_set(use_imu);
}

void Drive::pid_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QLength p_small_error, okapi::QTime p_big_exit_time, okapi::QLength p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout) {
void Drive::pid_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QLength p_small_error, okapi::QTime p_big_exit_time, okapi::QLength p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu) {
// Convert okapi units to doubles
double se = p_small_error.convert(okapi::inch);
double be = p_big_error.convert(okapi::inch);
Expand All @@ -23,14 +25,15 @@ void Drive::pid_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi::
int vet = p_velocity_exit_time.convert(okapi::millisecond);
int mAt = p_mA_timeout.convert(okapi::millisecond);

pid_drive_exit_condition_set(set, se, bet, be, vet, mAt);
pid_drive_exit_condition_set(set, se, bet, be, vet, mAt, use_imu);
}

void Drive::pid_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) {
void Drive::pid_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) {
turnPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
turnPID.velocity_sensor_secondary_toggle_set(use_imu);
}

void Drive::pid_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) {
void Drive::pid_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) {
// Convert okapi units to doubles
double se = p_small_error.convert(okapi::degree);
double be = p_big_error.convert(okapi::degree);
Expand All @@ -39,14 +42,15 @@ void Drive::pid_turn_exit_condition_set(okapi::QTime p_small_exit_time, okapi::Q
int vet = p_velocity_exit_time.convert(okapi::millisecond);
int mAt = p_mA_timeout.convert(okapi::millisecond);

pid_turn_exit_condition_set(set, se, bet, be, vet, mAt);
pid_turn_exit_condition_set(set, se, bet, be, vet, mAt, use_imu);
}

void Drive::pid_swing_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) {
void Drive::pid_swing_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) {
swingPID.exit_condition_set(p_small_exit_time, p_small_error, p_big_exit_time, p_big_error, p_velocity_exit_time, p_mA_timeout);
swingPID.velocity_sensor_secondary_toggle_set(use_imu);
}

void Drive::pid_swing_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) {
void Drive::pid_swing_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) {
// Convert okapi units to doubles
double se = p_small_error.convert(okapi::degree);
double be = p_big_error.convert(okapi::degree);
Expand All @@ -55,7 +59,7 @@ void Drive::pid_swing_exit_condition_set(okapi::QTime p_small_exit_time, okapi::
int vet = p_velocity_exit_time.convert(okapi::millisecond);
int mAt = p_mA_timeout.convert(okapi::millisecond);

pid_swing_exit_condition_set(set, se, bet, be, vet, mAt);
pid_swing_exit_condition_set(set, se, bet, be, vet, mAt, use_imu);
}

// User wrapper for exit condition
Expand All @@ -67,6 +71,8 @@ void Drive::pid_wait() {
exit_output left_exit = RUNNING;
exit_output right_exit = RUNNING;
while (left_exit == RUNNING || right_exit == RUNNING) {
leftPID.velocity_sensor_secondary_set(drive_imu_accel_get());
rightPID.velocity_sensor_secondary_set(drive_imu_accel_get());
left_exit = left_exit != RUNNING ? left_exit : leftPID.exit_condition(left_motors[0]);
right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]);
pros::delay(util::DELAY_TIME);
Expand All @@ -82,6 +88,7 @@ void Drive::pid_wait() {
else if (mode == TURN) {
exit_output turn_exit = RUNNING;
while (turn_exit == RUNNING) {
turnPID.velocity_sensor_secondary_set(drive_imu_accel_get());
turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]});
pros::delay(util::DELAY_TIME);
}
Expand All @@ -97,6 +104,7 @@ void Drive::pid_wait() {
exit_output swing_exit = RUNNING;
pros::Motor& sensor = current_swing == ez::LEFT_SWING ? left_motors[0] : right_motors[0];
while (swing_exit == RUNNING) {
swingPID.velocity_sensor_secondary_set(drive_imu_accel_get());
swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor);
pros::delay(util::DELAY_TIME);
}
Expand Down Expand Up @@ -135,6 +143,8 @@ void Drive::wait_until_drive(double target) {
// Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop
if (util::sgn(l_error) == l_sgn || util::sgn(r_error) == r_sgn) {
if (left_exit == RUNNING || right_exit == RUNNING) {
leftPID.velocity_sensor_secondary_set(drive_imu_accel_get());
rightPID.velocity_sensor_secondary_set(drive_imu_accel_get());
left_exit = left_exit != RUNNING ? left_exit : leftPID.exit_condition(left_motors[0]);
right_exit = right_exit != RUNNING ? right_exit : rightPID.exit_condition(right_motors[0]);
pros::delay(util::DELAY_TIME);
Expand Down Expand Up @@ -182,6 +192,7 @@ void Drive::wait_until_turn_swing(double target) {
// Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop
if (util::sgn(g_error) == g_sgn) {
if (turn_exit == RUNNING) {
turnPID.velocity_sensor_secondary_set(drive_imu_accel_get());
turn_exit = turn_exit != RUNNING ? turn_exit : turnPID.exit_condition({left_motors[0], right_motors[0]});
pros::delay(util::DELAY_TIME);
} else {
Expand All @@ -205,6 +216,7 @@ void Drive::wait_until_turn_swing(double target) {
// Before robot has reached target, use the exit conditions to avoid getting stuck in this while loop
if (util::sgn(g_error) == g_sgn) {
if (swing_exit == RUNNING) {
swingPID.velocity_sensor_secondary_set(drive_imu_accel_get());
swing_exit = swing_exit != RUNNING ? swing_exit : swingPID.exit_condition(sensor);
pros::delay(util::DELAY_TIME);
} else {
Expand Down

0 comments on commit 97cc097

Please sign in to comment.