diff options
Diffstat (limited to 'src/PID.c')
| -rw-r--r-- | src/PID.c | 16 |
1 files changed, 15 insertions, 1 deletions
@@ -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; +} |