diff --git a/firmware/cup.c b/firmware/cup.c
index de52544..7a4eb6a 100644
--- a/firmware/cup.c
+++ b/firmware/cup.c
@@ -12,6 +12,8 @@
 
 #define TIMEOUT 255
 
+const uint16_t TARGET_TEMP_MAX = 1520;
+
 uint8_t led_color = 0;
 
 uint8_t setting_timeout = 0;
@@ -23,6 +25,8 @@ uint16_t target = 0;
 
 uint16_t temperature = 0;
 
+uint8_t heat_output = 0;
+
 uint8_t heat_pwm = 0;
 uint8_t heat_on[2] = {0, 0};
 uint8_t heat_off[2] = {0, 0};
@@ -67,13 +71,23 @@ ISR(PCINT0_vect)
 	input_state = new_state;
 }
 
+uint8_t control_output(uint16_t current_temp, uint16_t target_temp)
+{
+	if (current_temp > target_temp)
+	{
+		return 0;
+	} else {
+		return (uint8_t)((uint16_t)((uint32_t)((uint32_t)255*target_temp)/current_temp)/TARGET_TEMP_MAX);
+	}
+}
+
 // Measure: 5V --- 10k --- --- PTY81-121 -- GND
 //                        |
 //                    uC ADC pin (with 1.11V reference)
 uint16_t linearize_temp(uint16_t temp_in)
 {
 	static const uint16_t one_point_one_volt = 65472; //2^16-2^6
-	uint16_t temp_out = ~0; //burning
+	uint16_t temp_out = TARGET_TEMP_MAX; //burning
 	// made from datasheet by:
 	// for i in 677 740 807 877 951 1029 1111 1196 1286 1378 1475 1575 1679 1786 1896 2003 2103 2189; do calc 5*$i/9860*65472/1.11; done
 	static const uint16_t coeffs[] =
@@ -85,7 +99,7 @@ uint16_t linearize_temp(uint16_t temp_in)
 		26232, 100, // 10 celsius
 		28445, 200,
 		30778, 300,
-		33231, 400, // 50 celsius
+		33231, 400, // 40 celsius
 		35773, 500, // ..
 		38465, 600,
 		41216, 700,
@@ -177,6 +191,7 @@ int main(void)
 				adc_sum = 0;
 				adc_pos = 0;
 				temperature = linearize_temp(temperature_adc);
+				heat_output = control_output(temperature, target);
 			}
 		}