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 04bc410..26bddca 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
@@ -119,10 +119,10 @@ void setup() {
   pinMode(GADJ_R1, INPUT);        // declare input to set high impedance
   pinMode(GADJ_R2, INPUT);        // declare input to set high impedance
   pinMode(GADJ_R3, INPUT);        // declare input to set high impedance
-  Serial.begin(9600);
 
   attachInterrupt(digitalPinToInterrupt(Z_TRG), pulse, FALLING);
 
+  Serial.begin(9600);
   Serial.println("Initializing Pyr0-Piezo Sensor...");
 
   restoreConfig();
@@ -138,8 +138,8 @@ void loop() {
     // Blink LED's on init
     if (BlinkCount > 0) {
       BlinkState = !BlinkState;
-      digitalWrite(ERR_LED, BlinkState);
-      digitalWrite(TRG_OUT, BlinkState);
+      digitalWriteFast(ERR_LED, BlinkState);
+      digitalWriteFast(TRG_OUT, BlinkState);
       --BlinkCount;
     }
 
@@ -156,8 +156,8 @@ void loop() {
 
     // Check voltage of first and second stages and compare against thresholds
     readVin();
-    VComp = analogRead(VCOMP_SENSE_PIN);
-    VFol = analogRead(V_FOLLOW_PIN);
+    VComp = analogReadFast(VCOMP_SENSE_PIN);
+    VFol = analogReadFast(V_FOLLOW_PIN);
 
     VLast = VOld - Vin;
     if (VLast > Hyst || VLast < -Hyst) {
@@ -174,8 +174,8 @@ void loop() {
     // Blink LED's on init
     if (BlinkCount > 0) {
       BlinkState = !BlinkState;
-      digitalWrite(ERR_LED, BlinkState);
-      digitalWrite(TRG_OUT, BlinkState);
+      digitalWriteFast(ERR_LED, BlinkState);
+      digitalWriteFast(TRG_OUT, BlinkState);
       --BlinkCount;
     } else {
       // Check for error state
@@ -187,7 +187,7 @@ void loop() {
       serialPrintState();
     }
     // Sets trigger output state to false after completing loop
-    //digitalWrite(TRG_OUT, HIGH);
+    //digitalWriteFast(TRG_OUT, HIGH);
     sensorHReading = 0;
   }
 }
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 f10701e..4395428 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
@@ -12,6 +12,14 @@ void digitalWriteFast(uint8_t pin, uint8_t x) {
   }
 }
 
+int inline analogReadFast(byte ADCpin)
+{ byte ADCSRAoriginal = ADCSRA;
+  ADCSRA = (ADCSRA & B11111000) | 4;
+  int adc = analogRead(ADCpin);
+  ADCSRA = ADCSRAoriginal;
+  return adc;
+}
+
 /*------------------------------------------------*/
 
 void pulse() {
@@ -120,25 +128,25 @@ void adjustGain() {
   }
   else if (GAIN_FACTOR > 0) {
     pinMode(GADJ_R3, OUTPUT);
-    digitalWrite(GADJ_R3, LOW);
+    digitalWriteFast(GADJ_R3, LOW);
     pinMode(GADJ_R2, INPUT);
     pinMode(GADJ_R1, INPUT);
     pinMode(GADJ_R0, INPUT);
   }
   else if (GAIN_FACTOR > 1) {
     pinMode(GADJ_R2, OUTPUT);
-    digitalWrite(GADJ_R2, LOW);
+    digitalWriteFast(GADJ_R2, LOW);
     pinMode(GADJ_R1, INPUT);
     pinMode(GADJ_R0, INPUT);
   }
   else if (GAIN_FACTOR > 2) {
     pinMode(GADJ_R1, OUTPUT);
-    digitalWrite(GADJ_R1, LOW);
+    digitalWriteFast(GADJ_R1, LOW);
     pinMode(GADJ_R0, INPUT);
   }
   else if (GAIN_FACTOR > 3) {
     pinMode(GADJ_R0, OUTPUT);
-    digitalWrite(GADJ_R0, LOW);
+    digitalWriteFast(GADJ_R0, LOW);
   }
 }
 
@@ -146,11 +154,11 @@ void adjustGain() {
 
 void checkError () {
   if (ERR_STATE == 1) {
-    digitalWrite(ERR_LED, BlinkState);
+    digitalWriteFast(ERR_LED, BlinkState);
     BlinkState = !BlinkState;
   }
   else if (ERR_STATE == 0) {
     BlinkState = LOW;
-    digitalWrite(ERR_LED, BlinkState);
+    digitalWriteFast(ERR_LED, BlinkState);
   }
 }