diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_cmd.cpp b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_cmd.cpp
index 4ae86c5..856e6db 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_cmd.cpp
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_cmd.cpp
@@ -1,6 +1,7 @@
 #include "EEPROM.h"
 #include "pP_config.h"
 #include "pP_function.h"
+#include "pP_volatile.h"
 
 /*------------------------------------------------*/
 
@@ -88,6 +89,13 @@ void updateConstant(long value) {
   }
 }
 
+void adjustConstant(int value) {
+  if (value > 0 && Vin > 0) {
+    voltMeterConstant = (long)(1.1 * value / Vin * 1023 * 1000);
+    EEPROM.put(VM_CONST_ADDRESS, voltMeterConstant);
+  }
+}
+
 /*------------------------------------------------*/
 
 void updateDebug(int value) {
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_cmd.h b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_cmd.h
index 9edfb82..a38149c 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_cmd.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_cmd.h
@@ -11,6 +11,7 @@ void updateLogic(int value);
 void updatePzDet(int value);
 void updateVccSwitch(int value);
 void updateConstant(long value);
+void adjustConstant(int value);
 void updateDebug(int value);
 
 #endif // PP_CMD_H
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.cpp b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.cpp
index 7edbb5a..f41c200 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.cpp
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.cpp
@@ -5,7 +5,7 @@
 #include <Wire1.h>
 
 uint8_t command;
-uint16_t value;
+uint32_t value;
 
 void i2cWrite(uint8_t *buffer, int offset, int data) {
   buffer[offset] = (uint8_t)(data >> 8);
@@ -33,8 +33,9 @@ void i2cReportConfig() {
   i2cWrite(buffer, 10, Hyst);
   i2cWrite(buffer, 12, LOGIC);
   i2cWrite(buffer, 14, PZDET);
-  i2cWrite(buffer, 16, voltMeterConstant);
-  memcpy(buffer + 20, PP_VERSION, length - 20);
+  i2cWrite(buffer, 16, VCCSW);
+  i2cWrite(buffer, 18, voltMeterConstant);
+  memcpy(buffer + 22, PP_VERSION, length - 22);
   Wire1.write(buffer, length);
 }
 
@@ -50,8 +51,6 @@ void i2cReportState() {
 }
 
 void i2cReply() {
-  Serial.print("Requested ");
-  Serial.println(command);
   switch (command) {
   case CMD_CONFIG:
   case CMD_ERASE:
@@ -72,18 +71,11 @@ void i2cInput(int bytesReceived) {
       command = Wire1.read();
     } else if (a == 1) {
       value = Wire1.read();
-    } else if (a == 2) {
-      value = value << 8 | Wire1.read();
     } else {
-      Wire1.read(); //
+      value = value << 8 | Wire1.read();
     }
   }
 
-  Serial.print("Command ");
-  Serial.print(command);
-  Serial.print(" ");
-  Serial.println(value);
-
   // Parse commands and apply changes or actions
   switch (command) {
   case CMD_GAIN_F:
@@ -122,6 +114,10 @@ void i2cInput(int bytesReceived) {
     break;
   case CMD_VCCSW:
     updateVccSwitch(value);
+    break;
+  case CMD_VCCADJUST:
+    adjustConstant(value);
+    break;
   default:
     return;
   }
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.hpp b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.hpp
index 1cad709..f34640c 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.hpp
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c.hpp
@@ -4,21 +4,6 @@
 #include "pP_config.h"
 #include "stdint.h"
 
-#define status_Offset 0x00       // Status register
-#define senseInt_Offset 0x01     // Integer of sense threshold in millivolts
-#define compInt_Offset 0x02      // Integer of comparator threshold in millivolts
-#define gainFactor_Offset 0x03   // Gain adjustment factor. 0=3x, 1=3.5x, 2=4.33x, 3=6x, 4=11x
-#define hysteresis_Offset 0x04   // Hysteresis value for ADC measurements
-#define loopDuration_Offset 0x05 // duration of time between ADC checks and other loop functions
-#define logicLevel_Offset 0x06
-#define piezoDetect_Offset 0x07
-#define triggerDuration_Offset 0x08 // duration of the Z-axis pulse sent, in ms
-#define debugEnable_Offset 0x09
-#define voltMeterLong_Offset 0x0a // For fine-tuning the input volt master
-#define versionRegister_Offset 0x0b
-#define configRegister_Offset 0x0c
-#define identRegister_Offset 0x0d
-
 void i2cInit();
 
 #endif // _pP_i2c_h_
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c_config.h b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c_config.h
index cd286e5..510a3c9 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c_config.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_i2c_config.h
@@ -10,4 +10,5 @@
 #define CMD_CONFIG 0x09
 #define CMD_ERASE 0x0a
 #define CMD_STATE 0x0b
-#define CMD_VCCSW 0x0c
\ No newline at end of file
+#define CMD_VCCSW 0x0c
+#define CMD_VCCADJUST 0x0d
\ No newline at end of file
diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_serial.h b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_serial.h
index 60b94f8..88699d4 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_serial.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/src/pP_serial.h
@@ -1,5 +1,6 @@
 #include "pP_cmd.h"
 #include "pP_volatile.h"
+#include "string.h"
 
 void parseData() {
 
@@ -128,6 +129,7 @@ void serialPrintState() {
 
 void updateParams() {
   serialIncoming = false;
+  strupr(serialMessageIn);
   if (strcmp(serialMessageIn, "GAIN_F") == 0) {
     updateGainFactor(serialLong);
   } else if (strcmp(serialMessageIn, "VFOL") == 0) {
@@ -148,6 +150,8 @@ void updateParams() {
     updateVccSwitch(serialLong);
   } else if (strcmp(serialMessageIn, "CONST") == 0) {
     updateConstant(serialLong);
+  } else if (strcmp(serialMessageIn, "VCCADJUST") == 0) {
+    adjustConstant(serialLong);
   } else if (strcmp(serialMessageIn, "DEBUG") == 0) {
     updateDebug(serialLong);
   } else if (strcmp(serialMessageIn, "CONFIG") == 0) {
@@ -171,6 +175,7 @@ void updateParams() {
     Serial.println("To change the main voltage of the circuit: VCCSW [0|1]");
     Serial.println("  (0 for 3.3v, 1 for 5v)");
     Serial.println("To change ADC hysteresis value: HYST [integer in millivolts]");
+    Serial.println("To adjust VCC voltage readings: VCCADJUST [integer in millivolts, use value from multimeter]");
     Serial.println("To enable or disable debug output: DEBUG [0|1]");
     Serial.println("To print current config: CONFIG");
     Serial.println("To set config to defaults: ERASE");
diff --git a/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/i2c.cpp b/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/i2c.cpp
index 02fd0b8..fc36a88 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/i2c.cpp
+++ b/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/i2c.cpp
@@ -3,11 +3,17 @@
 #include "Wire.h"
 
 uint16_t read16() {
-  return Wire.read() << 8 | Wire.read();
+  uint8_t value = Wire.read();
+  uint8_t value1 = Wire.read();
+  return ((uint16_t)value) << 8 | value1;
 }
 
 uint32_t read32() {
-  return (uint32_t)Wire.read() << 24 | (uint32_t)Wire.read() << 16 | Wire.read() << 8 | Wire.read();
+  uint8_t value = Wire.read();
+  uint8_t value1 = Wire.read();
+  uint8_t value2 = Wire.read();
+  uint8_t value3 = Wire.read();
+  return ((uint32_t)value) << 24 | ((uint32_t)value1) << 16 | ((uint16_t)value2) << 8 | value3;
 }
 
 void write(uint8_t cmd) {
@@ -40,7 +46,6 @@ config_t requestConfig() {
   Wire.endTransmission();
 
   uint8_t bytes = Wire.requestFrom(ADDRESS, 255);
-  Serial.println(bytes);
 
   config_t config;
   config.GAIN_FACTOR = read16();
@@ -51,6 +56,7 @@ config_t requestConfig() {
   config.Hyst = read16();
   config.LOGIC = read16();
   config.PZDET = read16();
+  config.VCCSW = read16();
   config.voltMeterConstant = read32();
   config.version = Wire.readString();
 
@@ -70,4 +76,6 @@ state_t requestState() {
   state.VFol = read16();
   state.ERR_STATE = read16();
   state.PZ_STATE = read16();
+
+  return state;
 }
\ No newline at end of file
diff --git a/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/i2c.h b/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/i2c.h
index b976fd9..97b79ae 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/i2c.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/i2c.h
@@ -16,6 +16,7 @@
 #define CMD_ERASE 0x0a
 #define CMD_STATE 0x0b
 #define CMD_VCCSW 0x0c
+#define CMD_VCCADJUST 0x0d
 
 #include "WString.h"
 
@@ -28,6 +29,7 @@ typedef struct {
   uint16_t Hyst;
   uint16_t LOGIC;
   uint16_t PZDET;
+  uint16_t VCCSW;
   uint32_t voltMeterConstant;
   String version;
 } config_t;
diff --git a/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/pP_serial.h b/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/pP_serial.h
index 4604178..3357a42 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/pP_serial.h
+++ b/firmware/AVR-Source/Pyr0_Piezo_i2c_Bridge/src/pP_serial.h
@@ -1,5 +1,6 @@
 #include "i2c.h"
 #include "stdint.h"
+#include "string.h"
 
 #define buffSize 40
 bool serialIncoming = false;
@@ -89,6 +90,20 @@ void serialPrintConfig() {
   Serial.print("PZDET ");
   Serial.println(config.PZDET);
 
+  Serial.print("VCCSW ");
+  Serial.print(config.VCCSW);
+  switch (config.VCCSW) {
+  case 0:
+    Serial.println(" 3.3v");
+    break;
+  case 1:
+    Serial.println(" 5v");
+    break;
+  default:
+    Serial.println(" INVALID");
+    break;
+  }
+
   Serial.print("VM_CONST ");
   Serial.println(config.voltMeterConstant);
 
@@ -106,11 +121,11 @@ void serialPrintState() {
   Serial.print(",");
 
   Serial.print("\"VComp\":");
-  Serial.print((uint32_t)state.VComp * state.Vin / 1023);
+  Serial.print(state.VComp);
   Serial.print(",");
 
   Serial.print("\"VFol\":");
-  Serial.print((uint32_t)state.VFol * state.Vin / 1023);
+  Serial.print(state.VFol);
   Serial.print(",");
 
   Serial.print("\"Err\":");
@@ -125,6 +140,7 @@ void serialPrintState() {
 
 void updateParams() {
   serialIncoming = false;
+  strupr(serialMessageIn);
   if (strcmp(serialMessageIn, "GAIN_F") == 0) {
     write(CMD_GAIN_F, (uint16_t)serialLong);
   } else if (strcmp(serialMessageIn, "VFOL") == 0) {
@@ -145,6 +161,8 @@ void updateParams() {
     write(CMD_VCCSW, (uint16_t)serialLong);
   } else if (strcmp(serialMessageIn, "CONST") == 0) {
     write(CMD_CONST, serialLong);
+  } else if (strcmp(serialMessageIn, "VCCADJUST") == 0) {
+    write(CMD_VCCADJUST, (uint16_t)serialLong);
   } else if (strcmp(serialMessageIn, "CONFIG") == 0) {
     serialPrintConfig();
   } else if (strcmp(serialMessageIn, "ERASE") == 0) {
@@ -166,6 +184,7 @@ void updateParams() {
     Serial.println("To change the main voltage of the circuit: VCCSW [0|1]");
     Serial.println("  (0 for 3.3v, 1 for 5v)");
     Serial.println("To change ADC hysteresis value: HYST [integer in millivolts]");
+    Serial.println("To adjust VCC voltage readings: VCCADJUST [integer in millivolts, use value from multimeter]");
     Serial.println("To enable or disable debug output: DEBUG [0|1]");
     Serial.println("To print current config: CONFIG");
     Serial.println("To set config to defaults: ERASE");