Lesson 12: Ultrasonic Sensors and Obstacle Detection

// HC-SR04 Ultrasonic Sensor
#define TRIG_PIN 7
#define ECHO_PIN 8

void setup() {
    Serial.begin(9600);
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
    Serial.println("🔊 Ultrasonic Distance Sensor Ready");
}

float getDistance() {
    // Send trigger pulse
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    // Measure echo pulse duration
    long duration = pulseIn(ECHO_PIN, HIGH);
    // Calculate distance in cm
    float distance = (duration * 0.0343) / 2;
    return distance;
}

void loop() {
    float dist = getDistance();
    Serial.print("Distance: ");
    Serial.print(dist);
    Serial.println(" cm");
    delay(500);
}

// Obstacle detection with thresholds
#define STOP_DISTANCE 10    // Stop if closer than 10cm
#define SLOW_DISTANCE 30    // Slow down if closer than 30cm
#define WARN_DISTANCE 50    // Warning if closer than 50cm

enum RobotState {
    MOVING_FAST,
    MOVING_SLOW,
    STOPPED,
    TURNING
};

RobotState currentState = MOVING_FAST;

void checkObstacles() {
    float distance = getDistance();
    // Validate reading (HC-SR04 range: 2-400cm)
    if (distance < 2 || distance > 400) {
        Serial.println("⚠️ Invalid sensor reading");
        return;
    }
    // Decision making based on distance
    if (distance <= STOP_DISTANCE) {
        currentState = STOPPED;
        stopRobot();
        Serial.println("🛑 EMERGENCY STOP - Obstacle too close!");
        delay(1000);
        // Try to turn away
        currentState = TURNING;
        turnRight(150);
        delay(500);
    } else if (distance <= SLOW_DISTANCE) {
        currentState = MOVING_SLOW;
        moveForward(100);  // Reduced speed
        Serial.println("🐌 Slowing down - Obstacle detected");
    } else if (distance <= WARN_DISTANCE) {
        currentState = MOVING_FAST;
        moveForward(200);  // Normal speed
        Serial.println("⚠️ Obstacle ahead - Monitoring");
    } else {
        currentState = MOVING_FAST;
        moveForward(255);  // Full speed
        Serial.println("🚀 Path clear - Full speed");
    }
}

unsigned long lastTurnTime = 0;
int consecutiveObstacles = 0;

void smartNavigation() {
    float frontDistance = getDistance();
    // Check if we're stuck in a corner
    if (frontDistance <= STOP_DISTANCE) {
        consecutiveObstacles++;
        Serial.print("Obstacle count: ");
        Serial.println(consecutiveObstacles);
        if (consecutiveObstacles >= 3) {
            // Escape sequence - back up and turn around
            Serial.println(" Executing escape sequence");
            moveBackward(150);
            delay(1000);
            turnRight(200);
            delay(1500);  // 180 degree turn
            consecutiveObstacles = 0;
        } else {
            // Simple avoidance
            stopRobot();
            delay(500);
            // Random turn direction
            if (random(0, 2) == 0) {
                turnLeft(150);
                Serial.println(" Turning left");
            } else {
                turnRight(150);
                Serial.println(" Turning right");
            }
            delay(800);
        }
        lastTurnTime = millis();
    } else {
        // Clear path - reset obstacle counter
        consecutiveObstacles = 0;
        moveForward(200);
    }
}

// Wall following behavior
void followWall() {
    // Keep a constant distance from wall
    float wallDistance = getDistance();
    float targetDistance = 20.0;  // 20cm from wall
    float tolerance = 5.0;
    if (wallDistance < targetDistance - tolerance) {
        // Too close to wall - turn away
        setMotorSpeed(200, 150);  // Curve away
    } else if (wallDistance > targetDistance + tolerance) {
        // Too far from wall - turn toward
        setMotorSpeed(150, 200);  // Curve toward
    } else {
        // Perfect distance - go straight
        setMotorSpeed(200, 200);
    }
}

// Robust distance measurement
float getReliableDistance() {
    float readings[5];
    int validReadings = 0;
    // Take multiple readings
    for (int i = 0; i < 5; i++) {
        float dist = getDistance();
        if (dist >= 2 && dist <= 400) {
            readings[validReadings] = dist;
            validReadings++;
        }
        delay(50);  // Wait between readings
    }
    // Return median of valid readings
    if (validReadings >= 3) {
        // Sort readings
        for (int i = 0; i < validReadings - 1; i++) {
            for (int j = i + 1; j < validReadings; j++) {
                if (readings[i] > readings[j]) {
                    float temp = readings[i];
                    readings[i] = readings[j];
                    readings[j] = temp;
                }
            }
        }
        return readings[validReadings / 2];  // Median
    } else {
        Serial.println("⚠️ Sensor unreliable - using default");
        return 100;  // Safe default distance
    }
}