// https://arduino.stackexchange.com/questions/93302/how-to-drive-two-stepper-motors-and-use-an-ultrasonic-sensor-together

#include <Stepper.h>
#include <Ultrasonic.h>

/*
 * Pass as a parameter the trigger and echo pin, respectively,
 * or only the signal pin (for sensors 3 pins), like:
 * Ultrasonic ultrasonic(13);
 */
 
 Ultrasonic ultrasonic(13, 12);
 
 int distance;

 int slowSpeed          =  100;
 int driveSpeed         = 1000;
 int stepsPerRevolution =  100;
 int leftMotorStep      =    0;                         // current step speed and direction
 int rightMotorStep     =    0;

 int stepSpeed          =   10;                         // this is the motor speed

 const int stepInterval =    1;                         // Time interval between steps for both motors

 unsigned long leftMotorLastStepTime = 0;               // Last step time for each motor
 unsigned long rightMotorLastStepTime = 0;

 Stepper rightMotor(stepsPerRevolution, 8, 9, 10, 11);
 Stepper leftMotor (stepsPerRevolution, 4, 5,  6,  7);


 void setup() {
   Serial.begin(9600);
   rightMotor.setSpeed(1);
   leftMotor. setSpeed(1);
 }


 void loop() {

  distance = ultrasonic.read();

  Serial.print("Distance in cm: ");
  Serial.print(distance);  Serial.print("\t");

  switch (distance) {
    case 0 ... 14:
      Serial.println("avoid");
      avoidObstacle();                             // Perform obstacle avoidance behavior
      break;

    case 15 ... 29:
      Serial.println("approach");
      slowApproach();                              // Perform slow approach behavior
      break;

    default:
      Serial.println("drive");
      drive();                                     // Perform normal driving behavior
      break;
  }

  rightMotor.step(rightMotorStep);                 // this actually moves the motors
  leftMotor .step(leftMotorStep);
}

 void avoidObstacle() {
   halt();                                          // Stop the robot
   leftM();                                         // Rotate the robot to avoid the obstacle
 }

 void slowApproach() {
   rightMotor.setSpeed(slowSpeed);                  // Slow down the motors
   leftMotor. setSpeed(slowSpeed);
   forwardM();                                      // Move the robot forward slowly
 }

 void drive() {
   rightMotor.setSpeed(driveSpeed);                 // Set the motors back to the normal speed
   leftMotor. setSpeed(driveSpeed);
   forwardM();                                      // Move the robot forward
 }

 void forwardM() {
   rightMotorStep =  stepSpeed;                     // select direction of step only
   leftMotorStep  =  stepSpeed;                     // do not move motor
 }                                                  // actual stepping is done at end of loop()
 void leftM() {
   rightMotorStep =  stepSpeed;
   leftMotorStep  = -stepSpeed;
 }
 void rightM() {
   rightMotorStep = -stepSpeed;
   leftMotorStep  =  stepSpeed;
 }
 void reverseM() {
   rightMotorStep = -stepSpeed;
   leftMotorStep  = -stepSpeed;
 }
 void halt() {
   rightMotorStep =  0;
   leftMotorStep  =  0;
 }