sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
zamofing_t
2019-01-10 16:48:08 +01:00
parent 3a08775210
commit 03bce16acc
2 changed files with 141 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

112
python/helicalscan.py
View File

@@ -710,35 +710,21 @@ class HelicalScan(MotionBase):
res=(cx,cz,w,fy) res=(cx,cz,w,fy)
return res return res
def calcParamFast(self,x=((-241.,96.),(-162.,-293.)), def calcParam1Pt(self,x=(-241.,-162.),
y=(575.,175.), y=(575.,175.),
z=((-1401.,-1401.),(-1802.,-1303.)), z=(-1401.,-1802.),
w=(1.2,1.4), w=1.2):
mode=1):
''' '''
the rotation center of the stage does not change for a new cristal. the rotation center of the stage does not change for a new cristal.
after the function calcParam was called once, after the function calcParam was called once,
this faster coordinate transformation can be used. this 1 point coordinate transformation can be used.
FOR SMALL ANGLES USE MODE==0. it needs 1 point at start and 1 point at end of the crystal
!!! THIS CODE IS NOT YET TESTED !!!
''' '''
#x: ((x_w0y0, x_w1y0),(x_w0y1, x_w1y1)
#y: lower and upper cristal point
#z: distance, similar to x
#w: start and end angle in radians
#mode 0:use x and z needs to define 1 point at start and 1 point at end
# 1:use x change with 2 angles needs to define 2 point at start and 2 point at end
#mode 0 uses:
#x: ((x_w0y0, None ),(None , x_w1y1)
#z: ((z_w0y0, None ),(None , z_w1y1)
#w: (w0,w1)
#mode 1 uses:
#x: ((x_w0y0, x_w1y0),(x_w0y1, x_w1y1)
#z: ((None, None ),(None , None )
#w: (w0,w1)
#x: x position at y0 and y1 : (x_y0, x_y1)
#y: lower and upper cristal point (y0, y1)
#z: x position at y0 and y1 : (z_y0, z_y1)
#w: stage angle in radians
# param[i]=(z_i, y_i, x_i, r_i,phi_i) # param[i]=(z_i, y_i, x_i, r_i,phi_i)
# z_i not changed # z_i not changed
@@ -747,41 +733,73 @@ class HelicalScan(MotionBase):
# r_i calculate # r_i calculate
# phi_i calculate # phi_i calculate
try:
param=self.param
except AttributeError as e:
raise AttributeError('calcParam must be called first')
for i in range(len(y)):
r_i =np.sqrt(x[i]**2+z[i]**2)
phi_i=np.arctan2(z[i],x[i])
param[i, 1]=y[i]
param[i, 3:]=(r_i,phi_i-w)
pass
def calcParam2Pt(self,x=((-241.,96.),(-162.,-293.)),
y=(575.,175.),
w=(1.2,1.4)):
'''
the rotation center of the stage does not change for a new cristal.
after the function calcParam was called once,
this 2 point coordinate transformation can be used.
it needs 2 point at start and 2 point at end of the crystal
'''
#x: ((x_w0y0, x_w1y0),(x_w0y1, x_w1y1)
#y: lower and upper cristal point (y0, y1)
#w: start and end stage angle in radians (w0,w1)
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
# z_i not changed
# y_i trivial
# x_i not changed
# r_i calculate
# phi_i calculate
try: try:
param=self.param param=self.param
except AttributeError as e: except AttributeError as e:
raise AttributeError('calcParam must be called first') raise AttributeError('calcParam must be called first')
if mode==0: for i in range(len(y)):
for i in range(len(y)): # param[i]=(z_i, y_i, x_i, r_i,phi_i)
# param[i]=(z_i, y_i, x_i, r_i,phi_i) r_i=np.sqrt(x[i]**2+z[i]**2)
r_i =np.sqrt(x[i]**2+z[i]**2) x0=x[i][0]-param[i,2]
phi_i=np.arctan2(z[i],x[i]) x1=x[i][1]-param[i,2]
param[i, 1]=y[i] ww=w[i]
param[i, 3:]=(r_i,phi_i) if x0>x1:
else: #mode==1: phi_i=np.arctan2(np.cos(ww)-x1/x0,np.sin(ww))
for i in range(len(y)): r_i =x0/np.cos(phi_i)
# param[i]=(z_i, y_i, x_i, r_i,phi_i) else:
r_i=np.sqrt(x[i]**2+z[i]**2) phi_i=np.arctan2(np.cos(ww)-x0/x1,np.sin(ww))
x0=x[i][0]-param[i,2] r_i =x1/np.cos(phi_i)
x1=x[i][1]-param[i,2] param[i, 1]=y[i]
ww=w[i] param[i, 3:]=(r_i,phi_i)
if x0>x1:
phi_i=np.arctan2(np.cos(ww)-x1/x0,np.sin(ww))
r_i =x0/np.cos(phi_i)
else:
phi_i=np.arctan2(np.cos(ww)-x0/x1,np.sin(ww))
r_i =x1/np.cos(phi_i)
param[i, 1]=y[i]
param[i, 3:]=(r_i,phi_i)
pass pass
def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)), def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)),
y=(575.,175.), y=(575.,175.),
z=((-1401.,-1401.,-1802.),(-1802.,-1303.,-1402.))): z=((-1401.,-1401.,-1802.),(-1802.,-1303.,-1402.))):
'''
calculates coordinate parameters out of measurements at
aequidistant angles (typically 0,120,240 deg)
if a needle tip is used to calibrate (only one y value) use:
x=(x_meas,x_meas), (x_meas is a nx1 array)
z=(x_meas,x_meas), (z_meas is a nx1 array)
y=(y_meas,x_meas+ofs), (y_meas is a value, ofs is any value>0 recommended 100.)
'''
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
#real measured values: #real measured values:
#y : 2x1 array : y position were the measurements were taken #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] #x : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1]

117
python/trajectory.py
View File

@@ -17,25 +17,56 @@ import numpy as np
import matplotlib as mpl import matplotlib as mpl
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
def gen_pvt(p,v,t,ts):
'''generates a pvt motion
p: position array
v: velocity array
t: time array
ts: servo cycle time
!!! it is assumed, that the time intervals are constant !!!
'''
return
pvt=np.ndarray(len(tt))*0
t[-1]/ts
tt1=np.arange(0,t[1]-t[0],ts)
for i in range(len(t)-1):
d=p[i]
c=v[i]
a=(-2*(p[i+1]-p[i]-v[i]*w)+w*(v[i+1]-v[i]))/w**3
b=(3*w*(p[i+1]-p[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3
pvt[i*n:(i+1)*n]=a*tt1**3+b*tt1**2+c*tt1+d
return pvt
w=40. # ms step between samples w=40. # ms step between samples
ts=.2 # sampling time ts=.2 # sampling time
x = np.arange(0, 400, w) n=int(w/ts)# servo cycle between samples
y=np.cos(x) k=8 #number of unique samples
xx = np.arange(0, 400, ts) t = np.arange(0, w*(k+1), w) #time array of trajectory
#p=3.*np.cos(t)+4. #position array of trajectory
np.random.seed(10)
p=np.random.random(k+1)*4. #position array of trajectory
#p=3.*np.sin(1.3+2.*t/(w*k)*2.*np.pi)+10. #position array of trajectory
#p+=np.cos(1.5*t/(w*k)*2.*np.pi) #position array of trajectory
p[-1]=p[0] # put the first position at the end
tt = np.arange(t[0],t[-1], ts) #time array of servo cycles
ax=plt.gca() ax=plt.gca()
ax.xaxis.set_ticks(x) ax.xaxis.set_ticks(t)
markerline, stemlines, baseline = ax.stem(x, y, '-') markerline, stemlines, baseline = ax.stem(t, p, '-')
yf=np.fft.fft(y)
#best trajectory with lowest frequency #best trajectory with lowest frequency
y_iftf=np.hstack((yf,np.zeros(len(xx)-len(x)))) p_iftf=np.fft.fft(p[:-1])
y_ift=np.fft.ifft(y_iftf)*w/ts ft=np.hstack((p_iftf[:k/2],np.zeros((n-1)*k),p_iftf[k/2:]))
ax.plot(xx,y_ift,'-b',label='ift') pp_ift=np.fft.ifft(ft)*n
ax.plot(tt,pp_ift,'-b',label='ift')
#plt.figure() #plt.figure()
#ax=plt.gca() #ax=plt.gca()
@@ -43,33 +74,35 @@ ax.plot(xx,y_ift,'-b',label='ift')
#markerline, stemlines, baseline = ax.stem(x, y, '-') #markerline, stemlines, baseline = ax.stem(x, y, '-')
#PVT move #PVT move
t=np.hstack((y[-1:],y,y[:1])) p2=np.hstack((p[-2],p,p[1]))
n=int(w/ts) v=(p2[2:]-p2[:-2])/(w*2)
v=(t[2:]-t[:-2])/(w*2)
y_pvt=np.ndarray(len(xx))*0 gen_pvt(p,v,t,ts)
xx1=xx[:n]
for i in range(len(x)-1):
d=y[i] pp_pvt=np.ndarray(len(tt))*0
tt1=tt[:n]
for i in range(len(t)-1):
d=p[i]
c=v[i] c=v[i]
a=( -2*(y[i+1]-y[i]-v[i]*w)+ w*(v[i+1]-v[i]))/w**3 a=( -2*(p[i+1]-p[i]-v[i]*w)+ w*(v[i+1]-v[i]))/w**3
b=(3*w*(y[i+1]-y[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3 b=(3*w*(p[i+1]-p[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3
y_pvt[i*n:(i+1)*n]=a*xx1**3+b*xx1**2+c*xx1+d pp_pvt[i*n:(i+1)*n]=a*tt1**3+b*tt1**2+c*tt1+d
ax.plot(xx,y_pvt,'-g',label='pvt') ax.plot(tt,pp_pvt,'-g',label='pvt')
#PVT move with stop #PVT move with stop
v*=0 v*=0
y_p0t=np.ndarray(len(xx))*0 pp_p0t=np.ndarray(len(tt))*0
for i in range(len(x)-1): for i in range(len(t)-1):
d=y[i] d=p[i]
c=v[i] c=v[i]
a=( -2*(y[i+1]-y[i]-v[i]*w)+ w*(v[i+1]-v[i]))/w**3 a=( -2*(p[i+1]-p[i]-v[i]*w)+ w*(v[i+1]-v[i]))/w**3
b=(3*w*(y[i+1]-y[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3 b=(3*w*(p[i+1]-p[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3
y_p0t[i*n:(i+1)*n]=a*xx1**3+b*xx1**2+c*xx1+d pp_p0t[i*n:(i+1)*n]=a*tt1**3+b*tt1**2+c*tt1+d
ax.plot(xx,y_p0t,'-r',label='p0t') ax.plot(tt,pp_p0t,'-r',label='p0t')
ax.legend(loc='best') ax.legend(loc='best')
plt.show(block=False) plt.show(block=False)
@@ -78,26 +111,28 @@ plt.show(block=False)
fig=plt.figure() fig=plt.figure()
ax=fig.add_subplot(1,1,1)#ax=plt.gca() ax=fig.add_subplot(1,1,1)#ax=plt.gca()
y_iftf=np.fft.fft(y_ift) #normalize with l -> value of k means amplitude of k at a given frequency
y_pvtf=np.fft.fft(y_pvt) pp_iftf=np.fft.rfft(pp_ift)/(2*n)
y_p0tf=np.fft.fft(y_p0t) pp_pvtf=np.fft.rfft(pp_pvt)/(2*n)
pp_p0tf=np.fft.rfft(pp_p0t)/(2*n)
f=np.fft.rfftfreq(pp_ift.shape[0], d=ts*1E-3)
f=f[1:] #remove dc value frequency
#f=np.arange(0,1E3/(2*ts),1E3/(2*ts*(len(xx)-1))) mag=abs(pp_iftf[1:])#; mag=20*np.log10(abs(mag))
f=np.linspace(0,1E3/(2*ts),len(xx)) ax.semilogx(f,mag,'-b',label='ift') # Bode magnitude plot
mag=abs(pp_pvtf[1:])#; mag=20*np.log10(abs(mag))
db_mag=20*np.log10(abs(y_iftf)) ax.semilogx(f,mag,'-g',label='pvt') # Bode magnitude plot
ax.semilogx(f,db_mag,'-b',label='ift') # Bode magnitude plot mag=abs(pp_p0tf[1:])#; mag=20*np.log10(abs(mag))
db_mag=20*np.log10(abs(y_pvtf)) ax.semilogx(f,mag,'-r',label='p0t') # Bode magnitude plot
ax.semilogx(f,db_mag,'-g',label='pvt') # Bode magnitude plot #ax.yaxis.set_label_text('dB ampl')
db_mag=20*np.log10(abs(y_p0tf)) ax.yaxis.set_label_text('ampl')
ax.semilogx(f,db_mag,'-r',label='p0t') # Bode magnitude plot
ax.yaxis.set_label_text('dB ampl')
ax.xaxis.set_label_text('frequency [Hz]') ax.xaxis.set_label_text('frequency [Hz]')
plt.grid(True) plt.grid(True)
ax.legend(loc='best') ax.legend(loc='best')
plt.show(block=False) plt.show(block=False)
plt.show()