diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_function.h b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_function.h
index 67831e9..8aaa458 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_function.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_function.h
@@ -62,8 +62,9 @@ update the voltMeterConstant variable in pP_config.h with the correct value*/
 	 Vin = readVcc(), DEC;
    followerLong = followerThrs * 1023L;
    compLong = compThrs * 1023L;
+   followerInt = (long long) followerLong / Vin;
    compInt = (long long) compLong / Vin;
-   senseInt = (int) senseInt;
+   followerInt = (int) followerInt;
    compInt = (int) compInt;
  }
 
@@ -73,7 +74,7 @@ update the voltMeterConstant variable in pP_config.h with the correct value*/
   /* Compares diffs of threshold vs read value
    if positive, adjusts the follower to within
    the range set above*/
-  ADJ_FOLLOW = (senseInt / 4);
+  ADJ_FOLLOW = (followerInt / 4);
 
   // Analog output (PWM) of duty cycle
   analogWrite(V_FOL_PWM, ADJ_FOLLOW);
@@ -105,7 +106,6 @@ void adjustGain() {
     pinMode(GADJ_R2, INPUT);
     pinMode(GADJ_R1, INPUT);
     pinMode(GADJ_R0, INPUT);
-    ERR_STATE = 0;
   }
   else if (GAIN_FACTOR > 0) {
     pinMode(GADJ_R3, OUTPUT);
@@ -113,25 +113,21 @@ void adjustGain() {
     pinMode(GADJ_R2, INPUT);
     pinMode(GADJ_R1, INPUT);
     pinMode(GADJ_R0, INPUT);
-    ERR_STATE = 0;
   }
   else if (GAIN_FACTOR > 1) {
     pinMode(GADJ_R2, OUTPUT);
     digitalWrite(GADJ_R2, LOW);
     pinMode(GADJ_R1, INPUT);
     pinMode(GADJ_R0, INPUT);
-    ERR_STATE = 0;
   }
   else if (GAIN_FACTOR > 2) {
     pinMode(GADJ_R1, OUTPUT);
     digitalWrite(GADJ_R1, LOW);
     pinMode(GADJ_R0, INPUT);
-    ERR_STATE = 0;
   }
   else if (GAIN_FACTOR > 3) {
     pinMode(GADJ_R0, OUTPUT);
     digitalWrite(GADJ_R0, LOW);
-    ERR_STATE = 0;
   }
 }
 
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_serial.h b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_serial.h
index 1eee7e2..bbb7443 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_serial.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_serial.h
@@ -56,6 +56,7 @@ void identifyMarkers() {
 }
 
 /*------------------------------------------------*/
+
 void updateGainFactor()
 {
   if (serialInt >= 0)
@@ -65,7 +66,7 @@ void updateGainFactor()
     EEPROM.put(GAIN_FACTOR_ADDRESS, GAIN_FACTOR);
   }
 }
-}
+
 /*------------------------------------------------*/
 
 void updateVFol() {
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2/src/pP_config.cpp b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2/src/pP_config.cpp
index 218238c..b2909b4 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2/src/pP_config.cpp
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2/src/pP_config.cpp
@@ -9,10 +9,11 @@ int TRG_DUR = TRG_DUR_DEFAULT;   // duration of the Z-axis pulse sent, in ms
 int Hyst = HYST_DEFAULT;         // Hysteresis value for ADC measurements
 int Debug = 0;
 long voltMeterConstant = 1125300L; // For fine tuning input voltage sense
-// byte pP_i2c_address = 0xa0; // I2C Bus Address
+#ifdef I2C_INPUT
+  byte pP_i2c_address = 0xa0; // I2C Bus Address
+#endif
 
-void resetEEPROM()
-{
+void resetEEPROM() {
     resetConfig();
     EEPROM.put(GAIN_FACTOR_ADDRESS, GAIN_FACTOR);
     EEPROM.put(FOLLOWER_THRESHOLD_ADDRESS, followerThrs);
@@ -23,73 +24,53 @@ void resetEEPROM()
 }
 
 // Restore config from EEPROM, otherwise reset config and write to EEPROM
-void restoreConfig()
-{
+void restoreConfig() {
     int temp;
 
     EEPROM.get(GAIN_FACTOR_ADDRESS, temp);
-    if (temp < 0 || temp > 4)
-    {
+    if (temp < 0 || temp > 4) {
         resetEEPROM();
-    }
-    else
-    {
+    } else {
         GAIN_FACTOR = temp;
     }
 
     EEPROM.get(FOLLOWER_THRESHOLD_ADDRESS, temp);
-    if (temp < 0 || temp > 5000)
-    {
+    if (temp < 0 || temp > 5000) {
         resetEEPROM();
-    }
-    else
-    {
+    } else {
         followerThrs = temp;
     }
 
     EEPROM.get(COMP_THRESHOLD_ADDRESS, temp);
-    if (temp < 0 || temp > 5000)
-    {
+    if (temp < 0 || temp > 5000) {
         resetEEPROM();
-    }
-    else
-    {
+    } else {
         compThrs = temp;
     }
 
     EEPROM.get(LOOP_DUR_ADDRESS, temp);
-    if (temp < 0 && temp > 1000)
-    {
+    if (temp < 0 && temp > 1000) {
         resetEEPROM();
-    }
-    else
-    {
+    } else {
         LOOP_DUR = temp;
     }
 
     EEPROM.get(TRG_DUR_ADDRESS, temp);
-    if (temp < 0 || temp > 1000)
-    {
+    if (temp < 0 || temp > 1000) {
         resetEEPROM();
-    }
-    else
-    {
+    } else {
         TRG_DUR = temp;
     }
 
     EEPROM.get(HYST_ADDRESS, temp);
-    if (temp < 0 || temp > 1000)
-    {
+    if (temp < 0 || temp > 1000) {
         resetEEPROM();
-    }
-    else
-    {
+    } else {
         Hyst = temp;
     }
 }
 
-void resetConfig()
-{
+void resetConfig() {
     GAIN_FACTOR = GAIN_FACTOR_DEFAULT;
     followerThrs = FOLLOWER_THRESHOLD_DEFAULT;
     compThrs = COMP_THRESHOLD_DEFAULT;