added uts

tests/controllers/control_laws/pid_first_order_test.cpp

@@ -11,30 +11,32 @@ TEST(PID, SimpleModel){
 
   double position = 0;
   double velocity = 0;
+  double expected_position = 5;
+  double precision = 0.1;
 
   // 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);
+    double u = pid.update(position, expected_position);
 
     position += (velocity + u) * param.kDt;
     velocity += 0.1 * param.kDt;
   }
 
   // Position should be near setpoint.
-  EXPECT_NEAR(position, 5, 0.1);
+  EXPECT_NEAR(position, expected_position, precision);
 
   pid = PID(param);
   position = 0;
   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);
+    double u = pid.update(position, -expected_position);
 
     position += (velocity + u) * param.kDt;
     velocity += 0.1 * param.kDt;
   }
 
-  EXPECT_NEAR(position, -5, 0.1);
+  EXPECT_NEAR(position, -expected_position, precision);
 }
 
 TEST(PID, ClampU) {
@@ -44,14 +46,17 @@ TEST(PID, ClampU) {
   param.kUMin = -100;
   param.enable_ramp_rate_limit = false;
 
+  double vel = 0;
+  double vel_d = 1;
+
   PID pid(param);
-  EXPECT_LE(pid.updateSaturated(0, 1), 100);
-  EXPECT_GE(pid.updateSaturated(0, -1), -100);
+  EXPECT_LE(pid.updateSaturated(vel, vel_d), 100);
+  EXPECT_GE(pid.updateSaturated(vel, -vel_d), -100);
 
   PIDParameters no_negative_param = param;
   no_negative_param.kUMin = 0;
   PID no_negative_pid(no_negative_param);
-  EXPECT_GE(no_negative_pid.updateSaturated(0, -1), 0);
+  EXPECT_GE(no_negative_pid.updateSaturated(vel, -vel_d), 0);
 }
 
 TEST(PID, RampRateLimit){
@@ -61,12 +66,18 @@ TEST(PID, RampRateLimit){
   param.ramp_rate = 1;
   param.kDt = 0.1;
 
-  PID pid(param);
+  double velocity = 0;
+  double velocity_d = 10;
+  double rate = 0.1;
+  double precision = 0.01;
 
+  PID pid(param);
   // U should only be allowed to rise by 0.1
-  EXPECT_NEAR(pid.update(0, 10), 0.1, 0.01);
+  EXPECT_NEAR(pid.update(velocity, velocity_d), rate, precision);
 
   pid = PID(param);
+  velocity_d = 0.05;
+  rate = 0.05;
   // U should rise to any value less than 0.1
-  EXPECT_NEAR(pid.update(0, 0.05), 0.05, 0.01);
+  EXPECT_NEAR(pid.update(velocity, velocity_d), rate, precision);
 }

tests/controllers/feedback_linearization/fblin_pid_test.cpp

@@ -0,0 +1,76 @@
+#include <gtest/gtest.h>
+#include "controllers/feedback_linearization/PID/first_order/fblin_pid.hpp"
+
+TEST(PIDLin, SimpleModel){
+
+  PIDParameters params;
+  params.kP = 10;
+  params.kI = 10;
+  params.kD = 0.1;
+  params.kDt = 0.01;
+
+  double FB_LIN_UMAX = 255;
+  double FB_LIN_UMIN = -255;
+  double g_x = 1;
+  double accel_d = 1;
+
+  double position = 0;
+  double velocity = 0;
+
+  double position_d = 5;
+  double distance = 0.1;
+
+  PIDLin pid(FB_LIN_UMAX, FB_LIN_UMIN, params);
+
+  pid.g_x_ = g_x;
+
+  // u_ = g_x_^(-1)*(chi1_dot_d - f_x_ + u_n - u_aux)
+  // Run a simple simulation, check that setpoint is reached with full PID impl.
+  for(int i = 0; i < 100; i++){
+    double u = pid.calculateManipulations(position, position_d, accel_d);
+    position += (velocity + u) * params.kDt;
+    velocity += 0.1 * params.kDt;
+  }
+
+  // Position should be near setpoint.
+  EXPECT_NEAR(position, position_d, distance);
+
+  pid = PIDLin(FB_LIN_UMAX, FB_LIN_UMIN, params);
+  position = 0;
+  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.calculateManipulations(position, -position_d, accel_d);
+    position -= (velocity + u) * params.kDt;
+    velocity += 0.1 * params.kDt;
+  }
+
+  EXPECT_NEAR(position, -position_d, distance);
+}
+
+TEST(PIDLin, ClampU) {
+
+  PIDParameters params;
+  params.kP = 100;
+  params.kDt = 0.01;
+
+  double FB_LIN_UMAX = 10;
+  double FB_LIN_UMIN = -10;
+  double g_x = 1;
+  double accel_d = 1;
+
+  double vel = 0;
+  double vel_d = 5;
+  double distance = 0.1;
+
+  PIDLin pid(FB_LIN_UMAX, FB_LIN_UMIN, params);
+
+  EXPECT_LE(pid.calculateManipulations(vel, vel_d, accel_d), FB_LIN_UMAX);
+  EXPECT_GE(pid.calculateManipulations(vel, -vel_d, accel_d), FB_LIN_UMIN);
+
+  FB_LIN_UMIN = 0;
+  pid = PIDLin(FB_LIN_UMAX, FB_LIN_UMIN, params);
+
+  EXPECT_GE(pid.calculateManipulations(vel, -vel_d, accel_d), FB_LIN_UMIN);
+}

tests/controllers/feedback_linearization/fblin_test.cpp

@@ -0,0 +1,51 @@
+#include <gtest/gtest.h>
+#include "controllers/feedback_linearization/base/fb_lin_control.hpp"
+
+TEST(FBLin, SimpleModel){
+  double f_x{5};
+  double g_x{1};
+
+  double U_MAX{255};
+  double U_MIN{-255};
+  double u_aux{10};
+  double u_n{2};
+  double chiX_dot_d{1};
+
+  FBLin controller(U_MAX, U_MIN);
+  controller.f_x_ = f_x;
+  controller.g_x_ = g_x;
+
+  controller.u_n_ = u_n;
+  controller.u_aux_ = u_aux;
+  controller.chiX_dot_d_ = chiX_dot_d;
+
+  controller.updateControlSignal();
+  // u_ = g_x_^(-1)*(chi1_dot_d - f_x_ + u_n - u_aux)
+  double expected_u = -12;
+  EXPECT_EQ(controller.u_, expected_u);
+}
+
+TEST(FBLin, ClampU){
+  double f_x{5};
+  double g_x{1};
+
+  double U_MAX{255};
+  double U_MIN{-255};
+  double u_aux{300};
+  double u_n{2};
+  double chiX_dot_d{1};
+
+  FBLin controller(U_MAX, U_MIN);
+  controller.f_x_ = f_x;
+  controller.g_x_ = g_x;
+
+  controller.u_n_ = u_n;
+  controller.u_aux_ = u_aux;
+  controller.chiX_dot_d_ = chiX_dot_d;
+
+  controller.updateControlSignal();
+  // u_ = g_x_^(-1)*(chi1_dot_d - f_x_ + u_n - u_aux)
+
+  EXPECT_LE(controller.u_, U_MAX);
+  EXPECT_GE(controller.u_, U_MIN);
+}

tests/controllers/feedback_linearization/vtec_sdc1_pid_test.cpp

@@ -0,0 +1,51 @@
+// #include <gtest/gtest.h>
+// #include "controllers/feedback_linearization/base/fb_lin_control.hpp"
+
+// TEST(FBLin, SimpleModel){
+//   double f_x{5};
+//   double g_x{1};
+
+//   double U_MAX{255};
+//   double U_MIN{-255};
+//   double u_aux{10};
+//   double u_n{2};
+//   double chiX_dot_d{1};
+
+//   FBLin controller(U_MAX, U_MIN);
+//   controller.f_x_ = f_x;
+//   controller.g_x_ = g_x;
+
+//   controller.u_n_ = u_n;
+//   controller.u_aux_ = u_aux;
+//   controller.chiX_dot_d_ = chiX_dot_d;
+
+//   controller.updateControlSignal();
+//   // u_ = g_x_^(-1)*(chi1_dot_d - f_x_ + u_n - u_aux)
+
+//   EXPECT_EQ(controller.u_, -12);
+// }
+
+// TEST(FBLin, ClampU){
+//   double f_x{5};
+//   double g_x{1};
+
+//   double U_MAX{255};
+//   double U_MIN{-255};
+//   double u_aux{300};
+//   double u_n{2};
+//   double chiX_dot_d{1};
+
+//   FBLin controller(U_MAX, U_MIN);
+//   controller.f_x_ = f_x;
+//   controller.g_x_ = g_x;
+
+//   controller.u_n_ = u_n;
+//   controller.u_aux_ = u_aux;
+//   controller.chiX_dot_d_ = chiX_dot_d;
+
+//   controller.updateControlSignal();
+//   // u_ = g_x_^(-1)*(chi1_dot_d - f_x_ + u_n - u_aux)
+
+//   EXPECT_LE(controller.u_, U_MAX);
+//   EXPECT_GE(controller.u_, U_MIN);
+// }