add missing files

pid.c

@@ -0,0 +1,32 @@
+#include <util/atomic.h>
+#include "pid.h"
+
+float pid_step(volatile struct pid* controller, float dt, float error) {
+    // Calculate p term
+    float p = error * controller->kP;
+
+    // Calculate i term
+    ATOMIC_BLOCK(ATOMIC_FORCEON){
+        controller->integral += error * dt * controller->kI;
+        if(controller->integral > 80)
+            controller->integral = 80;
+        if(controller->integral < -80)
+            controller->integral = -80;
+    }
+
+    // Calculate d term, taking care to not divide by zero
+    float d = dt == 0 ? 0 : ((error - controller->lastError) / dt) * controller->kD;
+    controller->lastError = error;
+
+    return p + controller->integral + d;
+}
+
+void init_pid(volatile struct pid* controller, float p, float i, float d){
+    *controller = (struct pid){
+        .kP = p,
+        .kI = i,
+        .kD = d,
+        .lastError = 0,
+        .integral = 0
+    };
+}