Path finder robot code

 import RPi.GPIO as GPIO # Replace with appropriate library for your microcontroller (e.g., Arduino.h)

import time

# --- Pin Definitions ---
# Define the GPIO pins connected to the ultrasonic sensor
TRIG_PIN = 17  # Example: GPIO pin 17 for Trig
ECHO_PIN = 18  # Example: GPIO pin 18 for Echo

# Define GPIO pins for motor control (adjust based on your motor driver)
MOTOR_LEFT_FORWARD = 22  # Example
MOTOR_LEFT_BACKWARD = 23  # Example
MOTOR_RIGHT_FORWARD = 24  # Example
MOTOR_RIGHT_BACKWARD = 25  # Example

# --- Robot Parameters ---
ROBOT_WIDTH = 15  # Distance between wheels in cm (approximate)
WHEEL_DIAMETER = 6.5  # Diameter of the wheels in cm (approximate)
WHEEL_CIRCUMFERENCE = 3.14159 * WHEEL_DIAMETER # Calculate wheel circumference
DISTANCE_THRESHOLD = 25  # Distance in cm to trigger obstacle avoidance
TURN_DELAY = 0.2  # Delay in seconds for a short turn
BASE_SPEED = 50 # Duty Cycle for basic speed
TURN_SPEED = 50 # Duty Cycle for Turn speed

# --- Setup (Initialization) ---
def setup():
    GPIO.setmode(GPIO.BCM)  # Use Broadcom pin numbering
    GPIO.setwarnings(False)  # Suppress warnings

    # Ultrasonic sensor pins
    GPIO.setup(TRIG_PIN, GPIO.OUT)
    GPIO.setup(ECHO_PIN, GPIO.IN)

    # Motor control pins
    GPIO.setup(MOTOR_LEFT_FORWARD, GPIO.OUT)
    GPIO.setup(MOTOR_LEFT_BACKWARD, GPIO.OUT)
    GPIO.setup(MOTOR_RIGHT_FORWARD, GPIO.OUT)
    GPIO.setup(MOTOR_RIGHT_BACKWARD, GPIO.OUT)

    # Set up PWM for motor speed control (adjust frequency as needed)
    global pwm_left_forward, pwm_left_backward, pwm_right_forward, pwm_right_backward

    pwm_left_forward  = GPIO.PWM(MOTOR_LEFT_FORWARD, 100)  # Frequency 100 Hz
    pwm_left_backward = GPIO.PWM(MOTOR_LEFT_BACKWARD, 100)
    pwm_right_forward = GPIO.PWM(MOTOR_RIGHT_FORWARD, 100)
    pwm_right_backward = GPIO.PWM(MOTOR_RIGHT_BACKWARD, 100)

    pwm_left_forward.start(0) # Initialize with 0% duty cycle (stopped)
    pwm_left_backward.start(0)
    pwm_right_forward.start(0)
    pwm_right_backward.start(0)



# --- Distance Measurement ---
def get_distance():
    """Measures distance in cm using the ultrasonic sensor."""
    GPIO.output(TRIG_PIN, True)
    time.sleep(0.00001)  # Send a 10us pulse
    GPIO.output(TRIG_PIN, False)

    pulse_start_time = time.time()
    pulse_end_time = time.time()

    # Wait for the echo pin to go high (with a timeout)
    max_time = time.time() + 0.05 # 50ms timeout
    while GPIO.input(ECHO_PIN) == 0:
        pulse_start_time = time.time()
        if time.time() > max_time:
            return 999 # Indicate a timeout or error

    # Wait for the echo pin to go low (with a timeout)
    max_time = time.time() + 0.05
    while GPIO.input(ECHO_PIN) == 1:
        pulse_end_time = time.time()
        if time.time() > max_time:
            return 999 #Indicate a timeout or error

    pulse_duration = pulse_end_time - pulse_start_time
    distance = (pulse_duration * 34300) / 2  # Speed of sound = 34300 cm/s
    return distance


# --- Motor Control Functions ---
def move_forward(speed):
    """Moves the robot forward."""
    pwm_left_forward.ChangeDutyCycle(speed)
    pwm_left_backward.ChangeDutyCycle(0)
    pwm_right_forward.ChangeDutyCycle(speed)
    pwm_right_backward.ChangeDutyCycle(0)



def move_backward(speed):
    """Moves the robot backward."""
    pwm_left_forward.ChangeDutyCycle(0)
    pwm_left_backward.ChangeDutyCycle(speed)
    pwm_right_forward.ChangeDutyCycle(0)
    pwm_right_backward.ChangeDutyCycle(speed)


def turn_left(speed):
    """Turns the robot left."""
    pwm_left_forward.ChangeDutyCycle(0)
    pwm_left_backward.ChangeDutyCycle(speed)
    pwm_right_forward.ChangeDutyCycle(speed)
    pwm_right_backward.ChangeDutyCycle(0)



def turn_right(speed):
    """Turns the robot right."""
    pwm_left_forward.ChangeDutyCycle(speed)
    pwm_left_backward.ChangeDutyCycle(0)
    pwm_right_forward.ChangeDutyCycle(0)
    pwm_right_backward.ChangeDutyCycle(speed)


def stop():
    """Stops the robot."""
    pwm_left_forward.ChangeDutyCycle(0)
    pwm_left_backward.ChangeDutyCycle(0)
    pwm_right_forward.ChangeDutyCycle(0)
    pwm_right_backward.ChangeDutyCycle(0)


# --- Pathfinding Logic ---
def avoid_obstacle():
    """Simple obstacle avoidance strategy."""
    stop()
    time.sleep(0.2)  # Short pause

    # Back up a little
    move_backward(BASE_SPEED)
    time.sleep(0.5)
    stop()
    time.sleep(0.2)

    # Turn right and check distance
    turn_right(TURN_SPEED)
    time.sleep(TURN_DELAY) #Short Turn
    stop()
    right_distance = get_distance()
    time.sleep(0.2)

    # Turn left and check distance
    turn_left(TURN_SPEED)
    time.sleep(2 * TURN_DELAY)  # Turn back left and then a bit more.
    stop()
    left_distance = get_distance()
    time.sleep(0.2)

    # Turn back to original position.
    turn_right(TURN_SPEED)
    time.sleep(TURN_DELAY) #Short Turn
    stop()

    # Choose direction based on distances
    if right_distance > left_distance:
        print("Turning right")
        turn_right(TURN_SPEED)
        time.sleep(0.7)  # Turn for longer if right is clearer
        stop()
    else:
        print("Turning left")
        turn_left(TURN_SPEED)
        time.sleep(0.7)  # Turn for longer if left is clearer
        stop()


# --- Main Loop ---
def main():
    try:
        setup()
        print("Robot is ready.  Starting...")

        while True:
            distance = get_distance()
            print(f"Distance: {distance:.2f} cm")

            if distance < DISTANCE_THRESHOLD:
                print("Obstacle detected!")
                avoid_obstacle()
            else:
                move_forward(BASE_SPEED) # move forward at a slower speed to give more time for obstacle detection

            time.sleep(0.1)  # Small delay for stability

    except KeyboardInterrupt:
        print("Program stopped by user.")
    finally:
        stop()
        pwm_left_forward.stop()
        pwm_left_backward.stop()
        pwm_right_forward.stop()
        pwm_right_backward.stop()

        GPIO.cleanup()  # Clean up GPIO pins

if __name__ == "__main__":
    main()