762 lines
24 KiB
Python
Executable File
762 lines
24 KiB
Python
Executable File
#!/usr/bin/env python
|
|
# *-----------------------------------------------------------------------*
|
|
# | |
|
|
# | Copyright (c) 2017 by Paul Scherrer Institute (http://www.psi.ch) |
|
|
# | |
|
|
# | Author Thierry Zamofing (thierry.zamofing@psi.ch) |
|
|
# *-----------------------------------------------------------------------*
|
|
'''
|
|
tools to setup and execute a helical scan of a cristal
|
|
|
|
motors CX CZ RY FY
|
|
7 8 1 2
|
|
Mot 1: Rotation stage LS Mecapion MDM-DC06DNC0H 32 poles = 1 rev = 16*2048=32768 phase_step
|
|
Mot 2: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step
|
|
Mot 3: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step
|
|
Mot/Enc 4: camera base plate X
|
|
Mot/Enc 5: camera base plate Y
|
|
Mot 6: Backlight 2.3A
|
|
Mot 7: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
|
|
Mot 8: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
|
|
|
|
verbose bits:
|
|
#1 basic info
|
|
#2 plot sorting steps
|
|
4 list program
|
|
#4 upload progress
|
|
#8 plot gather path
|
|
|
|
|
|
|
|
'''
|
|
|
|
import os, sys, json
|
|
import numpy as np
|
|
import matplotlib as mpl
|
|
import matplotlib.pyplot as plt
|
|
import mpl_toolkits.mplot3d as plt3d
|
|
import matplotlib.animation as anim
|
|
from matplotlib.widgets import Slider
|
|
import subprocess as sprc
|
|
from utilities import *
|
|
|
|
d2r=2*np.pi/360
|
|
|
|
|
|
#ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
|
|
#plot coordinates: X Y Z
|
|
#data Z X Y
|
|
|
|
class Trf:
|
|
#https://stackoverflow.com/questions/6802577/python-rotation-of-3d-vector
|
|
@staticmethod
|
|
def rotZ(rad):
|
|
""" Return matrix for rotating about the z-axis by 'radians' radians """
|
|
c = np.cos(rad)
|
|
s = np.sin(rad)
|
|
m=np.identity(4)
|
|
m[0:2,0:2]=((c, -s),(s, c))
|
|
return m
|
|
|
|
@staticmethod
|
|
def rotY(rad):
|
|
""" Return matrix for rotating about the z-axis by 'radians' radians """
|
|
c = np.cos(rad)
|
|
s = np.sin(rad)
|
|
m=np.identity(4)
|
|
m[np.ix_((0,2),(0,2))]=((c, s),(-s, c))
|
|
return m
|
|
|
|
@staticmethod
|
|
def trans(x,y,z):
|
|
m=np.identity(4)
|
|
m[0:3, 3] =(x,y,z)
|
|
return m
|
|
|
|
class HelicalScan:
|
|
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:
|
|
fn='/tmp/shapepath4'
|
|
#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_grid_points(w=5,h=5,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']}
|
|
#cfg = {"sequencer": ['test_find_rot_ctr()']}
|
|
#cfg = {"sequencer": ['test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)']}
|
|
cfg = {"sequencer": ['test_coord_trf()']}
|
|
|
|
self.cfg=dotdict(cfg)
|
|
self.args=args
|
|
|
|
def sequencer(self):
|
|
print('args='+str(self.args))
|
|
print('cfg='+str(self.cfg))
|
|
#try:
|
|
# self.points=np.array(self.cfg.points)
|
|
#except AttributeError:
|
|
# pass
|
|
try:
|
|
sequencer= self.cfg.pop('sequencer')
|
|
except KeyError:
|
|
print('no command sequence to execute')
|
|
else:
|
|
dryrun=self.args.dryrun
|
|
for cmd in sequencer:
|
|
print('>'*5+' '+cmd+' '+'<'*5)
|
|
if not dryrun:
|
|
eval('self.' + cmd)
|
|
|
|
|
|
@staticmethod
|
|
def test_find_rot_ctr(n=3.,per=1.,bias=4.1,ampl=2.4,phi=37):
|
|
# find the rotation center, amplitude out of n (niminum 3) measurements
|
|
# n number of equidistant measurements
|
|
# per number of periods (full rotation of all measurements nut be a interger value for precise measurements)
|
|
# phi phase
|
|
# bias bias value
|
|
# ampl amplitude
|
|
|
|
t = np.arange(n)
|
|
w=2*np.pi*per/n*t
|
|
y=ampl*np.cos(w+phi*d2r)+bias
|
|
plt.figure(1)
|
|
plt.subplot(311)
|
|
plt.plot(t,y,'b.-')
|
|
|
|
plt.subplot(312)
|
|
f = np.fft.fft(y)
|
|
plt.step(t, f.real,'b.-', t, f.imag,'r.-', where='mid')
|
|
|
|
(bias,ampl,phase)=HelicalScan.meas_rot_ctr(y, per)
|
|
print('bias: '+str(bias))
|
|
print('amplitude: '+str(ampl))
|
|
print('phase: '+str(phase*360./2/np.pi))
|
|
|
|
plt.subplot(313)
|
|
t2 = np.linspace(0,2*np.pi,64)
|
|
y2=ampl*np.cos(t2+phase)+bias
|
|
plt.plot(t2,y2,'g-')
|
|
plt.stem(w,y,'b-')
|
|
|
|
|
|
plt.show()
|
|
pass
|
|
|
|
|
|
def test_coord_trf(self):
|
|
self.calcParam()
|
|
param = self.param
|
|
dx, dz, w, y, = (0.2,0.3,0.1,4.3)
|
|
print 'input : dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)
|
|
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
|
|
print 'inv_trf: cx:%.3g cz:%.3g w:%.3g fy:%.3g' % (cx,cz,w/d2r,fy)
|
|
(dx, dz,w,y) = self.fwd_transform(cx,cz,w,fy)
|
|
print 'fwd_trf: dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)
|
|
|
|
# plt.ion()
|
|
# fig = plt.figure()
|
|
|
|
# a=anim.FuncAnimation(fig,self.my_anim_func3,100,fargs=(horig,pt),interval=20,blit=False)
|
|
# plt.show()
|
|
|
|
# def my_anim_func3(self,idx):
|
|
# self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist)
|
|
|
|
def interactive_cx_cz_w_fy(self):
|
|
fig = plt.figure()
|
|
self.manip=False#True#False
|
|
self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
|
|
ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
|
|
ax.view_init(elev=14., azim=10)
|
|
self.calcParam()
|
|
param=self.param
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
ctr=param[:,0:3].mean(0)[::-1]
|
|
self.axSetCenter(ctr,10)
|
|
|
|
axCx=plt.axes([0.1, 0.01, 0.8, 0.02])
|
|
axCz=plt.axes([0.1, 0.04, 0.8, 0.02])
|
|
axW =plt.axes([0.1, 0.07, 0.8, 0.02])
|
|
axFy=plt.axes([0.1, 0.10, 0.8, 0.02])
|
|
if self.manip:
|
|
lz=ax.get_xlim()
|
|
lx=ax.get_ylim()
|
|
ly=ax.get_zlim()
|
|
else:
|
|
lx = ly=lz=[-5,5]
|
|
self.sldCx=sCx=Slider(axCx, 'cx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
|
|
self.sldCz=sCz=Slider(axCz, 'cz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.)
|
|
self.sldW =sW =Slider(axW, 'ang', -180., 180.0, valinit=0)
|
|
self.sldFy=sFy=Slider(axFy, 'fy', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.)
|
|
sCx.on_changed(self.update_cx_cz_w_fy)
|
|
sCz.on_changed(self.update_cx_cz_w_fy)
|
|
sW.on_changed(self.update_cx_cz_w_fy)
|
|
sFy.on_changed(self.update_cx_cz_w_fy)
|
|
|
|
hCrist,pt=self.pltCrist()
|
|
|
|
self.hCrist=hCrist;self.fig=fig
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
p=np.ndarray((param.shape[0], 3))
|
|
for i in range(2):
|
|
(z_i, y_i, x_i, r_i, phi_i)=param[i]
|
|
p[i,0]=x_i+r_i*np.sin(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
|
|
p[i,1]=y_i # y= y_i
|
|
p[i,2]=z_i+r_i*np.cos(phi_i) # z= z_i+r_i*sin(phi_i*w)
|
|
print p
|
|
ofs=(p[1]+p[0])/2. # = center of the cristal
|
|
|
|
m=Trf.trans(*ofs); self.hOrig=self.pltOrig(m)
|
|
plt.show()
|
|
|
|
|
|
def update_cx_cz_w_fy(self,val):
|
|
cx = self.sldCx.val
|
|
cz = self.sldCz.val
|
|
w = self.sldW.val
|
|
fy = self.sldFy.val
|
|
if self.manip:
|
|
param = self.param
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
p = np.ndarray((param.shape[0], 3))
|
|
for i in range(2):
|
|
(z_i, y_i, x_i, r_i, phi_i) = param[i]
|
|
p[i, 0] = x_i + r_i * np.cos(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
|
|
p[i, 1] = y_i # y= y_i
|
|
p[i, 2] = z_i + r_i * np.sin(phi_i) # z= z_i+r_i*sin(phi_i*w)
|
|
print p
|
|
ofs = (p[1] + p[0]) / 2. # = center of the cristal
|
|
|
|
m = Trf.trans(cx,fy,cz)
|
|
m= m.dot(Trf.rotY(w*d2r))
|
|
self.hOrig = self.pltOrig(m,self.hOrig)
|
|
else:
|
|
self.hCrist,pt=self.pltCrist(fy,cx,cz,w*d2r,self.hCrist)
|
|
#l.set_ydata(amp * np.sin(2 * np.pi * freq * t))
|
|
self.fig.canvas.draw_idle()
|
|
|
|
|
|
def interactive_dx_dz_w_y(self):
|
|
fig = plt.figure()
|
|
self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
|
|
ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
|
|
ax.view_init(elev=14., azim=10)
|
|
self.calcParam()
|
|
param=self.param
|
|
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
ctr=(0,0,0)
|
|
self.axSetCenter(ctr,10)
|
|
|
|
axDx=plt.axes([0.1, 0.01, 0.8, 0.02])
|
|
axDz=plt.axes([0.1, 0.04, 0.8, 0.02])
|
|
axW =plt.axes([0.1, 0.07, 0.8, 0.02])
|
|
axY =plt.axes([0.1, 0.10, 0.8, 0.02])
|
|
|
|
lx=[-1,1];ly=[0,1];lz=[-1,1]
|
|
ly = param[:,1]
|
|
self.sldDx=sDx=Slider(axDx, 'dx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
|
|
self.sldDz=sDz=Slider(axDz, 'dz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.)
|
|
self.sldW =sW =Slider(axW, 'ang', -180., 180.0, valinit=0)
|
|
self.sldY =sY =Slider(axY, 'y', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.)
|
|
sDx.on_changed(self.update_dx_dz_w_y)
|
|
sDz.on_changed(self.update_dx_dz_w_y)
|
|
sW.on_changed(self.update_dx_dz_w_y)
|
|
sY.on_changed(self.update_dx_dz_w_y)
|
|
|
|
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
p=np.ndarray((param.shape[0], 3))
|
|
for i in range(2):
|
|
(z_i, y_i, x_i, r_i, phi_i)=param[i]
|
|
p[i,0]=x_i+r_i*np.sin(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
|
|
p[i,1]=y_i # y= y_i
|
|
p[i,2]=z_i+r_i*np.cos(phi_i) # z= z_i+r_i*sin(phi_i*w)
|
|
ofs=(p[1]+p[0])/2. # = center of the cristal
|
|
print 'p, ofs',p,ofs
|
|
|
|
m=Trf.trans(0,0,0); self.hOrig=self.pltOrig(m)
|
|
hCrist,pt=self.pltCrist(cx=-ofs[0],fy=-ofs[1],cz=-ofs[2])
|
|
|
|
self.hCrist=hCrist;self.fig=fig
|
|
plt.show()
|
|
|
|
|
|
def update_dx_dz_w_y(self,val):
|
|
dx = self.sldDx.val
|
|
dz = self.sldDz.val
|
|
w = self.sldW.val
|
|
y = self.sldY.val
|
|
w=w*d2r
|
|
(cx,cz,w,fy)=self.inv_transform(dx,dz,w,y)
|
|
#print (cx,cz,w,fy)
|
|
self.hCrist,pt=self.pltCrist(-fy,-cx,-cz,w,self.hCrist)
|
|
self.fig.canvas.draw_idle()
|
|
|
|
|
|
def fwd_transform(self,cx,cz,w,fy):
|
|
#cx,cy: coarse stage
|
|
#input: cx,cz,w,fy
|
|
#output: dx,dz,w,y
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
|
|
param=self.param
|
|
p=np.ndarray((param.shape[0], 3))
|
|
for i in range(2):
|
|
(z_i, y_i, x_i, r_i, phi_i)=param[i]
|
|
p[i,0]=x_i+r_i*np.sin(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
|
|
p[i,1]=y_i # y= y_i
|
|
p[i,2]=z_i+r_i*np.cos(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
|
|
v=p[1]-p[0]
|
|
#for y = 0..1:
|
|
#v=v*y
|
|
#for y = y_0..y_1:
|
|
y_0=param[0,1];y_1=param[1,1];v=v*(fy-y_0)/(y_1-y_0)
|
|
|
|
v=v+p[0]
|
|
dx=cx-v[0]
|
|
dz=cz-v[2]
|
|
y=v[1]
|
|
res=(dx,dz,w,y)
|
|
return res
|
|
|
|
|
|
def inv_transform(self,dx,dz,w,y):
|
|
#input: dx,dz,w,y
|
|
#output: cx,cz,w,fy
|
|
#dx,dy: deviation from cristal center line
|
|
param=self.param
|
|
p=np.ndarray((param.shape[0], 3))
|
|
for i in range(2):
|
|
(z_i, y_i, x_i, r_i, phi_i)=param[i]
|
|
p[i,0]=x_i+r_i*np.sin(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
|
|
p[i,1]=y_i # y= y_i
|
|
p[i,2]=z_i+r_i*np.cos(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
|
|
v=p[1]-p[0]
|
|
#for y = 0..1:
|
|
#v=v*y
|
|
#for y = y_0..y_1:
|
|
y_0=param[0,1];y_1=param[1,1];v=v*(y-y_0)/(y_1-y_0)
|
|
|
|
v=v+p[0]
|
|
cx=dx+v[0]
|
|
cz=dz+v[2]
|
|
fy=v[1]
|
|
res=(cx,cz,w,fy)
|
|
return res
|
|
|
|
|
|
def calcParam(self):
|
|
n = 3.;
|
|
per = 1.;
|
|
w = 2 * np.pi * per / n * np.arange(n)
|
|
p = ((2.3, .71, 4.12, 10.6 * d2r),(6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi)
|
|
self.param = param = np.ndarray((len(p), 5))
|
|
z = 14.5 # fix z position
|
|
for i in range(2):
|
|
(y, bias, ampl, phi) = p[i]
|
|
x = ampl * np.cos(w + phi) + bias
|
|
print('yMeas_%d=' % i + str(y) + ' xMeas_%d=' % i + str(x))
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
param[i, 0] = z
|
|
param[i, 1] = y
|
|
param[i, 2:] = HelicalScan.meas_rot_ctr(x) # (bias,ampl,phase)
|
|
(bias, ampl, phase) = param[i][2:]
|
|
print param
|
|
|
|
|
|
def pltOrig(self,m,h=None):
|
|
ax=self.ax
|
|
# m is a 4x4 matrix. the transformed matrix
|
|
r=m[:3,0] #1st
|
|
g=m[:3,1] #2nd
|
|
b=m[:3,2] #3rd
|
|
o=m[:3,3] #origin
|
|
if h is None:
|
|
hr=ax.plot((o[2],o[2]+r[2]), (o[0],o[0]+r[0]), (o[1],o[1]+r[1]), 'r')
|
|
hg=ax.plot((o[2],o[2]+g[2]), (o[0],o[0]+g[0]), (o[1],o[1]+g[1]), 'g')
|
|
hb=ax.plot((o[2],o[2]+b[2]), (o[0],o[0]+b[0]), (o[1],o[1]+b[1]), 'b')
|
|
return hr + hg + hb # this is a list
|
|
else:
|
|
hr, hg, hb = h
|
|
hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0]))
|
|
hr.set_3d_properties((o[1], o[1] + r[1]))
|
|
hg.set_data((o[2], o[2] + g[2]), (o[0], o[0] + g[0]))
|
|
hg.set_3d_properties((o[1], o[1] + g[1]))
|
|
hb.set_data((o[2], o[2] + b[2]), (o[0], o[0] + b[0]))
|
|
hb.set_3d_properties((o[1], o[1] + b[1]))
|
|
return h
|
|
|
|
|
|
def pltCrist(self,fy=0,cx=0,cz=0,w=0,h=None):
|
|
#h are the handles
|
|
if h is None:
|
|
h=[] #handels
|
|
ax=self.ax
|
|
param=self.param
|
|
pt = np.ndarray((4, 3))
|
|
for i in range(2):
|
|
(z, y, x, r, phi) = param[i]
|
|
x+=cx;y+=fy;z+=cz;phi+=w
|
|
pt[i] = (x, y, z)
|
|
pt[i + 2] = (x + r * np.sin(phi), y, z + r * np.cos(phi))
|
|
obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3))
|
|
h1=ax.add_patch(obj)
|
|
h2=plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
|
|
h.append(obj)
|
|
#hs=ax.scatter(pt[:, 2], pt[:, 0], pt[:, 1])
|
|
hs=ax.plot(pt[:, 2], pt[:, 0], pt[:, 1],'.')
|
|
hp=ax.plot(pt[2:, 2], pt[2:, 0], pt[2:, 1])
|
|
h+=(hs[0],hp[0])
|
|
else:
|
|
ax=self.ax
|
|
param=self.param
|
|
pt = np.ndarray((4, 3))
|
|
for i in range(2):
|
|
(z, y, x, r, phi) = param[i]
|
|
x+=cx;y+=fy;z+=cz;phi+=w
|
|
pt[i] = (x, y, z)
|
|
pt[i + 2] = (x + r * np.sin(phi), y, z + r * np.cos(phi))
|
|
h[i].remove()
|
|
obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('r', alpha=0.3))
|
|
ax.add_patch(obj)
|
|
plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
|
|
h[i]=obj
|
|
h[2].set_data(pt[:, 2], pt[:, 0])#, pt[:, 1]))
|
|
h[2].set_3d_properties(pt[:, 1])
|
|
|
|
h[3].set_data(pt[2:, 2], pt[2:, 0])#, pt[:, 1]))
|
|
h[3].set_3d_properties(pt[2:, 1])
|
|
#hr.set_data((o[2], o[2] + r[2]), (o[0], o[0] + r[0]))
|
|
#hr.set_3d_properties((o[1], o[1] + r[1]))
|
|
|
|
#h[3].set_3d_properties(zs=pt[:, 1]))
|
|
#h[4].set_data((pt[2:, 2], pt[2:, 0]))#, pt[2:, 1]))
|
|
#hp=ax.plot(pt[2:, 2], pt[2:, 0], pt[2:, 1], label='zs=0, zdir=z')
|
|
#h+=(hs,hp[0])
|
|
return (h,pt)
|
|
|
|
|
|
@staticmethod
|
|
def meas_rot_ctr(y,per=1):
|
|
# find the amplitude bias and phase of an equidistant sampled sinus
|
|
# it needs at least 3 measurements e.g. at 0,120 240 deg or 0 90 180 270 deg
|
|
# per is the number of persiods, default is 1 period =360 deg
|
|
n=len(y)
|
|
f = np.fft.fft(y)
|
|
idx=int(per)
|
|
bias=np.absolute(f[0]/n)
|
|
phase=np.angle(f[idx])
|
|
ampl=np.absolute(f[idx])*2/n
|
|
return (bias,ampl,phase)
|
|
|
|
|
|
def axSetCenter(self,v,l):
|
|
ax=self.ax
|
|
#v=center vector, l= length of each axis
|
|
l2=l/2.
|
|
ax.set_xlim(v[2]-l2, v[2]+l2);
|
|
ax.set_ylim(v[0]-l2, v[0]+l2);
|
|
ax.set_zlim(v[1]-l2, v[1]+l2)
|
|
|
|
|
|
def gen_coord_trf_code(self,file=None,host=None):
|
|
param=self.param
|
|
prg = []
|
|
prg.append('''
|
|
// Set the motors as inverse kinematic axes in CS 1
|
|
//motors CX CZ RY FY
|
|
// 7 8 1 2
|
|
|
|
&1
|
|
#7->I
|
|
#8->I
|
|
#1->I
|
|
#2->I
|
|
|
|
open forward
|
|
define(qCX='L7', qCZ='L8', qW='L1', qFY='L2')
|
|
define(DX='C6', DZ='C8', W='C1', Y='C7')
|
|
//coord X Z B Y
|
|
define(p0_x='L10', p0_y='L11', p0_z='L12')
|
|
define(p1_x='L13', p1_y='L14', p1_z='L15')
|
|
define(f='L16')
|
|
|
|
send 1"forward kinematic\\n"''')
|
|
for i in range(2):
|
|
#https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
|
|
l=[j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
|
|
prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)"%tuple(l))
|
|
prg.append('''
|
|
p0_x=x_0+r_0*sin(phi_0+W)
|
|
p1_x=x_1+r_1*sin(phi_1+W)
|
|
p0_y=y_0
|
|
p1_y=y_1
|
|
p0_z=z_0+r_0*cos(phi_0+W)
|
|
p1_z=z_1+r_1*cos(phi_0+W)
|
|
|
|
f=(qFY-y_0)/(y_1-y_0)
|
|
p0_x=p0_x+f*(p1_x-p0_x)
|
|
p0_y=p0_y+f*(p1_y-p0_y)
|
|
p0_z=p0_z+f*(p1_z-p0_z)
|
|
DX=qCX-p0_x
|
|
DZ=qCZ-p0_z
|
|
Y=qFY
|
|
W=qW
|
|
|
|
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
|
|
close
|
|
''')
|
|
|
|
prg.append('''
|
|
open inverse
|
|
define(DX='C6', DZ='C8', W='C1', Y='C7')
|
|
//coord X Z B Y
|
|
//D0 is set to $000001c2
|
|
define(qCX='L7', qCZ='L8', qW='L1', qFY='L2')
|
|
define(p0_x='L10', p0_y='L11', p0_z='L12')
|
|
define(p1_x='L13', p1_y='L14', p1_z='L15')
|
|
define(f='L16')
|
|
|
|
send 1"inverse kinematic\\n"''')
|
|
for i in range(2):
|
|
# https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
|
|
l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
|
|
prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)" % tuple(l))
|
|
prg.append('''
|
|
p0_x=x_0+r_0*sin(phi_0+W)
|
|
p1_x=x_1+r_1*sin(phi_1+W)
|
|
p0_y=y_0
|
|
p1_y=y_1
|
|
p0_z=z_0+r_0*cos(phi_0+W)
|
|
p1_z=z_1+r_1*cos(phi_0+W)
|
|
|
|
f=(qFY-y_0)/(y_1-y_0)
|
|
p0_x=p0_x+f*(p1_x-p0_x)
|
|
p0_y=p0_y+f*(p1_y-p0_y)
|
|
p0_z=p0_z+f*(p1_z-p0_z)
|
|
qCX=DX+p0_x
|
|
qCZ=DZ+p0_z
|
|
qFY=Y
|
|
qW=W
|
|
|
|
close
|
|
''')
|
|
|
|
if self.args.verbose & 4:
|
|
for ln in prg:
|
|
print(ln)
|
|
if file is not None:
|
|
fh = open(file, 'w')
|
|
fh.write('\n'.join(prg))
|
|
fh.close()
|
|
if host is not None:
|
|
cmd = '/home/zamofing_t/scripts/gpasciiCommander --host ' + host + ' ' + file
|
|
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
|
|
|
|
def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs):
|
|
'''
|
|
kwargs:
|
|
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
|
|
pt2pt_time : time to move from one point to the next point
|
|
'''
|
|
prg=[]
|
|
acq_per=kwargs.get('acq_per',1)
|
|
gather={"MaxSamples":1000000, "Period":acq_per}
|
|
#Sys.ServoPeriod is dependent of !common() macro
|
|
ServoPeriod= .2 #0.2ms
|
|
#ServoPeriod = .05
|
|
self.meta = {'timebase': ServoPeriod*gather['Period']}
|
|
#channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
|
|
channels=["Motor[7].ActPos","Motor[8].ActPos","Motor[1].ActPos","Motor[2].ActPos"]
|
|
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'%(prgId))
|
|
# this uses Coord[1].Tm and limits with MaxSpeed
|
|
if mode==-1: #### jog a 10mm square
|
|
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('jog2:10000')
|
|
prg.append('dwell 100')
|
|
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: #### linear motion
|
|
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: #### pvt motion
|
|
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.
|
|
try:
|
|
cnt=kwargs['cnt'] #move path multiple times
|
|
except KeyError:
|
|
cnt=1
|
|
|
|
try:
|
|
pt=self.ptsCorr
|
|
except AttributeError:
|
|
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')
|
|
if cnt>1:
|
|
prg.append('P100=%d'%cnt)
|
|
prg.append('N100:')
|
|
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)]))
|
|
if cnt>1:
|
|
prg.append('dwell 10')
|
|
prg.append('P100=P100-1')
|
|
prg.append('if(P100>0)')
|
|
prg.append('{')
|
|
prg.append(' linear abs')
|
|
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
|
|
prg.append('dwell 100')
|
|
prg.append('goto 100')
|
|
prg.append('}')
|
|
else:
|
|
prg.append('dwell 1000')
|
|
prg.append('Gather.Enable=0')
|
|
elif mode==2: #### spline motion
|
|
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.
|
|
pos=self.points
|
|
pcor=np.ndarray(pos.shape,dtype=pos.dtype);pcor[:]=np.NaN
|
|
pcor[(0,-1),:]=pos[(0,-1),:]
|
|
pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6.
|
|
#pcor=pos
|
|
prg.append(' linear abs')
|
|
prg.append('X(%g) Y(%g)' % tuple(pcor[0, :]))
|
|
prg.append('dwell 10')
|
|
prg.append('Gather.Enable=2')
|
|
prg.append(' spline%g abs'%pt2pt_time) #100ms to next position
|
|
for idx in range(pcor.shape[0]):
|
|
prg.append('X%g Y%g'%tuple(pcor[idx,:]))
|
|
prg.append('dwell 100')
|
|
prg.append('Gather.Enable=0')
|
|
|
|
prg.append('close')
|
|
prg.append('&1\nb%dr\n'%prgId)
|
|
if self.args.verbose & 4:
|
|
for ln in prg:
|
|
print(ln)
|
|
|
|
if file is not None:
|
|
fh=open(file,'w')
|
|
fh.write('\n'.join(prg))
|
|
fh.close()
|
|
if host is not None:
|
|
cmd ='gpasciiCommander --host '+host+' '+ file
|
|
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()
|
|
self.prg=prg
|
|
|
|
|
|
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=('-n',
|
|
'-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('-n', '--dryrun', action='store_true', help='dryrun to stdout')
|
|
parser.add_option('--xy', action='store_true', help='sort x,y instead y,x')
|
|
parser.add_option('--cfg', help='config file containing json configuration structure')
|
|
|
|
(args, other)=parser.parse_args()
|
|
args.other=other
|
|
|
|
hs=HelicalScan(args)
|
|
#hs.sequencer()
|
|
hs.args.verbose=255;hs.calcParam();hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485')
|
|
#return
|
|
hs.test_find_rot_ctr()
|
|
hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
|
|
hs.test_coord_trf()
|
|
hs.interactive_cx_cz_w_fy()
|
|
hs.interactive_dx_dz_w_y()
|
|
|
|
|
|
#------------------ Main Code ----------------------------------
|
|
#ssh_test()
|
|
ret=parse_args()
|
|
exit(ret)
|