From 03bce16accd3062789b70af75906f2120ecb5ef3 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Thu, 10 Jan 2019 16:48:08 +0100 Subject: [PATCH] wip --- python/helicalscan.py | 112 +++++++++++++++++++++++----------------- python/trajectory.py | 117 +++++++++++++++++++++++++++--------------- 2 files changed, 141 insertions(+), 88 deletions(-) diff --git a/python/helicalscan.py b/python/helicalscan.py index 45b2623..d0dfe24 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -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] diff --git a/python/trajectory.py b/python/trajectory.py index f4516b3..1315681 100755 --- a/python/trajectory.py +++ b/python/trajectory.py @@ -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()