From af5111573ac6e2a5a43a57dd18e7d6bf93ed592e Mon Sep 17 00:00:00 2001
From: Javier Balloffet <javier.balloffet@gmail.com>
Date: Sat, 10 Feb 2024 22:00:32 +0100
Subject: [PATCH] Add PID class tests

Signed-off-by: Javier Balloffet <javier.balloffet@gmail.com>
---
 andino_firmware/platformio.ini                |   1 +
 .../test/desktop/test_pid/pid_test.cpp        | 104 +++++++++++++++++-
 2 files changed, 101 insertions(+), 4 deletions(-)

diff --git a/andino_firmware/platformio.ini b/andino_firmware/platformio.ini
index 6a686645..f3b40bbe 100644
--- a/andino_firmware/platformio.ini
+++ b/andino_firmware/platformio.ini
@@ -31,6 +31,7 @@ test_build_src = true
 build_src_filter =
   +<encoder.cpp>
   +<motor.cpp>
+  +<pid.cpp>
 
 ; Environment for Arduino Uno.
 [env:uno]
diff --git a/andino_firmware/test/desktop/test_pid/pid_test.cpp b/andino_firmware/test/desktop/test_pid/pid_test.cpp
index 72f9e57a..d2bd7c7f 100644
--- a/andino_firmware/test/desktop/test_pid/pid_test.cpp
+++ b/andino_firmware/test/desktop/test_pid/pid_test.cpp
@@ -31,12 +31,108 @@
 
 #include <gtest/gtest.h>
 
-// TODO(jballoffet): Add unit tests.
-TEST(PidTest, ConstructOk) {
-  andino::PID pid_controller(0, 0, 0, 0, 0, 0);
-  EXPECT_EQ(1, 1);
+namespace andino {
+namespace test {
+namespace {
+
+TEST(PidTest, Initialize) {
+  andino::PID pid_controller(1, 0, 0, 1, -100, 100);
+  int output = 0;
+
+  // Controller is disabled by default, so output variable should remain unchanged.
+  pid_controller.compute(5, output);
+  EXPECT_EQ(output, 0);
+}
+
+TEST(PidTest, ComputeOutputProportionalGain) {
+  andino::PID pid_controller(3, 0, 0, 1, -100, 100);
+  int output = 0;
+  pid_controller.set_setpoint(15);
+  pid_controller.enable();
+
+  pid_controller.compute(5, output);
+  EXPECT_EQ(output, 30);
+
+  pid_controller.compute(10, output);
+  EXPECT_EQ(output, 60);
+}
+
+TEST(PidTest, ComputeOutputProportionalAndDerivativeGain) {
+  andino::PID pid_controller(3, 2, 0, 1, -100, 100);
+  int output = 0;
+  pid_controller.set_setpoint(15);
+  pid_controller.enable();
+
+  pid_controller.compute(5, output);
+  EXPECT_EQ(output, 20);
+
+  pid_controller.compute(10, output);
+  EXPECT_EQ(output, 50);
+}
+
+TEST(PidTest, ComputeOutputProportionalAndIntegralGain) {
+  andino::PID pid_controller(3, 0, 1, 1, -100, 100);
+  int output = 0;
+  pid_controller.set_setpoint(15);
+  pid_controller.enable();
+
+  pid_controller.compute(5, output);
+  EXPECT_EQ(output, 30);
+
+  pid_controller.compute(10, output);
+  EXPECT_EQ(output, 70);
 }
 
+TEST(PidTest, ComputeOutputProportionalDerivativeAndIntegralGain) {
+  andino::PID pid_controller(3, 2, 1, 1, -100, 100);
+  int output = 0;
+  pid_controller.set_setpoint(15);
+  pid_controller.enable();
+
+  pid_controller.compute(5, output);
+  EXPECT_EQ(output, 20);
+
+  pid_controller.compute(10, output);
+  EXPECT_EQ(output, 60);
+}
+
+TEST(PidTest, Reset) {
+  andino::PID pid_controller(3, 0, 0, 1, -100, 100);
+  int output = 0;
+  pid_controller.set_setpoint(15);
+  pid_controller.enable();
+
+  pid_controller.compute(5, output);
+  EXPECT_EQ(output, 30);
+
+  // Controller reset, obtained output should be the same as before.
+  pid_controller.reset(0);
+  pid_controller.set_setpoint(15);
+  pid_controller.compute(5, output);
+  EXPECT_EQ(output, 30);
+}
+
+TEST(PidTest, SetTunings) {
+  andino::PID pid_controller(3, 0, 0, 1, -100, 100);
+  int output = 0;
+  pid_controller.set_setpoint(15);
+  pid_controller.enable();
+
+  pid_controller.compute(5, output);
+  EXPECT_EQ(output, 30);
+
+  // Proportional gain doubled, obtained output should be doubled.
+  pid_controller.reset(0);
+  pid_controller.set_setpoint(15);
+  pid_controller.set_tunings(6, 0, 0, 1);
+  pid_controller.compute(5, output);
+  EXPECT_EQ(output, 60);
+}
+
+}  // namespace
+}  // namespace test
+}  // namespace andino
+
 int main(int argc, char** argv) {
   ::testing::InitGoogleTest(&argc, argv);
   if (RUN_ALL_TESTS()) {