towards stage model

python/PBTuning.py

@@ -46,6 +46,7 @@ import matplotlib.pyplot as plt
 import subprocess as sprc
 import telnetlib
 from scipy.signal.waveforms import chirp
+from scipy import signal
 from utilities import *
 
 class PBTuning:
@@ -287,18 +288,22 @@ class PBTuning:
       #np.savez_compressed(file, bode=bode, meta=meta)
 
     def bode_full_plot(self, mot,base):
-      fn='sine_ol_mot%d_ext.npz'%mot
+      import glob
+      fn='sine_ol_mot%d.npz'%mot
       file=os.path.join(base,fn)
       f=np.load(file)
       bode=f['bode']
       meta=f['meta'].item()
       frq=bode[:,0]
       mag=bode[:,1]
-      phase=bode[:,2]
+      phase=np.unwrap(bode[:,2])
 
-      for fn in ('chirp_ol_mot%d_exta.npz','chirp_ol_mot%d_extb.npz','chirp_ol_mot%d_extc.npz'):
+      l=[0,len(frq)]
-        fn=fn%mot
+      #for fn in ('chirp_ol_mot%da.npz','chirp_ol_mot%db.npz','chirp_ol_mot%dc.npz','chirp_ol_mot%dd.npz'):
-        file=os.path.join(base,fn)
+      #  fn=fn%mot
+      #  file=os.path.join(base,fn)
+      for file in sorted(glob.glob(os.path.join(base,'chirp_ol_mot%d*'%mot))):
+        print(os.path.basename(file))
         f=np.load(file)
         data=f['data']
         meta=f['meta'].item()
@@ -326,25 +331,156 @@ class PBTuning:
 
         ft=ftY/ftX
         frq=np.concatenate((frq,f))
-        phase=np.concatenate((phase,np.angle(ft)))
+        phase_=np.unwrap(np.angle(ft))
-        #if amp==50:amp*=np.sqrt(2)
+        if phase_[0]>0:
+          phase_-=2*np.pi
+        phase=np.concatenate((phase,phase_))
         mag=np.concatenate((mag,(np.abs(ftY)/np.abs(ftX))))
+        l.append(len(frq))
+
 
       db_mag=20*np.log10(mag)
-      phase=np.degrees(np.unwrap(phase))# numpy.unwrap(p, discont=3.141592653589793, axis=-1)
+      phase=np.degrees(phase)# numpy.unwrap(p, discont=3.141592653589793, axis=-1)
+
       fig = plt.figure()
       fig.canvas.set_window_title('full bode of motor %d'%mot)
       ax = fig.add_subplot(2, 1, 1)
-      ax.semilogx(frq, db_mag,'-')  # Bode magnitude plot
+      plt.title('bode of motor %d'%mot)
-      ax.xaxis.set_label_text('dB Mag.')
+      for i in range(len(l)-1):
+        ax.semilogx(frq[l[i]:l[i+1]], db_mag[l[i]:l[i+1]],'-')  # Bode magnitude plot
+      ax.yaxis.set_label_text('dB ampl')
+      ax.set_xlim(1,2000)
       plt.grid(True)
       #ax.loglog(frqLst, bode[:,0],'.-')  # Bode magnitude plot
       ax = fig.add_subplot(2, 1, 2)
-      ax.semilogx(frq, phase,'-')  # Bode phase plot
+      for i in range(len(l)-1):
-      ax.xaxis.set_label_text('phase')
+        ax.semilogx(frq[l[i]:l[i+1]], phase[l[i]:l[i+1]],'-')#,zorder=i)  # Bode phase plot
+      ax.yaxis.set_label_text('phase')
+      ax.xaxis.set_label_text('frequency [Hz]')
+      ax.set_xlim(1,2000)
       ax.set_ylim(-360,0)
       plt.grid(True)
 
+    def bode_model_plot(self, mot,base):
+      self.bode_full_plot(mot,base)
+      fig=plt.gcf()
+      if mot==1:
+        db_mag1=17.3 #dB
+        mag1=10**(db_mag1/20)
+        f1=6.5 #Hz
+        w1=f1*2*np.pi #rad/sec
+        T1=1/w1
+        d1=.7 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
+        num1=np.poly1d([mag1])
+        den1 = np.poly1d([T1**2,2*T1*d1,1])
+
+        #first resonance frequency
+        f2=np.array([191,198])
+        d2=np.array([.05,.05])#daempfung
+        w2=f2*2*np.pi #rad/sec
+        T2=1/w2
+        num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1])
+        den2 = np.poly1d([T2[1]**2,2*T2[1]*d2[1],1])
+        mdl= signal.lti(num2, den2) #num denum
+        #bode(mdl)
+
+
+        #current loop 2nd order approx
+        f4=900.
+        d4=1 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
+        w4=f4*2*np.pi #rad/sec
+        T4=1/w4
+        num4 = np.poly1d([1.])
+        den4 = np.poly1d([T4**2,2*T4*d4,1])
+        #mdl= signal.lti(num4, den4) #num denum
+        #bode(mdl)
+
+        num=num1*num2*num4#*num3
+        den=den1*den2*den4#*den3
+        mdl= signal.lti(num, den) #num denum
+        print num,den
+        print mdl
+      elif mot==2:
+        # basic 1/s^2 system with damping an d resonance
+        db_mag1=17.3 #dB
+        mag1=10**(db_mag1/20)
+        f1=4.5 #Hz
+        w1=f1*2*np.pi #rad/sec
+        d1=.3 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
+        T1=1/w1
+        num1 = np.poly1d([mag1])
+        den1 = np.poly1d([T1**2,2*T1*d1,1])
+
+        #first resonance frequency
+        f2=np.array([57.8,63.8])
+        d2=np.array([.06,.1])#daempfung
+        w2=f2*2*np.pi #rad/sec
+        T2=1/w2
+        num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1])
+        den2 = np.poly1d([T2[1]**2,2*T2[1]*d2[1],1])
+        mdl= signal.lti(num2, den2) #num denum
+        #bode(mdl)
+
+        #second resonance frequency
+        f3=np.array([130,141])
+        d3=np.array([.05,.07])#daempfung
+        w3=f3*2*np.pi #rad/sec
+        T3=1/w3
+        num3 = np.poly1d([T3[0]**2,2*T3[0]*d3[0],1])
+        den3 = np.poly1d([T3[1]**2,2*T3[1]*d3[1],1])
+        #mdl= signal.lti(num3, den3) #num denum
+        #bode(mdl)
+
+        #second resonance frequency
+        f4=np.array([405,415])
+        d4=np.array([.02,.02])#daempfung
+        w4=f4*2*np.pi #rad/sec
+        T4=1/w4
+        num4 = np.poly1d([T4[0]**2,2*T4[0]*d4[0],1])
+        den4 = np.poly1d([T4[1]**2,2*T4[1]*d4[1],1])
+        #mdl= signal.lti(num3, den3) #num denum
+        #bode(mdl)
+
+
+
+        #current loop 2nd order approx
+        fc=900.
+        dc=1 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
+        wc=fc*2*np.pi #rad/sec
+        Tc=1/wc
+        numc = np.poly1d([1.])
+        denc = np.poly1d([Tc**2,2*Tc*dc,1])
+        #mdl= signal.lti(num4, den4) #num denum
+        #bode(mdl)
+
+        num=num1*num2*num3*num4*numc
+        den=den1*den2*den3*den4*denc
+        mdl= signal.lti(num, den) #num denum
+      bode(mdl)
+      w=np.logspace(0,3,1000)*2*np.pi
+      w,mag,phase = signal.bode(mdl,w)
+      f=w/(2*np.pi)
+      ax=fig.axes[0]
+      ax.semilogx(f, mag,'-k',lw=2)    # Bode magnitude plot
+      ax=fig.axes[1]
+      ax.semilogx(f, phase,'-k',lw=2)  # Bode phase plot
+      # tp print see also: print(np.poly1d([1,2,3], variable='s')), print(np.poly1d([1,2,3], r=True, variable='s'))
+
+def bode(mdl):
+  w,mag,phase = signal.bode(mdl)
+  f=w/(2*np.pi)
+  fig = plt.figure()
+  ax = fig.add_subplot(2, 1, 1)
+  ax.semilogx(f,mag,'-')  # Bode magnitude plot
+  ax.yaxis.set_label_text('dB ampl')
+  plt.grid(True)
+  ax = fig.add_subplot(2, 1, 2)
+  ax.semilogx(f,phase,'-')  # Bode magnitude plot
+  ax.yaxis.set_label_text('phase')
+  ax.xaxis.set_label_text('frequency [Hz]')
+  plt.grid(True)
+  #plt.show()
+
 
 if __name__=='__main__':
   from argparse import ArgumentParser,RawDescriptionHelpFormatter
@@ -366,7 +502,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
     parser.add_argument('--dryrun', '-n', action='store_true', help='dryrun to stdout')
     parser.add_argument('--mode', '-m', type=int, help='modes (see below)', default=1)
     parser.add_argument('--mot', type=int, help='motor', default=1)
-    parser.add_argument('--tag', help='tag', default='')
+    parser.add_argument('--dir', help='dir', default='')
 
     args=parser.parse_args()
 
@@ -396,6 +532,11 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
     #plt.show()
     #return
     base='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/PBTuning/'
+    if args.dir is not None:
+      base=os.path.join(base,args.dir)
+      if not os.path.exists(base):
+        os.mkdir(base)
+
     #file=os.path.join(base, 'sine_ol_mot_tst.npz')
     #tune.bode_sine(openloop=True,file=file,motor=2, minFrq=20,maxFrq=200,numFrq=30)
     #file=os.path.join(base, 'chirp_ol_mot_tst.npz')
@@ -405,11 +546,16 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
 
     if args.plot:
       #display bode plots
-      if args.plot[0]=='ext':
+      if args.plot[0]=='full':
         tune.bode_full_plot(mot=1,base=base)
         tune.bode_full_plot(mot=2,base=base)
         plt.show()
         return
+      if args.plot[0]=='model':
+        tune.bode_model_plot(mot=1,base=base)
+        tune.bode_model_plot(mot=2,base=base)
+        plt.show()
+        return
       for fn in args.plot:
         if os.path.basename(fn).startswith('sine_ol_mot'):
           tune.bode_sine(openloop=True,file=fn)
@@ -423,27 +569,32 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
       return
 
     mode=args.mode
-    tag=args.tag
     mot=args.mot
     if mode==1:
       ol=True
-      file=os.path.join(base,'sine_ol_mot%d_%s.npz'%(mot,tag))
+      file=os.path.join(base,'sine_ol_mot%d.npz'%(mot))
       #def bode_sine(self, openloop=True, motor=1, minFrq=1, maxFrq=300, numFrq=150, amp=10, file='/tmp/bode.npz'):
       tune.bode_sine(openloop=True,file=file,motor=mot)
     elif mode==2:
-      file=os.path.join(base,'chirp_ol_mot%d_%s.npz'%(mot,tag))
+      file=os.path.join(base,'chirp_ol_mot%d.npz'%(mot))
       #def bode_chirp(self,openloop=True,motor=1,minFrq=10,maxFrq=150,tSec=30.,amp=10,mode=1,file='/tmp/gather.npz'):
       #tune.bode_chirp(openloop=True,file=file,motor=mot, minFrq=10, maxFrq=300, tSec=30)
       tune.bode_chirp(openloop=True,file=file[:-4]+'a.npz',motor=mot,       minFrq=10, maxFrq=300, tSec=30)
       tune.bode_chirp(openloop=True,file=file[:-4]+'b.npz',motor=mot,amp=50,minFrq=100,maxFrq=500, tSec=30)
       tune.bode_chirp(openloop=True,file=file[:-4]+'c.npz',motor=mot,amp=50,minFrq=300,maxFrq=1500,tSec=10)
+      tune.bode_chirp(openloop=True,file=file[:-4]+'d.npz',motor=mot,amp=100,minFrq=300,maxFrq=2000,tSec=10)
     elif mode==3:
       ol=True
-      file=os.path.join(base,'sine_cl_mot%d_%s.npz'%(mot,tag))
+      file=os.path.join(base,'sine_cl_mot%d.npz'%(mot))
       tune.bode_sine(openloop=False,file=file,motor=mot)
     elif mode==4:
-      file=os.path.join(base,'chirp_cl_mot%d_%s.npz'%(mot,tag))
+      file=os.path.join(base,'chirp_cl_mot%d.npz'%(mot))
       tune.bode_chirp(openloop=False,file=file,motor=mot)
+    elif mode==100: #testing
+      file=os.path.join(base,'chirp_ol_mot%d.npz'%(mot))
+      #def bode_chirp(self,openloop=True,motor=1,minFrq=10,maxFrq=150,tSec=30.,amp=10,mode=1,file='/tmp/gather.npz'):
+      #tune.bode_chirp(openloop=True,file=file,motor=mot, minFrq=10, maxFrq=300, tSec=30)
+      tune.bode_chirp(openloop=True,file=file[:-4]+'TEST.npz',motor=mot,amp=100,minFrq=300,maxFrq=2000,tSec=10)
     #tune.do_command('stepmove',1,100,500,0,0)
     plt.show()
   #------------------ Main Code ----------------------------------
@@ -452,12 +603,12 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
   exit(ret)
 
 #enable plc1
-#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 1 --tag ext
+#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 1 --dir tmp
 #enable plc1
-#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 1 --tag ext
+#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 1 --tag tmp
 #enable plc1
-#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 2 --tag ext
+#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 2 --tag tmp
 #enable plc1
-#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 2 --tag ext
+#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 2 --tag tmp
 #enable plc1
 #./PBTuning.py --host SAR-CPPM-EXPMX1 --plot ext