diff --git a/Makefile b/Makefile
--- a/Makefile
+++ b/Makefile
@@ -10,7 +10,7 @@
 
 
 # SOURCES: list of sources in the user application
-SOURCES = main.c system/usbd_conf.c system/usbd_cdc_if.c system/usb_device.c system/usbd_desc.c system/interrupts.c system/system_stm32f0xx.c gpio.c spi.c ssd1306.c stringhelpers.c display.c system/syslib.c storage.c flash.c max31855.c max31865.c
+SOURCES = main.c system/usbd_conf.c system/usbd_cdc_if.c system/usb_device.c system/usbd_desc.c system/interrupts.c system/system_stm32f0xx.c gpio.c spi.c ssd1306.c stringhelpers.c display.c system/syslib.c storage.c flash.c max31855.c max31865.c pid.c
 #SRC = $(shell find . -name *.c)
 
 # TARGET: name of the user application
diff --git a/main.c b/main.c
--- a/main.c
+++ b/main.c
@@ -2,6 +2,7 @@
 
 #include "config.h"
 #include "syslib.h"
+#include "pid.h"
 #include "states.h"
 #include "ssd1306.h"
 #include "max31855.h"
@@ -22,9 +23,6 @@ void process();
 therm_settings_t set;
 therm_status_t status;
 
-// Globalish setting vars
-static __IO uint32_t TimingDelay;
-
 int main(void)
 {
     // Initialize HAL
@@ -94,154 +92,91 @@ int main(void)
 
     HAL_Delay(1500);
     ssd1306_clearscreen();
- 
+
+    // Soft timers
+    uint32_t last_ssr_on = 0;
+    uint32_t last_vcp_tx = 0;
+    uint32_t last_led = 0;
+    uint32_t last_pid = 0;
+    int16_t ssr_output = 0; // Duty cycle of ssr, 0 to SSR_PERIOD 
+
     // Main loop
     while(1)
     {
         // Process sensor inputs
-        process();
+
+        if(HAL_GetTick() - last_led > 400) 
+        {
+            last_led = HAL_GetTick();
+        }
+
+        if((HAL_GetTick() - last_pid > PID_PERIOD))
+        {
+            #ifdef MAX31855_TC_SENSOR
+            max31855_readtemp(spi_get(), &set, &status); // Read MAX31855
+            #endif
+
+            #ifdef MAX31865_RTD_SENSOR
+            max31865_readtemp(&set, &status);
+            #endif
+
+            HAL_GPIO_TogglePin(LED_POWER);
+
+            if(status.pid_enabled) 
+            {
+                // Get ssr output for next time
+                int16_t power_percent = pid_update(set.val.k_p, set.val.k_i, set.val.k_d, status.temp, status.temp_frac, status.setpoint, &set, &status);
+                //power-percent is 0-1000
+                ssr_output = power_percent; //(((uint32_t)SSR_PERIOD * (uint32_t)10 * (uint32_t)100) * power_percent) / (uint32_t)1000000;
+            }
+            else 
+            {
+                ssr_output = 0;
+            }
+
+            last_pid = HAL_GetTick();
+        }
+
+        // Every 200ms, set the SSR on unless output is 0
+        if(HAL_GetTick() - last_ssr_on > SSR_PERIOD)
+        {
+
+            // Only support heating (ssr_output > 0) right now
+            if(ssr_output > 0) {
+
+                char tempstr[6];
+                itoa(ssr_output, tempstr, 10);
+                ssd1306_DrawString(tempstr, 0, 90);
+
+                HAL_GPIO_WritePin(SSR_PIN, 1);
+                last_ssr_on = HAL_GetTick();
+            }
+        }
+        
+        // Kill SSR after elapsed period less than SSR_PERIOD 
+        if(HAL_GetTick() - last_ssr_on > ssr_output || ssr_output == 0)
+        {
+            HAL_GPIO_WritePin(SSR_PIN, 0);
+        }
+
+        if(HAL_GetTick() - last_vcp_tx > VCP_TX_FREQ)
+        {
+            // Print temp to cdc
+            char tempstr[16];
+            itoa_fp(status.temp, status.temp_frac, tempstr);
+            uint8_t numlen = strlen(tempstr);
+            tempstr[numlen] = '\r';
+            tempstr[numlen+1] = '\n';
+
+    //        if(set.val.usb_plugged)
+    //            CDC_Transmit_FS(tempstr, numlen+2);
+           // while(CDC_Transmit_FS("\r\n", 2) == USBD_BUSY);
+
+            last_vcp_tx = HAL_GetTick();
+        }
 
         // Run state machine
         display_process(&set, &status); 
     }
-
 }
-
-// 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.
-int16_t last_pid_temp = 0;
-uint8_t last_pid_temp_frac = 0;
-int32_t i_state = 0;
-
-int16_t update_pid(uint16_t k_p, uint16_t k_i, uint16_t k_d, int16_t temp, uint8_t temp_frac, int16_t setpoint) 
-{
-  // 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;
-}
-
-
-uint32_t last_ssr_on = 0;
-uint32_t last_vcp_tx = 0;
-uint32_t last_led = 0;
-uint32_t last_pid = 0;
-int16_t ssr_output = 0; // Duty cycle of ssr, 0 to SSR_PERIOD 
-
-// Turn SSR output on/off according to set duty cycle.
-// TODO: Eventually maybe replace with a very slow timer or something. Double-check this code...
-void process()
-{
-
-    uint32_t ticks = HAL_GetTick();
-
-    if(ticks - last_led > 400) 
-    {
-        last_led = ticks;
-    }
-
-    if((ticks - last_pid > PID_PERIOD))
-    {
-        #ifdef MAX31855_TC_SENSOR
-        max31855_readtemp(spi_get(), &set, &status); // Read MAX31855
-        #endif
-
-        #ifdef MAX31865_RTD_SENSOR
-        max31865_readtemp(&set, &status);
-        #endif
-
-    HAL_GPIO_TogglePin(LED_POWER);
-
-        if(status.pid_enabled) 
-        {
-            // Get ssr output for next time
-            int16_t power_percent = update_pid(set.val.k_p, set.val.k_i, set.val.k_d, status.temp, status.temp_frac, status.setpoint);
-            //power-percent is 0-1000
-            ssr_output = power_percent; //(((uint32_t)SSR_PERIOD * (uint32_t)10 * (uint32_t)100) * power_percent) / (uint32_t)1000000;
-        }
-        else 
-        {
-            ssr_output = 0;
-        }
-
-        last_pid = ticks;
-    }
-
-    // Every 200ms, set the SSR on unless output is 0
-    if((ticks - last_ssr_on > SSR_PERIOD))
-    {
-
-        // Only support heating (ssr_output > 0) right now
-        if(ssr_output > 0) {
-
-            char tempstr[6];
-            itoa(ssr_output, tempstr, 10);
-            ssd1306_DrawString(tempstr, 0, 90);
-
-            HAL_GPIO_WritePin(SSR_PIN, 1);
-            last_ssr_on = ticks;
-        }
-    }
-    
-    // Kill SSR after elapsed period less than SSR_PERIOD 
-    if(ticks - last_ssr_on > ssr_output || ssr_output == 0)
-    {
-        HAL_GPIO_WritePin(SSR_PIN, 0);
-    }
-
-    if(ticks - last_vcp_tx > VCP_TX_FREQ)
-    {
-        // Print temp to cdc
-        char tempstr[16];
-        itoa_fp(status.temp, status.temp_frac, tempstr);
-        uint8_t numlen = strlen(tempstr);
-        tempstr[numlen] = '\r';
-        tempstr[numlen+1] = '\n';
-
-//        if(set.val.usb_plugged)
-//            CDC_Transmit_FS(tempstr, numlen+2);
-       // while(CDC_Transmit_FS("\r\n", 2) == USBD_BUSY);
-
-        last_vcp_tx = ticks;
-    }
-}
-
 // vim:softtabstop=4 shiftwidth=4 expandtab