add preliminary path shaping and programming tools

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
 

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.idx<len(obj.ch)-1:
+            obj.idx+=1
+        else:
+          if obj.idx>0:
+            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()

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)

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