From b4e28084d854c1728afae0f7b8e735d76ac92c3c Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Wed, 21 Dec 2016 11:08:36 +0100 Subject: [PATCH] add preliminary path shaping and programming tools --- cfg/mx-stage.cfg | 111 ++++++++++++++++++++++++- cfg/mygatherplot.py | 180 ++++++++++++++++++++++++++++++++++++++++ cfg/shapepath.py | 196 ++++++++++++++++++++++++++++++++++++++++++++ cfg/torqueCtrl.cfg | 2 +- 4 files changed, 486 insertions(+), 3 deletions(-) create mode 100755 cfg/mygatherplot.py create mode 100755 cfg/shapepath.py diff --git a/cfg/mx-stage.cfg b/cfg/mx-stage.cfg index 513a3dd..710bb9b 100644 --- a/cfg/mx-stage.cfg +++ b/cfg/mx-stage.cfg @@ -40,6 +40,113 @@ $$$*** !common() !torqueCtrl() -!sh sleep 1 -#1,2,3,4,6j/ + +&1 +//#1-> 0.00001X+ 0.00001Y + A +//#2-> +1. X + .5Y + 0.01A +//#3-> + .5X +1. Y + 0.01A + +#1-> A +#2-> X +#3-> Y + +Coord[1].AltFeedRate=0 +Coord[1].Tm=1 //1ms time + +Motor[1].MaxSpeed=360 +Motor[2].MaxSpeed=50 +Motor[3].MaxSpeed=50 + +open prog 1 + //this uses jogspeed + rapid abs + X(10000) Y(0) A(0) + X(0) Y(10000) A(0) + X(0) Y(0) A(36000) + X(0) Y(0) A(0) +close + +open prog 2 + //this uses Coord[1].Tm and limits with MaxSpeed + linear abs + X(10000) Y(0) A(0) + X(0) Y(10000) A(0) + X(0) Y(0) A(0) + X(0) Y(0) A(36000) + X(0) Y(0) A(0) +close + + + +open prog 2 + //this uses Coord[1].Tm and limits with MaxSpeed + linear abs + X(10000) Y(0) A(0) + X(0) Y(10000) A(0) + X(0) Y(0) A(0) + X(0) Y(0) A(36000) + X(0) Y(0) A(0) +close + +open prog 3 + Gather.Enable=2 + //this uses Coord[1].Tm and limits with MaxSpeed + linear abs + X(10000) //Y(0) + //X(0) Y(0) + //dwell 0 + X(0) Y(10000) + X(0) Y(0) + X(10000) Y(10000) + X(0) Y(0) + dwell 100 + Gather.Enable=0 +close + + +open prog 4 + Gather.Enable=2 + //this uses Coord[1].Tm and limits with MaxSpeed + linear abs + X(100) Y(100) + X(200) Y(110) + X(300) Y(140) + X(400) Y(130) + X(500) Y(134) + X(600) Y(146) + X(700) Y(178) + X(800) Y(143) + X(900) Y(133) + X(1000) Y(147) + X(1100) Y(196) + X(1200) Y(104) + X(1300) Y(183) + X(1400) Y(135) + X(1500) Y(134) + X(1600) Y(195) + X(1700) Y(116) + X(1800) Y(150) + X(1900) Y(127) + X(1900) Y(272) + X(1800) Y(215) + X(1700) Y(232) + X(1600) Y(276) + X(1500) Y(215) + X(1400) Y(298) + X(1300) Y(235) + X(1200) Y(297) + X(1100) Y(256) + X(1000) Y(244) + X(900) Y(279) + X(800) Y(238) + X(700) Y(265) + X(600) Y(279) + X(500) Y(234) + X(400) Y(235) + X(300) Y(297) + X(200) Y(227) + X(100) Y(256) + dwell 100 + Gather.Enable=0 +close diff --git a/cfg/mygatherplot.py b/cfg/mygatherplot.py new file mode 100755 index 0000000..aff654d --- /dev/null +++ b/cfg/mygatherplot.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python +# *-----------------------------------------------------------------------* +# | | +# | Copyright (c) 2015 by Paul Scherrer Institute (http://www.psi.ch) | +# | | +# | Author Thierry Zamofing (thierry.zamofing@psi.ch) | +# *-----------------------------------------------------------------------* +''' +gathers data on powerpmac and plot + +#mode bits: + + 0 1 configure acquisition -> needs -c + 1 2 start acquisition ->Gather.Enable=2 + 2 4 stop acquisition ->Gather.Enable=0 + 3 8 wait acquisition stopped ->while(1){if(Gather.Enable==0) break} + 4 16 upload acquisition + 5 32 plot acquisition -> needs -a (or -c) + +mode 1+2+8+16+32 | user sets a stop +mode 1+2 | motion, wait some time| #mode 4+16+32 +mode 1+8+16+32 | start acqu, do motion stop acqu. + +verbose bits: + 1 basic info + 2 wait progress + 4 upload progress + +#config file example: +{ + "channels": [ + "Motor[1].ActPos", + "Motor[9].ActPos", + "Motor[10].ActPos", + "Motor[11].ActPos", + "PowerBrick[0].GpioData[0]" + ], + "axes": [ + [0, "b", "ActPos 1"], + [1, "g", "ActPos_9"], + [2, "r", "ActPos_10"], + [3, "m", "ActPos_11"], + [4, ["bits", 15, 3], "GPIO"]], + "gather": { + "MaxSamples":1000, + "Period":10 + } +} + +Acquired time is:MaxSamples*Period*.2 + +''' +#gatherPlot +#channels -c '[Sys.ServoCount,Motor[1].ActPos,Motor[9].ActPos,Motor[10].ActPos,Motor[11].ActPos,PowerBrick[0].GpioData[0]]' +#axis -a '(((0,1),'b','ActPos 1'), ((0,2),'g','ActPos 9'), ((0,3),'r','ActPos 10'), ((0,4),'m','ActPos 11'), ((0,5),('bits',14,18),'GPIO')) +#('bits',14,18) means decode bits 14-18 +# + +#check: Gather.Enable Gather.Index + +#on PPMAC do something like: +#Gather.Items=6 +#Gather.MaxSamples=10000 +#Gather.Period=10 +#Gather.Addr[0]=Sys.ServoCount.a +#Gather.Addr[1]=Motor[1].ActPos.a +#Gather.Addr[2]=Motor[9].ActPos.a +#Gather.Addr[3]=Motor[10].ActPos.a +#Gather.Addr[4]=Motor[11].ActPos.a +#Gather.Addr[5]=PowerBrick[0].GpioData[0].a +# +#Gather.Enable=2 +## move the motor[1] +#Gather.Enable=0 +# +#1;&1;#1->1000X +#k;j=0;P1=1000 +#b3r + +# to run that script without password, make first an ssh connection e.g. ssh root@PPMACZT84 +# + +import os, sys, json +import numpy as np +import matplotlib as mpl +import subprocess as sprc + +class GatherPlot: + def __init__(self): + pass + + def plot1(self): + import matplotlib.pyplot as plt + fnLoc='/tmp/gather.txt' + data = np.genfromtxt(fnLoc, delimiter=' ') + x=np.arange(data.shape[0])*2. #-> msec + pass #-> needs -a (or -c) + fig=plt.figure() + ax0 = fig.add_subplot(1,1,1) + ax0.set_xlabel('msec') + + ax=[ax0,] + axArgs=[(0,'r','A'),(1,'g','B'),(2,'b','C')] + for i in range(len(axArgs)-1): + ax.append(ax0.twinx()) + fig.subplots_adjust(right=0.75) + n=(len(ax)-2);dx=.2/n + for i in range(n): + ax[2+i].spines['right'].set_position(('axes', 1+dx*(1+i))) + ax[2+i].set_frame_on(True) + ax[2+i].patch.set_visible(False) + + + for i in range(len(axArgs)): + idx,param,lbl=axArgs[i] + col=param + ax[i].set_ylabel(lbl, color=col) + ax[i].plot(x, data[:,idx], color=col) + ax[i].tick_params(axis='y', colors=col) + + plt.show() + + def plot(self): + import matplotlib.pyplot as plt + fnLoc='/tmp/gather.txt' + data = np.genfromtxt(fnLoc, delimiter=' ') + fig=plt.figure() + ax0 = fig.add_subplot(1,1,1) + ax0.set_xlabel('msec') + ax=[ax0,] + #fig.subplots_adjust(right=0.75) + col='b' + #idx=1 + #ax[0].set_ylabel(ch[idx], color=col) + x=data[:,1] + y=data[:,2] + hl=ax[0].plot(x, y, color=col) + ax[0].tick_params(axis='y', colors=col) + + + cid = fig.canvas.mpl_connect('scroll_event', self.onclick) + fig.obj=self + self.data=data + #self.ch=ch + self.idx=1 + self.ax=ax + self.col=col + self.hl=hl + + plt.show() + + @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 + if event.button=='up': + if obj.idx0: + obj.idx-=1 + + obj.ax[0].set_ylabel(obj.ch[obj.idx], color=obj.col) + d=obj.data[:,obj.idx] + if obj.args.diff: + d=np.diff(d) + obj.hl[0].set_ydata(d) + #obj.ax[0].plot(x, obj.data[:,obj.idx]obj.data[:,obj.idx]obj.data[:,obj.idx], color=obj.col) + min=bottom=d.min() + max=d.max() + delta=(max-min)/10. + obj.ax[0].set_ylim(bottom=min-delta,top=max+delta,emit=True,auto=True) + event.canvas.figure.show() + + + +if __name__=='__main__': + gp=GatherPlot() + gp.plot() diff --git a/cfg/shapepath.py b/cfg/shapepath.py new file mode 100755 index 0000000..f5ed7c1 --- /dev/null +++ b/cfg/shapepath.py @@ -0,0 +1,196 @@ +#!/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 + +#mode bits: + + 0 1 sort and plot random points + 1 2 sort and plot grid(+some random) points + 2 4 generate motion program + +verbose bits: + 1 basic info + 2 plot sorting steps + 4 upload progress + +#config file example: +{ + "points": [ + [100,523],[635,632],[756,213], + "mode": ["plot","program","gather","???"], +} + +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 + +def ConvUtf8(s): + 'convert unicoded json object to ASCII encoded' + #http://stackoverflow.com/questions/956867/how-to-get-string-objects-instead-of-unicode-ones-from-json-in-python + if isinstance(s, dict): + return {ConvUtf8(key): ConvUtf8(value) for key, value in s.iteritems()} + elif isinstance(s, list): + return [ConvUtf8(element) for element in s] + elif isinstance(s, unicode): + return s.encode('utf-8') + else: + return s + +class GpasciiCommunicator(): + '''Communicates with the Delta Tau gpascii programm + ''' + gpascii_ack="\x06\r\n" + gpascii_inp='Input\r\n' + + def connect(self, host, username='root', password='deltatau',prompt='ppmac# '): + p=telnetlib.Telnet(host) + print p.read_until('login: ') + p.write(username+'\n') + print p.read_until('Password: ') + p.write(password+'\n') + print p.read_until(prompt) # command prompt + p.write('gpascii -2\n') # execute gpascii command + print p.read_until(self.gpascii_inp) + return p + + +class ShapePath: + def __init__(self,args): + if args.cfg: + fh=open(args.cfg,'r') + s=fh.read() + cfg=json.loads(s, object_hook=ConvUtf8) + s=json.dumps(cfg, indent=2, separators=(',', ': '));print s + else: + cfg={"points": [[100,523],[635,632],[756,213]],"mode": ["plot","program","gather","???"]} + #args.cfg={"points": [[100,523],[635,632],[756,213]],"mode": ["plot","program","gather","???"]} + self.cfg=cfg + self.args=args + + def run(self): + print('args='+str(self.args)) + print('cfg='+str(self.cfg)) + mode=self.args.mode + if mode&1: + #generate random points and sort + np.random.seed(0) + #data=np.random.randint(0,1000,(30,2)) + data=np.random.rand(107,2)*1000 + self.points=data + self.sort_points() + if mode&2: + np.random.seed(0) + xx,yy=np.meshgrid(range(10), range(10)) + data=np.array([xx.reshape(-1),yy.reshape(-1)],dtype=np.float).transpose()*100 + data+=np.random.rand(100,2)*20 + self.points=data + self.sort_points() + if mode&4: + pass + + def sort_points(self): + data=self.points + verb=self.args.verbose + if verb&2: + self.plot_points(data) + + #sort points along y + data=data[data[:, 1].argsort()] + if verb&2: + self.plot_points(data) + + #group sorting + cnt=data.shape[0] + idx=np.ndarray(cnt,dtype=np.int32) + grp_cnt=int(np.sqrt(cnt)) + grp_sz=int(np.ceil(float(cnt)/grp_cnt)) + for i in range(grp_cnt): + a=i*grp_sz + #print a,a+grp_sz + if i%2: + idx[a:a+grp_sz]=a+data[a:a+grp_sz,0].argsort()[::-1] + else: + idx[a:a+grp_sz]=a+data[a:a+grp_sz,0].argsort() + #print(idx) + data=data[idx] + + if verb&2: + self.plot_points(data) + plt.show() + self.points=data + + @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_points(self,data): + fig=plt.figure() + ax = fig.add_subplot(1,1,1) + #hl=ax[0].plot(x, y, color=col) + hl=ax.plot(data[:,0],data[:,1],'r.') + hl=ax.plot(data[:,0],data[:,1],'y--') + cid = fig.canvas.mpl_connect('button_press_event', self.onclick) + fig.obj=self + self.data=data + self.ax=ax + self.hl=hl + + +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) + parser.add_option('-m', '--mode', type="int", dest='mode', help='mode bits (see below)', default=0) + parser.add_option('--cfg', help='config file containing json configuration structure') + + (args, other)=parser.parse_args() + args.other=other + + sp=ShapePath(args) + sp.run() +#------------------ Main Code ---------------------------------- + #ssh_test() + ret=parse_args() + exit(ret) diff --git a/cfg/torqueCtrl.cfg b/cfg/torqueCtrl.cfg index 52c4d78..6735878 100644 --- a/cfg/torqueCtrl.cfg +++ b/cfg/torqueCtrl.cfg @@ -51,7 +51,7 @@ Motor[1].pPhaseEnc=Acc84B[0].Chan[0].SerialEncDataA.a //Motor[1].pAbsPhasePos=Acc84B[0].Chan[0].SerialEncDataA.a !motor_servo(mot=1,ctrl='ServoCtrl',Kp=0.8,Kvfb=20,Ki=0.001,Kvff=40,Kaff=0,MaxInt=1000) -!motor(mot=1,dirCur=0,contCur=1000,peakCur=2000,timeAtPeak=1,IiGain=1.5,IpfGain=0,IpbGain=3,JogSpeed=180.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./8192,PhaseFindingDac=1000,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=3000,WarnFeLimit=1000,InPosBand=10) +!motor(mot=1,dirCur=0,contCur=1000,peakCur=2000,timeAtPeak=1,IiGain=1.5,IpfGain=0,IpbGain=3,JogSpeed=180.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./8192,PhaseFindingDac=1000,PhaseFindingTime=1000,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=3000,WarnFeLimit=1000,InPosBand=10) //Stage X Parker MX80L