diff --git a/src/main.rs b/src/main.rs
index ee3eecc..142065b 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -53,10 +53,15 @@ const HYSTERESIS: u16 = 10;
 
 const PEAK_TEMPERATURE: u16 = 240;
 
+const POWER_FACTOR: u16 = 10;
+const POWER_MAX: u16 = 1000;
+
 const PREHEAT_TEMPERATURE: u16 = 150;
 
 const SOAK_TIME: MicrosDurationU64 = MicrosDurationU64::secs(90);
 
+const TEMPERATURE_BUFF: u16 = 10;
+
 enum State<'a> {
     Off,
     Preheat,
@@ -65,6 +70,14 @@ enum State<'a> {
     Cooling,
 }
 
+fn clamp(value: u16, max: u16) -> u16 {
+    if value > max {
+        max
+    } else {
+        value
+    }
+}
+
 fn print_serial<'a, B: UsbBusTrait>(
     serial: &mut SerialPort<'a, B>,
     msg: &str,
@@ -231,11 +244,12 @@ fn main() -> ! {
     button_cooldown.start(0.secs());
 
     let mut counter = 0;
-    let mut current_temperature = Option::<u16>::None;
     let mut state = State::Off;
+    let mut heating = false;
+    let mut power = 0;
     loop {
         delay.delay_ms(5);
-        counter = (counter + 1) % 100;
+        counter = (counter + 1) % 2000;
 
         if let Ok(_) = button_cooldown.wait() {
             let button_pressed = button.is_low().unwrap();
@@ -254,14 +268,58 @@ fn main() -> ! {
             button_held = button_pressed;
         }
 
-        if counter == 0 {
-            led.toggle().unwrap();
+        if counter % 50 == 0 {
+            let thermocouple_result = thermocouple.read_temperature(&mut timer.count_down());
+
+            if let Some(mut temperature) = thermocouple_result.as_ref().copied().ok() {
+                temperature = (temperature >> 2) + ((temperature & 2) >> 1);
+
+                match state {
+                    State::Off => (),
+                    State::Preheat => {
+                        if temperature < PREHEAT_TEMPERATURE {
+                            heating = true;
+                            power = clamp((PREHEAT_TEMPERATURE - temperature + TEMPERATURE_BUFF) * POWER_FACTOR, POWER_MAX);
+                        } else {
+                            let mut time = timer.count_down();
+                            time.start(SOAK_TIME);
+                            state = State::Soak(time);
+                        }
+                    }
+                    State::Soak(ref mut time) => match time.wait() {
+                        Ok(_) => state = State::Reflow,
+                        Err(WouldBlock) => {
+                            if temperature < PREHEAT_TEMPERATURE - HYSTERESIS {
+                                heating = true;
+                                power = clamp((PREHEAT_TEMPERATURE - temperature + TEMPERATURE_BUFF) * POWER_FACTOR, POWER_MAX);
+                            } else if temperature > PREHEAT_TEMPERATURE + HYSTERESIS * 2 {
+                                state = State::Reflow;
+                            } else if temperature > PREHEAT_TEMPERATURE {
+                                heating = false;
+                            }
+                        }
+                        Err(_) => state = State::Off,
+                    },
+                    State::Reflow => {
+                        if temperature < PEAK_TEMPERATURE {
+                            heating = true;
+                            power = clamp((PEAK_TEMPERATURE - temperature + TEMPERATURE_BUFF) * POWER_FACTOR, POWER_MAX);
+                        } else {
+                            heating = false;
+
+                            state = State::Cooling;
+                        }
+                    }
+                    State::Cooling => state = State::Off,
+                }
+            } else {
+                state = State::Off;
+            }
 
             let mut bytes: [u8; 7] = [32, 32, 32, 32, 194, 176, 67];
-            let text = match thermocouple.read_temperature(&mut timer.count_down()) {
+            let text = match thermocouple_result {
                 Ok(mut temperature) => {
-                    temperature >>= 2;
-                    current_temperature = Some(temperature);
+                    temperature = (temperature >> 2) + ((temperature & 2) >> 1);
 
                     for i in (0..3).rev() {
                         bytes[i] = 48u8 + (temperature % 10) as u8;
@@ -303,48 +361,23 @@ fn main() -> ! {
             }
         }
 
-        if let Some(temperature) = current_temperature {
-            match state {
-                State::Off => (),
-                State::Preheat => {
-                    if temperature < PREHEAT_TEMPERATURE {
-                        relay.set_high().unwrap();
-                    } else {
-                        let mut time = timer.count_down();
-                        time.start(SOAK_TIME);
-                        state = State::Soak(time);
-                    }
-                }
-                State::Soak(ref mut time) => match time.wait() {
-                    Ok(_) => state = State::Reflow,
-                    Err(WouldBlock) => {
-                        if temperature < PREHEAT_TEMPERATURE - HYSTERESIS {
-                            relay.set_high().unwrap();
-                        } else if temperature > PREHEAT_TEMPERATURE + HYSTERESIS * 2 {
-                            state = State::Reflow;
-                        } else if temperature > PREHEAT_TEMPERATURE {
-                            relay.set_low().unwrap();
-                        }
-                    }
-                    Err(_) => state = State::Off,
-                },
-                State::Reflow => {
-                    if temperature < PEAK_TEMPERATURE {
-                        relay.set_high().unwrap();
-                    } else {
-                        relay.set_low().unwrap();
-
-                        state = State::Cooling;
-                    }
-                }
-                State::Cooling => state = State::Off,
-            }
-        } else {
-            state = State::Off;
-        }
-
         if let State::Off = state {
             relay.set_low().unwrap();
+            heating = false;
+        }
+
+        if heating {
+            if counter == 0 {
+                relay.set_high().unwrap();
+            } else if counter >= power {
+                relay.set_low().unwrap();
+            }
+        } else {
+            relay.set_low().unwrap();
+        }
+
+        if counter % 100 == 0 {
+            led.toggle().unwrap();
         }
 
         if !usb_dev.poll(&mut [&mut serial]) {