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-03-19 09:10:28 +01:00
parent 0c45705715
commit eda8caf985
10 changed files with 268 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
matlab/DeltaTauOptimizer.m
View File

@@ -10,73 +10,55 @@ function DeltaTauOptimizer()
mot=identifyFxFyStage(7);
end
%SIM1=[1 1; 1 2; 8 2; 9 2;9 3];
%SIM1=[9 1; 9 2; 9 3; 9 4];
%SIM1=[9 1; 9 2; 9 3; 9 4; 9 5; 9 6];
SIM1=[9 1; 9 2; 9 5; 9 6];
%SIM2=[1 1; 1 2; 8 2; 9 2];
SIM1=[9 1; 9 2; 9 3; 9 4];
SIM2=[9 1; 9 2; 9 3; 9 4];
%SIM2=[8 1; 8 2; 8 3; 8 4];
%SIM2=[8 1; 8 2; 9 1; 9 2];
%SIM2=[8 2; 9 2;8 3; 9 3];
%SIM2=[8 3; 9 3;8 4; 9 4];
%SIM2=[9 4;9 0];
%SIM2=[8 3; 9 3];
SIM1=[9 1; 9 2; 9 3; 9 4; 9 5];
%SIM2=[9 1; 9 2; 9 3; 9 4];
SIM2=[9 1; 9 2];
if isempty(simData1)
close all;
simData1=ExecSim(mot{1},SIM1);
end
%if isempty(simData2)
% close all;
% simData2=ExecSim(mot{2},SIM2);
%end
if isempty(simData2)
close all;
simData2=ExecSim(mot{2},SIM2);
end
close all;
test()
%bodeSim(simData1);
%bodeSim(simData2);
%test1();return
%test2();return
bodeSim(simData1);
bodeSim(simData2);
for i =1:2
set(i,'OuterPosition',[80 400 1000 700]);
print(i,sprintf('figures/sim_optimize%d',i),'-depsc');
end
end
function test()
global pb mot simData1 simData2;
%pb=DeltaTauParam(mot{2},8,0);
function test1()
global pb mot simData1;
%pb=DeltaTauParam(mot{1},8,0);
pb=DeltaTauParam(mot{1},9,0);
sim('DeltaTauSim');
i=6;
i=7;
simData1(i).pb=pb;
simData1(i).desPos_actPos=desPos_actPos;
simData1=bodeSim(simData1);
return
%simData2(i).mot_mdl_param=SIM(i,:);
%pb.C=[0.04877];
%pb.D=[1 -0.9512];
%pb.C=[1 -1.3236 6.2472 -11.8555 11.3067 -5.4188 1.0440];
%pb.D=[1.0000 -6.6330 17.6945 -24.5314 18.7409 -7.5020 1.2309];
global tfs
Ts=1/5000;
frq0=55;d0=.5;
frq1=85;d1=.5;
w0=frq0*2*pi;
w1=frq1*2*pi;
tf0=tf([w0^2],[1 2*d0*w0 w0^2 ])
tf1=tf([1 2*d1*w1 w1^2 ],[w1^2])
tfs=tf0*tf1
tfz=c2d(tfs,Ts)
%h=bodeplot(tfz,tfs);setoptions(h,'FreqUnits','Hz','Grid','on');
%pb.C=tfz.Numerator{1};
%pb.D=tfz.Denominator{1};
opt=tfestOptions;
opt.Display='off';
% %tfa=tfest(simData2(i).tfEst, 6, 5,opt);
tfa=tfest(simData2(i).tfEst, 6, 6,opt);
% tfb=1/tfa
% tfc=c2d(tfb,1/5000)
%
% tfs=tf([1],[.001 1])
% tfz=c2d(tfs,1/5000)
% h=bodeplot(tfs,tfz)
% setoptions(h,'FreqUnits','Hz','Grid','on');
% controlSystemDesigner(tfa)
end
function test2()
global pb mot simData2;
%pb=DeltaTauParam(mot{2},8,0); % ss_cq no resonance
pb=DeltaTauParam(mot{2},9,0); % ss_cqr with resonance
sim('DeltaTauSim');
i=3;
simData2(i).pb=pb;
simData2(i).desPos_actPos=desPos_actPos;
simData2=bodeSim(simData2);
return
end
function simData=ExecSim(mot,SIM)
global pb;
@@ -130,4 +112,38 @@ function simData=bodeSim(simData)
set(ax2, 'XScale', 'log');
linkaxes([ax1,ax2],'x')
legend('Location','best');
end
end
function SCRATCH()
%simData2(i).mot_mdl_param=SIM(i,:);
%pb.C=[0.04877];
%pb.D=[1 -0.9512];
%pb.C=[1 -1.3236 6.2472 -11.8555 11.3067 -5.4188 1.0440];
%pb.D=[1.0000 -6.6330 17.6945 -24.5314 18.7409 -7.5020 1.2309];
global tfs
Ts=1/5000;
frq0=55;d0=.5;
frq1=85;d1=.5;
w0=frq0*2*pi;
w1=frq1*2*pi;
tf0=tf([w0^2],[1 2*d0*w0 w0^2 ])
tf1=tf([1 2*d1*w1 w1^2 ],[w1^2])
tfs=tf0*tf1
tfz=c2d(tfs,Ts)
%h=bodeplot(tfz,tfs);setoptions(h,'FreqUnits','Hz','Grid','on');
%pb.C=tfz.Numerator{1};
%pb.D=tfz.Denominator{1};
opt=tfestOptions;
opt.Display='off';
% %tfa=tfest(simData2(i).tfEst, 6, 5,opt);
tfa=tfest(simData2(i).tfEst, 6, 6,opt);
% tfb=1/tfa
% tfc=c2d(tfb,1/5000)
%
% tfs=tf([1],[.001 1])
% tfz=c2d(tfs,1/5000)
% h=bodeplot(tfs,tfz)
% setoptions(h,'FreqUnits','Hz','Grid','on');
% controlSystemDesigner(tfa)
end

188
matlab/DeltaTauParam.m
View File

@@ -45,13 +45,7 @@ function [pb]=DeltaTauParam(mot,mdl,param)
switch param
case 0 %scratch
desc=desc+"scratch";
pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=1/(curr2acc*(pb.Ts^2));pb.MaxInt=1000;
%pure feed forward
pb.Kp=0;pb.Kvfb=0;pb.Ki=0.02;pb.Kvff=0;pb.Kaff=1/(curr2acc*(pb.Ts^2));pb.MaxInt=1000;
%pure feedback
pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=0;pb.Kaff=0;pb.MaxInt=1000;
pl=[-300+350i -300-350i -2513];
pl=[-800+150i -800-150i -2513];
[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
@@ -59,33 +53,49 @@ function [pb]=DeltaTauParam(mot,mdl,param)
Kfb=K*(Cm^-1);
pb.V=V;
pb.Kfb=Kfb;
pb.Kp=V;
pb.B=tf(Kfb(3)/V);
%pb.B=tf(Kfb(3)/V);
pb.Kvfb=Kfb(2)/pb.Ts;
pb.Kvff=1*pb.Kvfb;%0;
pb.Kvff=0;%1*pb.Kvfb;
pb.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;%0;
pb.Ki=0.002;
pb.Kaff=0;%1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.001;
pb.MaxInt=1000;
%https://ch.mathworks.com/help/control/ref/tf.html
%feed forward filter attenuating high frequencies
%fs=1/pb.Ts;%[n,d] = butter(6,fc/(fs/2));
fc=125;%Hz
fc=200;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Fz2=tf(num,den,pb.Ts);
Fz=tf([1 0 0],den/sum(num),pb.Ts);
h=bodeplot(Fz,Fz2);setoptions(h,'FreqUnits','Hz','Grid','on');
%h=bodeplot(Fz,Fz2);setoptions(h,'FreqUnits','Hz','Grid','on');
%pb.F=Fz;
fc=800;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Ez=tf([1 0 0],den/sum(num),pb.Ts);
Bz=tf([.5 .5],[1 0],pb.Ts)
pb.E=Ez; %less noise sensitive on feedback
pb.B=Bz; %less noise sensitive on feedback
pb.F=Fz;
% THE F FILTER HELPS TO NOT OVERSHOOT ON HIGH FREQUENCY TRAJECTORIES,
% BUT THE INPUT TRAJECTORY DOES NOT CONTAIN THESE FREQUENCIES.
% THEREFORE THE FILTERS IN THE CLOSED LOOP MUST BE TWEAKED!
%tf([.2 .8],[1],pb.Ts,'variable','z^-1') = tf([.2 .8],[1 0],pb.Ts)
%pb.A=tf([.0 1],[1 0],pb.Ts)
aa=.9;
k=1.8;
T=1/200;
C_D=tf(k*[T 1],[T*aa 1]);
C_Dz=c2d(C_D,pb.Ts)
%h=bodeplot(C_D,C_Dz);setoptions(h,'FreqUnits','Hz','Grid','on');
pb.C_D=C_Dz;
display(pb.B)
display(pb.E)
display(pb.C_D)
display(pb.F)
Fz=tf([1 0 0],den/sum(num),pb.Ts);
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
case 1 %origin parameters
@@ -137,7 +147,8 @@ function [pb]=DeltaTauParam(mot,mdl,param)
display(pb.B)
fprintf('Kp:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.Kvfb,pb.Kafb);
case 5 % optimize higher gain and filter
pl=[-600+750i -600-750i -2513];
desc=desc+"opt5";
pl=[-800+150i -800-150i -2513];
[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
@@ -145,26 +156,95 @@ function [pb]=DeltaTauParam(mot,mdl,param)
Kfb=K*(Cm^-1);
pb.V=V;
pb.Kfb=Kfb;
pb.Kp=V;
pb.B=tf(Kfb(3)/V);
%pb.B=tf(Kfb(3)/V);
pb.Kvfb=Kfb(2)/pb.Ts;
pb.Kvff=.15*pb.Kvfb;%0;
pb.Kvff=1*pb.Kvfb;
pb.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=.2/(curr2acc*(pb.Ts^2))+pb.Kafb;%0;
pb.Ki=0.002;
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.001;
pb.MaxInt=1000;
%https://ch.mathworks.com/help/control/ref/tf.html
%feed forward filter attenuating high frequencies
%fs=1/pb.Ts;%[n,d] = butter(6,fc/(fs/2));
fc=125;%Hz
fc=200;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Fz2=tf(num,den,pb.Ts);
Fz=tf([1 0 0],den/sum(num),pb.Ts);
h=bodeplot(Fz,Fz2);setoptions(h,'FreqUnits','Hz','Grid','on');
pb.F=Fz;
%h=bodeplot(Fz,Fz2);setoptions(h,'FreqUnits','Hz','Grid','on');
fc=800;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Ez=tf([1 0 0],den/sum(num),pb.Ts);
Bz=tf([.5 .5],[1 0],pb.Ts) % fast simple lowpass: -3dB at 1250Hz
%pb.F=Fz; %filtering does not help in the domain below 50Hz. it attenuates at high frq. but makes things worse at low frq.
pb.E=Ez; %less noise sensitive on feedback
pb.B=Bz; %less noise sensitive on feedback
display(pb.B)
display(pb.F)
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
Fz=tf([1 0 0],den/sum(num),pb.Ts);
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
case 6
desc=desc+"opt6";
pl=[-800+150i -800-150i -2513];
[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
V=V(end);
Kfb=K*(Cm^-1);
pb.V=V;
pb.Kfb=Kfb;
pb.Kp=V;
%pb.B=tf(Kfb(3)/V);
pb.Kvfb=Kfb(2)/pb.Ts;
pb.Kvff=1*pb.Kvfb;
pb.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.001;
pb.MaxInt=1000;
%https://ch.mathworks.com/help/control/ref/tf.html
%feed forward filter attenuating high frequencies
%fs=1/pb.Ts;%[n,d] = butter(6,fc/(fs/2));
fc=200;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Fz2=tf(num,den,pb.Ts);
Fz=tf([1 0 0],den/sum(num),pb.Ts);
%h=bodeplot(Fz,Fz2);setoptions(h,'FreqUnits','Hz','Grid','on');
%pb.F=Fz;
fc=800;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Ez=tf([1 0 0],den/sum(num),pb.Ts);
Bz=tf([.5 .5],[1 0],pb.Ts)
pb.E=Ez; %less noise sensitive on feedback
pb.B=Bz; %less noise sensitive on feedback
aa=.9;
k=1.8;
T=1/200;
C_D=tf(k*[T 1],[T*aa 1]);
C_Dz=c2d(C_D,pb.Ts)
%h=bodeplot(C_D,C_Dz);setoptions(h,'FreqUnits','Hz','Grid','on');
pb.C_D=C_Dz;
display(pb.B)
display(pb.E)
display(pb.C_D)
display(pb.F)
Fz=tf([1 0 0],den/sum(num),pb.Ts);
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
end
%pb.Kp=0.1;pb.Kvfb=0;pb.Ki=0.00;pb.Kvff=0;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000;
%filter [z^0 z^-1 ... z^-n];
@@ -176,29 +256,37 @@ function [pb]=DeltaTauParam(mot,mdl,param)
%!motor(mot=2,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index')
switch param
case 0 %scratch
desc=desc+"scratch";
pb.Kp=20.3255;
pb.Kvfb=582.4551;pb.Kvff=582.4551;
pb.Kaff=5595.2;pb.Kafb=1077.9;
pb.Ki=0.01;pb.MaxInt=1000;
desc=desc+"scratch";
%pole(mot.ss_dq)
pl=[-300+150i -300-150i -2513];
[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
V=V(end);
Kfb=K*(Cm^-1);
pb.V=V;
pb.Kfb=Kfb;
pb.Kp=V;
%pb.B=tf(Kfb(3)/V);
pb.Kvfb=Kfb(2)/pb.Ts;
pb.Kvff=1*pb.Kvfb;
pb.Kafb=0.1*Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.001;
pb.MaxInt=1000;
% Ts=pb.Ts;
% frq=75;d=.6;
% w0=frq*2*pi;
% tfs=tf([w0^2],[1 2*d*w0 w0^2 ])
% global currTf
% frq0=55;d0=.5;
% frq1=85;d1=.5;
% w0=frq0*2*pi;
% w1=frq1*2*pi;
% tf0=tf([w0^2],[1 2*d0*w0 w0^2 ])
% tf1=tf([1 2*d1*w1 w1^2 ],[w1^2])
% tfs=tf0*tf1
% tfz=c2d(tfs,Ts)
% h=bodeplot(tfz,tfs);setoptions(h,'FreqUnits','Hz','Grid','on');
% pb.C=tfz.Numerator{1};
% pb.D=tfz.Denominator{1};
% currTf=tfz
fc=800;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Ez=tf([1 0 0],den/sum(num),pb.Ts);
%Az=tf([.1 .2 .7],[1 0 0],pb.Ts) %is similar to the phase at feed forward only
%h=bodeplot(Az);setoptions(h,'FreqUnits','Hz','Grid','on');
%Bz=tf([.5 .5],[1 0],pb.Ts)
%pb.E=Ez; %less noise sensitive on feedback
%pb.B=Bz; %less noise sensitive on feedback
fprintf('Kp:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.Kvfb,pb.Kafb);
case 1 %origin parameters
desc=desc+"orig";

BIN
matlab/DeltaTauSim.slx
View File

Binary file not shown.