//Arduino Human Following Robot using Stepper Motors
// Modified from DIY Builder's original code
// Modified to use Stepper motors instead of DC motors with AF_Motor library

//include the library code:
#include <NewPing.h>
#include <Servo.h>
#include <Stepper.h>

#define RIGHT A2              // Right IR sensor connected to analog pin A2 of Arduino Uno
#define LEFT A3               // Left IR sensor connected to analog pin A3 of Arduino Uno
#define TRIGGER_PIN A1        // Trigger pin connected to analog pin A1 of Arduino Uno
#define ECHO_PIN A0           // Echo pin connected to analog pin A0 of Arduino Uno
#define MAX_DISTANCE 200      // Maximum ping distance

// Define stepper motor parameters
const int stepsPerRevolution = 200;  // Most common stepper has 200 steps per revolution (1.8 deg/step)

// Define stepper motor pin connections
// Left side motors
Stepper leftStepper(stepsPerRevolution, 2, 3, 4, 5);    // Motor1 & Motor2 equivalent
// Right side motors
Stepper rightStepper(stepsPerRevolution, 6, 7, 8, 9);   // Motor3 & Motor4 equivalent

unsigned int distance = 0;    // Variable to store ultrasonic sensor distance
unsigned int Right_Value = 0; // Variable to store Right IR sensor value
unsigned int Left_Value = 0;  // Variable to store Left IR sensor value

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);  // NewPing setup of pins and maximum distance

Servo myservo; // Create servo object to control the servo
int pos = 0;   // Variable to store the servo position

// Define speed parameters for stepper motors
const int normalSpeed = 60;     // RPM
const int turningSpeed = 80;    // RPM

void setup() { // The setup function runs only once when power on the board or reset the board
  
  Serial.begin(9600); // Initialize serial communication at 9600 bits per second
  
  // Set initial speeds for stepper motors
  leftStepper.setSpeed(normalSpeed);
  rightStepper.setSpeed(normalSpeed);
  
  myservo.attach(10); // Servo attached to pin 10 of Arduino UNO
  
  // Servo initialization sequence
  for(pos = 90; pos <= 180; pos += 1) {    // Goes from 90 degrees to 180 degrees
    myservo.write(pos);                    // Tell servo to move according to the value of 'pos' variable
    delay(15);                             // Wait 15ms for the servo to reach the position
  } 
  for(pos = 180; pos >= 0; pos-= 1) {      // Goes from 180 degrees to 0 degrees
    myservo.write(pos);                    // Tell servo to move according to the value of 'pos' variable
    delay(15);                             // Wait 15ms for the servo to reach the position
  }
  for(pos = 0; pos<=90; pos += 1) {        // Goes from 0 degrees to 90 degrees
    myservo.write(pos);                    // Tell servo to move according to the value of 'pos' variable
    delay(15);                             // Wait 15ms for the servo to reach the position
  }
  
  pinMode(RIGHT, INPUT); // Set analog pin RIGHT as an input
  pinMode(LEFT, INPUT);  // Set analog pin LEFT as an input
}

// The loop function runs forever
void loop() {                             
  delay(50);                              // Wait 50ms between pings
  distance = sonar.ping_cm();             // Send ping, get distance in cm and store it in 'distance' variable
  
  Serial.print("distance: ");                   
  Serial.println(distance);               // Print the distance in serial monitor

  Right_Value = digitalRead(RIGHT);       // Read the value from Right IR sensor
  Left_Value = digitalRead(LEFT);         // Read the value from Left IR sensor
 
  Serial.print("RIGHT: ");                      
  Serial.println(Right_Value);            // Print the right IR sensor value in serial monitor
  Serial.print("LEFT: ");                       
  Serial.println(Left_Value);             // Print the left IR sensor value in serial monitor

  // Set motor speeds based on the conditions
  if((distance > 1) && (distance < 15)) {  // Check whether the ultrasonic sensor's value stays between 1 to 15
    // Move Forward
    Serial.println("Moving Forward");
    leftStepper.setSpeed(normalSpeed);
    rightStepper.setSpeed(normalSpeed);
    
    // Step both motors forward (positive steps)
    leftStepper.step(10);
    rightStepper.step(10);
    
  } else if((Right_Value==0) && (Left_Value==1)) {  // Turn left
    // Turn Left
    Serial.println("Turning Left");
    leftStepper.setSpeed(turningSpeed);
    rightStepper.setSpeed(turningSpeed);
    
    // Step left motor forward, right motor backward
    leftStepper.step(10);
    rightStepper.step(-10);
    
  } else if((Right_Value==1) && (Left_Value==0)) {  // Turn right
    // Turn Right
    Serial.println("Turning Right");
    leftStepper.setSpeed(turningSpeed);
    rightStepper.setSpeed(turningSpeed);
    
    // Step left motor backward, right motor forward
    leftStepper.step(-10);
    rightStepper.step(10);
    
  } else if(distance > 15) {  // If nothing detected or too far, stop
    // Stop - for stepper motors, not stepping means stopped
    Serial.println("Stopped");
    // No step commands = no movement
    delay(10);  // Small delay to prevent excessive loop cycling
  }
}