diff --git a/buffer.c b/buffer.c
new file mode 100644
index 0000000..5fd59fe
--- /dev/null
+++ b/buffer.c
@@ -0,0 +1,42 @@
+#include <util/atomic.h>
+
+#include "buffer.h"
+
+void insert_to_buffer(uint16_t val, volatile buffer_t* buf){
+    ATOMIC_BLOCK(ATOMIC_FORCEON){
+        buf->position++;
+        if(buf->position == BUFFER_SIZE)
+            buf->position = 0;
+        buf->values[buf->position] = val;
+    }
+}
+
+float get_buffer_mean(volatile buffer_t* buf){
+
+    ///* discard lowest and highest value */
+    //uint16_t low=0xFFFF;
+    //uint16_t high=0;
+    //uint16_t index_l = 0;
+    //uint16_t index_h = 0;
+    //for(uint16_t i=0; i<BUFFER_SIZE; i++){
+    //    if(buf->values[i] < low){
+    //        low=buf->values[i];
+    //        index_l = i;
+    //    }
+    //    if(buf->values[i] > high){
+    //        high=buf->values[i];
+    //        index_h = i;
+    //    }
+    //}
+        
+
+    uint32_t sum = 0;
+    for(uint16_t i=0; i<BUFFER_SIZE; i++){
+        //if(i == index_h || i == index_l)
+        //    continue;
+        sum += buf->values[i];
+    }
+
+    uint16_t res = sum/(BUFFER_SIZE/*-2*/);
+    return res;
+}
diff --git a/buffer.h b/buffer.h
new file mode 100644
index 0000000..5ba5852
--- /dev/null
+++ b/buffer.h
@@ -0,0 +1,11 @@
+#include <stdint.h>
+
+#define BUFFER_SIZE 200
+
+typedef struct {
+    uint16_t values[BUFFER_SIZE];
+    uint16_t position;
+} buffer_t;
+
+void insert_to_buffer(uint16_t val, volatile buffer_t* buf);
+float get_buffer_mean(volatile buffer_t* buf);
diff --git a/pid.c b/pid.c
new file mode 100644
index 0000000..47405d2
--- /dev/null
+++ b/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
+    };
+}
diff --git a/pid.h b/pid.h
new file mode 100644
index 0000000..d7e0580
--- /dev/null
+++ b/pid.h
@@ -0,0 +1,14 @@
+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);
+void init_pid(volatile struct pid* controller, float p, float i, float d);
+