Line Following Car - Code

Return back to project page: Line Following Car - Jakub Vojtek

Python code for the Line Following Car project:

 

import cv2
import numpy as np
import time
from buildhat import Motor

left_wheel = Motor('A')
right_wheel = Motor('B')
no_line_start_time = None

cam_width = 320
cam_height = 240

# black color range in HSV
lower_black = np.array([75, 30, 0])
upper_black = np.array([180, 255, 255])

camera = cv2.VideoCapture(0)
camera.set(cv2.CAP_PROP_FRAME_WIDTH, cam_width)
camera.set(cv2.CAP_PROP_FRAME_HEIGHT, cam_height)


def forward():
    left_wheel.start(-10)
    right_wheel.start(10)


def draw_arrow(frame, position, direction):
    if direction == 'left':
        cv2.arrowedLine(frame, position, (position[0] - 50, position[1]), (0, 255, 0), 2)
    elif direction == 'right':
        cv2.arrowedLine(frame, position, (position[0] + 50, position[1]), (0, 255, 0), 2)
    elif direction == 'up':
        cv2.arrowedLine(frame, position, (position[0], position[1] - 50), (0, 255, 0), 2)


def follow_line():
    global no_line_start_time
    while True:
        ret, frame = camera.read()
        if not ret:
            break

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

        # Threshold the HSV image to get only black colors
        mask = cv2.inRange(hsv_frame, lower_black, upper_black)

        # Bitwise-AND mask and original image
        res = cv2.bitwise_and(frame, frame, mask=mask)

        # Crop region of interest (ROI)
        roi = res[cam_height//2:cam_height, :]

        # Convert the ROI to grayscale
        gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)

        # Find contours
        contours, _ = cv2.findContours(gray, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        if contours:
            no_line_start_time = None
            largest_contour = max(contours, key=cv2.contourArea)

            # Get the centroid of the largest contour
            moments = cv2.moments(largest_contour)
            if moments["m00"] != 0:
                centroid_x = int(moments["m10"] / moments["m00"])
                centroid_y = int(moments["m01"] / moments["m00"])
                cv2.circle(res, (centroid_x, centroid_y), 5, (0, 255, 0), -1)  # Draw a green circle at the centroid

                if centroid_x < cam_width // 2 - 10:
                    left_wheel.stop()
                    right_wheel.start(15)
                    draw_arrow(res, (centroid_x, centroid_y), 'left')
                elif centroid_x > cam_width // 2 + 10:
                    left_wheel.start(-15)
                    right_wheel.stop()
                    draw_arrow(res, (centroid_x, centroid_y), 'right')
                else:
                    forward()
                    draw_arrow(res, (centroid_x, centroid_y), 'up')
        else:
            if no_line_start_time is not None:
                forward()
                cv2.putText(res, "Waiting for line", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
                current_time = time.time()
                elapsed_time = current_time - no_line_start_time
                if elapsed_time >= 2:
                    left_wheel.stop()
                    right_wheel.stop()
                    print("No line found")
                    break
            else:
                no_line_start_time = time.time()

        cv2.imshow('mask', mask)
        cv2.imshow('res', res)

        # Exit if 'q' is pressed
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break


def main():
    if not camera.isOpened():
        print("Error: Unable to open camera.")
        return
    forward()
    follow_line()
    camera.release()
    cv2.destroyAllWindows()


if __name__ == "__main__":
    main()