diff --git a/pid.c b/pid.c
new file mode 100644
index 0000000..2274139
--- /dev/null
+++ b/pid.c
@@ -0,0 +1,36 @@
+#include <util/atomic.h>
+#include "pid.h"
+
+extern volatile uint16_t holdingRegisters[10];
+int16_t pid_step(volatile struct pid* controller, float dt, int16_t error) {
+    // Calculate p term
+
+    uint16_t 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;
+    }
+    //holdingRegisters[3]=((float)(error) * controller->kI);
+    //holdingRegisters[4] = error;
+
+    // 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
+    };
+}
diff --git a/pid.h b/pid.h
new file mode 100644
index 0000000..21299e3
--- /dev/null
+++ b/pid.h
@@ -0,0 +1,21 @@
+#ifndef _PID_H_
+#define _PID_H_
+
+#include <stdint.h>
+
+struct pid{
+    // Controller gains
+    float kP;
+    float kI;
+    float kD;
+
+    // State variables
+    float lastError;
+    float integral;
+};
+
+//float pid_step(volatile struct pid* controller, float dt, float error);
+int16_t pid_step(volatile struct pid* controller, float dt, int16_t error);
+void init_pid(volatile struct pid* controller, float p, float i, float d);
+
+#endif