-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathroot_locus_and_bode_for_PID.m
133 lines (111 loc) · 3.34 KB
/
root_locus_and_bode_for_PID.m
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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
%--------------------------------------------------------------------------
%% CONSTANTS AND PARAMS
%--------------------------------------------------------------------------
% Load Niki params
setup_niki;
% Iterable params
Ux_ = 5:3:29; % [m/s] NOTE: make the number of iterable values have
% a round square root! (e.g 4, 9, 16)
lenUx = length(Ux_);
%% GENERATE CONTROLLER TF
s = tf('s');
a = 0.1;
b = 1;
K_s = (a*s^2 + s + b) / s;
K_p = 8;
%% PLOT ROOT LOCI
figure;
plot_dim = ceil(sqrt(lenUx));
subplot(plot_dim, plot_dim, 1);
% sgtitle("Plant Dynamics Without Control");
sgtitle("Plant Dynamics With Control, a = " + a + ", b = " + b);
% Run for different Ux values
for i = 1:lenUx
% Select speed
Ux = Ux_(i);
% CALCULATE PLANT DYNAMICS
% A = [aM, bM, cM, dM]
% [eM, fM, gM, hM]
% [iM, jM, kM, lM]
% [mM, nM, oM, pM]
aM = 0;
bM = 1;
cM = 0;
dM = 0;
eM = 0;
fM = -(f_tire.Ca_lin + r_tire.Ca_lin)/(veh.m*Ux);
gM = (f_tire.Ca_lin + r_tire.Ca_lin)/veh.m;
hM = (-veh.a * f_tire.Ca_lin + veh.b*r_tire.Ca_lin)/(veh.m*Ux);
iM = 0;
jM = 0;
kM = 0;
lM = 1;
mM = 0;
nM = (veh.b * r_tire.Ca_lin - veh.a*f_tire.Ca_lin)/(veh.Iz*Ux);
oM = (veh.a * f_tire.Ca_lin - veh.b*r_tire.Ca_lin)/veh.Iz;
pM = -(veh.a^2 * f_tire.Ca_lin + veh.b^2 *r_tire.Ca_lin)/(veh.Iz*Ux);
A = [[aM, bM, cM, dM];
[eM, fM, gM, hM];
[iM, jM, kM, lM];
[mM, nM, oM, pM]];
B = [0; f_tire.Ca_lin / veh.m; 0; veh.a * f_tire.Ca_lin / veh.Iz];
C = [1, 0, 0, 0];
D = [0];
plant_sys = ss(A, B, C, D);
subplot(plot_dim, plot_dim, i);
% rlocus(plant_sys);
rlocus(plant_sys * K_s);
hold on;
xlabel('Real Axis')
ylabel('Imaginary Axis')
title("Ux = " + Ux + " m/s");
end
%% GENERATE BODE PLOTS OF FINAL CONTROL + PLANT
opts = bodeoptions('cstprefs');
opts.Magunits = 'abs';
opts.Magscale = 'log';
figure;
plot_dim = ceil(sqrt(lenUx));
subplot(plot_dim, plot_dim, 1);
% sgtitle("Plant Dynamics Without Control");
sgtitle("Plant Dynamics With Control, a = " + a + ", b = " + b + ", K_p = " + K_p);
% Run for different Ux values
for i = 1:lenUx
% Select speed
Ux = Ux_(i);
% CALCULATE PLANT DYNAMICS
% A = [aM, bM, cM, dM]
% [eM, fM, gM, hM]
% [iM, jM, kM, lM]
% [mM, nM, oM, pM]
aM = 0;
bM = 1;
cM = 0;
dM = 0;
eM = 0;
fM = -(f_tire.Ca_lin + r_tire.Ca_lin)/(veh.m*Ux);
gM = (f_tire.Ca_lin + r_tire.Ca_lin)/veh.m;
hM = (-veh.a * f_tire.Ca_lin + veh.b*r_tire.Ca_lin)/(veh.m*Ux);
iM = 0;
jM = 0;
kM = 0;
lM = 1;
mM = 0;
nM = (veh.b * r_tire.Ca_lin - veh.a*f_tire.Ca_lin)/(veh.Iz*Ux);
oM = (veh.a * f_tire.Ca_lin - veh.b*r_tire.Ca_lin)/veh.Iz;
pM = -(veh.a^2 * f_tire.Ca_lin + veh.b^2 *r_tire.Ca_lin)/(veh.Iz*Ux);
A = [[aM, bM, cM, dM];
[eM, fM, gM, hM];
[iM, jM, kM, lM];
[mM, nM, oM, pM]];
B = [0; f_tire.Ca_lin / veh.m; 0; veh.a * f_tire.Ca_lin / veh.Iz];
C = [1, 0, 0, 0];
D = [0];
plant_sys = ss(A, B, C, D);
subplot(plot_dim, plot_dim, i);
bode(K_p * plant_sys * K_s, opts);
% bode(plant_sys, opts);
hold on;
grid on;
title("Ux = " + Ux + " m/s");
end