Line Following Car - Code

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

Python code for the Line Following Car project:

<syntaxhighlight lang=python>

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

1. 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()