// Arduino MEGA sketch per Pulley con interrupt su timer5
//#include <SPI.h>           // Not needed with I2C
#include <Wire.h>
#include <EEPROM.h>
//#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <AccelStepper.h>
#include <Bounce2.h>
#include <HX711.h> //HX711 loadcell;

//-------wiring -----------------------------------------------------
// Load Cell X  HX711 wiring
const int LoadCell_X_DOUT_PIN = 4;
const int LoadCell_X_SCK_PIN = 5;


//X axis stepper motor wiring with driver A4988
const int dirPin_stepperX = 2;
const int stepPin_stepperX = 3;
const int enablePin_stepperX = 12;

// button wiring
#define button_D_PIN 6    //Down button
#define button_R_PIN 7    //Right button
#define button_S_PIN 8    //Select button
#define button_L_PIN 9    //Left button
#define button_U_PIN 10   //Up button
#define button_B_PIN 11   //Back button
// microswitch wiring
#define homeSw_x_PIN 14   //homeSw X
#define endSw_x_PIN 15    //endSw X

//------------------------------------------------------------
//----Istantiate ---------------------------------------------
//Instatiate Accelstepper
AccelStepper stepperX(1, stepPin_stepperX, dirPin_stepperX);

//instantiate Load Cell
HX711 LoadCell_X;

//Instatiate Oled SSD1306
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin  (verificare se inserire -1)
#define SCREEN_ADDRESS 0x3C ///< see datasheet or use I2C Scanner
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//Instatiate the Bounce
Bounce button_D = Bounce();
Bounce button_R = Bounce();
Bounce button_S = Bounce();
Bounce button_L = Bounce();
Bounce button_U = Bounce();
Bounce button_B = Bounce();

Bounce homeSw_x = Bounce();
Bounce endSw_x = Bounce();

//---------------------------------------------------------------

// Calibration factors of Load Cells
float calibration_X = 113047.9f;  //1109000 coefficiente cal in kg --> 1109000/9.81=113047.9f calib in Newton
float Tension_X = 0;       //Initial mesh tension
float X_position = 0.0;     //Starting position
int TensSpeed = 50.0;     //Tensioning speed in mm/min

float screwPitch = 1.0;         //screw pitch in mm
float displacementPerStep = 1;

long initial_homingX = -1;  // Used to Home Stepper at startup
long step = 0;        // initial step
const long interval = 100;     //display update time in millisecond

long stepPerRev = 200 * 8; //8 microstepping MS1=H, MS2=H, MS3=L
int stato = 0;          // initial step
const int addressTravel = 0; // Indirizzo di memoria EEPROM dove salvare la variabile
const int addressSpeed= 10;
const int addressSD= 20;
long MaxTravel=50; //spostamento in millimetri

const int debounce = 5;    //debounce time in millisecond

bool FIRST = true;
bool LoadCompleted = false;
bool UnloadCompleted = false;

unsigned long previousMillis = 0;

int SD = 1;


//------------------------------------------------------------
ISR(TIMER5_COMPA_vect){
stepperX.run();

}


void setup() {
  Serial.begin(115200);


  pinMode(button_D_PIN, INPUT_PULLUP);
  pinMode(button_R_PIN, INPUT_PULLUP );
  pinMode(button_S_PIN, INPUT_PULLUP );
  pinMode(button_L_PIN, INPUT_PULLUP );
  pinMode(button_U_PIN, INPUT_PULLUP );
  pinMode(button_B_PIN, INPUT_PULLUP );
  pinMode(homeSw_x_PIN, INPUT_PULLUP);
  pinMode(endSw_x_PIN, INPUT_PULLUP);
  pinMode(enablePin_stepperX, OUTPUT);
  

  //Attach debounce & define time interval
  button_D.attach( button_D_PIN);
  button_D.interval(debounce);
  button_R.attach( button_R_PIN);
  button_R.interval(debounce);
  button_S.attach( button_S_PIN);
  button_S.interval(debounce);
  button_L.attach( button_L_PIN);
  button_L.interval(debounce);
  button_U.attach( button_U_PIN);
  button_U.interval(debounce);
  button_B.attach( button_B_PIN);
  button_B.interval(debounce);

  homeSw_x.attach(homeSw_x_PIN , INPUT_PULLUP);
  homeSw_x.interval(debounce);
  endSw_x.attach(endSw_x_PIN , INPUT_PULLUP);
  endSw_x.interval(debounce);


  //inizializzazione Stepper
  stepperX.setMaxSpeed(1000.0);      // Set Max Speed of Stepper (Slower to get better accuracy)
  stepperX.setAcceleration(1000.0);
  stepperX.setEnablePin(enablePin_stepperX);
  stepperX.setPinsInverted(false, false, false, false, true);



  // Inizializzazione display ---------------------------------------------
  // Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Old Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }
  // Title screen
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);   // Draw white text
  display.setCursor(20, 10);
  display.setTextSize(2);                // Medium
  display.println("starting");
  show();
  // Inizializzazione LoadCells -------------------------------------
  // Inizializzazione LoadCells X
  Serial.println("Inialization LoadCell X");

  LoadCell_X.begin(LoadCell_X_DOUT_PIN, LoadCell_X_SCK_PIN);
  LoadCell_X.set_scale(calibration_X);  // this value is obtained by calibrating the scale with known weights; see the README for details   scale.set_scale(2280.f)
  LoadCell_X.tare();                // reset the scale to 0

  Serial.println("LoadCells X calibrated");
  // Inizializzazione LoadCells X Completed ---------------------------------



  displacementPerStep = screwPitch / stepPerRev;

// setup timer 5
  TCCR5A = 0;          // normal operation
  //TCCR5B = bit(WGM12) | bit(CS10)| bit(CS12);   // CTC, prescaler 1024
  TCCR5B = bit(WGM12) | bit(CS10);   // CTC, prescaler 1 0B00001001 WGM12 è settatto a 1 per avere il CTC (Cleat Timer Compare) con il valore di OCR5A 
 //   Serial.print("TCCR5A = ");
 //   Serial.println(TCCR5A, BIN);
  //  Serial.print("TCCR5B = ");
  //  Serial.println(TCCR5B, BIN);
  OCR5A =  4000;    // con prescaler 1, periodo clock 62.5nS --> periodo di interrupt 250microS -->4000 Step/s
// TIMSK5 = bit (OCIE1A);             // Attiva timer inteterrupt settando a 1 il bit OCIE1A
  TIMSK5 = 0;              // Disattiva interrupt timer 5
//    Serial.print("TIMSK5 = ");
//    Serial.println(TIMSK5, BIN);
// end of setup TIMER 5



  //---------Setup completed--------------------------

   MaxTravel = readLongFromEEPROM(addressTravel);
   TensSpeed = EEPROM.read(addressSpeed);
   SD = EEPROM.read(addressSD);
}


void loop() {


  switch (stato) {
    case 0:
		mainloop();
	  break;
    case 1010:
		Max_travel();
    break;
    case 1020:
		SetSpeed();
    break;
    case 1025:
		SetData();
    break;	  
    case 1030:
		Balance();
    break;
    case 1040:
		Start_Tension();
    break;
	  case 1050:  
		Homing_menu();
	break;
  }

}

void setStato(int s) {
  stato = s;
  FIRST = true;
  display.clearDisplay();
  delay(100);
}


void mainloop() {            //stato 0

  Tens_X_measurement();

  if (FIRST) {
    homing_procedure();
    setStato(0);
    Serial.print("Stato: \t");
    Serial.println(stato);
    LoadCell_X.tare();

    FIRST = false;
  }
  unsigned long currentMillis = millis();

  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(45, 4);
    display.println("Ready");
    display.setCursor(5, 25);

    display.print("Force:");
    display.setCursor(70, 25);
    display.print(Tension_X);
    display.setCursor(107, 25);
    display.print("N");
    display.setCursor(5, 35); 
    display.print("Position:");
    display.setCursor(70, 35);
    display.print(X_position);
    display.setCursor(107, 35);
    display.print("mm");

    display.setCursor(5, 55);

    display.print("S: Menu");
    display.setCursor(85, 55);
    display.print("D: Bal");
    show();
  }

  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();

  if (button_D.fell()) { //Se il pulsante è premuto
    LoadCell_X.tare();  //Azzera
  }
 

  if (button_S.fell()) { //Se il pulsante è premuto
    setStato(1010);  //Passa al menù tensioning
  }

}


void Max_travel() {       //stato 1010
  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);
    FIRST = false;
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(32, 5);
    display.println("Max travel");
    display.setCursor(5, 20);
    display.setTextSize(1);
    display.print("Travel:");
    display.setCursor(70, 20);
    display.print(MaxTravel);
    display.setCursor(107, 20);
    display.print("mm");

    display.setCursor(5, 35);
    display.print("S:Save");
    display.setCursor(85, 35);
    display.print("U:prev");
    display.setCursor(5, 45);
    display.print("B:Back");
    display.setCursor(85, 45);
    display.print("D:Next");
    display.setCursor(5, 55);
    display.print("L:Dec");
    display.setCursor(85, 55);
    display.print("R:Inc");
    show();
  }


  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();


  
  if (button_R.fell()) { //se il pulsante è premuto
    MaxTravel = MaxTravel + 1; //Increase speed
  }
  if (button_R.read() == LOW && button_R.currentDuration() > 500) { //se il pulsante è premuto
    MaxTravel = MaxTravel + 10; //Increase speed
    delay(100);
  }

  if (button_L.fell()) { //se il pulsante è premuto
    MaxTravel = MaxTravel - 1; //decrease speed
  }
  if (button_L.read() == LOW && button_L.currentDuration() > 500) { //se il pulsante è premuto
    MaxTravel = MaxTravel - 10; //decrease speed
    delay(100);
  }
  if (button_S.fell()) { //se il pulsante è premuto
    //EEPROM.write(addressTravel, MaxTravel);
    writeLongToEEPROM(addressTravel, MaxTravel);
    Serial.println("MaxTravel Saved on EEPROM"); 
    int pausa=0;
    while (pausa==0){
        display.clearDisplay();
        display.setTextSize(1);
        display.setCursor(32, 5);
        display.println("Max travel");
        display.setCursor(5, 20);
        display.setTextSize(1);
        display.print("SAVED in EEPROM");

        display.setCursor(5, 35);
        display.print("S:Save");
        display.setCursor(85, 35);
        display.print("U:prev");
        display.setCursor(5, 45);
        display.print("B:Back");
        display.setCursor(85, 45);
        display.print("D:Next");
        display.setCursor(5, 55);
        display.print("L:Dec");
        display.setCursor(85, 55);
        display.print("R:Inc");
        show();
        delay(500);
        pausa=1;
      }
        
  }

  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Vai al menu "Main loop"
  }

  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1050);  //Vai al menu "Homing"
   // stepperX.disableOutputs();
    stepperX.enableOutputs();

  }

  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1020);  //Vai al menu "Balance"
    stepperX.disableOutputs();

  }
 

}



void SetSpeed() {             //stato 1020

  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);

    FIRST = false;
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(20, 5);
    display.println("Travel Speed");
    display.setCursor(5, 20);
    display.print("Speed");
    display.setCursor(55, 20);
    display.print(TensSpeed);
    display.setCursor(90, 20);
    display.print("mm/min");
    display.setCursor(5, 35);
    display.print("S:Save");
    display.setCursor(85, 35);
    display.print("U:prev");
    display.setCursor(5, 45);
    display.print("B:Back");
    display.setCursor(85, 45);
    display.print("D:Next");
    display.setCursor(5, 55);
    display.print("L:Dec");
    display.setCursor(85, 55);
    display.print("R:Inc");
    show();
  }

  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();


  if (button_R.fell()) { //se il pulsante è premuto
    TensSpeed = TensSpeed + 1; //Increase speed
  }
  if (button_R.read() == LOW && button_R.currentDuration() > 500) { //se il pulsante è premuto
    TensSpeed = TensSpeed + 10; //Increase speed
    delay(200);
  }


  if (button_L.fell()) { //se il pulsante è premuto
    TensSpeed = TensSpeed - 1; //decrease speed
  }

  if (button_L.read() == LOW && button_L.currentDuration() > 500) { //se il pulsante è premuto
    TensSpeed = TensSpeed - 10; //decrease speed
    delay(200);
  }

  if (button_S.fell()) { //se il pulsante è premuto
    EEPROM.write(addressSpeed, TensSpeed);
    Serial.println("TensSpeed Saved on EEPROM"); 

    int pausa=0;
    while (pausa==0){
        display.clearDisplay();
        display.setTextSize(1);
        display.setCursor(20, 5);
        display.println("Travel Speed");
        display.setCursor(5, 20);
        display.print("SAVED on EEPROM");

        display.setCursor(5, 35);
        display.print("S:Save");
        display.setCursor(85, 35);
        display.print("U:prev");
        display.setCursor(5, 45);
        display.print("B:Back");
        display.setCursor(85, 45);
        display.print("D:Next");
        display.setCursor(5, 55);
        display.print("L:Dec");
        display.setCursor(85, 55);
        display.print("R:Inc");
        show();
        delay(500);
        pausa=1;
      }
  }

  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Vai al menu "Main loop"
  }

  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1010);  //Vai al menu "Balance Tensioning"
    stepperX.disableOutputs();

  }

  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1025);  //Vai al menu "Start Tensioning"
    stepperX.enableOutputs();

  }
  

}


void SetData() {       //stato 1025
  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);
    FIRST = false;
  }



  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(40, 5);
    display.println("DATA ON");

	if (SD==1) {
    display.setCursor(45, 25);
		display.print("SD Card");
	} else if (SD==0){
    display.setCursor(55, 25);
		display.print("USB");
	}
   
    display.setCursor(5, 45);
    display.setTextSize(1);
    display.print("S/B:Save/Back");
    display.setCursor(85, 45);
    display.print("U:Prev");
    display.setCursor(5, 55);
    display.print("L/R:SD/USB");
    display.setCursor(85, 55);
    display.print("D:Next");
    show();
  }

  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();


  if (button_S.fell()) { //Se il pulsante è premuto
		EEPROM.write(addressSD, SD);
		Serial.println("Data on ... saved on EEPROM"); 
		int pausa=0;
		while (pausa==0){
			display.clearDisplay();
			display.setTextSize(1);
      display.setCursor(40, 5);
      display.println("DATA ON");
			display.setCursor(17, 25);
			display.print("SAVED in EEPROM");
	
			display.setCursor(5, 45);
			display.setTextSize(1);
			display.print("S/B:Save/Back");
			display.setCursor(85, 45);
			display.print("U:Prev");
			display.setCursor(5, 55);
			display.print("L/R:SD/USB");
			display.setCursor(85, 55);
			display.print("D:Next");
			show();
			delay(500);
			pausa=1;	
      
		}
	}
	
  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Torna al "Menù principale"
    stepperX.disableOutputs();
  }

  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1030);  //Vai al menu "Balance"
    stepperX.disableOutputs();
  }

  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1020);  //Vai al menu "Set Speed"
    stepperX.disableOutputs();
  }
  if (button_L.fell()) { //se il pulsante è premuto
   SD = 1;
    stepperX.disableOutputs();
  }
    if (button_R.fell()) { //se il pulsante è premuto
   SD = 0;
    stepperX.disableOutputs();
  }
  
  
}



void Balance() {       //stato 1030
  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);
    FIRST = false;
  }

  Tens_X_measurement();


  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(20, 5);
    display.println("Balance force");
    display.setCursor(5, 30);
    display.setTextSize(1);
    display.print("Force:");
    display.setCursor(50, 30);
    display.print(Tension_X);
    display.setCursor(107, 30);
    display.print("N");

    display.setCursor(5, 45);
    display.setTextSize(1);
    display.print("S:Balance");
    display.setCursor(85, 45);
    display.print("U:Prev");
    display.setCursor(5, 55);
    display.print("B:Back");
    display.setCursor(85, 55);
    display.print("D:Next");
    show();
  }

  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();


  if (button_S.fell()) { //se il pulsante è premuto
    LoadCell_X.tare();  //Azzera

  }

  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Torna al "Menù principale"
    stepperX.disableOutputs();
  }

  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1040);  //Vai al menu "Travel Speed"
    stepperX.enableOutputs();
  }

  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1025);  //Vai al menu "Max Travel"
    stepperX.disableOutputs();
  }
  stepperX.setMaxSpeed(TensSpeed / screwPitch / 60 * stepPerRev);
}


void Start_Tension() {       //stato 1040
  float NumSteps_float=1;

  if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);
    stepperX.setSpeed(TensSpeed / screwPitch / 60 * stepPerRev);
  
    FIRST = false;
  }

  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    Tens_X_measurement(); 
    display.clearDisplay();
    display.setTextSize(1);
    display.setCursor(32, 5);
    display.println("Start Tens");

    display.setCursor(1, 20);
    display.setTextSize(1);
    display.print("Force:");
    display.setCursor(70, 20);
    display.print(Tension_X);
    display.setCursor(107, 20);
    display.print("N");

    display.setCursor(1, 30); 
    display.print("Position:");
    display.setCursor(70, 30);
    display.print(X_position);
    display.setCursor(107, 30);
    display.print("mm");

    display.setCursor(1, 45);
    display.print("S/R:Strt/Stp");
    display.setCursor(90, 45);
    display.print("U:Prev");
    display.setCursor(1, 55);
    display.print("L/B:UnLd/Bck");
    display.setCursor(90, 55);
    display.print("D:Next");
    show();
  }
  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();

  if (button_S.fell()) { //  Se il valore del pulsante è 1
    Serial.println("Start"); //Fa partire il motore
    LoadCompleted=false;
    NumSteps_float = MaxTravel * stepPerRev / screwPitch;
    long NumSteps=(long)NumSteps_float;
    
      Serial.print("Max travel:");
      Serial.println(MaxTravel);
      
      Serial.print("StepsPerRev:");
      Serial.println(stepPerRev); 

      Serial.print("screwPitch:");
      Serial.println(screwPitch);
      
    Serial.print("NumSteps:");
    Serial.println(NumSteps);
    Load(NumSteps); 
    Tens_X_measurement();
 
    stepperX.stop();

    Serial.println("Travel completed");
  }

  if (button_R.fell()) { //se il pulsante è premuto
    Serial.println("Stop"); //Arresta il motore
    stepperX.stop();

  }

  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1030); //Set Balance
    stepperX.disableOutputs();
   // stepperY.disableOutputs();
  }
  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1050); //homing
    stepperX.enableOutputs();

  }
  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Vai al menu "main menu"
    stepperX.disableOutputs();
   // stepperY.disableOutputs();
  }
  if (button_L.fell()) { //se il pulsante è premuto
    homing_procedure(); //Chiama la funzione homing
  }

}


void  Homing_menu() {     // stato 1050
   if (FIRST) {
    Serial.print("Stato: \t");
    Serial.println(stato);
    display.clearDisplay();


    display.setCursor(28, 10);
    display.setTextSize(1);
    display.print("Homing Menu");

    display.setCursor(5, 45);
    display.print("S:Start");
    display.setCursor(85, 45);
    display.print("U:Prev");
    display.setCursor(5, 55);
    display.print("B:Back");
    display.setCursor(85, 55);
    display.print("D:Next");
    show();
    FIRST = false;
  }
  button_D.update();
  button_R.update();
  button_S.update();
  button_L.update();
  button_U.update();
  button_B.update();

  // key = digitalRead(button_S);  //Lettura del pulsante
  if (button_S.fell()) { //se il pulsante è premuto
    homing_procedure();
    setStato(0);  //Esegui l'homing e passa a "Tensioning_menu"
  }

  //key = digitalRead(button_B);  //Lettura del pulsante
  if (button_B.fell()) { //se il pulsante è premuto
    setStato(0);  //Torna al Main menù
  }
  //key = digitalRead(button_D);  //Lettura del pulsante
  if (button_D.fell()) { //se il pulsante è premuto
    setStato(1010);  //Vai al menu "Max travel"
  }
  //key = digitalRead(button_U);  //Lettura del pulsante
  if (button_U.fell()) { //se il pulsante è premuto
    setStato(1040);  //Vai al menu "Start"
  }
}


void homing_procedure() {   
  display.clearDisplay();
  //display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);   // Draw white text
  display.setCursor(20, 10);
  display.setTextSize(2);                // Medium
  display.println("Homing X");
  show();

  stepperX.enableOutputs();

  homeSw_x.update();
 // homeSw_y.update();
  Serial.println("Stepper X is Homing . . . . . . . . . . . ");

  while (homeSw_x.read() == 1) {     // Make the Stepper move CCW until the switch is activated
    homeSw_x.update();
    stepperX.moveTo(initial_homingX);   // Set the position to move to
    initial_homingX--;                  // Decrease by 1 for next move if needed
    stepperX.run();                     // Start moving the stepper
  }

  stepperX.setCurrentPosition(0);       // Set the current position as zero for now
  initial_homingX = 1;


  while (homeSw_x.read() == 0) {    // Make the Stepper move CW until the switch is deactivated
    homeSw_x.update();
    stepperX.moveTo(initial_homingX);
    stepperX.run();
    initial_homingX++;
  }

  stepperX.setCurrentPosition(0);
  Serial.println("Homing X Completed");
  Serial.println("");
  stepperX.disableOutputs();
   X_position = 0;
   setStato(0);
  // Homing X procedure Completed

}


void Load(long Steps) { 

  Tens_X_measurement();

  TIMSK5 = bit (OCIE1A); //attiva timer 5
  stepperX.setMaxSpeed(TensSpeed / screwPitch / 60 * stepPerRev);
  Serial.print("step/sec:  ");
  Serial.println(TensSpeed / screwPitch / 60 * stepPerRev);
  
  stepperX.moveTo(Steps);


  while ((stepperX.distanceToGo() != 0)) {
      button_R.update();
      endSw_x.update();
      unsigned long currentMillis = millis();
      if (currentMillis - previousMillis >= 250) {
        previousMillis = currentMillis;
        Tens_X_measurement();
       // Tens_Y_measurement();
        X_position = stepperX.currentPosition() * displacementPerStep;
       // Y_position = stepperY.currentPosition() * displacementPerStep;

        Serial.print(X_position);
        Serial.print(",");
        Serial.println(Tension_X);
     

        display.clearDisplay();
        display.setTextSize(1);
        display.setCursor(32, 5);
        display.println("Start Tens");

        display.setCursor(1, 20);
        display.setTextSize(1);
        display.print("Force:");
        display.setCursor(70, 20);
        display.print(Tension_X);
        display.setCursor(107, 20);
        display.print("N");
        display.setCursor(1, 30); 
        display.print("Position:");
        display.setCursor(70, 30);
        display.print(X_position);
        display.setCursor(107, 30);
        display.print("mm");


        display.setCursor(1, 45);
        display.print("S/R:Strt/Stp");
        display.setCursor(90, 45);
        display.print("U:Prev");
        display.setCursor(1, 55);
        display.print("L/B:UnLd/Bck");
        display.setCursor(90, 55);
        display.print("D:Next");
        show();
      
      }


      if (button_R.fell()) { //se il pulsante è premuto
        Serial.println("Stop"); //Arresta il motore
        stepperX.stop();
        Tens_X_measurement();
        LoadCompleted=true;
    
      }


      
      if (endSw_x.fell()) { //se il pulsante è premuto
        Serial.println("End Switch Hit"); //End Switch premuto
        stepperX.stop();
        Tens_X_measurement();
        LoadCompleted=true;
       
  
      }
    
    }
  LoadCompleted=true;
  TIMSK5 =0;
}



void show() { // Often used sequence - Function to simplify code
  display.display();
  //delay(500); display.fillScreen(SSD1306_BLACK);
}


void Tens_X_measurement() {
  Tension_X = LoadCell_X.get_units(1)*125;
}


void writeLongToEEPROM(int address, long value) {
  // Decompone il valore long in 4 byte
  byte byte1 = (value >> 24) & 0xFF;
  byte byte2 = (value >> 16) & 0xFF;
  byte byte3 = (value >> 8) & 0xFF;
  byte byte4 = value & 0xFF;

  // Scrive ogni byte in EEPROM
  EEPROM.write(address, byte4);
  EEPROM.write(address + 1, byte3);
  EEPROM.write(address + 2, byte2);
  EEPROM.write(address + 3, byte1);
}

long readLongFromEEPROM(int address) {
  long value = 0;
  value += ((long)EEPROM.read(address + 3)) << 24;
  value += ((long)EEPROM.read(address + 2)) << 16;
  value += ((long)EEPROM.read(address + 1)) << 8;
  value += EEPROM.read(address);
  return value;
}