diff --git a/src/main.c b/src/main.c
--- a/src/main.c
+++ b/src/main.c
@@ -6,61 +6,106 @@
 
 #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.h"
-#include "dma.h"
-#include "interrupts.h"
-#include "buttons.h"
+#include "ssd1306/ssd1306.h"
+#include "pwmout.h"
+
 
 int main(void)
 {
 	sysclock_init();
 	hal_init();
 	gpio_init();
+
 	ssd1306_init();
-	ssd1306_clearscreen();
+
+	// Startup screen
+    display_startup_screen();
+    HAL_Delay(2000);
+    ssd1306_clearscreen();
 	ssd1306_drawlogo();
-	HAL_Delay(2000);
-	flash_init();
-	watchdog_init();
 
-	//just some example code for getting flash values
-//	flash_getsettings()->values.can_id = 67;
-//
-//	if(flash_getsettings()->values.can_id == 12);
+    // 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;
 
-//	ssd1306_drawstring(const char *dataPtr, unsigned char row, unsigned char xPos)
-//	ssd1306_drawstring("[ ProtoFuse ]", 0, 0);
+    pid_init();
+    pwmout_init();
+    flash_init();
+	watchdog_init();
+	tempsense_init();
 
-	float temp_counter = 0;
-
-	// Software timers
-	uint32_t last_screen_update_time = HAL_GetTick();
-
+	// Soft timers
+    uint32_t last_pid = 0;
+    uint32_t last_thermostat = 0;
+    uint32_t last_1hz = 0;
+    uint32_t last_5hz = 0;
 
 	while (1)
 	{
-		// function that checks all the buttons
-		freaking_debounce();
+		float duty = 0.0;
 
-		// Grab and transmit data
-		if(HAL_GetTick() - last_screen_update_time > 100)
+		if(HAL_GetTick() - last_1hz > 750)
 		{
-			HAL_GPIO_TogglePin(LED_GPIO_Port, LED_PIN);
-			char buffer[256];
-			snprintf(buffer, 256, "Counter: %.1f", temp_counter);
-			ssd1306_drawstring(buffer, 3, 45);
-			ssd1306_drawstring("Therm 0.4", 0, 40);
-			temp_counter = temp_counter + 0.1;
-			last_screen_update_time = HAL_GetTick();
+			display_1hz();
+			last_1hz = HAL_GetTick();
+		}
+
+		if(HAL_GetTick() - last_5hz > 200)
+		{
+			tempsense_readtemp();
+//			runtime_status = tempsense_gettemp();
+			last_5hz = HAL_GetTick();
 		}
 
-		watchdog_feed();
+        if(flash_getsettings()->val.control_mode == MODE_PID && (HAL_GetTick() - last_pid > PID_PERIOD))
+        {
+//        	runtime_status()->temp = tempsense_readtemp();
+        	duty = pid_process();
+            last_pid = HAL_GetTick();
+        }
+
+        // Thermostatic control
+        if(flash_getsettings()->val.control_mode == MODE_THERMOSTAT && HAL_GetTick() - last_thermostat > SSR_PERIOD)
+        {
+//        	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)
+//        {
+//            // 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();
+//        }
+
 	}
 }