TURL-X FINAL COMMAND

// Obstacle Avoiding TURL-X with Scanning Sensor
// Servo on pin 3 sweeps the HC-SR04 left and right
// HC-SR04: Trig = 13, Echo = 12
// LED on pin 2 flashes when it sees something close

#include <Servo.h>          // This library lets us control the servo easily

Servo ultraServo;           // Give the servo a name

// Pin names (easy to change later)
#define MOTOR1_IN1  9       // Right motor pin 1
#define MOTOR1_IN2 10       // Right motor pin 2
#define MOTOR2_IN1  5       // Left motor pin 1
#define MOTOR2_IN2  6       // Left motor pin 2

#define TRIG_PIN   13
#define ECHO_PIN   12
#define SERVO_PIN   3
#define LED_PIN     2

void setup() {
  // Tell Arduino these pins send signals out
  pinMode(MOTOR1_IN1, OUTPUT);
  pinMode(MOTOR1_IN2, OUTPUT);
  pinMode(MOTOR2_IN1, OUTPUT);
  pinMode(MOTOR2_IN2, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);

  ultraServo.attach(SERVO_PIN);   // Plug the servo into pin 3
  ultraServo.write(90);           // Start looking straight ahead

  // Make sure motors are stopped at the beginning
  digitalWrite(MOTOR1_IN1, LOW);
  digitalWrite(MOTOR1_IN2, LOW);
  digitalWrite(MOTOR2_IN1, LOW);
  digitalWrite(MOTOR2_IN2, LOW);

  delay(1000);   // Wait 1 second so you can see it’s ready
}

void loop() {
  // === 1. Go forward ===
  digitalWrite(MOTOR1_IN1, HIGH);   // Right motor forward
  digitalWrite(MOTOR1_IN2, LOW);
  digitalWrite(MOTOR2_IN1, HIGH);   // Left motor forward
  digitalWrite(MOTOR2_IN2, LOW);

  // === 2. Sweep the sensor slowly left to right ===
  for(int angle = 60; angle <= 120; angle += 4) {
    ultraServo.write(angle);        // Move servo a little bit
    delay(20);                      // Wait so the move looks slow and smooth

    // Measure distance right now
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(5);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    long duration = pulseIn(ECHO_PIN, HIGH);
    int distance = duration / 58;

    // If something is closer than 30 cm → obstacle!
    if(distance > 0 && distance <= 30) {
      avoidObstacle();
    }
  }

  // === 3. Sweep the sensor slowly right to left ===
  for(int angle = 120; angle >= 60; angle -= 4) {
    ultraServo.write(angle);
    delay(20);

    // Measure distance again
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(5);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    long duration = pulseIn(ECHO_PIN, HIGH);
    int distance = duration / 58;

    if(distance > 0 && distance <= 30) {
      avoidObstacle();
    }
  }
}

// ─────────────────────────────────────────────
// This is a helper block called a “function”
// You call it by writing avoidObstacle();
// It does everything needed when the robot sees something close
// ─────────────────────────────────────────────
void avoidObstacle() {
  // Stop moving forward
  digitalWrite(MOTOR1_IN1, LOW);
  digitalWrite(MOTOR1_IN2, LOW);
  digitalWrite(MOTOR2_IN1, LOW);
  digitalWrite(MOTOR2_IN2, LOW);

  // Keep turning right until the path ahead is clear (>40 cm)
  bool pathIsClear = false;
  while(!pathIsClear) {
    
    // Spin right (turn in place)
    digitalWrite(MOTOR1_IN1, LOW);    // Right motor backward
    digitalWrite(MOTOR1_IN2, HIGH);
    digitalWrite(MOTOR2_IN1, HIGH);   // Left motor forward
    digitalWrite(MOTOR2_IN2, LOW);

    // Look straight ahead while turning
    ultraServo.write(90);
    delay(30);

    // Measure distance straight ahead
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(5);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    long duration = pulseIn(ECHO_PIN, HIGH);
    int distance = duration / 58;

    // If nothing is closer than 40 cm → path is clear!
    if(distance > 40 || distance == 0) {
      pathIsClear = true;
    }

    // Flash the LED (200 ms on, 200 ms off)
    digitalWrite(LED_PIN, HIGH);
    delay(200);
    digitalWrite(LED_PIN, LOW);
    delay(200);
  }

  // Stop turning when path is clear
  digitalWrite(MOTOR1_IN1, LOW);
  digitalWrite(MOTOR1_IN2, LOW);
  digitalWrite(MOTOR2_IN1, LOW);
  digitalWrite(MOTOR2_IN2, LOW);

  delay(300);   // Small pause before going forward again
}