test: Split test cases

test/test_main.cpp

-#include <mbed.h>
 #include <unity.h>
 
-#include <cmath>
-
-#include "regulator/PID.hpp"
-#include "odometry/Odometer.hpp"
-#include "motor/LeftMotor.hpp"
-#include "motor/RightMotor.hpp"
+#include "unit_tests/test_encoders.hpp"
+#include "unit_tests/test_motors.hpp"
+#include "unit_tests/test_pid.hpp"
+#include "unit_tests/test_odometer.hpp"
 
 void wait_for_enter()
 {
-    int c;
+    int c = -1;
 
-    while ((c = getchar()) != '\n' && c != EOF)
+    do
     {
-        continue;
-    }
-}
-
-DigitalOut led1(LED1);
-
-void test_led_builtin_pin_number()
-{
-    TEST_ASSERT_EQUAL(0x13, LED1);
-}
-
-void test_led_state_high()
-{
-    led1.write(1);
-    wait_ms(50);
-    TEST_ASSERT_EQUAL(1, led1.read());
-}
-
-void test_led_state_low()
-{
-    led1.write(0);
-    wait_ms(50);
-    TEST_ASSERT_EQUAL(0, led1.read());
-}
-
-void test_pid_compute_all_Kp_Ki_Kd_1()
-{
-    PID pid({1, 1, 1}, -5, 5);
-    pid.setSetpoint(1);
-    pid.setProcessValue(0);
-    // error = sp - pv = 1-0
-    // last_error = error (as it is not set, we copy the error for this iteration)
-    // out = kp * (1-0) + ki * (1-0) * 0.01 + kd * (last_error - error) / dt = 1 * 1 + 1 * 1 * 0.01 = 1.01
-    TEST_ASSERT_EQUAL_DOUBLE(1.01, pid.compute(0.01));
-}
-
-void test_pid_compute_proportional_Kp1()
-{
-    PID pid({1, 0, 0}, -5, 5);
-    pid.setSetpoint(1);
-    pid.setProcessValue(0);
-    // error = sp - pv = 1-0
-    // out = kp * error = 1 * 1
-    TEST_ASSERT_EQUAL_DOUBLE(1, pid.compute(0.01));
-
-    pid.setProcessValue(1.2);
-    // error = sp - pv = 1-1.2 = -0.2
-    // out = kp * error = 1 * -0.2 = -0.2
-    TEST_ASSERT_EQUAL_DOUBLE(-0.2, pid.compute(0.01));
-}
-
-void test_pid_cumpute_proportional_Kp2()
-{
-    PID pid({2, 0, 0}, -5, 5);
-    pid.setSetpoint(1);
-    pid.setProcessValue(0);
-    // error = sp - pv = 1-0
-    // out = kp * error = 2 * 1
-    TEST_ASSERT_EQUAL_DOUBLE(2, pid.compute(0.01));
-}
-
-void test_pid_cumpute_integral_Ki_1()
-{
-    PID pid({0, 1, 0}, -1, 1);
-    pid.setSetpoint(1);
-    pid.setProcessValue(0);
-    // error = sp - pv = 1-0
-    // accum = accum + error * 0.01 = 0 + 1 * 0.01 = 0.01
-    // out = ki * accum = 1 * 0.01
-    TEST_ASSERT_EQUAL_DOUBLE(0.01, pid.compute(0.01));
-
-    // error = sp - pv = 1-0
-    // accum = accum + error * 0.01 = 0.01 + 1 * 0.01
-    // out = ki * accum = 1 * 0.02
-    TEST_ASSERT_EQUAL_DOUBLE(0.02, pid.compute(0.01));
-
-    // error = sp - pv = 1-0
-    // accum = accum + error * 0.01 = 0.02 + 1 * 0.01
-    // out = ki * accum = 1 * 0.03
-    TEST_ASSERT_EQUAL_DOUBLE(0.03, pid.compute(0.01));
-}
-
-void test_pid_cumpute_integral_Ki_2()
-{
-    PID pid({0, 2, 0}, -5, 5);
-    pid.setSetpoint(1);
-    pid.setProcessValue(0);
-    // error = sp - pv = 1-0
-    // accum = accum + error * 0.01 = 0 + 1 * 0.01 = 0.01
-    // out = ki * accum = 2 * 0.01
-    TEST_ASSERT_EQUAL_DOUBLE(0.02, pid.compute(0.01));
-
-    // error = sp - pv = 1-0
-    // accum = accum + error * 0.01 = 0.01 + 1 * 0.01 = 0.02
-    // out = ki * accum = 2 * 0.02
-    TEST_ASSERT_EQUAL_DOUBLE(0.04, pid.compute(0.01));
-
-    // error = sp - pv = 1-0
-    // accum = accum + error * 0.01 = 0.02 + 1 * 0.01 = 0.03
-    // out = ki * accum = 2 * 0.03
-    TEST_ASSERT_EQUAL_DOUBLE(0.06, pid.compute(0.01));
-}
-
-void test_pid_compute_derivative_division_by_zero()
-{
-    PID pid({0, 0, 1}, -1, 1);
-    pid.setSetpoint(1);
-    pid.setProcessValue(0);
-
-    // error = sp - pv = 1-0
-    // last_error = error (as it is not set, we copy the error for this iteration)
-    // out = ki * accum = 1 * 0.02
-    TEST_ASSERT_EQUAL_DOUBLE(0, pid.compute(0));
-}
-
-void test_odometer_init_values()
-{
-    const Odom::Geometry geometry = {
-        0.062, // Nominal wheel diameter
-        0.342  // Nominal wheelbase
-    };
-
-    Encoder leftEncoder(D7, D8, 1024);
-    Encoder rightEncoder(D10, D12, 1000);
-    Odometer odom(geometry, leftEncoder, rightEncoder);
-
-    // When creating the odometer, position should be all zeroes, same for speed
-    TEST_ASSERT_EQUAL(0, odom.getPosition().x);
-    TEST_ASSERT_EQUAL(0, odom.getPosition().y);
-    TEST_ASSERT_EQUAL(0, odom.getPosition().theta);
-    TEST_ASSERT_EQUAL(0, odom.getSpeed().linearSpeed);
-    TEST_ASSERT_EQUAL(0, odom.getSpeed().angularSpeed);
-    TEST_ASSERT_EQUAL(0, odom.getSpeed().leftWheelSpeed);
-    TEST_ASSERT_EQUAL(0, odom.getSpeed().rightWheelSpeed);
-}
-
-void test_encoder_left_resolution_X4()
-{
-    Encoder leftEncoder(D7, D8, 1024);
-    TEST_ASSERT_EQUAL(1024 * 4, leftEncoder.getResolution());
-}
-
-void test_encoder_left_still()
-{
-    Encoder leftEncoder(D7, D8, 1024);
-
-    TEST_ASSERT_EQUAL(0, leftEncoder.getTicks());
-    wait_ms(50);
-    TEST_ASSERT_EQUAL(0, leftEncoder.getTicks());
-}
-
-void test_encoder_left_forward()
-{
-    Encoder leftEncoder(D7, D8, 1024);
-    printf(">> Turn left encoder forward and press [ENTER] to continue.\n");
-    wait_for_enter();
-    TEST_ASSERT_GREATER_OR_EQUAL(1, leftEncoder.getTicks());
-}
-
-void test_encoder_left_backward()
-{
-    Encoder leftEncoder(D7, D8, 1024);
-    printf(">> Turn left encoder backward and press [ENTER] to continue.\n");
-    wait_for_enter();
-    TEST_ASSERT_LESS_OR_EQUAL(-1, leftEncoder.getTicks());
-}
-
-void test_encoder_right_resolution_X4()
-{
-    Encoder rightEncoder(D10, D12, 1000);
-    TEST_ASSERT_EQUAL(1000 * 4, rightEncoder.getResolution());
-}
-
-void test_encoder_right_still()
-{
-    Encoder rightEncoder(D10, D12, 1000);
-
-    TEST_ASSERT_EQUAL(0, rightEncoder.getTicks());
-    wait_ms(50);
-    TEST_ASSERT_EQUAL(0, rightEncoder.getTicks());
-}
-
-void test_encoder_right_backward()
-{
-    Encoder rightEncoder(D10, D12, 1000);
-    printf(">> Turn right encoder backward and press [ENTER] when done.\n");
-    wait_for_enter();
-    TEST_ASSERT_LESS_OR_EQUAL(-1, rightEncoder.getTicks());
-}
-
-void test_encoder_right_forward()
-{
-    Encoder rightEncoder(D10, D12, 1000);
-    printf(">> Turn right encoder forward and press [ENTER] when done.\n");
-    wait_for_enter();
-    TEST_ASSERT_GREATER_OR_EQUAL(1, rightEncoder.getTicks());
-}
-
-void test_left_motor_init()
-{
-    LeftMotor leftMotor;
-    TEST_ASSERT_TRUE(leftMotor.init());
-}
-
-void test_right_motor_init()
-{
-    RightMotor rightMotor;
-    TEST_ASSERT_TRUE(rightMotor.init());
-}
+        if (c != '\r') // Prevent '\r\n' to display the message twice
+        {
+            printf("\t[>] Press 'enter' to continue.\n");
+        }
 
-void test_left_motor_speed_0()
-{
-    LeftMotor leftMotor;
-    TEST_ASSERT_TRUE(leftMotor.init());
-    TEST_MESSAGE("Left motor should hold position for 5sec.");
-    leftMotor.enablePwm();
-    leftMotor.setSpeed(0);
-    wait_ms(5000);
-    leftMotor.disablePwm();
-    TEST_PASS();
-}
+        c = getchar();
 
-void test_right_motor_speed_0()
-{
-    RightMotor rightMotor;
-    TEST_ASSERT_TRUE(rightMotor.init());
-    TEST_MESSAGE("Right motor should hold position for 5sec.");
-    rightMotor.enablePwm();
-    rightMotor.setSpeed(0);
-    wait_ms(5000);
-    rightMotor.disablePwm();
-    TEST_PASS();
+    } while (c != '\n' && c != EOF);
 }
 
-void test_left_motor_fullspeed_forward()
+int wait_for_yes_no()
 {
-    LeftMotor leftMotor;
-    TEST_ASSERT_TRUE(leftMotor.init());
-    TEST_MESSAGE("Left motor should go fullspeed forward for 2sec.");
-    leftMotor.enablePwm();
-    leftMotor.setSpeed(1);
-    wait_ms(2000);
-    leftMotor.disablePwm();
-    TEST_PASS();
-}
 
-void test_right_motor_fullspeed_forward()
-{
-    RightMotor rightMotor;
-    TEST_ASSERT_TRUE(rightMotor.init());
-    TEST_MESSAGE("Right motor should go fullspeed forward for 2sec.");
-    rightMotor.enablePwm();
-    rightMotor.setSpeed(1);
-    wait_ms(2000);
-    rightMotor.disablePwm();
-    TEST_PASS();
-}
+    int c;
 
-void test_left_motor_fullspeed_backward()
-{
-    LeftMotor leftMotor;
-    TEST_ASSERT_TRUE(leftMotor.init());
-    TEST_MESSAGE("Left motor should go fullspeed backward for 2sec.");
-    leftMotor.enablePwm();
-    leftMotor.setSpeed(-1);
-    wait_ms(2000);
-    leftMotor.disablePwm();
-    TEST_PASS();
-}
+    do
+    {
+        printf("\t[>] Press 'y' or 'n'.\n");
+        c = getchar();
+    } while (c != 'y' && c != 'n');
 
-void test_right_motor_fullspeed_backward()
-{
-    RightMotor rightMotor;
-    TEST_ASSERT_TRUE(rightMotor.init());
-    TEST_MESSAGE("Right motor should go fullspeed backward for 2sec.");
-    rightMotor.enablePwm();
-    rightMotor.setSpeed(-1);
-    wait_ms(2000);
-    rightMotor.disablePwm();
-    TEST_PASS();
+    return c;
 }
 
 int main()
@@ -303,36 +42,11 @@ int main()
 
     UNITY_BEGIN();
 
-    RUN_TEST(test_led_builtin_pin_number);
-    RUN_TEST(test_led_state_high);
-    RUN_TEST(test_led_state_low);
-
-    RUN_TEST(test_pid_compute_proportional_Kp1);
-    RUN_TEST(test_pid_cumpute_proportional_Kp2);
-    RUN_TEST(test_pid_cumpute_integral_Ki_1);
-    RUN_TEST(test_pid_cumpute_integral_Ki_2);
-    RUN_TEST(test_pid_compute_derivative_division_by_zero);
-    RUN_TEST(test_pid_compute_all_Kp_Ki_Kd_1);
-
-    RUN_TEST(test_odometer_init_values);
-
-    RUN_TEST(test_encoder_left_resolution_X4);
-    RUN_TEST(test_encoder_right_resolution_X4);
-    RUN_TEST(test_encoder_left_still);
-    RUN_TEST(test_encoder_right_still);
-    RUN_TEST(test_encoder_left_forward);
-    RUN_TEST(test_encoder_left_backward);
-    RUN_TEST(test_encoder_right_forward);
-    RUN_TEST(test_encoder_right_backward);
+    //encoders_tests::run_all();
+    motors_tests::run_all();
 
-    RUN_TEST(test_left_motor_init);
-    RUN_TEST(test_right_motor_init);
-    RUN_TEST(test_left_motor_speed_0);
-    RUN_TEST(test_right_motor_speed_0);
-    RUN_TEST(test_left_motor_fullspeed_forward);
-    RUN_TEST(test_right_motor_fullspeed_forward);
-    RUN_TEST(test_left_motor_fullspeed_backward);
-    RUN_TEST(test_right_motor_fullspeed_backward);
+    pid_tests::run_all();
+    odometer_tests::run_all();
 
     UNITY_END();
 }
\ No newline at end of file

test/unit_tests/test_encoders.hpp

+#ifndef TEST_ENCODERS_HPP
+#define TEST_ENCODERS_HPP
+
+#include <unity.h>
+#include <mbed.h>
+#include "Encoder.hpp"
+
+// Provided by test_main.cpp
+void wait_for_enter();
+int wait_for_yes_no();
+
+namespace encoders_tests
+{
+
+    void test_encoder_left_resolution_X4()
+    {
+        Encoder leftEncoder(D7, D8, 1024);
+        TEST_ASSERT_EQUAL(1024 * 4, leftEncoder.getResolution());
+    }
+
+    void test_encoder_left_still()
+    {
+        Encoder leftEncoder(D7, D8, 1024);
+
+        TEST_ASSERT_EQUAL(0, leftEncoder.getTicks());
+        wait_ms(50);
+        TEST_ASSERT_EQUAL(0, leftEncoder.getTicks());
+    }
+
+    void test_encoder_left_forward()
+    {
+        Encoder leftEncoder(D7, D8, 1024);
+        printf("[*] Turn left encoder forward.\n");
+        wait_for_enter();
+        TEST_ASSERT_GREATER_OR_EQUAL(1, leftEncoder.getTicks());
+    }
+
+    void test_encoder_left_backward()
+    {
+        Encoder leftEncoder(D7, D8, 1024);
+        printf("[*] Turn left encoder backward.\n");
+        wait_for_enter();
+        TEST_ASSERT_LESS_OR_EQUAL(-1, leftEncoder.getTicks());
+    }
+
+    void test_encoder_right_resolution_X4()
+    {
+        Encoder rightEncoder(D10, D12, 1000);
+        TEST_ASSERT_EQUAL(1000 * 4, rightEncoder.getResolution());
+    }
+
+    void test_encoder_right_still()
+    {
+        Encoder rightEncoder(D10, D12, 1000);
+
+        TEST_ASSERT_EQUAL(0, rightEncoder.getTicks());
+        wait_ms(50);
+        TEST_ASSERT_EQUAL(0, rightEncoder.getTicks());
+    }
+
+    void test_encoder_right_backward()
+    {
+        Encoder rightEncoder(D10, D12, 1000);
+        printf("[*] Turn right encoder backward.\n");
+        wait_for_enter();
+        TEST_ASSERT_LESS_OR_EQUAL(-1, rightEncoder.getTicks());
+    }
+
+    void test_encoder_right_forward()
+    {
+        Encoder rightEncoder(D10, D12, 1000);
+        printf("[*] Turn right encoder forward.\n");
+        wait_for_enter();
+        TEST_ASSERT_GREATER_OR_EQUAL(1, rightEncoder.getTicks());
+    }
+
+    void run_all()
+    {
+        printf("\n*===============================*\n\tTesting encoders\n*===============================*\n\n");
+
+        RUN_TEST(test_encoder_left_resolution_X4);
+        RUN_TEST(test_encoder_right_resolution_X4);
+        RUN_TEST(test_encoder_left_still);
+        RUN_TEST(test_encoder_right_still);
+        RUN_TEST(test_encoder_left_forward);
+        RUN_TEST(test_encoder_left_backward);
+        RUN_TEST(test_encoder_right_forward);
+        RUN_TEST(test_encoder_right_backward);
+    }
+
+}
+
+#endif
\ No newline at end of file

test/unit_tests/test_motors.hpp

+#ifndef TEST_MOTORS_HPP
+#define TEST_MOTORS_HPP
+
+#include <unity.h>
+#include <mbed.h>
+#include "motor/LeftMotor.hpp"
+#include "motor/RightMotor.hpp"
+
+// Provided by test_main.cpp
+void wait_for_enter();
+int wait_for_yes_no();
+
+namespace motors_tests
+{
+
+    void test_left_motor_init()
+    {
+        LeftMotor leftMotor;
+        TEST_ASSERT_TRUE(leftMotor.init());
+    }
+
+    void test_right_motor_init()
+    {
+        RightMotor rightMotor;
+        TEST_ASSERT_TRUE(rightMotor.init());
+    }
+
+    void test_left_motor_speed_0()
+    {
+        LeftMotor leftMotor;
+        TEST_ASSERT_TRUE(leftMotor.init());
+        leftMotor.enablePwm();
+        leftMotor.setSpeed(0);
+        printf("[?] Is the left motor holding position ? (y/n)\n");
+        TEST_ASSERT_EQUAL_CHAR('y', wait_for_yes_no());
+        leftMotor.disablePwm();
+        TEST_PASS();
+    }
+
+    void test_right_motor_speed_0()
+    {
+        RightMotor rightMotor;
+        TEST_ASSERT_TRUE(rightMotor.init());
+        rightMotor.enablePwm();
+        rightMotor.setSpeed(0);
+        printf("[?] Is the right motor holding position ? (y/n)\n");
+        TEST_ASSERT_EQUAL_CHAR('y', wait_for_yes_no());
+        rightMotor.disablePwm();
+        TEST_PASS();
+    }
+
+    void test_left_motor_fullspeed_forward()
+    {
+        LeftMotor leftMotor;
+        TEST_ASSERT_TRUE(leftMotor.init());
+        leftMotor.enablePwm();
+        leftMotor.setSpeed(1);
+        printf("[?] Is the left motor going fullspeed forward ? (y/n)\n");
+        TEST_ASSERT_EQUAL_CHAR('y', wait_for_yes_no());
+        leftMotor.disablePwm();
+        TEST_PASS();
+    }
+
+    void test_right_motor_fullspeed_forward()
+    {
+        RightMotor rightMotor;
+        TEST_ASSERT_TRUE(rightMotor.init());
+        rightMotor.enablePwm();
+        rightMotor.setSpeed(1);
+        printf("[?] Is the right motor going fullspeed forward ? (y/n)\n");
+        TEST_ASSERT_EQUAL_CHAR('y', wait_for_yes_no());
+        rightMotor.disablePwm();
+        TEST_PASS();
+    }
+
+    void test_left_motor_fullspeed_backward()
+    {
+        LeftMotor leftMotor;
+        TEST_ASSERT_TRUE(leftMotor.init());
+        leftMotor.enablePwm();
+        leftMotor.setSpeed(-1);
+        printf("[?] Is the left motor going fullspeed backward ? (y/n)\n");
+        TEST_ASSERT_EQUAL_CHAR('y', wait_for_yes_no());
+        leftMotor.disablePwm();
+        TEST_PASS();
+    }
+
+    void test_right_motor_fullspeed_backward()
+    {
+        RightMotor rightMotor;
+        TEST_ASSERT_TRUE(rightMotor.init());
+        rightMotor.enablePwm();
+        rightMotor.setSpeed(-1);
+        printf("[?] Is the right motor going fullspeed backward ? (y/n)\n");
+        TEST_ASSERT_EQUAL_CHAR('y', wait_for_yes_no());
+        rightMotor.disablePwm();
+        TEST_PASS();
+    }
+
+    void run_all()
+    {
+        printf("\n*===============================*\n\tTesting motors\n*===============================*\n\n");
+        printf("[!] Make sure that power is ON and the wheels don't touch the floor.\n");
+        wait_for_enter();
+
+        RUN_TEST(test_left_motor_init);
+        RUN_TEST(test_right_motor_init);
+        RUN_TEST(test_left_motor_speed_0);
+        RUN_TEST(test_right_motor_speed_0);
+        RUN_TEST(test_left_motor_fullspeed_forward);
+        wait_ms(1000); // Wait for PWM to properly shut down
+        RUN_TEST(test_left_motor_fullspeed_backward);
+        wait_ms(1000);
+        RUN_TEST(test_right_motor_fullspeed_forward);
+        wait_ms(1000);
+        RUN_TEST(test_right_motor_fullspeed_backward);
+    }
+
+}
+
+#endif
\ No newline at end of file

test/unit_tests/test_odometer.hpp

+#ifndef TEST_ODOMETER_HPP
+#define TEST_ODOMETER_HPP
+
+#include <unity.h>
+#include <mbed.h>
+#include "odometry/Odometer.hpp"
+
+// Provided by test_main.cpp
+void wait_for_enter();
+int wait_for_yes_no();
+
+namespace odometer_tests
+{
+
+    void test_odometer_init_values()
+    {
+        const Odom::Geometry geometry = {
+            0.062, // Nominal wheel diameter
+            0.342  // Nominal wheelbase
+        };
+
+        //TODO(cjaverliat): Mock encoders
+        Encoder leftEncoder(D7, D8, 1024);
+        Encoder rightEncoder(D10, D12, 1000);
+        Odometer odom(geometry, leftEncoder, rightEncoder);
+
+        // When creating the odometer, position should be all zeroes, same for speed
+        TEST_ASSERT_EQUAL(0, odom.getPosition().x);
+        TEST_ASSERT_EQUAL(0, odom.getPosition().y);
+        TEST_ASSERT_EQUAL(0, odom.getPosition().theta);
+        TEST_ASSERT_EQUAL(0, odom.getSpeed().linearSpeed);
+        TEST_ASSERT_EQUAL(0, odom.getSpeed().angularSpeed);
+        TEST_ASSERT_EQUAL(0, odom.getSpeed().leftWheelSpeed);
+        TEST_ASSERT_EQUAL(0, odom.getSpeed().rightWheelSpeed);
+    }
+
+    void run_all()
+    {
+        printf("\n*===============================*\n\tTesting odometer\n*===============================*\n\n");
+
+        RUN_TEST(test_odometer_init_values);
+    }
+
+}
+
+#endif
\ No newline at end of file

test/unit_tests/test_pid.hpp

+#ifndef TEST_PID_HPP
+#define TEST_PID_HPP
+
+#include <unity.h>
+#include <mbed.h>
+#include "regulator/SpeedRegulator.hpp"
+
+// Provided by test_main.cpp
+void wait_for_enter();
+int wait_for_yes_no();
+
+namespace pid_tests
+{
+
+    void test_pid_compute_all_Kp_Ki_Kd_1()
+    {
+        PID pid({1, 1, 1}, -5, 5);
+        pid.setSetpoint(1);
+        pid.setProcessValue(0);
+        // error = sp - pv = 1-0
+        // last_error = error (as it is not set, we copy the error for this iteration)
+        // out = kp * (1-0) + ki * (1-0) * 0.01 + kd * (last_error - error) / dt = 1 * 1 + 1 * 1 * 0.01 = 1.01
+        TEST_ASSERT_EQUAL_DOUBLE(1.01, pid.compute(0.01));
+    }
+
+    void test_pid_compute_proportional_Kp1()
+    {
+        PID pid({1, 0, 0}, -5, 5);
+        pid.setSetpoint(1);
+        pid.setProcessValue(0);
+        // error = sp - pv = 1-0
+        // out = kp * error = 1 * 1
+        TEST_ASSERT_EQUAL_DOUBLE(1, pid.compute(0.01));
+
+        pid.setProcessValue(1.2);
+        // error = sp - pv = 1-1.2 = -0.2
+        // out = kp * error = 1 * -0.2 = -0.2
+        TEST_ASSERT_EQUAL_DOUBLE(-0.2, pid.compute(0.01));
+    }
+
+    void test_pid_cumpute_proportional_Kp2()
+    {
+        PID pid({2, 0, 0}, -5, 5);
+        pid.setSetpoint(1);
+        pid.setProcessValue(0);
+        // error = sp - pv = 1-0
+        // out = kp * error = 2 * 1
+        TEST_ASSERT_EQUAL_DOUBLE(2, pid.compute(0.01));
+    }
+
+    void test_pid_cumpute_integral_Ki_1()
+    {
+        PID pid({0, 1, 0}, -1, 1);
+        pid.setSetpoint(1);
+        pid.setProcessValue(0);
+        // error = sp - pv = 1-0
+        // accum = accum + error * 0.01 = 0 + 1 * 0.01 = 0.01
+        // out = ki * accum = 1 * 0.01
+        TEST_ASSERT_EQUAL_DOUBLE(0.01, pid.compute(0.01));
+
+        // error = sp - pv = 1-0
+        // accum = accum + error * 0.01 = 0.01 + 1 * 0.01
+        // out = ki * accum = 1 * 0.02
+        TEST_ASSERT_EQUAL_DOUBLE(0.02, pid.compute(0.01));
+
+        // error = sp - pv = 1-0
+        // accum = accum + error * 0.01 = 0.02 + 1 * 0.01
+        // out = ki * accum = 1 * 0.03
+        TEST_ASSERT_EQUAL_DOUBLE(0.03, pid.compute(0.01));
+    }
+
+    void test_pid_cumpute_integral_Ki_2()
+    {
+        PID pid({0, 2, 0}, -5, 5);
+        pid.setSetpoint(1);
+        pid.setProcessValue(0);
+        // error = sp - pv = 1-0
+        // accum = accum + error * 0.01 = 0 + 1 * 0.01 = 0.01
+        // out = ki * accum = 2 * 0.01
+        TEST_ASSERT_EQUAL_DOUBLE(0.02, pid.compute(0.01));
+
+        // error = sp - pv = 1-0
+        // accum = accum + error * 0.01 = 0.01 + 1 * 0.01 = 0.02
+        // out = ki * accum = 2 * 0.02
+        TEST_ASSERT_EQUAL_DOUBLE(0.04, pid.compute(0.01));
+
+        // error = sp - pv = 1-0
+        // accum = accum + error * 0.01 = 0.02 + 1 * 0.01 = 0.03
+        // out = ki * accum = 2 * 0.03
+        TEST_ASSERT_EQUAL_DOUBLE(0.06, pid.compute(0.01));
+    }
+
+    void test_pid_compute_derivative_division_by_zero()
+    {
+        PID pid({0, 0, 1}, -1, 1);
+        pid.setSetpoint(1);
+        pid.setProcessValue(0);
+
+        // error = sp - pv = 1-0
+        // last_error = error (as it is not set, we copy the error for this iteration)
+        // out = ki * accum = 1 * 0.02
+        TEST_ASSERT_EQUAL_DOUBLE(0, pid.compute(0));
+    }
+
+    void run_all()
+    {
+        printf("\n*===============================*\n\tTesting PID\n*===============================*\n\n");
+
+        RUN_TEST(test_pid_compute_proportional_Kp1);
+        RUN_TEST(test_pid_cumpute_proportional_Kp2);
+        RUN_TEST(test_pid_cumpute_integral_Ki_1);
+        RUN_TEST(test_pid_cumpute_integral_Ki_2);
+        RUN_TEST(test_pid_compute_derivative_division_by_zero);
+        RUN_TEST(test_pid_compute_all_Kp_Ki_Kd_1);
+    }
+
+}
+
+#endif
\ No newline at end of file