diff --git a/main.c b/main.c
--- a/main.c
+++ b/main.c
@@ -49,6 +49,56 @@ char* itoa(int16_t i, char b[]){
     return b;
 }
 
+char* itoa_fp(int16_t i, uint8_t frac, char b[]){
+    char const digit[] = "0123456789";
+
+    // set p to beginning of char array
+    char* p = b;
+
+    // If negative, set current char to '-' and inc, unnegate number
+    if(i<0){
+        *p++ = '-';
+        i *= -1;
+    }
+
+    // Init shifter to numeric value
+    uint16_t shifter = i;
+    uint16_t frac_shifter = frac;
+
+    // Iterate through 10s places, incrementing text pointer as we go
+    do{ 
+        ++p;
+        shifter = shifter/10;
+    }while(shifter);
+    
+    ++p; // increment for decimal point
+
+    do{
+        ++p;
+        frac_shifter = frac_shifter/10;
+    }while(frac_shifter);
+        
+
+    // Null-terminate the string
+    *p = '\0';
+
+    // Go backwards and write out fractional digits 
+    do{ 
+        *--p = digit[frac%10];
+        frac = frac/10;
+    }while(frac);
+
+    *--p = '.'; // insert decimal point
+
+    // Go backwards and write out remaining digits 
+    do{ 
+        *--p = digit[i%10];
+        i = i/10;
+    }while(i);
+    return b;
+}
+
+
 static __IO uint32_t TimingDelay;
 
 // Move to header file
@@ -89,9 +139,7 @@ int main(void)
     ssd1306_Init();
     ssd1306_block_write();
     ssd1306_DrawString("therm 0.1", 0, 40);
-    ssd1306_DrawString("Status: Idle", 2, 40);
-  //  ssd1306_block_write();
-    ssd1306_test();
+
 
     uint8_t toggle = 0;
 
@@ -109,24 +157,12 @@ int main(void)
         // probably just passed the actual port
 
         // TODO: Grab buttonpresses with interrupts
-        uint8_t sw_btn = GPIO_ReadInputDataBit(SW_BTN);
+        //uint8_t sw_btn = GPIO_ReadInputDataBit(SW_BTN);
         //uint8_t sw_up = GPIO_ReadInputDataBit(SW_UP);
         //uint8_t sw_down = GPIO_ReadInputDataBit(SW_DOWN);
         //uint8_t sw_left = GPIO_ReadInputDataBit(SW_LEFT);
         //uint8_t sw_right = GPIO_ReadInputDataBit(SW_RIGHT);
 
-        if(!sw_btn) {
-            GPIO_ToggleBits(LED_STAT);
-            if(!toggle) {
-                GPIO_SetBits(GPIOB, GPIO_Pin_13);
-                toggle = ! toggle;
-            }
-            else  {
-                GPIO_ResetBits(GPIOB, GPIO_Pin_13);
-                toggle = ! toggle;
-            }    
-        }
-
         GPIO_SetBits(LED_POWER);
         Delay(50);
         GPIO_ResetBits(LED_POWER);
@@ -134,16 +170,12 @@ int main(void)
     }
 }
 
-int32_t temp = 0;
-int32_t setpoint = 0;
-int32_t p = 1;
-int32_t i = 1;
-int32_t d = 1;
 
 
-// Process things
-void process()
-{
+// Read temperature and update global temp vars
+int16_t temp = 0;
+uint8_t temp_frac = 0;
+void update_temp() {
     // Assert CS
     GPIO_ResetBits(MAX_CS);
     Delay(1);
@@ -159,19 +191,14 @@ void process()
     else if(temp_pre & 0b0000000000000001) {
         ssd1306_DrawString("TC Fault", 3, 35);
     }
-    else {
-        ssd1306_DrawString("TempSense OK", 3, 35);
-    }
 
     uint8_t sign = temp >> 15;// top bit is sign
 
-
     temp_pre = temp_pre >> 2; // Drop 2 lowest bits
-    uint8_t frac = temp_pre & 0b11; // get fractional part
-    frac *= 25; // each bit is .25 a degree, up to fixed point
+    temp_frac = temp_pre & 0b11; // get fractional part
+    temp_frac *= 25; // each bit is .25 a degree, up to fixed point
     temp_pre = temp_pre >> 2; // Drop 2 fractional bits 
 
-    int16_t temp = 0;
     if(sign) {
         temp = -temp_pre;
     }
@@ -182,22 +209,25 @@ void process()
     // Deassert CS
     Delay(1);
     GPIO_SetBits(MAX_CS);
+}
 
-    if(temp > 0) {
-        GPIO_SetBits(LED_STAT);
-    }
-    char tempstr[9];
-    itoa(temp_pre, tempstr);
-    ssd1306_DrawString("Temp: ", 1, 40);
-    ssd1306_DrawString("    ", 1, 70);
-    ssd1306_DrawString(tempstr, 1, 70);
-    itoa(frac, tempstr);
-    ssd1306_DrawString("  ", 1, 90);
-    ssd1306_DrawString(tempstr, 1, 90);
+
+int32_t setpoint = 0;
+int32_t p = 1;
+int32_t i = 1;
+int32_t d = 1;
+
+
+// Process things
+void process()
+{
+    update_temp(); // Read MAX31855
 
     // TODO: Add calibration offset (linear)
 
     // Perform PID calculations
+    //if(
+    GPIO_SetBits(LED_STAT);
 
     // Write output to SSR
 }
@@ -222,7 +252,8 @@ uint8_t state = STATE_IDLE;
 // State machine
 void machine()
 {
-    
+    uint8_t last_state = state;
+
     switch(state)
     {
         // Idle state
@@ -232,6 +263,13 @@ void machine()
             // [ therm :: idle ]
             ssd1306_DrawString("therm :: idle ", 0, 40);
 
+            ssd1306_drawlogo();
+            char tempstr[9];
+            itoa_fp(temp, temp_frac, tempstr);
+            ssd1306_DrawString("Temp: ", 2, 40);
+            ssd1306_DrawString("    ", 2, 70);
+            ssd1306_DrawString(tempstr, 2, 72);
+
             // Button handler
             if(!GPIO_ReadInputDataBit(SW_BTN)) {
                 state = STATE_SETP;
@@ -376,6 +414,11 @@ void machine()
         } break;
             
     }
+
+    if(last_state != state) {
+        // Clear screen on state change
+        ssd1306_block_write();
+    }
 }