Issue 14-2: Rewritten PIDReguator.cpp. Unit tests updated

Author: taketook34

src/kpi_rover_ecu/include/PIDRegulator.h

@@ -4,30 +4,19 @@
 #pragma once
 
 #include <array>
-#include <chrono>
-
-#define SECONDSTOMINUTE 60
-#define MILISECONDSTOSECOND 1000
 
 class PIDRegulator {
    public:
-    // PIDRegulator(vector<float> _coeficients);
-    void Init(std::array<float, 3> _coeficients, int _loopsTicks, int _maxRPM);
-    int Run(int setpoint, int ticksValue);
+    void Init(std::array<float, 3> _coeficients);
+    int Run(float kError, float kTimeDt);
 
    private:
-    float kp_;
-    float ki_;
-    float kd_;
-    int previousError_ = 0;
-    int integral_ = 0;
-    const int kintegralLimit_ = 26500;
-    const int koutputLimit_ = 26500;
-    const int kSpeedIndexMultipler_ = 100;
-    int loopsTicks_;
-    int maxRPM_;
-
-    std::chrono::time_point<std::chrono::high_resolution_clock> lastTimePoint_;
+    float kp_ = 0;
+    float ki_ = 0;
+    float kd_ = 0;
+    float previousError_ = 0;
+    float integral_ = 0;
+    const float kIntegralLimit = 26500;
 };
 
 #endif

src/kpi_rover_ecu/include/motor.h

@@ -2,14 +2,19 @@
 #define MOTOR_H
 
 #include <array>
+#include <chrono>
 
 #include "PIDRegulator.h"
 
+#define SECONDSTOMINUTE 60
+#define MILISECONDSTOSECOND 1000
+
 class Motor {
    public:
     static constexpr int kMinRpm = 8000;
     static constexpr int kMaxRpm = 26500;
     static constexpr int kLoopTicks = 821;
+    static constexpr int kSpeedIndexMultipler = 100;
 
     Motor(int assigned_number, bool is_inverted, std::array<float, 3> _coeficients);
     int MotorGo(int newRPM);
@@ -21,7 +26,10 @@ class Motor {
     static constexpr int kBrakeTime = 100000;  // 100 ms
 
     PIDRegulator pidRegulator_;
-    int currentRpm_;
+    int setpointRpm_;
+    int actualRpm_;
+    std::chrono::time_point<std::chrono::high_resolution_clock> lastTimePoint_;
+
     int motorNumber_;
     bool inverted_;
     double currentDutyCycle_;

src/kpi_rover_ecu/src/PIDRegulator.cpp

@@ -1,50 +1,28 @@
 #include "PIDRegulator.h"
 
 #include <array>
-#include <chrono>
 #include <cmath>
-#include <iostream>
-#include <vector>
 
-void PIDRegulator::Init(std::array<float, 3> _coeficients, int _loopsTicks, int _maxRPM) {
+void PIDRegulator::Init(std::array<float, 3> _coeficients) {
     kp_ = _coeficients[0];
     ki_ = _coeficients[1];
     kd_ = _coeficients[2];
-
-    loopsTicks_ = _loopsTicks;
-    maxRPM_ = _maxRPM;
-    lastTimePoint_ = std::chrono::high_resolution_clock::now();
-    // cout << "PID regulator initialized" << '\n';
 }
 
-int PIDRegulator::Run(int setpoint, int ticks_value) {
-    const auto kCurrentTimePoint = std::chrono::high_resolution_clock::now();
-    const std::chrono::duration<double, std::milli> kElapsedMilliseconds = kCurrentTimePoint - lastTimePoint_;
-    lastTimePoint_ = kCurrentTimePoint;
-    const auto kTimeDt = static_cast<float>(kElapsedMilliseconds.count());
-
-    const float kRevolutions = static_cast<float>(ticks_value) / static_cast<float>(loopsTicks_);
-
-    const float kInputPoint =
-        static_cast<float>(std::round((kRevolutions * SECONDSTOMINUTE * MILISECONDSTOSECOND) / kTimeDt)) *
-        kSpeedIndexMultipler_;
-
-    const float kError = static_cast<float>(setpoint) - kInputPoint;
-    std::cout << "set point " << setpoint << " current point " << kInputPoint << " error " << kError << '\n';
-
+int PIDRegulator::Run(float kError, float kTimeDt) {
     const float kPTerm = kp_ * kError;
 
     integral_ += kError * kTimeDt;
-    if (integral_ > kintegralLimit_) {
-        integral_ = kintegralLimit_;
+    if (integral_ > kIntegralLimit) {
+        integral_ = kIntegralLimit;
     }
-    if (integral_ < -kintegralLimit_) {
-        integral_ = -kintegralLimit_;
+    if (integral_ < -kIntegralLimit) {
+        integral_ = -kIntegralLimit;
     }
 
     const float kITerm = ki_ * integral_;
 
-    const float kDerivativePart = (kTimeDt > 0) ? (kError - previousError_) / kTimeDt : 0.0F;
+    const float kDerivativePart = (kTimeDt > 0.0F) ? (kError - previousError_) / kTimeDt : 0.0F;
     const float kDTerm = kd_ * kDerivativePart;
 
     previousError_ = kError;

src/kpi_rover_ecu/src/motor.cpp

@@ -3,13 +3,17 @@
 #include <rc/encoder.h>
 #include <rc/motor.h>
 
+#include <array>
+#include <chrono>
+#include <cmath>
 #include <iostream>
 
 #include "PIDRegulator.h"
 
 Motor::Motor(int assigned_number, bool is_inverted, std::array<float, 3> _coeficients)
-    : motorNumber_(assigned_number), inverted_(is_inverted), currentDutyCycle_(0), currentRpm_(0) {
-    pidRegulator_.Init(_coeficients, kLoopTicks, kMaxRpm);
+    : motorNumber_(assigned_number), inverted_(is_inverted), currentDutyCycle_(0), setpointRpm_(0), actualRpm_(0) {
+    pidRegulator_.Init(_coeficients);
+    lastTimePoint_ = std::chrono::high_resolution_clock::now();
 }
 
 int Motor::MotorGo(int newRPM) {
@@ -27,7 +31,7 @@ int Motor::MotorGo(int newRPM) {
         return -1;
     }
 
-    currentRpm_ = newRPM;
+    setpointRpm_ = newRPM;
     return 0;
 }
 
@@ -61,10 +65,24 @@ int Motor::GetEncoderCounter() {
         pid_encoder_ticks *= -1;
     }
 
-    const int kPidOutput = pidRegulator_.Run(currentRpm_, pid_encoder_ticks);
+    const auto kCurrentTimePoint = std::chrono::high_resolution_clock::now();
+    const std::chrono::duration<double, std::milli> kElapsedMilliseconds = kCurrentTimePoint - lastTimePoint_;
+    lastTimePoint_ = kCurrentTimePoint;
+    const auto kTimeDt = static_cast<float>(kElapsedMilliseconds.count());
+
+    const float kRevolutions = static_cast<float>(pid_encoder_ticks) / static_cast<float>(kLoopTicks);
+    const float kInputPoint =
+        static_cast<float>(std::round((kRevolutions * SECONDSTOMINUTE * MILISECONDSTOSECOND) / kTimeDt)) *
+        kSpeedIndexMultipler;
+    actualRpm_ = static_cast<int>(kInputPoint);
+    const float kError = static_cast<float>(setpointRpm_) - kInputPoint;
+
+    std::cout << "set point " << setpointRpm_ << " current point " << kInputPoint << " error " << kError << '\n';
+
+    const int kPidOutput = pidRegulator_.Run(kError, kTimeDt);
     rc_encoder_write(motorNumber_, 0);
 
-    MotorSet(currentRpm_ + kPidOutput);
+    MotorSet(setpointRpm_ + kPidOutput);
     return kEncoderTicks;
 }
 

src/kpi_rover_ecu/tests/test_motor.cpp

@@ -38,7 +38,7 @@ TEST_F(MotorTest, MotorGoCallsRcMotorSet) {
     // Expect rc_motor_set to be called with channel 0 and duty cycle 256/1000.0
     EXPECT_CALL(GetMockRCMotor(), set(0, ::testing::DoubleEq(1))).WillOnce(::testing::Return(0));
 
-    motor = new Motor(0, false);
+    motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
     int result = motor->MotorGo(Motor::kMaxRpm);
 
     EXPECT_EQ(0, result) << "MotorGo should return success";
@@ -48,7 +48,7 @@ TEST_F(MotorTest, MotorGoCallsRcMotorSet) {
 TEST_F(MotorTest, MotorGoMinRPM) {
     EXPECT_CALL(GetMockRCMotor(), set(0, ::testing::_)).WillOnce(::testing::Return(0));
 
-    motor = new Motor(0, false);
+    motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
     int result = motor->MotorGo(Motor::kMinRpm);
 
     EXPECT_EQ(0, result);
@@ -59,7 +59,7 @@ TEST_F(MotorTest, MotorGoInverted) {
     // For inverted motor, the duty cycle should be negative
     EXPECT_CALL(GetMockRCMotor(), set(0, ::testing::Lt(0))).WillOnce(::testing::Return(0));
 
-    motor = new Motor(0, true);
+    motor = new Motor(0, true, {1.5f, 0.056f, 1.5f});
     int result = motor->MotorGo(Motor::kMaxRpm);
 
     EXPECT_EQ(0, result);
@@ -70,7 +70,7 @@ TEST_F(MotorTest, MotorGoBelowMinRPM) {
     // Verify behavior with RPM below minimum (should set to MIN_RPM or handle specially)
     EXPECT_CALL(GetMockRCMotor(), set(0, ::testing::_)).WillOnce(::testing::Return(0));
 
-    motor = new Motor(0, false);
+    motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
     int result = motor->MotorGo(Motor::kMinRpm - 1);
 
     EXPECT_EQ(0, result);
@@ -81,7 +81,7 @@ TEST_F(MotorTest, MotorGoAboveMaxRPM) {
     // Verify behavior with RPM above maximum (should clamp to MAX_RPM)
     EXPECT_CALL(GetMockRCMotor(), set(0, ::testing::DoubleEq(1))).WillOnce(::testing::Return(0));
 
-    motor = new Motor(0, false);
+    motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
     int result = motor->MotorGo(Motor::kMaxRpm + 1);
 
     EXPECT_EQ(0, result);
@@ -91,7 +91,7 @@ TEST_F(MotorTest, MotorGoAboveMaxRPM) {
 TEST_F(MotorTest, MotorGoSetError) {
     EXPECT_CALL(GetMockRCMotor(), set(0, ::testing::_)).WillOnce(::testing::Return(-1));
 
-    motor = new Motor(0, false);
+    motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
     int result = motor->MotorGo(Motor::kMaxRpm);
 
     EXPECT_NE(0, result) << "MotorGo should return error";
@@ -102,7 +102,7 @@ TEST_F(MotorTest, MotorStop) {
     // Stopping motor should set duty cycle to 0
     EXPECT_CALL(GetMockRCMotor(), brake(0)).WillOnce(::testing::Return(0));
 
-    motor = new Motor(0, false);
+    motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
     int result = motor->MotorStop();
 
     EXPECT_EQ(0, result) << "MotorStop should return success";
@@ -112,7 +112,7 @@ TEST_F(MotorTest, MotorStop) {
 TEST_F(MotorTest, MotorStopError) {
     EXPECT_CALL(GetMockRCMotor(), brake(0)).WillOnce(::testing::Return(-1));
 
-    motor = new Motor(0, false);
+    motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
     int result = motor->MotorStop();
 
     EXPECT_NE(0, result) << "MotorStop should return error";
@@ -122,7 +122,7 @@ TEST_F(MotorTest, MotorStopError) {
 TEST_F(MotorTest, GetEncoderCounter) {
     EXPECT_CALL(GetMockRCEncoder(), read(0)).WillOnce(::testing::Return(42));
 
-    motor = new Motor(0, false);
+    motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
     int result = motor->GetEncoderCounter();
 
     EXPECT_EQ(42, result) << "GetEncoderCounter should return encoder value";
@@ -132,7 +132,7 @@ TEST_F(MotorTest, GetEncoderCounter) {
 TEST_F(MotorTest, GetEncoderCounterError) {
     EXPECT_CALL(GetMockRCEncoder(), read(0)).WillOnce(::testing::Return(-1));
 
-    motor = new Motor(0, false);
+    motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
     int result = motor->GetEncoderCounter();
 
     EXPECT_EQ(-1, result) << "GetEncoderCounter should return error";

src/kpi_rover_ecu/tests/test_protocol.cpp

@@ -48,8 +48,8 @@ class ProtocolTest : public ::testing::Test {
         EXPECT_CALL(GetMockRCEncoder(), init()).WillOnce(::testing::Return(0));
 
         // Initialize with the same configuration as in main.cpp
-        std::vector<MotorConfig> chassis_config = {MotorConfig(1, false), MotorConfig(2, false), MotorConfig(3, true),
-                                                   MotorConfig(4, true)};
+        std::vector<MotorConfig> chassis_config = {MotorConfig(1, false, {1.5f, 0.056f, 1.5f}), MotorConfig(2, false, {1.5f, 0.056f, 1.5f}), MotorConfig(3, true, {1.5f, 0.056f, 1.5f}),
+                                                   MotorConfig(4, true, {1.5f, 0.056f, 1.5f})};
 
         motors_processor.Init(chassis_config, kMotorNumber);
         protocol_handler = new ProtocolHanlder(&motors_processor);