// Nema 17 200 passi per giro accoppiato a 
// vite T8 Trapezoidale Senza Fine Ø8 Mm Pitch 2mm 1 Principio
#define DIR_PIN0  11
#define STEP_PIN0 12   // gearRatio 1:1 --> 1 giro= 200 step --> 2mm di spostamento
#define DIR_PIN1  8
#define STEP_PIN1 9   // set gearRatio 1.8:1 to get 1° for 1 step  (1.8=360/200)
#define DIR_PIN2  3
#define STEP_PIN2 4  // set gearRatio 1.8:1 to get 1° for 1 step  (1.8=360/200)
#define DELAY_ST 2000  // 2000 micros
#define DELAY_STZ 1000  // 2000 micros


int idMotor;  // 1,2 ... 

void setup() {
  pinMode(DIR_PIN0, OUTPUT);
  pinMode(STEP_PIN0, OUTPUT);
  pinMode(DIR_PIN1, OUTPUT);
  pinMode(STEP_PIN1, OUTPUT);
  pinMode(DIR_PIN2, OUTPUT);
  pinMode(STEP_PIN2, OUTPUT);
  delay(1000);
}

void loop() {
  // rotazione ORARIA 30°  
  movelinkTo(1,30,HIGH);  
  delay(1000);
 
  movelinkTo(2,90,HIGH);  
  delay(1000);
 
  movezTo(20,LOW);  
  delay(2000);

  movelinkTo(2,90,LOW);  
  delay(1000);

  movelinkTo(1,30,LOW);  
  delay(1000);

  movezTo(20,HIGH);  
  delay(2000);

}

void movelinkTo(int id, int postion, int orientation) {
  if (id==1) {
    digitalWrite(DIR_PIN1, orientation);
    for (int i = 0; i < postion; i++) {
      digitalWrite(STEP_PIN1, HIGH); delayMicroseconds(DELAY_ST);
      digitalWrite(STEP_PIN1, LOW);  delayMicroseconds(DELAY_ST);
    }
  }
  else if (id==2) {
    digitalWrite(DIR_PIN2, orientation);
    for (int i = 0; i < postion; i++) {
      digitalWrite(STEP_PIN2, HIGH); delayMicroseconds(DELAY_ST);
      digitalWrite(STEP_PIN2, LOW);  delayMicroseconds(DELAY_ST);
    }
  }

}

// spostamento relativo in mm
void movezTo(int z, int orientation) {
    // calcolo numero di step necessari (1mm=100 step)
    int step = z * 100; 
    digitalWrite(DIR_PIN0, orientation);
    for (int i = 0; i < step; i++) {
      digitalWrite(STEP_PIN0, HIGH); delayMicroseconds(DELAY_STZ);
      digitalWrite(STEP_PIN0, LOW);  delayMicroseconds(DELAY_STZ);
    }

}