int stpPin = A4;
int dirPin = A5;

int stepsPerRevolution = 200;  // Number of steps per revolution of your motor
int currentStep = 0;  // Current step position
int direction = 1;  // Motor direction (1 for clockwise, -1 for counterclockwise)

AccelStepper stepper(AccelStepper::DRIVER, stpPin, dirPin);

void setup() {
  shoulderL.attach(shoulderLpin);
  armL.attach(armLpin);
  shoulderR.attach(shoulderRpin);
  armR.attach(armRpin);

  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);

  Serial.begin(9600);
  ready();

}

void loop() {
  stepper.setSpeed(100);  // Set the desired speed (in steps per second)
  stepper.moveTo(200);
  stepper.run();
  delay(1000);
}

void ready() {
  shoulderL.write(0);
  armL.write(90);
  shoulderR.write(180);
  armR.write(90);
}

void normal() {
  shoulderL.write(180);
  armL.write(180);
  shoulderR.write(0);
  armR.write(0);
}

void ura() {
  shoulderL.write(0);
  armL.write(180);
  shoulderR.write(180);
  armR.write(0);
}

// void STEPPER(int DIR) //0=high, 1=low
// {
//   stepper.setSpeed(100);  // Set the desired speed (in steps per second)
//   stepper.setDirection(DIR);  // Set the desired direction (0 for clockwise, 1 for counterclockwise)

//   // Step the motor one step
//   stepper.runSpeed();

//   // Uncomment the following code if needed for additional functionality
//   /
//   for (int j = 0; j < revNum; j++)
//   {
//     digitalWrite(dirPin, DIR); // Set the direction pin
//     for (int i = 0; i < stepsPerRevolution; i++)
//     {
//       digitalWrite(stpPin, HIGH);  // Step the motor
//       delayMicroseconds(300);  // Adjust the delay as needed
//       digitalWrite(stpPin, LOW);
//       delayMicroseconds(300);
//     }
//     Serial.print(" motor step : ");
//     Serial.println
//   }
// }