1
2
3
4
5
6
7
8
9
10
|
function [t,y,u,y_c,t_e,y_e,e_e] = ppp_lin_run (Name,Simulate,ControlType,w,x_0,p_c,p_o)
## usage: [t,y,u,t_e,y_e,e_e] = ppp_lin_run (Name,Simulate,ControlType,w,x_0,p_c,p_o)
##
##
## Linear closed-loop PPP of lego system (and simulation)
##
## Name: Name of system (in mtt terms)
## Simulate = 0: real thing
## Simulate = 1: simulate
|
|
|
|
1
2
3
4
5
6
7
8
9
10
|
function [t,y,u,X_est,y_c,t_e,y_e,e_e,p_c,p_o] = ppp_lin_run (Name,Simulate,ControlType,w,x_0,p_c,p_o)
## usage: [t,y,u,y_c,t_e,y_e,e_e,p_c,p_o] = ppp_lin_run (Name,Simulate,ControlType,w,x_0,p_c,p_o)
##
##
## Linear closed-loop PPP of lego system (and simulation)
##
## Name: Name of system (in mtt terms)
## Simulate = 0: real thing
## Simulate = 1: simulate
|
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
|
if nargin<7
p_o.sigma = 1e-1;
endif
## System
sys = mtt2sys(Name); # Create system
[A,B,C_0,D_0] = sys2ss(sys); # SS form
## Extract matrices for controlled and constrained outputs.
if !struct_contains(p_c,"I_0") # Indices for controlled outputs
p_c.I_0 = 1:n_y
endif
if !struct_contains(p_c,"I_1") # Indices for constarined outputs
p_c.I_1 = 1:n_y
endif
C = C_0(p_c.I_0,:)
C_c = C_0(p_c.I_1,:);
D = D_0(p_c.I_0,:);
D_c = D_0(p_c.I_1,:);
|
>
|
|
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
|
if nargin<7
p_o.sigma = 1e-1;
endif
## System
sys = mtt2sys(Name); # Create system
[A,B,C_0,D_0] = sys2ss(sys); # SS form
[n_x, n_u, n_y] = abcddim(A,B,C_0,D_0);
## Extract matrices for controlled and constrained outputs.
if !struct_contains(p_c,"I_0") # Indices for controlled outputs
p_c.I_0 = 1:n_y
endif
if !struct_contains(p_c,"I_1") # Indices for constrained outputs
p_c.I_1 = 1:n_y
endif
C = C_0(p_c.I_0,:)
C_c = C_0(p_c.I_1,:);
D = D_0(p_c.I_0,:);
D_c = D_0(p_c.I_1,:);
|
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
|
overrun = 2;
Ustar = ppp_ustar (p_c.A_u, n_u, [0:dt:overrun*p_c.delta_ol], 0,0);
if p_c.integrate # Integrate Ustar
disp("Integrating Ustar");
Ustar = cumsum(Ustar)*dt;
endif
disp("Writing Ustar.h");
ppp_ustar2h(Ustar);
## Control loop
y = [];
y_c = [];
u = [];
t = [];
y_e = [];
t_e = [];
e_e = [];
tick = time;
i=0;
for j=1:p_c.Iterations
for k=1:I
tim=time; # Timing
i++;
if Simulate # Exact simulation
X = x; # Current state
t_sim = [1:p_c.N]*dt; # Simulation time points
[yi,ui,xsi] = ppp_ystar(A,B,C,D,x,p_c.A_u,U,t_sim); # Simulate
x = xsi(:,p_c.N); # Current state (for next time)
ti = [(i-1)*p_c.N:i*p_c.N-1]*dt;
y_i = yi(1); # Current output
t_i = ti(1);
##X = xsi(:,1); # Wrong!!
else # The real thing
if strcmp(p_o.method, "remote")
[t_i,y_i,X] = ppp_put_get_X(U); # Remote-state interface
u_i = X(3); # Integrated control is third state
else
[t_i,y_i,u_i] = ppp_put_get(U); # Generic interface to real-time
endif
endif
## Observer
if strcmp(p_o.method, "intermittent")
[x_est y_est y_new, e_est] = ppp_int_obs \
(x_est,y_i,U,A,B,C,D,p_c.A_u,p_c.delta_ol,L);
elseif strcmp(p_o.method, "continuous")
Ui = U; # U at sub intervals
for k = 1:p_c.N
[x_est y_est y_new e_est] = ppp_int_obs \
(x_est,yi(:,k),Ui,A,B,C,D,p_c.A_u,dt,L);
Ui = A_ud'*Ui;
y_e = [y_e; y_new'];
e_e = [e_e; e_est'];
endfor
elseif strcmp(p_o.method, "remote")
## predict from remote state (with zero L)
[x_est y_est y_new e_est] = ppp_int_obs \
(X,y_i,U,A,B,C,D,p_c.A_u,p_c.delta_ol,L);
endif
##Control
if ( (p_c.Tau_u==[])&&(p_c.Tau_y==[]) )
U = K_w*w - K_x*x_est;
else
## Input constraints
[Gamma_u, gamma_u] = \
ppp_input_constraints(p_c.A_u,p_c.Tau_u,p_c.Min_u,p_c.Max_u);
## Output constraints
[Gamma_y,gamma_y] = \
ppp_output_constraints(A,B,C_c,D_c,x_est,p_c.A_u,\
p_c.Tau_y,p_c.Min_y,p_c.Max_y);
## Composite constraints - t=0
Gamma = [Gamma_u; Gamma_y];
gamma = [gamma_u; gamma_y];
[u_qp,U] = ppp_qp \
(x_est,w,J_uu,J_ux,J_uw,Us0,Gamma,gamma,1e-6,1);
endif
## Save data
if Simulate
t = [t;ti'];
y = [y;yi'];
y_c = [y_c;(C_c*xsi)'];
u = [u;ui'];
else
t = [t;t_i];
y = [y;y_i'];
u = [u;u_i'];
endif
if strcmp(p_o.method, "intermittent")
y_e = [y_e; y_new'];
e_e = [e_e; e_est'];
t_e = [t_e; t_i];
endif
if !Simulate
delta_comp = time-tim;
usleep(floor(1e6*(p_c.delta_ol-delta_comp-0.01)));
|
|
>
>
|
<
|
|
>
>
>
>
>
|
|
>
>
|
|
>
>
|
|
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
|
overrun = 2;
Ustar = ppp_ustar (p_c.A_u, n_u, [0:dt:overrun*p_c.delta_ol], 0,0);
if p_c.integrate # Integrate Ustar
disp("Integrating Ustar");
Ustar = cumsum(Ustar)*dt;
endif
disp("Writing Ustar.h ...");
ppp_ustar2h(Ustar);
disp("done.");
## Control loop
y = [];
y_c = [];
u = [];
t = [];
y_e = [];
X_est = [];
t_e = [];
e_e = [];
tick = time;
i=0;
for j=1:p_c.Iterations
for k=1:I
tim=time; # Timing
i++;
if Simulate # Exact simulation
X = x; # Current state
t_sim = [1:p_c.N]*dt; # Simulation time points
[yi,ui,xsi] = ppp_ystar(A,B,C,D,x,p_c.A_u,U,t_sim); # Simulate
x = xsi(:,p_c.N); # Current state (for next time)
ti = [(i-1)*p_c.N:i*p_c.N-1]*dt;
y_i = yi(1); # Current output
t_i = ti(1);
##X = xsi(:,1); # Wrong!!
else # The real thing
if strcmp(p_o.method, "remote")
[t_i,y_i,u_i,X] = ppp_put_get_X(U); # Remote-state interface
else
[t_i,y_i,u_i] = ppp_put_get(U); # Generic interface to real-time
endif
endif
## Observer
if strcmp(p_o.method, "intermittent")
[x_est y_est y_new, e_est] = ppp_int_obs \
(x_est,y_i,U,A,B,C,D,p_c.A_u,p_c.delta_ol,L);
elseif strcmp(p_o.method, "continuous")
Ui = U; # U at sub intervals
for k = 1:p_c.N
[x_est y_est y_new e_est] = ppp_int_obs \
(x_est,yi(:,k),Ui,A,B,C,D,p_c.A_u,dt,L);
Ui = A_ud'*Ui;
y_e = [y_e; y_new'];
e_e = [e_e; e_est'];
endfor
elseif strcmp(p_o.method, "remote")
## predict from remote state (with zero L)
if (ControlType==2) # Closed-loop
# [x_est y_est y_new e_est] = ppp_int_obs \
# (X,y_i,U,A,B,C,D,p_c.A_u,p_c.delta_ol,zeros(n_x,1));
x_est = X; y_est=y_i; y_new=y_i; e_est=0;
else # Open-loop
[x_est y_est y_new e_est] = ppp_int_obs \
(x_est,y_i,U,A,B,C,D,p_c.A_u,p_c.delta_ol,zeros(n_x,1));
endif
endif
##Control
if ( length(p_c.Tau_u)==0&&length(p_c.Tau_y)==0 )
U = K_w*w - K_x*x_est;
else
## Input constraints
[Gamma_u, gamma_u] = \
ppp_input_constraints(p_c.A_u,p_c.Tau_u,p_c.Min_u,p_c.Max_u);
## Output constraints
[Gamma_y,gamma_y] = \
ppp_output_constraints(A,B,C_c,D_c,x_est,p_c.A_u,\
p_c.Tau_y,p_c.Min_y,p_c.Max_y);
## Composite constraints - t=0
Gamma = [Gamma_u; Gamma_y];
gamma = [gamma_u; gamma_y];
[u_qp,U,n_active] = ppp_qp \
(x_est,w,J_uu,J_ux,J_uw,Us0,Gamma,gamma,1e-6,1);
endif
## Save data
if Simulate
t = [t;ti'];
y = [y;yi'];
X_est = [X_est;x_est'];
y_c = [y_c;(C_c*xsi)'];
u = [u;ui'];
else
t = [t;t_i];
y = [y;y_i'];
X_est = [X_est;x_est'];
u = [u;u_i'];
endif
if strcmp(p_o.method, "intermittent")||strcmp(p_o.method, "remote")
y_e = [y_e; y_new'];
e_e = [e_e; e_est'];
t_e = [t_e; t_i];
endif
if !Simulate
delta_comp = time-tim;
usleep(floor(1e6*(p_c.delta_ol-delta_comp-0.01)));
|