From ed1704f9a110c9fa909dccb3169bf388f48279e4 Mon Sep 17 00:00:00 2001
From: nathansmithsmith <thenathansmithsmith@gmail.com>
Date: Sun, 16 Jul 2023 18:57:08 -0600
Subject: Chatgpt saved my ass again
---
src/PID.c | 16 +++++++++++++++-
1 file changed, 15 insertions(+), 1 deletion(-)
(limited to 'src/PID.c')
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;
+}
--
cgit v1.2.3