Nine Lives

# adapted from https://github.com/phillies5678/ELEC424FinalProjectAccessCode/blob/main/LaneKeepingAlgorithm.py

import cv2
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
import math
import time

import board
import busio
import adafruit_mcp4728

i2c = busio.I2C(board.SCL, board.SDA)
mcp4728 = adafruit_mcp4728.MCP4728(i2c, 0x64)

# 0 = left, 0.5 = straight, 1 = right
def set_angle(angle):
  mcp4728.channel_b.value = int(angle * 65535)

# 30000 = off
# 35000 = fast
def set_power(power):
    global current_speed
    current_speed = power
    mcp4728.channel_a.value = int(power)

def clamp(x, minv, maxv):
    if x < minv: return minv
    if x > maxv: return maxv
    return x

# based on: https://www.instructables.com/Autonomous-Lane-Keeping-Car-Using-Raspberry-Pi-and/
# and https://www.hackster.io/really-bad-idea/autonomous-path-following-car-6c4992

# Throttle
current_speed = 31000

# Lists for graphs
speed_list = []
steering_list = []

#Max speed value
max_speed = 1650000
# Step size of speed increase/decrease
speed_step = 2500
#Delay of going faster
go_faster_tick_delay = 80
 # Do not change this here. Code will set this value after seeing stop sign
go_faster_tick = 0

# Max number of loops
max_ticks = 2000

def isOrangeFloorVisible(frame):
    """
    Detects whether or not the floor is orange
    :param frame: Image
    :return: [(True is the camera sees a orange on the floor, false otherwise), video output]
    """
    return isMostlyColor(frame, (((140, 40, 170), (200, 255, 250)), (10, 100)))

def isMostlyColor(image, boundaries):
    """
    Detects whether or not the majority of a color on the screen is a particular color
    :param image:
    :param boundaries: [[color boundaries], [success boundaries]]
    :return: boolean if image satisfies provided boundaries, and an image used for debugging
    """
    #Convert to HSV color space
    hsv_img = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

    # cv2.imshow('hsv', hsv_img)

    #parse out the color boundaries and the success boundaries
    color_boundaries = boundaries[0]
    percentage = boundaries[1]

    lower = np.array(color_boundaries[0])
    upper = np.array(color_boundaries[1])
    mask = cv2.inRange(hsv_img, lower, upper)

    # cv2.imshow('mask', mask)

    #Calculate what percentage of image falls between color boundaries
    percentage_detected = np.count_nonzero(mask) * 100 / np.size(mask)
    # If the percentage percentage_detected is betweeen the success boundaries, we return true, otherwise false for result
    result = percentage[0] < percentage_detected <= percentage[1]
    print('detected', percentage_detected, 'orange')
    # return False
    return result

def stop():
    """
    Stops the car
    :return: none
    """

    #Write to stop the car
    set_power(30000)


    print("Stopped")

def go():
    """
    Sends the car forward at a default PWM
    :return: none
    """
    set_power(32000)
    # set_power(30000)

def boost():
    set_power(34000)


def detect_edges(frame):
    # filter for blue lane lines
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    #cv2.imshow("HSV",hsv)
    lower_blue = (80, 80, 0)
    upper_blue = (140, 255, 255)
    mask = cv2.inRange(hsv, lower_blue, upper_blue)
    #cv2.imshow("mask",mask)

    # detect edges
    edges = cv2.Canny(mask, 50, 100)
    # cv2.imshow("edges",edges)

    return edges


def region_of_interest(edges):
    # Define our region of interest to be the lower half
    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)
    #cv2.imshow("roi",cropped_edges)

    return cropped_edges


def detect_line_segments(cropped_edges):
    # Attempt to dectect line segements
    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):
    # Find the average slope intercept
    lane_lines = []

    if line_segments is None:
        #print("no line segments detected")
        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:
                print("skipping vertical lines (slope = infinity")
                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):
    # Create points
    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):
    #Display lines
    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):
    # Display the heading line
    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):
    # Get the steering angle
    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):
    # Plot the PD
    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):
    # Plot the PWM
    fig, ax1 = plt.subplots()
    t_ax = np.arange(len(speed_pwms))
    ax1.plot(t_ax, speed_pwms, '-', label="Speed Voltage")
    ax1.plot(t_ax, turn_pwms, '-', label="Steering Voltage")
    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()



# set up the car throttle and steering PWMs
print("initializing car")
print("Car Initialized!")

# set up video
video = cv2.VideoCapture(0)
video.set(cv2.CAP_PROP_FRAME_WIDTH, 160)
video.set(cv2.CAP_PROP_FRAME_HEIGHT, 120)

# wait for video to load
time.sleep(1)

# PD variables
kp = 0.02
kd = kp * 0.05
# kd = kp * 0.1
lastTime = 0
lastError = 0

# counter for number of ticks
counter = 0

# arrays for making the final graphs
p_vals = []
d_vals = []
err_vals = []
speed_pwm = []
steer_pwm = []

# Booleans for handling stop light
passedFirstStopSign = False

secondStopLightTick = 0

#See if we've check the stop sign
stopSignCheck = 1
# Sight debuging
sightDebug = True
# Check if it is a stop sting
isStopSignBool = False
dif = 0
# start the engines
go()
print("go!")

# main while loop
print("main loop")
try:
    while counter < max_ticks:
        # Read in the video feed
        ret, frame = video.read()
        
        # If there is no video
        if frame is None:
            print("No frame")

        # Show the resized frame
        # if sightDebug:
            # cv2.imshow("Resized Frame", frame)
            
        # check for stop sign every couple ticks
        if ((counter + 1) % stopSignCheck) == 0:
            # check for the first stop sign
            if not passedFirstStopSign:
                isStopSignBool = isOrangeFloorVisible(frame)
                # Trigger when first stop signed is encountered
                if isStopSignBool:
                    print("detected first stop sign, stopping")
                    stop()
                    time.sleep(4)
                    passedFirstStopSign = True
                    # this is used to not check for the second stop sign until many frames later
                    secondStopSignTick = counter + 300
                    # now check for stop sign less frequently
                    stopSignCheck = 1
                    # add a delay to calling go faster
                    go_faster_tick = counter + go_faster_tick_delay
                    print("first stop finished!")
                    go()


            # check for the second stop sign
            elif passedFirstStopSign and counter > secondStopSignTick:
                print(secondStopSignTick)
                print (counter)
                isStop2SignBool = isOrangeFloorVisible(frame)
                if isStop2SignBool:
                    # last stop sign detected, exits while loop
                    print("detected second stop sign, stopping")
                    time.sleep(1)
                    stop()
                    break


        # process the frame to determine the desired steering angle
        #cv2.imshow("original",frame)
        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)
        #cv2.imshow("real line",frame)
        # cv2.imshow("heading line",heading_image)

        # calculate changes for PD
        now = time.time()
        dt = now - lastTime
        #if sightDebug:
            #cv2.imshow("Cropped sight", roi)
        deviation = steering_angle - 90

        # PD Code
        error = -deviation
        base_turn = 0.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
        derivative = clamp(derivative, -.1, .1)
        print("turn amnt:", turn_amt, "=", base_turn, "+", proportional, "+", derivative)

        turn_amt = 1 - clamp(turn_amt, 0, 1)

        set_angle(turn_amt)

        # Update speed and turning
        steer_pwm.append(turn_amt * 5)
        speed_pwm.append(current_speed * 5 / 65535)

        # Used for graphs
        lastError = error
        lastTime = time.time()

        # Use speed encoding to give the car a 'boost' if needed
        with open('/sys/module/gpiod_driver/parameters/diff') as f:
            lines = f.readlines()
            if lines:
                if dif == int(lines[0]):
                    boost()
                    print("Starting up boost")
                else:
                    dif = int(lines[0])
                    print("Received difference of", dif, 'ns')
                    if dif > 50*1000000:
                        boost()
                        print("Boosting now")
                    else:
                        go()

        # update PD values for next loop
        #print("power, ", mcp4728.channel_a.value)
        lastTime = time.time()

        key = cv2.waitKey(1)
        if key == 27: # escape pressed
            print("escaped, window broke")
            break

        counter += 1
except KeyboardInterrupt:
    # clean up resources
    stop()
    video.release()
    cv2.destroyAllWindows()

stop()
video.release()
cv2.destroyAllWindows()
# plot values
plot_pd(p_vals, d_vals, err_vals, True)
plot_pwm(speed_pwm, steer_pwm, err_vals, True)