commit 2d3a3860e780afdd300a35e9ca3989281f19e63c
Author: Alekseev <alekseev2014s@gmail.com>
Date:   Sun May 8 00:13:46 2022 +0700

    first commit

diff --git a/PID.c b/PID.c
new file mode 100644
index 0000000..dd382de
--- /dev/null
+++ b/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;
+}
diff --git a/PID.h b/PID.h
new file mode 100644
index 0000000..5f7b46b
--- /dev/null
+++ b/PID.h
@@ -0,0 +1,51 @@
+#ifndef PID_CONTROLLER_H
+#define PID_CONTROLLER_H
+
+#include <stdbool.h>
+
+typedef struct {
+
+    /* Controller gains */
+    float Kp;
+    float Ki;
+    float Kd;
+
+    /* Default controller gains */
+    float KpDefault;
+    float KiDefault;
+    float KdDefault;
+
+    /* Derivative low-pass filter time constant */
+    float tau;
+
+    /* Output limits */
+    float limMin;
+    float limMax;
+
+    /* Integrator limits */
+    float limMinInt;
+    float limMaxInt;
+
+    /* Sample time (in seconds) */
+    float T;
+
+    bool enable;
+
+    /* Controller "memory" */
+    float proportional;
+    float integrator;
+    float prevError;            /* Required for integrator */
+    float differentiator;
+    float prevMeasurement;        /* Required for differentiator */
+
+    /* Controller output */
+    float out;
+
+} PidController;
+
+void pidControllerInit(PidController *pid, float Kp, float Ki, float Kd, float tau, float limMin, float limMax,
+                       float limMinInt, float limMaxInt, float T);
+
+float pidControllerUpdate(PidController *pid, float setpoint, float measurement);
+
+#endif //PID_CONTROLLER_H