304 lines
8.9 KiB
Matlab
304 lines
8.9 KiB
Matlab
function [ssc]=StateSpaceControlDesign(mot)
|
|
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() to build 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
|
|
%
|
|
% the matchich simulink model is: 'observer'
|
|
|
|
%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
|
|
|
|
%mPlt: mode to select plant
|
|
%mMdl: mode to select model for observer
|
|
%0 ss_plt :real plant (model of real plant)
|
|
%1 ss_c1 :current, mechanic, no resonance
|
|
%2 ss_d1 :simpl. current, mechanic, no resonance
|
|
%3 ss_1 :no current, mechanic, no resonance
|
|
%4 ss_0 :no current, simpl. mechanic, no resonance
|
|
|
|
%mPrefilt:prefilter mode
|
|
%0 no filter
|
|
%1 inverse resonance filter
|
|
%2 manual setup filter
|
|
|
|
|
|
%mShow: mode(bits) to plot/simulate
|
|
% 0: 1: bode plots of open loop
|
|
% 1: 2: step answer on open loop
|
|
% 2: 4: step answer on closed loop with space state controller
|
|
% 3: 8: step answer on closed loop with observer controller
|
|
% 4:16: step answer on closed loop with disctrete observer controller
|
|
% 5:32: plot all closed loop bode and pole-zero diagrams of desPos->actPos
|
|
% 6:64:
|
|
|
|
|
|
%use_lqr: use lqr instead of pole placement
|
|
|
|
mPlt=0;
|
|
mMdl=1;
|
|
mPrefilt=2;
|
|
mShow=32+64;
|
|
use_lqr=0;
|
|
|
|
switch mPlt
|
|
case 0
|
|
ss_plt=mot.ss_plt;
|
|
case 1
|
|
ss_plt=mot.ss_c1;
|
|
case 2
|
|
ss_plt=mot.ss_d1;
|
|
case 3
|
|
ss_plt=mot.ss_1;
|
|
case 4
|
|
ss_plt=mot.ss_0;
|
|
end
|
|
ss_plt.Name='open loop plant';
|
|
|
|
switch mMdl
|
|
case 0
|
|
ss_mdl=mot.ss_plt;
|
|
case 1
|
|
ss_mdl=mot.ss_c1;
|
|
case 2
|
|
ss_mdl=mot.ss_d1;
|
|
case 3
|
|
ss_mdl=mot.ss_1;
|
|
case 4
|
|
ss_mdl=mot.ss_0;
|
|
end
|
|
ss_mdl.Name='open loop model'; %model for observer
|
|
|
|
|
|
[Ap,Bp,Cp,Dp]=ssdata(ss_plt);
|
|
[Am,Bm,Cm,Dm]=ssdata(ss_mdl);
|
|
|
|
if bitand(mShow,1)
|
|
figure();h=bodeplot(ss_plt,ss_mdl);
|
|
setoptions(h,'IOGrouping','all')
|
|
end
|
|
|
|
xp0 = zeros(1,length(Ap));
|
|
xm0 = zeros(1,length(Am));
|
|
|
|
if bitand(mShow,2)
|
|
% step answer on open loop:
|
|
t = 0:1E-4:.5;
|
|
u = ones(size(t));
|
|
[yp,t,x] = lsim(ss_plt,u,t,xp0);
|
|
[ym,t,x] = lsim(ss_mdl,u,t,xm0);
|
|
figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)');
|
|
legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
|
|
end
|
|
poles = eig(Am);
|
|
%w0=abs(poles);
|
|
%ang=angle(-poles);
|
|
%-------------------
|
|
%p=w0.*exp(j.*ang)
|
|
|
|
% *** space state controller ***
|
|
%
|
|
%place poles for the controller feedback
|
|
if use_lqr %use the lqr controller
|
|
Q=eye(length(ss_mdl.A));
|
|
R=1;
|
|
[K,P,E]=lqr(ss_mdl,Q,R,0);
|
|
else
|
|
if mot.id==1
|
|
%2500rad/s = 397Hz -> locate poles here
|
|
%6300rad/s = 1027Hz -> locate poles here
|
|
switch mMdl
|
|
case 0
|
|
p1=-3300+2800i; p2=-2700+500i; p3=-2500+10i;
|
|
P=[p1 p1' p2 p2' p3 p3'];
|
|
case 1
|
|
%p1=-6300+280i; p2=-6200+150i;
|
|
%P=[p1 p1' p2 p2'];
|
|
P=[-4100 -4000 -1500+10j -1500-10j];
|
|
case 2
|
|
%p1=-6300+280i; p2=-6200+150i;
|
|
%P=[p1 p1' p2 p2'];
|
|
P=[-1500+10j -1500-10j];
|
|
case 3
|
|
%p1=-6300+280i; p2=-6200+150i;
|
|
%P=[p1 p1' p2 p2'];
|
|
P=[-1500+10j -1500-10j -1400 -1300];
|
|
end
|
|
else
|
|
%2500rad/s = 397Hz -> locate poles here
|
|
%6300rad/s = 1027Hz -> locate poles here
|
|
switch mMdl
|
|
case 0
|
|
p1=-3300+2800i; p2=-1900+130i; p3=-2900+80i;
|
|
p4=-2300+450i; p5=-2000+20i; p6=-1500+10i;
|
|
P=[p1 p1' p2 p2' p3 p3' p4 p4' p5 p5' p6 p6'];
|
|
case 1
|
|
%p1=-6300+2800i; p2=-6200+1500i;
|
|
%P=[p1 p1' p2 p2'];
|
|
P=[-2500 -2800 -1500+10j -1500-10j];
|
|
case 2
|
|
%p1=-6300+2800i; p2=-6200+1500i;
|
|
%P=[p1 p1' p2 p2'];
|
|
P=[-2500 -2800 -1500+10j -1500-10j];
|
|
end
|
|
end
|
|
K = place(Am,Bm,P);
|
|
%K = acker(Am,Bm,Pm);
|
|
end %if lqr
|
|
|
|
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
|
|
if length(V)>1
|
|
V=V(3); % only the position scaling needed
|
|
end
|
|
|
|
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ss_mdl.InputName,'OutputName',ss_mdl.OutputName);
|
|
if bitand(mShow,4)
|
|
% step answer on closed loop with space state controller:
|
|
t = 0:1E-4:.5;
|
|
[y,t,x]=lsim(ss_cl,V*u,t,xm0);
|
|
figure();plot(t,y);title('step on closed loop');
|
|
end
|
|
|
|
% *** observer controller ***
|
|
%
|
|
%observer poles-> 5 times farther left than system poles
|
|
OP=2*P;
|
|
L=place(Am',Cm',OP)';
|
|
%L=acker(A',C',OP)';
|
|
|
|
At = [ Am-Bm*K Bm*K
|
|
zeros(size(Am)) Am-L*Cm ];
|
|
Bt = [ Bm*V
|
|
zeros(size(Bm)) ];
|
|
Ct = [ Cm zeros(size(Cm)) ];
|
|
|
|
Dt=0;
|
|
ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ss_mdl.OutputName);
|
|
if bitand(mShow,8)
|
|
% step answer on closed loop with observer controller:
|
|
figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
|
|
end
|
|
|
|
% *** disctrete observer controller ***
|
|
%
|
|
Ts=1/5000; % 5kHz
|
|
ss_tz = c2d(ss_t,Ts);
|
|
[Atz,Btz,Ctz,Dtz]=ssdata(ss_tz );
|
|
ss_tz.Name='discrete obsvr ctrl';
|
|
|
|
if bitand(mShow,16)
|
|
% step answer on closed loop with disctrete observer controller:
|
|
t = 0:Ts:.05;
|
|
figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
|
|
end
|
|
|
|
if bitand(mShow,32)
|
|
%plot all bode diagrams of desPos->actPos
|
|
figure();
|
|
if mMdl==2 || mMdl==3
|
|
idx=1;
|
|
else
|
|
idx=3;
|
|
end
|
|
h=bodeplot(ss_cl(idx),ss_t(idx),ss_tz(idx));
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
|
|
|
|
figure();
|
|
h=pzplot(ss_cl(idx),ss_t(idx));
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
|
|
figure();
|
|
h=pzplot(ss_tz(idx));
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
|
|
end
|
|
|
|
%calculate matrices for the simulink system
|
|
Ao=Am-L*Cm;
|
|
Bo=[Bm L];
|
|
Co=K;
|
|
Do=zeros(size(Co,1),size(Bo,2));
|
|
|
|
if mMdl==2 || mMdl==3
|
|
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','actPos'},'OutputName',{'k*xt'});
|
|
else
|
|
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'});
|
|
end
|
|
|
|
%discrete plant
|
|
%ss_pltz = c2d(ss_plt,Ts);
|
|
%[Apz,Bpz,Cpz,Dpz]=ssdata(ss_pltz);
|
|
|
|
%discrete observer controller
|
|
ss_oz = c2d(ss_o,Ts);
|
|
[Aoz,Boz,Coz,Doz]=ssdata(ss_oz);
|
|
%mdlName='observer';
|
|
%open(mdlName);
|
|
|
|
|
|
%prefilter to compensate non observable resonance frequencies
|
|
prefilt=Prefilt(mot,mPrefilt);
|
|
|
|
numV=prefilt.Numerator{1};
|
|
denV=prefilt.Denominator{1};
|
|
|
|
%discrete prefilter
|
|
prefiltz=c2d(prefilt,Ts);
|
|
numVz=prefiltz.Numerator{1};
|
|
denVz=prefiltz.Denominator{1};
|
|
|
|
if bitand(mShow,64)
|
|
h=bodeplot(prefilt,prefiltz);
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
|
|
end
|
|
|
|
%state space controller
|
|
ssc=struct();
|
|
%for k=["Ts","At","Bt","Ct","Dt","Atz","Btz","Ctz","Dtz","Ap","Bp","Cp","Dp","Am","Bm","Cm","Dm","Ao","Bo","Co","Do","Apz","Bpz","Cpz","Dpz","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"]
|
|
%for k=["Ts","Ap","Bp","Cp","Dp","Ao","Bo","Co","Do","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"]
|
|
for k=["Ts","ss_plt","ss_o","ss_oz","prefilt","prefiltz","V"]
|
|
ssc=setfield(ssc,k,eval(k));
|
|
end
|
|
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
|
|
end
|
|
|
|
function pf=Prefilt(mot,mode)
|
|
switch mode
|
|
case 0 %no filter
|
|
pf=tf(1,1);
|
|
case 1 %inverse resonance
|
|
if mot.id==1
|
|
den=mot.mdl.num2;%num=1;
|
|
num=mot.mdl.den2;%den=[1 0 0];
|
|
pf=tf(num,den);
|
|
else
|
|
den=conv(conv(conv(mot.mdl.num2,mot.mdl.num3),mot.mdl.num4),mot.mdl.num5);%num=1;
|
|
num=conv(conv(conv(mot.mdl.den2,mot.mdl.den3),mot.mdl.den4),mot.mdl.den5);%den=[1 0 0];
|
|
pf=tf(num,den);
|
|
end
|
|
case 2
|
|
if mot.id==1
|
|
f=200;w0=f*2*pi; num1=[1 300 w0^2]; den1=[1 200 w0^2];
|
|
numV=num1;
|
|
denV=den1;
|
|
pf=tf(numV,denV);
|
|
else
|
|
f=277;w0=f*2*pi; num1=[1 20 w0^2]; den1=[1 500 w0^2];
|
|
f=138;w0=f*2*pi; num2=[1 300 w0^2]; den2=[1 100 w0^2];
|
|
f=60;w0=f*2*pi; num3=[1 33 w0^2]; den3=[1 20 w0^2];
|
|
numV=conv(num1,num2);
|
|
denV=conv(den1,den2);
|
|
numV=conv(conv(num1,num2),num3);
|
|
denV=conv(conv(den1,den2),den3) ;
|
|
pf=tf(numV,denV);
|
|
end
|
|
end
|
|
%controlSystemDesigner('bode',1,pf); % <<<<<<<<< This opens a transferfunction that can be edited
|
|
end
|
|
|
|
|