pvt motion

Readme.md

@@ -12,6 +12,8 @@ Resistance Ohms 8.8
 Inductance mH 2.4
 Max.BuxVoltage V 80
 
+Continous Force 4N -> assume 1kg load -> acceleration=a=F/m=4m/s^2
+
 Mecapion rot stage
 ------------------
 ```
@@ -46,11 +48,13 @@ Servo Test Motor QBL 4208-41-04-006
 
 
 ******* Path shaping test *****************
+cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg
 gpasciiCommander --host SAROP11-CPPM-MOT6871 -i
 !mx-stage()
 #1..3$
 &1
 #1..3j/
 
-cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg
+cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python
 ./shapepath.py -v255
+

cfg/mx-stage.cfg

@@ -52,6 +52,18 @@ $$$***
 
 Coord[1].AltFeedRate=0
 Coord[1].Tm=1  //1ms time
+Coord[1].Ta=1
+Coord[1].Td=1
+Coord[1].Ts=0
+
+//Parker: Continous Force 4N -> assume 1kg load -> acceleration=a=F/m=4m/s^2
+//do not use time to accelerate but acceleration
+//4m/s^2=4um/(ms)^2 Motor units=um -> JogTa= -1ms^2/4mu -2.5-> 4m/s^2 -25->0.4m/s^2
+Motor[2].JogTs=0
+Motor[2].JogTa=-2.5
+Motor[3].JogTs=0
+Motor[3].JogTa=-2.5
+
 
 Motor[1].MaxSpeed=360
 Motor[2].MaxSpeed=50

python/MAError.py

@@ -45,16 +45,16 @@ class MPLCanvasErr(FigureCanvas):
     self.fig=fig
     self.ax=ax
 
-  def InitChild(self,err):
+  def InitChild(self,meta,err):
 
     fig=self.fig
     ax=self.ax
     errx,erry,err=err
     hl = []
-    hl += ax.plot(errx, 'g-',label='x-error')
+    hl += ax.plot(errx, 'b-',label='x-error')
-    hl += ax.plot(erry, 'y-',label='y-error')
+    hl += ax.plot(erry, 'g-',label='y-error')
     hl += ax.plot(err, 'r-',label='error')
-    ax.xaxis.set_label_text('time (10x servo cycle)')
+    ax.xaxis.set_label_text('datapoint (timebase: %g ms per data point)'%meta['timebase'])
     ax.yaxis.set_label_text('pos-error um')
     legend = ax.legend(loc='upper right', shadow=True)
     ax.plot([.5,.5],[0.01, 0.99],'k',transform=ax.transAxes)
@@ -128,6 +128,7 @@ class MAErrorFrame(wx.Frame):
 
     toolbar=ut.AddToolbar(canvas,sizer)
 
+    meta = doc.fh['meta'].item()
     try:
       err=doc.err
     except AttributeError:
@@ -136,7 +137,7 @@ class MAErrorFrame(wx.Frame):
       erry = abs(rec[:, 2] - rec[:, 5])
       errxy = np.sqrt(errx ** 2 + erry ** 2)
       doc.err = err = (errx, erry, errxy)
-    canvas.InitChild(err)
+    canvas.InitChild(meta,err)
 
     self.Centre()
 

python/MAVelocity.py

@@ -45,19 +45,22 @@ class MPLCanvasVelo(FigureCanvas):
     self.fig=fig
     self.ax=ax
 
-  def InitChild(self,vel):
+  def InitChild(self,meta,vel):
-
     fig=self.fig
     ax=self.ax
 
-    velxAct,velxDes,velyAct,velyDes=vel
+    velxAct,velxDes,velyAct,velyDes,velAct,velDes=vel
+    l=velxAct.shape[0]
     hl = []
-    hl += ax.plot(velxAct, 'g-',label='vel x act')
+    hl += ax.plot(velxAct, 'b-',label='vel x act')
-    hl += ax.plot(velxDes, 'g--',label='vel x des')
+    hl += ax.plot(velxDes, 'b--',label='vel x des')
-    hl += ax.plot(velyAct, 'b-',label='vel y act')
+    hl += ax.plot(velyAct, 'g-',label='vel y act')
-    hl += ax.plot(velyDes, 'b--',label='vel y des')
+    hl += ax.plot(velyDes, 'g--',label='vel y des')
-    ax.xaxis.set_label_text('time (10x servo cycle)')
+    hl += ax.plot(velAct, 'r-',label='vel act')
-    ax.yaxis.set_label_text('pos-error um')
+    hl += ax.plot(velDes, 'r--',label='vel des')
+
+    ax.xaxis.set_label_text('datapoint (timebase: %g ms per data point)'%meta['timebase'])
+    ax.yaxis.set_label_text('um/ms')
     legend = ax.legend(loc='upper right', shadow=True)
     #axvline(linewidth=4, color='r')
     #mpl.axvline(linewidth=4, color='r')
@@ -136,16 +139,24 @@ class MAVelocityFrame(wx.Frame):
 
     toolbar=ut.AddToolbar(canvas,sizer)
 
+    meta = doc.fh['meta'].item()
     try:
       vel=doc.vel
     except AttributeError:
+      tb=meta['timebase']
+      #data points are um
+      #datapoint timebase: 2 ms () per data point
+      #velocity: um/ms (deltatau desVel= motor units per serco cycle)
       rec=doc.fh['rec']
-      velxAct = np.diff(rec[:, 1])
+      velxAct = np.diff(rec[:, 1])/tb
-      velxDes = np.diff(rec[:, 4])
+      velxDes = np.diff(rec[:, 4])/tb
-      velyAct = np.diff(rec[:, 2])
+      velyAct = np.diff(rec[:, 2])/tb
-      velyDes = np.diff(rec[:, 5])
+      velyDes = np.diff(rec[:, 5])/tb
-      doc.vel = vel = (velxAct,velxDes,velyAct,velyDes)
+      velAct = np.sqrt(velxAct**2+velyAct**2)
-    canvas.InitChild(vel)
+      velDes = np.sqrt(velxDes**2+velyDes**2)
+      doc.vel = vel = (velxAct,velxDes,velyAct,velyDes,velAct,velDes)
+
+    canvas.InitChild(meta,vel)
 
     self.Centre()
 
@@ -184,7 +195,6 @@ class MAVelocityFrame(wx.Frame):
       canvas = self.canvas
       ax = canvas.ax
       len = usrData
-      err = self.doc.err
       hl = canvas.hl
       #hl[0].set_data(rec[:len, 4], rec[:len, 5])
       #hl[3].set_data(rec[:len, 1], rec[:len, 2])

python/PBMotionAnalyzer.py

@@ -109,7 +109,7 @@ class MAMainFrame(wx.Frame):
 
     sizer.Add(wxTxt, 1, wx.LEFT | wx.TOP | wx.GROW)
 
-    wxTimeCtrl=ut.SliderGroup(self, label='Time', range=(0, 1000))
+    wxTimeCtrl=ut.SliderGroup(self, label='DataPoint', range=(0, 1000))
     wxTimeCtrl.SetCallback(MAMainFrame.OnSetTime, wxTimeCtrl)
     sizer.Add(wxTimeCtrl.sizer, 0, wx.EXPAND | wx.ALIGN_CENTER | wx.ALL, border=5)  #wxSplt.SplitVertically(wxTree, wxTxt)
     wxPosCtrl=ut.SliderGroup(self, label='Position', range=(0, 1000))
@@ -126,6 +126,7 @@ class MAMainFrame(wx.Frame):
     self.wxTxt=wxTxt
     #self.wxTree=wxTree
     #self.display=wxTxt
+
   def __del__(self):
     self.CloseFile()
 
@@ -298,6 +299,14 @@ if __name__ == '__main__':
       if args.npzFile:
         frame.OpenFile(args.npzFile)
       frame.Show(True)
+
+      f1 = MAxyPlotFrame(frame, doc)
+      f1.Show(True)
+      f2= MAErrorFrame(frame, doc)
+      f2.Show(True)
+      f3= MAVelocityFrame(frame, doc)
+      f3.Show(True)
+
       self.SetTopWindow(frame)
       return True
 

python/shapepath.py

@@ -64,9 +64,19 @@ class ShapePath:
         #cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
         #cfg={"sequencer":['gen_grid_points(w=10,h=10,pitch=100,rnd=0.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg")','plot_gather()']}
         #cfg = {"sequencer": ['gen_rand_points(n=107, scale=1000)', 'sort_points()','plot_gather()']}
-        cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
+        #cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
-        cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg")','plot_gather()']}
+        #cfg={"sequencer":['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)','sort_points()','gen_prog(file="/tmp/shapepath.prg")','plot_gather()']}
-        #cfg={"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871")','plot_gather()']}
+        #fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S')
+        #cfg={"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871")','plot_gather("'+fn+'.npz")']}
+        #cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather()']}
+        #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=50,rnd=.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1)', 'plot_gather()']}
+        #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=100,rnd=0)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather()']}
+        #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather()']}
+        #cfg = {"points": [[0, 0],[100, 0],[200, 0],[300, 0],[400, 0],[400, 100],[300, 100],[200, 100],[100, 100],[0, 100],[10, 200],[100, 200],[200, 200],[300, 200],[400, 200],[410, 300],[300, 300],[200, 300],[100, 300],[0, 300],[0, 400],[100, 400],[200, 400],[300, 400],[400, 400]],"sequencer": ['gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather()']}
+        #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=-1)', 'plot_gather()']}
+        #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=1000)', 'plot_gather()']}
+        cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather()']}
+        #cfg = {"sequencer": ['gen_rand_points(n=400, scale=1000)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather()']}
 
       self.cfg=dotdict(cfg)
       self.args=args
@@ -103,9 +113,11 @@ class ShapePath:
           pts+=(np.random.rand(pts.shape[0],2)*(rnd*pitch))
         self.points=pts
 
-    def gen_prog(self,prgId=2,file=None,host=None,mode=0):
+    def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs):
       prg=[]
-      gather={"MaxSamples":100000, "Period":10 }
+      gather={"MaxSamples":100000, "Period":1 }
+      #Sys.ServoPeriod= 0.2
+      self.meta = {'timebase': 0.2*gather['Period']}
       #channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
       channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos","Motor[1].DesPos","Motor[2].DesPos","Motor[3].DesPos"]
       prg.append('Gather.Enable=0')
@@ -118,16 +130,64 @@ class ShapePath:
 
       prg.append('open prog %d'%(prgId))
       # this uses Coord[1].Tm and limits with MaxSpeed
-      prg.append('Gather.Enable=2')
+      if mode==-1:
-      if mode==0:
+        pos=self.points
         prg.append('  linear abs')
-        data=self.points
+        prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
-        for idx in range(data.shape[0]):
+        prg.append('dwell 10')
-          prg.append('X(%f) Y(%f)'%tuple(data[idx,:]))
+        prg.append('Gather.Enable=2')
+        prg.append('jog2:10000')
         prg.append('dwell 100')
-      if mode==1:
+        prg.append('jog3:10000')
-        pass
+        prg.append('dwell 100')
-      prg.append('Gather.Enable=0')
+        prg.append('jog2:-10000')
+        prg.append('dwell 100')
+        prg.append('jog3:-10000')
+        prg.append('dwell 100')
+        prg.append('Gather.Enable=0')
+      elif mode==0:
+        pos=self.points
+        prg.append('  linear abs')
+        prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
+        prg.append('dwell 10')
+        prg.append('Gather.Enable=2')
+        prg.append('  linear abs')
+        for idx in range(pos.shape[0]):
+          prg.append('X%g Y%g'%tuple(pos[idx,:]))
+        prg.append('dwell 100')
+        prg.append('Gather.Enable=0')
+      elif mode==1:
+        try:
+          pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
+        except KeyError:
+          print 'missing pt2pt_time, use default=100ms'
+          pt2pt_time=100.
+
+        pt=self.points
+        vel=pt[2:,:]-pt[:-2,:]
+        #pv is an array of posx posy velx vely
+        pv=np.ndarray(shape=(pt.shape[0]+2,4),dtype=pt.dtype)
+        pv[:]=np.NaN
+        #pv[     0,(0,1)]=2*pt[0,:]-pt[1,:]
+        pv[     0,(0,1)]=pt[0,:]
+        pv[  1:-1,(0,1)]=pt
+        #pv[    -1,(0,1)]=2*pt[-1,:]-pt[-2,:]
+        pv[    -1,(0,1)]=pt[-1,:]
+        pv[(0,0,-1,-1),(2,3,2,3)]=0
+        dist=pv[2:,(0,1)] - pv[:-2,(0,1)]
+        pv[  1:-1,(2,3)] = 1000.*dist/(2.*pt2pt_time)
+
+        prg.append('  linear abs')
+        prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
+        prg.append('dwell 10')
+        prg.append('Gather.Enable=2')
+        prg.append('  pvt%g abs'%pt2pt_time) #100ms to next position
+        for idx in range(1,pv.shape[0]):
+          prg.append('X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)]))
+        prg.append('X%g Y%g' % tuple(pv[-1, (0,1)]))
+        prg.append('dwell 1000')
+        prg.append('Gather.Enable=0')
+
       prg.append('close')
       prg.append('&1\nb%dr\n'%prgId)
       if self.args.verbose & 4:
@@ -200,11 +260,17 @@ class ShapePath:
       self.ax=ax
       self.hl=hl
 
-    def plot_gather(self,fnLoc='/tmp/gather.txt',fnOut='/tmp/shapepath.npz'):
+    def plot_gather(self,fnOut='/tmp/shapepath.npz',fnLoc='/tmp/gather.txt'):
+      meta=self.meta
       pts=self.points
       rec = np.genfromtxt(fnLoc, delimiter=' ')
       if fnOut:
-        np.savez_compressed(fnOut, rec=rec, pts=pts)
+        # time base:
+        # Gather.Period=10
+        # Sys.ServoPeriod=0.2 -> 5kHz
+        # -> aquired data all 10*0.2ms= 2 ms -> 30000 points = 60 sec
+
+        np.savez_compressed(fnOut, rec=rec, pts=pts, meta=meta)
       fig=plt.figure()
       ax = fig.add_subplot(1,1,1)
       #hl=ax[0].plot(x, y, color=col)
@@ -223,7 +289,7 @@ class ShapePath:
       ax = fig.add_subplot(1, 1, 1)
       err=np.sqrt((rec[:,1]-rec[:,4])**2+(rec[:,1]-rec[:,4])**2)
       hl = ax.plot(err, 'r-')
-      ax.xaxis.set_label_text('time (10x servo cycle)')
+      ax.xaxis.set_label_text('datapoint (timebase: %g ms per data point)' % meta['timebase'])
       ax.yaxis.set_label_text('pos-error um')
       plt.show()