first commit

PID.c

@@ -0,0 +1,103 @@
+#include "PID.h"
+
+void pidControllerInit(PidController *pid, float Kp, float Ki, float Kd, float tau, float limMin, float limMax,
+                       float limMinInt, float limMaxInt, float T) {
+    pid->Kp = Kp;
+    pid->Ki = Ki;
+    pid->Kd = Kd;
+    pid->KpDefault = Kp;
+    pid->KiDefault = Ki;
+    pid->KdDefault = Kd;
+
+    pid->tau = tau;
+    pid->limMin = limMin;
+    pid->limMax = limMax;
+    pid->limMinInt = limMinInt;
+    pid->limMaxInt = limMaxInt;
+    pid->T = T;
+
+    /* Clear controller variables */
+    pid->integrator = 0.0f;
+    pid->prevError = 0.0f;
+
+    pid->differentiator = 0.0f;
+    pid->prevMeasurement = 0.0f;
+
+    pid->out = 0.0f;
+
+    pid->enable = false;
+}
+
+float pidControllerUpdate(PidController *pid, float setpoint, float measurement) {
+
+    /*
+    * Error signal
+    */
+    float error = setpoint - measurement;
+
+
+    /*
+    * Proportional
+    */
+    pid->proportional = pid->Kp * error;
+
+
+    /*
+    * Integral
+    */
+    pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);
+
+    /* Anti-wind-up via integrator clamping */
+    if (pid->integrator > pid->limMaxInt) {
+
+        pid->integrator = pid->limMaxInt;
+
+    } else if (pid->integrator < pid->limMinInt) {
+
+        pid->integrator = pid->limMinInt;
+
+    }
+
+
+    /*
+    * Derivative (band-limited differentiator)
+    */
+
+    pid->differentiator = -(2.0f * pid->Kd * (measurement -
+                                              pid->prevMeasurement)    /* Note: derivative on measurement, therefore minus sign in front of equation! */
+                            + (2.0f * pid->tau - pid->T) * pid->differentiator)
+                          / (2.0f * pid->tau + pid->T);
+
+
+    /*
+    * Compute output and apply limits
+    */
+    pid->out = pid->proportional + pid->integrator + pid->differentiator;
+
+    if (pid->proportional > pid->limMax) {
+
+        pid->proportional = pid->limMax;
+
+    } else if (pid->proportional < pid->limMin) {
+
+        pid->proportional = pid->limMin;
+
+    }
+
+    if (pid->out > pid->limMax) {
+
+        pid->out = pid->limMax;
+
+    } else if (pid->out < pid->limMin) {
+
+        pid->out = pid->limMin;
+
+    }
+
+    /* Store error and measurement for later use */
+    pid->prevError = error;
+    pid->prevMeasurement = measurement;
+
+    /* Return controller output */
+    return pid->out;
+}