diff --git a/stm-firmware/main.c b/stm-firmware/main.c
index 3a8fdc5..be83450 100644
--- a/stm-firmware/main.c
+++ b/stm-firmware/main.c
@@ -32,7 +32,9 @@
 #include <reflow-controller/systick.h>
 #include <reflow-controller/adc-meas.h>
 #include <reflow-controller/shell.h>
+#include <reflow-controller/pid-controller.h>
 #include <reflow-controller/digio.h>
+#include <reflow-controller/temp-converter.h>
 #include <reflow-controller/rotary-encoder.h>
 #include <stm-periph/stm32-gpio-macros.h>
 #include <stm-periph/clock-enable-manager.h>
@@ -55,8 +57,8 @@ static void setup_nvic_priorities()
 static float pt1000_value;
 static volatile int pt1000_value_status;
 static uint32_t rot;
-
-
+static volatile float pid_out;
+static volatile float current_temperature;
 
 static inline void uart_gpio_config()
 {
@@ -113,6 +115,9 @@ int main()
 	shellmatta_handle_t shell_handle;
 	int uart_receive_status;
 
+	static struct pid_controller pid;
+	uint64_t pid_timestamp = 0;
+
 	setup_nvic_priorities();
 	systick_setup();
 
@@ -134,9 +139,20 @@ int main()
 		f_close(&test_file);
 	}
 
+	pid_init(&pid, 0.1, 0.1, 4.0, 0.0, 100.0, 40.0);
+	pid_zero(&pid);
 
 	while (1) {
 		pt1000_value_status = adc_pt1000_get_current_resistance(&pt1000_value);
+
+		if (pt1000_value_status >= 0) {
+			(void)temp_converter_convert_resistance_to_temp(pt1000_value, (float *)&current_temperature);
+			if ((systick_get_global_tick() - pid_timestamp) >= 250) {
+				pid_out = pid_sample(&pid, 100.0 - current_temperature);
+				pid_timestamp = systick_get_global_tick();
+			}
+		}
+
 		rot = rotary_encoder_get_abs_val();
 		uart_receive_status = uart_receive_data_with_dma(&shell_uart, &uart_input, &uart_input_len);
 		if (uart_receive_status >= 1)