adding optimizer tool and found better parameters
This commit is contained in:
91
matlab/DeltaTauOptimizer.m
Normal file
91
matlab/DeltaTauOptimizer.m
Normal file
@@ -0,0 +1,91 @@
|
||||
%before calling that put following line in the command window:
|
||||
%
|
||||
% global mot pb;
|
||||
% or even:
|
||||
% clear global all;clear all;global mot pb;
|
||||
|
||||
function DeltaTauOptimizer()
|
||||
global mot simData1 simData2;
|
||||
if isempty(mot)
|
||||
mot=identifyFxFyStage(7);
|
||||
end
|
||||
SIM1=[1 1; 1 2; 8 2; 9 2];
|
||||
SIM2=[1 1; 1 2; 8 2; 9 2];
|
||||
if isempty(simData1)
|
||||
close all;
|
||||
simData1=ExecSim(mot{1},SIM1);
|
||||
end
|
||||
if isempty(simData2)
|
||||
close all;
|
||||
simData2=ExecSim(mot{2},SIM2);
|
||||
end
|
||||
close all;
|
||||
%test()
|
||||
bodeSim(simData1);
|
||||
bodeSim(simData2);
|
||||
end
|
||||
|
||||
function test()
|
||||
global pb mot simData1 simData2;
|
||||
%pb=DeltaTauParam(mot{2},7,0);
|
||||
pb=DeltaTauParam(mot{2},9,0);
|
||||
sim('DeltaTauSim');
|
||||
i=6;
|
||||
%simData2(i).mot_mdl_param=SIM(i,:);
|
||||
simData2(i).pb=pb;
|
||||
simData2(i).desPos_actPos=desPos_actPos;
|
||||
bodeSim(simData2);
|
||||
end
|
||||
|
||||
|
||||
function simData=ExecSim(mot,SIM)
|
||||
global pb;
|
||||
% mot mdl param
|
||||
simData=struct;
|
||||
for i =1:size(SIM,1)
|
||||
[mdl,param]=feval(@(x) x{:}, num2cell(SIM(i,:)));
|
||||
pb=DeltaTauParam(mot,mdl,param);
|
||||
sim('DeltaTauSim');
|
||||
simData(i).mot_mdl_param=SIM(i,:);
|
||||
simData(i).pb=pb;
|
||||
simData(i).desPos_actPos=desPos_actPos;
|
||||
|
||||
fig=bodeSamples(desPos_actPos);
|
||||
title(fig.Children(2),pb.desc);
|
||||
end
|
||||
end
|
||||
|
||||
function bodeSim(simData)
|
||||
fig=figure();
|
||||
ax1=subplot(2,1,1); hold(ax1,'on')
|
||||
ax2=subplot(2,1,2); hold(ax2,'on')
|
||||
Ts=1/5000; % sampling period
|
||||
|
||||
for i =1:length(simData)
|
||||
sd=simData(i).desPos_actPos;
|
||||
t=0:Ts:sd.Time(end);
|
||||
posI=interp1(sd.Time,sd.Data(:,1),t);
|
||||
posO=interp1(sd.Time,sd.Data(:,2),t);
|
||||
ftI=fft(posI);
|
||||
ftO=fft(posO);
|
||||
tf=ftO./ftI;
|
||||
|
||||
L=length(t);
|
||||
k=(L-1)*Ts; % fmax =1/Ts at sample (L-1)/2 (index0-base)
|
||||
N=[10 220]*k; % number of relevant indexes (index0-base)
|
||||
frq=(N(1):N(2))/k;
|
||||
N=N+1;%index0-base -> index1-base
|
||||
tfn=tf(N(1):N(2));
|
||||
|
||||
%fig=figure(); h=plot(abs(ftI));
|
||||
%fig=figure(); h=plot(abs(ftO));
|
||||
%fig=figure(); h=plot(abs(ftI(N(1):N(2))));
|
||||
h=plot(ax1,frq,abs(tfn), 'DisplayName',simData(i).pb.desc);
|
||||
p=unwrap(phase(tfn))/(2*pi)*360;
|
||||
h=plot(ax2,frq,p, 'DisplayName',simData(i).pb.desc);
|
||||
end
|
||||
grid(ax1,'on');grid(ax2,'on');
|
||||
set(ax1, 'XScale', 'log');
|
||||
set(ax2, 'XScale', 'log');
|
||||
legend('Location','best');
|
||||
end
|
||||
@@ -1,4 +1,4 @@
|
||||
function [pb]=DeltaTauParam(mot)
|
||||
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
|
||||
@@ -10,45 +10,70 @@ function [pb]=DeltaTauParam(mot)
|
||||
'Ts', 2E-4, ... % 0.2ms=5kHz
|
||||
'MaxDac' ,2011.968, ...
|
||||
'MaxPosErr', 10000);
|
||||
%pb.ss_plt=mot.ss_plt; pb.sel={3,[3]};
|
||||
%pb.ss_plt=mot.ss_plt0; pb.sel={3,[3]};
|
||||
%pb.ss_plt=mot.ss_c1; pb.sel={3,[3]};
|
||||
%pb.ss_plt=mot.ss_d1; pb.sel={3,[3]};
|
||||
%pb.ss_plt=mot.ss_1; pb.sel={2,[2]};
|
||||
%pb.ss_plt=mot.ss_p; pb.sel={2,[2]};
|
||||
%pb.ss_plt=mot.ss_q; pb.sel={2,[2]};
|
||||
%pb.ss_plt=mot.ss_cq; pb.sel={3,[3]};
|
||||
%pb.ss_plt=mot.ss_cqr; pb.sel={3,[3]};
|
||||
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]};
|
||||
end
|
||||
pb.A=[1];pb.B=[1];pb.C=[1];pb.D=[1];pb.E=[1];pb.F=[1];
|
||||
|
||||
desc=desc+" param:";
|
||||
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')
|
||||
%pb.Kp=25;pb.Kvfb=400;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=5000;pb.MaxInt=1000;
|
||||
pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000;
|
||||
switch param
|
||||
case 0 %scratch
|
||||
desc=desc+"scratch";
|
||||
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+"2";
|
||||
pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000;
|
||||
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];
|
||||
pb.A=[1];
|
||||
pb.B=[1];
|
||||
pb.C=[1];
|
||||
pb.D=[1];
|
||||
pb.E=[1];
|
||||
pb.F=[1];
|
||||
%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')
|
||||
%pb.Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000;
|
||||
pb.Kp=22;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=240;pb.Kaff=3500;pb.MaxInt=1000;
|
||||
switch param
|
||||
case 0 %scratch
|
||||
desc=desc+"scratch";
|
||||
pb.Kp=22;pb.Kvfb=450;pb.Ki=0.02;pb.Kvff=450;pb.Kaff=4517;pb.MaxInt=1000;
|
||||
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+"2";
|
||||
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;
|
||||
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)
|
||||
|
||||
Binary file not shown.
@@ -39,6 +39,63 @@ iqVolts-->iqMeas
|
||||
iqMeas--->actPos
|
||||
```
|
||||
|
||||
Optimizing params
|
||||
-----------------
|
||||
```
|
||||
%Kaff = 1/(Ts*Ts*K) = 1/((11.84*2*np.pi)**2/5000**2) = 4517.278506241803
|
||||
%Kfff=100
|
||||
|
||||
|
||||
pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000;
|
||||
%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
|
||||
|
||||
|
||||
/opt/gfa/python-2.7/2018.12/bin/python -c 'import wx'
|
||||
OTHER VERSION DO NOT YET HAVE WX
|
||||
|
||||
|
||||
|
||||
********** ORIG **************
|
||||
Motor[1].Servo.Kp=25
|
||||
Motor[1].Servo.Kvfb=400
|
||||
Motor[1].Servo.Ki=0.02
|
||||
Motor[1].Servo.Kvff=350
|
||||
Motor[1].Servo.Kaff=5000
|
||||
Motor[1].Servo.MaxInt=1000
|
||||
Motor[1].Servo.Kfff=0
|
||||
|
||||
Motor[2].Servo.Kp=22
|
||||
Motor[2].Servo.Kvfb=350
|
||||
Motor[2].Servo.Ki=0.02
|
||||
Motor[2].Servo.Kvff=240
|
||||
Motor[2].Servo.Kaff=1500
|
||||
Motor[2].Servo.MaxInt=1000
|
||||
Motor[2].Servo.Kfff=0
|
||||
|
||||
|
||||
|
||||
|
||||
********** OPTIMIZED **************
|
||||
Motor[1].Servo.Kp=25
|
||||
Motor[1].Servo.Kvfb=350
|
||||
Motor[1].Servo.Ki=0.02
|
||||
Motor[1].Servo.Kvff=350
|
||||
Motor[1].Servo.Kaff=1615. // 1/(1.548e04*(pb.Ts^2))
|
||||
Motor[1].Servo.MaxInt=1000
|
||||
Motor[1].Servo.Kfff=100
|
||||
|
||||
Motor[2].Servo.Kp=22
|
||||
Motor[2].Servo.Kvfb=450
|
||||
Motor[2].Servo.Ki=0.02
|
||||
Motor[2].Servo.Kvff=450
|
||||
Motor[2].Servo.Kaff=4517
|
||||
Motor[2].Servo.MaxInt=1000
|
||||
Motor[2].Servo.Kfff=100
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -270,7 +270,6 @@ function motCell=identifyFxFyStage(mode)
|
||||
mot.ss_q.Name='simplified mechanics, no current loop, no resonance';
|
||||
chkCtrlObsv(mot.ss_q,'ss_q fyStage');
|
||||
|
||||
% TESTS
|
||||
% u +-----------+ y
|
||||
%iqCmd------->|1 1|-------> iqMeas
|
||||
% | 2|-------> actVel
|
||||
@@ -281,7 +280,6 @@ function motCell=identifyFxFyStage(mode)
|
||||
mot.ss_cq.Name='simplified mechanics, current loop, no resonance';
|
||||
tfq.InputName{1}=s;%restore
|
||||
|
||||
% TESTS
|
||||
% u +-----------+ y
|
||||
%iqCmd------->|1 1|-------> actVel
|
||||
% | 2|-------> actPos
|
||||
@@ -386,6 +384,26 @@ function motCell=identifyFxFyStage(mode)
|
||||
% +-----------+
|
||||
mot.ss_q=ss(tfq);
|
||||
chkCtrlObsv(mot.ss_q,'ss_q fxStage');
|
||||
|
||||
% u +-----------+ y
|
||||
%iqCmd------->|1 1|-------> iqMeas
|
||||
% | 2|-------> actVel
|
||||
% | 3|-------> actPos
|
||||
% +-----------+
|
||||
s=tfq.InputName{1};tfq.InputName{1}='iqMeas';
|
||||
mot.ss_cq=connect(tfc,tfq,'iqCmd',{'iqMeas','actVel','actPos'});
|
||||
mot.ss_cq.Name='simplified mechanics, current loop, no resonance';
|
||||
tfq.InputName{1}=s;%restore
|
||||
|
||||
% u +-----------+ y
|
||||
%iqCmd------->|1 1|-------> actVel
|
||||
% | 2|-------> actPos
|
||||
% +-----------+
|
||||
s=tf1.InputName{1};tf1.InputName{1}='iqCmd';
|
||||
mot.ss_qr=connect(tfq,tf1,tf2,tf3,tf4,'iqCmd',{'actVel','actPos'});
|
||||
mot.ss_qr.Name='simplified mechanics, no current loop, resonance';
|
||||
tfp.InputName{1}=s;%restore
|
||||
|
||||
|
||||
plotBode(mot)
|
||||
end
|
||||
|
||||
@@ -1217,7 +1217,7 @@ if __name__=='__main__':
|
||||
gpascii = comm.gpascii
|
||||
|
||||
# direct start
|
||||
#hs=HelicalScan(comm, gather, args.verbose,sync_mode=0)
|
||||
#hs=HelicalScan(comm, gather, args.verbose,sync_mode=0,pt2pt_time=20)
|
||||
|
||||
#simulated start and frame trigger no sync
|
||||
#hs=HelicalScan(comm, gather, args.verbose,sync_mode=1,sync_flag=3)
|
||||
|
||||
@@ -812,7 +812,7 @@ if __name__=='__main__':
|
||||
#sp = ShapePath(comm, gather, args.verbose)
|
||||
|
||||
# direct start
|
||||
#sp = ShapePath(comm, gather, args.verbose,sync_mode=0)
|
||||
#sp = ShapePath(comm, gather, args.verbose,sync_mode=0,pt2pt_time=10)
|
||||
|
||||
#simulated start and frame trigger no sync
|
||||
#sp = ShapePath(comm, gather, args.verbose,sync_mode=1,sync_flag=3)
|
||||
@@ -842,7 +842,6 @@ if __name__=='__main__':
|
||||
# 10ms 3 6860
|
||||
# 10ms 4 9180
|
||||
|
||||
#xy=False
|
||||
#sp.gen_grid_points(w=6,h=6,pitch=100,rnd=0,ofs=(0,0));sp.sort_points(False);
|
||||
#sp.gen_grid_points(w=100,h=100,pitch=10,rnd=.2)
|
||||
#sp.gen_swissfel_points(width=1000,ofs=(-500,0));sp.sort_points(xy=xy)
|
||||
@@ -851,13 +850,13 @@ if __name__=='__main__':
|
||||
#sp.gen_closed_shifted()
|
||||
#sp.gen_swissmx_points(width=1000,ofs=(-500,0))
|
||||
#sp.gen_swissfel_points(width=1000,ofs=(-500,0))
|
||||
#sp.gen_rand_points(n=14, scale=1000);sp.sort_points(xy=xy)
|
||||
sp.gen_rand_points(n=200, scale=1000);sp.sort_points(xy=False)
|
||||
#sp.gen_swissmx_points(width=1000, ofs=(-500, 0));
|
||||
#sp.gen_spiral_points(rStart=100,rInc=10,numSeg=4,numCir=60, ofs=(0, 0))
|
||||
#sp.gen_spiral_points(rStart=100,rInc=130,numSeg=4,numCir=2, ofs=(0, 0))
|
||||
#sp.gen_grid_points(w=10,h=10,pitch=100,rnd=0,ofs=(0,0));sp.sort_points(False);
|
||||
#sp.gen_grid_points(w=1,h=10,pitch=100,rnd=0,ofs=(0,0))
|
||||
sp.gen_spiral_points(rStart=100,rInc=20,numSeg=8,numCir=32, ofs=(0, 0))
|
||||
#sp.gen_spiral_points(rStart=100,rInc=20,numSeg=8,numCir=32, ofs=(0, 0))
|
||||
#sp.gen_closed_shifted()
|
||||
|
||||
sp.setup_gather(acq_per=1)
|
||||
|
||||
Reference in New Issue
Block a user