#!/usr/bin/env python # *-----------------------------------------------------------------------* # | | # | Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch) | # | | # | Author Thierry Zamofing (thierry.zamofing@psi.ch) | # *-----------------------------------------------------------------------* ''' shape an optimal path with given points verbose bits: 1 basic info 4 list program 4 upload progress 8 plot gather path prog_1(),plot_1(): position dependent friction(=current) Move multiple times with different speeds generate friction lut and save to friction.mat prog_2(),plot_2(): plot position speed acceleration. Move multiple times with different accelerations Acquired time is:MaxSamples*Period*.2 ''' import os, sys, json import numpy as np import matplotlib as mpl import matplotlib.pyplot as plt import subprocess as sprc import telnetlib, re from utilities import * os.environ['PATH']='/home/zamofing_t/scripts/:'+os.environ['PATH'] class MoveRecord: def __init__(self,args): fn='/tmp/moverecord' self.fnPrg=fn+'.prg' self.fnNpz=fn+'.npz' #cfg = {"sequencer": ['prog_1(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_1()']} #cfg = {"sequencer": ['plot_1()']} #cfg = {"sequencer": ['prog_2(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_2()']} #cfg = {"sequencer": ['plot_2()']} cfg = {"sequencer": ['prog_3(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_2()']} self.cfg=dotdict(cfg) self.args=args def run(self): print('args='+str(self.args)) print('cfg='+str(self.cfg)) try: sequencer= self.cfg.pop('sequencer') except KeyError: print('no command sequence to execute') else: for cmd in sequencer: print '>'*5+' '+cmd+' '+'<'*5 eval('self.' + cmd) def prg_prolog(self): prg=self.prg gather=self.gather channels=self.channels prg.append('Gather.Enable=0') prg.append('Gather.Items=%d'%len(channels)) for k,v in gather.iteritems(): prg.append('Gather.%s=%d'%(k,v)) for i,c in enumerate(channels): prg.append('Gather.Addr[%d]=%s.a'%(i,c)) prg.append('open prog %d'%(self.prgId)) def prg_epilog(self): fnPrg=self.fnPrg fnNpz=self.fnNpz host=self.host prg=self.prg prg.append('close') prg.append('&1\nb%dr\n'%self.prgId) if self.args.verbose & 4: for ln in prg: print(ln) if fnPrg is not None: fh=open(fnPrg,'w') fh.write('\n'.join(prg)) fh.close() if host is not None: cmd ='gpasciiCommander --host '+host+' '+ fnPrg print cmd p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT) #res=p.stdout.readlines(); print res retval = p.wait() #gather -u /var/ftp/gather/out.txt cmd ='PBGatherPlot -m24 -v7 --host '+host print cmd p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT) retval = p.wait() meta = self.meta channels = self.channels fnLoc='/tmp/gather.txt' self.rec=rec=np.genfromtxt(fnLoc, delimiter=' ') # rec=Motor[1].ActPos,Motor[2].ActPos,Motor[3].ActPos,Motor[1].DesPos,Motor[2].DesPos,Motor[3].DesPos # res=rot.ActPos,y.ActPos,x.ActPos,rot.DesPos,y.DesPos,x.DesPos # idx 0 1 2 3 4 5 if fnNpz: np.savez_compressed(fnNpz, rec=rec, prg=self.prg, channels=self.channels, meta=meta) def prog_1(self,prgId=2,host=None,acq_per=1): ''' kwargs: acq_per : acquire period: acquire data all acq_per servo loops (default=1) prgId : used program number file : used filename to save if None, not saved host : host to send and execute program, if None, nothing is executed ''' ServoPeriod= .2 #0.2ms Sys.ServoPeriod is dependent of !common() macro self.prgId=prgId self.file=file self.host=host self.gather={"MaxSamples":1000000, "Period":acq_per} self.channels=channels=["Motor[3].ActPos","Motor[3].DesPos","Motor[3].PhasePos","Motor[3].idMeas","Motor[3].iqMeas"] self.meta=meta = {'timebase': ServoPeriod*self.gather['Period']} self.prg=prg=[] #prg.append('#1..3$') #prg.append('#1..3j/') self.prg_prolog() prg=self.prg prg.append(' linear abs') prg.append('jog3=300') prg.append('dwell 100') prg.append('Gather.Enable=2') #for spd in (5,10,20,30,40): #for spd in (10,20): for spd in (10,10,10,10,20,20,20,20,20,20): prg.append('Motor[3].JogSpeed=%d'%spd) #prg.append('jog3=27000') prg.append('jog3=28500') prg.append('dwell 100') prg.append('jog3=300') prg.append('dwell 100') prg.append('Gather.Enable=0') self.prg_epilog() rec=self.rec meta['HomePos']=rec[0,1]-300 #rec[0,1]= first DesPos, 300 um is the start position of the motion print meta if self.fnNpz: np.savez_compressed(self.fnNpz, rec=rec, prg=prg, channels=channels, meta=meta) #!mx-stage() # &1 #1->0 # &1 #2->0 # #3$ # an anschlag (kabel) bewegen # #3hmz # Motor[3].PhasePos=1200 # #3j/ # &1p @staticmethod def onclick(event): print 'button=%s, x=%d, y=%d, xdata=%f, ydata=%f'%( event.button, event.x, event.y, event.xdata, event.ydata) obj=event.canvas.figure.obj def plot_1(self): try: rec=self.rec prg=self.prg channels=self.channels meta=self.meta except AttributeError: print 'load file %s'%self.fnNpz fh=np.load(self.fnNpz) rec=fh['rec'] prg=list(fh['prg']) channels=list(fh['channels']) meta=fh['meta'].item() actPos=rec[:,0]-meta['HomePos'] desPos=rec[:,1]-meta['HomePos'] desVel=np.diff(desPos) time=np.arange(0,rec.shape[0])*meta['timebase'] fig=plt.figure(); fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick) fig.canvas.set_window_title('position') ax = fig.add_subplot(1,1,1) hl=[] hl+=ax.plot(time,actPos,'r-',label=channels[0]) hl+=ax.plot(time,desPos,'g-',label=channels[1]) hl+=ax.plot(time,rec[:,2],'b-',label=channels[2]) ax.xaxis.set_label_text('ms') ax.yaxis.set_label_text('um') legend = ax.legend(loc='upper right', shadow=True) fig=plt.figure(); fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick) fig.canvas.set_window_title('current') ax = fig.add_subplot(1,1,1) hl=[] sz=100; weights = np.repeat(1.0, sz) / sz v = np.convolve(rec[:,3], weights, 'same') hl+=ax.plot(time,v,'r-',label=channels[3]) v = np.convolve(rec[:, 4], weights, 'same') hl+=ax.plot(time,v,'g-',label=channels[4]) ax.xaxis.set_label_text('ms') ax.yaxis.set_label_text('current in bits') legend = ax.legend(loc='upper right', shadow=True) print 'abs average ',channels[4], np.abs(rec[:, 4]).mean() fig=plt.figure(); fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick) fig.canvas.set_window_title('friction(=current) at position current') ax = fig.add_subplot(1,1,1) hl=[] sz=10; weights = np.repeat(1.0, sz) / sz curr = np.convolve(rec[:,4], weights, 'same') hl+=ax.plot(desPos,curr,'y-',label=channels[4]) sz=100; weights = np.repeat(1.0, sz) / sz idx=desVel[:-1]>0 arg=np.argsort(desPos[idx]) p1=desPos[idx][arg];c1=curr[idx][arg] c1 = np.convolve(c1, weights, 'same') idx=desVel[:-1]<0 arg=np.argsort(desPos[idx]) p2=desPos[idx][arg];c2=curr[idx][arg] c2 = np.convolve(c2, weights, 'same') hl+=ax.plot(p1,c1,'g-',label=channels[4]+' vel>0') hl+=ax.plot(p2,c2,'r-',label=channels[4]+' lut') #hl+=ax.plot(p1,c1-c1.mean(),'g-',label=channels[4]+' vel>0') #hl+=ax.plot(p2,c2-c2.mean(),'r-',label=channels[4]+' vel<0') cAll=c1;pAll=p1 cAll=cAll-cAll.mean() #p=p[::100];c=c[::100] # poor selection of points p=np.arange(500,28000,100) #np.arange(500,28000,128) c=np.interp(p,pAll,cAll) hl+=ax.plot(p,c,'b.',label=channels[4]+' vel<0') FfricLut=np.array([p,c]).T Ffric=np.array([c1.mean(),c2.mean()]) print 'Avg current forward:',Ffric[0],'Avg current backward:',Ffric[1] #print 'FfricLut',FfricLut print '//positions '+'%g,%g,%g'%tuple(p[0:3]),'...%g,%g,%g'%tuple(p[-3:]) print 'float lutCur[%d]={'%len(p) for i in range(len(p)): print '%g,'%(c[i]), print '};' import scipy.io fn='/home/zamofing_t/afs/ESB-MX/data/friction.mat' #scipy.io.savemat(fn,mdict={'Ffric':Ffric,'FfricLut':FfricLut}) print '\n\nuncomment line above to saved to matlab file',fn #ax.xaxis.set_label_text(channels[1]) ax.yaxis.set_label_text('current in bits: '+channels[4]) legend = ax.legend(loc='upper right', shadow=True) plt.show() def prog_2(self,prgId=2,host=None,acq_per=1): ''' kwargs: acq_per : acquire period: acquire data all acq_per servo loops (default=1) prgId : used program number file : used filename to save if None, not saved host : host to send and execute program, if None, nothing is executed ''' ServoPeriod= .2 #0.2ms Sys.ServoPeriod is dependent of !common() macro self.prgId=prgId self.file=file self.host=host self.gather={"MaxSamples":1000000, "Period":acq_per} self.channels=["Motor[3].ActPos","Motor[3].DesPos","Motor[3].PhasePos","Motor[3].idMeas","Motor[3].iqMeas"] self.meta=meta = {'timebase': ServoPeriod*self.gather['Period']} self.prg=prg=[] #prg.append('#1..3$') #prg.append('#1..3j/') self.prg_prolog() prg=self.prg prg.append(' linear abs') prg.append('Motor[3].JogTs=0') prg.append('Motor[3].JogSpeed=10') prg.append('Motor[3].JogTa=-2.5') prg.append('jog3=10000') prg.append('dwell 100') prg.append('Gather.Enable=2') for spd in (5,10,20,30,40): prg.append('Motor[3].JogSpeed=%d'%spd) #for acc in (-2.5,-1.25,-.5,-.25): # prg.append('Motor[3].JogTa=%g'%acc) prg.append('jog3=17000') prg.append('dwell 100') prg.append('jog3=10000') prg.append('dwell 1000') prg.append('Gather.Enable=0') self.prg_epilog() # &1 #1->0 # &1 #2->0 # #3$ # an anschlag (kabel) bewegen # #3hmz # Motor[3].PhasePos=1200 # or for motor 2: Motor[2].PhasePos=300 # #3j/ # &1p #folgende parameter setzen: Motor[3].HomePos meta['HomePos']=-223.8 #Motor[3].HomePos meta['PhasePos']=1200 #Motor[3].PhasePos def plot_2(self): try: rec=self.rec prg=self.prg channels=self.channels meta=self.meta except AttributeError: fh=np.load(self.fnNpz) rec=fh['rec'] prg=list(fh['prg']) channels=list(fh['channels']) meta=fh['meta'].item() time=np.arange(0,rec.shape[0])*meta['timebase'] fig=plt.figure(); fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick) fig.canvas.set_window_title('position') ax = fig.add_subplot(1,1,1) hl=[] p_avg=rec[:,1].mean(); p=rec[:,0]-p_avg hl+=ax.plot(time,p,'r-',label=channels[0]) #d=np.diff(p)*100 #hl+=ax.plot(time[1:],d,'r-',label='vel of '+channels[0]) #a=np.diff(d)*100 #hl+=ax.plot(time[2:],a,'r-',label='acc of '+channels[0]) p=rec[:,1]-p_avg hl+=ax.plot(time,p,'g-',label=channels[1]) d=np.diff(p)*50 hl+=ax.plot(time[1:],d,'b-',label='vel of '+channels[0]) a=np.diff(d)*50 hl+=ax.plot(time[2:],a,'m-',label='acc of '+channels[0]) #hl+=ax.plot(time,rec[:,2],'b-',label=channels[2]) ax.xaxis.set_label_text('ms') ax.yaxis.set_label_text('um') legend = ax.legend(loc='upper right', shadow=True) fig=plt.figure(); fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick) fig.canvas.set_window_title('current') ax = fig.add_subplot(1,1,1) hl=[] sz=100; weights = np.repeat(1.0, sz) / sz v = np.convolve(rec[:,3], weights, 'same') hl+=ax.plot(time,v,'r-',label=channels[3]) v = np.convolve(rec[:, 4], weights, 'same') hl+=ax.plot(time,v,'g-',label=channels[4]) hl+=ax.plot(time,p/100.,'y-',label=channels[1]) hl+=ax.plot(time,(rec[:,0]-p_avg)/100.,'g-',label=channels[0]) hl+=ax.plot(time,(rec[:,0]-rec[:,1]),'c-',label='PosError') ax.xaxis.set_label_text('ms') ax.yaxis.set_label_text('current in bits') legend = ax.legend(loc='upper right', shadow=True) print 'abs average ',channels[4], np.abs(rec[:, 4]).mean() print 'abs average pos Error',np.abs(rec[:,0]-rec[:,1]).mean() plt.show() def prog_3(self,prgId=2,host=None,acq_per=1): ''' kwargs: acq_per : acquire period: acquire data all acq_per servo loops (default=1) prgId : used program number file : used filename to save if None, not saved host : host to send and execute program, if None, nothing is executed ''' ServoPeriod= .2 #0.2ms Sys.ServoPeriod is dependent of !common() macro self.prgId=prgId self.file=file self.host=host self.gather={"MaxSamples":1000000, "Period":acq_per} self.channels=channels=["Motor[3].ActPos","Motor[3].DesPos","Motor[3].PhasePos","Motor[3].idMeas","Motor[3].iqMeas"] self.meta=meta = {'timebase': ServoPeriod*self.gather['Period']} self.prg=prg=[] #prg.append('#1..3$') #prg.append('#1..3j/') self.prg_prolog() prg=self.prg fh=open('/tmp/shapepath.prg','r') lines=fh.read().splitlines() fh.close() prg.extend(lines[11:-3]) self.prg_epilog() #print '\n'.join(prg) rec=self.rec lines[12] x0=float(re.match('X(\S+)\sY\S+',lines[12]).group(1)) meta['HomePos']=rec[0,1]-x0 #rec[0,1]= first DesPos is x0, the start position of the motion print meta if self.fnNpz: np.savez_compressed(self.fnNpz, rec=rec, prg=prg, channels=channels, meta=meta) # &1 #1->0 # &1 #2->0 # #3$ # an anschlag (kabel) bewegen # #3hmz # Motor[3].PhasePos=1200 # or for motor 2: Motor[2].PhasePos=300 # #3j/ # &1p #folgende parameter setzen: Motor[3].HomePos meta['HomePos']=-223.8 #Motor[3].HomePos meta['PhasePos']=1200 #Motor[3].PhasePos if __name__=='__main__': from optparse import OptionParser, IndentedHelpFormatter class MyFormatter(IndentedHelpFormatter): 'helper class for formating the OptionParser' def __init__(self): IndentedHelpFormatter.__init__(self) def format_epilog(self, epilog): if epilog: return epilog else: return "" def parse_args(): 'main command line interpreter function' #usage: gpasciiCommunicator.py --host=PPMACZT84 myPowerBRICK.cfg (h, t)=os.path.split(sys.argv[0]);cmd='\n '+(t if len(h)>3 else sys.argv[0])+' ' exampleCmd=('--host=PPMAC1391 -m 63 --cfg gather.cfg', 'samplePowerBrick.cfg', '-n stackCheck1.cfg', '--host=PPMACZT84 stackCheck1.cfg', '--host=PPMACZT84 stackCheck1.cfg -v15', ) epilog=__doc__+''' Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' fmt=MyFormatter() parser=OptionParser(epilog=epilog, formatter=fmt) parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=0) (args, other)=parser.parse_args() args.other=other mr=MoveRecord(args) mr.run() #------------------ Main Code ---------------------------------- #ssh_test() ret=parse_args() exit(ret)