wip

2018-10-08 17:30:26 +02:00
parent be82fa20e0
commit 5543582557
6 changed files with 136 additions and 126 deletions
--- a/matlab/identifyFxFyStage.m
+++ b/matlab/identifyFxFyStage.m
@@ -22,6 +22,10 @@ function [mot1,mot2]=identifyFxFyStage()
    obj.phase=f.deg_phase*pi/180;  %phase in rad
    response = obj.mag.*exp(1j*obj.phase);
    obj.meas= idfrd(response,obj.w,0);
+
+    fMdl=load(strcat(path,sprintf('model%d.mat',motid)));
+    obj.mdl=fMdl;
+    
  end

  function tfc=currstep(obj)
@@ -36,7 +40,11 @@ function [mot1,mot2]=identifyFxFyStage()
    [y,t]=step(tfc,t);
    f=figure();
    subplot(1,2,1);
-    plot(t,y*1000,'r',t,obj.currstep.OutputData(11:210),'b');
+    plot(t*1000,obj.currstep.OutputData(11:210),'b',t*1000,y*1000,'r');
+    xlabel('ms')
+    ylabel('curr\_bits')
+    grid on
+    legend('real signal','model','Location','southeast')
    title(s);
    subplot(1,2,2);
    h=bodeplot(tfc,'r');
@@ -52,26 +60,6 @@ function [mot1,mot2]=identifyFxFyStage()
    s=sprintf('k:%g w0:%g damp:%g\n',k,w0,damp);
  end

-  function tf=fyModel()
-    num=[ 2.36527033e+11,   1.17108082e+13,   3.62387303e+17];
-    den=[ 1.00000000e+00,   6.64495206e+03,   2.12777376e+07, ...
-          1.23728427e+10,   3.07054470e+13,   1.72592127e+15, ...
-          5.39888656e+17];
-
-    tf=idtf(num,den);
-  end
-
-  function tf=fxModel()
-    num=[ 1.23284092e+11,   4.14791803e+13,   1.18702926e+18, ...
-          2.96296718e+20,   2.67179357e+24,   4.04662786e+26, ...
-          1.59131515e+30,   1.02778572e+32,   1.64551888e+35];
-    den=[ 1.00000000e+00,   6.93892369e+03,   3.17041055e+07, ...
-          7.66104262e+10,   2.36504992e+14,   2.23054854e+17, ...
-          5.12578678e+20,   2.04416512e+23,   3.27771400e+26, ...
-          4.77145416e+28,   3.85452959e+31,   1.28911178e+33, ...
-          9.52157664e+34];
-     tf=idtf(num,den);
-  end

  function mot=fyStage()
    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',1);
@@ -84,12 +72,26 @@ function [mot1,mot2]=identifyFxFyStage()

    figure(); 
    mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0));
-    mot.tf_mdl= fyModel();
+    mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
+
+    g11=tf(mot.mdl.numc,mot.mdl.denc);  % iqCmd->iqMeas  
+    g12=tf([1 0],mot.mdl.denc*12); % iqCmd->iqVolts  : iqVolts= i_meas*R+i_meas'*L
+    num=conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.numc);
+    den=conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.denc);
+    g13=tf(num,den); %iqCmd->ActPos
+    %sys=ss([g11;g12])
+    sys=ss([g11;g12;g13])
+    
+    
    %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g');
    h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
    setoptions(h,'FreqUnits','Hz','Grid','on');
    
  end
+  function y=myNorm(y)
+    %normalizes num and den by factor 1000
+    %y.*10.^(3*(length(y):-1:1))    
+  end

  function mot=fxStage()
    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',2);
@@ -103,7 +105,65 @@ function [mot1,mot2]=identifyFxFyStage()
    figure();
    mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0));
    mot.tf13_9 = tfest(mot.meas, 13, 9, opt);
-    mot.tf_mdl = fxModel();
+    mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
+
+    %create ss from tf MIMO:
+    %https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system
+    %Gspm=[tf_iqCmd_actVolts;tf_iqCmd_iqMeas;tf_iqCmd_actPos];
+    %sys=ss(Gspm);
+    
+    %normalize: [1E6 1E3 1].*mot.mdl.denc
+    numc=myNorm(mot.mdl.numc);
+    denc=myNorm(mot.mdl.denc);
+    num1=myNorm(mot.mdl.num1);
+    den1=myNorm(mot.mdl.den1);
+    num2=myNorm(mot.mdl.num2);
+    den2=myNorm(mot.mdl.den2);
+    num3=myNorm(mot.mdl.num3);
+    den3=myNorm(mot.mdl.den3);
+    num4=myNorm(mot.mdl.num4);
+    den4=myNorm(mot.mdl.den4);
+    num5=myNorm(mot.mdl.num5);
+    den5=myNorm(mot.mdl.den5);
+    num=myNorm(mot.mdl.num);
+    den=myNorm(mot.mdl.den);
+
+    %http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600
+    %tf2ss MIMO
+    %https://ch.mathworks.com/help/control/ref/append.html
+    g1=tf(numc,denc);  % iqCmd->iqMeas  
+    s1=ss(g1);
+    s1.C=[s1.C; 1/s1.B(1) 0];   % add output iqVolts: iqVolts= i_meas*R+i_meas'*L
+    tf(s1) % display all transfer functions
+    
+    num=conv(conv(conv(conv(num1,num2),num3),num4),num5);
+    den=conv(conv(conv(conv(den1,den2),den3),den4),den5);  
+    g2=tf(num,den); %iqMeas->ActPos
+    s2=ss(g2);
+
+    s3=append(s1,s2);
+    tf(s3)
+    %connect iqMeas from s1 to iqMeas of s2
+    s3.A(3,1)=1 %WHAT NUMBER ??? s3.B(3,2), s3.C2,:)?
+    %remove the direct iqMeas input
+    %s3.B(3,2)=0
+    
+    t_=tf(s3) 
+    t_(3,1)
+    figure;
+    bode(t_(3,1))
+    %compare with tf iqCmd->ActPos
+    num=conv(conv(conv(conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.num3),mot.mdl.num4),mot.mdl.num5),mot.mdl.numc);
+    den=conv(conv(conv(conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.den3),mot.mdl.den4),mot.mdl.den5),mot.mdl.denc);
+    g4=tf(num,den); %iqCmd->ActPos
+    figure;
+    bode(g4)
+    
+    %sys=ss([g11;g12])
+    sys=ss([g11;g12;g13])
+    sys=ss(g13)
+
+    
    %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g',mot.tf13_9,'m',mot.tf_py,'b');
    h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
    setoptions(h,'FreqUnits','Hz','Grid','on');
@@ -115,106 +175,3 @@ function [mot1,mot2]=identifyFxFyStage()
  mot2=fxStage();

 end
-
-
-function f=SCRATCH()
-  [m1,m2]=identifyFxFyStage();
-  controlSystemDesigner(1,m2.tf_py);    % <<<<<<<<< This opens a transferfiûnction that can be edited
-
-  %identification toolbox
-  systemIdentification
-
-  
-  
-  
-  
-  %opt=tfestOptions('Display','off');
-  %opt=tfestOptions('Display','on','initializeMethod','svf');
-  %opt=tfestOptions('Display','on','initializeMethod','iv','WeightingFilter',[]);
-  %opt=tfestOptions('Display','on','initializeMethod','iv','WeightingFilter',[1,5;20,570]);
-  %tf1 = tfest(mot1frq, 6, 4, opt);
-  % Model refinement
-  % Options = tf1.Report.OptionsUsed;
-  % Options.WeightingFilter = 'prediction';
-  % tf1_1 = pem(mot1frq, tf1, Options)
-
-
-
-
-
-  bodeplot(mot1frq,tf1)
-  mag,phase=bode(tf1,frq)
-  figure(1)
-  subplot(211)
-
-
-  bodeplot(tf1)
-
-  Opt = n4sidOptions('N4Horizon',[15 15 15]);
-  n4s3 = n4sid(mot1frq, 3, Opt)
-
-
-
-
-
-  %tf([1 2],[1 0 10])
-  %specifies the transfer function (s+2)/(s^2+10) while
-  sys=tf([1],[1,0,0])
-  bode(sys)
-  step(sys)
-
-  sys=tf([1],[1,-1,2]) %instable
-
-  sys=tf([1],[1,1,2]) %stable
-
-
-  %0dB at 12 Hz=12*2*pi rad/s =75.4=k^2 -> k=8.6833
-
-  sys=tf([10],[1,0,0])
-  %1/s^2 -> 0dB at 1Hz -40dB/decade
-  %10=+20dB
-
-
-  sys=tf([1],[1,0,2]) %not damped constant sine after step
-
-
-  sys=zpk([],[1,0,0],100) %stable
-
-  sys=zpk([],[-10,-10],100)
-
-
-  %parker stage 1 
-  %!encoder_sim(enc=1,tbl=9,mot=9,posSf=13000./2048)
-  %!encoder_inc(enc=1,tbl=1,mot=1,posSf=13000./650000)
-  %!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')
-  Ts=2E-4 % discrete sample time (servo period)
-  Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000
-  Kp=25,Kvfb=0,Ki=0,Kvff=0,Kaff=0,MaxInt=0
-
-
-  num=7.32
-  den=[5.995e-04 4.897e-02   1.]
-  open('stage_closed_loop.slx')
-  %sim('stage_closed_loop.slx')
-
-  sys=tf(num,den)
-  bode(sys)
-  G = tf(1.5,[1 14 40.02]);
-  controlSystemDesigner('bode',sys);
-  controlSystemDesigner
-  linearSystemAnalyzer
-  load ltiexamples
-  linearSystemAnalyzer(sys_dc)
-
-  controlSystemDesigner('bode',sys);
-  controlSystemDesigner(1,sys);    % <<<<<<<<< This opens a transferfiûnction that can be edited
-
-  num=[8.32795069e-11, 1.04317228e-08, 6.68431323e-05, 3.31861324e-03, 7.32824533e+00];
-  den=[5.26156641e-18, 1.12897840e-14, 7.67853031e-12, 1.03201301e-08, 2.05154780e-06, 1.34279894e-03, 7.19229912e-02, 1.00000000e+00];
-  mot2=tf(num,den);
-  controlSystemDesigner('bode',mot2);
-  
-  
-
-end
--- a/matlab/libESB_MX.slx
+++ b/matlab/libESB_MX.slx
--- a/matlab/simFxFyStage.m
+++ b/matlab/simFxFyStage.m
@@ -0,0 +1,39 @@
+function out=simFxFyStage()
+  global Kp Kvfb Ki Kvff Kaff MaxInt
+  global m1 m2 mot_num mot_den Ts MaxDac MaxPosErr
+  global A B C D
+    function ServoDeltaTau_z(motid)
+    Ts=2E-4; % 0.2ms=5kHz
+    MaxDac=2011.968;
+    MaxPosErr=10000;
+    if motid==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')
+      Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000;       
+      mot_num=m1.tf_mdl.Numerator;
+      mot_den=m1.tf_mdl.Denominator;
+    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')
+      Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000;
+      mot_num=m2.tf_mdl.Numerator;
+      mot_den=m2.tf_mdl.Denominator;
+    end
+  end
+  mdlName='stage_closed_loop';
+  open(mdlName)
+  ServoDeltaTau_z(2)
+  [A,B,C,D]=tf2ss(mot_num,mot_den);
+  
+  %mdlWks=get_param(mdlName,'ModelWorkspace')
+  %whos global
+  %whos(mdlWks)
+  %assignin(mdlWks,'Ts',1234)
+  %getVariable(mdlWks,'Ts')
+  % in global space call:
+  %global Kp Kvfb Ki Kvff Kaff MaxInt
+  %global m1 m2 mot_num mot_den Ts MaxDac MaxPosErr
+  %[m1,m2]=identifyFxFyStage();
+ 
+  %
+end
--- a/matlab/stage_closed_loop.slx
+++ b/matlab/stage_closed_loop.slx
--- a/python/MXTuning.py
+++ b/python/MXTuning.py
@@ -104,6 +104,13 @@ class MXTuning(Tuning):
        print num
        print den
        print mdl
+        d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'numc':numc.coeffs,
+           'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'denc':denc.coeffs}
+        fn=os.path.join(base,'model%d.mat'%mot)
+        import scipy.io
+        scipy.io.savemat(fn, mdict=d)
+        print('save to matlab file:'+fn)
+
      elif mot==2:
        #identify matlab: k:1.7282 w0:51.069 damp:0.327613
        mag1=1.7282 #10**(db_mag1/20)
@@ -171,6 +178,13 @@ class MXTuning(Tuning):
        print num
        print den
        print mdl
+        d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'num3':num3.coeffs,'num4':num4.coeffs,'num5':num5.coeffs,'numc':numc.coeffs,
+           'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'den3':den3.coeffs,'den4':den4.coeffs,'den5':den5.coeffs,'denc':denc.coeffs}
+        fn=os.path.join(base,'model%d.mat'%mot)
+        import scipy.io
+        scipy.io.savemat(fn, mdict=d)
+        print('save to matlab file:'+fn)
+
      bode(mdl)
      w=np.logspace(0,np.log10(2000),1000)*2*np.pi
      w,mag,phase = signal.bode(mdl,w)
--- a/python/MXTuningDoc/fastStageTune.odt
+++ b/python/MXTuningDoc/fastStageTune.odt