diff --git a/TeensyPwm.cpp b/TeensyPwm.cpp
index a472c71..b4e7030 100644
--- a/TeensyPwm.cpp
+++ b/TeensyPwm.cpp
@@ -7,15 +7,19 @@
 
 
 const uint8_t PWM_PIN = 23;
+const uint8_t PWM_LOOPBACK = 22;
 const uint32_t PWM_FREQ = 23437;
+// const uint32_t PWM_FREQ = 46875;
 const uint32_t PWM_RES = 10;
-const uint32_t PWM_MIN = 0;
+const uint32_t PWM_MIN = 1;
 const uint32_t PWM_MAX = 1023;
 
-const uint8_t ADC_IN = 22;
+const uint8_t ADC_IN = 21;
 const uint8_t ADC_RES = 10;
 const uint32_t ADC_MIN = 0;
 const uint32_t ADC_MAX = 1023;
+const uint8_t ADC_AVG = 32;
+
 const float R_top = 100000.0;
 const float R_bottom = 6200.0;
 const float U_ref = 3.3;
@@ -29,38 +33,23 @@ const uint8_t LCD_D7 = 7;
 const uint8_t LCD_ROWS = 2;
 const uint8_t LCD_COLS = 16;
 
-const uint32_t CYCLE_TIME = 250; // microseconds
-const uint32_t DISPLAY_UPDATE_TIME = 5e5; // microseconds
 
+const uint32_t CYCLE_DIV = 5;
 
-const float Ctrl_P = 5.0;
-const float Ctrl_I = 2.0;
+const float Ctrl_P = 10.0;
+const float Ctrl_I = 5.0;
 const float Ctrl_D = 0.0;
 
-volatile float U_des = 12.0;
+volatile Control ctrl((float)PWM_MIN, (float)PWM_MAX, Ctrl_P, Ctrl_I, Ctrl_D);
+
+volatile float U_des = 14.0;
 
 
 LiquidCrystal lcd(LCD_RS, LCD_E, LCD_D4, LCD_D5, LCD_D6, LCD_D7);
-volatile Control ctrl((float)PWM_MIN, (float)PWM_MAX, Ctrl_P, Ctrl_I, Ctrl_D);
+const uint32_t DISPLAY_UPDATE_TIME = 5e5; // microseconds
 
 
 
-void setup() {
-	lcd.begin(LCD_COLS, LCD_ROWS);
-	lcd.print("Teensy SMPS");
-	pinMode(PWM_PIN, OUTPUT);
-	analogWrite(PWM_PIN, 0);
-	analogWriteFrequency(PWM_PIN, PWM_FREQ);
-	analogWriteResolution(PWM_RES);
-  attachInterrupt(PWM_PIN, cycle, RISING);
-
-	analogReadResolution(ADC_RES);
-	analogReference(DEFAULT);
-	// analogReadAveraging(16);
-	pinMode(ADC_IN, INPUT);
-
-}
-
 
 volatile uint32_t lastCycle = 0;
 volatile float u_curr = 0;
@@ -69,46 +58,77 @@ volatile uint32_t cycleDelay = 0;
 volatile uint32_t maxCycleDelay = 0;
 
 void cycle() {
-	uint32_t currentTime = micros();
-	cycleDelay = currentTime - lastCycle;
-	if (cycleDelay > maxCycleDelay) {
-		maxCycleDelay = cycleDelay;
+	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);
 	}
-	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;
+	cycles++;
+}
 
-	float newPwm_f = ctrl.cycle(U_des, u_curr);
-	newPwm = (uint16_t) newPwm_f;
+void setup() {
+	lcd.begin(LCD_COLS, LCD_ROWS);
+	lcd.print("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);
 
-	analogWrite(PWM_PIN, newPwm);
 }
 
 void loop() {
-
-
 	static uint32_t lastDisplayCycle = 0;
 	static uint32_t cycleCnt = 0;
-	currentTime = micros();
+	uint32_t currentTime = micros();
 	if ((lastDisplayCycle + DISPLAY_UPDATE_TIME <= currentTime) || (lastDisplayCycle > currentTime)) {
 		lastDisplayCycle = currentTime;
 		cycleCnt++;
 		if (cycleCnt == 60) {
 			maxCycleDelay = 0;
 		}
+
+		noInterrupts();
+		float my_u_curr = u_curr;
+		uint16_t my_newPwm = newPwm;
+		uint32_t my_cycleDelay = cycleDelay;
+		interrupts();
+
 		lcd.clear();
 		lcd.setCursor(0, 0);
 		lcd.print(U_des);
 		lcd.setCursor(8, 0);
-		lcd.print(u_curr);
+		lcd.print(my_u_curr);
 		lcd.setCursor(0, 1);
 		lcd.print(maxCycleDelay);
-		float dutyCycle = ((float)newPwm) / ((float)PWM_MAX) * 100.0;
-		lcd.setCursor(8, 1);
-//		lcd.print(dutyCycle);
-//		lcd.print("%");
-    lcd.print(cycleDelay);
+		float dutyCycle = ((float)my_newPwm) / ((float)PWM_MAX) * 100.0;
+		lcd.print(" ");
+		lcd.print(my_cycleDelay);
+		lcd.print(" ");
+		lcd.print(dutyCycle);
+		lcd.print("%");
 
 	}
 }
diff --git a/control.cpp b/control.cpp
index bdbdd92..49db893 100644
--- a/control.cpp
+++ b/control.cpp
@@ -37,7 +37,7 @@ Control::Control(float p_rMin, float p_rMax, float p_kP, float p_kI, float p_kD)
 }
 
 
-float Control::cycle(float vDes, float vCur) {
+float Control::cycle(float vDes, float vCur) volatile {
   float e = vDes - vCur;
 
   float rMot = m_rOld +
diff --git a/control.h b/control.h
index fd7e9c4..1c0eafb 100644
--- a/control.h
+++ b/control.h
@@ -15,7 +15,7 @@
 class Control {
 public:
 	Control(float p_rMin, float p_rMax, float p_kP, float p_kI, float p_kD);
-	float cycle(float vDes, float vCur);
+	float cycle(float vDes, float vCur) volatile;
 private:
 	float m_rOld;
 	float m_eOld;