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

major renaming of transfer functions

Browse Source
This commit is contained in:
zamofing_t
2019-02-15 10:46:29 +01:00
parent f3c5fafe5f
commit e792d0b028
11 changed files with 592 additions and 580 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
matlab/StateSpaceControlDesign.m
View File

@@ -39,43 +39,67 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
%use_lqr: use lqr instead of pole placement
verb=1;
t=0:1E-4:.5; %time vector for simulations
verb=0;
use_lqr=0;
MaxDac=2011.968;
filt_pos_err=Prefilt(mot,2);
tfDesPos=tf(1,1);
tfPosErr=tf(1,1);
ssc=struct();
ssc.sel1={3,[3]};
%locate poles: 2500rad/s = 397Hz, 6300rad/s = 1027Hz
switch mode
case -1 %TESTING
case 0 %for document: best observer without prefilter
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 ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_c1; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_cp; %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
case 1 %for document: best observer without prefilter
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
ss_mdl=mot.ss_cp; %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;
tfDesPos=Prefilt(mot,2);%user designed envelope
tfPosErr=Prefilt(mot,1);%inverse resonance
case 2 %for document: best observer with prefilter
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_dp; %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
case 3
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
ss_mdl=mot.ss_dp; %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 4
%this is the hovering ball model
A = [ 0 1 0; 980 0 -2.8;0 0 -100 ];
B = [ 0 0 100 ]';
C = [ 1 0 0 ];
D = 0;
ssBall=ss(A,B,C,D,'InputName','iCmd','OutputName','actPos');
ss_plt=ssBall; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=ssBall; %ss_plt ss_c1 ss_d1 ss_1 ss_0
pl=[-10+10i -10-10i -50];
plObs=[-100 -101 -102];
end
[Am,Bm,Cm,Dm]=ssdata(ss_mdl);
@@ -100,12 +124,14 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
if bitand(verb,4)
% 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')
figure();hold on; plot(t,yp,'DisplayName',ss_plt.OutputName{1})
plot(t,ym,'--','DisplayName',ss_plt.OutputName{1});
title('step on open loop (plant and model)');
%legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
legend('location','best')
end
%w0=abs(poles);
%ang=angle(-poles);
@@ -126,13 +152,13 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
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
disp(['scaling (should be actPos): ' ss_mdl.OutputName{end}])
V=V(end); % 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(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);
figure();plot(t,y);title('step on closed loop');
end
@@ -206,7 +232,8 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
ss_oz = c2d(ss_o,Ts);
%discrete prefilter
filt_pos_err_z=c2d(filt_pos_err,Ts);
tfDesPos_z=c2d(tfDesPos,Ts);
tfPosErr_z=c2d(tfPosErr,Ts);
if bitand(verb,128)
h=bodeplot(filt_pos_err,filt_pos_err_z);
@@ -214,11 +241,10 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
end
%state space controller
ssc=struct();
for k=["Ts","ss_plt","ss_o","ss_oz","filt_pos_err","filt_pos_err_z","V","MaxDac"]
for k=["Ts","ss_plt","ss_o","ss_oz","tfDesPos","tfDesPos_z","tfPosErr","tfPosErr_z","V","MaxDac"]
ssc=setfield(ssc,k,eval(k));
end
mat2py=struct();
%[ozA,ozB,ozC,ozD]=ssdata(ss_oz);
%[pos_err_num,pos_err_den]=tfdata(filt_pos_err_z);
@@ -230,12 +256,9 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
mat2py.ozB=ss_oz.B;
mat2py.ozC=ss_oz.C;
mat2py.ozD=ss_oz.D;
mat2py.ozInpName=ss_oz.InputName
mat2py.ozOutName=ss_oz.OutputName
mat2py.pos_err_num=filt_pos_err_z.Numerator{1};
mat2py.pos_err_den=filt_pos_err_z.Denominator{1};
mat2py.ozInpName=ss_oz.InputName;
mat2py.ozOutName=ss_oz.OutputName;
fn=[pwd '/' sprintf( 'ssc%d.mat',mot.id)];
save(fn,'-struct','mat2py');
disp(['saved ' fn]);
@@ -247,23 +270,26 @@ function pf=Prefilt(mot,mode)
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];
den=mot.mdl.num1;%num=1;
num=mot.mdl.den1;%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];
den=conv(conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.num3),mot.mdl.num4);%num=1;
num=conv(conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.den3),mot.mdl.den4);%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);
%f=200;w0=f*2*pi; num1=[1 300 w0^2]; den1=[1 200 w0^2];
%numV=num1;
%denV=den1;
%pf=tf(numV,denV);
%Lag
f=[200 300]; w=f*2*pi; T=1./w;
pf=tf([T(1) 1],[T(2) 1]);
else
%Lag
f=[100 200]; w=f*2*pi; T=1./w;
f=[200 400]; 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';