diff --git a/matlab/DeltaTauOptimizer.m b/matlab/DeltaTauOptimizer.m new file mode 100644 index 0000000..3af9d1a --- /dev/null +++ b/matlab/DeltaTauOptimizer.m @@ -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 \ No newline at end of file diff --git a/matlab/DeltaTauParam.m b/matlab/DeltaTauParam.m index 719c87a..79e613e 100644 --- a/matlab/DeltaTauParam.m +++ b/matlab/DeltaTauParam.m @@ -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) diff --git a/matlab/DeltaTauSim.slx b/matlab/DeltaTauSim.slx index d8c6e00..e008ace 100644 Binary files a/matlab/DeltaTauSim.slx and b/matlab/DeltaTauSim.slx differ diff --git a/matlab/Readme.md b/matlab/Readme.md index 931f9a0..d7780da 100644 --- a/matlab/Readme.md +++ b/matlab/Readme.md @@ -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 + + + + diff --git a/matlab/identifyFxFyStage.m b/matlab/identifyFxFyStage.m index 9b10473..86f1458 100644 --- a/matlab/identifyFxFyStage.m +++ b/matlab/identifyFxFyStage.m @@ -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 diff --git a/python/helicalscan.py b/python/helicalscan.py index d7f4988..dfc0aac 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -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) diff --git a/python/shapepath.py b/python/shapepath.py index 1f66e4d..9d17bd7 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -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)