diff --git a/pid.c b/pid.c
new file mode 100644
--- /dev/null
+++ b/pid.c
@@ -0,0 +1,56 @@
+#include "stm32f0xx_hal.h"
+#include "states.h"
+
+// PID implementation
+// TODO: Make struct that has the last_temp and i_state in it, pass by ref. Make struct that has other input values maybe.
+static int16_t last_pid_temp = 0;
+static uint8_t last_pid_temp_frac = 0;
+static int32_t i_state = 0;
+
+int16_t pid_update(uint16_t k_p, uint16_t k_i, uint16_t k_d, int16_t temp, uint8_t temp_frac, int16_t setpoint, therm_settings_t* set, therm_status_t* status) 
+{
+  // Calculate instantaneous error
+  int16_t error = setpoint - temp; // TODO: Use fixed point fraction
+
+  // Proportional component
+  int32_t p_term = k_p * error;
+
+  // Error accumulator (integrator)
+  i_state += error;
+
+  // to prevent the iTerm getting huge from lots of 
+  //  error, we use a "windup guard" 
+  // (this happens when the machine is first turned on and
+  // it cant help be cold despite its best efforts)
+  // not necessary, but this makes windup guard values 
+  // relative to the current iGain
+  int32_t windup_guard_res = set->val.windup_guard / k_i;  
+
+  // Calculate integral term with windup guard 
+  if (i_state > windup_guard_res) 
+    i_state = windup_guard_res;
+  else if (i_state < -windup_guard_res) 
+    i_state = -windup_guard_res;
+
+  int32_t i_term = k_i * i_state;
+
+  // Calculate differential term (slope since last iteration)
+  int32_t d_term = (k_d * (status->temp - last_pid_temp));
+
+  // Save temperature for next iteration
+  last_pid_temp = status->temp;
+  last_pid_temp_frac = status->temp_frac;
+
+  int16_t result = p_term + i_term - d_term;
+
+  // Put out tenths of percent, 0-1000. 
+  if(result > 1000)
+    result = 1000;
+  else if(result < -1000)
+    result = -1000;
+
+  // Return feedback
+  return result;
+}
+
+