diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/Pyr0_Piezo_Sensor_v2.x.x.ino b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/Pyr0_Piezo_Sensor_v2.x.x.ino
index 3c0143f..163bf69 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/Pyr0_Piezo_Sensor_v2.x.x.ino
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/Pyr0_Piezo_Sensor_v2.x.x.ino
@@ -1,166 +1,156 @@
-/*
-  Piezoelectric Z-Axis sensor using AtMega88/168/328 (AtMega 48 doesnt have enough memory for this version)
-
-  This sketch reads a piezo element to detect a touch of the printer's nozzle to the bed.
-  The sense pin is tied to an interrupt, which is pulled high by internal pullup resistor.
-  When the piezo touches the bed, the amplification circuit will draw the interrupt pin low
-  and the atmega will output a pulse based on the programmed trigger duration
-  
- * PD2 INT0 (Piezo In 'D2')
- * D7 PCINT23 (Trigger OUT 'D7')
- * PC0 ADC0 (Voltage Reference Check 'A0')
- * PC1 ADC1 (Sensitivity Adjustment Check 'A1')
- * PD4 PCINT20 (Error feedback LED 'D4')
- * PB6 PCINT6 (Voltage Adjustment Resistor 0 'D20')
- * PB7 PCINT7 (Voltage Adjustment Resistor 1 'D21')
- * PD5 T1 (Voltage Adjustment Resistor 2 'D5')
- * PD6 PCINT22 (Voltage Adjustment Resistor 3 'D6')
- * PB1 OC1A (Comparator VRef PWM Out 'D9')
- * PD3 OC2B (Voltage Follower VRef PWM Out 'D3')
-
-  Schematics for this project can be found here: https://github.com/pyr0ball/pyr0piezo/tree/master/docs/Schematics
-
-  For Arduino IDE use MCUdude MiniCore: https://mcudude.github.io/MiniCore/package_MCUdude_MiniCore_index.json
-
-
-  created 2/18/2019
-  by Alan "pyr0ball" Weinstock
-
-  This code is in the public domain.
-*/
-
-/* To set the below parameters using serial input, use the following:
-
-To change trigger active duration: TRG_D [integer for milliseconds]
-To change gain factor: GAIN_F [integer for gain state - see note*]
-To change ADC hysteresis value: HYST [integer]
-To change sensor input pullup vRef low threshold: VADJ [float value]
-To change comparator trigger high threshold: VCOMP [float value]
-
-
-These commands should be wrapped in this format: 
-<CMD, INT, FLOAT>
-
-You must include the unused variable for each instance. 
-
-Examples:
-<GAIN_F, 3, 0.00>
-<VADJ, 0, 2.35>
-
-*Note for Gain Factor:
-The gain STATE is representative of these values:
-0 = 3x
-1 = 3.5x
-2 = 4.33x
-3 = 6x
-4 = 11x
-*/
-
-// Configurable settings:
-int GAIN_FACTOR = 2;           // Gain adjustment factor. 0=3x, 1=3.5x, 2=4.33x, 3=6x, 4=11x
-#define InitCount 6             // Number of times to blink the LED on start
-int LOOP_DUR = 50;        // duration of time between ADC checks and other loop functions
-int TRG_DUR = 20;             // duration of the Z-axis pulse sent, in ms
-#define senseThrs 1.85
-#define compThrs 2.54
-
-int Hyst = 20;                 // Hysteresis value for ADC measurements
-#define Vin 5                   // input reference voltage
-
-/*------------------------------------------------------------*/
-
-// Debug output toggle. Uncomment to enable
-#define DEBUG true
-
-// Headers, variables, and functions
-#include "pP_pins.h"
-#include "pP_volatile.h"
-#include "pP_functions.h"
-#include "pP_serial.h"
-
-// i2c input toggle. Uncomment to enable
-//#define I2C true
-#ifdef I2C
-  #include "pP_i2c.h"
-#endif
-
-void setup() {
-  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
-  pinMode(V_FOLLOW_PIN, INPUT);
-  pinMode(VCOMP_SENSE_PIN, INPUT);
-  pinMode(GADJ_R0, INPUT); // declare input to break pull to ground
-  pinMode(GADJ_R1, INPUT); // declare input to break pull to ground
-  pinMode(GADJ_R2, INPUT); // declare input to break pull to ground
-  pinMode(GADJ_R3, INPUT); // declare input to break pull to ground
-  Serial.begin(9600);
-
-  // Uncomment the following lines to use PCInt pins instead of hardware interrupt
-  //#include <PinChangeInterrupt.h>
-  //attachPCINT(digitalPinToPCINT(Z_TRG), pulse, FALLING);
-
-  // Uncomment the followoing line to use hardware interrupt pin
-  attachInterrupt(digitalPinToInterrupt(Z_TRG), pulse, FALLING);
-
-  Serial.println("Initializing Pyr0-Piezo Sensor...");
-  
-}
-
-
-/*------------------------------------------------*/
-
-void loop() {
-  
-  // Blink LED's on init
-  if (BlinkCount > 0) {
-	BlinkState = !BlinkState;
-	digitalWrite(ERR_LED, BlinkState);
-	digitalWrite(TRG_OUT, BlinkState);
-	delay(LOOP_DUR);
-	--BlinkCount;
-  }
-
-  // Get Serial Input
-  serialInput();
-  
-  // Set any new parameters from serial input
-  updateParams();
-  
-  // Check voltage of first and second stages and compare against thresholds
-  VComp = analogRead(VCOMP_SENSE_PIN);
-  diffCompL = (VComp - compInt) - Hyst;
-  diffCompH = (compInt - VComp) - Hyst;
-  
-  VAdj = analogRead(V_FOLLOW_PIN);
-  diffAdjL = (VAdj - senseInt) - Hyst;
-  diffAdjH = (senseInt - VAdj) - Hyst;
-
-  
-  // Set the amplification gain factor
-  adjustGain();
-  
-  // Voltage Follower adjustment
-  if (diffAdjL > 0 || diffAdjH > 0) {
-    adjustFollow();
-  }
-  
-  // Voltage Comparator adjustment
-  if (diffCompL > 0 || diffCompH > 0) {
-    adjustComp();
-  }
-  
-  // Alert the user that auto-calibration is ongoing
-  calibrateAlert();
-  
-  // Check for error state
-  checkError();
-  
-  // Reply with status
-  serialReply();
-  
-  // Sets trigger output state to false after completing loop
-  delay(LOOP_DUR);
-  digitalWrite(TRG_OUT, HIGH);
-  sensorHReading = 0;
-}
+/*
+  Piezoelectric Z-Axis sensor using AtMega88/168/328 (AtMega 48 doesnt have enough memory for this version)
+
+  This sketch reads a piezo element to detect a touch of the printer's nozzle to the bed.
+  The sense pin is tied to an interrupt, which is pulled high by internal pullup resistor.
+  When the piezo touches the bed, the amplification circuit will draw the interrupt pin low
+  and the atmega will output a pulse based on the programmed trigger duration
+  
+ * PD2 INT0 (Piezo In 'D2')
+ * D7 PCINT23 (Trigger OUT 'D7')
+ * PC0 ADC0 (Voltage Reference Check 'A0')
+ * PC1 ADC1 (Sensitivity Adjustment Check 'A1')
+ * PD4 PCINT20 (Error feedback LED 'D4')
+ * PB6 PCINT6 (Voltage Adjustment Resistor 0 'D20')
+ * PB7 PCINT7 (Voltage Adjustment Resistor 1 'D21')
+ * PD5 T1 (Voltage Adjustment Resistor 2 'D5')
+ * PD6 PCINT22 (Voltage Adjustment Resistor 3 'D6')
+ * PB1 OC1A (Comparator VRef PWM Out 'D9')
+ * PD3 OC2B (Voltage Follower VRef PWM Out 'D3')
+
+  Schematics for this project can be found here: https://github.com/pyr0ball/pyr0piezo/tree/master/docs/Schematics
+
+  For Arduino IDE use MCUdude MiniCore: https://mcudude.github.io/MiniCore/package_MCUdude_MiniCore_index.json
+
+
+  created 2/18/2019
+  by Alan "pyr0ball" Weinstock
+
+  This code is in the public domain.
+*/
+
+/* To set the below parameters using serial input, use the following:
+
+To change trigger active duration: TRG_D [integer for milliseconds]
+To change gain factor: GAIN_F [integer for gain state - see note*]
+To change ADC hysteresis value: HYST [integer]
+To change sensor input pullup vRef low threshold: VADJ [float value]
+To change comparator trigger high threshold: VCOMP [float value]
+
+
+These commands should be wrapped in this format: 
+<CMD, INT, FLOAT>
+
+You must include the unused variable for each instance. 
+
+Examples:
+<GAIN_F, 3, 0.00>
+<VADJ, 0, 2.35>
+
+*Note for Gain Factor:
+The gain STATE is representative of these values:
+0 = 3x
+1 = 3.5x
+2 = 4.33x
+3 = 6x
+4 = 11x
+*/
+
+// Configurable settings:
+int GAIN_FACTOR = 2;           // Gain adjustment factor. 0=3x, 1=3.5x, 2=4.33x, 3=6x, 4=11x
+#define InitCount 6             // Number of times to blink the LED on start
+int LOOP_DUR = 50;        // duration of time between ADC checks and other loop functions
+int TRG_DUR = 20;             // duration of the Z-axis pulse sent, in ms
+#define senseThrs 1450
+#define compThrs 2850
+int dAdjFac = 1;		        // adjustment divider. Higher number will cause the DAC output to adjust more slowly.
+int Hyst = 20;                 // Hysteresis value for ADC measurements
+
+/*------------------------------------------------------------*/
+
+// Debug output toggle. Uncomment to enable
+#define DEBUG true
+//#define VERBOSE true
+
+// Headers, variables, and functions
+#include "pP_pins.h"
+#include "pP_volatile.h"
+#include "pP_function.h"
+#include "pP_serial.h"
+
+// i2c input toggle. Uncomment to enable
+//#define I2C true
+#ifdef I2C
+  #include "pP_i2c.h"
+#endif
+
+void setup() {
+  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
+  pinMode(V_FOLLOW_PIN, INPUT);
+  pinMode(VCOMP_SENSE_PIN, INPUT);
+  pinMode(GADJ_R0, INPUT); // declare input to break pull to ground
+  pinMode(GADJ_R1, INPUT); // declare input to break pull to ground
+  pinMode(GADJ_R2, INPUT); // declare input to break pull to ground
+  pinMode(GADJ_R3, INPUT); // declare input to break pull to ground
+  Serial.begin(9600);
+
+  attachInterrupt(digitalPinToInterrupt(Z_TRG), pulse, FALLING);
+
+  Serial.println("Initializing Pyr0-Piezo Sensor...");
+  
+}
+
+
+/*------------------------------------------------*/
+
+void loop() {
+  
+  // Blink LED's on init
+  if (BlinkCount > 0) {
+	BlinkState = !BlinkState;
+	digitalWrite(ERR_LED, BlinkState);
+	digitalWrite(TRG_OUT, BlinkState);
+	delay(LOOP_DUR);
+	--BlinkCount;
+  }
+
+  // Get Serial Input
+  serialInput();
+  
+  // Set any new parameters from serial input
+  updateParams();
+  
+  // Set the amplification gain factor
+  adjustGain();
+  
+  // Check voltage of first and second stages and compare against thresholds
+  adjustVin();
+  VComp = analogRead(VCOMP_SENSE_PIN);
+  VAdj = analogRead(V_FOLLOW_PIN);
+  
+  // Voltage Follower adjustment
+  if (VLast > Hyst || VLast < -Hyst) {
+    adjustFollow();
+  }
+  
+  // Voltage Comparator adjustment
+  if (VLast > Hyst || VLast > -Hyst) {
+    adjustComp();
+  }
+
+  // Alert the user that auto-calibration is ongoing
+  calibrateAlert();
+  
+  // Check for error state
+  checkError();
+  
+  // Reply with status
+  serialReply();
+  
+  // Sets trigger output state to false after completing loop
+  delay(LOOP_DUR);
+  digitalWrite(TRG_OUT, HIGH);
+  sensorHReading = 0;
+}
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
new file mode 100644
index 0000000..1d79dd0
--- /dev/null
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_function.h
@@ -0,0 +1,142 @@
+/*
+  pyr0-piezo functions library
+  Created by Alan "pyr0ball" Weinstock 6/26/2019
+*/
+
+/*------------------------------------------------*/
+
+void pulse() {
+  digitalWrite(TRG_OUT, LOW);
+  sensorHReading = 1;
+  delay(TRG_DUR);
+  digitalWrite(TRG_OUT, HIGH);
+}
+
+
+/*------------------------------------------------*/
+
+long readVcc() {
+  // Read 1.1V reference against AVcc
+  // set the reference to Vcc and the measurement to the internal 1.1V reference
+  #if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+    ADMUX = _BV(REFS0) | _BV(MUX4) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
+  #elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__)
+    ADMUX = _BV(MUX5) | _BV(MUX0);
+  #elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
+    ADMUX = _BV(MUX3) | _BV(MUX2);
+  #else
+    ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
+  #endif  
+
+  delay(2); // Wait for Vref to settle
+  ADCSRA |= _BV(ADSC); // Start conversion
+  while (bit_is_set(ADCSRA,ADSC)); // measuring
+
+  uint8_t low  = ADCL; // must read ADCL first - it then locks ADCH  
+  uint8_t high = ADCH; // unlocks both
+
+  long result = (high<<8) | low;
+
+  result = 1125300L / result; // Calculate Vcc (in mV); 1125300 = 1.1*1023*1000
+  return result; // Vcc in millivolts
+}
+ 
+/*------------------------------------------------*/
+ 
+ void adjustVin() {
+   VOld = Vin;
+	 Vin = readVcc(), DEC;
+   senseLong = senseThrs * 1024L;
+   compLong = compThrs * 1024L;
+   senseInt = (long long) senseLong / Vin;
+   compInt = (long long) compLong / Vin;
+   senseInt = (int) senseInt;
+   compInt = (int) compInt;
+ }
+
+/*------------------------------------------------*/
+
+ void adjustFollow() {
+    /* Compares diffs of threshold vs read value
+     if positive, adjusts the follower to within
+     the range set above*/
+    
+    ADJ_FOLLOW = (senseInt / 4);
+
+    // Analog output (PWM) of duty cycle
+    analogWrite(V_FOL_PWM, ADJ_FOLLOW);
+  }
+
+/*------------------------------------------------*/
+
+void adjustComp() {
+  ADJ_COMP = (compInt / 4);
+
+  analogWrite(VCOMP_PWM, ADJ_COMP);
+}
+  
+/*------------------------------------------------*/
+  
+void calibrateAlert() {
+  VLast = VOld - Vin;
+  if (VLast > Hyst || VLast < -Hyst ) {
+    ERR_STATE = 1;
+  }
+}
+  
+/*------------------------------------------------*/
+
+void adjustGain() {
+
+  if (GAIN_FACTOR == 0) {
+    pinMode(GADJ_R3, INPUT);
+    pinMode(GADJ_R2, INPUT);
+    pinMode(GADJ_R1, INPUT);
+    pinMode(GADJ_R0, INPUT);
+    ERR_STATE = 0;
+  }
+  else if (GAIN_FACTOR > 0) {
+    pinMode(GADJ_R3, OUTPUT);
+    digitalWrite(GADJ_R3, LOW);
+    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;
+  }
+}
+  
+/*------------------------------------------------*/
+
+void checkError () {
+  if (ERR_STATE == 1) {
+    digitalWrite(ERR_LED, BlinkState);
+    BlinkState = !BlinkState;
+  }
+  else if (ERR_STATE == 0) {
+    BlinkState = LOW;
+    digitalWrite(ERR_LED, BlinkState);
+  }
+}
+
+/*------------------------------------------------*/
+  
+  
+// #endif
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_i2c.h b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_i2c.h
index 97643a2..80046a3 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_i2c.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_i2c.h
@@ -1,23 +1,25 @@
-/*------------------------------------------------*/
-
-#ifdef I2C
-	void i2cInput() {
-
-	  // receive data from Serial and save it into inputBuffer
-	  
-	  while(Wire.available()) {
-		identifyMarkers();
-		updateParams();
-		i2cReply();
-	  }
-	}
-#endif
-
-/*------------------------------------------------*/
-#ifdef I2C
-  void i2cReply() {
-    if (serialIncoming) {
-      Wire.write("OK");
-    }
-  }
+#include <Wire.h>
+
+/*------------------------------------------------*/
+
+#ifdef I2C
+	void i2cInput() {
+
+	  // receive data from Serial and save it into inputBuffer
+	  
+	  while(Wire.available()) {
+		identifyMarkers();
+		updateParams();
+		i2cReply();
+	  }
+	}
+#endif
+
+/*------------------------------------------------*/
+#ifdef I2C
+  void i2cReply() {
+    if (serialIncoming) {
+      Wire.write("OK");
+    }
+  }
 #endif
\ No newline at end of file
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_pins.h b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_pins.h
index 6de03c2..64de630 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_pins.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_pins.h
@@ -1,32 +1,32 @@
-/* pyr0-piezo pins configuration file
-
-Default pins (based on Rev.2.x.xPCB layout) 
-
- * PD2 INT0 (Piezo In 'D2')
- * D7 PCINT23 (Trigger OUT 'D7')
- * PC0 ADC0 (Voltage Reference Check 'A0')
- * PC1 ADC1 (Sensitivity Adjustment Check 'A1')
- * PD4 PCINT20 (Error feedback LED 'D4')
- * PB6 PCINT6 (Voltage Adjustment Resistor 0 'D20')
- * PB7 PCINT7 (Voltage Adjustment Resistor 1 'D21')
- * PD5 T1 (Voltage Adjustment Resistor 2 'D5')
- * PD6 PCINT22 (Voltage Adjustment Resistor 3 'D6')
- * PB1 OC1A (Comparator VRef PWM Out 'D9')
- * PD3 OC2B (Voltage Follower VRef PWM Out 'D3')
-*/
-
-// Analog Pin Assignments
-#define V_FOLLOW_PIN	A0  // Sense pin to check Voltage Follower stage
-#define VCOMP_SENSE_PIN	A1  // Sense pin to check comparator stage voltage
-
-// Digital Pin Assignments
-#define TRG_OUT 7         // LED and Z-Min trigger output connected to digital pin 7
-//#define TRG_OUT 13      // For testing on Atmega328/2560, Output is moved to onboard LED pin
-#define Z_TRG 2           // the piezo is connected to INT0 / digital pin 2
-#define ERR_LED 4         // LED will blink if optimal voltage range cannot be achieved
-#define GADJ_R0 20        // Auto-adjust ladder pin assignments
-#define GADJ_R1 21        // "
-#define GADJ_R2 5         // "
-#define GADJ_R3 6         // "
-#define V_FOL_PWM 3       // PWM analog output pin for voltage follower adjustment
+/* pyr0-piezo pins configuration file
+
+Default pins (based on Rev.2.x.xPCB layout) 
+
+ * PD2 INT0 (Piezo In 'D2')
+ * D7 PCINT23 (Trigger OUT 'D7')
+ * PC0 ADC0 (Voltage Reference Check 'A0')
+ * PC1 ADC1 (Sensitivity Adjustment Check 'A1')
+ * PD4 PCINT20 (Error feedback LED 'D4')
+ * PB6 PCINT6 (Voltage Adjustment Resistor 0 'D20')
+ * PB7 PCINT7 (Voltage Adjustment Resistor 1 'D21')
+ * PD5 T1 (Voltage Adjustment Resistor 2 'D5')
+ * PD6 PCINT22 (Voltage Adjustment Resistor 3 'D6')
+ * PB1 OC1A (Comparator VRef PWM Out 'D9')
+ * PD3 OC2B (Voltage Follower VRef PWM Out 'D3')
+*/
+
+// Analog Pin Assignments
+#define V_FOLLOW_PIN	A0  // Sense pin to check Voltage Follower stage
+#define VCOMP_SENSE_PIN	A1  // Sense pin to check comparator stage voltage
+
+// Digital Pin Assignments
+#define TRG_OUT 7         // LED and Z-Min trigger output connected to digital pin 7
+//#define TRG_OUT 13      // For testing on Atmega328/2560, Output is moved to onboard LED pin
+#define Z_TRG 2           // the piezo is connected to INT0 / digital pin 2
+#define ERR_LED 4         // LED will blink if optimal voltage range cannot be achieved
+#define GADJ_R0 20        // Auto-adjust ladder pin assignments
+#define GADJ_R1 21        // "
+#define GADJ_R2 5         // "
+#define GADJ_R3 6         // "
+#define V_FOL_PWM 3       // PWM analog output pin for voltage follower adjustment
 #define VCOMP_PWM 9       // PWM analog output pin for comparator adjustment
\ No newline at end of file
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 1761435..78c738e 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
@@ -1,54 +1,184 @@
-void serialInput() {
-
-  // receive data from Serial and save it into inputBuffer
-  
-  if(Serial.available() > 0) {
-
-    // the order of these IF clauses is significant
-    identifyMarkers();
-
-  }
-}
-
-/*------------------------------------------------*/
-
-void serialReply() {
-  if (serialIncoming) {
-  serialIncoming = false;
-  #ifdef DEBUG
-    Serial.print("Comp Reference:");
-    Serial.print(VComp);
-    Serial.print(" ");
-    Serial.print("Comparator State:");
-    Serial.print(ADJ_COMP);
-    Serial.print(" ");
-    Serial.println(compInt);
-
-    Serial.print("Diff");
-    Serial.print(" ");
-    Serial.print(diffCompL);
-    Serial.print(" ");
-    Serial.println(diffCompH);
-    
-    Serial.print("Amp Sense:");
-    Serial.print(VAdj);
-    Serial.print(" ");
-    Serial.print("Follower State:");
-    Serial.print(ADJ_FOLLOW);
-    Serial.print(" ");
-    Serial.println(senseInt);
-
-    Serial.print("Diff");
-    Serial.print(" ");
-    Serial.print(diffAdjL);
-    Serial.print(" ");
-    Serial.println(diffAdjH);
-  #endif
-  
-  Serial.print("Delay:");
-  Serial.println(TRG_DUR);
-  Serial.print("Error State:");
-  Serial.println(ERR_STATE);
-  Serial.println("------------------");
-  }
-}
+
+/*------------------------------------------------*/
+
+void parseData() {
+
+  // split the data into its parts
+  
+  char * strtokIndx; // this is used by strtok() as an index
+  
+  strtokIndx = strtok(inputBuffer,",");      // get the first part - the string
+  strcpy(serialMessageIn, strtokIndx); // copy it to serialMessageIn
+  
+  strtokIndx = strtok(NULL, ",");   // this continues where the previous call left off
+  serialInt = atoi(strtokIndx);     // convert this part to an integer
+   
+  strtokIndx = strtok(NULL, ","); 
+  serialFloat = atof(strtokIndx);     // convert this part to a float
+
+}
+/*------------------------------------------------*/
+
+void identifyMarkers() {
+  
+  char x = Serial.read();
+//  char y = Wire.read();
+
+  if (x == endMarker) {
+    readInProgress = false;
+    serialIncoming = true;
+    inputBuffer[bytesRecvd] = 0;
+    parseData();
+  }
+
+  else if(readInProgress) {
+    inputBuffer[bytesRecvd] = x;
+    bytesRecvd ++;
+    if (bytesRecvd == buffSize) {
+      bytesRecvd = buffSize - 1;
+    }
+  }
+
+  else if (x == startMarker) { 
+    bytesRecvd = 0; 
+    readInProgress = true;
+  }
+  #ifdef I2C
+    if (y == endMarker) {
+      readInProgress = false;
+      serialIncoming = true;
+      inputBuffer[bytesRecvd] = 0;
+      parseData();
+    }
+
+    if(readInProgress) {
+      inputBuffer[bytesRecvd] = y;
+      bytesRecvd ++;
+      if (bytesRecvd == buffSize) {
+      bytesRecvd = buffSize - 1;
+      }
+    }
+
+    if (y == startMarker) { 
+      bytesRecvd = 0; 
+      readInProgress = true;
+    }
+  #endif
+}
+
+/*------------------------------------------------*/
+
+void updateTrigDuration() {
+  if (serialInt >= 0) {
+    TRG_DUR = serialInt;
+  }
+}
+/*------------------------------------------------*/
+
+void updateGainFactor() {
+  if (serialInt >= 0) {
+    GAIN_FACTOR = serialInt;
+  }
+}
+/*------------------------------------------------*/
+
+void updateVComp() {
+  if (serialInt >= 0) {
+    compInt = (serialFloat * 1024) / Vin;
+    //senseInt = compInt; // syncing these params til #24 is fixed
+  }
+}
+/*------------------------------------------------*/
+
+void updateVAdj() {
+  if (serialInt >= 0) {
+    senseInt = (serialFloat * 1024) / Vin;
+    //compInt = senseInt; // syncing these params til #24 is fixed
+  }
+}
+/*------------------------------------------------*/
+
+void updateHysteresis() {
+  if (serialInt >= 0) {
+    Hyst = serialInt;
+  }
+}
+/*------------------------------------------------*/
+
+/*------------------------------------------------*/
+
+void updateParams() {
+  if (strcmp(serialMessageIn, "TRG_D") == 0) {
+    updateTrigDuration();
+  }
+  else if (strcmp(serialMessageIn, "GAIN_F") == 0) {
+    updateGainFactor();
+  }
+  else if (strcmp(serialMessageIn, "VCOMP") == 0) {
+    updateVComp();
+  }
+  else if (strcmp(serialMessageIn, "VADJ") == 0) {
+    updateVAdj();
+  }
+}
+
+void serialInput() {
+
+  // receive data from Serial and save it into inputBuffer
+  
+  if(Serial.available() > 0) {
+
+    // the order of these IF clauses is significant
+    identifyMarkers();
+
+  }
+}
+
+/*------------------------------------------------*/
+
+void serialReply() {
+  #ifndef VERBOSE
+    if (serialIncoming) {
+    serialIncoming = false;
+  #endif
+  #ifdef DEBUG
+    Serial.print("Vcc:");
+    Serial.println(Vin);
+    Serial.print("Comp Sense:");
+    Serial.print(VComp);
+    Serial.print(" ");
+    Serial.print("Comparator State:");
+    Serial.print(ADJ_COMP);
+    Serial.print(" ");
+    Serial.println(compInt);
+
+    Serial.print("Diff");
+    Serial.print(" ");
+    Serial.print(diffCompL);
+    Serial.print(" ");
+    Serial.println(diffCompH);
+    
+    Serial.print("Amp Sense:");
+    Serial.print(VAdj);
+    Serial.print(" ");
+    Serial.print("Follower State:");
+    Serial.print(ADJ_FOLLOW);
+    Serial.print(" ");
+    Serial.println(senseInt);
+
+    Serial.print("Diff");
+    Serial.print(" ");
+    Serial.print(diffAdjL);
+    Serial.print(" ");
+    Serial.println(diffAdjH);
+  #endif
+  
+  Serial.print("Delay:");
+  Serial.println(TRG_DUR);
+  Serial.print("Error State:");
+  Serial.println(ERR_STATE);
+  Serial.println("------------------");
+  #ifndef VERBOSE
+  }
+  #endif
+}
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_volatile.h b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_volatile.h
index 4600f0a..2208516 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_volatile.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/pP_volatile.h
@@ -1,37 +1,41 @@
-// these variables will change on their own. Do not edit ANYTHING below this line
-volatile int sensorHReading = 0;      // variable to store the value read from the sensor pin
-volatile int ADJ_FOLLOW = 0;          // Variable for Follower adjustment
-volatile int ADJ_COMP = 0;            // Variable for Comparator adjustment
-volatile int ERR_STATE = 0;
-
-// Convert float to integer for adjustment functions
-int senseInt = (senseThrs * 1024) / Vin;    // Voltage Follower upper converted to adg interger
-int compInt = (compThrs * 1024) / Vin;      // Upper threshold of Comparator before adjustment
-
-// Voltage Comparator Adjustment parameters
-//float VCompRef = 0.00;                    // variable to store the float value read from the comparator reference
-int VComp = 0;
-int diffCompL = VComp - compInt;
-int diffCompH = compInt - VComp;
-
-// Voltage Follower Adjustment parameters
-//float vAdjRead = 0.00;                    // variable to store the value read from the follower
-int VAdj = 0;                    
-int diffAdjL = VAdj - senseInt;
-int diffAdjH = senseInt - VAdj;
-
-// Error blink parameters
-int BlinkState = LOW;
-int BlinkCount = InitCount * 2;           // Multiply Blink count by 2 to handle toggle state
-
-// Serial Input Parsing Variables
-#define buffSize 40
-char inputBuffer[buffSize];
-#define startMarker '<'
-#define endMarker '>'
-byte bytesRecvd = 0;
-bool readInProgress = false;
-bool serialIncoming = false;
-char serialMessageIn[buffSize] = {0};
-int serialInt = 0;
-float serialFloat = 0.00;
\ No newline at end of file
+// these variables will change on their own. Do not edit ANYTHING below this line
+volatile int sensorHReading = 0;      // variable to store the value read from the sensor pin
+volatile int ADJ_FOLLOW = 0;          // Variable for Follower adjustment
+volatile int ADJ_COMP = 0;            // Variable for Comparator adjustment
+volatile int ERR_STATE = 0;
+
+int Vin = 5000;                   // input reference voltage in millivolts (multiply V by 1000)
+int VOld = 5000;                  // Variable to store previous cycle's Vin
+int VLast = 0;
+
+// Convert threshold values based on the input voltage
+long senseLong = senseThrs * 1024L;
+long compLong = compThrs * 1024L;
+long senseInt = senseLong / Vin;
+long compInt = compLong / Vin;
+
+// Voltage Comparator Adjustment parameters
+int VComp = 0;
+int diffCompL = VComp - compInt;
+int diffCompH = compInt - VComp;
+
+// Voltage Follower Adjustment parameters
+int VAdj = 0;                    
+int diffAdjL = VAdj - senseInt;
+int diffAdjH = senseInt - VAdj;
+
+// Error blink parameters
+int BlinkState = LOW;
+int BlinkCount = InitCount * 2;           // Multiply Blink count by 2 to handle toggle state
+
+// Serial Input Parsing Variables
+#define buffSize 40
+char inputBuffer[buffSize];
+#define startMarker '<'
+#define endMarker '>'
+byte bytesRecvd = 0;
+bool readInProgress = false;
+bool serialIncoming = false;
+char serialMessageIn[buffSize] = {0};
+int serialInt = 0;
+float serialFloat = 0.00;