Added tests for PID

.gitignore

@@ -1 +1,3 @@
-build/*
+build/*
+cmake-build-debug/
+.idea/

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

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 {

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;
   }
-};
+
+  // 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);
+}