diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/Pyr0_Piezo_Sensor_V2.x.x.cpp b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/Pyr0_Piezo_Sensor_V2.x.x.cpp
index 5c2813b..ac84fbc 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/Pyr0_Piezo_Sensor_V2.x.x.cpp
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/Pyr0_Piezo_Sensor_V2.x.x.cpp
@@ -100,6 +100,16 @@ update the voltMeterConstant variable in pP_config.h with the correct value
 //#define I2C_INPUT true
 
 void setup() {
+  //Setup PWM on voltage follower (PD3)
+  TCCR2A = (1 << COM2B1) | (0 << COM2B0) | (0 << WGM21) | (1 << WGM20);
+  TCCR2B = (0 << WGM22) | (0 << CS22) | (0 << CS21) | (1 << CS20);
+  DDRD |= (1 << DDD3);
+
+  //Setup PWM on comparator (PB1)
+  TCCR1A = (1 << COM1A1) | (0 << COM1A0) | (1 << WGM11) | (1 << WGM10);
+  TCCR1B = (0 << WGM13) | (0 << WGM12) | (0 << CS12) | (0 << CS11) | (1 << CS10);
+  DDRB |= (1 << DDB1);
+
   pinMode(TRG_OUT, OUTPUT);       // declare the Trigger as as OUTPUT
   pinMode(ERR_LED, OUTPUT);
   pinMode(Z_TRG, INPUT_PULLUP);   // declare z-sense input with pullup
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_function.h b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_function.h
index 2c0979c..4368748 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_function.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_function.h
@@ -89,15 +89,13 @@ update the voltMeterConstant variable in pP_config.h with the correct value
   ADJ_FOLLOW = (followerInt / 4);
 
   // Analog output (PWM) of duty cycle
-  analogWrite(V_FOL_PWM, ADJ_FOLLOW);
+  OCR2B = ADJ_FOLLOW;
 }
 
 /*------------------------------------------------*/
 
 void adjustComp() {
-  ADJ_COMP = (compInt / 4);
-
-  analogWrite(VCOMP_PWM, ADJ_COMP);
+  OCR1A = compInt;
 }
 
 /*------------------------------------------------*/