First files added

firmware/Core/Src/pid.c

@@ -0,0 +1,83 @@
+/*
+ * pid.c
+ *
+ *  Created on: Jan 18, 2022
+ *      Author: mcfly
+ */
+
+#include "pid.h"
+
+void PID_Init(PIDController *pid) {
+	//Clear controller values
+	pid->integrator = 0.0f;
+	pid->prevError  = 0.0f;
+
+	pid->differentiator  = 0.0f;
+	pid->prevMeasurement = 0.0f;
+
+	pid->out = 0.0f;
+}
+
+
+float PID_Update(PIDController *pid, float setpoint, float measurement) {
+	// PID = Kp + Ki * 1/s + Kd * s/(s*tau+1)
+
+	// e = error tothe setpoint
+	// p[n] = Kp * e[n]
+	// i[n] = Ki*T/2 * (e[n]+e[n-1]) + i[n-1]
+	// d[n] = 2*Kd/(2*tau+T) * (e[n]-e[n-1]) + (2*tau-T)/(2*tau+T) * d[n-1]
+	// PID = p[n]+i[n]+d[n]
+
+
+	//Error signal
+	float error = setpoint - measurement;
+
+	//Proportional
+	float proportional = pid->Kp * error;
+
+	//Integral
+	pid->integrator += 0.5f * pid->Ki * pid->T * (error + pid->prevError);
+
+	//Anti-wind-up via dynamic integrator clamping
+	float limMinInt, limMaxInt;
+
+	//Compute integrator limits
+	if (pid->limMax > proportional) {
+		limMaxInt = pid->limMax - proportional;
+	} else {
+		limMaxInt = 0.0f;
+	}
+	if (pid->limMin < proportional) {
+		limMinInt = pid->limMin - proportional;
+	} else {
+		limMinInt = 0.0f;
+	}
+
+	//Clamp integrator
+	if (pid->integrator > limMaxInt) {
+		pid->integrator = limMaxInt;
+	} else if (pid->integrator < limMinInt) {
+		pid->integrator = limMinInt;
+	}
+
+	//Derivative (band-limited differentiator)
+	pid->differentiator = (2.0f * pid->Kd * (measurement - pid->prevMeasurement)		//Note: derivative on measurement
+						+ (2.0f * pid->tau - pid->T) * pid->differentiator)
+						/ (2.0f * pid->tau + pid->T);
+
+	//Compute output and apply limits
+	pid->out = proportional + pid->integrator + pid->differentiator;
+	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;
+
+}