Chatgpt saved my ass again

1 files changed, 15 insertions, 1 deletions

diff --git a/src/PID.c b/src/PID.c
index edecd5e..5349358 100644
--- a/src/PID.c
+++ b/src/PID.c
@@ -1,4 +1,5 @@
 #include "PID.h"
+#include "util.h"
 
 PID createPID(PIDConfig config) {
 	PID pid = (PID){
@@ -8,6 +9,7 @@ PID createPID(PIDConfig config) {
 		.kP = config.kP,
 		.kI = config.kI,
 		.kD = config.kD,
+		.angleMode = config.angleMode,
 		.doClamp = config.doClamp,
 		.min = config.min,
 		.max = config.max,
@@ -21,7 +23,10 @@ PID createPID(PIDConfig config) {
 
 float runPID(float setpoint, float processValue, PID * pid) {
 	// Get error.
-	pid->error = setpoint - processValue;
+	if (pid->angleMode)
+		pid->error = angleDis(setpoint, processValue);
+	else
+		pid->error = setpoint - processValue;
 
 	// Set p, i and d.
 	pid->p = pid->error * pid->kP;
@@ -49,3 +54,12 @@ void resetPID(PID * pid) {
 	pid->pastError = 0.0;
 	pid->output = 0.0;
 }
+
+float angleDis(float a, float b) {
+	float dir = b - a;
+
+	if (fabsf(dir) > (PI/2))
+		dir = -(signum(dir) * PI) + dir;
+
+	return dir;
+}