From 847075af88638b33ff49cc40fbd27ed45c36269e Mon Sep 17 00:00:00 2001 From: Andrew Lu Date: Tue, 19 Mar 2024 21:26:13 -0400 Subject: [PATCH] Add PID util class (#82) * create PID class * use PID class in controllers, fix issues with exit conditions * Committing clang-format changes --------- Co-authored-by: github-actions <41898282+github-actions[bot]@users.noreply.github.com> --- include/VOSS/controller/ArcPIDController.hpp | 5 +-- .../VOSS/controller/BoomerangController.hpp | 9 +--- include/VOSS/controller/PIDController.hpp | 9 +--- include/VOSS/controller/SwingController.hpp | 6 +-- .../exit_conditions/SettleExitCondition.hpp | 2 +- .../exit_conditions/TimeOutExitCondition.hpp | 2 +- .../ToleranceExitCondition.hpp | 1 + include/VOSS/utils/PID.hpp | 18 ++++++++ src/VOSS/controller/ArcPIDController.cpp | 18 ++------ .../controller/ArcPIDControllerBuilder.cpp | 4 +- src/VOSS/controller/BoomerangController.cpp | 36 +++------------- .../controller/BoomerangControllerBuilder.cpp | 8 +--- src/VOSS/controller/PIDController.cpp | 42 ++++--------------- src/VOSS/controller/PIDControllerBuilder.cpp | 8 +--- src/VOSS/controller/SwingController.cpp | 18 +------- .../controller/SwingControllerBuilder.cpp | 4 +- .../ToleranceAngularExitCondition.cpp | 7 +--- .../ToleranceExitCondition.cpp | 11 +++++ src/VOSS/utils/PID.cpp | 33 +++++++++++++++ src/main.cpp | 1 + 20 files changed, 100 insertions(+), 142 deletions(-) create mode 100644 include/VOSS/utils/PID.hpp create mode 100644 src/VOSS/utils/PID.cpp diff --git a/include/VOSS/controller/ArcPIDController.hpp b/include/VOSS/controller/ArcPIDController.hpp index 94c13e69..724ba2f7 100644 --- a/include/VOSS/controller/ArcPIDController.hpp +++ b/include/VOSS/controller/ArcPIDController.hpp @@ -1,6 +1,7 @@ #pragma once #include "VOSS/controller/AbstractController.hpp" +#include "VOSS/utils/PID.hpp" #include namespace voss::controller { @@ -8,7 +9,7 @@ namespace voss::controller { class ArcPIDController : public AbstractController { protected: std::shared_ptr p; - double linear_kP, linear_kI, linear_kD; + utils::PID linear_pid; double track_width; double min_error; double can_reverse; @@ -23,8 +24,6 @@ class ArcPIDController : public AbstractController { public: ArcPIDController(std::shared_ptr l); - double linear_pid(double error); - chassis::DiffChassisCommand get_command(bool reverse, bool thru, std::shared_ptr ec) override; diff --git a/include/VOSS/controller/BoomerangController.hpp b/include/VOSS/controller/BoomerangController.hpp index 316e692e..2697002e 100644 --- a/include/VOSS/controller/BoomerangController.hpp +++ b/include/VOSS/controller/BoomerangController.hpp @@ -6,6 +6,7 @@ #include "VOSS/exit_conditions/AbstractExitCondition.hpp" #include "VOSS/localizer/AbstractLocalizer.hpp" #include "VOSS/utils/flags.hpp" +#include "VOSS/utils/PID.hpp" namespace voss::controller { @@ -15,8 +16,7 @@ class BoomerangController : public AbstractController { double lead_pct; Pose carrotPoint; - double linear_kP, linear_kI, linear_kD; - double angular_kP, angular_kI, angular_kD; + utils::PID linear_pid, angular_pid; double vel; double exit_error; double angular_exit_error; @@ -30,8 +30,6 @@ class BoomerangController : public AbstractController { double prev_angle; double min_vel; - double prev_lin_err, total_lin_err, prev_ang_err, total_ang_err; - public: BoomerangController(std::shared_ptr l); @@ -43,9 +41,6 @@ class BoomerangController : public AbstractController { voss::AngularDirection direction, std::shared_ptr ec) override; - double linear_pid(double error); - double angular_pid(double error); - void reset() override; std::shared_ptr diff --git a/include/VOSS/controller/PIDController.hpp b/include/VOSS/controller/PIDController.hpp index f92d985d..085ac578 100644 --- a/include/VOSS/controller/PIDController.hpp +++ b/include/VOSS/controller/PIDController.hpp @@ -1,6 +1,7 @@ #pragma once #include "VOSS/controller/AbstractController.hpp" +#include "VOSS/utils/PID.hpp" #include namespace voss::controller { @@ -9,8 +10,7 @@ class PIDController : public AbstractController { protected: std::shared_ptr p; - double linear_kP, linear_kI, linear_kD; - double angular_kP, angular_kI, angular_kD; + utils::PID linear_pid, angular_pid; double tracking_kP; double min_error; bool can_reverse; @@ -18,14 +18,9 @@ class PIDController : public AbstractController { double min_vel; bool turn_overshoot; - double prev_lin_err, total_lin_err, prev_ang_err, total_ang_err; - public: PIDController(std::shared_ptr l); - double linear_pid(double error); - double angular_pid(double error); - chassis::DiffChassisCommand get_command(bool reverse, bool thru, std::shared_ptr ec) override; diff --git a/include/VOSS/controller/SwingController.hpp b/include/VOSS/controller/SwingController.hpp index 2d38c81f..86688a3f 100644 --- a/include/VOSS/controller/SwingController.hpp +++ b/include/VOSS/controller/SwingController.hpp @@ -2,17 +2,17 @@ #include "PIDController.hpp" #include "VOSS/controller/AbstractController.hpp" +#include "VOSS/utils/PID.hpp" #include namespace voss::controller { class SwingController : public AbstractController { protected: std::shared_ptr p; - double angular_kP, angular_kI, angular_kD; + utils::PID angular_pid; bool can_reverse; bool turn_overshoot; - double prev_ang_err, total_ang_err; double prev_ang_speed; public: @@ -26,8 +26,6 @@ class SwingController : public AbstractController { voss::AngularDirection direction, std::shared_ptr ec) override; - double angular_pid(double error); - void reset() override; std::shared_ptr diff --git a/include/VOSS/exit_conditions/SettleExitCondition.hpp b/include/VOSS/exit_conditions/SettleExitCondition.hpp index 561ca6ef..7d90618f 100644 --- a/include/VOSS/exit_conditions/SettleExitCondition.hpp +++ b/include/VOSS/exit_conditions/SettleExitCondition.hpp @@ -16,7 +16,7 @@ class SettleExitCondition : public AbstractExitCondition { public: SettleExitCondition(int settle_time, double tolerance, int initial_delay); - bool is_met(Pose current_pose, bool thru); + bool is_met(Pose current_pose, bool thru) override; void reset() override; }; } // namespace voss::controller \ No newline at end of file diff --git a/include/VOSS/exit_conditions/TimeOutExitCondition.hpp b/include/VOSS/exit_conditions/TimeOutExitCondition.hpp index 4aabe039..d01455a9 100644 --- a/include/VOSS/exit_conditions/TimeOutExitCondition.hpp +++ b/include/VOSS/exit_conditions/TimeOutExitCondition.hpp @@ -12,7 +12,7 @@ class TimeOutExitCondition : public AbstractExitCondition { public: TimeOutExitCondition(int timeout); - bool is_met(Pose current_pose, bool thru); + bool is_met(Pose current_pose, bool thru) override; void reset() override; }; } // namespace voss::controller \ No newline at end of file diff --git a/include/VOSS/exit_conditions/ToleranceExitCondition.hpp b/include/VOSS/exit_conditions/ToleranceExitCondition.hpp index bc253bf3..f50eb5de 100644 --- a/include/VOSS/exit_conditions/ToleranceExitCondition.hpp +++ b/include/VOSS/exit_conditions/ToleranceExitCondition.hpp @@ -11,6 +11,7 @@ class ToleranceExitCondition : public AbstractExitCondition { std::shared_ptr lin_exit = nullptr; public: + void set_target(voss::Pose target) override; bool is_met(Pose pose, bool thru) override; void add_ang_exit(double angular_tolerance); void add_lin_exit(double linear_tolerance); diff --git a/include/VOSS/utils/PID.hpp b/include/VOSS/utils/PID.hpp new file mode 100644 index 00000000..6125889c --- /dev/null +++ b/include/VOSS/utils/PID.hpp @@ -0,0 +1,18 @@ +#pragma once + +namespace voss::utils { + +class PID { + private: + double kP, kI, kD; + double prev_error, total_error; + + public: + PID(); + PID(double kP, double kI, double kD); + double update(double error); + void reset(); + void set_constants(double kP, double kI, double kD); +}; + +} // namespace voss::utils \ No newline at end of file diff --git a/src/VOSS/controller/ArcPIDController.cpp b/src/VOSS/controller/ArcPIDController.cpp index 060f3567..44b5e6c9 100644 --- a/src/VOSS/controller/ArcPIDController.cpp +++ b/src/VOSS/controller/ArcPIDController.cpp @@ -8,7 +8,7 @@ namespace voss::controller { ArcPIDController::ArcPIDController( std::shared_ptr l) - : AbstractController(std::move(l)), prev_lin_err(0.0), total_lin_err(0.0) { + : AbstractController(std::move(l)) { } chassis::DiffChassisCommand @@ -48,7 +48,7 @@ ArcPIDController::get_command(bool reverse, bool thru, angle_error = voss::norm_delta(angle_error); - double lin_speed = thru ? 100.0 : this->linear_pid(distance_error); + double lin_speed = thru ? 100.0 : this->linear_pid.update(distance_error); if (distance_error < this->min_error) { this->can_reverse = true; @@ -96,20 +96,8 @@ chassis::DiffChassisCommand ArcPIDController::get_angular_command( return chassis::DiffChassisCommand{chassis::Stop{}}; } -double ArcPIDController::linear_pid(double error) { - this->total_lin_err += error; - - double speed = linear_kP * error + linear_kD * (error - prev_lin_err) + - linear_kI * total_lin_err; - - this->prev_lin_err = error; - - return speed; -} - void ArcPIDController::reset() { - this->prev_lin_err = 0.0; - this->total_lin_err = 0.0; + this->linear_pid.reset(); this->prev_lin_speed = 0.0; } diff --git a/src/VOSS/controller/ArcPIDControllerBuilder.cpp b/src/VOSS/controller/ArcPIDControllerBuilder.cpp index b2d1cb22..4a87cc6e 100644 --- a/src/VOSS/controller/ArcPIDControllerBuilder.cpp +++ b/src/VOSS/controller/ArcPIDControllerBuilder.cpp @@ -27,9 +27,7 @@ ArcPIDControllerBuilder ArcPIDControllerBuilder::from(ArcPIDController arc) { ArcPIDControllerBuilder& ArcPIDControllerBuilder::with_linear_constants(double kP, double kI, double kD) { - this->ctrl.linear_kP = kP; - this->ctrl.linear_kI = kI; - this->ctrl.linear_kD = kD; + this->ctrl.linear_pid.set_constants(kP, kI, kD); return *this; } diff --git a/src/VOSS/controller/BoomerangController.cpp b/src/VOSS/controller/BoomerangController.cpp index 32f1c608..5fce9642 100644 --- a/src/VOSS/controller/BoomerangController.cpp +++ b/src/VOSS/controller/BoomerangController.cpp @@ -44,7 +44,7 @@ BoomerangController::get_command(bool reverse, bool thru, angle_error = voss::norm_delta(angle_error); - double lin_speed = linear_pid(distance_error); + double lin_speed = linear_pid.update(distance_error); if (thru) { lin_speed = copysign(fmax(fabs(lin_speed), this->min_vel), lin_speed); } @@ -63,7 +63,7 @@ BoomerangController::get_command(bool reverse, bool thru, while (fabs(poseError) > M_PI) poseError -= 2 * M_PI * poseError / fabs(poseError); - ang_speed = angular_pid(poseError); + ang_speed = angular_pid.update(poseError); } // reduce the linear speed if the bot is tangent to the target @@ -75,7 +75,7 @@ BoomerangController::get_command(bool reverse, bool thru, lin_speed = -lin_speed; } - ang_speed = angular_pid(angle_error); + ang_speed = angular_pid.update(angle_error); } lin_speed *= cos(angle_error); @@ -100,35 +100,9 @@ chassis::DiffChassisCommand BoomerangController::get_angular_command( return chassis::DiffChassisCommand{chassis::Stop{}}; } -double BoomerangController::linear_pid(double error) { - total_lin_err += error; - - this->vel = error - prev_ang_err; - - double speed = linear_kP * error + linear_kD * (error - prev_lin_err) + - linear_kI * total_lin_err; - - this->prev_lin_err = error; - - return speed; -} - -double BoomerangController::angular_pid(double error) { - total_ang_err += error; - - double speed = angular_kP * error + angular_kD * (error - prev_ang_err) + - angular_kI * total_ang_err; - - this->prev_ang_err = error; - - return speed; -} - void BoomerangController::reset() { - this->prev_lin_err = 0; - this->total_lin_err = 0; - this->prev_ang_err = 0; - this->total_ang_err = 0; + this->linear_pid.reset(); + this->angular_pid.reset(); this->can_reverse = false; this->counter = 0; } diff --git a/src/VOSS/controller/BoomerangControllerBuilder.cpp b/src/VOSS/controller/BoomerangControllerBuilder.cpp index f30f76b9..4bcc9dc4 100644 --- a/src/VOSS/controller/BoomerangControllerBuilder.cpp +++ b/src/VOSS/controller/BoomerangControllerBuilder.cpp @@ -29,18 +29,14 @@ BoomerangControllerBuilder::from(BoomerangController bmr) { BoomerangControllerBuilder& BoomerangControllerBuilder::with_linear_constants(double kP, double kI, double kD) { - this->ctrl.linear_kP = kP; - this->ctrl.linear_kI = kI; - this->ctrl.linear_kD = kD; + this->ctrl.linear_pid.set_constants(kP, kI, kD); return *this; } BoomerangControllerBuilder& BoomerangControllerBuilder::with_angular_constants(double kP, double kI, double kD) { - this->ctrl.angular_kP = kP; - this->ctrl.angular_kI = kI; - this->ctrl.angular_kD = kD; + this->ctrl.angular_pid.set_constants(kP, kI, kD); return *this; } diff --git a/src/VOSS/controller/PIDController.cpp b/src/VOSS/controller/PIDController.cpp index a8501064..2de02c08 100644 --- a/src/VOSS/controller/PIDController.cpp +++ b/src/VOSS/controller/PIDController.cpp @@ -9,8 +9,7 @@ namespace voss::controller { PIDController::PIDController(std::shared_ptr l) - : AbstractController(std::move(l)), prev_lin_err(0.0), total_lin_err(0.0), - prev_ang_err(0.0), total_ang_err(0.0) { + : AbstractController(std::move(l)) { } chassis::DiffChassisCommand @@ -43,7 +42,8 @@ PIDController::get_command(bool reverse, bool thru, angle_error = voss::norm_delta(angle_error); - double lin_speed = (thru ? 100.0 : (linear_pid(distance_error))) * dir; + double lin_speed = + (thru ? 100.0 : (linear_pid.update(distance_error))) * dir; double ang_speed; if (distance_error < min_error) { @@ -58,7 +58,7 @@ PIDController::get_command(bool reverse, bool thru, while (fabs(poseError) > M_PI) poseError -= 2 * M_PI * poseError / fabs(poseError); - ang_speed = angular_pid(poseError); + ang_speed = angular_pid.update(poseError); } // reduce the linear speed if the bot is tangent to the target @@ -71,7 +71,7 @@ PIDController::get_command(bool reverse, bool thru, lin_speed = -lin_speed; } - ang_speed = angular_pid(angle_error); + ang_speed = angular_pid.update(angle_error); } // Runs at the end of a through movement if (ec->is_met(this->l->get_pose(), thru)) { @@ -129,7 +129,7 @@ PIDController::get_angular_command(bool reverse, bool thru, } } - double ang_speed = angular_pid(angular_error); + double ang_speed = angular_pid.update(angular_error); if (ec->is_met(this->l->get_pose(), thru) || chainedExecutable) { if (thru) { return chassis::DiffChassisCommand{ @@ -140,37 +140,11 @@ PIDController::get_angular_command(bool reverse, bool thru, return chassis::DiffChassisCommand{ chassis::diff_commands::Voltages{-ang_speed, ang_speed}}; } -// What is calculating the required motor power for a linear movement -// Returns value for motor power with type double -double PIDController::linear_pid(double error) { - total_lin_err += error; - - double speed = linear_kP * error + linear_kD * (error - prev_lin_err) + - linear_kI * total_lin_err; - - this->prev_lin_err = error; - - return speed; -} -// What is calculating the required motor power for a turn -// Returns value for motor power with type double -double PIDController::angular_pid(double error) { - total_ang_err += error; - - double speed = angular_kP * error + angular_kD * (error - prev_ang_err) + - angular_kI * total_ang_err; - - this->prev_ang_err = error; - - return speed; -} // Function to reset every variable used in the PID controller void PIDController::reset() { - this->prev_lin_err = 0; - this->total_lin_err = 0; - this->prev_ang_err = 0; - this->total_ang_err = 0; + this->linear_pid.reset(); + this->angular_pid.reset(); this->can_reverse = false; this->turn_overshoot = false; } diff --git a/src/VOSS/controller/PIDControllerBuilder.cpp b/src/VOSS/controller/PIDControllerBuilder.cpp index 341a200a..ba0e3b80 100644 --- a/src/VOSS/controller/PIDControllerBuilder.cpp +++ b/src/VOSS/controller/PIDControllerBuilder.cpp @@ -28,17 +28,13 @@ PIDControllerBuilder PIDControllerBuilder::from(PIDController pid) { PIDControllerBuilder& PIDControllerBuilder::with_linear_constants(double kP, double kI, double kD) { - this->ctrl.linear_kP = kP; - this->ctrl.linear_kI = kI; - this->ctrl.linear_kD = kD; + this->ctrl.linear_pid.set_constants(kP, kI, kD); return *this; } PIDControllerBuilder& PIDControllerBuilder::with_angular_constants(double kP, double kI, double kD) { - this->ctrl.angular_kP = kP; - this->ctrl.angular_kI = kI; - this->ctrl.angular_kD = kD; + this->ctrl.angular_pid.set_constants(kP, kI, kD); return *this; } diff --git a/src/VOSS/controller/SwingController.cpp b/src/VOSS/controller/SwingController.cpp index 3e8cc15c..58c8403a 100644 --- a/src/VOSS/controller/SwingController.cpp +++ b/src/VOSS/controller/SwingController.cpp @@ -57,7 +57,7 @@ chassis::DiffChassisCommand SwingController::get_angular_command( } } - double ang_speed = angular_pid(angular_error); + double ang_speed = angular_pid.update(angular_error); chassis::DiffChassisCommand command; if (!((ang_speed >= 0.0) ^ (this->prev_ang_speed < 0.0)) && this->prev_ang_speed != 0) { @@ -91,24 +91,10 @@ chassis::DiffChassisCommand SwingController::get_angular_command( : chassis::diff_commands::Swing{-ang_speed, 0}; } -// What is calculating the required motor power for the turn -// Returns value for motor power with type double -double SwingController::angular_pid(double error) { - total_ang_err += error; - - double speed = angular_kP * error + angular_kD * (error - prev_ang_err) + - angular_kI * total_ang_err; - - this->prev_ang_err = error; - - return speed; -} - // Resets all the variables used in the swing controller void SwingController::reset() { - this->prev_ang_err = 0; + this->angular_pid.reset(); this->prev_ang_speed = 0; - this->total_ang_err = 0; this->can_reverse = false; this->turn_overshoot = false; } diff --git a/src/VOSS/controller/SwingControllerBuilder.cpp b/src/VOSS/controller/SwingControllerBuilder.cpp index 78d44279..9c46459d 100644 --- a/src/VOSS/controller/SwingControllerBuilder.cpp +++ b/src/VOSS/controller/SwingControllerBuilder.cpp @@ -29,9 +29,7 @@ SwingControllerBuilder SwingControllerBuilder::from(SwingController swc) { SwingControllerBuilder& SwingControllerBuilder::with_angular_constants(double kP, double kI, double kD) { - this->ctrl.angular_kP = kP; - this->ctrl.angular_kI = kI; - this->ctrl.angular_kD = kD; + this->ctrl.angular_pid.set_constants(kP, kI, kD); return *this; } diff --git a/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp b/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp index 1319165b..cd39c74e 100644 --- a/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp +++ b/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp @@ -1,18 +1,15 @@ #include "VOSS/exit_conditions/ToleranceAngularExitCondition.hpp" #include "VOSS/utils/angle.hpp" #include +#include namespace voss::controller { ToleranceAngularExitCondition::ToleranceAngularExitCondition(double tolerance) - : tolerance(tolerance) { + : tolerance(voss::to_radians(tolerance)) { } bool ToleranceAngularExitCondition::is_met(Pose current_pose, bool thru) { - // if there is no target angle, return false so the other conditions run. - if (!this->target_pose.theta.has_value()) { - return false; - } return std::abs(voss::norm_delta(current_pose.theta.value() - this->target_pose.theta.value())) < this->tolerance; diff --git a/src/VOSS/exit_conditions/ToleranceExitCondition.cpp b/src/VOSS/exit_conditions/ToleranceExitCondition.cpp index 9b2a67f4..996232a9 100644 --- a/src/VOSS/exit_conditions/ToleranceExitCondition.cpp +++ b/src/VOSS/exit_conditions/ToleranceExitCondition.cpp @@ -13,6 +13,17 @@ bool ToleranceExitCondition::is_met(Pose pose, bool thru) { } return false; } + +void ToleranceExitCondition::set_target(Pose target) { + AbstractExitCondition::set_target(target); + if (this->ang_exit) { + this->ang_exit->set_target(target); + } + if (this->lin_exit) { + this->lin_exit->set_target(target); + } +} + void ToleranceExitCondition::add_ang_exit(double angular_tolerance) { this->ang_exit = std::make_shared(angular_tolerance); diff --git a/src/VOSS/utils/PID.cpp b/src/VOSS/utils/PID.cpp new file mode 100644 index 00000000..61bb1cb7 --- /dev/null +++ b/src/VOSS/utils/PID.cpp @@ -0,0 +1,33 @@ +#include "VOSS/utils/PID.hpp" + +namespace voss::utils { + +PID::PID() : PID(0.0, 0.0, 0.0) { +} + +PID::PID(double kP, double kI, double kD) + : kP(kP), kI(kI), kD(kD), prev_error(0.0), total_error(0.0) { +} + +double PID::update(double error) { + total_error += error; + + double output = kP * error + kI * total_error + kD * (error - prev_error); + + prev_error = error; + + return output; +} + +void PID::reset() { + total_error = 0.0; + prev_error = 0.0; +} + +void PID::set_constants(double kP, double kI, double kD) { + this->kP = kP; + this->kI = kI; + this->kD = kD; +} + +} // namespace voss::utils \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 31f511ad..a6320a78 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -160,6 +160,7 @@ void opcontrol() { printf("5.\n"); master.rumble("--"); chassis.turn(0); + printf("end\n"); } pros::lcd::clear_line(1);