// Raspberry Pi Pico + Stepper Motor Example

#define DIR_PIN1 2 //main cylindr
#define STEP_PIN1 3 //main cylindr
#define DIR_PIN2 4 //seconde main cylindr
#define STEP_PIN2 5 //seconde main cylindr
#define DIR_PIN3 6 //first elbow cylindr
#define STEP_PIN3 7 //first elbow cylindr
#define DIR_PIN4 8 //seconde elbow cylindr
#define STEP_PIN4 9 //seconde elbow cylindr
#define DIR_PIN5 10 //first hand cylindr
#define STEP_PIN5 11 //first hand cylindr
#define DIR_PIN6 14 //seconde hand cylindr
#define STEP_PIN6 15 //seconde hand cylindr
#define STEPS_PER_REV 200 

const int stepsFor90Degrees = STEPS_PER_REV / 4;

void setup() {
  pinMode(STEP_PIN1, OUTPUT);
  pinMode(DIR_PIN1, OUTPUT);
  digitalWrite(STEP_PIN1, LOW);

  // Move 200 steps (one rotation) CW over one second
  TakeCup();
}

void loop() {
  
}

void RotateCylindre(int dirPin, int stepPin,bool direction, int angle){
    int steps = (STEPS_PER_REV * angle) / 360;
    if(direction)
      digitalWrite(dirPin, HIGH);
    else
      digitalWrite(dirPin, LOW);
    for (int i = 0; i < steps; i++) {
      digitalWrite(stepPin, HIGH);
      digitalWrite(stepPin, LOW);
      delay(5); // 5 ms * 200 = 1 second
    }
}

void TakeCup(){
  RotateCylindre(DIR_PIN2,STEP_PIN2,true,150);
  RotateCylindre(DIR_PIN3,STEP_PIN3,false,45);
  RotateCylindre(DIR_PIN5,STEP_PIN5,true,45);
}