Team 10's Epic Car

#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: Tony Martinez
* Class: COMP 424 ELEC 424/553 001
* Final Project: Team 10
*
* Interrupt code drawn from:
* https://github.com/Johannes4Linux/Linux_Driver_Tutorial/blob/main/11_gpio_irq/gpio_irq.c
* Code pulled from project:
* https://www.hackster.io/paul-walker/autonomous-rc-car-eebfb4
**/



// 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");

# Final Project - Team 10 - Spring 2026
#  
# Code Citations: This one uses new base of Team Joseph
# Team Joseph's Youngsters, "Joseph's Youngsters". Hackster. URL: https://www.hackster.io/joseph-s-youngsters/joseph-s-youngsters-c70b0b
# User raja_961, Autonomous Lane-Keeping Car Using Raspberry Pi and OpenCV. Instructables. URL: https://www.instructables.com/Autonomous-Lane-Keeping-Car-Using-Raspberry-Pi-and/
# Team NEGLECT, "Neglect", Hackster, URL: https://www.hackster.io/neglect/neglect-eaf7f9

import time
import cv2
import numpy as np
import math
import sys
import os
import RPi.GPIO as GPIO
import matplotlib.pyplot as plt
import pandas as pd


GPIO.setmode(GPIO.BCM)

# PWM throttle constants 
THROTTLE_MIN = 7.5   # full stop duty
THROTTLE_MAX = 8.05   # full forward duty

# PWM steering constants
STEERING_ZERO_TURN = 7.5 # straight steering duty
STEERING_MIN = 5.0     # full left duty
STEERING_MAX = 10.0    # full right duty


# === GPIO SETUP ===
THROTTLE_PIN = 18  # ESC throttle
STEER_PIN = 19  # servo steering

GPIO.setup(THROTTLE_PIN, GPIO.OUT)
GPIO.setup(STEER_PIN,    GPIO.OUT)

pwm_esc = GPIO.PWM(THROTTLE_PIN, 50)
pwm_serv = GPIO.PWM(STEER_PIN, 50)

pwm_esc.start(THROTTLE_MIN)  # neutral throttle
pwm_serv.start(STEERING_ZERO_TURN) # straight steering

# === Encoder & Speed PD Settings ===
mods = [m for m in os.listdir("/sys/module") if m.startswith("encoder")]
ENCODER_PATH = f"/sys/module/{mods[0]}/parameters/encoder_val"
print("Using encoder_path:", ENCODER_PATH)

DESIRED_NS = 9_133_716.0  # target interval between pulses
DESIRED_MS = int(DESIRED_NS / 1e6)  # convert to ms for debugging printouts
KP, KI, KD = 0.1, 0, 0.01
integral_t = 0.0
BASE_SPEED = 7.9   # nominal running duty

STEERING_SCALE = 1.0 # adjust to make steering more or less sensitive

# === Steering PD Settings ===
KP_S, KI_S, KD_S = 0.06, 0.0, 0.008
integral_st = 0.0

# === Camera Settings (resolution size) ===
FRAME_W, FRAME_H = 250, 120
CAM_IDX = 0

# === Laneline EMA ===
EMA_ALPHA = 0.7
prev_left = None
prev_right = None

# === Stopsign for Redfloor ===
RED_THRESHOLD = 20  # pixel threshold for stopping at red light
MAX_FRAMES     = 10000
STOP_TIME = 3
MIN_TIME_BETWEEN_STOPS = 5  # seconds to wait before allowing another stop

# === Variables for plotting ===
START_FRAME = 50
# PWM values
steer_p_vals = []
steer_i_vals = []
steer_d_vals = []

speed_p_vals = []
speed_i_vals = []
speed_d_vals = []
# Error values
speed_err_vals = []
steer_err_vals = []
# Duty Cycles
speed_vals = []
steer_vals = []
# Timestamps of stops
stop_times = []



def read_encoder():
    try:
        with open(ENCODER_PATH, "r") as f:
            val = float(f.read().strip())
            if val < 0 or val > 1e16:  # sanity check for encoder values
                print(f"[read_encoder] WARNING: Unreasonable encoder value {val} ns", file=sys.stderr)
                return None
            return val  # read speed in ns
    except Exception as e:
        print(f"[read_encoder] ERROR: {e}", file=sys.stderr)
        return None

def save_data():
    df = pd.DataFrame({
        'Frame': range(len(speed_err_vals)),
        'Turn_Angle_Error': steer_err_vals,
        'Speed_Error': speed_err_vals,
        'Steer_P_Response': steer_p_vals,
        'Steer_I_Response': steer_i_vals,
        'Steer_D_Response': steer_d_vals,
        'Speed_Duty': speed_vals,
        'Steer_Duty': steer_vals
    })
    df.to_csv('control_data.csv', index=False)
    print("[save_data] Control data saved to control_data.csv")

def pid_control(setpoint: float, measurement: float, last_err: float, last_time: float, kp: float, ki: float, kd: float, integral: float, integral_limit: tuple[float, float] = None):
    now = time.time()
    dt = max(now - last_time, 0.01) if last_time is not None else 0.1

    err = setpoint - measurement
    
    integral += err * dt
    if integral_limit is not None:
        integral = max(min(integral, integral_limit[1]), integral_limit[0])

    p_term = kp * err
    i_term = ki * integral  # Integral term
    d_term = kd * ((err - last_err) / dt) if last_err is not None else 0.0  # Derivative term with dt check
    
    return (p_term, i_term, d_term), err, now, integral

last_thr = BASE_SPEED 
def manage_speed(last_err_sp: float, last_time_sp: float, steering_err: float):
    """
    Adjust throttle duty cycle using PD control of encoder interval.
    """
    global integral_t
    global last_thr
    enc_ns = read_encoder()
    # convert to ms for more intuitive printouts, but keep in ns for control calculations
    enc_ms = enc_ns / 1e6 if enc_ns is not None else None
    if enc_ns is None:
        speed_err_vals.append(last_err_sp)  # keep lengths consistent
        speed_p_vals.append(0.0)
        speed_i_vals.append(0.0)
        speed_d_vals.append(0.0)
        speed_vals.append(last_thr)
        return BASE_SPEED, last_err_sp, last_time_sp  # if encoder read fails, keep previous speed and timing
    
    turn_factor = 1.0 - min(abs(steering_err) / 90.0, 1.0) * 0.3
    (p_term, i_term, d_term), err_sp, now, integral_t = pid_control(enc_ms, DESIRED_MS + turn_factor * 5, last_err_sp, last_time_sp, KP, KI, KD, integral=integral_t, integral_limit=(-0.5, 0.5))
    

    thr = (BASE_SPEED + p_term + i_term + d_term)  # adjust throttle with PD control
    thr = max(THROTTLE_MIN, min(THROTTLE_MAX, thr))
    last_thr = thr  # store last throttle for next iteration

    # append for plotting
    speed_err_vals.append(err_sp)
    speed_p_vals.append(p_term)
    speed_i_vals.append(i_term)
    speed_d_vals.append(d_term)
    speed_vals.append(thr)

    # print for debugging
    print(f"[manage_speed] enc={enc_ms:.1f}ms speed err={err_sp:.1f} p={p_term:.3f} i={i_term:.3f} d={d_term:.3f}  thr={thr:.3f}")
    
    return thr, err_sp, now

def manage_steering(angle: float, last_err_st: float, last_time_st:float):
    global integral_st 
    # Clamp dt to avoid spikes 
    err_angle = (angle - 90)
    (p_term, i_term, d_term), err_st, now, integral_st = pid_control(0, err_angle, last_err_st, last_time_st, KP_S, KI_S, KD_S, integral=integral_st, integral_limit=(-10, 10)) 
    raw = p_term + i_term + d_term

    #raw = max(min(raw, 1.0), -1.0)
    steering_duty = STEERING_ZERO_TURN + raw * STEERING_SCALE # adjust to scale steer sensitivity
    steering_duty = max(STEERING_MIN, min(STEERING_MAX, steering_duty))
    
    # plotting data
    steer_err_vals.append(err_st)  # append for plotting
    steer_p_vals.append(p_term)
    steer_i_vals.append(i_term)
    steer_d_vals.append(d_term)
    steer_vals.append(steering_duty)  # append for plotting
    
    print(f"[manage_steering] angle={angle:.1f} err={err_st:.1f} raw={raw:.3f}  duty={steering_duty:.3f}")

    return steering_duty, err_st, now

def detect_red(frame):
    # low_red = np.array([161, 155, 84])
    # high_red = np.array([179, 255, 255])
    # red_mask = cv2.inRange(frame, low_red, high_red)
    # lower mask (0-10)
    h, w, _ = frame.shape
    y0 = int(h * 0.95)
    roi = frame[y0:, :] # checks lower part of frame
    roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
    lower_red = np.array([0, 50, 50])
    upper_red = np.array([10, 255, 255])
    mask0 = cv2.inRange(roi, lower_red, upper_red)

    # upper mask (170-180)
    lower_red = np.array([170, 50, 50])
    upper_red = np.array([180, 255, 255])
    mask1 = cv2.inRange(roi, lower_red, upper_red)

    # join my masks
    red_mask = cv2.bitwise_or(mask0, mask1)
    # red = cv2.bitwise_and(frame, frame, mask = red_mask)

    countRed = np.count_nonzero(red_mask)
    # if countRed > 0:
    # print("Red value: ",countRed)
    return countRed


def detect_edges(frame):
    # Apply hsv for better color contrast, thus better lane detection
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # blue for tape lanes
    mask = cv2.inRange(hsv, (90,120,0), (120,255,255))
    return cv2.Canny(mask, 50, 100)

def region_of_interest(edges):
    h, w = edges.shape
    mask = np.zeros_like(edges)
    poly = np.array([[(0, int(h*0.1)), (w, int(h*0.1)), (w,h), (0,h)]], np.int32)
    
    # try to get the the lane detection near car (not other regions)
    cv2.fillPoly(mask, poly, 255)
    return edges & mask

def detect_line_segments(cropped):
    return cv2.HoughLinesP( # finds straight lines in cropped frame
        cropped, 1, np.pi/180, 10,
        np.array([]), minLineLength=5, maxLineGap=150
    )

def make_points(frame, line):
    h, _, _ = frame.shape
    m, b = line # uses lane's start and end to map lines
    if m == 0: m = 0.1
    y1, y2  = h, h//2
    x1 = int((y1 - b) / m)
    x2 = int((y2 - b) / m)
    return [[x1, y1, x2, y2]]

def average_slope_intercept(frame, segs):
    global prev_left, prev_right
    
    # if line detection fails (mainly due to camera frame rate)
    if segs is None or len(segs) == 0: # uses previous lines
        lanes = []
        if prev_left is not None:  lanes.append(make_points(frame, prev_left))
        if prev_right is not None: lanes.append(make_points(frame, prev_right))
        return lanes

    h, w, _ = frame.shape
    left_fit = []
    right_fit = []
    boundary = w * (1/3) # divides frame into thirds

    for seg in segs:
        x1, y1, x2, y2 = seg[0]
        if x1 == x2: continue # skip vertical lines
        slope = (y2 - y1) / (x2 - x1)
        intercept = y1 - slope * x1

        if abs(slope) > 0.3 and x1 < boundary and x2 < boundary:
            left_fit.append((slope, intercept))
        elif abs(slope) > 0.3 and x1 > 2*boundary and x2 > 2*boundary:
            right_fit.append((slope, intercept))

    lanes = [] # smooths result using exp moving average
    if left_fit:
        avg_left = np.average(left_fit, axis=0)
        prev_left = avg_left if prev_left is None else EMA_ALPHA*avg_left + (1-EMA_ALPHA)*prev_left
    if right_fit:
        avg_right = np.average(right_fit, axis=0)
        prev_right = avg_right if prev_right is None else EMA_ALPHA*avg_right + (1-EMA_ALPHA)*prev_right
    # averaged slopes and intercepts into line coordinates
    if prev_left is not None:  lanes.append(make_points(frame, prev_left))
    if prev_right is not None: lanes.append(make_points(frame, prev_right))
    return lanes

def display_lines(frame, lanes):
    overlay = np.zeros_like(frame) # draws green line for lanes
    
    for ln in lanes:
        x1, y1, x2, y2 = ln[0]
        cv2.line(overlay, (x1,y1), (x2,y2), (0,255,0), 6)
        
    return cv2.addWeighted(frame, 0.8, overlay, 1, 1)

def get_steering_angle(frame, lanes):
    # calculate angle between center of car and canter of lane
    h, w, _ = frame.shape
    if len(lanes) == 2:
        _,_,lx2,_ = lanes[0][0]
        _,_,rx2,_ = lanes[1][0]
        off = ((lx2 + rx2) / 2) - (w / 2) # midpoint of lane
    elif len(lanes) == 1:
        x1, _, x2, _ = lanes[0][0]
        off = x2 - (w / 2)  # offset of detected lane from frame center
        # off = x2 - x1
    else:
        off = 0 # no lane detected
    return int(math.degrees(math.atan2(off, h/2))) + 90

def display_heading_line(frame, angle):
    # draws red line showing car direction
    overlay = np.zeros_like(frame)
    h, w, _ = frame.shape
    r = math.radians(angle)
    x1, y1 = w//2, h
    x2 = int(x1 - (h/2)/math.tan(r)) # projects angle up
    y2 = h//2
    cv2.line(overlay, (x1,y1), (x2,y2), (0,0,255), 5)
    return cv2.addWeighted(frame, 0.8, overlay, 1, 1)

def main():
    cap = cv2.VideoCapture(CAM_IDX)  # set camera idx and frame
    cap.set(cv2.CAP_PROP_FRAME_WIDTH,  FRAME_W)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_H)

    just_stopped = False
    last_err_st, last_time_st = 0, time.time()
    last_err_sp, last_time_sp = 0, time.time()
    frame_count = 0

    # initialise esc
    pwm_esc.ChangeDutyCycle(THROTTLE_MIN)
    pwm_serv.ChangeDutyCycle(STEERING_ZERO_TURN)
    time.sleep(1) # give ESC time to arm
    
    try:
        while frame_count < MAX_FRAMES:
            ret, frame = cap.read()
            if not ret:
                break # exit if frame not captured
            frame = cv2.flip(frame, 1)
            

            # Expensive stuff gets run only every 12 frames
            if frame_count % 12 == 0:
                red_count = detect_red(frame)
                if red_count > RED_THRESHOLD and not just_stopped:
                    print("[red_floor] Detected red floor - stopping immediately!")
                    pwm_esc.ChangeDutyCycle(THROTTLE_MIN)
                    stop_times.append((frame_count, time.time()))
                    just_stopped = True
                    if len(stop_times) >= 2: # stop program after 2 stops
                        print("[red_floor] Final stop reached :D")
                        break
                    time.sleep(STOP_TIME) 
                elif just_stopped and time.time() - stop_times[-1][1] > MIN_TIME_BETWEEN_STOPS:
                    print("[red_floor] Resuming scanning after stop...")
                    just_stopped = False

            edges = detect_edges(frame) # lane detection
            # Look at the bottom of the frame
            roi = region_of_interest(edges)
            # Detect line segments in the ROI
            segs = detect_line_segments(roi)
            # Combining lanes into single lane lines
            lanes = average_slope_intercept(frame, segs)
            angle = get_steering_angle(frame, lanes) # angle calc
            vis = display_heading_line(display_lines(frame, lanes), angle)
            cv2.imshow("heading", vis)

            # throttle control
            now = time.time()
            thr, last_err_sp, last_time_sp = manage_speed(last_err_sp, last_time_sp, last_err_st)
            pwm_esc.ChangeDutyCycle(thr)
            
            # steering control
            duty, last_err_st, last_time_st = manage_steering(angle, last_err_st, last_time_st)
            pwm_serv.ChangeDutyCycle(duty)

            key = cv2.waitKey(1) & 0xFF
            if key == 27 or key == ord('s'):
                break
            

            frame_count += 1

    except KeyboardInterrupt:
        print("\nprogram stopped...")

    finally:
        # Stopping so we don't slam into a wall
        pwm_esc.ChangeDutyCycle(THROTTLE_MIN)
        pwm_serv.ChangeDutyCycle(STEERING_ZERO_TURN)
        frames = range(START_FRAME, len(steer_err_vals))

        # Save data for later if we really mess up somehow
        save_data()

        # plot 1
        fig, ax1 = plt.subplots()
        ax2 = ax1.twinx()
        ax1.plot(frames, steer_vals[START_FRAME:], label='Steering Duty Cycle', color='orange')
        ax1.plot(frames, speed_vals[START_FRAME:], label='Speed Duty Cycle', color='blue')
        ax2.plot(frames, steer_err_vals[START_FRAME:],   linestyle='--', label='Steering Error', color='gray')
        # Print out all stop times
        for stop_frame, _ in stop_times:
            if stop_frame >= START_FRAME:
                ax1.axvline(x=stop_frame, color='red', linestyle=':', alpha=0.7, label='_nolegend_')

        ax1.set_xlabel('Frame Number')
        ax1.set_ylabel('Duty Cycle')
        ax2.set_ylabel('Steering Error')
        lines1, labels1 = ax1.get_legend_handles_labels()
        lines2, labels2 = ax2.get_legend_handles_labels()
        ax1.legend(lines1 + lines2, labels1 + labels2, loc='upper left')
        plt.title('Duty Cycles & Error vs Frame Number (from frame 10)')
        plt.savefig('plt1.png')
        plt.close()

        # plot 2
        fig, ax1 = plt.subplots()
        ax2 = ax1.twinx()
        ax1.plot(frames, steer_p_vals[START_FRAME:], linestyle='-',  label='Steering Proportional Response', color='blue')
        ax1.plot(frames, steer_i_vals[START_FRAME:], linestyle='-', label='Steering Integral Response', color='green')
        ax1.plot(frames, steer_d_vals[START_FRAME:], linestyle='-.', label='Steering Derivative Response', color='orange')
        ax2.plot(frames, steer_err_vals[START_FRAME:], color='gray', linestyle='--', label='Steering Error')
        for stop_frame, _ in stop_times:
            if stop_frame >= START_FRAME:
                ax1.axvline(x=stop_frame, color='red', linestyle=':', label='_nolegend_', alpha=0.7)

        ax1.set_xlabel('Frame Number')
        ax1.set_ylabel('Steering PID Response')
        ax2.set_ylabel('Steering Error')
        lines1, labels1 = ax1.get_legend_handles_labels()
        lines2, labels2 = ax2.get_legend_handles_labels()
        ax1.legend(lines1 + lines2, labels1 + labels2, loc='upper left')
        plt.title('PID Responses & Error vs Frame Number (from frame 10)')
        plt.savefig('plt2.png')
        plt.close()

        
        # cleanup
        pwm_esc.ChangeDutyCycle(THROTTLE_MIN)
        pwm_serv.ChangeDutyCycle(STEERING_ZERO_TURN)
        cap.release()
        cv2.destroyAllWindows()


if __name__ == "__main__":
    main()