Skip to content

Commit 2201c12

Browse files
JarmillJarmill
committed
disturbance characterization and connection
started to characterize the disturbance, almost acts like an impulse train. Is a time varying system, with the period depending on the pod velocity. Just got the feedback system online, so will find out what happens when I pass a disturbance into the system.
1 parent cd15c03 commit 2201c12

9 files changed

+304
-14
lines changed

‎air_spring/AirSpringPlant1d.m

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,6 @@
77
% x(1) - z position [cm]
88
% x(2) - z speed [cm/s]
99
% x(3) - bag pressure [N/m^2 = Pa]
10-
% x(4) - underneath skate pressure [Pa]
1110
% input:
1211
% u(1) - mass flow into bag
1312

@@ -173,8 +172,8 @@
173172
end
174173

175174
%incoming mass flow rate
176-
%min = u_thresh;
177-
min = obj.u0;
175+
min = u_thresh;
176+
%min = obj.u0;
178177

179178

180179
%darcy flow (from bag to outside
@@ -230,7 +229,7 @@
230229
function [c,V] = hoverLQR(obj, ROA)
231230
x0 = Point(obj.getStateFrame, obj.x0);
232231
u0 = Point(obj.getInputFrame, obj.u0);
233-
Q = diag([1 1 1e-2 1e-2]);
232+
Q = diag([10 1 1e-2]);
234233
R = diag(1)*obj.delta;
235234

236235
if ROA

‎air_spring/AirSpringPlant1d_disturbed.m

Lines changed: 16 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -175,16 +175,21 @@
175175
u_thresh = 0;
176176
end
177177

178+
179+
178180
%incoming mass flow rate
179181

180182
%active control
181183
%response from solenoid
182-
eff_height = x(1) - u(2);
183-
184+
w_disturbance = u(2);
185+
186+
184187
%open loop control
185-
%min = obj.u0(1);
186-
%eff_height = x(1);
188+
%u_thresh = obj.u0(1);
189+
%w_disturbance = 0;
190+
%w_disturbance = -5e-3;
187191

192+
eff_height = x(1) - w_disturbance;
188193
min = x(4);
189194

190195
%darcy flow (from bag to outside
@@ -233,9 +238,14 @@
233238

234239
p_init = obj.pa;
235240

241+
%min_init = obj.x0(4);
242+
min_init = 0;
243+
244+
236245
x_init = [z_init;
237-
0; %initial z speed
238-
p_init]; %initial bottom pressure
246+
0; %initial z speed
247+
p_init %initial bottom pressure
248+
min_init];%nominal mass flow
239249
end
240250

241251
function [c,V] = hoverLQR(obj, ROA)

‎air_spring/air_spring_roa.m

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
1-
p = AirSpringPlant1d;
2-
%p = AirSpringPlant1d_disturbed;
1+
%p = AirSpringPlant1d;
2+
p = AirSpringPlant1d_disturbed;
33
x0 = Point(p.getStateFrame, p.x0);
44
u0 = Point(p.getInputFrame, p.u0);
55
pp = p.taylorApprox(0,x0,u0,3); % make polynomial approximation
@@ -8,7 +8,7 @@
88
%options.method = {'levelset', 'bilinear'};
99
options.method = 'levelset';
1010
%options.method = {'levelset', 'bilinear', 'sampling'}
11-
options.degV = 2;
11+
options.degV = 4;
1212
options.degL1 = 6;
1313
options.degL2 = 6;
1414
V = regionOfAttraction(pp, x0, options);

‎air_spring/disturbance_fft.m

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
%% Vehicle and Track Parameters
2+
panel_length = 12.5; %12.5'
3+
panel_length_gap = 0.25/12; %0.25"
4+
panel_gap_depth = 0.5/12; %0.5"
5+
6+
7+
total_length = panel_length + panel_length_gap;
8+
duty_cycle = 100*panel_length/(total_length);
9+
10+
%vehicle_speed = 60; %60 mph
11+
vehicle_speed = 400; %60 mph
12+
13+
14+
fps_over_mph = 0.6818; % 1ft/s = 0.6818 mph
15+
v = vehicle_speed * fps_over_mph;
16+
17+
%% Sampling
18+
tmax = 1;
19+
N = 100000;
20+
t = linspace(0, tmax, N);
21+
%w = zeros(size(t));
22+
23+
24+
%% Disturbances
25+
x = v.*t;
26+
%x = zeros(size(t));
27+
ws = square(2*pi.*x/total_length, duty_cycle);
28+
29+
w = (ws/2 - 0.5)*panel_gap_depth;
30+
31+
%% Fourier
32+
Fs = N/tmax;
33+
y = fft(x);
34+
y = fftshift(y);
35+
yo = y(N/2:end);
36+
37+
f = Fs*(0:(N/2))/N;
38+
39+
f_ind = 100;
40+
41+
%% Plotting Output
42+
figure(1)
43+
%plot(x, w)
44+
subplot(2, 1, 1)
45+
plot(t, w)
46+
47+
subplot(2, 1, 2)
48+
plot(f(1:f_ind), abs(yo(1:f_ind)))

‎air_spring/disturbance_input.asv

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
classdef disturbance_input < DrakeSystem
2+
3+
%bag is in full contact with the ground
4+
%purely heave motion, no horizontal travel
5+
6+
% state (world coordinates):
7+
% x(1) - z position [cm]
8+
% x(2) - z speed [cm/s]
9+
% x(3) - bag pressure [N/m^2 = Pa]
10+
% x(4) - solenoid response [single pole filter]
11+
% input:
12+
% u(1) - mass flow into bag
13+
% u(2) - DISTURBANCE (change in track height)
14+
15+
properties
16+
u0; %nominal input
17+
end
18+
19+
methods
20+
function obj = disturbance_input(p)
21+
obj = obj@DrakeSystem(0, ... %number of continuous states
22+
0, ... %number of discrete states
23+
4, ... %number of inputs
24+
2, ... %number of output
25+
true, ... %because the output does not depend on u
26+
false); %because the dynamics and output do not depend on t
27+
%obj = obj.setOutputFrame(p.getInputFrame); % allow full-state feedback
28+
obj.u0 = p.u0;
29+
30+
end
31+
32+
function y = output(obj,t,x,u)
33+
% full state feedback is allowed here
34+
%u_base = 0;
35+
%w_base = 0;
36+
%y = [u_base; w_base];
37+
y
38+
end
39+
40+
end
41+
42+
end

‎air_spring/disturbance_input.m

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
classdef disturbance_input < DrakeSystem
2+
3+
%bag is in full contact with the ground
4+
%purely heave motion, no horizontal travel
5+
6+
% state (world coordinates):
7+
% x(1) - z position [cm]
8+
% x(2) - z speed [cm/s]
9+
% x(3) - bag pressure [N/m^2 = Pa]
10+
% x(4) - solenoid response [single pole filter]
11+
% input:
12+
% u(1) - mass flow into bag
13+
% u(2) - DISTURBANCE (change in track height)
14+
15+
properties
16+
u0; %nominal input
17+
end
18+
19+
methods
20+
function obj = disturbance_input(p)
21+
obj = obj@DrakeSystem(0, ... %number of continuous states
22+
0, ... %number of discrete states
23+
4, ... %number of inputs
24+
2, ... %number of output
25+
true, ... %because the output does not depend on u
26+
false); %because the dynamics and output do not depend on t
27+
%obj = obj.setOutputFrame(p.getInputFrame); % allow full-state feedback
28+
obj.u0 = p.u0;
29+
30+
end
31+
32+
function y = output(obj,t,x,u)
33+
% full state feedback is allowed here
34+
u_base = obj.u0(1);
35+
w_base = -5e-3;
36+
y = [u_base; w_base];
37+
%y = obj.u0;
38+
end
39+
40+
end
41+
42+
end

‎air_spring/run_air_spring.m

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,4 +62,4 @@
6262
%ylabel('force (N)')
6363
legend('p')
6464

65-
v.playback(xtraj);
65+
% v.playback(xtraj);

‎air_spring/run_air_spring_disturbed.m

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
mass_flow = 1;
2+
3+
p = AirSpringPlant1d_disturbed;
4+
5+
v = FlowEngineVisualizer(p);
6+
7+
time_span = [0, 1];
8+
9+
%time_span = [0, 5];
10+
11+
n = 1;
12+
13+
%find region of attraction
14+
ROA = 0;
15+
16+
%[c, V] = hoverLQR(p, ROA);
17+
%sys = feedback(p, c);
18+
tic;
19+
%xtraj = simulate(sys, time_span);
20+
xtraj = simulate(p, time_span);
21+
toc;
22+
%output plots
23+
figure(35)
24+
25+
Nt = 201;
26+
27+
t = linspace(time_span(1), time_span(2), Nt);
28+
xtraj_num = xtraj.eval(t);
29+
30+
31+
%subplot(5, 1, 1)
32+
subplot(4, 1, 1)
33+
hold on
34+
plot(t, 100*xtraj_num(1, :))
35+
plot(time_span, [0, 0] + p.x0(1)*100, '--k')
36+
hold off
37+
title('$$z$$', 'interpreter', 'latex')
38+
legend('z')
39+
ylabel('heave (cm)')
40+
41+
%subplot(5, 1, 2)
42+
subplot(4, 1, 2)
43+
hold on
44+
plot(t, 100*xtraj_num(2, :))
45+
plot(time_span, [0, 0], '--k')
46+
hold off
47+
title('$$\dot{z}$$', 'interpreter', 'latex')
48+
legend('dz')
49+
ylabel('vertical speed (cm/s)')
50+
51+
subplot(4, 1, 3)
52+
hold on
53+
plot(t, xtraj_num(3, :)/1000)
54+
plot(time_span, [0, 0] + p.x0(3)/1000, '--k')
55+
hold off
56+
title('$$p$$', 'interpreter', 'latex')
57+
xlabel('time (s)')
58+
ylabel('bagpressure (kPa)')
59+
%ylabel('force (N)')
60+
legend('p')
61+
62+
subplot(4, 1, 4)
63+
hold on
64+
plot(t, xtraj_num(4, :))
65+
plot(time_span, [0, 0] + p.x0(4), '--k')
66+
hold off
67+
title('$$\dot{m}_{in}$$', 'interpreter', 'latex')
68+
xlabel('time (s)')
69+
ylabel('massflowin (kg/s)')
70+
%ylabel('force (N)')
71+
legend('min')
72+
73+
% v.playback(xtraj);

‎air_spring/run_air_spring_disturbed_connected.m

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
mass_flow = 1;
2+
3+
p = AirSpringPlant1d_disturbed;
4+
c = disturbance_input(p);
5+
6+
c = c.setOutputFrame(p.getInputFrame);
7+
p = p.setOutputFrame(c.getInputFrame);
8+
9+
10+
v = FlowEngineVisualizer(p);
11+
12+
time_span = [0, 1];
13+
14+
n = 1;
15+
16+
%find region of attraction
17+
ROA = 0;
18+
19+
%[c, V] = hoverLQR(p, ROA);
20+
sys = feedback(p, c);
21+
tic;
22+
xtraj = simulate(sys, time_span);
23+
%xtraj = simulate(p, time_span);
24+
toc;
25+
%output plots
26+
figure(35)
27+
28+
Nt = 201;
29+
30+
t = linspace(time_span(1), time_span(2), Nt);
31+
xtraj_num = xtraj.eval(t);
32+
33+
34+
%subplot(5, 1, 1)
35+
subplot(4, 1, 1)
36+
hold on
37+
plot(t, 100*xtraj_num(1, :))
38+
plot(time_span, [0, 0] + p.x0(1)*100, '--k')
39+
hold off
40+
title('$$z$$', 'interpreter', 'latex')
41+
legend('z')
42+
ylabel('heave (cm)')
43+
44+
%subplot(5, 1, 2)
45+
subplot(4, 1, 2)
46+
hold on
47+
plot(t, 100*xtraj_num(2, :))
48+
plot(time_span, [0, 0], '--k')
49+
hold off
50+
title('$$\dot{z}$$', 'interpreter', 'latex')
51+
legend('dz')
52+
ylabel('vertical speed (cm/s)')
53+
54+
subplot(4, 1, 3)
55+
hold on
56+
plot(t, xtraj_num(3, :)/1000)
57+
plot(time_span, [0, 0] + p.x0(3)/1000, '--k')
58+
hold off
59+
title('$$p$$', 'interpreter', 'latex')
60+
xlabel('time (s)')
61+
ylabel('bagpressure (kPa)')
62+
%ylabel('force (N)')
63+
legend('p')
64+
65+
subplot(4, 1, 4)
66+
hold on
67+
plot(t, xtraj_num(4, :))
68+
plot(time_span, [0, 0] + p.x0(4), '--k')
69+
hold off
70+
title('$$\dot{m}_{in}$$', 'interpreter', 'latex')
71+
xlabel('time (s)')
72+
ylabel('massflowin (kg/s)')
73+
%ylabel('force (N)')
74+
legend('min')
75+
76+
% v.playback(xtraj);

0 commit comments

Comments
 (0)