From 3f0be672f9c5a07a98be0dc703b95f1bbe73f33e Mon Sep 17 00:00:00 2001 From: nathansmithsmith Date: Sun, 9 Jul 2023 00:53:10 -0600 Subject: Mouse control working --- src/PID.c | 51 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 51 insertions(+) create mode 100644 src/PID.c (limited to 'src/PID.c') diff --git a/src/PID.c b/src/PID.c new file mode 100644 index 0000000..edecd5e --- /dev/null +++ b/src/PID.c @@ -0,0 +1,51 @@ +#include "PID.h" + +PID createPID(PIDConfig config) { + PID pid = (PID){ + .p = 0.0, + .i = 0.0, + .d = 0.0, + .kP = config.kP, + .kI = config.kI, + .kD = config.kD, + .doClamp = config.doClamp, + .min = config.min, + .max = config.max, + .error = 0.0, + .pastError = 0.0, + .output = 0.0 + }; + + return pid; +} + +float runPID(float setpoint, float processValue, PID * pid) { + // Get error. + pid->error = setpoint - processValue; + + // Set p, i and d. + pid->p = pid->error * pid->kP; + pid->i += pid->error * pid->kI; + pid->d = (pid->error - pid->pastError) * pid->kD; + + // Update error. + pid->pastError = pid->error; + + // Set output. + pid->output = pid->p + pid->i + pid->d; + + // Clamp. + if (pid->doClamp) + pid->output = Clamp(pid->output, pid->min, pid->max); + + return pid->output; +} + +void resetPID(PID * pid) { + pid->p = 0.0; + pid->i = 0.0; + pid->d = 0.0; + pid->error = 0.0; + pid->pastError = 0.0; + pid->output = 0.0; +} -- cgit v1.2.3