1217 lines
40 KiB
Python
Executable File
1217 lines
40 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
|
|
|
|
Gather motor order
|
|
"Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos
|
|
motors CX CZ RY FY
|
|
4 5 3 1
|
|
Mot 1: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step
|
|
Mot 2: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step
|
|
Mot 3: Rotation stage LS Mecapion MDM-DC06DNC0H 32 poles = 1 rev = 16*2048=32768 phase_step
|
|
Mot 4: Stage X Stada Stepper 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
|
|
Mot 5: Stage Z Stada Stepper 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
|
|
Enc 6: Interferometer Y
|
|
Enc 7: Interferometer X
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
verbose bits:
|
|
#1 basic info
|
|
#2 plot sorting steps
|
|
4 list program
|
|
#4 upload progress
|
|
#8 plot gather path
|
|
|
|
|
|
|
|
'''
|
|
|
|
import os, sys, json,re
|
|
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 *
|
|
import telnetlib
|
|
|
|
sys.path.insert(0,os.path.expanduser('~/Documents/prj/SwissFEL/PBTools/'))
|
|
#sys.path.insert(0,'/sf/bernina/config/swissmx/zamofing_t')
|
|
#sys.path.insert(0,'/sf/bernina/config/swissmx/zamofing_t/pbtools/misc/')
|
|
from pbtools.misc.pp_comm import PPComm
|
|
from pbtools.misc.gather import Gather
|
|
from MXMotion import MotionBase
|
|
|
|
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 HelicalScanGui():
|
|
def __init__(self,helicalScan):
|
|
self.helScn=helicalScan
|
|
|
|
def show_vel(self):
|
|
rec=self.helScn.rec
|
|
fig = plt.figure()
|
|
#y = np.diff(rec[:, 2])
|
|
y = np.diff(rec,axis=0)
|
|
mx=y.max(0)
|
|
mn=y.min(0)
|
|
scl=np.maximum(mx,-mn)
|
|
scl=np.where(scl==0, 1, scl) # replace 0 by 1
|
|
y/=scl
|
|
x = np.arange(0, y.shape[0]);
|
|
x= x.reshape(-1,1).dot(np.ones((1,y.shape[1])))
|
|
lbl=('cx','cz','w','fy')
|
|
#plt.plot(x, y,label=lbl)
|
|
for i in range(y.shape[1]):
|
|
plt.plot(x[:,i], y[:,i],label=lbl[i])
|
|
|
|
plt.legend()
|
|
|
|
def show_pos(self):
|
|
rec=self.helScn.rec
|
|
y = rec
|
|
#plt.ion()
|
|
fig = plt.figure(figsize=(20,6))
|
|
c='bgrc'
|
|
lbl=('cx','cz','w','fy')
|
|
dx=.25/len(lbl)
|
|
for i in range(rec.shape[1]):
|
|
if i==0:
|
|
ax = fig.add_axes([.2, .05, .75, .90])
|
|
axl =[ax,]
|
|
else:
|
|
ax = fig.add_axes(axl[0].get_position(), sharex=ax)
|
|
ax.spines['top'].set_visible(False)
|
|
ax.spines['bottom'].set_visible(False)
|
|
ax.xaxis.set_visible(False)
|
|
ax.patch.set_visible(False)
|
|
ax.spines['left'].set_position(('axes', -dx*i))
|
|
axl.append(ax)
|
|
ax.set_ylabel(lbl[i], color=c[i])
|
|
ax.tick_params(axis='y', colors=c[i])
|
|
|
|
x = np.arange(0, y.shape[0]);
|
|
h=[]
|
|
for i in range(rec.shape[1]):
|
|
h+=axl[i].plot(x, y[:,i],c[i],label=lbl[i])
|
|
plt.legend(handles=h)
|
|
cid = fig.canvas.mpl_connect('motion_notify_event', self.onmove)
|
|
fig.obj=self
|
|
fig.data=y
|
|
|
|
@staticmethod
|
|
def onmove(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
|
|
data = event.canvas.figure.data
|
|
if(event.xdata):
|
|
idx=round(event.xdata)
|
|
msg='%d: cx:%.3f cz:%.3f w:%.1f fy:%.3f'%((idx,)+tuple(data[idx,:]))
|
|
#print msg
|
|
event.canvas.toolbar.set_message(msg)
|
|
|
|
def interactive_cx_cz_w_fy(self,manip=False):
|
|
'''interactive shows a cristal with sliders for cx,cz,w,fy
|
|
self.helScn.param is used a cristal parameters
|
|
THE AXES ARE RELABELED !
|
|
The generated members are:
|
|
self.fig : handle to figure
|
|
self.hCrist : handle to cristal object
|
|
self.hOrig : handle to xyz-origin cross (xyz-length= 1/5 height of cristal
|
|
self.manip : True: moves the origin cross, False moves the Cristal
|
|
'''
|
|
param=self.helScn.param
|
|
self.fig=fig=plt.figure()
|
|
self.manip=manip
|
|
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=-170)
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
ctr=param[:,0:3].mean(0)[::-1]
|
|
l=max(2*param[:,3].max(),param[:,1].ptp()) #max of diameter and y peaktopeak
|
|
self.scale=l #scale is the length of a cube were the pltCrist object fits into
|
|
self.axSetCenter((0,0,0),l)
|
|
|
|
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])
|
|
#lz=ax.get_xlim()
|
|
#lx=ax.get_ylim() #x-450 -> center==0
|
|
#ly=ax.get_zlim()
|
|
ly = param[::-1,1]
|
|
x0=param[:, 2].mean()
|
|
lx=(-l/2+x0,l/2+x0)
|
|
z0=param[:, 0].mean()
|
|
lz=(-l/2+z0,l/2+z0)
|
|
|
|
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)
|
|
|
|
self.update_cx_cz_w_fy()
|
|
#self.pltCrist(0,0,0,0)
|
|
|
|
# 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
|
|
|
|
ofs=(0,0,0)
|
|
m=Trf.trans(*ofs); self.pltOrig(m)
|
|
plt.show()
|
|
|
|
def update_cx_cz_w_fy(self,val=None):
|
|
cx = self.sldCx.val
|
|
cz = self.sldCz.val
|
|
w = self.sldW.val
|
|
fy = self.sldFy.val
|
|
if self.manip:
|
|
param = self.helScn.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)
|
|
m = Trf.trans(cx,fy,cz)
|
|
m= m.dot(Trf.rotY(w*d2r))
|
|
self.pltOrig(m)
|
|
else:
|
|
w=w*d2r
|
|
self.pltCrist(-cx, -cz, w, -fy)
|
|
#self.pltCrist(cx,cz,w*d2r,fy)
|
|
self.fig.canvas.draw_idle()
|
|
|
|
def interactive_dx_dz_w_y(self):
|
|
param=self.helScn.param
|
|
self.fig=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)
|
|
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
l=max(2*param[:,3].max(),param[:,1].ptp()) #max of diameter and y peaktopeak
|
|
self.scale=l #scale is the length of a cube were the pltCrist object fits into
|
|
ctr=(0,0,0)
|
|
self.axSetCenter(ctr,param[0,3]+param[1,3])
|
|
|
|
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])
|
|
|
|
lz=ax.get_xlim()
|
|
lx=ax.get_ylim()
|
|
ly = param[::-1,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.pltOrig(m)
|
|
self.pltCrist(cx=-ofs[0],cz=-ofs[2],w=0,fy=-ofs[1])
|
|
plt.show()
|
|
|
|
def update_dx_dz_w_y(self,val=None):
|
|
print(val)
|
|
helScn=self.helScn
|
|
dx = self.sldDx.val
|
|
dz = self.sldDz.val
|
|
w = self.sldW.val
|
|
y = self.sldY.val
|
|
w=w*d2r
|
|
(cx,cz,w,fy)=helScn.inv_transform(dx,dz,w,y)
|
|
#print (cx,cz,w,fy)
|
|
self.pltCrist(-cx,-cz,w,-fy)
|
|
self.fig.canvas.draw_idle()
|
|
|
|
def interactive_anim(self):
|
|
param=self.helScn.param
|
|
rec=self.helScn.rec
|
|
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)
|
|
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
ctr=(0,0,0)
|
|
self.axSetCenter(ctr,param[0,3]+param[1,3])
|
|
|
|
axFrm=plt.axes([0.1, 0.01, 0.8, 0.02])
|
|
|
|
lx=[-1,1];ly=[0,1];lz=[-1,1]
|
|
ly = param[:,1]
|
|
self.sldFrm=sFrm=Slider(axFrm, 'frm', 0, rec.shape[0]-1, valinit=0)
|
|
sFrm.on_changed(self.update_anim)
|
|
m=Trf.trans(0,0,0); self.pltOrig(m)
|
|
self.hCrist=None
|
|
self.fig=fig
|
|
|
|
animCnt=100
|
|
self.step=rec.shape[0]/animCnt
|
|
#a = anim.FuncAnimation(fig, self.anim_gather_data, animCnt, fargs=(), interval=20, repeat=False, blit=False)
|
|
self.update_anim(0)
|
|
plt.show()
|
|
pass
|
|
|
|
def update_anim(self,frm):
|
|
print(frm)
|
|
rec=self.helScn.rec
|
|
(cx, cz, w, fy)=rec[int(frm),:]
|
|
#data/=. #scale from um to mm
|
|
w*=d2r/1000 # scale from deg to rad
|
|
#print (cx,cz,w,fy)
|
|
self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist)
|
|
self.fig.canvas.draw_idle()
|
|
|
|
def anim_gather_data(self,idx):
|
|
rec=self.helScn.rec
|
|
(cx, cz, w, fy)=rec[int(idx*self.step),:]
|
|
w*=d2r/1000 # scale from deg to rad
|
|
self.hCrist,pt=self.pltCrist(-cx,-cz,w,-fy,self.hCrist)
|
|
#data=self.rec[int(idx*self.step),:]
|
|
#self.hCrist,pt=self.pltCrist(*data,h=self.hCrist)
|
|
|
|
def pltOrig(self,m):
|
|
'''plots a xyz axes in rgb colors that shows the transformation matrix m
|
|
if h is not none, the handles are the existing object and is modified
|
|
m is a 4x4 matrix. the transformed matrix
|
|
'''
|
|
ax=self.ax
|
|
idx=(2,0,1)
|
|
r=m[idx,0]*self.scale/3.#1st
|
|
g=m[idx,1]*self.scale/3.#2nd
|
|
b=m[idx,2]*self.scale/3.#3rd
|
|
o=m[idx,3]*self.scale/3.#origin
|
|
lines = np.ndarray((3, 2, 3)) # numlines, points, xyz
|
|
lines[:, 0, :] = o
|
|
lines[0, 1, :] = o + r
|
|
lines[1, 1, :] = o + g
|
|
lines[2, 1, :] = o + b
|
|
|
|
try:
|
|
h=self.hOrig
|
|
except AttributeError:
|
|
lseg = tuple(lines)
|
|
col=('r','g','b')
|
|
h = plt3d.art3d.Line3DCollection(lseg, colors=col, linewidths=2) # , *args[argi:], **kwargs)
|
|
ax.add_collection(h)
|
|
self.hOrig=h
|
|
else:
|
|
h.set_segments(lines)
|
|
|
|
def pltCrist(self,cx,cz,w,fy):
|
|
#h are the handles
|
|
ax = self.ax
|
|
helScn = self.helScn
|
|
param = helScn.param
|
|
pt = np.ndarray((4, 3))
|
|
try:
|
|
h=self.hCrist
|
|
except AttributeError:
|
|
h=[] #handels
|
|
for i in range(2):
|
|
(z, y, x, r, phi) = param[i]
|
|
x+=cx;y+=fy;z+=cz;phi+=w
|
|
pt[i] = (z, x, y)
|
|
pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y)
|
|
obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('y', alpha=0.2))
|
|
h1=ax.add_patch(obj)
|
|
h2=plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
|
|
#print h1._segment3d
|
|
h.append(obj)
|
|
#hs=ax.scatter(pt[:, 2], pt[:, 0], pt[:, 1])
|
|
hs=ax.plot(pt[:, 0], pt[:, 1], pt[:, 2],'.')
|
|
hp=ax.plot(pt[2:, 0], pt[2:, 1], pt[2:, 2])
|
|
h+=(hs[0],hp[0])
|
|
if hasattr(helScn,'points'):
|
|
lines = self.get_meas_lines(pt,cx,cz,fy,w)
|
|
p=tuple(lines[:,0,:].T)
|
|
hl =plt3d.art3d.Line3D(*p,color='r',marker='.')#, *args[argi:], **kwargs)
|
|
ax.add_artist(hl);h.append(hl)
|
|
|
|
lseg=tuple(lines)
|
|
col=(mpl.colors.colorConverter.to_rgba('r'),)*len(lseg)
|
|
hlc=plt3d.art3d.Line3DCollection(lseg,colors=col)#, *args[argi:], **kwargs)
|
|
ax.add_collection(hlc);h.append(hlc)
|
|
self.hCrist=h
|
|
else:
|
|
for i in range(2):
|
|
(z, y, x, r, phi) = param[i]
|
|
x+=cx;y+=fy;z+=cz;phi+=w
|
|
pt[i] = (z, x, y)
|
|
pt[i + 2] = (z + r * np.cos(phi), x + r * np.sin(phi), y)
|
|
h[i].remove()
|
|
obj = mpl.patches.Circle((z, x), r, facecolor=mpl.colors.colorConverter.to_rgba('y', alpha=0.2))
|
|
ax.add_patch(obj)
|
|
plt3d.art3d.pathpatch_2d_to_3d(obj, z=y, zdir="z")
|
|
h[i]=obj
|
|
h[2].set_data(pt[:, 0], pt[:, 1])#, pt[:, 1]))
|
|
h[2].set_3d_properties(pt[:, 2])
|
|
|
|
h[3].set_data(pt[2:, 0], pt[2:, 1])#, pt[:, 1]))
|
|
h[3].set_3d_properties(pt[2:, 2])
|
|
if hasattr(helScn,'points'):
|
|
lines = self.get_meas_lines(pt,cx,cz,fy,w)
|
|
hl=h[4]
|
|
#hl.set_data(lines[:,0,0], lines[:,0,1]) # , pt[:, 1]))
|
|
#hl.set_3d_properties(lines[:,0,2])
|
|
p=tuple(lines[:,0,:].T)
|
|
hl._verts3d=p;hl.stale=True
|
|
|
|
hlc=h[5]
|
|
hlc.set_segments(lines)
|
|
|
|
return (h,pt)
|
|
|
|
def get_meas_lines(self,pt,cx,cz,fy,w):
|
|
param = self.helScn.param
|
|
pts = self.helScn.points
|
|
dx_ = pts[:, 0] # add 0.2 to test
|
|
dz_ = pts[:, 1] # add 0.2 to test
|
|
w_ = pts[:, 2] * (d2r / 1000.)
|
|
y_ = pts[:, 3]
|
|
|
|
# self.inv_transform(self, dx, dz, w, y):
|
|
|
|
f = (pts[:, 3] - param[0, 1]) / (param[1, 1] - param[0, 1])
|
|
lines = np.ndarray((pts.shape[0], 2, 3))
|
|
ofx=dx_*-np.sin(w-w_)+dz_*np.cos(w-w_) # 0.2=dx
|
|
ofy=dx_*np.cos(w-w_)+dz_*np.sin(w-w_)
|
|
lines[:, 0, 0] = pt[2, 0] + (pt[3, 0] - pt[2, 0]) * f +ofx # z data
|
|
lines[:, 0, 1] = pt[2, 1] + (pt[3, 1] - pt[2, 1]) * f +ofy # x data
|
|
lines[:, 0, 2] = pts[:, 3] + fy # y data
|
|
lines[:, 1, 0] = lines[:, 0, 0] + np.cos(w-w_)*.1
|
|
lines[:, 1, 1] = lines[:, 0, 1] + np.sin(w-w_)*.1
|
|
lines[:, 1, 2] = lines[:, 0, 2]
|
|
return lines
|
|
|
|
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)
|
|
|
|
|
|
class HelicalScanTests():
|
|
@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
|
|
|
|
@staticmethod
|
|
def test_coord_trf(helScn):
|
|
param = helScn.param
|
|
cx, cz, w, fy, = (0.2,0.3,0.1,0.4)
|
|
#cx, cz, w, fy, = (10.,20,3.,40)
|
|
print('input : cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy))
|
|
(dx,dz,w,y) = helScn.fwd_transform(cx,cz,w,fy)
|
|
print('fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y))
|
|
(cx,cz,w,fy) = helScn.inv_transform(dx,dz,w,y)
|
|
print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy))
|
|
|
|
dx, dz, w, y, = (0.2,0.3,0.1,0.4)
|
|
#dx, dz, w, y, = (10.,20,3.,40)
|
|
print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y))
|
|
(cx,cz,w,fy) = helScn.inv_transform(dx,dz,w,y)
|
|
print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy))
|
|
(dx,dz,w,y) = helScn.fwd_transform(cx,cz,w,fy)
|
|
print('fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y))
|
|
|
|
@staticmethod
|
|
def mpl_test():
|
|
plt.ion()
|
|
fig = plt.figure()
|
|
ax = fig.add_axes([.2, .05, .75, .90])
|
|
x=np.arange(0,2*np.pi,.1)
|
|
y=np.sin(x)
|
|
h=ax.plot(x,y)
|
|
# ax.set_position([.2,.05,.75,.90])
|
|
#ax2 = ax.twinx()
|
|
ax2 = fig.add_axes([.2, .05, .75, .90],sharex=ax)
|
|
ax2.set_position([.1,.05,.75,.90])
|
|
|
|
ax2.spines['top'].set_visible(False)
|
|
ax2.spines['bottom'].set_visible(False)
|
|
|
|
ax2.xaxis.set_visible(False)
|
|
ax2.patch.set_visible(False)
|
|
|
|
y2=y+.1*np.random.random(y.shape)
|
|
h+=ax2.plot(x,y2,'g')
|
|
ax2.set_position(ax.get_position())
|
|
ax2.set_ylabel('mylbl', color='r')
|
|
ax2.tick_params(axis='y', colors='b')
|
|
ax2.spines['left'].set_position(('axes',-.1))
|
|
plt.legend(handels=h)
|
|
pass
|
|
|
|
def calcParamSim(helScn):
|
|
#simulated test values
|
|
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)
|
|
p = ((2.3, -100., 10, 10. * d2r),(6.2, 100., 10., -10. * d2r)) # (y, bias, ampl, phi)
|
|
helScn.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)
|
|
|
|
|
|
class HelicalScan(MotionBase):
|
|
def __init__(self,comm, gather, verbose):
|
|
MotionBase.__init__(self,comm, gather, verbose)
|
|
|
|
def load_rec(self,fn_npz='/tmp/helicalscan.npz'):
|
|
try:
|
|
fh=np.load(fn_npz)
|
|
except IOError as e:
|
|
sys.stderr.write('Unable to open File: '+fn_npz+'\n')
|
|
else:
|
|
pass
|
|
s='content of numpy file: '+fn_npz+'\n'
|
|
for k,v in fh.items():
|
|
s+=' '+k+': '+str(v.dtype)+' '+str(v.shape)+'\n'
|
|
setattr(self,k,v)
|
|
print(s)
|
|
|
|
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.cos(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.sin(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.cos(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.sin(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,x=((-241.,96.,-53.),(-162.,-293.,246.)),
|
|
y=(575.,175.),
|
|
z=((-1401.,-1401.,-1802.),(-1802.,-1303.,-1402.))):
|
|
# #1,3,4,5p
|
|
# point 1 0,120,240 deg
|
|
# 575.5 0 -241.5 -1401.3
|
|
# 575.5 120000 96.7 -1401.7
|
|
# 575.5 240000 -53.8 -1802.4
|
|
#
|
|
# point 2 0,120,240 deg
|
|
# 175.5 0 -162.3 -1802.5
|
|
# 175.5 120000 -293.2 -1303.7
|
|
# 175.5 240000 246.4 -1402.25
|
|
|
|
#real measured values:
|
|
#y : 2x1 array : y position were the measurements were taken
|
|
#x : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1]
|
|
#z : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1]
|
|
# the z value is only used to find a rought bias of z
|
|
|
|
assert(len(y)==2)
|
|
n = float(len(x[0])) #number of angles
|
|
per = 1 #number of rotations
|
|
self.param = param = np.ndarray((2,5))
|
|
for i in range(len(y)):
|
|
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
|
param[i, 0] = HelicalScan.meas_rot_ctr(z[i])[0]
|
|
param[i, 1] = y[i]
|
|
param[i, 2:] = HelicalScan.meas_rot_ctr(x[i]) # (bias,ampl,phase)
|
|
(bias, ampl, phase) = param[i][2:]
|
|
|
|
#check correctness of center:
|
|
w = 2 * np.pi * per / n * np.arange(n)
|
|
x_ = ampl * np.cos(w + phase) + bias
|
|
print(x_)
|
|
(dx,dz,w,y_) = (0,0,0,y[0])
|
|
print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y_))
|
|
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y_)
|
|
print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy))
|
|
(dx, dz, w, y_) = (0,0,0,y[1])
|
|
print('input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx, dz, w / d2r * 1000., y_))
|
|
(cx, cz, w, fy) = self.inv_transform(dx, dz, w, y_)
|
|
print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx, cz, w / d2r * 1000., fy))
|
|
|
|
print(param)
|
|
|
|
|
|
@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)
|
|
assert(np.imag(f[0])==0.)
|
|
bias=np.real(f[0]/n)
|
|
phase=np.angle(f[idx])
|
|
ampl=np.absolute(f[idx])*2/n
|
|
return (bias,ampl,phase)
|
|
|
|
def setup_gather(self,acq_per=1):
|
|
'''
|
|
setup the channels to gather
|
|
kwargs:
|
|
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
|
|
'''
|
|
comm=self.comm
|
|
gt=self.gather
|
|
gt.set_phasemode(False)
|
|
gt.set_address("Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos")
|
|
gt.set_property(MaxSamples=1000000, Period=acq_per)
|
|
ServoPeriod= .2 #0.2ms #Sys.ServoPeriod is dependent of !common() macro
|
|
#ServoPeriod=comm.gpascii.servo_period
|
|
self.meta = {'timebase': ServoPeriod*acq_per}
|
|
|
|
def setup_coord_trf(self,fnCrdTrf='/tmp/coordTrf.cfg'):
|
|
comm = self.comm
|
|
gpascii = comm.gpascii
|
|
param=self.param
|
|
prg = '''
|
|
// Set the motors as inverse kinematic axes in CS 1
|
|
//motors CX CZ RY FY
|
|
// 4 5 3 1
|
|
&1
|
|
a
|
|
#4->0
|
|
#5->0
|
|
#3->0
|
|
#1->0
|
|
|
|
#4->I
|
|
#5->I
|
|
#3->I
|
|
#1->I
|
|
'''
|
|
s = ('z', 'y', 'x', 'r', 'phi')
|
|
a = np.ones(param.shape[1], dtype=np.uint8).reshape(1, -1)
|
|
b = np.arange(param.shape[0], dtype=np.uint8).reshape(1, -1)
|
|
c = np.matmul(b.T, a).reshape(-1)
|
|
subsParam=dict(map(lambda k, i, v: (k + '_' + str(i), v), s * param.shape[0], c, param.reshape(-1)))
|
|
subsParam['d2r']=d2r/1000.
|
|
subsParam['r2d']=1000./d2r
|
|
|
|
|
|
subs={'qCX':'L4', 'qCZ':'L5', 'qW':'L3', 'qFY':'L1',
|
|
'DX':'C6', 'DZ':'C8', 'W':'C1', 'Y':'C7',
|
|
#coord X Z B Y
|
|
'p0_x':'L10', 'p0_y':'L11', 'p0_z':'L12',
|
|
'p1_x':'L13', 'p1_y':'L14', 'p1_z':'L15',
|
|
'scale':'L16'}
|
|
subs.update(subsParam)
|
|
|
|
prg+='''
|
|
open forward
|
|
send 1"fwd_inp(%f) %f %f %f %f\\n",D0,{qCX},{qCZ},{qW},{qFY}
|
|
{W}={qW}
|
|
{qW}={qW}*{d2r} //scale from 1000*deg to rad
|
|
|
|
{p0_x}={x_0}+{r_0}*cos({phi_0}+{qW})
|
|
{p1_x}={x_1}+{r_1}*cos({phi_1}+{qW})
|
|
{p0_y}={y_0}
|
|
{p1_y}={y_1}
|
|
{p0_z}={z_0}-{r_0}*sin({phi_0}+{qW})
|
|
{p1_z}={z_1}-{r_1}*sin({phi_1}+{qW})
|
|
|
|
{scale}=({qFY}-({y_0}))/({y_1}-({y_0}))
|
|
{p0_x}={p0_x}+{scale}*({p1_x}-{p0_x})
|
|
{p0_y}={p0_y}+{scale}*({p1_y}-{p0_y})
|
|
{p0_z}={p0_z}+{scale}*({p1_z}-{p0_z})
|
|
{DX}={qCX}-{p0_x}
|
|
{DZ}={qCZ}-{p0_z}
|
|
{Y}={qFY}
|
|
//send 1"fwd_res %f %f %f %f\\n",{DX},{DZ},{W},{Y}
|
|
//P1001+=1
|
|
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
|
|
close
|
|
'''.format(**subs)
|
|
|
|
#coord X Z B Y
|
|
subs={ 'DX':'C6' , 'DZ':'C8' , 'W':'C1', 'Y':'C7',
|
|
'vDX':'C38', 'vDZ':'C40', 'vW':'C33', 'vY':'C39',
|
|
#motor q4 q5 q3 q1
|
|
'qCX':'L4', 'qCZ':'L5', 'qW':'L3', 'qFY':'L1',
|
|
'vqCX':'R4', 'vqCZ':'R5', 'vqW':'R3', 'vqFY':'R1',
|
|
|
|
'p0_x':'L10', 'p0_y':'L11', 'p0_z':'L12',
|
|
'p1_x':'L13', 'p1_y':'L14', 'p1_z':'L15',
|
|
'p_x':'L16', 'p_y':'L17', 'p_z':'L18',
|
|
'sclY':'L19',
|
|
'scl':'L20'}
|
|
|
|
subs.update(subsParam)
|
|
|
|
prg+='''
|
|
open inverse
|
|
//if(D0>0)
|
|
// send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,{DX},{vDX},{DZ},{vDZ},{W},{vW},{Y},{vY}
|
|
//else
|
|
// send 1"inv_inp(%f) %f %f %f %f\\n",D0,{DX},{DZ},{W},{Y}
|
|
{qW}={W}
|
|
{W}={W}*{d2r} //scale from 1000*deg to rad
|
|
|
|
{p0_x}={x_0}+{r_0}*cos({phi_0}+{W})
|
|
{p1_x}={x_1}+{r_1}*cos({phi_1}+{W})
|
|
{p0_y}={y_0}
|
|
{p1_y}={y_1}
|
|
{p0_z}={z_0}-{r_0}*sin({phi_0}+{W})
|
|
{p1_z}={z_1}-{r_1}*sin({phi_1}+{W})
|
|
|
|
{sclY}=({Y}-({y_0}))/({y_1}-({y_0}))
|
|
{p_x}={p0_x}+{sclY}*({p1_x}-{p0_x})
|
|
{p_y}={p0_y}+{sclY}*({p1_y}-{p0_y})
|
|
{p_z}={p0_z}+{sclY}*({p1_z}-{p0_z})
|
|
{qCX}={DX}+{p_x}
|
|
{qCZ}={DZ}+{p_z}
|
|
{qFY}={Y}
|
|
if(D0>0)
|
|
{{ // calculate velocities for PVT motion
|
|
{vW}={vW}*{d2r} //scale from 1000*deg to rad
|
|
{p_x}={sclY}*({p1_x}-{p0_x})
|
|
{p_z}={sclY}*({p1_z}-{p0_z})
|
|
{vqFY}={vY}
|
|
{vqCX}={vDX} + ({p1_x}-{p0_x})/({p1_y}-{p0_y})*{vY} //+ vqW*p_z
|
|
{vqCZ}={vDZ} + ({p1_z}-{p0_z})/({p1_y}-{p0_y})*{vY} //+ vqW*p_x
|
|
//vqW=vqW+(vqCX+(p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+(vqCZ+(p1_z-p0_z)/(p1_y-p0_y)*vY)*p_x
|
|
{vqW}={vW}//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x
|
|
{vqW}={vqW}*{r2d} //scale from rad to 1000*deg
|
|
// send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",{qCX},{vqCX},{qCZ},{vqCZ},{qW},{vqW},{qFY},{vqFY}
|
|
}}
|
|
//else
|
|
// send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY}
|
|
//P1002+=1
|
|
close
|
|
'''.format(**subs)
|
|
|
|
# vqCX=vDX + (p1_x-p0_x)/(p1_y-p0_y)*vY + vqW*p_x
|
|
# vDX is in same direction, so add as it is
|
|
# (p1_x-p0_x)/(p1_y-p0_y)*vY velocity part in vqCX direction, when moving in vY
|
|
# vqW*p_x velocity part of the rotation (vqW is in rad)
|
|
|
|
gpascii.send_block('disable plc 0')
|
|
time.sleep(.5)
|
|
gpascii.send_block(prg)
|
|
if self.verbose & 4:
|
|
print(prg)
|
|
if fnCrdTrf is not None :
|
|
fh=open(fnCrdTrf,'w')
|
|
fh.write(prg)
|
|
fh.close()
|
|
time.sleep(.5)
|
|
gpascii.send_block('enable plc 0')
|
|
time.sleep(.5)
|
|
gpascii.send_block('#1..7j/')
|
|
|
|
def setup_motion(self,prgId=2,fnPrg='/tmp/prg.cfg',mode=0,**kwargs):
|
|
'''
|
|
kwargs:
|
|
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
|
|
mode=-1: test motion
|
|
mode=0: #linear motion with 100 ms break at each measurement
|
|
cnt : move path multiple times
|
|
cntVert : number of vertical measurements
|
|
cntHor : number of horizontal measurements
|
|
hRng : min, max horizontal boundaries
|
|
wRng : starting and ending angle
|
|
yRng : starting and ending height
|
|
mode=1: #PVT motion
|
|
pt2pt_time : time to move from one point to the next point
|
|
cnt : move path multiple times
|
|
cntVert : number of vertical measurements
|
|
cntHor : number of horizontal measurements
|
|
hRng : min, max horizontal boundaries
|
|
wRng : starting and ending angle
|
|
yRng : starting and ending height
|
|
'''
|
|
comm = self.comm
|
|
gpascii = comm.gpascii
|
|
param=self.param
|
|
prg=[]
|
|
acq_per=kwargs.get('acq_per',10)
|
|
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"]
|
|
# CX CZ W FY
|
|
channels=["Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos"]
|
|
prg.append('Gather.Enable=0')
|
|
prg.append('Gather.Items=%d'%len(channels))
|
|
for k,v in gather.items():
|
|
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))
|
|
prg.append(' P1000=0')
|
|
# this uses Coord[1].Tm and limits with MaxSpeed
|
|
|
|
#******** mode -1 ********
|
|
if mode==-1: #### jog all motors 10000um (or 10000 mdeg)
|
|
pos=np.array([[0,0,0,0],])
|
|
prg.append(' linear abs')
|
|
#prg.append('X(%g) Z(%g) B(%g) Y(%g)' % tuple(pos[0, :]))
|
|
prg.append(' jog1,2,7,8=0')
|
|
prg.append(' dwell 10')
|
|
prg.append(' Gather.Enable=2')
|
|
prg.append(' jog7:10000')
|
|
prg.append(' dwell 100')
|
|
prg.append(' jog8:10000')
|
|
prg.append(' dwell 100')
|
|
prg.append(' jog1:10000')
|
|
prg.append(' dwell 100')
|
|
prg.append(' jog2:10000')
|
|
prg.append(' dwell 100')
|
|
prg.append(' jog1,2,7,8=0')
|
|
prg.append(' dwell 100')
|
|
prg.append(' Gather.Enable=0')
|
|
|
|
# ******** mode 0 ********
|
|
elif mode==0: #### linear motion
|
|
#y=2.3 6.2 dx=0, dz=0 w=0..3600000 # 10 rev
|
|
cnt= kwargs.get('cnt', 1) #move path multiple times
|
|
cntVert = kwargs.get('cntVert', 12)
|
|
cntHor = kwargs.get('cntHor', 4)
|
|
hRng = kwargs.get('hRng', (-.2,.2))
|
|
wRng = kwargs.get('wRng', (0,360000))
|
|
yRng = kwargs.get('yRng', self.param[:,1])
|
|
|
|
numPt=cntVert*cntHor
|
|
pt=np.zeros((numPt,4))
|
|
if cntHor>1:
|
|
a=np.linspace(hRng[0],hRng[1],cntHor)
|
|
a=np.append(a,a[::-1])
|
|
a=np.tile(a,(cntVert+1)//2)
|
|
pt[:,0]= a[0:pt.shape[0]]
|
|
#pt[:,1]= np.linspace(0,.2,numPt) #dz
|
|
pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w
|
|
pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y
|
|
self.points=pt
|
|
#prg.append(' Coord[1].SegMoveTime=.05') #to calculate every 0.05 ms the inverse kinematics
|
|
prg.append(' nofrax') #needed to set all axis to use AltFeedRate
|
|
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
|
|
prg.append(' Coord[1].SegMoveTime=0') #turn off segmented mode
|
|
prg.append(' linear abs')
|
|
prg.append(' X%g Z%g B%g Y%g' % tuple(pt[0, :]))
|
|
prg.append(' dwell 100')
|
|
prg.append(' Gather.Enable=2')
|
|
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
|
|
prg.append(' Coord[1].SegMoveTime=0') #to calculate every 1 ms the inverse kinematics
|
|
prg.append(' P2000=0')
|
|
for i in range(pt.shape[0]):
|
|
prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :]))
|
|
#prg.append(' P2000=%d'%(i,))
|
|
prg.append(' dwell 10')
|
|
prg.append(' Gather.Enable=0')
|
|
|
|
# ******** mode 1 ********
|
|
elif mode==1: #### pvt motion
|
|
#y=2.3 6.2 dx=0, dz=0 w=0..3600000 # 10 rev
|
|
cnt= kwargs.get('cnt', 1) #move path multiple times
|
|
cntVert = kwargs.get('cntVert', 12)
|
|
cntHor = kwargs.get('cntHor', 4)
|
|
hRng = kwargs.get('hRng', (-.2,.2))
|
|
wRng = kwargs.get('wRng', (0,360000))
|
|
yRng = kwargs.get('yRng', self.param[:,1])
|
|
pt2pt_time = kwargs.get('pt2pt_time', 100)
|
|
smt = kwargs.get('smt', 1) # SegMoveTime, default = 1ms -> velocity calc not yet 100% correct (smt=0 not 100% working)
|
|
numPt=cntVert*cntHor
|
|
pt=np.zeros((numPt,4))
|
|
if cntHor>1:
|
|
a=np.linspace(hRng[0],hRng[1],cntHor)
|
|
a=np.append(a,a[::-1])
|
|
a=np.tile(a,(cntVert+1)//2)
|
|
pt[:,0]= a[0:pt.shape[0]]
|
|
#pt[:,1]= np.linspace(0,.2,numPt) #dz
|
|
pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w
|
|
pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y
|
|
self.points=pt
|
|
#pv is an array of dx,dz,w,y,vel_dx,vel_dz,vel_w,vel_y
|
|
pv=np.ndarray(shape=(pt.shape[0]+2,8),dtype=pt.dtype)
|
|
pv[:]=np.NaN
|
|
pv[ 0,(0,1,2,3)]=pt[0,:]
|
|
pv[ 1:-1,(0,1,2,3)]=pt
|
|
pv[ -1,(0,1,2,3)]=pt[-1,:]
|
|
pv[(0,0,0,0,-1,-1,-1,-1),(4,5,6,7,4,5,6,7)]=0
|
|
dist=pv[2:,(0,1,2,3)] - pv[:-2,(0,1,2,3)]
|
|
pv[ 1:-1,(4,5,6,7)] = 1000.*dist/(2.*pt2pt_time)
|
|
|
|
prg.append(' nofrax') #needed to set all axis to use AltFeedRate
|
|
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
|
|
prg.append(' Coord[1].SegMoveTime=0') #turn off segmented mode
|
|
prg.append(' linear abs')
|
|
prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0,1,2,3)]))
|
|
prg.append(' dwell 10')
|
|
prg.append(' Gather.Enable=2')
|
|
if cnt>1:
|
|
prg.append(' P100=%d'%cnt)
|
|
prg.append(' N100:')
|
|
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
|
|
prg.append(' Coord[1].SegMoveTime=%d'%smt) #to calculate every 1 ms the inverse kinematics
|
|
prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position
|
|
for idx in range(1,pv.shape[0]):
|
|
#prg.append(' P2000=%d'%idx)
|
|
prg.append(' X%g:%g Z%g:%g B%g:%g Y%g:%g' % tuple(pv[idx, (0,4,1,5,2,6,3,7)]))
|
|
#prg.append('Y%g:%g' % tuple(pv[idx, (5, 7)]))
|
|
#prg.append('B%g:%g' %(idx*1000,0))
|
|
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 Z%g B%g Y%g' % tuple(pv[0, (0, 1, 2, 3)]))
|
|
prg.append(' dwell 100')
|
|
prg.append(' goto 100')
|
|
prg.append(' }')
|
|
else:
|
|
prg.append(' dwell 1000')
|
|
prg.append(' Gather.Enable=0')
|
|
|
|
prg.append(' P1000=1')
|
|
prg.append('close')
|
|
#prg.append('&1\nb%dr\n'%prgId)
|
|
|
|
prg='\n'.join(prg) + '\n'
|
|
gpascii.send_block(prg)
|
|
if self.verbose & 4:
|
|
print(prg)
|
|
if fnPrg is not None :
|
|
fh=open(fnPrg,'w')
|
|
fh.write(prg)
|
|
fh.close()
|
|
|
|
def gather_upload(self,fnRec='/tmp/helicalscan.npz'):
|
|
gpascii=self.comm.gpascii
|
|
gt=self.gather
|
|
gt.wait_stopped(verbose=True)
|
|
self.rec=rec=gt.upload()
|
|
channels = ["Motor[4].HomePos", "Motor[5].HomePos", "Motor[3].HomePos", "Motor[1].HomePos"]
|
|
ofs = np.ndarray(len(channels))
|
|
for i, v in enumerate(channels):
|
|
ofs[i] = gpascii.get_variable(v,float)
|
|
rec -= ofs
|
|
if fnRec:
|
|
np.savez_compressed(fnRec, rec=rec, points=self.points, param=self.param, meta=self.meta)
|
|
|
|
|
|
if __name__=='__main__':
|
|
def run_test(args):
|
|
args.host=None
|
|
if args.host is None:
|
|
comm=gather=None
|
|
else:
|
|
comm = PPComm(host=args.host)
|
|
gather = Gather(comm)
|
|
gpascii = comm.gpascii
|
|
hs=HelicalScan(comm, gather, args.verbose)
|
|
hsg=HelicalScanGui(hs)
|
|
|
|
#hs.test_find_rot_ctr()
|
|
#hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
|
|
fn='/tmp/helicalscan'
|
|
hs.load_rec(fn+'.npz')
|
|
#hsg.interactive_anim()
|
|
|
|
#hs.param = np.ndarray((2,5))
|
|
#hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
|
|
#hs.param[1]=(15,4,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
|
|
|
|
#hsg.interactive_cx_cz_w_fy()
|
|
#while True: hsg.update_cx_cz_w_fy() #for debug purpose
|
|
|
|
|
|
hsg.interactive_dx_dz_w_y()
|
|
while True: hsg.update_dx_dz_w_y() #for debug purpose
|
|
|
|
return
|
|
|
|
#TODO: FE Digitizers PBPS117 timing not working!
|
|
|
|
#hs.calcParam()
|
|
|
|
#gpasci: #1,4,5p // y,-x ,-z
|
|
# 0deg 256.7 -762.5 -396.4
|
|
#120deg 258.5 731.7 -1896.9
|
|
#240deg 256.2 -1282.8 -2496.5
|
|
# 0deg 586.5 -1023.4 -696.8
|
|
# 120deg 574.8 619.8 -1797.5
|
|
# 240deg 580.0 -900.0 -2496.3
|
|
|
|
hs.calcParam(x = ((-762.5, 731.7, -1282.8), (-1023.4, 619.8, -900.0)),
|
|
y = (258.5,580.0),
|
|
z = (( -396.4,-1896.9,-2496.5),(-696.8,-1797.5,-2496.3)))
|
|
#&1p
|
|
#B0.3504637004225515 X5.101934282692127 Y586.5400000000002 Z96.49435276710267
|
|
|
|
|
|
#cpx X0 Z0 B0 Y258
|
|
#cpx X0 Z0 B120000 Y258
|
|
#cpx X0 Z0 B240000 Y258
|
|
#cpx X0 Z0 B0 Y580
|
|
#cpx X0 Z0 B120000 Y580
|
|
#cpx X0 Z0 B240000 Y580
|
|
|
|
#cpx X-100Z0B0Y258.5
|
|
|
|
|
|
|
|
|
|
#hs.calcParamSim()
|
|
#hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
|
|
#hs.param[1]=(15,4,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
|
|
#hs.param[0]=(-100, 100,0,50,0)#(z_i, y_i, x_i, r_i,phi_i)
|
|
#hs.param[1]=(-100,-100,0,70,0)#(z_i, y_i, x_i, r_i,phi_i)
|
|
|
|
#hs.test_coord_trf()
|
|
#hs.interactive_cx_cz_w_fy()
|
|
#hs.interactive_dx_dz_w_y()
|
|
|
|
#mode bits:
|
|
#0:1 config simulated motors
|
|
#1:2 config real motors
|
|
#2:4 config coord trf
|
|
mode=4#5#4#0
|
|
os.chdir(os.path.join(os.path.dirname(__file__),'../cfg'))
|
|
if mode&1:
|
|
hs.download(file='sim_8_motors.cfg')
|
|
if mode&2:
|
|
hs.download(['$$$***','!common()','!SAR-EXPMX1()','#1..7j/','enable plc 1','Motor[1].MaxSpeed=25','Motor[2].MaxSpeed=25'])
|
|
raw_input('press return when homed')
|
|
if mode&4:
|
|
hs.setup_coord_trf()
|
|
|
|
hs.setup_sync() # no sync at all
|
|
hs.setup_gather()
|
|
|
|
#hs.gen_prog(mode=-1)
|
|
#hs.gen_prog(mode=0,cntHor=1,cntVert=3,wRng=(120000,120000))
|
|
#hs.gen_prog(mode=0,cntHor=1)
|
|
#hs.gen_prog(mode=0)
|
|
#hs.gen_prog(mode=0,cntHor=1,cntVert=5,wRng=(120000,120000))
|
|
#hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100)
|
|
#hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100,smt=0)
|
|
#hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(0,360000))
|
|
#hs.gen_prog(mode=1,cntHor=1,cntVert=5,hRng=(-.3,.3),wRng=(0,360000),smt=1)
|
|
#hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-3,3),wRng=(120000,120000),smt=0)
|
|
#hs.gen_prog(mode=1,cntHor=3,cntVert=6,hRng=(-5,5),wRng=(00,120000),smt=0,pt2pt_time=10)
|
|
#hs.gen_prog(mode=0,cntHor=3,cntVert=10,hRng=(-5,5),wRng=(0,120000))
|
|
#hs.gen_prog(mode=0,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000))
|
|
#hs.gen_prog(mode=1,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000),smt=0,pt2pt_time=300)
|
|
hs.setup_motion(mode=1,cntHor=5,cntVert=15,hRng=(-150,150),wRng=(0,120000),smt=0,pt2pt_time=300)
|
|
#hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=40)
|
|
#hs.gen_prog(mode=1,cntHor=3,cntVert=20,hRng=(-5,5),wRng=(0,1200),smt=0,pt2pt_time=200)
|
|
#hs.gen_prog(mode=1, cntHor=2, cntVert=2, wRng=(0, 360000), smt=0)
|
|
#hs.gen_prog(mode=1)
|
|
#hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3))
|
|
#hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=10,cntHor=3,hRng=(-30,30),wRng=(0,36000),yRng=(-50,-100))
|
|
#hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-100,50),wRng=(000,10000),smt=0)
|
|
|
|
|
|
hs.run()
|
|
print('temporary wait that the program started')
|
|
time.sleep(5)#temporary wait that the program started
|
|
hs.gather_upload(fn+'.npz')
|
|
|
|
hs.load_rec(fn+'.npz')
|
|
hsg.show_pos()
|
|
hsg.show_vel()
|
|
hsg.interactive_anim()
|
|
#hs.show_vel(); plt.show()
|
|
#hs.show_pos(); plt.show()
|
|
|
|
|
|
|
|
|
|
|
|
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=255)
|
|
parser.add_option('--host', help='hostname', default='SAR-CPPM-EXPMX1')
|
|
|
|
(args, other)=parser.parse_args()
|
|
args.other=other
|
|
run_test(args)
|
|
return
|
|
|
|
#------------------ Main Code ----------------------------------
|
|
ret=parse_args()
|
|
exit(ret)
|