 void Unrecognized() {
   Serial.print("NK\r");
 }
-
-
-/****************************************************************************
- * Functions for writing/reading mechanical errors to/from EPRROM
- ***************************************************************************/
-void writeMechanicalErrors (double z1) {
-    // Writes mechanical error corrections to EEPROM
-    // Convert values to long int
-    long _z1 = long (z1 * 1e8);
-    EEPROM.write (0x00, _z1);
-    EEPROM.write (0x01, _z1 >> 8);
-    EEPROM.write (0x02, _z1 >> 16);
-    /*
-    long _z2 = long (z2 * 1e8);
-    EEPROM.write (0x03, _z2 >> 16);
-    EEPROM.write (0x04, _z2 >> 8);
-    EEPROM.write (0x05, _z2);
-    
-    long _z3 = long (z3 * 1e8);
-    EEPROM.write (0x06, _z3 >> 16);
-    EEPROM.write (0x07, _z3 >> 8);
-    EEPROM.write (0x08, _z3); 
-    */
-}
-
-double readMechanicalError (int n) {
-    // Reads one of the three mechanical errors, defined by the int n
-    long res = 0;
-    
-    for (unsigned int k = 0; k < 3; k++) {
-        res |= (Serial.println(EEPROM.read(3*n + k)) << 8*k );
-    }
-    return double(res);
-}
-
 /****************************************************************************/
 
 void setup() {

// Motor 2 on port 2, 200 steps/rev
SkyPointer_MicroStepper *motor2 = MS.getMicroStepper(STEPS, 2);


// Interruption routine
void ISR_rotate() {
  #ifdef DEBUG

  // Start motor shield
  MS.begin();
  motor1->setSpeed(RPM);
  motor2->setSpeed(RPM);  

  long err = Z1 * 1e8;
  Serial.print ("Val = "); Serial.println (err, DEC);
  Serial.print ("HEX = "); Serial.println (err, HEX);
  
  writeErrorToEEPROM (1, Z1);
  
  readErrorFromEEPROM (1);


}

void loop() {
  sCmd.readSerial();  // Read commands from serial port
