diff --git a/src/main.c b/src/main.c
--- a/src/main.c
+++ b/src/main.c
@@ -6,20 +6,17 @@
 
 #include "stm32f3xx_hal.h"
 #include "config.h"
-
 #include "watchdog.h"
 #include "system.h"
 #include "display.h"
+#include "thermostat.h"
 #include "gpio.h"
+#include "tempsense.h"
 #include "pid.h"
 #include "error.h"
 #include "flash.h"
 #include "ssd1306/ssd1306.h"
-#include "interrupts.h"
-
-
-therm_status_t status;
-pid_state_t pid_state;
+#include "pwmout.h"
 
 
 int main(void)
@@ -28,7 +25,6 @@ int main(void)
 	hal_init();
 	gpio_init();
 
-
 	ssd1306_init();
 
 	// Startup screen
@@ -37,149 +33,46 @@ int main(void)
 
 	ssd1306_drawlogo();
 
-
+    // Default status
+	runtime_status()->temp = 0.0;
+	runtime_status()->state_resume = 0;
+	runtime_status()->state = STATE_IDLE;
+	runtime_status()->setpoint = 70;
+	runtime_status()->pid_enabled = 0;
 
-    // Default status
-    status.temp = 0;
-    status.temp_frac = 0;
-    status.state_resume = 0;
-    status.state = STATE_IDLE;
-    status.setpoint = 70;
-    status.pid_enabled = 0;
-
-    pid_init(&pid_state);
-
+    pid_init();
+    pwmout_init();
     flash_init();
 	watchdog_init();
-
+	tempsense_init();
 
 	// Soft timers
-    uint32_t last_ssr_on = 0;
-    uint32_t last_vcp_tx = 0;
-    uint32_t last_led = 0;
     uint32_t last_pid = 0;
     uint32_t last_thermostat = 0;
-    int16_t ssr_output = 0; // Duty cycle of ssr, 0 to SSR_PERIOD
-    uint8_t thermostat_plant_on = 0;
-
 
 	while (1)
 	{
+		float duty = 0.0;
 
         if(flash_getsettings()->val.control_mode == MODE_PID && (HAL_GetTick() - last_pid > PID_PERIOD))
         {
-
-//            #ifdef MAX31865_RTD_SENSOR
-//            max31865_readtemp(spi_get(), &set, &status);
-//			#else
-//			max31855_readtemp(spi_get(), &set, &status); // Read MAX31855
-//			#endif
-
-
-            if(status.pid_enabled)
-            {
-                // Get ssr output for next time
-                int16_t power_percent = pid_update(flash_getsettings(), &status, &pid_state);
-
-                if(flash_getsettings()->val.plant_type == PLANT_HEATER)
-                    power_percent *= -1;
-
-                //power-percent is 0-1000?
-                ssr_output = power_percent; //(((uint32_t)SSR_PERIOD * (uint32_t)10 * (uint32_t)100) * power_percent) / (uint32_t)1000000;
-
-
-            	// put ssr output on display
-                ssd1306_drawstring("      ", 0, 90); //fixme: this is bad, but I can't get the old digits to clear otherwise
-                char tempstr[6];
-                itoa(ssr_output, tempstr, 10);
-                ssd1306_drawstring(tempstr, 0, 90);
-            }
-            else
-            {
-                ssr_output = 0;
-            }
-
+        	runtime_status()->temp = tempsense_readtemp();
+        	duty = pid_process();
             last_pid = HAL_GetTick();
         }
 
-//        // Kill SSR once the desired on-time has elapsed
-//        if(set.val.control_mode == MODE_PID && (HAL_GetTick() - last_ssr_on > ssr_output || ssr_output <= 0))
-//        {
-//            HAL_GPIO_WritePin(SSR_PIN, 0);
-//            HAL_GPIO_WritePin(LED_POWER, 0);
-//        }
-//
-//
-//        // Every 200ms, set the SSR on unless output is 0
-//        if(set.val.control_mode == MODE_PID && HAL_GetTick() - last_ssr_on > SSR_PERIOD)
-//        {
-//            // Heat or cool, if we need to
-//            if(ssr_output > 0)
-//            {
-//                HAL_GPIO_WritePin(SSR_PIN, 1);
-//                HAL_GPIO_WritePin(LED_POWER, 1);
-//                last_ssr_on = HAL_GetTick();
-//            }
-//            else {
-//                // Make sure everything is off
-//                HAL_GPIO_WritePin(SSR_PIN, 0);
-//                HAL_GPIO_WritePin(LED_POWER, 0);
-//            }
-//
-//        }
-
-
         // Thermostatic control
         if(flash_getsettings()->val.control_mode == MODE_THERMOSTAT && HAL_GetTick() - last_thermostat > SSR_PERIOD)
         {
-
-//            #ifdef MAX31865_RTD_SENSOR
-//            max31865_readtemp(spi_get(), &set, &status);
-//			#else
-//			max31855_readtemp(spi_get(), &set, &status); // Read MAX31855
-//			#endif
-
-            // TODO: Migrate this FxP conversion to the readtemp code or similar
-            int8_t temp_frac = status.temp_frac > 9 ? status.temp_frac / 10 : status.temp_frac;
-            temp_frac = status.temp > 0 ? temp_frac : temp_frac * -1;
-            int32_t temp = (status.temp * 10) + temp_frac;
-
-
-            // EMZ FIXME: This could be way simpler
-            if(flash_getsettings()->val.plant_type == PLANT_HEATER && status.setpoint * 10 < temp - flash_getsettings()->val.hysteresis * 10)
-                thermostat_plant_on = 1;
-            else if(flash_getsettings()->val.plant_type == PLANT_HEATER && status.setpoint * 10 > temp + flash_getsettings()->val.hysteresis * 10)
-                thermostat_plant_on = 0;
-
-            if(flash_getsettings()->val.plant_type == PLANT_COOLER && status.setpoint * 10 > temp + flash_getsettings()->val.hysteresis * 10)
-                thermostat_plant_on = 1;
-            else if(flash_getsettings()->val.plant_type == PLANT_COOLER && status.setpoint * 10 < temp - flash_getsettings()->val.hysteresis * 10)
-                thermostat_plant_on = 0;
-
-            // EMZ: TODO: Refactor to output_enabled or something
-            if(status.pid_enabled && thermostat_plant_on)
-            {
-                // EMZ TODO: functionalize this
-            	// put ssr output on display
-                ssd1306_drawstring("      ", 0, 90); //fixme: this is bad, but I can't get the old digits to clear otherwise
-                char tempstr[6];
-                itoa(ssr_output, tempstr, 10);
-                ssd1306_drawstring(tempstr, 0, 90);
-
-
-
-//                HAL_GPIO_WritePin(SSR_PIN, 1);
-                HAL_GPIO_WritePin(LED, 1);
-            }
-            else
-            {
-//                HAL_GPIO_WritePin(SSR_PIN, 0);
-                HAL_GPIO_WritePin(LED, 0);
-            }
-
+        	runtime_status()->temp = tempsense_readtemp();
+        	duty = thermostat_process();
             last_thermostat = HAL_GetTick();
         }
 
+        pwmout_process(duty);
+        display_process();
+        watchdog_feed();
+
 
 //        // Transmit temperature over USB-CDC on a regular basis
 //        if(HAL_GetTick() - last_vcp_tx > VCP_TX_FREQ)
@@ -198,10 +91,6 @@ int main(void)
 //            last_vcp_tx = HAL_GetTick();
 //        }
 
-        // Run state machine
-        display_process(&status);
-
-		watchdog_feed();
 	}
 }