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

@@ -1,14 +1,15 @@
 #pragma once
 
 #include "VOSS/controller/AbstractController.hpp"
+#include "VOSS/utils/PID.hpp"
 #include <memory>
 
 namespace voss::controller {
 
 class ArcPIDController : public AbstractController {
   protected:
     std::shared_ptr<ArcPIDController> 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<localizer::AbstractLocalizer> l);
 
-    double linear_pid(double error);
-
     chassis::DiffChassisCommand
     get_command(bool reverse, bool thru,
                 std::shared_ptr<AbstractExitCondition> ec) override;

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<localizer::AbstractLocalizer> l);
 
@@ -43,9 +41,6 @@ class BoomerangController : public AbstractController {
                         voss::AngularDirection direction,
                         std::shared_ptr<AbstractExitCondition> ec) override;
 
-    double linear_pid(double error);
-    double angular_pid(double error);
-
     void reset() override;
 
     std::shared_ptr<BoomerangController>

include/VOSS/controller/PIDController.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "VOSS/controller/AbstractController.hpp"
+#include "VOSS/utils/PID.hpp"
 #include <memory>
 
 namespace voss::controller {
@@ -9,23 +10,17 @@ class PIDController : public AbstractController {
   protected:
     std::shared_ptr<PIDController> 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;
 
     double min_vel;
     bool turn_overshoot;
 
-    double prev_lin_err, total_lin_err, prev_ang_err, total_ang_err;
-
   public:
     PIDController(std::shared_ptr<localizer::AbstractLocalizer> l);
 
-    double linear_pid(double error);
-    double angular_pid(double error);
-
     chassis::DiffChassisCommand
     get_command(bool reverse, bool thru,
                 std::shared_ptr<AbstractExitCondition> ec) override;

include/VOSS/controller/SwingController.hpp

@@ -2,17 +2,17 @@
 
 #include "PIDController.hpp"
 #include "VOSS/controller/AbstractController.hpp"
+#include "VOSS/utils/PID.hpp"
 #include <memory>
 
 namespace voss::controller {
 class SwingController : public AbstractController {
   protected:
     std::shared_ptr<SwingController> 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<AbstractExitCondition> ec) override;
 
-    double angular_pid(double error);
-
     void reset() override;
 
     std::shared_ptr<SwingController>

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

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

include/VOSS/exit_conditions/ToleranceExitCondition.hpp

@@ -11,6 +11,7 @@ class ToleranceExitCondition : public AbstractExitCondition {
     std::shared_ptr<ToleranceLinearExitCondition> 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);

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

src/VOSS/controller/ArcPIDController.cpp

@@ -8,7 +8,7 @@ namespace voss::controller {
 
 ArcPIDController::ArcPIDController(
     std::shared_ptr<localizer::AbstractLocalizer> 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;
 }
 

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;
 }
 

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;
 }

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;
 }
 

src/VOSS/controller/PIDController.cpp

@@ -9,8 +9,7 @@
 namespace voss::controller {
 
 PIDController::PIDController(std::shared_ptr<localizer::AbstractLocalizer> 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;
 }