Issue #21 (pid): Change PID value from additional to absolute

Author: taketook34

src/kpi_rover_ecu/include/PIDRegulator.h

@@ -8,7 +8,7 @@
 class PIDRegulator {
    public:
     void Init(std::array<float, 3> _coeficients);
-    int Run(float kError, float kTimeDt);
+    int Run(float actual, float setpoint, float dt);
 
    private:
     float kp_ = 0;

src/kpi_rover_ecu/src/PIDRegulator.cpp

@@ -9,23 +9,24 @@ void PIDRegulator::Init(std::array<float, 3> _coeficients) {
     kd_ = _coeficients[2];
 }
 
-int PIDRegulator::Run(float kError, float kTimeDt) {
-    const float kPTerm = kp_ * kError;
+int PIDRegulator::Run(float actual, float setpoint, float dt) {
+    float error = setpoint - actual;
+    float pTerm = kp_ * error;
 
-    integral_ += kError * kTimeDt;
-    if (integral_ > integralLimit_) {
-        integral_ = integralLimit_;
-    }
-    if (integral_ < -integralLimit_) {
-        integral_ = -integralLimit_;
-    }
+    integral_ += error * dt;
+    if (integral_ > integralLimit_) integral_ = integralLimit_;
+    if (integral_ < -integralLimit_) integral_ = -integralLimit_;
 
-    const float kITerm = ki_ * integral_;
+    float iTerm = ki_ * integral_;
+    float dTerm = kd_ * ((dt > 0.0F) ? (error - previousError_) / dt : 0.0F);
 
-    const float kDerivativePart = (kTimeDt > 0.0F) ? (kError - previousError_) / kTimeDt : 0.0F;
-    const float kDTerm = kd_ * kDerivativePart;
+    previousError_ = error;
 
-    previousError_ = kError;
+    float output = pTerm + iTerm + dTerm;
 
-    return static_cast<int>(kPTerm + kITerm + kDTerm);
+    // Optionally clamp output to max/min here if needed
+    // if (output > maxOutput_) output = maxOutput_;
+    // if (output < minOutput_) output = minOutput_;
+
+    return static_cast<int>(output);
 }

src/kpi_rover_ecu/src/main.cpp

@@ -93,11 +93,17 @@ int main(int argc, char* argv[]) {
 
     MotorController motors_processor;
     const uint8_t kMotorNumber = 4;
+    // 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}),
+    // };
     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}),
+        MotorConfig(3, false, {0, 0, 0}),
+        MotorConfig(4, false, {0, 0, 0}),
+        MotorConfig(1, true, {0, 0, 0}),
+        MotorConfig(2, true, {0, 0, 0}),
     };
 
     motors_processor.Init(kShassisVector, kMotorNumber);

src/kpi_rover_ecu/src/motor.cpp

@@ -19,15 +19,15 @@ Motor::Motor(int assigned_number, bool is_inverted, std::array<float, 3> _coefic
 }
 
 int Motor::MotorGo(int newRPM) {
-    if (newRPM > kMaxRpm) {
-        LOG_WARNING << " RPM out of range for motor" << motorNumber_;
-        newRPM = kMaxRpm;
-    }
-
-    if (newRPM < -kMaxRpm) {
-        LOG_WARNING << "RPM out of range for motor" << motorNumber_;
-        newRPM = -kMaxRpm;
-    }
+    // if (newRPM > kMaxRpm) {
+    //     LOG_WARNING << " RPM out of range for motor" << motorNumber_;
+    //     newRPM = kMaxRpm;
+    // }
+
+    // if (newRPM < -kMaxRpm) {
+    //     LOG_WARNING << "RPM out of range for motor" << motorNumber_;
+    //     newRPM = -kMaxRpm;
+    // }
     LOG_DEBUG << "Set new RPM for motor " << motorNumber_;
     if (MotorSet(newRPM) == -1) {
         LOG_ERROR << "MotorSet failed";
@@ -94,12 +94,11 @@ int Motor::GetEncoderCounter() {
     LOG_INFO << "set point " << setpointRpm_ << " current point " << actualRpm_ << " error " << kError << " for motor"
              << motorNumber_;
 
-    LOG_DEBUG << "Run PID regulator";
-    const int kPidOutput = pidRegulator_.Run(static_cast<float>(kError), kTimeDt);
+    // LOG_DEBUG << "Run PID regulator";
+    const int kPidOutput = pidRegulator_.Run(actualRpm_, setpointRpm_, kTimeDt);
     rc_encoder_write(motorNumber_, 0);
-    LOG_DEBUG << "set encoder value to " << 0;
-    LOG_DEBUG << "Set additional result PID value " << kPidOutput << " for motor " << motorNumber_;
-    MotorSet(setpointRpm_ + kPidOutput);
+    LOG_DEBUG << "Set absoulute PID value" << kPidOutput;
+    MotorSet(kPidOutput);
     return pid_encoder_ticks;
 }