diff --git a/Readme.md b/Readme.md index 315e11f..55801f6 100644 --- a/Readme.md +++ b/Readme.md @@ -832,4 +832,27 @@ Motor[1].ActPos=-1100474.61749599339 -``` + +Motor[1].HomeOffset=-20907 +Motor[1].HomePos=-372569.750242860231 +Motor[1].ActPos=-8.86266251358743773e-17 +#1p 372569.7502428602 +but should be -8000 + +#1p=Motor[1].ActPos-Motor[1].HomePos + +open plc 0 + if (Motor[1].ActPos-Motor[1].HomePos>180000) + Motor[1].HomePos=Motor[1].HomePos+360000 + if (Motor[2].ActPos-Motor[2].HomePos>180000) + Motor[2].HomePos=Motor[2].HomePos+360000 + if (Motor[3].ActPos-Motor[3].HomePos>180000) + Motor[3].HomePos=Motor[3].HomePos+360000 + if (Motor[4].ActPos-Motor[4].HomePos>180000) + Motor[4].HomePos=Motor[4].HomePos+360000 + if (Motor[5].ActPos-Motor[5].HomePos>180000) + Motor[5].HomePos=Motor[5].HomePos+360000 + disable plc 0 +close plc 0``` + +enable plc 0 \ No newline at end of file diff --git a/SAR-EXPMX1.subs b/SAR-EXPMX1.subs index db981d8..e012ba3 100644 --- a/SAR-EXPMX1.subs +++ b/SAR-EXPMX1.subs @@ -18,10 +18,10 @@ file asyn.template {{P="$(P_M)1", PORT=$(PORT_M)}} file PPMACMotor.template { pattern{ DESC , P , M , PORT , ADDR, DIR, VELO, HVEL, ACCL, JAR, MRES , PREC, EGU , DHLM, DLLM} { "Sample F-Trans Y", "$(P_M)", "MOT_FY" , "$(PORT_M)", 1 , 1 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } - { "Sample F-Trans X", "$(P_M)", "MOT_FX" , "$(PORT_M)", 2 , 1 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } + { "Sample F-Trans X", "$(P_M)", "MOT_FX" , "$(PORT_M)", 2 , 0 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } { "Rotation Y" , "$(P_M)", "MOT_ROT_Y", "$(PORT_M)", 3 , 1 , 50 , 50 , 0.1 , 20 , -0.001, 3 , "deg", 0 , 0 } - { "Sample C-Trans X", "$(P_M)", "MOT_CX" , "$(PORT_M)", 4 , 1 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } - { "Sample C-Trans Z", "$(P_M)", "MOT_CZ" , "$(PORT_M)", 5 , 1 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } + { "Sample C-Trans X", "$(P_M)", "MOT_CX" , "$(PORT_M)", 4 , 0 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } + { "Sample C-Trans Z", "$(P_M)", "MOT_CZ" , "$(PORT_M)", 5 , 0 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } { "Interfero Y" , "$(P_M)", "ENC_FY" , "$(PORT_M)", 6 , 1 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } { "Interfero X" , "$(P_M)", "ENC_FX" , "$(PORT_M)", 7 , 1 , 2 , 2 , 0.1 , 20 , -0.001, 3 , "mm" , 0 , 0 } } diff --git a/cfg/MX1_setup.cfg b/cfg/MX1_setup.cfg index 86d5dcd..4e7ce0d 100644 --- a/cfg/MX1_setup.cfg +++ b/cfg/MX1_setup.cfg @@ -60,7 +60,7 @@ !encoder_sim(enc=2,tbl=10,mot=10,posSf=13000./2048) !encoder_inc(enc=2,tbl=2,mot=2,posSf=13000./650000) -!motor_servo(mot=2,ctrl='ServoCtrl',Kp=20,Kvfb=220,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000) +!motor_servo(mot=2,ctrl='ServoCtrl',Kp=18,Kvfb=220,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000) //PhaseFreq=20000,PhasePerServo=1 -> Kvfb=220*4 Ki/=4,Kvff*=4,Kaff*=4*4 !motor(mot=2,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=1,IpfGain=0,IpbGain=2,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index') diff --git a/python/MAxyPlot.py b/python/MAxyPlot.py index f63f1ec..9053ad0 100644 --- a/python/MAxyPlot.py +++ b/python/MAxyPlot.py @@ -66,13 +66,13 @@ class MPLCanvasImg(FigureCanvas): idx.extend(np.where(l2<1)[0].tolist()) #print idx recPts=rec[idx,:] - f2 = plt.figure() - a2 = f2.add_axes([0.075,0.075,0.85,0.85]) - a2.plot(rec[:,1],'r-',label='ptsDot') #,picker=5 default value - a2.plot(rec[:,2],'g-',label='ptsDot') #,picker=5 default value - a2.plot(rec[:,4],'r--',label='ptsDot') #,picker=5 default value - a2.plot(rec[:,5],'g--',label='ptsDot') #,picker=5 default value - plt.show() + #f2 = plt.figure() + #a2 = f2.add_axes([0.075,0.075,0.85,0.85]) + #a2.plot(rec[:,1],'r-',label='ptsDot') #,picker=5 default value + #a2.plot(rec[:,2],'g-',label='ptsDot') #,picker=5 default value + #a2.plot(rec[:,4],'r--',label='ptsDot') #,picker=5 default value + #a2.plot(rec[:,5],'g--',label='ptsDot') #,picker=5 default value + #plt.show() hl=[] diff --git a/python/findxtal.py b/python/findxtal.py new file mode 100755 index 0000000..9682854 --- /dev/null +++ b/python/findxtal.py @@ -0,0 +1,226 @@ +#!/usr/bin/env python +#*-----------------------------------------------------------------------* +#| | +#| Copyright (c) 2018 by Paul Scherrer Institute (http://www.psi.ch) | +#| | +#| Author Thierry Zamofing (thierry.zamofing@psi.ch) | +#*-----------------------------------------------------------------------* +''' +implements an image alalyser for ESB MX +''' + +from scipy import fftpack, ndimage +import scipy.ndimage as ndi +import matplotlib.pyplot as plt +import numpy as np +#plt.ion() + +def ffttest(): + #find the main frequency and phase in 1-D + d2r=np.pi/180. + n=16. + x=np.arange(n) + amp=1. + frq=4.5 + phi=40.*d2r + y=amp*np.cos(phi+frq*x/n*2.*np.pi) + #y*=np.hamming(n) + y*=np.hanning(n) + #y*=1.-np.cos(x/(n-1.)*2.*np.pi) + + #y=[1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1] + plt.ion() + plt.figure() + plt.stem(x,y) + + fy=np.fft.fft(y) + fya=np.abs(fy) + plt.figure() + plt.subplot(211) + plt.stem(x,fya) + plt.subplot(212) + plt.stem(x,np.angle(fy)/d2r) + print(np.angle(fy[frq])/d2r) + + i=(fya.reshape(2,-1)[0,:]).argmax() + (vn,v0,vp)=fya[i-1:i+2] + frq2=i+(vn-vp)/(2.*(vp+vn-2*v0)) + print('freq: %g phase %g'%(frq2,np.angle(fy[i])/d2r)) + #TODO: THE PHASE CALCULATION IS NOT YET WORKING!!! + + #y_inv=np.fft.fft(fy) + #plt.figure() + #plt.plot(x,np.abs(y_inv)) + + + pass + +def findGrid(image,numPeak=2): + #image = ndimage.imread('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/images/grid_20180409_115332.png', flatten=True) # flatten=True gives a greyscale image + s=image.shape + w1=np.hamming(s[0]).reshape((-1,1)) + w2=np.hamming(s[1]).reshape((1,-1)) + wnd=w1*w2 + #plt.figure(num='hamming window') + #plt.imshow(wnd, interpolation="nearest") + image=wnd*image + #plt.figure(num='hamming window*img') + #plt.imshow(image, interpolation="nearest") + + fft2 = fftpack.fft2(image) + fft2=np.fft.fftshift(fft2) + fa =abs(fft2) + fal =np.log(fa) + #img=fft3; + ofs=int(fal.min()) + mx2=(image.shape[0]/2,image.shape[1]/2) + fal[mx2[0] - 1:mx2[0] + 2, mx2[1] - 1:mx2[1] + 2]=ofs + for i in range(numPeak*2): + mx=fal .argmax() + mx2=divmod(mx,fal .shape[1]) + peakPos=(mx2[0] - image.shape[0] / 2, mx2[1] - image.shape[1] / 2) + peak=fal[mx2[0] - 1:mx2[0] + 2, mx2[1] - 1:mx2[1] + 2] + print(peakPos) + print(peak) + (vn, v0, vp)=fal[mx2[0], mx2[1] - 1:mx2[1] + 2] + frq1x=peakPos[1]+(vn-vp)/(2.*(vp+vn-2*v0)) + (vn, v0, vp)=fal[mx2[0]-1:mx2[0]+2,mx2[1]] + frq1y=peakPos[0]+(vn-vp)/(2.*(vp+vn-2*v0)) + print((frq1x,frq1y)) + fal[mx2[0]-1:mx2[0]+2,mx2[1]-1:mx2[1]+2]=i+ofs + + + + + plt.figure(num='fft of hamming window*img') + plt.imshow(fal, interpolation="nearest") + plt.xlim(s[1]/2-50, s[1]/2+50) + plt.ylim(s[0]/2-50, s[0]/2+50) + plt.show() + pass + +def findObj(image,objSize=150,tol=0,viz=0): + + #objSiz is the rough diameter of the searched features in pixels + #tol = tolerance in object size (not yet implemented) + #tolShape = roudness tolerance in object roundness (not yet implemented) + + from scipy.signal import convolve2d + #plt.ion() + s=image.shape + #box=np.ones((1,objSize*3),dtype=np.float32)/500. + + #img2=ndi.filters.convolve1d(image,box.reshape(-1),0) + #img2=ndi.filters.convolve1d(img2,box.reshape(-1),1) + img2=ndi.filters.uniform_filter(np.float32(image),objSize*2) + + if viz&32: + plt.imshow(image, interpolation="nearest", cmap='gray') + plt.figure() + plt.imshow(img2, interpolation="nearest", cmap='gray') + #plt.show() + w=np.where(img2>image) + img2[w]=image[w] + img3=image-img2 + if viz&16: + plt.figure() + plt.imshow(img3, interpolation="nearest", cmap='gray') + #plt.show() + l=int(objSize/30) + if l>0: + img4=ndi.binary_fill_holes(img3, structure=np.ones((l,l))) + if viz&8: + plt.figure() + plt.imshow(img4, interpolation="nearest", cmap='gray') + #plt.show() + else: + img4=img3 + + l=int(objSize/5)#=int(objSize/10) + if l>=3: + #img5=ndi.binary_opening(img4, structure=np.ones((l,l))) + #img5=ndi.binary_erosion(img4, structure=np.ones((l,l))) + img5=ndi.binary_erosion(img4, iterations=l) + if viz&4: + plt.figure() + plt.imshow(img5, interpolation="nearest", cmap='gray') + #plt.show() + else: + img5=img4 + + import cv2 + #cvi1=np.zeros(shape=img5.shape+(3,), dtype=np.uint8) + #cvi1[:,:,0]=image + #image*np.array([1,1,1]).reshape(-1,1,1) + s=image.shape+(1,) + cvi1=image.reshape(s)*np.ones((1,1,3),dtype=np.uint8) + #cvi1=np.ones((3,1,1),dtype=np.uint8)image + contours, hierarchy=cv2.findContours(np.uint8(img5),cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) + #contours,hierarchy=cv2.findContours(np.uint8(img5),1,2) + cv2.drawContours(cvi1, contours, -1, (0,255,0), 3) + plt.figure() + plt.imshow(cvi1 , interpolation="nearest", cmap='gray') + m=cv2.moments(contours[0]) + + lbl = ndi.label(img5) + if viz&2: + plt.figure() + plt.imshow(lbl[0], interpolation="nearest") + #plt.show() + ctr=ndi.measurements.center_of_mass(image, lbl[0],range(lbl[1])) + ctr=np.array(ctr) + ctr2=np.ndarray(shape=(len(contours),2),dtype=np.uint16) + i=0 + for c in contours: + m=cv2.moments(c) + try: + m00=m['m00'] + m10=m['m10'] + m01=m['m01'] + print m00 + if m00>1000 and m00<7000: + ctr2[i,:]=(m10/m00,m01/m00) + i+=1 + except ZeroDivisionError: + pass + #ctr2[i, :]=c[0,0] + + if viz&1: + plt.figure() + plt.imshow(image, interpolation="nearest", cmap='gray') + plt.plot(ctr[:,1],ctr[:,0],'or',markeredgewidth=2, markersize=10) + plt.plot(ctr2[:i,0],ctr2[:i,1],'+y',markeredgewidth=2, markersize=10) + plt.show() + return ctr + +def genImg(shape,*args): + '''args is a list of tuples (freq_x,freq_y, phase) multiple args can be added''' + image=np.ndarray(shape)#,dtype=np.uint8) + x=np.linspace(0,2*np.pi,shape[1]) + y=np.linspace(0,2*np.pi,shape[0]) + #xx, yy = np.meshgrid(x, y, sparse=True) + xx, yy = np.meshgrid(x, y) + + for i,f in enumerate(args): + (freq_x, freq_y, phase)=f + if i==0: + image = np.cos(freq_x*xx + freq_y*yy + phase) + else: + image += np.cos(freq_x * xx + freq_y * yy + phase) + + plt.imshow(image, interpolation="nearest") + plt.show() + return image + +if __name__ == '__main__': + #ffttest() + #image = ndimage.imread('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/images/grid_20180409_115332.png') + #image = ndimage.imread('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/images/grid_20180409_115332_45deg.png') + #image=-image + image=genImg((600,800),(9.5,.2,0)) + #image=genImg((600,800),(9.5,.2,0),(.4,5.2,0)) + #image=genImg((600,800),(9.5,.2,0),(.4,5.2,0),(4,8,0)) + findGrid(image,numPeak=1) + #findObj(image,viz=1) + #findObj(image,viz=255) + #print(findObj(image))