  ***************************************************************************/
 void writeErrorToEEPROM (int n, double Z) {
     // Writes to EEPROM the n-th error correction, where n = 1, 2 or 3
-    /*
-    Values are converted to a double int by multiplying by 100.000.000
-    The longest value is 4-byte long:
-        LSB = 
-        
-        MSB = 
-    */
-    for (unsigned int k = 0; k < 12; k++) {
-        EEPROM.write (k, 0);
-    }
     signed long _z = (signed long) (Z * 1e8);
     for (unsigned int k = 0; k < 4; k++) {
         // Write the 3 lowest bytes (not the sign)

    }
}

signed long readErrorFromEEPROM (int n) {
    // Reads the n-th error stored in the EEPROM and converts it to double,
    // where n = 1, 2 or 3
    signed long res = 0;
    for (unsigned int k = 0; k < 4; k++) {        
        res |= (signed long) ((signed long) (EEPROM.read(4*(n-1)+k)) << 8*k);
        //int aux = EEPROM.read(4*(n-1)+k);
        //Serial.print ("Aux = "); Serial.println(aux, HEX);
        //signed long aux = (signed long) (EEPROM.read(4*(n-1)+k));
        //res += (aux << (8*k));
    }
    //Serial.print("Res = "); Serial.print(res, HEX); Serial.print(" = ");
    //Serial.println(res, DEC);
    return res;
}



/****************************************************************************/
// Interruption routine
void ISR_rotate() {

  motor1->setSpeed(RPM);
  motor2->setSpeed(RPM);  



  // Array with the errors
  double Z[3] = {(double) Z1, (double) Z2, (double) Z3};
  writeAllToEEPROM ((double) Z1, (double) Z2, (double) Z3);

}

void loop() {
  sCmd.readSerial();  // Read commands from serial port
  double a = readErrorFromEEPROM(1);
  double b = readErrorFromEEPROM(2);
  double c = readErrorFromEEPROM(3);
  
  Serial.print(a, 8);
  Serial.print ("  |  "); Serial.print(b, 8);
  Serial.print ("  |  "); Serial.print(c, 8);
  Serial.println("");
}
