Beagle Board - beagleboard.org
Ye ZhouKostis AlexopoulosJason Zhang
Published

Autonomous RC Car

Inspired by the Fast & Furious we built an autonomous RC car capable of staying within lane and stopping at traffic stop signs.

AdvancedFull instructions provided13
Autonomous RC Car

Things used in this project

Hardware components

BeagleBone AI-64
BeagleBoard.org BeagleBone AI-64
×1
USB WiFi Adapter
×1
Webcam
×1
Portable charger
×1
Webcam, Logitech® HD Pro
Webcam, Logitech® HD Pro
×1
4 to 1 USB Hub
×1

Software apps and online services

OpenCV
OpenCV

Story

Read more

Code

main.py

Python
# Team: Paul Walker
# Authors: Haochen(Jason) Zhang, Ye Zhou, Konstantinos Alexopoulos
# Class: COMP/ELEC 424/553
# Final Project: Autonomous RC Car 
# Fall 2022

# Code drawn from:
# https://www.hackster.io/really-bad-idea/autonomous-path-following-car-6c4992

# Tiny Traffic Stop Sign Detection model used from
# https://github.com/Alzaib/Traffic-Signs-Detection-Tensorflow-YOLOv3-YOLOv4


import cv2
import numpy as np
import matplotlib.pyplot as plt
import math
import sys
import time
import os
import time
from tensorflow.keras.models import load_model

# Manage car behaviour
speed_encode = False
stop_at_light = False




# Encoder
encoder_path = "/sys/module/gpiod_driver/parameters/encoder_val"
encoder_target_rotation = 4046040
encoder_rotation_variance = 1500000 

encoder_max_rotation = encoder_target_rotation + encoder_rotation_variance

# Camera
frame_width = 160
frame_height = 120
cam_idx = 2

sightDebug = False

# PD variables
kp = 0.07
kd = kp * 0.2

# Throttle
zero_speed = 1550000
base_speed = 1634000
speed_variance = 100

zero_turn = 1500000

go_forward = 8.18
go_faster_addition = 0.015
dont_move = 7.5

# Max number of loops
max_ticks = 240

# ML confidence
confidence_threshold = 0.3 

class NotSudo(Exception):
    pass

def check_stop(video_capture, net, output_layers, ):
    '''Test: WEBCAM '''
    _, img = video_capture.read()
    '''Test: SCREEN '''

    height, width, channels = img.shape
    window_width = width

    # Detecting objects (YOLO)
    blob = cv2.dnn.blobFromImage(img, 0.00392, (416, 416), (0, 0, 0), True, crop=False)
    net.setInput(blob)
    outs = net.forward(output_layers)

    # Showing informations on the screen (YOLO)
    class_ids = []
    confidences = []
    boxes = []
    
    for out in outs:
        for detection in out:
            scores = detection[5:]
            class_id = np.argmax(scores)
            confidence = scores[class_id]
            if confidence > confidence_threshold:
                # Object detected
                center_x = int(detection[0] * width)
                center_y = int(detection[1] * height)
                w = int(detection[2] * width)
                h = int(detection[3] * height)
                # Rectangle coordinates
                x = int(center_x - w / 2)
                y = int(center_y - h / 2)
                boxes.append([x, y, w, h])
                confidences.append(float(confidence))
                class_ids.append(class_id)
 
    return 3 in class_ids
        
def set_speed(pwm):
    # ensure is string
    if not isinstance(pwm, str):
        pwm = str(pwm)
    # write 
    with open('/dev/bone/pwm/1/a/duty_cycle', 'w') as filetowrite:
        filetowrite.write(pwm)

def set_turn(pwm):
    # ensure is string
    if not isinstance(pwm, str):
        pwm = str(pwm)
    # write 
    with open('/dev/bone/pwm/1/b/duty_cycle', 'w') as filetowrite:
        filetowrite.write(pwm)
    
def reset_car():
    set_speed(zero_speed)
    set_turn(zero_turn)
    print("Car: stopped & straightened")    

def start_car():
    print("Car ready (input somethingto start)")
    input()
    set_turn(zero_turn)
    set_speed(base_speed)

def manage_speed():
    # read the file
    f = open(encoder_path, "r")
    enc = int(f.readline())
    f.close()

    # If within bounds (wait for car to start / stop)
    ret = base_speed
    if enc <= encoder_max_rotation:
    
        # speed encode based on percentage
        enc_pct_rotation = (enc - (encoder_target_rotation - encoder_rotation_variance)) / (2 * encoder_rotation_variance)
        ret = int(((speed_variance * 2) * abs(enc_pct_rotation - 1)) + (base_speed - speed_variance)) 

    return ret
    
def turn_car(turn_amt):
   set_turn(int(turn_amt * 200000))

    
def detect_edges(frame):
    # filter for blue lane lines
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    lower_blue = np.array([90, 120, 0], dtype="uint8")
    upper_blue = np.array([150, 255, 255], dtype="uint8")
    mask = cv2.inRange(hsv, lower_blue, upper_blue)

    edges = cv2.Canny(mask, 50, 100)

    return edges

def region_of_interest(edges):
    height, width = edges.shape
    mask = np.zeros_like(edges)

    # only focus lower half of the screen
    polygon = np.array([[
        (0, height),
        (0, height / 2),
        (width, height / 2),
        (width, height),
    ]], np.int32)

    cv2.fillPoly(mask, polygon, 255)

    cropped_edges = cv2.bitwise_and(edges, mask)

    return cropped_edges

def detect_line_segments(cropped_edges):
    rho = 1
    theta = np.pi / 180
    min_threshold = 10

    line_segments = cv2.HoughLinesP(cropped_edges, rho, theta, min_threshold,
                                    np.array([]), minLineLength=5, maxLineGap=150)

    return line_segments

def average_slope_intercept(frame, line_segments):
    lane_lines = []

    if line_segments is None:
        return lane_lines

    height, width, _ = frame.shape
    left_fit = []
    right_fit = []

    boundary = 1 / 3
    left_region_boundary = width * (1 - boundary)
    right_region_boundary = width * boundary

    for line_segment in line_segments:
        for x1, y1, x2, y2 in line_segment:
            if x1 == x2:
                continue

            fit = np.polyfit((x1, x2), (y1, y2), 1)
            slope = (y2 - y1) / (x2 - x1)
            intercept = y1 - (slope * x1)

            if slope < 0:
                if x1 < left_region_boundary and x2 < left_region_boundary:
                    left_fit.append((slope, intercept))
            else:
                if x1 > right_region_boundary and x2 > right_region_boundary:
                    right_fit.append((slope, intercept))

    left_fit_average = np.average(left_fit, axis=0)
    if len(left_fit) > 0:
        lane_lines.append(make_points(frame, left_fit_average))

    right_fit_average = np.average(right_fit, axis=0)
    if len(right_fit) > 0:
        lane_lines.append(make_points(frame, right_fit_average))

    return lane_lines

def make_points(frame, line):
    height, width, _ = frame.shape

    slope, intercept = line

    y1 = height  # bottom of the frame
    y2 = int(y1 / 2)  # make points from middle of the frame down

    if slope == 0:
        slope = 0.1

    x1 = int((y1 - intercept) / slope)
    x2 = int((y2 - intercept) / slope)

    return [[x1, y1, x2, y2]]

def display_lines(frame, lines, line_color=(0, 255, 0), line_width=6):
    line_image = np.zeros_like(frame)

    if lines is not None:
        for line in lines:
            for x1, y1, x2, y2 in line:
                cv2.line(line_image, (x1, y1), (x2, y2), line_color, line_width)

    line_image = cv2.addWeighted(frame, 0.8, line_image, 1, 1)

    return line_image

def display_heading_line(frame, steering_angle, line_color=(0, 0, 255), line_width=5):
    heading_image = np.zeros_like(frame)
    height, width, _ = frame.shape

    steering_angle_radian = steering_angle / 180.0 * math.pi

    x1 = int(width / 2)
    y1 = height
    x2 = int(x1 - height / 2 / math.tan(steering_angle_radian))
    y2 = int(height / 2)

    cv2.line(heading_image, (x1, y1), (x2, y2), line_color, line_width)
    heading_image = cv2.addWeighted(frame, 0.8, heading_image, 1, 1)

    return heading_image

def get_steering_angle(frame, lane_lines):
    height, width, _ = frame.shape

    if len(lane_lines) == 2:
        _, _, left_x2, _ = lane_lines[0][0]
        _, _, right_x2, _ = lane_lines[1][0]
        mid = int(width / 2)
        x_offset = (left_x2 + right_x2) / 2 - mid
        y_offset = int(height / 2)

    elif len(lane_lines) == 1:
        x1, _, x2, _ = lane_lines[0][0]
        x_offset = x2 - x1
        y_offset = int(height / 2)

    elif len(lane_lines) == 0:
        x_offset = 0
        y_offset = int(height / 2)

    angle_to_mid_radian = math.atan(x_offset / y_offset)
    angle_to_mid_deg = int(angle_to_mid_radian * 180.0 / math.pi)
    steering_angle = angle_to_mid_deg + 90

    return steering_angle

def plot_pd(p_vals, d_vals, error, show_img=False):
    fig, ax1 = plt.subplots()
    t_ax = np.arange(len(p_vals))
    ax1.plot(t_ax, p_vals, '-', label="P values")
    ax1.plot(t_ax, d_vals, '-', label="D values")
    ax2 = ax1.twinx()
    ax2.plot(t_ax, error, '--r', label="Error")

    ax1.set_xlabel("Frames")
    ax1.set_ylabel("PD Value")
    ax2.set_ylim(-90, 90)
    ax2.set_ylabel("Error Value")

    plt.title("PD Values over time")
    fig.legend()
    fig.tight_layout()
    plt.savefig("pd_plot.png")

    if show_img:
        plt.show()
    plt.clf()

def plot_pwm(speed_pwms, turn_pwms, error, show_img=False):
    fig, ax1 = plt.subplots()
    t_ax = np.arange(len(speed_pwms))
    ax1.plot(t_ax, speed_pwms, '-', label="Speed PWM")
    ax1.plot(t_ax, turn_pwms, '-', label="Steering PWM")
    ax2 = ax1.twinx()
    ax2.plot(t_ax, error, '--r', label="Error")

    ax1.set_xlabel("Frames")
    ax1.set_ylabel("PWM Values")
    ax2.set_ylabel("Error Value")

    plt.title("PWM Values over time")
    fig.legend()
    plt.savefig("pwm_plot.png")

    if show_img:
        plt.show()
    plt.clf()

def main():
    seen_stop = False

    # set up video
    video = cv2.VideoCapture(cam_idx)
    video.set(cv2.CAP_PROP_FRAME_WIDTH, frame_width)
    video.set(cv2.CAP_PROP_FRAME_HEIGHT, frame_height)
    
    reset_car()  
    
    # arrays for making the final graphs
    p_vals = []
    d_vals = []
    err_vals = []
    speed_pwm = []
    steer_pwm = []
    current_speed = go_forward
    
    # other loop-related variables
    stopSignCheck = 1
    isStopSignBool = False
    counter = 0
    lastTime = 0
    lastError = 0
    
    # ---------- YOLOv4-tiny init -----------
    HEIGHT = 32
    WIDTH = 32
    '''Load YOLO (YOLOv3 or YOLOv4-Tiny)'''
    net = cv2.dnn.readNet("yolov4-tiny_training_last.weights", "yolov4-tiny_training.cfg")

    classes = []
    with open("signs.names.txt", "r") as f:
        classes = [line.strip() for line in f.readlines()]

    output_layers = net.getUnconnectedOutLayersNames()
    colors = np.random.uniform(0, 255, size=(len(classes), 3))
    check_time = True
    confidence_threshold = 0.3
    font = cv2.FONT_HERSHEY_SIMPLEX
    start_time = time.time()
    frame_count = 0

    detection_confidence = 0.3
    '''Load Classification Model'''
    classification_model = load_model('traffic.h5') #load mask detection model
    classes_classification = []
    with open("signs_classes.txt", "r") as f:
        classes_classification = [line.strip() for line in f.readlines()]
    
    '''To test the AI on the webcam'''
    video_capture_logi = cv2.VideoCapture(4)
    video_capture_logi.set(cv2.CAP_PROP_FRAME_WIDTH, 160)
    video_capture_logi.set(cv2.CAP_PROP_FRAME_HEIGHT, 120)
    
    # start car
    start_car()
    curr_speed = base_speed
    
    while counter < max_ticks:
    
        # manage video
        ret, original_frame = video.read()
        frame = cv2.resize(original_frame, (160, 120))
        
        # process the frame to determine the desired steering angle
        edges = detect_edges(frame)
        roi = region_of_interest(edges)
        line_segments = detect_line_segments(roi)
        lane_lines = average_slope_intercept(frame, line_segments)
        lane_lines_image = display_lines(frame, lane_lines)
        steering_angle = get_steering_angle(frame, lane_lines)
        heading_image = display_heading_line(lane_lines_image,steering_angle)
        
        if sightDebug:
            cv2.imshow("heading line",heading_image)

        # calculate changes for PD
        now = time.time()
        dt = now - lastTime
        deviation = steering_angle - 90

        # PD Code
        error = -deviation
        base_turn = 7.5
        proportional = kp * error
        derivative = kd * (error - lastError) / dt

        # take values for graphs
        p_vals.append(proportional)
        d_vals.append(derivative)
        err_vals.append(error)

        # determine actual turn to do
        turn_amt = base_turn + proportional + derivative
        turn_amt = max(turn_amt, 6)
        turn_amt = min(turn_amt, 9)
        
        turn_car(turn_amt) 
        
        # speed encoding
        if speed_encode:
            if counter % 3 == 0:
                temp_speed = manage_speed()
                if temp_speed != curr_speed:
                    set_speed(temp_speed)
                    curr_speed = temp_speed
        
        # detect traffic light
        if stop_at_light:
            if not seen_stop and (counter % 3 == 0) :
                if check_stop(video_capture_logi, net, output_layers):
                    print("SEEN STOP")
                    seen_stop = True
                    set_speed(0)
                    time.sleep(2)
                    set_speed(base_speed)

            
        # take values for graphs
        steer_pwm.append(turn_amt)
        speed_pwm.append(current_speed)
        
        # update PD values for next loop
        lastError = error
        lastTime = time.time()

        key = cv2.waitKey(1)
        if key == 27:
            break

        counter += 1
    
        
    # clean up resources
    reset_car()
    video.release()
    video_capture_logi.release()
    cv2.destroyAllWindows()
    plot_pd(p_vals, d_vals, err_vals, True)
    plot_pwm(speed_pwm, steer_pwm, err_vals, True)
    
if __name__ == "__main__":
    main()

gpiod_driver.c

C/C++
#include <linux/module.h>
#include <linux/of_device.h>
#include <linux/kernel.h>
#include <linux/gpio/consumer.h>
#include <linux/platform_device.h>
#include <linux/interrupt.h>
# include <linux/timekeeping.h>
# include <linux/ktime.h>
# include <linux/moduleparam.h>
/**
* Author: Haochen(Jason) Zhang, Ye Zhou, Konstantinos Alexopoulos
* Class: COMP 424 ELEC 424/553 001
* Final Project: Autonomous Car
* Fall 2022
*
* Interrupt code drawn from:
* https://github.com/Johannes4Linux/Linux_Driver_Tutorial/blob/main/11_gpio_irq/gpio_irq.c
**/



// Init variables
unsigned int irq_number;
static struct gpio_desc *my_enc;
static volatile u64 prev_time;
static volatile u64 curr_time;

static int encoder_val;
module_param(encoder_val, int, 0644);

// Interrupt Handler
static irq_handler_t gpio_irq_handler(unsigned int irq, void *dev_id, struct pt_regs *regs) {

	// Ensure time difference between interrupts is greater than 1 ms 
	int enc_val = gpiod_get_value(my_enc); 
	curr_time = ktime_get_ns();
	unsigned long elapsed_time = curr_time - prev_time;
	if (enc_val == 1 && elapsed_time > 1000000) {
		prev_time = curr_time;
		encoder_val = elapsed_time;
		printk(KERN_INFO "irq_handler - timing detected: %lu", elapsed_time);
	}
	
	return (irq_handler_t) IRQ_HANDLED; 
}


// probe function 
static int enc_probe(struct platform_device *pdev)
{
	//struct gpio_desc *my_btn;
	struct device *dev;
	dev = &pdev->dev;
	
	printk("enc_probe - RUNNING v5\n");

	// Get button
	my_enc = gpiod_get(dev, "userbutton", GPIOD_IN);
	if(IS_ERR(my_enc)) {
		printk("enc_probe - could not gpiod_get_index 0 for btn\n");
		return -1;
	}
	
	// Setup interrupt & debounce
	irq_number = gpiod_to_irq(my_enc);
	gpiod_set_debounce(my_enc, 1000000);
	
	if(request_irq(irq_number, (irq_handler_t) gpio_irq_handler, IRQF_TRIGGER_RISING, "my_gpio_irq", NULL) != 0){
		printk("Error!\nCan not request interrupt nr.: %d\n", irq_number);
		return -1;
	}
	prev_time = ktime_get_ns();
	
	printk("enc_probe - SUCCESS\n");
	return 0;
}

// remove function
static int enc_remove(struct platform_device *pdev)
{
	free_irq(irq_number, NULL);
	printk("enc_remove - Freed interrupt & Removing\n");
	return 0;
}

static struct of_device_id matchy_match[] = {
    { .compatible = "hello", },
	{},
};

// platform driver object
static struct platform_driver adam_driver = {
	.probe	 = enc_probe,
	.remove	 = enc_remove,
	.driver	 = {
	       .name  = "The Rock: this name doesn't even matter",
	       .owner = THIS_MODULE,
	       .of_match_table = matchy_match,
	},
};
module_platform_driver(adam_driver);

MODULE_DESCRIPTION("424\'s finest");
MODULE_AUTHOR("GOAT");
MODULE_LICENSE("GPL v2");
MODULE_ALIAS("platform:adam_driver");

BONE-PWM0.dts.txt

C/C++
// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (C) 2022 BeagleBoard.org - https://beagleboard.org/
 *
 * https://elinux.org/Beagleboard:BeagleBone_cape_interface_spec#PWM
 */

/dts-v1/;
/plugin/;


#include <dt-bindings/gpio/gpio.h>
#include <dt-bindings/board/k3-j721e-bone-pins.h>

/*
 * Helper to show loaded overlays under: /proc/device-tree/chosen/overlays/
 */
&{/chosen} {
	overlays {
		BONE-PWM0.kernel = __TIMESTAMP__;
	};
};

&bone_pwm_0 {
	status = "okay";
};
&{/} {
	gpio-leds {
		compatible = "hello";
		userbutton-gpios = <gpio_P8_03 GPIO_ACTIVE_HIGH>;
	};
};

Credits

Ye Zhou

Ye Zhou

1 project • 0 followers
Kostis Alexopoulos

Kostis Alexopoulos

1 project • 0 followers
Jason Zhang

Jason Zhang

1 project • 0 followers

Comments

Add projectSign up / Login