Issue #14-2: Adding PIDRegulator.cpp

Author: taketook34

src/kpi_rover_ecu/include/PIDRegulator.h

@@ -0,0 +1,33 @@
+#ifndef PIDREGULATOR_H
+#define PIDREGULATOR_H
+
+#pragma once
+
+#include <chrono>
+#include <array>
+
+#define SECONDSTOMINUTE 60
+#define MILISECONDSTOSECOND 1000
+#define SPEEDINDEXMULTIPLIER 100
+
+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 integralLimit_ = 26500;
+    const int outputLimit_ = 26500;
+    int loopsTicks_;
+    int maxRPM_;
+
+    std::chrono::time_point<std::chrono::high_resolution_clock> lastTimePoint_;
+};
+
+#endif

src/kpi_rover_ecu/include/motor.h

@@ -1,19 +1,26 @@
 #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
 
+    PIDRegulator pidRegulator_;
+    int currentRpm_;
     int motorNumber_;
     bool inverted_;
     double currentDutyCycle_;

src/kpi_rover_ecu/include/motorConfig.h

@@ -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

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

src/kpi_rover_ecu/src/PIDRegulator.cpp

@@ -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)) *
+                            SPEEDINDEXMULTIPLIER;
+    const float kError = static_cast<float>(setpoint) - kInputPoint; // get rid of abs() !!!
+    std::cout << "set point " << setpoint << " current point " << kInputPoint << " error " << kError << '\n';
+
+    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) ? (kError - previousError_) / kTimeDt : 0.0F;
+    const float kDTerm = kd_ * kDerivativePart;
+
+    previousError_ = kError;
+
+    return static_cast<int>(kPTerm + kITerm + kDTerm);
+}

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);

src/kpi_rover_ecu/src/motor.cpp

@@ -1,12 +1,15 @@
 #include "motor.h"
+#include "PIDRegulator.h"
 
 #include <rc/encoder.h>
 #include <rc/motor.h>
 
 #include <iostream>
 
-Motor::Motor(int assigned_number, bool is_inverted)
-    : motorNumber_(assigned_number), inverted_(is_inverted), currentDutyCycle_(0) {}
+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);
+}
 
 int Motor::MotorGo(int newRPM) {
     if (newRPM > kMaxRpm) {
@@ -19,7 +22,17 @@ int Motor::MotorGo(int newRPM) {
         newRPM = -kMaxRpm;
     }
 
-    currentDutyCycle_ = GetDC(newRPM);
+
+    if (MotorSet(newRPM) == -1) {
+        return -1;
+    }
+
+    currentRpm_ = newRPM;
+    return 0;
+}
+
+int Motor::MotorSet(int inputRPM) {
+    currentDutyCycle_ = GetDC(inputRPM);
     if (inverted_) {
         currentDutyCycle_ = -currentDutyCycle_;
     }
@@ -41,9 +54,17 @@ 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 int kPidOutput = pidRegulator_.Run(currentRpm_, pid_encoder_ticks);
     rc_encoder_write(motorNumber_, 0);
+
+    MotorSet(currentRpm_ + kPidOutput);
     return kEncoderTicks;
 }
 

src/kpi_rover_ecu/src/motorConfig.cpp

@@ -2,8 +2,10 @@
 
 #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_; }
 

src/kpi_rover_ecu/src/motorsController.cpp

@@ -40,7 +40,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;