diff --git a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/platformio.ini b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/platformio.ini
index 715165e..fb45aa2 100644
--- a/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/platformio.ini
+++ b/firmware/AVR-Source/Pyr0_Piezo_Sensor_v2.x.x/platformio.ini
@@ -11,12 +11,27 @@
 [platformio]
 default_envs = ATmega328PB
 
+[env:ICSP_Bootloader]
+platform = atmelavr
+framework = arduino
+upload_protocol = stk500v1
+; each flag in a new line
+upload_flags =
+    -P$UPLOAD_PORT
+    -b$UPLOAD_SPEED
+
+board_build.f_cpu = 8000000L
+board_hardware.oscillator = internal
+board_hardware.uart = uart0
+board_hardware.bod = 1.8v
+board_hardware.eesave = yes
+
 [env:ATmega88P]
 platform = atmelavr
 board = ATmega88P
 framework = arduino
 lib_deps = Arduino
-upload_protocol = stk500v1
+upload_protocol = arduino
 ; each flag in a new line
 upload_flags =
     -P$UPLOAD_PORT
@@ -28,24 +43,27 @@ upload_flags =
 upload_speed = 38400
 
 board_build.f_cpu = 8000000L
-board_fuses.lfuse = "0xE2"
-board_fuses.hfuse = "0xDF"
-board_fuses.efuse = "0xF9"
+board_hardware.oscillator = internal
+board_hardware.uart = uart0
+board_hardware.bod = 1.8v
+board_hardware.eesave = yes
 
 [env:ATmega328PB]
 platform = atmelavr
 board = ATmega328PB
 framework = arduino
 lib_deps = Arduino
-upload_protocol = stk500v1
+upload_protocol = arduino
 ; each flag in a new line
 upload_flags =
     -P$UPLOAD_PORT
     -b$UPLOAD_SPEED
-    -fmax_errors=5
 
 ; edit these lines
-;upload_port = COM4
 upload_speed = 38400
 
-board_build.f_cpu = 8000000L
\ No newline at end of file
+board_build.f_cpu = 8000000L
+board_hardware.oscillator = internal
+board_hardware.uart = uart0
+board_hardware.bod = 1.8v
+board_hardware.eesave = yes
\ No newline at end of file
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 19b8439..4d0e3a2 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
@@ -186,3 +186,100 @@ void pzConCheck () {
 }
 
 /*------------------------------------------------*/
+void eraseEEPROM() {
+	
+	setDefaultConfig();
+	
+    EEPROM.put(GAIN_FACTOR_ADDRESS, GAIN_FACTOR);
+    EEPROM.put(FOLLOWER_THRESHOLD_ADDRESS, followerThrs);
+    EEPROM.put(COMP_THRESHOLD_ADDRESS, compThrs);
+    EEPROM.put(LOOP_DUR_ADDRESS, LOOP_DUR);
+    EEPROM.put(TRG_DUR_ADDRESS, TRG_DUR);
+    EEPROM.put(HYST_ADDRESS, Hyst);
+    EEPROM.put(LOGIC_ADDRESS, LOGIC);
+    EEPROM.put(VM_CONST_ADDRESS, voltMeterConstant);
+}
+
+// Restore config from EEPROM, otherwise erase config and write to EEPROM
+void restoreConfig() {
+    int temp;
+	
+	bool erase = false;
+
+    EEPROM.get(GAIN_FACTOR_ADDRESS, temp);
+    if (temp < 0 || temp > 4) {
+		erase = true;
+    } else {
+        GAIN_FACTOR = temp;
+    }
+
+    EEPROM.get(FOLLOWER_THRESHOLD_ADDRESS, temp);
+    if (temp < 0 || temp > 5000) {
+        erase = true;
+    } else {
+        followerThrs = temp;
+    }
+
+    EEPROM.get(COMP_THRESHOLD_ADDRESS, temp);
+    if (temp < 0 || temp > 5000) {
+        erase = true;
+    } else {
+        compThrs = temp;
+    }
+
+    EEPROM.get(LOOP_DUR_ADDRESS, temp);
+    if (temp < 0 && temp > 1000) {
+        erase = true;
+    } else {
+        LOOP_DUR = temp;
+    }
+
+    EEPROM.get(TRG_DUR_ADDRESS, temp);
+    if (temp < 0 || temp > 1000) {
+        erase = true;
+    } else {
+        TRG_DUR = temp;
+    }
+
+    EEPROM.get(HYST_ADDRESS, temp);
+    if (temp < 0 || temp > 1000) {
+        erase = true;
+    } else {
+        Hyst = temp;
+    }
+
+    EEPROM.get(LOGIC_ADDRESS, temp);
+    if (temp < 0 || temp > 1) {
+        erase = true;
+    } else {
+        LOGIC = temp;
+    }
+
+    long longTemp;
+    EEPROM.get(VM_CONST_ADDRESS, longTemp);
+    if (longTemp < 1000000L || longTemp > 1200000L) {
+        erase = true;
+    } else {
+        voltMeterConstant = longTemp;
+    }
+	
+	if (erase) {
+		eraseEEPROM();
+	}
+
+    adjustFollow();
+    adjustComp();
+}
+
+void setDefaultConfig() {
+    GAIN_FACTOR = GAIN_FACTOR_DEFAULT;
+    followerThrs = FOLLOWER_THRESHOLD_DEFAULT;
+    compThrs = COMP_THRESHOLD_DEFAULT;
+    LOOP_DUR = LOOP_DUR_DEFAULT;
+    TRG_DUR = TRG_DUR_DEFAULT;
+    Hyst = HYST_DEFAULT;
+    LOGIC = LOGIC_DEFAULT;
+    voltMeterConstant = VM_CONST_DEFAULT;
+    adjustFollow();
+    adjustComp();
+}