clear
OPENLOOP=0
CLQR=1
SCLQR=1
F_DB=1
Ra=10;
Ki=10;
Kb=0.075;
J=0.006;
tf=20;
A=[-Ki*Kb/(Ra*J) 0;1 0];
B=[Ki/(Ra*J);0];
C=[1 0;0 1];
D=[0;0];
eig(A);
rank(ctrb(A,B));
if OPENLOOP==1,
t=[0:0.01:10];
[tr,tc]=size(t);
u=zeros(tr,tc);
y1=lsim(A,B,C,D,u,t);
y2=lsim(A,B,C,D,u,t,[pi/18 0]);
y3=lsim(A,B,C,D,u,t,[0 pi/18]);
plot(t,y1);
keyboard;
%pause
clg;
subplot(211)
plot(t,y2);
plot(t,y3);
end
if F_DB==1,
R=10;
%w=100;
w=0.025;
STOP=0;
while STOP==0,
R
Q=w*eye(2);
[k,s]=lqr(A,B,Q,R)
Acl=A;
Acl1=B*k;
Q=eye(2);;
P=lyap(Acl+Acl1, Q);
sigma=min(eig(Q))/2*max(abs(eig(P)));
delta=sqrt(max(abs(eig(P)))/min(abs(eig(P))));
tau1=sigma/(delta*norm(Acl1*(Acl+Acl1)))
tau=fix(tau1*1000)/1000
%%keyboard;
pause
STOP=1;
end
end
if CLQR==1,
dt=tau;
%%w=100;
%%R=10;
STOP=0;
while STOP==0,
w
R
dt
t=[0:dt:tf];
[tr,tc]=size(t);
%%Q=w*eye(2);
%%[k,s]=lqr(A,B,Q,R);
[An,Bn,Cn,Dn]=feedback(A,B*k,C,D*k,0,[0 0],[0;0],eye(2));
r=[zeros(1,tc);zeros(1,fix(tc/5)) ones(1,ceil(tc*4/5))*pi/18];
y=lsim(An,Bn,Cn,Dn,r',t);
%%clg
subplot(2,1,1);
plot(t,y(:,1),'.',t,y(:,2),t,r(2,:)');
xlabel('Time(sec)');
ylabel('rad/sec');
title('Loop 1 using a direct control');
%%keyboard;
pause
STOP=1;
end
end
if SCLQR==1,
dt=tau;
%%w=100;
%%R=10;
STOP=0;
while STOP==0,
w
R
dt
%%keyboard;
pause
t=[0:dt:tf];
[tr,tc]=size(t);
%r=[zeros(1,tc);zeros(1,round(tc/10)) ones(1,round(tc*9/10))*pi/18]';
%r=[zeros(1,tc);zeros(1,10) ones(1,tc-10)*pi/18]';
r=[zeros(1,tc);zeros(1,fix(tc/5)) ones(1,ceil(tc*4/5))*pi/18]';
%%Q=w*eye(2);
%%[k,s]=lqr(A,B,Q,R);
n=fix(tf/dt);
x0=[0 0];
x=x0;
for i=1:n,
i;
r0 = r(i:i+1,:);
t0 = [0 dt];
u0 = k*(r0(1,:)-x0)';
u0 = [u0;u0];
%pause
[ysc,x2]=lsim(A,B,C,D,u0,t0,x0);
x=[x;x2(2,:)];
x0 = x2(2,:);
end
%%clg
subplot(2,1,2);
plot(t,x(:,1),'.',t,x(:,2),t,r(:,2));
xlabel('Time(sec)');
ylabel('rad/sec');
title('Loop 1 using a networked control');
gtext('Theta_{ref}');
gtext('Theta');
gtext('Omega');
gtext('Theta_{ref}');
gtext('Theta');
gtext('Omega');
%%keyboard;
STOP=1;
end
end
