sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
zamofing_t
2019-02-11 16:55:58 +01:00
parent 583d86d4d6
commit 3412e33595
8 changed files with 130 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files

211
matlab/StateSpaceControlDesign.m
View File

@@ -1,4 +1,4 @@
function [ssc]=StateSpaceControlDesign(mot)
function [ssc]=StateSpaceControlDesign(mot,mode)
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() to build a motor object !!!
%
% builds a state space controller designed for the plant.
@@ -15,69 +15,81 @@ function [ssc]=StateSpaceControlDesign(mot)
%
% 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
%plant and model
% ss_plt :real plant (model of real plant)
% ss_c1 :current, mechanic, no resonance
% ss_d1 :simpl. current, mechanic, no resonance
% ss_1 :no current, mechanic, no resonance
% ss_0 :no current, simpl. mechanic, no resonance
%mPrefilt:prefilter mode
%prefilt: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:
%verb: mode(bits) to plot/simulate
% 0: 1: poles of model and placed poles of controller
% 1: 2: bode plots of open loop
% 2: 4: step answer on open loop
% 3: 8: step answer on closed loop with space state controller
% 4: 16: step answer on closed loop with observer controller
% 5: 32: step answer on closed loop with disctrete observer controller
% 6: 64: plot all closed loop bode and pole-zero diagrams of desPos->actPos
% 7:128: bode plot of filt_pos_err
%use_lqr: use lqr instead of pole placement
mPlt=0;
mMdl=1;
mPrefilt=2;
mShow=32+64;
verb=1;
use_lqr=0;
switch mPlt
filt_pos_err=Prefilt(mot,2);
%locate poles: 2500rad/s = 397Hz, 6300rad/s = 1027Hz
switch mode
case -1 %TESTING
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_d1; %ss_plt ss_c1 ss_d1 ss_1 ss_0
if mot.id==1
pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0;
else
pl=[-2500 -900 -800];
end
plObs=2*pl;
case 0
ss_plt=mot.ss_plt;
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_c1; %ss_plt ss_c1 ss_d1 ss_1 ss_0
if mot.id==1
pl=[-3300 -3200 -2900 -2800];
else
pl=[-3300 -3200 -2700 -2600];
end
plObs=2*pl;
case 1
ss_plt=mot.ss_c1;
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_d1; %ss_plt ss_c1 ss_d1 ss_1 ss_0
if mot.id==1
pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0;
else
pl=[-2500 -900 -800];
end
plObs=2*pl;
case 2
ss_plt=mot.ss_d1;
case 3
ss_plt=mot.ss_1;
case 4
ss_plt=mot.ss_0;
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_c1; %ss_plt ss_c1 ss_d1 ss_1 ss_0
use_lqr=1
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
[Am,Bm,Cm,Dm]=ssdata(ss_mdl);
if bitand(mShow,1)
if bitand(verb,1) && use_lqr==0
format compact
format shortG
disp(pole(ss_mdl)) %==eig(Am)
%damp(ss_mdl) %further informations
disp(pl)
disp(plObs)
format short
end
if bitand(verb,2)
figure();h=bodeplot(ss_plt,ss_mdl);
setoptions(h,'IOGrouping','all')
end
@@ -85,7 +97,7 @@ function [ssc]=StateSpaceControlDesign(mot)
xp0 = zeros(1,length(ss_plt.A));
xm0 = zeros(1,length(Am));
if bitand(mShow,2)
if bitand(verb,4)
% step answer on open loop:
t = 0:1E-4:.5;
u = ones(size(t));
@@ -94,7 +106,6 @@ function [ssc]=StateSpaceControlDesign(mot)
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);
%-------------------
@@ -104,51 +115,12 @@ function [ssc]=StateSpaceControlDesign(mot)
%
%place poles for the controller feedback
if use_lqr %use the lqr controller
Q=eye(length(ss_mdl.A));
Q=eye(length(Am));
R=1;
[K,P,E]=lqr(ss_mdl,Q,R,0);
[K,P,E]=lqr(Am,Bm,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];
P=[-2500 -2800 -1100+10j -1100-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);
K = place(Am,Bm,pl);
%K = acker(Am,Bm,pl);
end %if lqr
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
@@ -157,7 +129,7 @@ function [ssc]=StateSpaceControlDesign(mot)
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)
if bitand(verb,8)
% step answer on closed loop with space state controller:
t = 0:1E-4:.5;
[y,t,x]=lsim(ss_cl,V*u,t,xm0);
@@ -166,10 +138,15 @@ function [ssc]=StateSpaceControlDesign(mot)
% *** observer controller ***
%
%observer poles-> 5 times farther left than system poles
OP=2*P;
L=place(Am',Cm',OP)';
%L=acker(A',C',OP)';
%observer poles
if use_lqr %use the lqr controller
Q=eye(length(Am')); % ??????????????? CHANGES NEEDED ????????????
R=eye(size(Cm,1));
[L,P,E]=lqr(Am',Cm',Q,R,0);
else
L=place(Am',Cm',plObs)';
%L=acker(A',C',plObs)';
end
At = [ Am-Bm*K Bm*K
zeros(size(Am)) Am-L*Cm ];
@@ -179,7 +156,7 @@ function [ssc]=StateSpaceControlDesign(mot)
Dt=0;
ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ss_mdl.OutputName);
if bitand(mShow,8)
if bitand(verb,16)
% 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
@@ -191,20 +168,16 @@ function [ssc]=StateSpaceControlDesign(mot)
[Atz,Btz,Ctz,Dtz]=ssdata(ss_tz );
ss_tz.Name='discrete obsvr ctrl';
if bitand(mShow,16)
if bitand(verb,32)
% 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)
if bitand(verb,64)
%plot all bode diagrams of desPos->actPos
figure();
if mMdl==2 || mMdl==3
idx=1;
else
idx=3;
end
idx=length(ss_cl.OutputName);
h=bodeplot(ss_cl(idx),ss_t(idx),ss_tz(idx));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
@@ -221,13 +194,9 @@ function [ssc]=StateSpaceControlDesign(mot)
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
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',[{'desPos'}; ss_mdl.OutputName ],'OutputName',{'k*xt'});
%discrete plant
%ss_pltz = c2d(ss_plt,Ts);
%[Apz,Bpz,Cpz,Dpz]=ssdata(ss_pltz);
@@ -235,20 +204,17 @@ function [ssc]=StateSpaceControlDesign(mot)
%discrete observer controller
ss_oz = c2d(ss_o,Ts);
%prefilter to compensate non observable resonance frequencies
prefilt=Prefilt(mot,mPrefilt);
%discrete prefilter
prefiltz=c2d(prefilt,Ts);
filt_pos_err_z=c2d(filt_pos_err,Ts);
if bitand(mShow,64)
h=bodeplot(prefilt,prefiltz);
if bitand(verb,128)
h=bodeplot(filt_pos_err,filt_pos_err_z);
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
end
%state space controller
ssc=struct();
for k=["Ts","ss_plt","ss_o","ss_oz","prefilt","prefiltz","V"]
for k=["Ts","ss_plt","ss_o","ss_oz","filt_pos_err","filt_pos_err_z","V"]
ssc=setfield(ssc,k,eval(k));
end
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
@@ -275,14 +241,13 @@ function pf=Prefilt(mot,mode)
denV=den1;
pf=tf(numV,denV);
else
%Lag
f=[100 200]; w=f*2*pi; T=1./w;
tf1=tf([T(1) 1],[T(2) 1]);
%bo = bodeoptions;
%bo.FreqUnits = 'Hz'; bo.MagUnits='abs'; bo.Grid='on';
%bode(tf1,bo)
%k=1.2; aa=2; f=[40 60];w=f*2*pi; tf([1 33 w0^2]; den3=[1 20 w0^2];
%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];