#include "AccelStepper.h"
#define pul1 41 
#define dri1 40
#define pul2 5 
#define dri2 4
#define pul3 7 
#define dri3 6
#define pul4 9 
#define dri4 8
const int echo = 44;
int trig  = 45;
int x1 = 0;

AccelStepper stepper1(1,pul1,dri1 );
AccelStepper stepper2(1,pul2,dri2 );
AccelStepper stepper3(1,pul3,dri3 );
AccelStepper stepper4(1,pul4,dri4 );

void setup()
  
{
  Serial.begin(9600);
  stepper1.setMaxSpeed(1000);
  stepper1.setAcceleration(100);
  stepper2.setMaxSpeed(1000);
  stepper2.setAcceleration(100);
  stepper3.setMaxSpeed(1000);
  stepper3.setAcceleration(100);
  stepper4.setMaxSpeed(1000);
  stepper4.setAcceleration(100);
  pinMode(echo, OUTPUT);
  pinMode(trig, OUTPUT);
}

void loop()
{
  long duration, cm;
  digitalWrite(echo, 0);
  delayMicroseconds(2);
  digitalWrite(echo, 1);
  delayMicroseconds(5);
  digitalWrite(echo, 0);
  pinMode(trig, INPUT);
  duration = pulseIn(trig, 1);
  cm = microsecondsToCentimeters(duration);
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  Serial.print(x1);
  Serial.println();
  //delay(100);
 if (stepper1.currentPosition() >= 0 && stepper1.currentPosition() <= 200 && cm >=20 && x1 == 0 )
{
  stepper1.moveTo(200);
  stepper1.run();
  stepper2.moveTo(-200);
  stepper2.run();
  if (stepper1.currentPosition() >= 200 && stepper2.currentPosition() >= -200)
  {
    x1++;
  }
}
 if (stepper1.currentPosition() != 0 && stepper2.currentPosition() != 0 && cm >=20 && x1 == 1 )
{
    stepper1.setSpeed(100);
    stepper1.moveTo(0);
    stepper1.runSpeedToPosition();
    stepper2.setSpeed(100);
    stepper2.moveTo(0);
    stepper2.runSpeedToPosition();
    if (stepper1.currentPosition() == 0 && stepper1.currentPosition() == 0)
  {
    x1++;
  }
}
  //if (stepper2.currentPosition() == 200)
  //{
      //stepper1.moveTo(0);
      //stepper1.run();
      //stepper2.moveTo(0);
      //stepper2.run();
      //x1++;
  //}
//}
}
long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 29 / 2;
}