wip
This commit is contained in:
@@ -710,35 +710,21 @@ class HelicalScan(MotionBase):
|
||||
res=(cx,cz,w,fy)
|
||||
return res
|
||||
|
||||
def calcParamFast(self,x=((-241.,96.),(-162.,-293.)),
|
||||
y=(575.,175.),
|
||||
z=((-1401.,-1401.),(-1802.,-1303.)),
|
||||
w=(1.2,1.4),
|
||||
mode=1):
|
||||
def calcParam1Pt(self,x=(-241.,-162.),
|
||||
y=(575.,175.),
|
||||
z=(-1401.,-1802.),
|
||||
w=1.2):
|
||||
'''
|
||||
the rotation center of the stage does not change for a new cristal.
|
||||
after the function calcParam was called once,
|
||||
this faster coordinate transformation can be used.
|
||||
FOR SMALL ANGLES USE MODE==0.
|
||||
!!! THIS CODE IS NOT YET TESTED !!!
|
||||
this 1 point coordinate transformation can be used.
|
||||
it needs 1 point at start and 1 point at end of the crystal
|
||||
'''
|
||||
#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)
|
||||
# z_i not changed
|
||||
@@ -747,41 +733,73 @@ class HelicalScan(MotionBase):
|
||||
# r_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:
|
||||
param=self.param
|
||||
except AttributeError as e:
|
||||
raise AttributeError('calcParam must be called first')
|
||||
|
||||
if mode==0:
|
||||
for i in range(len(y)):
|
||||
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
||||
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)
|
||||
else: #mode==1:
|
||||
for i in range(len(y)):
|
||||
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
||||
r_i=np.sqrt(x[i]**2+z[i]**2)
|
||||
x0=x[i][0]-param[i,2]
|
||||
x1=x[i][1]-param[i,2]
|
||||
ww=w[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)
|
||||
|
||||
for i in range(len(y)):
|
||||
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
||||
r_i=np.sqrt(x[i]**2+z[i]**2)
|
||||
x0=x[i][0]-param[i,2]
|
||||
x1=x[i][1]-param[i,2]
|
||||
ww=w[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
|
||||
|
||||
|
||||
def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)),
|
||||
y=(575.,175.),
|
||||
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:
|
||||
#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]
|
||||
|
||||
@@ -17,25 +17,56 @@ import numpy as np
|
||||
import matplotlib as mpl
|
||||
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
|
||||
ts=.2 # sampling time
|
||||
x = np.arange(0, 400, w)
|
||||
y=np.cos(x)
|
||||
n=int(w/ts)# servo cycle between samples
|
||||
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.xaxis.set_ticks(x)
|
||||
markerline, stemlines, baseline = ax.stem(x, y, '-')
|
||||
|
||||
yf=np.fft.fft(y)
|
||||
ax.xaxis.set_ticks(t)
|
||||
markerline, stemlines, baseline = ax.stem(t, p, '-')
|
||||
|
||||
|
||||
#best trajectory with lowest frequency
|
||||
y_iftf=np.hstack((yf,np.zeros(len(xx)-len(x))))
|
||||
y_ift=np.fft.ifft(y_iftf)*w/ts
|
||||
ax.plot(xx,y_ift,'-b',label='ift')
|
||||
p_iftf=np.fft.fft(p[:-1])
|
||||
ft=np.hstack((p_iftf[:k/2],np.zeros((n-1)*k),p_iftf[k/2:]))
|
||||
pp_ift=np.fft.ifft(ft)*n
|
||||
|
||||
ax.plot(tt,pp_ift,'-b',label='ift')
|
||||
|
||||
#plt.figure()
|
||||
#ax=plt.gca()
|
||||
@@ -43,33 +74,35 @@ ax.plot(xx,y_ift,'-b',label='ift')
|
||||
#markerline, stemlines, baseline = ax.stem(x, y, '-')
|
||||
|
||||
#PVT move
|
||||
t=np.hstack((y[-1:],y,y[:1]))
|
||||
p2=np.hstack((p[-2],p,p[1]))
|
||||
|
||||
n=int(w/ts)
|
||||
v=(t[2:]-t[:-2])/(w*2)
|
||||
v=(p2[2:]-p2[:-2])/(w*2)
|
||||
|
||||
y_pvt=np.ndarray(len(xx))*0
|
||||
xx1=xx[:n]
|
||||
for i in range(len(x)-1):
|
||||
d=y[i]
|
||||
gen_pvt(p,v,t,ts)
|
||||
|
||||
|
||||
pp_pvt=np.ndarray(len(tt))*0
|
||||
tt1=tt[:n]
|
||||
for i in range(len(t)-1):
|
||||
d=p[i]
|
||||
c=v[i]
|
||||
a=( -2*(y[i+1]-y[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
|
||||
y_pvt[i*n:(i+1)*n]=a*xx1**3+b*xx1**2+c*xx1+d
|
||||
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
|
||||
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
|
||||
v*=0
|
||||
y_p0t=np.ndarray(len(xx))*0
|
||||
for i in range(len(x)-1):
|
||||
d=y[i]
|
||||
pp_p0t=np.ndarray(len(tt))*0
|
||||
for i in range(len(t)-1):
|
||||
d=p[i]
|
||||
c=v[i]
|
||||
a=( -2*(y[i+1]-y[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
|
||||
y_p0t[i*n:(i+1)*n]=a*xx1**3+b*xx1**2+c*xx1+d
|
||||
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
|
||||
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')
|
||||
plt.show(block=False)
|
||||
@@ -78,26 +111,28 @@ plt.show(block=False)
|
||||
fig=plt.figure()
|
||||
ax=fig.add_subplot(1,1,1)#ax=plt.gca()
|
||||
|
||||
y_iftf=np.fft.fft(y_ift)
|
||||
y_pvtf=np.fft.fft(y_pvt)
|
||||
y_p0tf=np.fft.fft(y_p0t)
|
||||
#normalize with l -> value of k means amplitude of k at a given frequency
|
||||
pp_iftf=np.fft.rfft(pp_ift)/(2*n)
|
||||
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)))
|
||||
f=np.linspace(0,1E3/(2*ts),len(xx))
|
||||
|
||||
db_mag=20*np.log10(abs(y_iftf))
|
||||
ax.semilogx(f,db_mag,'-b',label='ift') # Bode magnitude plot
|
||||
db_mag=20*np.log10(abs(y_pvtf))
|
||||
ax.semilogx(f,db_mag,'-g',label='pvt') # Bode magnitude plot
|
||||
db_mag=20*np.log10(abs(y_p0tf))
|
||||
ax.semilogx(f,db_mag,'-r',label='p0t') # Bode magnitude plot
|
||||
ax.yaxis.set_label_text('dB ampl')
|
||||
mag=abs(pp_iftf[1:])#; mag=20*np.log10(abs(mag))
|
||||
ax.semilogx(f,mag,'-b',label='ift') # Bode magnitude plot
|
||||
mag=abs(pp_pvtf[1:])#; mag=20*np.log10(abs(mag))
|
||||
ax.semilogx(f,mag,'-g',label='pvt') # Bode magnitude plot
|
||||
mag=abs(pp_p0tf[1:])#; mag=20*np.log10(abs(mag))
|
||||
ax.semilogx(f,mag,'-r',label='p0t') # Bode magnitude plot
|
||||
#ax.yaxis.set_label_text('dB ampl')
|
||||
ax.yaxis.set_label_text('ampl')
|
||||
ax.xaxis.set_label_text('frequency [Hz]')
|
||||
plt.grid(True)
|
||||
|
||||
ax.legend(loc='best')
|
||||
plt.show(block=False)
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user