diff --git a/Readme.md b/Readme.md index 5c85a88..42378a6 100644 --- a/Readme.md +++ b/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 + diff --git a/cfg/mx-stage.cfg b/cfg/mx-stage.cfg index dbfa674..d76e1ed 100644 --- a/cfg/mx-stage.cfg +++ b/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 diff --git a/python/MAError.py b/python/MAError.py index 9a44f37..62522d9 100644 --- a/python/MAError.py +++ b/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(erry, 'y-',label='y-error') + hl += ax.plot(errx, 'b-',label='x-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() diff --git a/python/MAVelocity.py b/python/MAVelocity.py index 336546d..6e7fb53 100644 --- a/python/MAVelocity.py +++ b/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(velxDes, 'g--',label='vel x des') - hl += ax.plot(velyAct, 'b-',label='vel y act') - hl += ax.plot(velyDes, 'b--',label='vel y des') - ax.xaxis.set_label_text('time (10x servo cycle)') - ax.yaxis.set_label_text('pos-error um') + hl += ax.plot(velxAct, 'b-',label='vel x act') + hl += ax.plot(velxDes, 'b--',label='vel x des') + hl += ax.plot(velyAct, 'g-',label='vel y act') + hl += ax.plot(velyDes, 'g--',label='vel y des') + hl += ax.plot(velAct, 'r-',label='vel act') + 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]) - velxDes = np.diff(rec[:, 4]) - velyAct = np.diff(rec[:, 2]) - velyDes = np.diff(rec[:, 5]) - doc.vel = vel = (velxAct,velxDes,velyAct,velyDes) - canvas.InitChild(vel) + velxAct = np.diff(rec[:, 1])/tb + velxDes = np.diff(rec[:, 4])/tb + velyAct = np.diff(rec[:, 2])/tb + velyDes = np.diff(rec[:, 5])/tb + velAct = np.sqrt(velxAct**2+velyAct**2) + 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]) diff --git a/python/PBMotionAnalyzer.py b/python/PBMotionAnalyzer.py index 85553d2..e1bb52d 100755 --- a/python/PBMotionAnalyzer.py +++ b/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 diff --git a/python/data/shapepath.npz b/python/data/shapepath.npz index 1887abf..1f56379 100644 Binary files a/python/data/shapepath.npz and b/python/data/shapepath.npz differ diff --git a/python/shapepath.py b/python/shapepath.py index f970c31..3056cf9 100755 --- a/python/shapepath.py +++ b/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")','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()']} + #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()']} + #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==0: + if mode==-1: + pos=self.points prg.append(' linear abs') - data=self.points - for idx in range(data.shape[0]): - prg.append('X(%f) Y(%f)'%tuple(data[idx,:])) + prg.append('X(%g) Y(%g)' % tuple(pos[0, :])) + prg.append('dwell 10') + prg.append('Gather.Enable=2') + prg.append('jog2:10000') prg.append('dwell 100') - if mode==1: - pass - prg.append('Gather.Enable=0') + prg.append('jog3:10000') + prg.append('dwell 100') + 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()