Add Generic PID Module

middleware/include/pid.h

@@ -0,0 +1,52 @@
+#ifndef PID_H
+#define PID_H
+
+#include "stm32f4xx_hal.h"
+#include <stdbool.h>
+
+typedef struct
+{
+    float kP, kI, kD;
+
+    float err_tol;
+    float err;
+    float prev_err;
+    float prev_time; // in seconds
+    float min_out;
+    float max_out;
+
+    float integral_sum;
+} pid_t;
+
+/**
+ * @brief Initializes a PID controller
+ *
+ * @param kP Proportional constant
+ * @param kI Integral constant
+ * @param kD Derivative constant
+ * @param err_tol Error tolerance for checking if reached setpoint
+ * @param min_out Minimum output value
+ * @param max_out Maximum output value
+ * @return pid_t
+ */
+pid_t *pid_init(float kP, float kI, float kD, float err_tol, float min_out, float max_out);
+
+/**
+ * @brief Returns PID output given the setpoint and current state value
+ *
+ * @param pid
+ * @param setpoint Desired state value
+ * @param measurement Current state value
+ */
+float pid_calculate(pid_t *pid, float setpoint, float measurement);
+
+/**
+ * @brief Returns whether or not the controller is within error tolerance
+ *
+ * @param pid
+ * @return true
+ * @return false
+ */
+bool pid_at_setpoint(pid_t *pid);
+
+#endif // PID_H

middleware/src/pid.c

@@ -0,0 +1,67 @@
+#include "pid.h"
+
+pid_t *pid_init(float kP, float kI, float kD, float err_tol, float min_out, float max_out)
+{
+    pid_t *pid = malloc(sizeof(pid_t));
+
+    pid->kP = kP;
+    pid->kI = kI;
+    pid->kD = kD;
+    pid->err_tol = err_tol;
+    pid->err = 0;
+    pid->prev_err = 0;
+    pid->min_out = min_out;
+    pid->max_out = max_out;
+    pid->prev_time = (float)HAL_GetTick() / 1000; // convert ms to s
+    pid->integral_sum = 0;
+
+    return pid;
+}
+
+float pid_calculate(pid_t *pid, float setpoint, float measurement)
+{
+    float output = 0;
+    float period = 0;
+
+    // set error
+    pid->err = setpoint - measurement;
+
+    if (pid->kI > 0 || pid->kD > 0)
+    {
+        float curr_time = (float)HAL_GetTick() / 1000;
+        period = curr_time - pid->prev_time;
+        pid->prev_time = curr_time;
+    }
+
+    // proportional
+    if (pid->kP > 0)
+    {
+        output += pid->kP * pid->err;
+    }
+
+    // + integral
+    if (pid->kI > 0)
+    {
+        // reimann sum
+        pid->integral_sum = pid->integral_sum + pid->err * period;
+        output += pid->kI * pid->integral_sum;
+    }
+
+    // + derivative
+    if (pid->kD > 0)
+    {
+        // change in error / change in time
+        float derivative = (pid->err - pid->prev_err) / period;
+        output += pid->kD * derivative;
+        pid->prev_err = pid->err;
+    }
+
+    // clamped to min_out and max_out
+    return output < pid->min_out ? pid->min_out : output > pid->max_out ? pid->max_out : output;
+}
+
+bool pid_at_setpoint(pid_t *pid)
+{
+    // not using abs function since it converts to int
+    return (pid->err < 0 ? pid->err * -1.0 : pid->err) < pid->err_tol;
+}