diff --git a/main.c b/main.c
--- a/main.c
+++ b/main.c
@@ -83,6 +83,8 @@ int main(void)
     delay(1500);
     ssd1306_clearscreen();
 
+    GPIO_ResetBits(LED_STAT);
+
     // Main loop
     while(1)
     {
@@ -94,11 +96,10 @@ int main(void)
     }
 }
 
-
-
 // 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);
@@ -139,6 +140,61 @@ void update_temp() {
     GPIO_SetBits(MAX_CS);
 }
 
+
+// PID implementation
+int16_t last_pid_temp = 0;
+uint8_t last_pid_temp_frac = 0;
+int16_t i_state = 0;
+#define WINDUP_GUARD_GAIN 100
+
+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 = (int16_t)setpoint - (int16_t)temp; // TODO: Use fixed point fraction
+
+  // Proportional component
+  int16_t p_term = k_p * error;
+
+  // Error accumulator (integrator)
+  i_state += error;
+
+  // to prevent the iTerm getting huge despite 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
+  int16_t windup_guard = WINDUP_GUARD_GAIN / k_i;  
+
+  // Calculate integral term with windup guard 
+  if (i_state > windup_guard) 
+    i_state = windup_guard;
+  else if (i_state < -windup_guard) 
+    i_state = -windup_guard;
+  int16_t i_term = k_i * i_state;
+
+  // Calculate differential term (slope since last iteration)
+  int16_t d_term = (k_d * (temp - last_pid_temp));
+
+  // Save temperature for next iteration
+  last_pid_temp = temp;
+  last_pid_temp_frac = 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 ticks = 0;
 uint32_t last_ssr_on = 0;
 uint32_t last_led = 0;
@@ -146,7 +202,7 @@ int32_t setpoint = 0;
 uint16_t k_p = 1;
 uint16_t k_i = 1;
 uint16_t k_d = 1;
-uint8_t ssr_output = 0; // Duty cycle of ssr, 0 to SSR_PERIOD 
+int16_t ssr_output = 0; // Duty cycle of ssr, 0 to SSR_PERIOD 
 
 
 // Process things
@@ -163,10 +219,25 @@ void process()
     }
 
     // Every 200ms, set the SSR on unless output is 0
-    if((ticks - last_ssr_on > SSR_PERIOD) && ssr_output > 0)
+    if((ticks - last_ssr_on > SSR_PERIOD))
     {
-        GPIO_SetBits(LED_STAT);
-        last_ssr_on = ticks;
+        // Get ssr output for next time
+        int16_t power_percent = update_pid(k_p, k_i, k_d, temp, temp_frac, 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;
+
+        // Only support heating (ssr_output > 0) right now
+        if(ssr_output > 0) {
+
+            char tempstr[6];
+            itoa(ssr_output, tempstr);
+            ssd1306_DrawString("#=", 2, 45);
+            ssd1306_DrawString("    ", 2, 57);
+            ssd1306_DrawString(tempstr, 2, 57);
+
+            GPIO_SetBits(LED_STAT);
+            last_ssr_on = ticks;
+        }
     }
     
     // Kill SSR after elapsed period less than SSR_PERIOD