towards matlab

python/MXTuning.py

@@ -76,6 +76,11 @@ class MXTuning(Tuning):
         num1=np.poly1d([mag1])
         den1 = np.poly1d([T1**2,2*T1*d1,1])
 
+        #reiner integrator: 30Hz=0dB -> k=30*2*pi=180
+        #num1=np.poly1d([120*120])
+        #den1 = np.poly1d([1,0,0])
+
+
         #first resonance frequency
         f2=np.array([197,199])
         d2=np.array([.02,.02])#daempfung
@@ -101,9 +106,9 @@ class MXTuning(Tuning):
         num=num1*num2*numc#*num3
         den=den1*den2*denc#*den3
         mdl= signal.lti(num, den) #num denum
-        print num
-        print den
-        print mdl
+        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)
@@ -175,9 +180,9 @@ class MXTuning(Tuning):
         num=num1*num2*num3*num4*num5*numc
         den=den1*den2*den3*den4*den5*denc
         mdl= signal.lti(num, den) #num denum
-        print num
-        print den
-        print mdl
+        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)
@@ -196,6 +201,100 @@ class MXTuning(Tuning):
       # tp print see also: print(np.poly1d([1,2,3], variable='s')), print(np.poly1d([1,2,3], r=True, variable='s'))
 
 
+    def custom_chirp(self):
+      motor = 1
+      amp, minFrq, maxFrq, tSec = (10, 10, 300, 30)
+      file='/tmp/gather.npz'
+      # if not os.path.isfile(f): tune.init_stage();plt.close('all')
+      # tune.bode_chirp(openloop=True, file=f, motor=mot, amp=amp, minFrq=minFrq, maxFrq=maxFrq, tSec=tSec)
+      prog = '''
+      &0 //cout works only in coord 0
+      open prog 999
+      L11=0
+      L10=0
+      L12=0
+      L13=0
+      L14=0
+      Gather.Enable=2
+      while(L10<300005)
+      {
+      L12=10*sin(31.415926535897931*(pow(1.058324104020218,(L10*0.000199996614513))-1)/log(1.058324104020218))
+      cout%d:(L12)
+      L10=L10+1
+      }
+      Gather.Enable=0
+      close
+      b999r
+      '''%motor
+      gpascii = self.comm.gpascii
+      gt = self.gather
+      print(gpascii.servo_period)
+      gt.set_phasemode(False)
+      address=("Motor[1].IqCmd", "Motor[1].ActPos",)
+      gt.set_address(*address)
+      #Gather.Enable=1
+      gt.set_property(MaxSamples=300000, Period=1)
+
+      # gt.enable(2)
+      gpascii.send_line(prog)
+      gpascii.sync()
+
+      gt.wait_stopped()
+      self.data=data=gt.upload()
+      meta={'motor':motor,'date':time.asctime(),'minFrq':minFrq,'maxFrq':maxFrq,'tSec':tSec,'amp':amp,'address':address}
+      np.savez_compressed(file, data=data, meta=meta)
+      meta['file'] = file
+      self.bode_chirp_plot(data, meta,True)
+      pass
+
+
+    def bode_sine(self,openloop=True,motor=1,minFrq=1,maxFrq=20,numFrq=15,amp=10,file='/tmp/gather.npz'):
+      '''calculates phase and amplitude at different frequencies and
+         saves:#loads and plots the bode diagram'''
+      if False:# os.path.isfile(file):
+        f=np.load(file)
+        bode=f['bode']
+        meta=f['meta'].item()
+        meta['file']=file
+      else:
+        gpascii=self.comm.gpascii
+        #motor 1 maximum: 13750
+        #amp= percentage of maximum amplitude
+        fig = plt.figure()
+        ax = fig.add_subplot(1, 1, 1)
+        frqLst=np.logspace(np.log10(minFrq),np.log10(maxFrq),numFrq)
+        n=len(frqLst)
+        #frqLst=(10,15,20,25,30)
+        bode=np.ndarray((n,3))
+        bode[:, 0]=frqLst
+        #for i in range(n):
+        for i in range(n-1,-1,-1):
+          frq=frqLst[i]
+          t=1
+          rep=max(1,frq*t)
+          if openloop:
+            data=self.do_command('openloopsine',motor,amp,frq,rep,0)
+          else:
+            data=self.do_command('sinusoidal',motor,amp,frq,rep,0)
+            data=data[:,(1,2)]
+          gpascii.send_line('#1j=0')
+          time.sleep(1)
+          ax.clear()
+          avg=data.mean(0)
+          print(avg)
+          ax.plot(data[:, 0]-avg[0] , 'b-', label='input')
+          ax.plot(data[:, 1]-avg[1], 'g-', label='output')
+          #plt.pause(.05)
+          bode[i,1:]=self.phase_amp(frq, rep)
+          print('frq %g ampl %g phase %g'%tuple(bode[i,:]))
+          plt.show(block=False);plt.pause(.05)
+
+        meta={'motor':motor,'date':time.asctime()}
+        np.savez_compressed(file, bode=bode, meta=meta)
+        meta['file']=file
+      self.bode_sine_plot(bode, meta)
+
+
 def bode(mdl):
   w,mag,phase = signal.bode(mdl,1000)
   f=w/(2*np.pi)
@@ -214,6 +313,16 @@ def bode(mdl):
 
 if __name__=='__main__':
   from argparse import ArgumentParser,RawDescriptionHelpFormatter
+  import logging
+  logger = logging.getLogger(__name__)
+  logger = logging.getLogger('pbtools.misc.pp_comm')
+  logger.setLevel(logging.DEBUG)
+  logging.basicConfig(format=('%(asctime)s %(name)-12s '
+                              '%(levelname)-8s %(message)s'),
+                      datefmt='%m-%d %H:%M',
+                      )
+
+
   def parse_args():
     'main command line interpreter function'
     #usage: gpasciiCommunicator.py --host=PPMACZT84 myPowerBRICK.cfg
@@ -315,7 +424,13 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
           tune.bode_sine(openloop=False, file=fn)
         if os.path.basename(fn).startswith('chirp_cl_mot'):
           tune.bode_chirp(openloop=False, file=fn)
-      print 'done'
+      print('done')
+    elif mode==7: #further tests
+        tune.init_stage();
+        plt.close('all')
+        tune.bode_sine()
+        tune.custom_chirp()
+
     plt.show()
   #------------------ Main Code ----------------------------------
   #ssh_test()