full openloop bode plot

python/PBTuning.py

@@ -16,14 +16,14 @@ other available tuning progs on the powerbrick are:
   currentautotunecalc
   currentstep
   filtercalculation
-  openloopchirp
+  openloopchirp +
-  openloopsine
+  openloopsine +
   openlooptestmove
   othertrajectory
   parabolicmove
   randommove
-  sinesweep
+  sinesweep +
-  sinusoidal
+  sinusoidal +
   stepmove
   user_gantry_crosscoupled.h
   usertrajectory
@@ -45,6 +45,7 @@ import matplotlib as mpl
 import matplotlib.pyplot as plt
 import subprocess as sprc
 import telnetlib
+from scipy.signal.waveforms import chirp
 from utilities import *
 
 class PBTuning:
@@ -181,33 +182,56 @@ class PBTuning:
         meta['file']=file
       else:
         mode=1
+        #Sys.ServoPeriod=0.2ms = 5kHz -> therefore factor 2000 for the time second->servoCnt
+        #don't know why the frequency must be scaled...
         if openloop:
-          data=self.do_command('openloopchirp',motor,amp,minFrq,maxFrq,tSec*1000,mode,0)
+          data=self.do_command('openloopchirp',motor,amp,minFrq/2.,maxFrq/2.,tSec*2000,mode,0)
         else:
-          #sinesweep, chirpmove?
+          data=self.do_command('sinesweep', motor, amp, minFrq/2., maxFrq/2., tSec*2000, mode, 0)
-          data=self.do_command('sinesweep', motor, amp, minFrq, maxFrq, tSec*1000, mode, 0)
           data = data[:, (1, 2)]
 
         meta={'motor':motor,'date':time.asctime(),'minFrq':minFrq,'maxFrq':maxFrq,'tSec':tSec,'amp':amp}
         np.savez_compressed(file, data=data, meta=meta)
         meta['file'] = file
-      self.bode_chirp_plot(data, meta)
+      self.bode_chirp_plot(data, meta,openloop)
 
-    def bode_chirp_plot(self, data, meta):
+    def bode_chirp_plot(self, data, meta,openloop):
       tSec=meta['tSec']
       minFrq=meta['minFrq']
       maxFrq=meta['maxFrq']
 
-      n=1000
+      #n=25000
-      d=np.concatenate((np.ones(n-1)*data[0, 1],data[:, 1]))
+      #t=np.linspace(0, tSec, 2*n+1, endpoint=True)
-      d=np.convolve(d,np.ones(n),'valid')/n
+      #c=chirp(t, f0=minFrq, f1=maxFrq, t1=tSec, phi=-90, method='logarithmic')
-      data[:, 1]-=d
 
-      #5..150 Hz
+      #plt.figure()
-      #15 sec
+      #plt.clf()
-      c = data[:,0]
+      #plt.subplot(2, 1, 1)
-      o=data[:,1]
+      #plt.plot(t, c)
-      o-=o[0]
+      #tstr="Logarithmic Chirp, f(0)=%g, f(%g)=%g"%(minFrq, tSec, maxFrq)
+      #plt.title(tstr)
+
+      #plt.subplot(2, 1, 2)
+      #plt.plot(t, minFrq*(maxFrq/minFrq)**(t/tSec), 'r')
+      ## yscale('log')
+      #plt.grid(True)
+      #plt.ylabel('Frequency (Hz)')
+      #plt.xlabel('time (sec)')
+
+      if openloop:
+        n=1000
+        d=np.concatenate((np.ones(n-1)*data[0, 1],data[:, 1]))
+        d=np.convolve(d,np.ones(n),'valid')/n
+        data[:, 1]-=d
+        # 5..150 Hz
+        # 15 sec
+        c = data[:, 0]
+        o = data[:, 1]
+        o -= o[0]
+      else:
+        data=data-data[0,:]
+        c = data[:,0]
+        o=data[:,1]
       t = np.linspace(0, tSec, data.shape[0], endpoint=True)
       n=data.shape[0]/2
       fig=plt.figure()
@@ -222,30 +246,34 @@ class PBTuning:
       #ts=t1/(2*n)#samplingperiode =t[1]-t[0]
       #w_k=2*pi*k/T
       #w=2*np.pi*np.arange(n+1)/t1*(2*n)
-      i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j])
+      f=np.arange(n+1)/tSec #Hz
-      w=np.arange(n+1)/tSec #Hz
-      w=w[i:j+1]
       #w*=2.*np.pi #rad/sec
       fig=plt.figure()
       fig.canvas.set_window_title(os.path.basename(meta['file']+' bode'))
       ftX=np.fft.rfft(c)
       ftY=np.fft.rfft(o)
+
+      i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j])
+      f=f[i:j+1]
+      ftX=ftX[i:j+1]
+      ftY=ftY[i:j+1]
+
       ft=ftY/ftX
       phase=np.angle(ft)
-      phase=np.degrees(np.unwrap(phase[i:j+1]))
+      phase=np.degrees(np.unwrap(phase))
 
       #magDb=10*np.log10(np.abs(ft)) #in decibel
       mag=np.abs(ftY)/np.abs(ftX)
-      magDb=20*np.log10(mag[i:j+1]) #in decibel (20=10*2: factor 2 because rfft only half)
+      magDb=20*np.log10(mag) #in decibel (20=10*2: factor 2 because rfft only half)
       #magDb=np.abs(ftY)/np.abs(ftX)
       ax=plt.subplot(2,1,1)
-      ax.semilogx(w,magDb,'b')    # Bode magnitude plot
+      ax.semilogx(f,magDb,'b')    # Bode magnitude plot
       #ax.plot(w,magDb)    # Bode magnitude plot
       ax.axvline(minFrq,c='k');ax.axvline(maxFrq,c='k')
       ax.grid(True)
       ax=plt.subplot(2,1,2)
 
-      ax.semilogx(w,phase,'b')  # Bode phase plot
+      ax.semilogx(f,phase,'b')  # Bode phase plot
       ax.yaxis.set_label_text('Amplitude [dB]')
 
       #ax.set_ylim(phase[i],phase[j])
@@ -258,6 +286,64 @@ class PBTuning:
       #meta={'motor':motor,'date':time.asctime()}
       #np.savez_compressed(file, bode=bode, meta=meta)
 
+    def bode_full_plot(self, mot,base):
+      fn='sine_ol_mot%d_ext.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]
+
+      for fn in ('chirp_ol_mot%d_exta.npz','chirp_ol_mot%d_extb.npz','chirp_ol_mot%d_extc.npz'):
+        fn=fn%mot
+        file=os.path.join(base,fn)
+        f=np.load(file)
+        data=f['data']
+        meta=f['meta'].item()
+        tSec=meta['tSec']
+        minFrq=meta['minFrq']
+        maxFrq=meta['maxFrq']
+        amp=meta['amp']
+        n=1000
+        d=np.concatenate((np.ones(n-1)*data[0, 1],data[:, 1]))
+        d=np.convolve(d,np.ones(n),'valid')/n
+        data[:, 1]-=d
+        # 5..150 Hz
+        # 15 sec
+        c = data[:, 0]
+        o = data[:, 1]
+        o -= o[0]
+        n=data.shape[0]/2
+        ftX=np.fft.rfft(c)
+        ftY=np.fft.rfft(o)
+        i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j])
+        f=np.arange(n+1)/tSec #Hz
+        f=f[i:j+1]
+        ftX=ftX[i:j+1]
+        ftY=ftY[i:j+1]
+
+        ft=ftY/ftX
+        frq=np.concatenate((frq,f))
+        phase=np.concatenate((phase,np.angle(ft)))
+        #if amp==50:amp*=np.sqrt(2)
+        mag=np.concatenate((mag,(np.abs(ftY)/np.abs(ftX))))
+
+      db_mag=20*np.log10(mag)
+      phase=np.degrees(np.unwrap(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
+      ax.xaxis.set_label_text('dB Mag.')
+      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
+      ax.xaxis.set_label_text('phase')
+      ax.set_ylim(-360,0)
+      plt.grid(True)
 
 
 if __name__=='__main__':
@@ -286,10 +372,44 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
 
     #plt.ion()
     tune=PBTuning(args)
+    #data=self.do_command('openloopsine',motor,amp,frq,rep,0)
+    # #data=self.do_command('openloopchirp',motor,amp,minFrq,maxFrq,tSec*1000,mode,0)
+
+    #THIS IS A TEST TO DO 5 Hz during 10 seconds
+    #frq=5
+    #frq2=frq*1.1
+    #ts=10 #seconds
+
+    #5 kHz Servo
+    #ds=tune.do_command('openloopsine', 2, 20, frq, frq*ts, 0)
+    #dc=tune.do_command('openloopchirp', 2, 20, frq/2., frq2/2., ts*2000, 1, 0) #THIS IS NOT 10 Seconds!!!
+
+    #dc=tune.do_command('openloopchirp', 2, 10, 5, 6, 10*1000, 1, 0) #THIS IS NOT 10 Seconds!!!
+    #ds=ds-ds[0,:]
+    #dc=dc-dc[0,:]
+    #plt.figure()
+    #plt.plot(ds[:,0],'b')
+    #plt.plot(ds[:,1],'g')
+    #plt.figure()
+    #plt.plot(dc[:,0],'b')
+    #plt.plot(dc[:,1],'g')
+    #plt.show()
+    #return
     base='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/PBTuning/'
+    #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')
+    #tune.bode_chirp(openloop=True, file=file, motor=2, minFrq=20,maxFrq=200,tSec=30.)
+    #plt.show()
+    #return
 
     if args.plot:
       #display bode plots
+      if args.plot[0]=='ext':
+        tune.bode_full_plot(mot=1,base=base)
+        tune.bode_full_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)
@@ -312,7 +432,11 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
       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))
-      tune.bode_chirp(openloop=True,file=file,motor=mot,maxFrq=500)
+      #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)
     elif mode==3:
       ol=True
       file=os.path.join(base,'sine_cl_mot%d_%s.npz'%(mot,tag))
@@ -321,10 +445,19 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
       file=os.path.join(base,'chirp_cl_mot%d_%s.npz'%(mot,tag))
       tune.bode_chirp(openloop=False,file=file,motor=mot)
     #tune.do_command('stepmove',1,100,500,0,0)
-
-
     plt.show()
   #------------------ Main Code ----------------------------------
   #ssh_test()
   ret=parse_args()
   exit(ret)
+
+#enable plc1
+#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 1 --tag ext
+#enable plc1
+#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 1 --tag ext
+#enable plc1
+#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 2 --tag ext
+#enable plc1
+#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 2 --tag ext
+#enable plc1
+#./PBTuning.py --host SAR-CPPM-EXPMX1 --plot ext