Alle Libraries eingefügt.

Libraries/pid/pid.cpp

@@ -0,0 +1,71 @@
+#include "PID.h"
+#include <Arduino.h>
+
+PID::PID(float kp, float ki, float kd) {
+    this->kp = kp;
+    this->ki = ki;
+    this->kd = kd;
+    this->minOutput = 0;
+    this->maxOutput = 255;
+    this->lastOutput = 0;
+    this->sampleTime = 100; // Default sample time in milliseconds
+    this->lastTime = millis();
+    this->integral = 0;
+    this->lastInput = 0;
+}
+
+void PID::setTunings(float kp, float ki, float kd) {
+    this->kp = kp;
+    this->ki = ki;
+    this->kd = kd;
+}
+
+void PID::setOutputLimits(float min, float max) {
+    this->minOutput = min;
+    this->maxOutput = max;
+}
+
+void PID::setSampleTime(int sampleTime) {
+    this->sampleTime = sampleTime;
+}
+
+void PID::reset() {
+    this->integral = 0;
+    this->lastInput = 0;
+    this->lastTime = millis();
+}
+
+float PID::compute(float setpoint, float input) {
+    unsigned long now = millis();
+    unsigned long timeChange = now - lastTime;
+
+    if (timeChange >= sampleTime) {
+        // Calculate error
+        float error = setpoint - input;
+
+        // Proportional term
+        float Pout = kp * error;
+
+        // Integral term
+        integral += (ki * error * timeChange / 1000.0);
+        integral = constrain(integral, minOutput, maxOutput);
+
+        // Derivative term
+        float derivative = (input - lastInput) / (timeChange / 1000.0);
+        float Dout = kd * derivative;
+
+        // Compute output
+        float output = Pout + integral - Dout;
+        output = constrain(output, minOutput, maxOutput);
+
+        // Remember some variables for next time
+        lastInput = input;
+        lastTime = now;
+
+        lastOutput = output
+
+        return output;
+    }
+
+    return lastOutput;
+}