Autonomous-Driving/longitudinal_control.py at main · hdsung98/Autonomous-Driving · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import find_peaks
from scipy.interpolate import splprep, splev
from scipy.optimize import minimize
import time


class LongitudinalController:
    """
    Longitudinal Control using a PID Controller

    functions:
        PID_step()
        control()
    """

    def __init__(self, KP=0.007, KI=0.006, KD=0.015):
        self.last_error = 0
        self.sum_error = 0
        self.last_control = 0
        self.speed_history = []
        self.target_speed_history = []
        self.step_history = []

        # PID parameters
        self.KP = KP
        self.KI = KI
        self.KD = KD

        # def PID_step(self, speed, target_speed):
        """
        ##### TODO ####
        Perform one step of the PID control
        - Implement the descretized control law.
        - Implement a maximum value for the sum of error you are using for the intgral term

        args:
            speed
            target_speed

        output:
            control (u)
        """

        # define error from set point target_speed to speed

        # derive PID elements

    def PID_step(self, current_velocity, desired_velocity):
        deviation = desired_velocity - current_velocity
        proportional = self.KP * deviation

        self.sum_error += deviation
        self.sum_error = np.clip(self.sum_error, -10, 10)
        integral = self.KI * self.sum_error

        derivative = self.KD * (deviation - self.last_error)
        self.last_error = deviation

        control_signal = proportional + integral + derivative
        return control_signal

    def control(self, speed, target_speed):
        """
        Derive action values for gas and brake via the control signal
        using PID controlling

        Args:
            speed (float)
            target_speed (float)

        output:
            gas
            brake
        """

        control = self.PID_step(speed, target_speed)
        brake = 0
        gas = 0

        # translate the signal from the PID controller
        # to the action variables gas and brake
        if control >= 0:
            gas = np.clip(control, 0, 0.8)
        else:
            brake = np.clip(-1.1 * control, 0, 1)

        return gas, brake

    def plot_speed(self, speed, target_speed, step, fig):
        self.speed_history.append(speed)
        self.target_speed_history.append(target_speed)
        self.step_history.append(step)
        plt.gcf().clear()
        plt.plot(self.step_history, self.speed_history, c="green")
        plt.plot(self.step_history, self.target_speed_history)
        fig.canvas.flush_events()