From 24bf2d5ffacf6312f5fbea18883232698c66f462 Mon Sep 17 00:00:00 2001
From: romi2002 <empoleom@gmail.com>
Date: Wed, 3 Apr 2024 10:48:01 -0600
Subject: [PATCH] Added tests for PID

---
 .gitignore                                    |  4 +-
 .../control_laws/PID/first_order/pid.cpp      |  8 ++-
 .../control_laws/PID/first_order/pid.hpp      |  6 +--
 tests/controllers/control_laws/pid_test.cpp   | 54 +++++++++++++++++--
 4 files changed, 62 insertions(+), 10 deletions(-)

diff --git a/.gitignore b/.gitignore
index 07ed706..5421419 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,3 @@
-build/*
\ No newline at end of file
+build/*
+cmake-build-debug/
+.idea/
\ No newline at end of file
diff --git a/src/controllers/control_laws/PID/first_order/pid.cpp b/src/controllers/control_laws/PID/first_order/pid.cpp
index c412fab..814b89b 100644
--- a/src/controllers/control_laws/PID/first_order/pid.cpp
+++ b/src/controllers/control_laws/PID/first_order/pid.cpp
@@ -25,11 +25,15 @@ double PID::update(double measurement, double desired) {
 
   // If ramp rate is disabled, or if we are within ramp rate, go to U.
   if (!params_.enable_ramp_rate_limit ||
-      std::abs((set_u_ - u) / params_.kDt) < params_.ramp_rate) {
+      std::abs((set_u_ - u) * params_.kDt) < params_.ramp_rate) {
         set_u_ = u;
   } else {
     // Ramp rate is enabled, and we can only increase by ramp rate.
-    set_u_ += std::copysign(params_.ramp_rate, u - set_u_);
+    set_u_ += std::copysign(params_.ramp_rate * params_.kDt, u - set_u_);
+  }
+
+  if(set_u_ < 0){
+    u = 0;
   }
 
   return std::clamp(set_u_, params_.kUMin, params_.kUMax);
diff --git a/src/controllers/control_laws/PID/first_order/pid.hpp b/src/controllers/control_laws/PID/first_order/pid.hpp
index 65c0a0a..6cb4576 100644
--- a/src/controllers/control_laws/PID/first_order/pid.hpp
+++ b/src/controllers/control_laws/PID/first_order/pid.hpp
@@ -16,11 +16,11 @@ struct PIDParameters {
     double kP{0}, kI{0}, kD{0};
     double kDt{0.01};
     
-    double kUMax{std::numeric_limits<double>::max()};
-    double kUMin{std::numeric_limits<double>::min()};
+    double kUMax{1e9};
+    double kUMin{-1e9};
 
     bool enable_ramp_rate_limit{false};
-    double ramp_rate{1}; // 
+    double ramp_rate{1}; // units / second
 };
 
 class PID {
diff --git a/tests/controllers/control_laws/pid_test.cpp b/tests/controllers/control_laws/pid_test.cpp
index cfffe00..08d8d8a 100644
--- a/tests/controllers/control_laws/pid_test.cpp
+++ b/tests/controllers/control_laws/pid_test.cpp
@@ -1,8 +1,54 @@
 #include <gtest/gtest.h>
 #include "controllers/control_laws/PID/first_order/pid.hpp"
 
-class SimpleModel {
-  double update(double u){
-    
+TEST(PID, SimpleModel){
+  PIDParameters param;
+  param.kP = 10;
+  param.kI = 1;
+  param.kD = 0.1;
+  param.kDt = 0.01;
+  PID pid(param);
+
+  double position = 0;
+  double velocity = 0;
+
+  // Run a simple simulation, check that setpoint is reached with full PID impl.
+  for(int i = 0; i < 1000; i++){
+    double u = pid.update(position, 5);
+
+    position += (velocity + u) * param.kDt;
+    velocity += 0.1 * param.kDt;
   }
-};
\ No newline at end of file
+
+  // Position should be near setpoint.
+  EXPECT_NEAR(position, 5, 0.1);
+}
+
+TEST(PID, ClampU) {
+  PIDParameters param;
+  param.kP = 1000;
+  param.kUMax = 100;
+  param.kUMin = -100;
+  param.enable_ramp_rate_limit = false;
+
+  PID pid(param);
+  EXPECT_LE(pid.update(0, 1), 100);
+  EXPECT_GE(pid.update(0, -1), -100);
+}
+
+TEST(PID, RampRateLimit){
+  PIDParameters param;
+  param.kP = 1;
+  param.enable_ramp_rate_limit = true;
+  param.ramp_rate = 1;
+  param.kDt = 0.1;
+
+  PID pid(param);
+
+  // U should only be allowed to rise by 0.1
+  EXPECT_NEAR(pid.update(0, 10), 0.1, 0.01);
+
+  pid = PID(param);
+  // U should rise to any value less than 0.1
+  EXPECT_NEAR(pid.update(0, 0.05), 0.05, 0.01);
+}
\ No newline at end of file