Mouse control working

1 files changed, 51 insertions, 0 deletions

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;
+}