% Simulation of the triple inverted pendulum closed-loop system
%
sim_pend
clp_ic = starp(pend_sm,K,6,2);
%
% response to the reference input
timedata = 0;
tf = 20;
ti = 0.04;
stepdata2 = -0.1;
stepdata3 = 0.2;
pert = abv(0,0,0,0,0,0,0,0);
ref1 = 0;
ref2 = step_tr(timedata,stepdata2,ti,tf);
ref3 = step_tr(timedata,stepdata3,ti,tf);
ref = abv(ref1,ref2,ref3);
dist = abv(0,0,0);
noise = abv(0,0,0);
y = trsp(clp_ic,abv(pert,ref,dist,noise),tf,ti);
figure(1)
vplot(sel(y,9,1),'c--',sel(y,10,1),'g-.',sel(y,11,1),'r-')
grid
title('Closed-loop transient response')
xlabel('Time (secs)')
ylabel('y_1, y_2, y_3 (rads)')
%legend('y_1','y_2','y_3')
figure(2)
vplot(sel(y,12,1),'r-',sel(y,13,1),'c--')
grid
title('Closed-loop control')
xlabel('Time (secs)')
ylabel('u_1, u_2 (V)')
%legend('u_1','u_2')
%
% response to the disturbance
timedata = 0;
tf = 20;
ti = 0.04;
stepdata1 = 0.1;
stepdata2 = 0.1;
stepdata3 = 0.1;
pert = abv(0,0,0,0,0,0,0,0);
ref = abv(0,0,0);
dist1 = step_tr(timedata,stepdata1,ti,tf);
dist2 = step_tr(timedata,stepdata2,ti,tf);
dist3 = step_tr(timedata,stepdata3,ti,tf);
dist = abv(dist1,dist2,dist3);
noise = abv(0,0,0);
y = trsp(clp_ic,abv(pert,ref,dist,noise),tf,ti);
figure(3)
vplot(sel(y,9,1),'c--',sel(y,10,1),'g-.',sel(y,11,1),'r-')
grid
title('Closed-loop disturbance response')
xlabel('Time (secs)')
ylabel('y_1, y_2, y_3 (rads)')
%legend('y_1','y_2','y_3')
figure(4)
vplot(sel(y,12,1),'r-',sel(y,13,1),'c--')
grid
title('Closed-loop control')
xlabel('Time (secs)')
ylabel('u_1, u_2 (V)')
%legend('u_1','u_2')
%
% response to the noise
pert = abv(0,0,0,0,0,0,0,0);
ref = abv(0,0,0);
dist = abv(0,0,0);
rand('state',0)
tf = 20;
ti = 0.01;
time = [ti:ti:tf];
noise1 = 0;
noise2 = 0;
noise3 = siggen('1.3*(rand-0.5)',time);
noise = abv(noise1,noise2,noise3);
y = trsp(clp_ic,abv(pert,ref,dist,noise),tf,ti);
figure(5)
vplot(sel(y,11,1),'r-')
grid
title('Closed-loop noise response')
xlabel('Time (secs)')
ylabel('y_3 (rads)')
figure(6)
subplot(2,1,1)
vplot(sel(y,12,1),'r-')
grid
title('Closed-loop control')
xlabel('Time (secs)')
ylabel('u_1 (V)')
subplot(2,1,2)
vplot(sel(y,13,1),'c-')
grid
xlabel('Time (secs)')
ylabel('u_2 (V)')