   } else {
     return 0;
   }
+}
+
+void EEPROMWriteInt(int addr, int p_value) {
+  EEPROM.write(addr, ((p_value >> 0) & 0xFF));
+  EEPROM.write(addr + 1, ((p_value >> 8) & 0xFF));
+}
+
+unsigned int EEPROMReadInt(int addr) {
+  byte lowByte = EEPROM.read(addr);
+  byte highByte = EEPROM.read(addr + 1);
+  return ((lowByte << 0) & 0xFF) + ((highByte << 8) & 0xFF00);
 }
 
 void readEEPROM() {

  digitalWrite(AC_3, stateAC3);
  stateAC4 = readAddress(6);
  digitalWrite(AC_4, stateAC4);
}

void writeEEPROM() {

        case 20:
          // Diagnostic function - set DAC1
          PosDac.output(param);
          Serial.println("ACK");
          break;
          
        case 21:
          // Diagnostic function - set DAC2
          NegDac.output(param);
          Serial.println("ACK");
          break;
          
        case 22:
          // Diagnostic function - raw ADC voltages
          {
            int vsp = analogRead(Vsense_P);
            int vsn = analogRead(Vsense_N);
            int asp = analogRead(Asense_P);
            int asn = analogRead(Asense_N);
            Serial.println(String("ADC:") + vsp + ", " + vsn + ", " + asp + ", " + asn);
          }
          break;
          
        case 23:
          // Diagnostic function - change offset calibration
          offsetP = param;
          EEPROMWriteInt(7, offsetP);
          Serial.println("ACK");
          break;
          
        case 24:
          // Diagnostic function - change negative offset calibration
          offsetN = param;
          EEPROMWriteInt(9, offsetN);
          Serial.println("ACK");
          break;
      }
    }
