237 lines
5.4 KiB
Matlab
237 lines
5.4 KiB
Matlab
function [ssc]=StateSpaceControlDesign(mot,motid)
|
|
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
|
|
%
|
|
% builds a state space controller designed for the plant.
|
|
% shows step answers of open and closed loop, also for the observer controller and the final discrete observer
|
|
%
|
|
% finally it opens a simulink observer file for testing
|
|
|
|
|
|
%References:
|
|
%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=ControlStateSpace
|
|
%space state controller:
|
|
% web(fullfile(docroot, 'simulink/examples.html'))
|
|
% web(fullfile(docroot, 'simulink/examples/inverted-pendulum-with-animation.html'))
|
|
% web(fullfile(docroot, 'simulink/examples/double-spring-mass-system.html'))
|
|
%
|
|
% https://www.youtube.com/watch?v=Lax3etc837U
|
|
|
|
ss_ol=mot.ss;
|
|
ss_ol.Name='open loop';
|
|
%sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only
|
|
|
|
figure();h=bodeplot(ss_ol);
|
|
setoptions(h,'IOGrouping','all')
|
|
A=ss_ol.A;
|
|
B=ss_ol.B;
|
|
C=ss_ol.C;
|
|
D=ss_ol.D;
|
|
|
|
P=ctrb(A,B);
|
|
if rank(A)==rank(P)
|
|
disp('sys controlable')
|
|
else
|
|
disp('sys not controlable')
|
|
end
|
|
|
|
Q=obsv(A,C);
|
|
if rank(A)==rank(Q)
|
|
disp('sys observable')
|
|
else
|
|
disp('sys not observable')
|
|
end
|
|
|
|
|
|
% step answer on open loop:
|
|
t = 0:1E-4:.5;
|
|
u = ones(size(t));
|
|
x0 = zeros(1,length(ss_ol.A));
|
|
|
|
[y,t,x] = lsim(ss_ol,u,t,x0);
|
|
figure();plot(t,y);title('step on open loop');
|
|
|
|
poles = eig(A);
|
|
w0=abs(poles);
|
|
ang=angle(-poles);
|
|
%-------------------
|
|
%p=w0.*exp(j.*ang)
|
|
|
|
% *** space state controller ***
|
|
%
|
|
%place poles for the controller feedback
|
|
if motid==1
|
|
%2500rad/s = 397Hz -> locate poles here
|
|
p1=-3300+2800i;
|
|
p2=-2700+500i;
|
|
p3=-2500+10i;
|
|
%p1=-3300+2800i;
|
|
%p2=-1500+500i;
|
|
%p3=-1200+10i;
|
|
P=[p1 p1' p2 p2' p3 p3'];
|
|
else
|
|
%2500rad/s = 397Hz -> locate poles here
|
|
p1=-3300+2800i;
|
|
p2=-1900+130i;
|
|
p3=-2900+80i;
|
|
p4=-2300+450i;
|
|
p5=-2000+20i;
|
|
p6=-1500+10i;
|
|
%p1=-3300+2800i;
|
|
%p2=-1500+500i;
|
|
%p3=-1200+10i;
|
|
P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6'];
|
|
end
|
|
K = place(A,B,P);
|
|
%K = acker(A,B,P);
|
|
V=-1./(C*(A-B*K)^-1*B); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
|
|
%Nbar(2)=1; %the voltage stuff is crap for now
|
|
if length(V)>1
|
|
V=V(3); % only the position scaling needed
|
|
end
|
|
|
|
% step answer on closed loop with space state controller:
|
|
t = 0:1E-4:.5;
|
|
ss_cl = ss(A-B*K,B*V,C,0,'Name','space state controller','InputName',mot.ss.InputName,'OutputName',mot.ss.OutputName);
|
|
[y,t,x]=lsim(ss_cl,V*u,t,x0);
|
|
figure();plot(t,y);title('step on closed loop');
|
|
|
|
|
|
% *** observer controller ***
|
|
%
|
|
%observer poles-> 5 times farther left than system poles
|
|
if motid==1
|
|
op1=(p1*5);
|
|
op2=(p2*5);
|
|
op3=(p3*5);
|
|
OP=[op1 op1' op2 op2' op3 op3'];
|
|
else
|
|
op1=(p1*2);
|
|
op2=(p2*2);
|
|
op3=(p3*2);
|
|
op4=(p4*2);
|
|
op5=(p5*2);
|
|
op6=(p6*2);
|
|
OP=[op1 op1' op2 op2' op3 op3'];% op4 op4' op5 op5' op6 op6'];
|
|
end
|
|
L=place(A',C',OP)';
|
|
%L=acker(A',C',OP)';
|
|
|
|
At = [ A-B*K B*K
|
|
zeros(size(A)) A-L*C ];
|
|
Bt = [ B*V
|
|
zeros(size(B)) ];
|
|
Ct = [ C zeros(size(C)) ];
|
|
|
|
% step answer on closed loop with observer controller:
|
|
ss_o = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ss.OutputName);
|
|
figure();lsim(ss_o,ones(size(t)),t,[x0 x0]);title('step on closed loop with observer');
|
|
|
|
|
|
% *** disctrete observer controller ***
|
|
%
|
|
Ts=1/5000; % 5kHz
|
|
ss_od = c2d(ss_o,Ts);
|
|
ss_od .Name='discrete obsvr ctrl';
|
|
% step answer on closed loop with disctrete observer controller:
|
|
t = 0:Ts:.05;
|
|
figure();lsim(ss_od ,ones(size(t)),t,[x0 x0]);title('step on closed loop with observer discrete');
|
|
|
|
|
|
%plot all bode diagrams of desPos->actPos
|
|
figure();
|
|
h=bodeplot(ss_cl(3),ss_o(3),ss_od(3));
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
|
|
|
|
figure();
|
|
h=pzplot(ss_cl(3),ss_o(3),ss_od(3));
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
|
|
|
|
|
|
%calculate matrices for the simulink system
|
|
Ao=A-L*C;
|
|
Bo=[B L];
|
|
Co=K;
|
|
Do=zeros(size(Co,1),size(Bo,2));
|
|
mdlName='observer';
|
|
open(mdlName);
|
|
|
|
%state space controller
|
|
ssc=struct();
|
|
for k=["Ts","A","B","C","D","Ao","Bo","Co","Do","V","K","L","ss_cl","ss_o","ss_od"]
|
|
ssc=setfield(ssc,k,eval(k));
|
|
end
|
|
|
|
end
|
|
|
|
|
|
%code snipplets from an example on youtube (see reference at top)
|
|
function SCRATCH()
|
|
A = [ 0 1 0
|
|
980 0 -2.8
|
|
0 0 -100 ];
|
|
|
|
B = [ 0
|
|
0
|
|
100 ];
|
|
|
|
C = [ 1 0 0 ];
|
|
|
|
poles = eig(A)
|
|
|
|
t = 0:0.01:2;
|
|
u = zeros(size(t));
|
|
x0 = [0.01 0 0];
|
|
|
|
sys = ss(A,B,C,0);
|
|
|
|
[y,t,x] = lsim(sys,u,t,x0);
|
|
plot(t,y)
|
|
title('Open-Loop Response to Non-Zero Initial Condition')
|
|
xlabel('Time (sec)')
|
|
ylabel('Ball Position (m)')
|
|
|
|
p1 = -10 + 10i;
|
|
p2 = -10 - 10i;
|
|
p3 = -50;
|
|
|
|
K = place(A,B,[p1 p2 p3]);
|
|
sys_cl = ss(A-B*K,B,C,0);
|
|
|
|
lsim(sys_cl,u,t,x0);
|
|
xlabel('Time (sec)')
|
|
ylabel('Ball Position (m)')
|
|
|
|
p1 = -20 + 20i;
|
|
p2 = -20 - 20i;
|
|
p3 = -100;
|
|
|
|
K = place(A,B,[p1 p2 p3]);
|
|
sys_cl = ss(A-B*K,B,C,0);
|
|
|
|
lsim(sys_cl,u,t,x0);
|
|
xlabel('Time (sec)')
|
|
ylabel('Ball Position (m)')
|
|
|
|
|
|
t = 0:0.01:2;
|
|
u = 0.001*ones(size(t));
|
|
|
|
sys_cl = ss(A-B*K,B,C,0);
|
|
|
|
lsim(sys_cl,u,t);
|
|
xlabel('Time (sec)')
|
|
ylabel('Ball Position (m)')
|
|
axis([0 2 -4E-6 0])
|
|
|
|
Nbar = rscale(sys,K)
|
|
|
|
lsim(sys_cl,Nbar*u,t)
|
|
title('Linear Simulation Results (with Nbar)')
|
|
xlabel('Time (sec)')
|
|
ylabel('Ball Position (m)')
|
|
axis([0 2 0 1.2*10^-3])
|
|
end
|
|
|
|
|
|
|