Repository: KPI-Rover/ecu_sw_bb
Pull request #20: Issue #14. Adding PID regulator to main
Author: login: taketook34
Created: 2025-04-01T13:47:44Z | Merged: 2025-04-08T07:50:04Z

Good afternoon @AksonovSergei 
I rewrite PID regulator in 14-2-resolve-pid-regulator . I even check and fix code style errors in my changes. 


Diff:
diff --git a/src/kpi_rover_ecu/include/ITransport.h b/src/kpi_rover_ecu/include/ITransport.h
index 9214dda..3e30e04 100644
--- a/src/kpi_rover_ecu/include/ITransport.h
+++ b/src/kpi_rover_ecu/include/ITransport.h
@@ -22,4 +22,4 @@ class ITransport {
     ITransport& operator=(ITransport&&) = delete;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/kpi_rover_ecu/include/KPIRoverECU.h b/src/kpi_rover_ecu/include/KPIRoverECU.h
index c5bfb36..2b7b18b 100644
--- a/src/kpi_rover_ecu/include/KPIRoverECU.h
+++ b/src/kpi_rover_ecu/include/KPIRoverECU.h
@@ -35,4 +35,4 @@ class KPIRoverECU {
     static int GetCounter();
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/kpi_rover_ecu/include/PIDRegulator.h b/src/kpi_rover_ecu/include/PIDRegulator.h
new file mode 100644
index 0000000..590f975
--- /dev/null
+++ b/src/kpi_rover_ecu/include/PIDRegulator.h
@@ -0,0 +1,22 @@
+#ifndef PIDREGULATOR_H
+#define PIDREGULATOR_H
+
+#pragma once
+
+#include <array>
+
+class PIDRegulator {
+   public:
+    void Init(std::array<float, 3> _coeficients);
+    int Run(float kError, float kTimeDt);
+
+   private:
+    float kp_ = 0;
+    float ki_ = 0;
+    float kd_ = 0;
+    float previousError_ = 0;
+    float integral_ = 0;
+    float integralLimit_ = 26500;
+};
+
+#endif
diff --git a/src/kpi_rover_ecu/include/TCPTransport.h b/src/kpi_rover_ecu/include/TCPTransport.h
index a927033..23ab2c5 100644
--- a/src/kpi_rover_ecu/include/TCPTransport.h
+++ b/src/kpi_rover_ecu/include/TCPTransport.h
@@ -37,4 +37,4 @@ class TCPTransport : public ITransport {
     MessageQueue messageQueue_;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/kpi_rover_ecu/include/messageQueue.h b/src/kpi_rover_ecu/include/messageQueue.h
index c0ec9eb..bb7dbfd 100644
--- a/src/kpi_rover_ecu/include/messageQueue.h
+++ b/src/kpi_rover_ecu/include/messageQueue.h
@@ -22,4 +22,4 @@ class MessageQueue {
     pthread_cond_t cv_ = PTHREAD_COND_INITIALIZER;       // NOLINT(misc-include-cleaner)
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/kpi_rover_ecu/include/motor.h b/src/kpi_rover_ecu/include/motor.h
index fdedb03..ebb80ab 100644
--- a/src/kpi_rover_ecu/include/motor.h
+++ b/src/kpi_rover_ecu/include/motor.h
@@ -1,18 +1,33 @@
 #ifndef MOTOR_H
 #define MOTOR_H
 
+#include <array>
+#include <chrono>
+
+#include "PIDRegulator.h"
+
 class Motor {
    public:
-    static constexpr int kMinRpm = 8000;
     static constexpr int kMaxRpm = 26500;
+    static constexpr int kLoopTicks = 821;
+    static constexpr int kSpeedIndexMultipler = 100;
+    static constexpr int kSecondsMinute = 60;
+    static constexpr int kMiliSecondsSeconds = 1000;
 
-    Motor(int assigned_number, bool is_inverted);
+    Motor(int assigned_number, bool is_inverted, std::array<float, 3> _coeficients);
     int MotorGo(int newRPM);
     int MotorStop() const;
-    int GetEncoderCounter() const;
+    int GetEncoderCounter();
 
    private:
-    static constexpr int kBrakeTime = 100000;  // 100 ms
+    float GetActualRpm(int _ticks, float _timeSegment);
+    float GetTimeSegment();
+    int MotorSet(int inputRPM);
+
+    PIDRegulator pidRegulator_;
+    int setpointRpm_;
+    int actualRpm_;
+    std::chrono::time_point<std::chrono::high_resolution_clock> lastTimePoint_;
 
     int motorNumber_;
     bool inverted_;
@@ -20,4 +35,4 @@ class Motor {
     double GetDC(int entryRPM);
 };
 
-#endif  // MOTOR_H
\ No newline at end of file
+#endif  // MOTOR_H
diff --git a/src/kpi_rover_ecu/include/motorConfig.h b/src/kpi_rover_ecu/include/motorConfig.h
index da96d09..d382afe 100644
--- a/src/kpi_rover_ecu/include/motorConfig.h
+++ b/src/kpi_rover_ecu/include/motorConfig.h
@@ -1,12 +1,14 @@
 #ifndef MOTOR_CONFIG_H
 #define MOTOR_CONFIG_H
 
+#include <array>
 #include <cstdint>
 
 class MotorConfig {
    public:
-    MotorConfig(uint8_t assignedNumber, bool invertedStatus);
+    MotorConfig(uint8_t assignedNumber, bool invertedStatus, const std::array<float, 3>& _coefs);
 
+    std::array<float, 3> GetPidCoefs() const;
     uint8_t GetNumber() const;
 
     bool IsInverted() const;
@@ -14,6 +16,7 @@ class MotorConfig {
    private:
     uint8_t number_;
     bool inverted_;
+    std::array<float, 3> pidCoefs_ = {};
 };
 
-#endif  // MOTOR_CONFIG_H
\ No newline at end of file
+#endif  // MOTOR_CONFIG_H
diff --git a/src/kpi_rover_ecu/include/motorsController.h b/src/kpi_rover_ecu/include/motorsController.h
index af57ce0..dbc2842 100644
--- a/src/kpi_rover_ecu/include/motorsController.h
+++ b/src/kpi_rover_ecu/include/motorsController.h
@@ -22,4 +22,4 @@ class MotorController {
     std::vector<Motor> motors_;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/kpi_rover_ecu/include/protocolHandler.h b/src/kpi_rover_ecu/include/protocolHandler.h
index ab09d23..8eef1ea 100644
--- a/src/kpi_rover_ecu/include/protocolHandler.h
+++ b/src/kpi_rover_ecu/include/protocolHandler.h
@@ -30,4 +30,4 @@ class ProtocolHanlder {
     std::vector<uint8_t> HandleGetAllEncoders(const std::vector<uint8_t>& message) const;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/kpi_rover_ecu/src/CMakeLists.txt b/src/kpi_rover_ecu/src/CMakeLists.txt
index 07a5cc0..e7837d4 100644
--- a/src/kpi_rover_ecu/src/CMakeLists.txt
+++ b/src/kpi_rover_ecu/src/CMakeLists.txt
@@ -6,6 +6,7 @@ add_library(kpi_rover_ecu_core STATIC
     TCPTransport.cpp
     motorConfig.cpp
     messageQueue.cpp
+    PIDRegulator.cpp
 )
 
 # Include public headers
diff --git a/src/kpi_rover_ecu/src/PIDRegulator.cpp b/src/kpi_rover_ecu/src/PIDRegulator.cpp
new file mode 100644
index 0000000..39726d7
--- /dev/null
+++ b/src/kpi_rover_ecu/src/PIDRegulator.cpp
@@ -0,0 +1,31 @@
+#include "PIDRegulator.h"
+
+#include <array>
+#include <cmath>
+
+void PIDRegulator::Init(std::array<float, 3> _coeficients) {
+    kp_ = _coeficients[0];
+    ki_ = _coeficients[1];
+    kd_ = _coeficients[2];
+}
+
+int PIDRegulator::Run(float kError, float kTimeDt) {
+    const float kPTerm = kp_ * kError;
+
+    integral_ += kError * kTimeDt;
+    if (integral_ > integralLimit_) {
+        integral_ = integralLimit_;
+    }
+    if (integral_ < -integralLimit_) {
+        integral_ = -integralLimit_;
+    }
+
+    const float kITerm = ki_ * integral_;
+
+    const float kDerivativePart = (kTimeDt > 0.0F) ? (kError - previousError_) / kTimeDt : 0.0F;
+    const float kDTerm = kd_ * kDerivativePart;
+
+    previousError_ = kError;
+
+    return static_cast<int>(kPTerm + kITerm + kDTerm);
+}
diff --git a/src/kpi_rover_ecu/src/TCPTransport.cpp b/src/kpi_rover_ecu/src/TCPTransport.cpp
index cc790d7..c569c87 100644
--- a/src/kpi_rover_ecu/src/TCPTransport.cpp
+++ b/src/kpi_rover_ecu/src/TCPTransport.cpp
@@ -113,4 +113,4 @@ void TCPTransport::Destroy() {
     std::cout << "closing socket" << '\n';
     close(client_sockfd_);
     close(sockfd_);
-}
\ No newline at end of file
+}
diff --git a/src/kpi_rover_ecu/src/main.cpp b/src/kpi_rover_ecu/src/main.cpp
index daa1261..97183d8 100644
--- a/src/kpi_rover_ecu/src/main.cpp
+++ b/src/kpi_rover_ecu/src/main.cpp
@@ -54,8 +54,12 @@ int main(int argc, char* argv[]) {
 
     MotorController motors_processor;
     const uint8_t kMotorNumber = 4;
-    const std::vector<MotorConfig> kShassisVector = {MotorConfig(3, false), MotorConfig(4, false), MotorConfig(1, true),
-                                                     MotorConfig(2, true)};
+    const std::vector<MotorConfig> kShassisVector = {
+        MotorConfig(3, false, {1.5, 0.056, 1.5}),
+        MotorConfig(4, false, {1.5, 0.056, 1.5}),
+        MotorConfig(1, true, {1.5, 0.056, 1.5}),
+        MotorConfig(2, true, {1.5, 0.056, 1.5}),
+    };
 
     motors_processor.Init(kShassisVector, kMotorNumber);
     ProtocolHanlder protocol_handler(&motors_processor);
diff --git a/src/kpi_rover_ecu/src/messageQueue.cpp b/src/kpi_rover_ecu/src/messageQueue.cpp
index d3c2629..ae44d90 100644
--- a/src/kpi_rover_ecu/src/messageQueue.cpp
+++ b/src/kpi_rover_ecu/src/messageQueue.cpp
@@ -48,4 +48,4 @@ void MessageQueue::Clear() {
     std::queue<std::vector<uint8_t>> empty;
     std::swap(queue_, empty);
     pthread_mutex_unlock(&mutex_);
-}
\ No newline at end of file
+}
diff --git a/src/kpi_rover_ecu/src/motor.cpp b/src/kpi_rover_ecu/src/motor.cpp
index f28dbec..c812cc0 100644
--- a/src/kpi_rover_ecu/src/motor.cpp
+++ b/src/kpi_rover_ecu/src/motor.cpp
@@ -3,10 +3,19 @@
 #include <rc/encoder.h>
 #include <rc/motor.h>
 
+#include <array>
+#include <chrono>
+#include <cmath>
 #include <iostream>
+#include <ratio>
 
-Motor::Motor(int assigned_number, bool is_inverted)
-    : motorNumber_(assigned_number), inverted_(is_inverted), currentDutyCycle_(0) {}
+#include "PIDRegulator.h"
+
+Motor::Motor(int assigned_number, bool is_inverted, std::array<float, 3> _coeficients)
+    : 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) {
     if (newRPM > kMaxRpm) {
@@ -19,7 +28,16 @@ int Motor::MotorGo(int newRPM) {
         newRPM = -kMaxRpm;
     }
 
-    currentDutyCycle_ = GetDC(newRPM);
+    if (MotorSet(newRPM) == -1) {
+        return -1;
+    }
+
+    setpointRpm_ = newRPM;
+    return 0;
+}
+
+int Motor::MotorSet(int inputRPM) {
+    currentDutyCycle_ = GetDC(inputRPM);
     if (inverted_) {
         currentDutyCycle_ = -currentDutyCycle_;
     }
@@ -41,10 +59,37 @@ int Motor::MotorStop() const {
     return 0;
 }
 
-int Motor::GetEncoderCounter() const {
+float Motor::GetTimeSegment() {
+    const auto kCurrentTimePoint = std::chrono::high_resolution_clock::now();
+    const std::chrono::duration<double, std::milli> kElapsedMilliseconds = kCurrentTimePoint - lastTimePoint_;
+    lastTimePoint_ = kCurrentTimePoint;
+    return static_cast<float>(kElapsedMilliseconds.count());
+}
+
+float Motor::GetActualRpm(int _ticks, float _timeSegment) {
+    const float kRevolutions = static_cast<float>(_ticks) / static_cast<float>(kLoopTicks);
+    const float kInputPoint =
+        (std::round((kRevolutions * kSecondsMinute * kMiliSecondsSeconds) / _timeSegment)) * kSpeedIndexMultipler;
+    return kInputPoint;
+}
+
+int Motor::GetEncoderCounter() {
     const int kEncoderTicks = rc_encoder_read(motorNumber_);
+    int pid_encoder_ticks = kEncoderTicks;
+    if (!inverted_) {
+        pid_encoder_ticks *= -1;
+    }
+
+    const float kTimeDt = GetTimeSegment();
+    actualRpm_ = static_cast<int>(GetActualRpm(pid_encoder_ticks, kTimeDt));
+    const int kError = setpointRpm_ - actualRpm_;
+    std::cout << "set point " << setpointRpm_ << " current point " << actualRpm_ << " error " << kError << '\n';
+
+    const int kPidOutput = pidRegulator_.Run(static_cast<float>(kError), kTimeDt);
     rc_encoder_write(motorNumber_, 0);
+
+    MotorSet(setpointRpm_ + kPidOutput);
     return kEncoderTicks;
 }
 
-double Motor::GetDC(int entryRPM) { return static_cast<double>(entryRPM) / kMaxRpm; }
\ No newline at end of file
+double Motor::GetDC(int entryRPM) { return static_cast<double>(entryRPM) / kMaxRpm; }
diff --git a/src/kpi_rover_ecu/src/motorConfig.cpp b/src/kpi_rover_ecu/src/motorConfig.cpp
index 8099be1..abe8eb5 100644
--- a/src/kpi_rover_ecu/src/motorConfig.cpp
+++ b/src/kpi_rover_ecu/src/motorConfig.cpp
@@ -1,10 +1,13 @@
 #include "motorConfig.h"
 
+#include <array>
 #include <cstdint>
 
-MotorConfig::MotorConfig(uint8_t assignedNumber, bool invertedStatus)
-    : number_(assignedNumber), inverted_(invertedStatus) {}
+MotorConfig::MotorConfig(uint8_t assignedNumber, bool invertedStatus, const std::array<float, 3>& _coefs)
+    : number_(assignedNumber), inverted_(invertedStatus), pidCoefs_(_coefs) {}
+
+std::array<float, 3> MotorConfig::GetPidCoefs() const { return pidCoefs_; }
 
 uint8_t MotorConfig::GetNumber() const { return number_; }
 
-bool MotorConfig::IsInverted() const { return inverted_; }
\ No newline at end of file
+bool MotorConfig::IsInverted() const { return inverted_; }
diff --git a/src/kpi_rover_ecu/src/motorsController.cpp b/src/kpi_rover_ecu/src/motorsController.cpp
index c3d8765..4feabcd 100644
--- a/src/kpi_rover_ecu/src/motorsController.cpp
+++ b/src/kpi_rover_ecu/src/motorsController.cpp
@@ -9,7 +9,6 @@
 #include <iostream>
 #include <vector>
 
-#include "motor.h"
 #include "motorConfig.h"
 
 constexpr double kKillProcessTimeout = 2.0;
@@ -40,7 +39,7 @@ int MotorController::Init(const std::vector<MotorConfig>& _motors, uint8_t _moto
     motor_number_ = _motorNumber;
 
     for (int i = 0; i < motor_number_; ++i) {
-        motors_.emplace_back(_motors[i].GetNumber(), _motors[i].IsInverted());
+        motors_.emplace_back(_motors[i].GetNumber(), _motors[i].IsInverted(), _motors[i].GetPidCoefs());
     }
 
     return 0;
@@ -52,8 +51,8 @@ int MotorController::SetMotorRPM(int channel, int newRPM) {
         return -1;
     }
 
-    int res = motors_[channel].MotorGo(newRPM);
-    if (res != 0) {
+    const int kRes = motors_[channel].MotorGo(newRPM);
+    if (kRes != 0) {
         std::cout << "[ERROR][RC] Error while set new RPM" << '\n';
         return -1;
     }
diff --git a/src/kpi_rover_ecu/tests/CMakeLists.txt b/src/kpi_rover_ecu/tests/CMakeLists.txt
index baedca3..66db8d4 100644
--- a/src/kpi_rover_ecu/tests/CMakeLists.txt
+++ b/src/kpi_rover_ecu/tests/CMakeLists.txt
@@ -30,6 +30,8 @@ add_executable(unitTests
     test_motor.cpp
     test_protocol.cpp
     test_message_queue.cpp
+    test_motor_controller.cpp
+    test_pid_regulator.cpp
 )
 
 target_link_libraries(unitTests
diff --git a/src/kpi_rover_ecu/tests/test_motor.cpp b/src/kpi_rover_ecu/tests/test_motor.cpp
index 1bf62d0..99b3ec5 100644
--- a/src/kpi_rover_ecu/tests/test_motor.cpp
+++ b/src/kpi_rover_ecu/tests/test_motor.cpp
@@ -38,60 +38,50 @@ 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";
 }
 
-// Test MotorGo with minimum RPM
-TEST_F(MotorTest, MotorGoMinRPM) {
-    EXPECT_CALL(GetMockRCMotor(), set(0, ::testing::_)).WillOnce(::testing::Return(0));
-
-    motor = new Motor(0, false);
-    int result = motor->MotorGo(Motor::kMinRpm);
-
-    EXPECT_EQ(0, result);
-}
-
 // Test MotorGo with inverted motor direction
 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);
 }
 
-// Test MotorGo with RPM below minimum
-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);
-    int result = motor->MotorGo(Motor::kMinRpm - 1);
-
-    EXPECT_EQ(0, result);
-}
-
 // Test MotorGo with RPM above maximum
 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);
 }
 
+// Test MotorGo with RPM below -maximum
+TEST_F(MotorTest, MotorGoBelowMinusMaxRPM) {
+    // Verify behaviour with RPM below -maximum (should clamp to -MAX_RPM)
+    EXPECT_CALL(GetMockRCMotor(), set(0, ::testing::DoubleEq(-1))).WillOnce(::testing::Return(0));
+
+    motor = new Motor(0, false, {1.5f, 0.056f, 1.5f});
+    int result = motor->MotorGo(-Motor::kMaxRpm - 1);
+
+    EXPECT_EQ(0, result);
+}
+
 // Test MotorGo with set function returning error
 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 +92,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 +102,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 +112,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 +122,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";
diff --git a/src/kpi_rover_ecu/tests/test_motor_controller.cpp b/src/kpi_rover_ecu/tests/test_motor_controller.cpp
new file mode 100644
index 0000000..db40de3
--- /dev/null
+++ b/src/kpi_rover_ecu/tests/test_motor_controller.cpp
@@ -0,0 +1,112 @@
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <iostream>
+#include <vector>
+
+#include "motor.h"
+#include "motorConfig.h"
+#include "motorsController.h"
+
+// Include librobotcontrol mocks
+#include "mocks/librobotcontrol/include/mock_rc_encoder.h"
+#include "mocks/librobotcontrol/include/mock_rc_motor.h"
+#include "mocks/librobotcontrol/include/mock_rc_start_stop.h"
+#include "mocks/librobotcontrol/include/mock_rc_time.h"
+
+class MotorControllerTest : public ::testing::Test {
+   protected:
+    MotorController motor_controller;
+    const uint8_t kMotorNumber = 4;
+
+    void SetUp() override {
+        // Reset all mock objects
+        ResetMockRCMotor();
+        ResetMockRCEncoder();
+        setupDefaultMockActions();  // Ensure the mock is initialized
+        ResetMockRCTime();
+
+        // Set up default behaviors for mocks instead of expectations
+        ON_CALL(getStartStopMock(), kill_existing_process(::testing::_)).WillByDefault(::testing::Return(0));
+        ON_CALL(getStartStopMock(), enable_signal_handler()).WillByDefault(::testing::Return(0));
+        ON_CALL(GetMockRCMotor(), init_freq(::testing::_)).WillByDefault(::testing::Return(0));
+        ON_CALL(GetMockRCEncoder(), init()).WillByDefault(::testing::Return(0));
+
+        // Initialize with the same configuration as in the provided example
+        std::vector<MotorConfig> motor_configs = {
+            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})
+        };
+
+        motor_controller.Init(motor_configs, kMotorNumber);
+    }
+
+    void TearDown() override {
+        // Clean up expectations for Destroy
+        EXPECT_CALL(GetMockRCMotor(), cleanup()).WillOnce(::testing::Return(0));
+        EXPECT_CALL(GetMockRCEncoder(), cleanup()).WillOnce(::testing::Return(0));
+
+        motor_controller.Destroy();
+
+        // Clean up mock objects to prevent memory leaks
+        DestroyMockRCMotor();
+        DestroyMockRCEncoder();
+        DestroyStartStopMock();
+        DestroyMockRCTime();
+    }
+};
+
+// Test MotorController::Init
+TEST_F(MotorControllerTest, Init) {
+    // Expectations are already set in SetUp, no additional checks needed here
+    ASSERT_EQ(motor_controller.GetMotorsNumber(), kMotorNumber);
+}
+
+// Test MotorController::SetMotorRPM
+TEST_F(MotorControllerTest, SetMotorRPM) {
+    int channel = 1;
+    int rpm = 1000;
+
+    EXPECT_CALL(GetMockRCMotor(), set(channel, ::testing::_)).WillOnce(::testing::Return(0));
+
+    int result = motor_controller.SetMotorRPM(channel - 1, rpm); // Convert to 0-based index
+    ASSERT_EQ(result, 0);
+}
+
+// Test MotorController::SetMotorRPM with invalid channel
+TEST_F(MotorControllerTest, SetMotorRPMInvalidChannel) {
+    int invalid_channel = kMotorNumber + 1; // Out of range
+    int rpm = 1000;
+
+    int result = motor_controller.SetMotorRPM(invalid_channel, rpm);
+    ASSERT_EQ(result, -1);
+}
+
+// Test MotorController::StopMotor
+TEST_F(MotorControllerTest, StopMotor) {
+    int channel = 1;
+
+    EXPECT_CALL(GetMockRCMotor(), brake(channel)).WillOnce(::testing::Return(0));
+
+    int result = motor_controller.StopMotor(channel - 1); // Convert to 0-based index
+    ASSERT_EQ(result, 0);
+}
+
+// Test MotorController::GetEncoderCounter
+TEST_F(MotorControllerTest, GetEncoderCounter) {
+    int channel = 1;
+    int encoder_value = 42;
+
+    EXPECT_CALL(GetMockRCEncoder(), read(channel)).WillOnce(::testing::Return(encoder_value));
+
+    int result = motor_controller.GetEncoderCounter(channel - 1); // Convert to 0-based index
+    ASSERT_EQ(result, encoder_value);
+}
+
+// Test MotorController::Destroy
+TEST_F(MotorControllerTest, Destroy) {
+    // Expectations are already set in TearDown, no additional checks needed here
+    SUCCEED();
+}
diff --git a/src/kpi_rover_ecu/tests/test_pid_regulator.cpp b/src/kpi_rover_ecu/tests/test_pid_regulator.cpp
new file mode 100644
index 0000000..0847062
--- /dev/null
+++ b/src/kpi_rover_ecu/tests/test_pid_regulator.cpp
@@ -0,0 +1,85 @@
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <array>
+#include <cmath>
+
+#include "../include/PIDRegulator.h"
+
+// Test fixture for PIDRegulator tests
+class PIDRegulatorTest : public ::testing::Test {
+   protected:
+    PIDRegulator pid_regulator;
+
+    void SetUp() override {
+        // Initialize PIDRegulator with some coefficients
+        std::array<float, 3> coefficients = {1.0f, 0.5f, 0.1f};
+        pid_regulator.Init(coefficients);
+    }
+};
+
+TEST_F(PIDRegulatorTest, Init) {
+    std::array<float, 3> coefficients = {2.0f, 1.0f, 0.2f};
+    pid_regulator.Init(coefficients);
+
+    // Run the PID regulator with a known error and time delta to check if the coefficients are set correctly
+    float error = 1.0f;
+    float time_dt = 1.0f;
+    int result = pid_regulator.Run(error, time_dt);
+
+    // Check if the result is as expected
+    float p_term = coefficients[0] * error;
+    float i_term = coefficients[1] * error * time_dt;
+    float d_term = coefficients[2] * (error / time_dt);
+
+    int expected = static_cast<int>(p_term + i_term + d_term);
+    ASSERT_EQ(result, expected);
+}
+
+TEST_F(PIDRegulatorTest, Run_PositiveError) {
+    float error = 10.0f;
+    float time_dt = 1.0f;
+
+    int result = pid_regulator.Run(error, time_dt);
+
+    // Expected result calculation
+    float p_term = 1.0f * error;
+    float i_term = 0.5f * error * time_dt;
+    float d_term = 0.1f * (error / time_dt);
+
+    int expected = static_cast<int>(p_term + i_term + d_term);
+
+    ASSERT_EQ(result, expected);
+}
+
+TEST_F(PIDRegulatorTest, Run_NegativeError) {
+    float error = -10.0f;
+    float time_dt = 1.0f;
+
+    int result = pid_regulator.Run(error, time_dt);
+
+    // Expected result calculation
+    float p_term = 1.0f * error;
+    float i_term = 0.5f * error * time_dt;
+    float d_term = 0.1f * (error / time_dt);
+
+    int expected = static_cast<int>(p_term + i_term + d_term);
+
+    ASSERT_EQ(result, expected);
+}
+
+TEST_F(PIDRegulatorTest, Run_ZeroTimeDt) {
+    float error = 10.0f;
+    float time_dt = 0.0f;
+
+    int result = pid_regulator.Run(error, time_dt);
+
+    // Expected result calculation
+    float p_term = 1.0f * error;
+    float i_term = 0.5f * error * time_dt;
+    float d_term = 0.0f;  // Derivative term should be zero when time_dt is zero
+
+    int expected = static_cast<int>(p_term + i_term);
+
+    ASSERT_EQ(result, expected);
+}
diff --git a/src/kpi_rover_ecu/tests/test_protocol.cpp b/src/kpi_rover_ecu/tests/test_protocol.cpp
index b2ae159..0962668 100644
--- a/src/kpi_rover_ecu/tests/test_protocol.cpp
+++ b/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);


Reviews:
user: login: AksonovSergei
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-04-01T19:16:26Z
user: login: AksonovSergei
state: CHANGES_REQUESTED
author_association: MEMBER
submitted_at: 2025-04-01T19:24:56Z
user: login: AksonovSergei
body: Super!
Pleas fix some small comments.
state: CHANGES_REQUESTED
author_association: MEMBER
submitted_at: 2025-04-03T19:24:27Z
user: login: AksonovSergei
state: APPROVED
author_association: MEMBER
submitted_at: 2025-04-07T20:11:27Z

Review comments:
diff_hunk: @@ -0,0 +1,33 @@
+#ifndef PIDREGULATOR_H
+#define PIDREGULATOR_H
+
+#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);
+
+   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_;
+};
+
+#endif
path: src/kpi_rover_ecu/include/PIDRegulator.h
user: login: AksonovSergei
body: Please add empty line at the end of file.
created_at: 2025-04-01T14:13:37Z
author_association: MEMBER
diff_hunk: @@ -1,19 +1,22 @@
 #ifndef MOTOR_CONFIG_H
 #define MOTOR_CONFIG_H
 
+#include <array>
 #include <cstdint>
 
 class MotorConfig {
    public:
-    MotorConfig(uint8_t assignedNumber, bool invertedStatus);
+    MotorConfig(uint8_t assignedNumber, bool invertedStatus, const std::array<float, 3>& _coefs);
 
+    std::array<float, 3> GetPidCoefs() const;
     uint8_t GetNumber() const;
 
     bool IsInverted() const;
 
    private:
     uint8_t number_;
     bool inverted_;
+    std::array<float, 3> pidCoefs_ = {};
 };
 
 #endif  // MOTOR_CONFIG_H
path: src/kpi_rover_ecu/include/motorConfig.h
user: login: AksonovSergei
body: Please add empty line at the end of the file
created_at: 2025-04-01T14:14:22Z
author_association: MEMBER
diff_hunk: @@ -0,0 +1,53 @@
+#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) {
+    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';
+
+    const float kPTerm = kp_ * kError;
+
+    integral_ += kError * kTimeDt;
+    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 kDTerm = kd_ * kDerivativePart;
+
+    previousError_ = kError;
+
+    return static_cast<int>(kPTerm + kITerm + kDTerm);
+}
path: src/kpi_rover_ecu/src/PIDRegulator.cpp
user: login: AksonovSergei
body: Please add empty line at the end of file
created_at: 2025-04-01T14:14:55Z
author_association: MEMBER
diff_hunk: @@ -1,19 +1,27 @@
 #ifndef MOTOR_H
 #define MOTOR_H
 
+#include <array>
+
+#include "PIDRegulator.h"
+
 class Motor {
    public:
     static constexpr int kMinRpm = 8000;
path: src/kpi_rover_ecu/include/motor.h
user: login: AksonovSergei
body: kMinRpm is not used in code. Please remove and remove related test case.
created_at: 2025-04-01T14:42:54Z
author_association: MEMBER
diff_hunk: @@ -1,19 +1,27 @@
 #ifndef MOTOR_H
 #define MOTOR_H
 
+#include <array>
+
+#include "PIDRegulator.h"
+
 class Motor {
    public:
     static constexpr int kMinRpm = 8000;
     static constexpr int kMaxRpm = 26500;
+    static constexpr int kLoopTicks = 821;
 
-    Motor(int assigned_number, bool is_inverted);
+    Motor(int assigned_number, bool is_inverted, std::array<float, 3> _coeficients);
     int MotorGo(int newRPM);
     int MotorStop() const;
-    int GetEncoderCounter() const;
+    int GetEncoderCounter();
 
    private:
+    int MotorSet(int inputRPM);
     static constexpr int kBrakeTime = 100000;  // 100 ms
path: src/kpi_rover_ecu/include/motor.h
user: login: AksonovSergei
body: kBrakeTime is not used in the code. Please remove.

created_at: 2025-04-01T14:44:32Z
author_association: MEMBER
diff_hunk: @@ -41,9 +58,31 @@ int Motor::MotorStop() const {
     return 0;
 }
 
-int Motor::GetEncoderCounter() const {
+int Motor::GetEncoderCounter() {
     const int kEncoderTicks = rc_encoder_read(motorNumber_);
+    int pid_encoder_ticks = kEncoderTicks;
+    if (!inverted_) {
+        pid_encoder_ticks *= -1;
+    }
+
+    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);
path: src/kpi_rover_ecu/src/motor.cpp
user: login: AksonovSergei
body: 1. Please extract method.
2. SECONDSTOMINUTE and MILISECONDSTOSECOND are not Google code style

created_at: 2025-04-01T19:24:50Z
author_association: MEMBER
diff_hunk: @@ -41,9 +58,39 @@ int Motor::MotorStop() const {
     return 0;
 }
 
-int Motor::GetEncoderCounter() const {
+float Motor::GetTimeSegment() {
+    const auto kCurrentTimePoint = std::chrono::high_resolution_clock::now();
+    const std::chrono::duration<double, std::milli> kElapsedMilliseconds = kCurrentTimePoint - lastTimePoint_;
+    lastTimePoint_ = kCurrentTimePoint;
+    return static_cast<float>(kElapsedMilliseconds.count());
+}
+
+float Motor::GetSpeedError(int _ticks, float _timeSegment) {
+    const float kRevolutions = static_cast<float>(_ticks) / static_cast<float>(kLoopTicks);
+    const float kInputPoint =
+        static_cast<float>(std::round((kRevolutions * kSecondsMinute * kMiliSecondsSeconds) / _timeSegment)) *
+        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';
+
+    return kError;
+}
path: src/kpi_rover_ecu/src/motor.cpp
user: login: AksonovSergei
body: This function taking two responsibilities:
1. Calculating actualRpm_
2. Calculating error.

It must be two separate functions.

created_at: 2025-04-03T19:17:49Z
author_association: MEMBER
diff_hunk: @@ -41,9 +58,39 @@ int Motor::MotorStop() const {
     return 0;
 }
 
-int Motor::GetEncoderCounter() const {
+float Motor::GetTimeSegment() {
+    const auto kCurrentTimePoint = std::chrono::high_resolution_clock::now();
+    const std::chrono::duration<double, std::milli> kElapsedMilliseconds = kCurrentTimePoint - lastTimePoint_;
+    lastTimePoint_ = kCurrentTimePoint;
+    return static_cast<float>(kElapsedMilliseconds.count());
+}
+
+float Motor::GetSpeedError(int _ticks, float _timeSegment) {
+    const float kRevolutions = static_cast<float>(_ticks) / static_cast<float>(kLoopTicks);
+    const float kInputPoint =
+        static_cast<float>(std::round((kRevolutions * kSecondsMinute * kMiliSecondsSeconds) / _timeSegment)) *
+        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';
+
+    return kError;
+}
+
+int Motor::GetEncoderCounter() {
     const int kEncoderTicks = rc_encoder_read(motorNumber_);
+    int pid_encoder_ticks = kEncoderTicks;
+    if (!inverted_) {
+        pid_encoder_ticks *= -1;
+    }
+
+    const float kTimeDt = GetTimeSegment();
+    const float kError = GetSpeedError(pid_encoder_ticks, kTimeDt);
path: src/kpi_rover_ecu/src/motor.cpp
user: login: AksonovSergei
body: actualRpm_ = GetActualRpm(encoder_ticks, kTimeDt)
kError = setpointRpm_ - actualRpm
created_at: 2025-04-03T19:20:12Z
author_association: MEMBER
diff_hunk: @@ -0,0 +1,112 @@
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <iostream>
+#include <vector>
+
+#include "motor.h"
+#include "motorConfig.h"
+#include "motorsController.h"
+
+// Include librobotcontrol mocks
+#include "mocks/librobotcontrol/include/mock_rc_encoder.h"
+#include "mocks/librobotcontrol/include/mock_rc_motor.h"
+#include "mocks/librobotcontrol/include/mock_rc_start_stop.h"
+#include "mocks/librobotcontrol/include/mock_rc_time.h"
+
+class MotorControllerTest : public ::testing::Test {
+   protected:
+    MotorController motor_controller;
+    const uint8_t kMotorNumber = 4;
+
+    void SetUp() override {
+        // Reset all mock objects
+        ResetMockRCMotor();
+        ResetMockRCEncoder();
+        setupDefaultMockActions();  // Ensure the mock is initialized
+        ResetMockRCTime();
+
+        // Set up default behaviors for mocks instead of expectations
+        ON_CALL(getStartStopMock(), kill_existing_process(::testing::_)).WillByDefault(::testing::Return(0));
+        ON_CALL(getStartStopMock(), enable_signal_handler()).WillByDefault(::testing::Return(0));
+        ON_CALL(GetMockRCMotor(), init_freq(::testing::_)).WillByDefault(::testing::Return(0));
+        ON_CALL(GetMockRCEncoder(), init()).WillByDefault(::testing::Return(0));
+
+        // Initialize with the same configuration as in the provided example
+        std::vector<MotorConfig> motor_configs = {
+            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})
+        };
+
+        motor_controller.Init(motor_configs, kMotorNumber);
+    }
+
+    void TearDown() override {
+        // Clean up expectations for Destroy
+        EXPECT_CALL(GetMockRCMotor(), cleanup()).WillOnce(::testing::Return(0));
+        EXPECT_CALL(GetMockRCEncoder(), cleanup()).WillOnce(::testing::Return(0));
+
+        motor_controller.Destroy();
+
+        // Clean up mock objects to prevent memory leaks
+        DestroyMockRCMotor();
+        DestroyMockRCEncoder();
+        DestroyStartStopMock();
+        DestroyMockRCTime();
+    }
+};
+
+// Test MotorController::Init
+TEST_F(MotorControllerTest, Init) {
+    // Expectations are already set in SetUp, no additional checks needed here
+    ASSERT_EQ(motor_controller.GetMotorsNumber(), kMotorNumber);
+}
+
+// Test MotorController::SetMotorRPM
+TEST_F(MotorControllerTest, SetMotorRPM) {
+    int channel = 1;
+    int rpm = 1000;
+
+    EXPECT_CALL(GetMockRCMotor(), set(channel, ::testing::_)).WillOnce(::testing::Return(0));
+
+    int result = motor_controller.SetMotorRPM(channel - 1, rpm); // Convert to 0-based index
+    ASSERT_EQ(result, 0);
+}
+
+// Test MotorController::SetMotorRPM with invalid channel
+TEST_F(MotorControllerTest, SetMotorRPMInvalidChannel) {
+    int invalid_channel = kMotorNumber + 1; // Out of range
+    int rpm = 1000;
+
+    int result = motor_controller.SetMotorRPM(invalid_channel, rpm);
+    ASSERT_EQ(result, -1);
+}
+
+// Test MotorController::StopMotor
+TEST_F(MotorControllerTest, StopMotor) {
+    int channel = 1;
+
+    EXPECT_CALL(GetMockRCMotor(), brake(channel)).WillOnce(::testing::Return(0));
+
+    int result = motor_controller.StopMotor(channel - 1); // Convert to 0-based index
+    ASSERT_EQ(result, 0);
+}
+
+// Test MotorController::GetEncoderCounter
+TEST_F(MotorControllerTest, GetEncoderCounter) {
+    int channel = 1;
+    int encoder_value = 42;
+
+    EXPECT_CALL(GetMockRCEncoder(), read(channel)).WillOnce(::testing::Return(encoder_value));
+
+    int result = motor_controller.GetEncoderCounter(channel - 1); // Convert to 0-based index
+    ASSERT_EQ(result, encoder_value);
+}
+
+// Test MotorController::Destroy
+TEST_F(MotorControllerTest, Destroy) {
+    // Expectations are already set in TearDown, no additional checks needed here
+    SUCCEED();
+}
path: src/kpi_rover_ecu/tests/test_motor_controller.cpp
user: login: AksonovSergei
body: No empty line at the end of file :)
created_at: 2025-04-03T19:22:18Z
author_association: MEMBER
diff_hunk: @@ -0,0 +1,85 @@
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <array>
+#include <cmath>
+
+#include "../include/PIDRegulator.h"
+
+// Test fixture for PIDRegulator tests
+class PIDRegulatorTest : public ::testing::Test {
+   protected:
+    PIDRegulator pid_regulator;
+
+    void SetUp() override {
+        // Initialize PIDRegulator with some coefficients
+        std::array<float, 3> coefficients = {1.0f, 0.5f, 0.1f};
+        pid_regulator.Init(coefficients);
+    }
+};
+
+TEST_F(PIDRegulatorTest, Init) {
+    std::array<float, 3> coefficients = {2.0f, 1.0f, 0.2f};
+    pid_regulator.Init(coefficients);
+
+    // Run the PID regulator with a known error and time delta to check if the coefficients are set correctly
+    float error = 1.0f;
+    float time_dt = 1.0f;
+    int result = pid_regulator.Run(error, time_dt);
+
+    // Check if the result is as expected
+    float p_term = coefficients[0] * error;
+    float i_term = coefficients[1] * error * time_dt;
+    float d_term = coefficients[2] * (error / time_dt);
+
+    int expected = static_cast<int>(p_term + i_term + d_term);
+    ASSERT_EQ(result, expected);
+}
+
+TEST_F(PIDRegulatorTest, Run_PositiveError) {
+    float error = 10.0f;
+    float time_dt = 1.0f;
+
+    int result = pid_regulator.Run(error, time_dt);
+
+    // Expected result calculation
+    float p_term = 1.0f * error;
+    float i_term = 0.5f * error * time_dt;
+    float d_term = 0.1f * (error / time_dt);
+
+    int expected = static_cast<int>(p_term + i_term + d_term);
+
+    ASSERT_EQ(result, expected);
+}
+
+TEST_F(PIDRegulatorTest, Run_NegativeError) {
+    float error = -10.0f;
+    float time_dt = 1.0f;
+
+    int result = pid_regulator.Run(error, time_dt);
+
+    // Expected result calculation
+    float p_term = 1.0f * error;
+    float i_term = 0.5f * error * time_dt;
+    float d_term = 0.1f * (error / time_dt);
+
+    int expected = static_cast<int>(p_term + i_term + d_term);
+
+    ASSERT_EQ(result, expected);
+}
+
+TEST_F(PIDRegulatorTest, Run_ZeroTimeDt) {
+    float error = 10.0f;
+    float time_dt = 0.0f;
+
+    int result = pid_regulator.Run(error, time_dt);
+
+    // Expected result calculation
+    float p_term = 1.0f * error;
+    float i_term = 0.5f * error * time_dt;
+    float d_term = 0.0f;  // Derivative term should be zero when time_dt is zero
+
+    int expected = static_cast<int>(p_term + i_term);
+
+    ASSERT_EQ(result, expected);
+}
path: src/kpi_rover_ecu/tests/test_pid_regulator.cpp
user: login: AksonovSergei
body: No empty line ;)

created_at: 2025-04-03T19:22:37Z
author_association: MEMBER

PR comments:
user: login: taketook34
created_at: 2025-04-01T13:50:06Z
body: As I have changed motor.cpp due to adding PID to code so unit tests don't pass
author_association: CONTRIBUTOR
user: login: AksonovSergei
created_at: 2025-04-01T14:28:58Z
body: https://github.com/KPI-Rover/ecu_sw_bb/pull/20/checks

Please fix all warning related to PIDRegulator.h and PIDRegulator.cpp

author_association: MEMBER
user: login: AksonovSergei
created_at: 2025-04-01T14:30:03Z
body: And please fix unit tests.
Also, we will need to cover PID regulator with tests. But after we agree on implementation.


author_association: MEMBER
user: login: AksonovSergei
created_at: 2025-04-01T15:27:31Z
body: In our code, we are violating the SOLID principle. We have a Motor object, which has a desired speed at which we want it to rotate and an actual speed measured using an encoder.

The variable names do not reflect their actual meaning. Motor::currentRpm_ represents the desired speed. Typically, such variables include the word "setpoint".

There is no variable for storing the actual measured speed. It is calculated inside the PID controller, which means the PID controller must be aware of the gearbox ratio. This is incorrect since these are properties of the motor, not the PID controller.

The calculation of the actual speed should be moved to the Motor class. This computation should take place every time the GetEncoderCounter function is called.



author_association: MEMBER
user: login: taketook34
created_at: 2025-04-01T17:26:45Z
body: Okay, I change PID and rewrite code as need. If I finish it quickly I will work on tests. If not I will work on tests tomorrow after job

author_association: CONTRIBUTOR
user: login: taketook34
created_at: 2025-04-01T19:20:35Z
body: All empty lines in end of file  is to add after  #endif ? 
author_association: CONTRIBUTOR
user: login: AksonovSergei
created_at: 2025-04-03T19:28:17Z
body: And please fix the following warnings:
/__w/ecu_sw_bb/ecu_sw_bb/src/kpi_rover_ecu/src/motorConfig.cpp:5:82: warning: no header providing "std::array" is directly included [misc-include-cleaner]
                                                   ^
/__w/ecu_sw_bb/ecu_sw_bb/src/kpi_rover_ecu/src/motorsController.cpp:12:1: warning: included header motor.h is not used directly [misc-include-cleaner]

   
/__w/ecu_sw_bb/ecu_sw_bb/src/kpi_rover_ecu/src/motor.cpp:63:46: warning: no header providing "std::milli" is directly included [misc-include-cleaner]

/__w/ecu_sw_bb/ecu_sw_bb/src/kpi_rover_ecu/src/motor.cpp:71:9: warning: redundant explicit casting to the same type 'float' as the sub-expression, remove this casting [readability-redundant-casting]
   
author_association: MEMBER
user: login: taketook34
created_at: 2025-04-03T20:18:16Z
body: Good night @AksonovSergei 
Can I also add emty line in end of all files that haven't got it? 
author_association: CONTRIBUTOR
user: login: AksonovSergei
created_at: 2025-04-03T20:36:07Z
body: Sure

author_association: MEMBER
user: login: taketook34
created_at: 2025-04-04T18:43:09Z
body: Good evening @AksonovSergei 
Is it okay if I solve this moment related to calculation of error next way:
```
const float kTimeDt = GetTimeSegment();
actualRpm_ = static_cast<int>(GetActualRpm(pid_encoder_ticks, kTimeDt));
const int kError = setpointRpm_ - actualRpm_;
std::cout << "set point " << setpointRpm_ << " current point " << actualRpm_ << " error " << kError << '\n';

const int kPidOutput = pidRegulator_.Run(static_cast<float>(kError), kTimeDt);
rc_encoder_write(motorNumber_, 0);

MotorSet(setpointRpm_ + kPidOutput);
return kEncoderTicks;
```
author_association: CONTRIBUTOR
user: login: AksonovSergei
created_at: 2025-04-07T08:07:49Z
body: > MotorSet(setpointRpm_ + kPidOutput);

I do not like this mixing. Why do you need to set at the same time setpoint and output power? I do not see reason for this.
author_association: MEMBER
user: login: taketook34
created_at: 2025-04-07T08:41:18Z
body: Good morning @AksonovSergei 
I don't clearly understand what you mean
I use MotorSet to set only new rpm, not changing setpointRPM  . I set PID regulator to return additional value to add to correct actualRPM. MotorSet generate duty cycle value and set on motor. MotorGo() now only call MotorSet and set setpointRPM. Pid regulator enumerate how we need to change speed to fit setpoint. 

author_association: CONTRIBUTOR
user: login: AksonovSergei
created_at: 2025-04-07T13:49:35Z
body: Why setpointRpm_ + kPidOutput?
I expect only MotorSet(kPidOutput)
author_association: MEMBER
user: login: taketook34
created_at: 2025-04-07T16:33:33Z
body: I tuned PID coeficients to count additional value to add it to setpoint

author_association: CONTRIBUTOR
user: login: taketook34
created_at: 2025-04-07T19:05:46Z
body: Good evening
What we do with PID, do we merge or not? Do I have moments besides this? I really wouldn't want to change PID fucntion, to retune PID coefs because result won't be better than now and I am even not sure that I will tune it accurately. When I developed PID regulator I thought about made it to count new value not additional but my tries wasn't success because coeficients would have been bigger and need more time.
author_association: CONTRIBUTOR
user: login: AksonovSergei
created_at: 2025-04-07T20:10:40Z
body: I’m not fully satisfied with keeping the current implementation as it is, but I also don’t want to push you into changing something you're not confident about. That’s why I created a separate task for myself to revisit and improve the PID logic later.

Feel free to go ahead and merge this pull request.
author_association: MEMBER