initial commit

include/pid_controller.h

@@ -0,0 +1,83 @@
+#pragma once
+
+class PidController {
+public:
+  PidController(float kp, float ki, float kd, float outputMin, float outputMax)
+      : kp_(kp),
+        ki_(ki),
+        kd_(kd),
+        outputMin_(outputMin),
+        outputMax_(outputMax) {}
+
+  void setTunings(float kp, float ki, float kd) {
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+  }
+
+  void setSetpoint(float setpoint) { setpoint_ = setpoint; }
+
+  void reset() {
+    integral_ = 0.0f;
+    prevInput_ = 0.0f;
+    firstSample_ = true;
+  }
+
+  float compute(float input, uint32_t nowMs) {
+    if (firstSample_) {
+      prevInput_ = input;
+      prevTimeMs_ = nowMs;
+      firstSample_ = false;
+      return outputMin_;
+    }
+
+    const float dt = static_cast<float>(nowMs - prevTimeMs_) / 1000.0f;
+    if (dt <= 0.0f) {
+      return lastOutput_;
+    }
+
+    const float error = setpoint_ - input;
+    integral_ += error * dt;
+
+    // Anti-windup: clamp integral so output cannot exceed limits
+    const float integralMax = (outputMax_ - outputMin_) / (ki_ > 0.0f ? ki_ : 1.0f);
+    if (integral_ > integralMax) {
+      integral_ = integralMax;
+    } else if (integral_ < 0.0f) {
+      integral_ = 0.0f;
+    }
+
+    const float derivative = (input - prevInput_) / dt;
+    float output = kp_ * error + ki_ * integral_ - kd_ * derivative;
+
+    if (output < outputMin_) {
+      output = outputMin_;
+    } else if (output > outputMax_) {
+      output = outputMax_;
+    }
+
+    prevInput_ = input;
+    prevTimeMs_ = nowMs;
+    lastOutput_ = output;
+    return output;
+  }
+
+  float lastOutput() const { return lastOutput_; }
+
+  float kp() const { return kp_; }
+  float ki() const { return ki_; }
+  float kd() const { return kd_; }
+
+private:
+  float kp_;
+  float ki_;
+  float kd_;
+  float outputMin_;
+  float outputMax_;
+  float setpoint_ = 0.0f;
+  float integral_ = 0.0f;
+  float prevInput_ = 0.0f;
+  float lastOutput_ = 0.0f;
+  uint32_t prevTimeMs_ = 0;
+  bool firstSample_ = true;
+};