From 97cc0972d01a03bc306ba099e175ab8370a2823b Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Sun, 2 Jun 2024 08:50:02 -0700 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8Imu=20velocity=20exiting=20#102?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * PID class has secondary sensor velocity support, implemented that with imu accel into exit conditions * Made names more clear, added missing functions --- include/EZ-Template/PID.hpp | 58 ++++++++++++++++++++++- include/EZ-Template/drive/drive.hpp | 31 +++++++++--- src/EZ-Template/PID.cpp | 34 ++++++++++++- src/EZ-Template/drive/drive.cpp | 1 + src/EZ-Template/drive/exit_conditions.cpp | 30 ++++++++---- 5 files changed, 135 insertions(+), 19 deletions(-) diff --git a/include/EZ-Template/PID.hpp b/include/EZ-Template/PID.hpp index 06f485d..9d5893f 100644 --- a/include/EZ-Template/PID.hpp +++ b/include/EZ-Template/PID.hpp @@ -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. * @@ -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 \ No newline at end of file diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index 204a3f9..7f4f51e 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -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. * @@ -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. @@ -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. @@ -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. @@ -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. @@ -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. @@ -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() diff --git a/src/EZ-Template/PID.cpp b/src/EZ-Template/PID.cpp index 290cc92..1e37dfd 100644 --- a/src/EZ-Template/PID.cpp +++ b/src/EZ-Template/PID.cpp @@ -97,6 +97,7 @@ void PID::timers_reset() { k = 0; j = 0; l = 0; + m = 0; is_mA = false; } @@ -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) { @@ -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(); @@ -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; } diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp index ee7e658..f4e89b0 100644 --- a/src/EZ-Template/drive/drive.cpp +++ b/src/EZ-Template/drive/drive.cpp @@ -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; } diff --git a/src/EZ-Template/drive/exit_conditions.cpp b/src/EZ-Template/drive/exit_conditions.cpp index 7927272..d59e834 100644 --- a/src/EZ-Template/drive/exit_conditions.cpp +++ b/src/EZ-Template/drive/exit_conditions.cpp @@ -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); @@ -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); @@ -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); @@ -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 @@ -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); @@ -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); } @@ -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); } @@ -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); @@ -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 { @@ -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 {