adding optimizer tool and found better parameters

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

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]};
+  desc=sprintf('mot:%d mdl:',mot.id);
-  %pb.ss_plt=mot.ss_plt0; pb.sel={3,[3]};
+  switch mdl
-  %pb.ss_plt=mot.ss_c1; pb.sel={3,[3]};
+    case 1
-  %pb.ss_plt=mot.ss_d1; pb.sel={3,[3]};
+      pb.ss_plt=mot.ss_plt;desc=desc+"plt";  pb.sel={3,[3]};
-  %pb.ss_plt=mot.ss_1; pb.sel={2,[2]};
+    case 2
-  %pb.ss_plt=mot.ss_p; pb.sel={2,[2]};
+      pb.ss_plt=mot.ss_plt0;desc=desc+"plt0"; pb.sel={3,[3]};
-  %pb.ss_plt=mot.ss_q; pb.sel={2,[2]};
+    case 3
-  %pb.ss_plt=mot.ss_cq; pb.sel={3,[3]};
+      pb.ss_plt=mot.ss_c1;desc=desc+"c1"; pb.sel={3,[3]};
-  %pb.ss_plt=mot.ss_cqr; 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;
+    switch param
-    pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000;
+      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;
+    switch param
-    pb.Kp=22;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=240;pb.Kaff=3500;pb.MaxInt=1000;
+      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) 

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
+
+
+
+
 
 
 

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

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)

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)