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

optimize

Browse Source
This commit is contained in:
zamofing_t
2019-03-08 10:18:20 +01:00
parent c962ebdc63
commit 0c45705715
4 changed files with 167 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
matlab/DeltaTauOptimizer.m
View File

@@ -19,24 +19,32 @@ function DeltaTauOptimizer()
%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];
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);
test()
%bodeSim(simData1);
%bodeSim(simData2);
end
function test()
global pb mot simData1 simData2;
%pb=DeltaTauParam(mot{2},8,0);
pb=DeltaTauParam(mot{2},9,0);
pb=DeltaTauParam(mot{1},9,0);
sim('DeltaTauSim');
i=6;
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];
@@ -51,15 +59,9 @@ function test()
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};
sim('DeltaTauSim');
i=2;
%simData2(i).mot_mdl_param=SIM(i,:);
simData2(i).pb=pb;
simData2(i).desPos_actPos=desPos_actPos;
simData2=bodeSim(simData2);
%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';

97
matlab/DeltaTauParam.m
View File

@@ -10,8 +10,9 @@ function [pb]=DeltaTauParam(mot,mdl,param)
'Ts', 2E-4, ... % 0.2ms=5kHz
'MaxDac' ,2011.968, ...
'MaxPosErr', 10000, ...
'A',1,'B',1,'C',1,'D',1,'E',1,'F',1, ...
'Kafb',0);
'A',tf(1),'B',tf(1),'C_D',tf(1),'E',tf(1),'F',tf(1), ...
'Kafb',0, ...
'V',0,'Kfb',0);
desc=sprintf('mot:%d mdl:',mot.id);
switch mdl
case 1
@@ -44,6 +45,49 @@ 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];
[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;%0;
pb.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;%0;
pb.Ki=0.002;
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
[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;
% 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)
display(pb.B)
display(pb.F)
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
case 1 %origin parameters
desc=desc+"orig";
pb.Kp=25;pb.Kvfb=400;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=5000;pb.MaxInt=1000;
@@ -63,13 +107,14 @@ function [pb]=DeltaTauParam(mot,mdl,param)
pb.Kfb=Kfb;
pb.Kp=V;
pb.B=Kfb(2)/V;
pb.B=tf(Kfb(2)/V);
pb.Kvfb=Kfb(1)/pb.Ts;
pb.Kvff=pb.Kvfb;
pb.Kaff=1/(curr2acc*(pb.Ts^2));
pb.Ki=0.01; % lower
pb.MaxInt=1000;
fprintf('Kp:%f B:%f Kvfb:%f\n',pb.Kp,pb.B,pb.Kvfb);
display(pb.B)
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
case 4 %pole placement on ss_dq (simplified motion, simplified current loop)
desc=desc+"pp ss\_dq";
pl=[-300+350i -300-350i -2513];
@@ -82,14 +127,44 @@ function [pb]=DeltaTauParam(mot,mdl,param)
pb.Kfb=Kfb;
pb.Kp=V;
pb.B=Kfb(3)/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*1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.01;
pb.MaxInt=1000;
fprintf('Kp:%f B:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.B,pb.Kvfb,pb.Kafb);
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];
[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=.15*pb.Kvfb;%0;
pb.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=.2/(curr2acc*(pb.Ts^2))+pb.Kafb;%0;
pb.Ki=0.002;
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
[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;
display(pb.B)
display(pb.F)
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];
@@ -144,13 +219,14 @@ function [pb]=DeltaTauParam(mot,mdl,param)
pb.Kfb=Kfb;
pb.Kp=V;
pb.B=Kfb(2)/V;
pb.B=tf(Kfb(2)/V);
pb.Kvfb=Kfb(1)/pb.Ts;
pb.Kvff=pb.Kvfb;
pb.Kaff=1/(curr2acc*(pb.Ts^2));
pb.Ki=0.01; % lower
pb.MaxInt=1000;
fprintf('Kp:%f B:%f Kvfb:%f\n',pb.Kp,pb.B,pb.Kvfb);
display(pb.B)
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
case 4 %pole placement on ss_dq (simplified motion, simplified current loop)
desc=desc+"pp ss\_dq";
%pole(mot.ss_dq)
@@ -164,14 +240,15 @@ function [pb]=DeltaTauParam(mot,mdl,param)
pb.Kfb=Kfb;
pb.Kp=V;
pb.B=Kfb(3)/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*1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.01;
pb.MaxInt=1000;
fprintf('Kp:%f B:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.B,pb.Kvfb,pb.Kafb);
display(pb.B)
fprintf('Kp:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.Kvfb,pb.Kafb);
end
%11.84Hz 0dB K=(11.84*2*np.pi)**2=5534.3 Ts=5kHz=.2ms
%Kaff = 1/(Ts*Ts*K) = 1/((11.84*2*np.pi)**2/5000**2) = 4517.278506241803

BIN
matlab/DeltaTauSim.slx
View File

Binary file not shown.