​

# Import necessary libraries

import cv2

import numpy as np

import lgpio

from time import time, sleep

 

# Initialize GPIO chip

h = lgpio.gpiochip_open(0)

 

# Define motor GPIO pins

IN1, IN2 = 22, 20  

IN3, IN4 = 12, 16  

motor_pins = [IN1, IN2, IN3, IN4]

 

# Define ultrasonic sensor pins

TRIG, ECHO = 4, 17  

 

# Configure motor pins as outputs

for pin in motor_pins:

    lgpio.gpio_claim_output(h, pin)

 

# Configure ultrasonic sensor pins

lgpio.gpio_claim_output(h, TRIG)

lgpio.gpio_claim_input(h, ECHO)

 

# Initialize camera

cap = cv2.VideoCapture(0)

cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)

cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)

 

# Define HSV color range for detecting orange

lower_orange = np.array([9, 130, 130])

upper_orange = np.array([22, 255, 255])

 

# Basic movement control functions for robot

def motor_forward(): #moves forward

    lgpio.gpio_write(h, IN1, 1)

    lgpio.gpio_write(h, IN2, 0)

    lgpio.gpio_write(h, IN3, 1)

    lgpio.gpio_write(h, IN4, 0)

 

def motor_left(): #moves left

    lgpio.gpio_write(h, IN1, 0)

    lgpio.gpio_write(h, IN2, 1)

    lgpio.gpio_write(h, IN3, 1)

    lgpio.gpio_write(h, IN4, 0)

 

def motor_right(): #moves right

    lgpio.gpio_write(h, IN1, 1)

    lgpio.gpio_write(h, IN2, 0)

    lgpio.gpio_write(h, IN3, 0)

    lgpio.gpio_write(h, IN4, 1)

 

def motor_reverse(): #moves backward

    lgpio.gpio_write(h, IN1, 0)

    lgpio.gpio_write(h, IN2, 1)

    lgpio.gpio_write(h, IN3, 0)

    lgpio.gpio_write(h, IN4, 1)

 

def motor_stop(): #stops moving

    for pin in motor_pins:

        lgpio.gpio_write(h, pin, 0)

 

# Brush VEX motor control setup

vex_IN3 = 19

vex_IN4 = 26

ENB = 18 #This pin is for power control of the motor

 

# Configure Brush VEX motor pins

lgpio.gpio_claim_output(h, vex_IN3)

lgpio.gpio_claim_output(h, vex_IN4)

lgpio.gpio_claim_output(h, ENB)

 

# Function to move brush VEX motor forward at given speed (0–100%)

def vex_motor_forward(speed_percent):

    lgpio.gpio_write(h, vex_IN3, 1)

    lgpio.gpio_write(h, vex_IN4, 0)

    lgpio.tx_pwm(h, ENB, 10000, speed_percent)

 

# Stop brush VEX motor

def vex_motor_stop():

    print("Stopping motor")

    lgpio.gpio_write(h, vex_IN3, 0)

    lgpio.gpio_write(h, vex_IN4, 0)

 

# Read distance from ultrasonic sensor

def get_distance():

    lgpio.gpio_write(h, TRIG, 1)

    sleep(0.00001)

    lgpio.gpio_write(h, TRIG, 0)

    pulse_start = time()

    while lgpio.gpio_read(h, ECHO) == 0:

        pulse_start = time()

    pulse_end = time()

    while lgpio.gpio_read(h, ECHO) == 1:

        pulse_end = time()

    duration = pulse_end - pulse_start

    distance = round(duration * 17150, 2)

    return distance

 

# Core behavior loop: zigzag, track ball, avoid obstacles

def move_zigzag():

    zigzag_state = 0  # Keeps track of zigzag direction

    while True:

        distance = get_distance()

        print(f"Distance: {distance} cm")

        # Read camera frame

        ret, frame = cap.read()

        if not ret:

            continue

        # Apply HSV mask for orange ball detection

        hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

        mask = cv2.inRange(hsv_frame, lower_orange, upper_orange)

        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        # Check if ball is detected

        ball_detected = False

        ball_x = None

        for cnt in contours:

            area = cv2.contourArea(cnt)

            if area > 300:

                ball_detected = True

                x, y, w, h = cv2.boundingRect(cnt)

                ball_x = x + w // 2

                break

        if ball_detected: #when ball is detected…

            print("Ball Detected - Tracking Faster!")

            frame_center = frame.shape[1] // 2

            reaction_time = 0.2

            # Turn left toward ball

            if ball_x < frame_center - 60:

                print("Adjusting Left for .2 s")

                motor_left()

                start_time = time()

                while time() - start_time < reaction_time: #this loop makes camera constantly update

 

                    ret, frame = cap.read()

                    if ret:

                        hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

                        mask = cv2.inRange(hsv_frame, lower_orange, upper_orange)

                        cv2.imshow("Camera Feed", frame)

                        cv2.imshow("Orange Mask", mask)

                        if cv2.waitKey(1) & 0xFF == ord('q'):

                            return

 

            # Turn right toward ball

            elif ball_x > frame_center + 60:

                print("Adjusting Right for .2 s")

                motor_right()

                start_time = time()

                while time() - start_time < reaction_time: #this loop makes camera constantly update

 

                    ret, frame = cap.read()

                    if ret:

                        hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

                        mask = cv2.inRange(hsv_frame, lower_orange, upper_orange)

                        cv2.imshow("Camera Feed", frame)

                        cv2.imshow("Orange Mask", mask)

                        if cv2.waitKey(1) & 0xFF == ord('q'):

                            return

 

            # Move forward toward ball

            else:

                print("Moving Forward Toward Ball for .2 s")

                motor_forward()

                start_time = time()

                while time() - start_time < 0.7: #this loop makes the camera constantly update

                    ret, frame = cap.read()

                    if ret:

                        hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

                        mask = cv2.inRange(hsv_frame, lower_orange, upper_orange)

                        cv2.imshow("Camera Feed", frame)

                        cv2.imshow("Orange Mask", mask)

                        if cv2.waitKey(1) & 0xFF == ord('q'):

                            return

                vex_motor_forward(70)

                sleep(1.25)

                vex_motor_forward(98)

                sleep(3)

 

            # Stop after interaction

            vex_motor_stop()

            motor_stop()

            start_time = time()

            while time() - start_time < 1.5:

                ret, frame = cap.read()

                if ret:

                    hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

                    mask = cv2.inRange(hsv_frame, lower_orange, upper_orange)

                    cv2.imshow("Camera Feed", frame)

                    cv2.imshow("Orange Mask", mask)

                    if cv2.waitKey(1) & 0xFF == ord('q'):

                        return

 

        # Obstacle avoidance

        elif distance < 75:

            print("Wall detected! Avoiding")

            motor_reverse()

            sleep(0.8)

            motor_stop()

            motor_right()

            sleep(0.6)

            motor_stop()

            start_time = time()

            while time() - start_time < 1.5: #this loop makes the camera constantly update

 

                distance = get_distance()

                ret, frame = cap.read()

                if ret:

                    cv2.imshow("Camera Feed", frame)

                    cv2.imshow("Orange Mask", mask)

                    if cv2.waitKey(1) & 0xFF == ord('q'):

                        return

 

        # Default zigzag movement

        else:

            print("Zigzagging...")

            if zigzag_state == 0:

                motor_left()

                start_time = time()

                while time() - start_time < 0.17: #this loop makes the camera constantly update

                    distance = get_distance()

                    ret, frame = cap.read()

                    if ret:

                        cv2.imshow("Camera Feed", frame)

                        cv2.imshow("Orange Mask", mask)

                        if cv2.waitKey(1) & 0xFF == ord('q'):

                            return

                motor_forward()

                zigzag_state = 1

            else:

                motor_right()

                start_time = time()

                while time() - start_time < 0.17:

                    distance = get_distance()

                    ret, frame = cap.read()

                    if ret:

                        cv2.imshow("Camera Feed", frame)

                        cv2.imshow("Orange Mask", mask)

                        if cv2.waitKey(1) & 0xFF == ord('q'):

                            return

                motor_forward()

                zigzag_state = 0

 

            start_time = time()

            while time() - start_time < 0.3: #this loop makes the camera constantly update

                distance = get_distance()

                ret, frame = cap.read()

                if ret:

                    cv2.imshow("Camera Feed", frame)

                    cv2.imshow("Orange Mask", mask)

                    if cv2.waitKey(1) & 0xFF == ord('q'):

                        return

 

            motor_stop()

            start_time = time()

            while time() - start_time < 1.5:

                distance = get_distance()

                ret, frame = cap.read()

                if ret:

                    cv2.imshow("Camera Feed", frame)

                    cv2.imshow("Orange Mask", mask)

                    if cv2.waitKey(1) & 0xFF == ord('q'):

                        return

 

# Start behavior loop

try:

    move_zigzag()

except KeyboardInterrupt: #makes the typical error exception 

    print("Stopping")

    motor_stop()

    lgpio.gpiochip_close(h)

    cap.release()

    cv2.destroyAllWindows()