diff --git a/TeensyPwm.cpp b/TeensyPwm.cpp
index 142a433..1525b11 100644
--- a/TeensyPwm.cpp
+++ b/TeensyPwm.cpp
@@ -3,112 +3,49 @@
 #include <stdint.h>
 #include <LiquidCrystal.h>
 
-#include "control.h"
 #include "hardware.h"
-#include "ctrlParams.h"
 #include "rotary.h"
+#include "pwm.h"
 
 
 
-volatile float u_des = 14.0;
 
 
 
-volatile Control ctrl((float)PWM_MIN, (float)PWM_MAX, Ctrl_P, Ctrl_I, Ctrl_D);
 
 LiquidCrystal lcd(LCD_RS, LCD_E, LCD_D4, LCD_D5, LCD_D6, LCD_D7);
 const uint32_t DISPLAY_UPDATE_TIME = 5e5; // microseconds
 
 
-volatile uint32_t lastCycle = 0;
-volatile float u_curr = 0;
-volatile uint16_t newPwm = 0;
-volatile uint32_t cycleDelay = 0;
-volatile uint32_t maxCycleDelay = 0;
-
-void cycle() {
-	static uint32_t cycles = 0;
-	if (cycles >= CYCLE_DIV) {
-		cycles = 0;
-		uint32_t currentTime = micros();
-		cycleDelay = currentTime - lastCycle;
-		if (cycleDelay > maxCycleDelay) {
-			maxCycleDelay = cycleDelay;
-		}
-		lastCycle = currentTime;
-
-		uint16_t adcIn = analogRead(ADC_IN);
-		float u_adc = ((float)adcIn) * U_ref / ((float)ADC_MAX);
-		u_curr = u_adc * (R_top + R_bottom) / R_bottom;
-
-		float newPwm_f = ctrl.cycle(u_des, u_curr);
-		newPwm = (uint16_t) newPwm_f;
-
-		analogWrite(PWM_PIN, newPwm);
-	}
-	cycles++;
-}
 
 void setup() {
 	Serial.begin(115200);
 	lcd.begin(LCD_COLS, LCD_ROWS);
 	lcd.print("Teensy SMPS");
 	Serial.println("Teensy SMPS");
-	pinMode(PWM_PIN, OUTPUT);
-	analogWrite(PWM_PIN, PWM_MIN);
-	analogWriteFrequency(PWM_PIN, PWM_FREQ);
-	analogWriteResolution(PWM_RES);
-	pinMode(PWM_LOOPBACK, INPUT);
-	attachInterrupt(PWM_LOOPBACK, cycle, RISING);
-
-	analogReadResolution(ADC_RES);
-	analogReference(DEFAULT);
-	if (ADC_AVG != 0) {
-		analogReadAveraging(ADC_AVG);
-	}
-	pinMode(ADC_IN, INPUT);
 
+  pwmInit();
 	rotaryInit();
 }
 
 void loop() {
 	static uint32_t lastDisplayCycle = 0;
-	static uint32_t cycleCnt = 0;
 	uint32_t currentTime = micros();
 	if ((lastDisplayCycle + DISPLAY_UPDATE_TIME <= currentTime) || (lastDisplayCycle > currentTime)) {
 		lastDisplayCycle = currentTime;
-		cycleCnt++;
-		if (cycleCnt == 60) {
-			maxCycleDelay = 0;
-		}
-		Serial.println("Tick");
-
 
 		int rotaryCount = getAndResetRotaryCount();
-		Serial.println(rotaryCount);
-
-		noInterrupts();
-		u_des += rotaryCount;
-		if (u_des < 0) {
-			u_des = 0;
-		}
-		float my_u_curr = u_curr;
-		uint16_t my_newPwm = newPwm;
-		uint32_t my_cycleDelay = cycleDelay;
-		interrupts();
+    if (rotaryCount != 0) {
+      setUDes(getUDes() + rotaryCount);
+    }
 
 		lcd.clear();
 		lcd.setCursor(0, 0);
-		lcd.print(u_des);
+		lcd.print(getUdes());
 		lcd.setCursor(8, 0);
-		lcd.print(my_u_curr);
+		lcd.print(getUCur());
 		lcd.setCursor(0, 1);
-		lcd.print(maxCycleDelay);
-		float dutyCycle = ((float)my_newPwm) / ((float)PWM_MAX) * 100.0;
-		lcd.print(" ");
-		lcd.print(my_cycleDelay);
-		lcd.print(" ");
-		lcd.print(dutyCycle);
+		lcd.print(geDutyCycle());
 		lcd.print("%");
 	}
 }
diff --git a/pwm.cpp b/pwm.cpp
new file mode 100644
index 0000000..9e84817
--- /dev/null
+++ b/pwm.cpp
@@ -0,0 +1,84 @@
+#include <Arduino.h>
+#include <stdint.h>
+
+#include "control.h"
+#include "ctrlParams.h"
+#include "pwm.h"
+#include "hardware.h"
+
+
+
+
+
+
+volatile Control ctrl((float)PWM_MIN, (float)PWM_MAX, Ctrl_P, Ctrl_I, Ctrl_D);
+
+volatile float u_des = 0;
+volatile float u_curr = 0;
+volatile uint16_t newPwm = 0;
+
+float getDutyCycle() {
+  nointerrupts();
+  uint16_t my_newPwm = newPwm;
+  interrupts();
+  float dutyCycle = ((float)my_newPwm) / ((float)PWM_MAX) * 100.0;
+  return dutyCycle;
+}
+
+float getUCur() {
+  nointerrupts();
+  float my_u_curr = u_curr;
+  interrupts(); 
+  return my_u_curr;
+}
+
+float getUDes() {
+  nointerrupts();
+  float my_u_des = u_des;
+  interrupts(); 
+  return my_u_des;
+}
+
+void setUDes(float uDes) {
+  if (uDes < 0) {
+    uDes= 0;
+  }
+  nointerrupts();
+  u_des = uDes;
+  interrupts();
+}
+
+void cycle() {
+	static uint32_t cycles = 0;
+	if (cycles >= CYCLE_DIV) {
+		cycles = 0;
+
+		uint16_t adcIn = analogRead(ADC_IN);
+		float u_adc = ((float)adcIn) * U_ref / ((float)ADC_MAX);
+		u_curr = u_adc * (R_top + R_bottom) / R_bottom;
+
+		float newPwm_f = ctrl.cycle(u_des, u_curr);
+		newPwm = (uint16_t) newPwm_f;
+
+		analogWrite(PWM_PIN, newPwm);
+	}
+	cycles++;
+}
+
+
+void pwmInit() {
+  pinMode(PWM_PIN, OUTPUT);
+	analogWrite(PWM_PIN, PWM_MIN);
+	analogWriteFrequency(PWM_PIN, PWM_FREQ);
+	analogWriteResolution(PWM_RES);
+	pinMode(PWM_LOOPBACK, INPUT);
+	attachInterrupt(PWM_LOOPBACK, cycle, RISING);
+
+	analogReadResolution(ADC_RES);
+	analogReference(DEFAULT);
+	if (ADC_AVG != 0) {
+		analogReadAveraging(ADC_AVG);
+	}
+	pinMode(ADC_IN, INPUT);
+}
+
diff --git a/pwm.h b/pwm.h
new file mode 100644
index 0000000..54de9bb
--- /dev/null
+++ b/pwm.h
@@ -0,0 +1,11 @@
+#ifndef PWM_H_
+#define PWM_H_
+
+void pwmInit();
+float getDutyCycle();
+float getUCur();
+float getUDes();
+void setUDes(float uDes);
+
+
+#endif /* PWM_H_ */