diff --git a/src/main.c b/src/main.c
--- a/src/main.c
+++ b/src/main.c
@@ -30,7 +30,7 @@ int main(void)
 	// Startup screen
     display_startup_screen();
     HAL_Delay(2000);
-
+    ssd1306_clearscreen();
 	ssd1306_drawlogo();
 
     // Default status
@@ -50,6 +50,7 @@ int main(void)
     uint32_t last_pid = 0;
     uint32_t last_thermostat = 0;
     uint32_t last_1hz = 0;
+    uint32_t last_5hz = 0;
 
 	while (1)
 	{
@@ -61,10 +62,17 @@ int main(void)
 			last_1hz = HAL_GetTick();
 		}
 
+		if(HAL_GetTick() - last_5hz > 200)
+		{
+			tempsense_readtemp();
+//			runtime_status = tempsense_gettemp();
+			last_5hz = HAL_GetTick();
+		}
+
         if(flash_getsettings()->val.control_mode == MODE_PID && (HAL_GetTick() - last_pid > PID_PERIOD))
         {
 //        	runtime_status()->temp = tempsense_readtemp();
-//        	duty = pid_process();
+        	duty = pid_process();
             last_pid = HAL_GetTick();
         }
 
@@ -72,7 +80,7 @@ int main(void)
         if(flash_getsettings()->val.control_mode == MODE_THERMOSTAT && HAL_GetTick() - last_thermostat > SSR_PERIOD)
         {
 //        	runtime_status()->temp = tempsense_readtemp();
-//        	duty = thermostat_process();
+        	duty = thermostat_process();
             last_thermostat = HAL_GetTick();
         }