-
Notifications
You must be signed in to change notification settings - Fork 0
/
longitudinal_control.py
103 lines (83 loc) · 2.9 KB
/
longitudinal_control.py
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
100
101
102
103
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.0, KI=0.0, KD=0.098):
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
# --> e(t) = target speed - speed
error = target_speed - speed
# derive PID elements
# --> control = KP*e(t) + KD*(e(t) - e(t - 1)) + KI(sum of all e(t))
control = (self.KP * error) + (self.KD * ((error - self.last_error))) + (self.KI * self.sum_error)
self.last_control = control
self.sum_error += error
self.sum_error = np.clip(self.sum_error, -(1/2), 1/2)
self.last_error = error
# CMNT: Tune parameters
# KP = 0.01, KI = 0, KD = 0
# vmax = 60, vmin = 30, KV = 4.5
# -- note : modify a single term at a time
return control
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
# CMNT : u(t) = control
# gas = u(t) when u(t) >= 0
# brake = -u(t) when u(t) < 0
if control >= 0:
gas = np.clip(control, 0, 0.8)
else: #CMNT: control < 0
brake = np.clip(-1*control, 0, 0.8)
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()