#include <Stepper.h>
const int stepsPerRevolution = 200; // change this to match the number of steps per revolution for your motor
Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11); // initialize the stepper library on pins 8, 9, 10, 11

const int trigPin1 = 7; // connect the trig pin of ultrasonic sensor 1 to pin 7
const int echoPin1 = 6; // connect the echo pin of ultrasonic sensor 1 to pin 6
const int trigPin2 = 5; // connect the trig pin of ultrasonic sensor 2 to pin 5
const int echoPin2 = 4; // connect the echo pin of ultrasonic sensor 2 to pin 4

long int previousDistance;
const long maxDistance = 400;

//unsigned long duration;
int currentDistance1; // distance read by ultrasonic sensor 1
int currentDistance2; // distance read by ultrasonic sensor 2

void setup() 
{
  //myStepper.setSpeed(30); // set the speed of the stepper (30 RPM)
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  previousDistance = 0;
  Serial.begin(9600);
}

void loop() 
{
  long duration;
  // Measure distance from ultrasonic sensor 1
  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);

  duration = pulseIn(echoPin1, HIGH);
  currentDistance1 = (duration/2) / 29.1;

  // Measure distance from ultrasonic sensor 2
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  
  duration = pulseIn(echoPin2, HIGH);
  currentDistance2 = (duration/2) / 29.1;
  int currentDistance = min(currentDistance1,currentDistance2);
  Serial.print("Distance 1 & 2: ");
  Serial.println(currentDistance);

  if(currentDistance > previousDistance && currentDistance < maxDistance)
    {
      myStepper.step(1); // move the stepper forward 1 step
      previousDistance = currentDistance;
      /*currentDistance = 300;
      if (currentDistance < currentDistance && currentDistance < maxDistance)
      {
        myStepper.step(-10);
        previousDistance = currentDistance;
        currentDistance = 200;
      }*/
    }
  else if (currentDistance < previousDistance)
    {
      myStepper.step(-1); // move the stepper backward 1 step
      previousDistance = currentDistance;
    }
  else
    {
      myStepper.step(0); // stop the stepper 
    }
}