car.py

import math
from scipy.integrate import solve_ivp


class Car:

    def __init__(self,
                 length=2.5,
                 velocity=5,
                 x_pos=0,
                 y_pos=0,
                 pose=0):
        self.__length = length
        self.__velocity = velocity
        self.__x_pos = x_pos
        self.__y_pos = y_pos
        self.__pose = pose
        self.__trajectory = [[x_pos, y_pos, pose]]

    def y_pos(self):
        # We need to define this method so as to export `y_pos`
        # (which is hidden) - it's a "private" attribute
        return self.__y_pos

    def trajectory(self):
        # Again, we implement this method to expose
        # the hidden attribute `__trajectory`
        return self.__trajectory

    def move(self, steering_angle, t):
        def dynamics(_t, z):
            # z = x, y, pose
            pose = z[2]
            return [self.__velocity * math.cos(pose),
                    self.__velocity * math.sin(pose),
                    self.__velocity * math.tan(steering_angle) / self.__length]

        t_final = t
        sol = solve_ivp(dynamics,
                        [0, t_final],
                        [self.__x_pos, self.__y_pos, self.__pose],
                        t_eval=[t_final])

        z_next = sol.y
        self.__x_pos = z_next[0][0]
        self.__y_pos = z_next[1][0]
        self.__pose = z_next[2][0]

        # Black box!
        self.__trajectory += [[self.__x_pos, self.__y_pos, self.__pose]]

main.py

from lanekeeping import PidController
from lanekeeping import Car
import matplotlib.pyplot as plt
import math

ts = 0.01  # sampling time
kp = 0.3  # proportional gain
kd = 0.1
ki = 0.05

# Construct an instance of PidController (called pid)
pid = PidController(kp=kp, kd=kd, ki=ki, ts=ts)

# Construct a car with given initial position and pose
car = Car(x_pos=0, y_pos=0.5, pose=0)

w = math.degrees(1)
num_sim_points = 3000
for k in range(num_sim_points):
    # We compute the control action u (the steering angle)
    # based on the y-position of the vehicle
    u = pid.control(car.y_pos(), set_point=0)
    # We will apply that action to the car
    car.move(u + w, ts)

trajectory = car.trajectory()
y_trajectory = [trajectory[k][1] for k in range(num_sim_points)]
time_span = [k*ts for k in range(num_sim_points)]

# trajectory = [[x0, **y0**, p0], [x1,**y1**,z1], [z2]]
plt.plot(time_span, y_trajectory)
plt.xlabel('Time (s)')
plt.ylabel('y-coordinate')
plt.title('Closed-loop simulation with PID controller')
plt.grid()
plt.show()

pid_controller.py

class PidController:

    def __init__(self, kp, kd=0, ki=0, ts=0.1):
        self.__kp = kp
        self.__ki = kd
        self.__kd = ki
        self.__ts = ts
        self.__err_previous = None
        self.__err_sum = 0
        self.__ki_discrete = ki * ts
        self.__kd_discrete = kd / ts

    def control(self, y, set_point=0):
        error = set_point - y

        if self.__err_previous is not None:
            delta_error = error - self.__err_previous
        else:
            delta_error = 0

        self.__err_sum += error

        u = self.__kp * error \
            + self.__kd_discrete * delta_error \
            + self.__ki_discrete * self.__err_sum

        self.__err_previous = error

        return u