352 lines
12 KiB
Matlab
352 lines
12 KiB
Matlab
function [pb]=DeltaTauParam(mot,mdl,param)
|
|
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
|
|
%
|
|
%loads the current (11.10.2018) controller settings of the
|
|
%Powerbrick and opens its simulink file
|
|
%the returned variable pb contains varous parameters:
|
|
% Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D
|
|
|
|
pb=struct(...
|
|
'Ts', 2E-4, ... % 0.2ms=5kHz
|
|
'MaxDac' ,2011.968, ...
|
|
'MaxPosErr', 10000, ...
|
|
'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
|
|
pb.ss_plt=mot.ss_plt;desc=desc+"plt"; pb.sel={3,[3]};
|
|
case 2
|
|
pb.ss_plt=mot.ss_plt0;desc=desc+"plt0"; pb.sel={3,[3]};
|
|
case 3
|
|
pb.ss_plt=mot.ss_c1;desc=desc+"c1"; pb.sel={3,[3]};
|
|
case 4
|
|
pb.ss_plt=mot.ss_d1;desc=desc+"d1"; pb.sel={3,[3]};
|
|
case 5
|
|
pb.ss_plt=mot.ss_1;desc=desc+"1"; pb.sel={2,[2]};
|
|
case 6
|
|
pb.ss_plt=mot.ss_p;desc=desc+"p"; pb.sel={2,[2]};
|
|
case 7
|
|
pb.ss_plt=mot.ss_q;desc=desc+"q"; pb.sel={2,[2]};
|
|
case 8
|
|
pb.ss_plt=mot.ss_cq;desc=desc+"cq"; pb.sel={3,[3]};
|
|
case 9
|
|
pb.ss_plt=mot.ss_cqr;desc=desc+"cqr"; pb.sel={3,[3]};
|
|
case 10
|
|
pb.ss_plt=mot.ss_dq;desc=desc+"dq"; pb.sel={3,[3]};
|
|
end
|
|
desc=desc+" param:";
|
|
curr2acc=mot.mdl.numq; %curr2acc curr_bit to acceleration: zero dB of simplified motion
|
|
pb.curr2acc=curr2acc;
|
|
if mot.id==1
|
|
%!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000)
|
|
%!motor(mot=1,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";
|
|
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=0;%1*pb.Kvfb;
|
|
pb.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
|
|
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=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);
|
|
|
|
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;
|
|
case 2%enhances first step
|
|
desc=desc+"opt1";
|
|
pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=1/(curr2acc*(pb.Ts^2));pb.MaxInt=1000;
|
|
case 3 %pole placement on ss_dq (simplified motion, no current loop)
|
|
desc=desc+"pp ss\_q";
|
|
pl=[-1400 -1440];
|
|
pl=[-300+350i -300-350i];
|
|
[Am,Bm,Cm,Dm]=ssdata(mot.ss_q);
|
|
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(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;
|
|
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];
|
|
[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*1/(curr2acc*(pb.Ts^2))+pb.Kafb;
|
|
pb.Ki=0.01;
|
|
pb.MaxInt=1000;
|
|
display(pb.B)
|
|
fprintf('Kp:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.Kvfb,pb.Kafb);
|
|
case 5 % optimize higher gain and filter
|
|
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 )
|
|
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');
|
|
|
|
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)
|
|
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];
|
|
%19.8Hz 0dB K=(19.8*2*np.pi)**2=15477.1 Ts=5kHz=.2ms
|
|
%Kaff = 1/(Ts*Ts*K) = 1/((19.8*2*np.pi)**2/5000**2) = 1615.2877200403302
|
|
%Kfff=100
|
|
else
|
|
%!motor_servo(mot=2,ctrl='ServoCtrl',Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000)
|
|
%!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";
|
|
%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;
|
|
|
|
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";
|
|
pb.Kp=22;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=240;pb.Kaff=1500;pb.MaxInt=1000;
|
|
case 2%enhances first step
|
|
desc=desc+"opt1";
|
|
pb.Kp=22;pb.Kvfb=450;pb.Ki=0.02;pb.Kvff=450;pb.Kaff=4517;pb.MaxInt=1000;
|
|
%pb.Kp=22;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=240;pb.Kaff=3500;pb.MaxInt=1000;
|
|
case 3 %pole placement on ss_dq (simplified motion, no current loop)
|
|
desc=desc+"pp ss\_q";
|
|
pl=[-300+100i -300-100i];
|
|
[Am,Bm,Cm,Dm]=ssdata(mot.ss_q);
|
|
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(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;
|
|
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)
|
|
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=Kfb(1)/(curr2acc*(pb.Ts^2));
|
|
pb.Kaff=1*1/(curr2acc*(pb.Ts^2))+pb.Kafb;
|
|
pb.Ki=0.01;
|
|
pb.MaxInt=1000;
|
|
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
|
|
%Kfff=100
|
|
%pb.MaxInt=200 %200mA should be enought to fix static errors
|
|
|
|
end
|
|
pb.desc=desc;
|
|
%mdlName='stage_closed_loop';
|
|
%open(mdlName)
|
|
%sim(mdlName)
|
|
end
|