         case 20:
           // Diagnostic function - set DAC1
           PosDac.output(param);
-          break;
+          Serial.println("ACK");
+          break;
+          
         case 21:
           // Diagnostic function - set DAC2
           NegDac.output(param);
-          break;
+          Serial.println("ACK");
+          break;
+          
         case 22:
           // Diagnostic function - raw ADC voltages
-          int vsp = analogReadV(10, Vsense_P);
-          int vsn = analogReadV(10, Vsense_N);
-          int asp = analogReadV(10, Asense_P);
-          int asn = analogReadV(10, Asense_N);
-          Serial.println(String("ADC:") + vsp + ", " + vsn + ", " + asp + ", " + asn);
+          {
+            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;
       }
     }

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

void writeEEPROM() {

  int cval = 0;

  // Read positive voltage
  vSenseP = (analogReadV(10, Vsense_P) * (VDRP+offsetP)) / 1000;

  // Read negative voltage
  vSenseN = (((Vref/2)-offsetN) - analogReadV(10, Vsense_N)) * -10;

  // Read positive current
  cval =  analogReadV(10, Asense_P) - 2500;
