// 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);


//enum class Action : unsigned int8_t   { stopped = 0, driving = 1, approaching = 2, avoiding = 3 };       // pointer to messages[] 
enum class Action { stop, drive, approach, avoid };       // pointer to messages[] 

const String messageText[] = {"stopped", "driving", "approaching", "avoiding"};

int message;

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


void loop() {

  distance = ultrasonic.read();

  switch (distance) {
    case 0 ... 14:
      message = (int) Action::avoid;
      avoidObstacle();                             // Perform obstacle avoidance behavior
      break;

    case 15 ... 29:
      message = (int) Action::approach;
      slowApproach();                              // Perform slow approach behavior
      break;

    default:
      message = (int) Action::drive;
      drive();                                     // Perform normal driving behavior
      break;
  }


  Serial.print(millis());
  Serial.print("\tDistance in cm: ");  Serial.print(distance);
  Serial.print("\t");  Serial.println(messageText[message]);

  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;
}