diff --git a/ESB_MX.subs b/ESB_MX.subs index c860410..a2bc1a9 100644 --- a/ESB_MX.subs +++ b/ESB_MX.subs @@ -20,9 +20,11 @@ file PPMACMotor.template { pattern {DESC ,P ,M ,PORT ,ADDR ,DIR,VMAX,VELO,JVEL,HVEL,ACCL,JAR,MRES ,PREC,EGU ,DHLM,DLLM} -{ "Rotation Y" ,"$(P_M)","MOTOR_ROT_Y1","$(PORT_M)","$(M1)",1 ,50 ,50 ,50 ,1 ,0.1 ,20 ,-0.001,3 ,"deg",0 ,0} -{ "Translation X","$(P_M)","MOTOR_X1" ,"$(PORT_M)","$(M2)",1 ,2 ,2 ,2 ,1 ,0.1 ,20 ,-0.001,3 ,"mm",0 ,0} -{ "Translation Y","$(P_M)","MOTOR_Y1" ,"$(PORT_M)","$(M3)",1 ,2 ,2 ,2 ,1 ,0.1 ,20 ,-0.001,3 ,"mm",0 ,0} -{ "TransBase X", "$(P_M)","MOTOR_AX" ,"$(PORT_M)","$(M7)",1 ,2 ,2 ,2 ,1 ,0.1 ,20 ,-0.001,3 ,"mm",0 ,0} -{ "TransBase Z", "$(P_M)","MOTOR_AZ" ,"$(PORT_M)","$(M8)",1 ,2 ,2 ,2 ,1 ,0.1 ,20 ,-0.001,3 ,"mm",0 ,0} +{ "Rotation Y" ,"$(P_M)","MOTOR_ROT_Y" ,"$(PORT_M)","$(M1)",1 ,50 ,50 ,50 ,1 ,0.1 ,20 ,-0.001,3 ,"deg",0 ,0} +{ "Translation X","$(P_M)","MOTOR_FX" ,"$(PORT_M)","$(M2)",1 ,2 ,2 ,2 ,1 ,0.1 ,20 ,-0.001,3 ,"mm",0 ,0} +{ "Translation Y","$(P_M)","MOTOR_FY" ,"$(PORT_M)","$(M3)",1 ,2 ,2 ,2 ,1 ,0.1 ,20 ,-0.001,3 ,"mm",0 ,0} +{ "CamBase X", "$(P_M)","MOTOR_CAMX" ,"$(PORT_M)","$(M4)",0 ,2 ,2 ,2 ,1 ,0.1 ,20 ,-0.001,3 ,"mm",0 ,0} +{ "CamBase Y", "$(P_M)","MOTOR_CAMY" ,"$(PORT_M)","$(M5)",1 ,2 ,2 ,2 ,1 ,0.1 ,20 ,-0.001,3 ,"mm",0 ,0} +{ "TransBase X", "$(P_M)","MOTOR_CX" ,"$(PORT_M)","$(M7)",1 ,2 ,2 ,2 ,1 ,0.1 ,20 ,-0.001,3 ,"mm",0 ,0} +{ "TransBase Z", "$(P_M)","MOTOR_CZ" ,"$(PORT_M)","$(M8)",1 ,2 ,2 ,2 ,1 ,0.1 ,20 ,-0.001,3 ,"mm",0 ,0} } diff --git a/add_device.cmd b/add_device.cmd index 15e7567..7e604b2 100644 --- a/add_device.cmd +++ b/add_device.cmd @@ -12,18 +12,22 @@ require gpasciiCommander M1=1 M2=$(M1)+1 M3=$(M1)+2 -#M4=$(M1)+3 +M4=$(M1)+3 +M5=$(M1)+4 +M6=$(M1)+5 M7=$(M1)+6 M8=$(M1)+7 powerPmacCreateAxis($(PORT), $(M1)) powerPmacCreateAxis($(PORT), $(M2)) powerPmacCreateAxis($(PORT), $(M3)) -#powerPmacCreateAxis($(PORT), $(M4)) +powerPmacCreateAxis($(PORT), $(M4)) +powerPmacCreateAxis($(PORT), $(M5)) +powerPmacCreateAxis($(PORT), $(M6)) powerPmacCreateAxis($(PORT), $(M7)) powerPmacCreateAxis($(PORT), $(M8)) # Load databse -dbLoadTemplate("$(ESB_MX_TEMPLATES)/ESB_MX.subs", "PORT_M=$(PORT),P_M=$(P),M1=$(M1),M2=$(M2),M3=$(M3),M7=$(M7),M8=$(M8)") +dbLoadTemplate("$(ESB_MX_TEMPLATES)/ESB_MX.subs", "PORT_M=$(PORT),P_M=$(P),M1=$(M1),M2=$(M2),M3=$(M3),M4=$(M4),M5=$(M5),M6=$(M6),M7=$(M7),M8=$(M8)") diff --git a/cfg/mx-stage.cfg b/cfg/mx-stage.cfg index 15f87ef..0cbb401 100644 --- a/cfg/mx-stage.cfg +++ b/cfg/mx-stage.cfg @@ -1,40 +1,3 @@ -// Here we use 'real encoder with direct PWM'. Further the axis are scaled -// in this configuration, the PID gives 'torque' to iqCmd. -// the phasePos is received from tne encoder on the motor shaft. -// The idCmd is set to 0 -// the PID regulates the position by setting torque, if the motor is not at the desired position -// Compared to 'real encoder with direct microstepping', following main elements have to be reconfigured: -// SlipGain=0 (instead 0.25) ,PhasePosSf= calculated value (instead of 0) -// look also at PwmSf,PhaseMode,PhaseCtrl, - -// -> PhasePosSf is calculated as follows: (2048*pole_cycle)/(256*enc_step) = 8*pole_cycle/enc_step - -// e.g. Motor[x].pPhaseEnc -> PowerBrick[.].Chan[.].PhaseCapt.a -// 1 rev = 8192 phase_step = 4 pole_cycle = 512000 PhaseCapt =256*2000 (256=scaling of encTable, 2000=enc_step/rev) -// PhasePosSf 8*4/2000=0.016 - - -// x einraster == -> x-N and x-S poles =2*x poles -> 1 rev = x*2048 ustep=phase_step -// changing the polarity from S-N-S (one pole cycle) are 2048 phase_step. phase_step is also called ustep - -//Mot 1: Rotation stage LS Mecapion MDM-DC06DNC0H 32 poles = 1 rev = 16*2048=32768 phase_step -//Enc 1: Rotation stage LS Mecapion 1 rev = 1048576 enc_steps - -//Mot 2: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step -//Enc 2: Stage X Parker MX80L D11 inc_enc 20nm one pole cycle = 13mm = 650000 enc_step (20nm/enc_step) - -//Mot 3: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step -//Enc 3: Stage Y Parker MX80L D11 inc_enc 20nm one pole cycle = 13mm = 650000 enc_step (20nm/enc_step) - -//Mot 4: Test Servo: Trinamic QBL 4208 motor 8 poles 1 rev = 4*2048=8192 phase_step -//Enc 4: Test Servo: Incremental encoder mounted with motor 1 1 rev = 2000 enc count (500 inc_ quadrature encoder) -//Enc 5: Test Servo: Trinamic QBL 4208 hall sensor 1 rev = 24 enc count (hall sensor encoder) - -//Mot 7: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) -//Enc 7: Renishaw absolute BiSS - -//Mot 8: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) -//Enc 8: Renishaw absolute BiSS //$$$*** //!common() diff --git a/cfg/torqueCtrl.cfg b/cfg/torqueCtrl.cfg index b6658ce..030f87f 100644 --- a/cfg/torqueCtrl.cfg +++ b/cfg/torqueCtrl.cfg @@ -26,6 +26,23 @@ //Mot 3: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step //Enc 3: Stage X Parker MX80L D11 inc_enc 20nm one pole cycle = 13mm = 650000 enc_step (20nm/enc_step) +//Enc 4: Interferometer 1 + +//Enc 5: Interferometer 2 + + + + +//Mot 7: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) +//Enc 7: Renishaw absolute BiSS + +//Mot 8: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) +//Enc 8: Renishaw absolute BiSS + + + + + //rot stage //--------- //use 360'000 for 360 deg as motor unit @@ -72,10 +89,28 @@ Motor[1].pPhaseEnc=Acc84B[0].Chan[0].SerialEncDataA.a //PhaseFreq=20000,PhasePerServo=1 -> Kvfb=220*4 Ki/=4,Kvff*=4,Kaff*=4*4 !motor(mot=3,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') -//Interferometer 1 Stage Y Parker MX80L (bottom stage) -//---------------------------------------------------- -!encoder_inc(enc=4,tbl=4,mot=4)# ,posSf=13000./650000) -!encoder_inc(enc=5,tbl=5,mot=5)# ,posSf=13000./650000) +//Interferometer 1,2 +//------------------ +//!encoder_inc(enc=4,tbl=4,mot=4)# ,posSf=13000./650000) +//!encoder_inc(enc=5,tbl=5,mot=5)# ,posSf=13000./650000) + +//Base plate +//---------- + +//102400000 ustep 200000 encCnt = 10mm = 10000um +//posSf = userUnits/encoder_steps = 10000/200000 =1./20 +//servoSf=motor_u_steps/userUnits = 102400000/10000 = 10240./1 +!encoder_inc(enc=4,posSf=1./20) +!motor(mot=4,dirCur=250,JogSpeed=.3,invDir=0,servoSf=10240.,InPosBand=1,HomeOffset=0) +Motor[4].Pos2Sf=0 // Pos2Sf set to 0 to avoid programming leak values + +//-51200000 ustep = -200000 encCnt == 10mm =10000 um +//posSf = userUnits/encoder_steps = 10000/-200000 =1/-20. +//servoSf=motor_u_steps/userUnits = -51200000/10000 = -5120./1 +!encoder_inc(enc=5,posSf=1./20) +!motor(mot=5,dirCur=250,JogSpeed=.3,invDir=0,servoSf=5120.,InPosBand=1,HomeOffset=0) +Motor[5].Pos2Sf=0 // Pos2Sf set to 0 to avoid programming leak values + //Stada stage //---------------------------------------------------- diff --git a/python/helicalscan.py b/python/helicalscan.py index ccffeaa..4aa7b57 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -28,7 +28,8 @@ class HelicalScan: #fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S') #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['test_find_rot_ctr()']} - cfg = {"sequencer": ['test_find_rot_ctr(n=5. ,per=1. ,phi=24.6 ,bias=2.31,ampl=4.12)']} + #cfg = {"sequencer": ['test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)']} + cfg = {"sequencer": ['test_coord_trf()']} self.cfg=dotdict(cfg) self.args=args @@ -51,7 +52,70 @@ class HelicalScan: if not dryrun: eval('self.' + cmd) - def meas_rot_ctr(self,y,per=1): + def test_coord_trf(self): + n = 3.; per = 1.; t = np.arange(n) + p=((2.3,2.31,4.12,24.6),(6.2,2.74,32.1,3.28)) #(y, bias, ampl, phi) + self.param=param=np.ndarray((len(p),5)) + z=4.5 # fix z position + for i in range(2): + (y, bias, ampl, phi) =p[i] + x= ampl * np.cos(2 * np.pi * (per / n * t + phi / 360.)) + 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) + pass + print param + #self.fwd_transform(param[0][1],0.,param[0][2],param[0][1]) + # y_0 ,120deg ,x_0 ,z_0) + self.fwd_transform(param[0][1],2*np.pi/3.,param[0][2],param[0][0]) + #self.fwd_transform(param[1][1],0.,param[1][2],param[1][3]) + + + + def fwd_transform(self,y,w,cx,cz): + #cx,cy: coarse stage + #TODO: NOT WORKING AT ALL NOW... + param=self.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): + #p[i][0]=param[i][2]+param[i][3]*np.cos(param[i][4]+w) # x= x_i+r_i*cos(phi_i*w)+cx + p[i][0]=cx+param[i][3]*np.cos(param[i][4]+w) # x= x_i+r_i*cos(phi_i*w)+cx + p[i][1]=param[i][1] # y= y_i + #p[i][2]=param[i][2]+param[i][3]*np.sin(param[i][4]+w) # z= z_i+r_i*sin(phi_i*w) + p[i][2] =cz + param[i][3] * np.sin(param[i][4] + w) # z= z_i+r_i*sin(phi_i*w) + print p + v=p[1]-p[0] + v=v/np.sqrt(v.dot(v)) # v/|v| + v=v*(y-param[0][1])/(param[1][1]-param[0][1]) # v(y)=v*(v-y_0)/(y_1-y_0) + v=p[0]+v + + #v=v/abs(v) + print v + + + + # + # + # + + + + #x,y,z + #returns y,w,dx,dz + pass + + def inv_transform(y,phi,dx=0,dz=0): + #dx,dy: deviation from cristal center line + #ps= #x,y,z + #returns y,phi,cx,cz + pass + + + @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 @@ -61,9 +125,10 @@ class HelicalScan: bias=np.absolute(f[0]/n) phase=np.angle(f[idx]) ampl=np.absolute(f[idx])*2/n - return (bias,phase,ampl) + return (bias,ampl,phase) - def test_find_rot_ctr(self,n=3. ,per=1. ,phi=37 ,bias=4.1,ampl=2.4): + @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) @@ -73,24 +138,18 @@ class HelicalScan: t = np.arange(n) y=ampl*np.cos(2*np.pi*(per/n*t+phi/360.))+bias - sp = np.fft.fft(y) - freq = np.fft.fftfreq(t.shape[-1]) plt.figure(1) plt.subplot(311) plt.plot(t,y,'b.-') - plt.subplot(312) - #plt.plot(t, sp.real,'b.-', t, sp.imag,'r.-') - plt.step(t, sp.real,'b.-', t, sp.imag,'r.-', where='mid') - #plt.stem(t, sp.real,'b-') - #plt.plot(freq, sp.real,'b.-', freq, sp.imag,'r.-') - idx=int(per) - bias=np.absolute(sp[0]/n) - phase=np.angle(sp[idx]) - ampl=np.absolute(sp[idx]) * 2 / n + 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('phase: '+str(phase*360./2/np.pi)) print('amplitude: '+str(ampl)) + print('phase: '+str(phase*360./2/np.pi)) plt.subplot(313) t2 = np.linspace(0,2*np.pi,64) diff --git a/python/helicalscan1.svg b/python/helicalscan1.svg index 0ef3f5a..21b37e3 100644 --- a/python/helicalscan1.svg +++ b/python/helicalscan1.svg @@ -18,6 +18,26 @@ sodipodi:docname="helicalscan1.svg"> + + + + + + inkscape:window-maximized="1" + inkscape:snap-bbox="false" + inkscape:bbox-paths="true" + inkscape:bbox-nodes="true" + inkscape:snap-bbox-edge-midpoints="true" + inkscape:snap-bbox-midpoints="true" + inkscape:object-paths="true" + inkscape:snap-intersection-paths="true" + inkscape:object-nodes="true" + inkscape:snap-smooth-nodes="true" /> @@ -160,8 +189,8 @@ id="path4155" inkscape:connector-curvature="0" /> x y φs pe pps φe + style="font-size:64.99999762%;baseline-shift:sub;-inkscape-font-specification:MathJax_Size1;font-family:MathJax_Size1;font-weight:normal;font-style:normal;font-stretch:normal;font-variant:normal;">e yys yye crystal + y="412.73996">v crystal rs re - + + + + + xe + xs + + + + + coord transformation:input ω,y,dx,dzoutput: ω,x,y,z,dx,dz are translation from the vector v after rotation + known values after fft: ys, rs, xs, φs, ye, re, xe, φe + + φe + + v=pe-ps + + + + + + ps(ω)= + xs+rs·cos(φs+ω) + ys+rs·sin(φs+ω) + + ys + + + + output:ω=ωx=psx+vx(y)+dxy=yz=psy+vz(y)+dz + + + v ·(ye-ys) + + v·(y-ys) + + + + + + + + vx(y) + + + vy(y) + vz(y) + + v(y)= + = + + diff --git a/python/helicalscan2.svg b/python/helicalscan2.svg index cf31eec..e6d6e54 100644 --- a/python/helicalscan2.svg +++ b/python/helicalscan2.svg @@ -139,15 +139,15 @@ borderopacity="1.0" inkscape:pageopacity="0.0" inkscape:pageshadow="2" - inkscape:zoom="1" - inkscape:cx="441.16057" - inkscape:cy="535.19697" + inkscape:zoom="1.0341797" + inkscape:cx="610.03809" + inkscape:cy="581.42706" inkscape:document-units="px" inkscape:current-layer="layer1" showgrid="false" - inkscape:window-width="1871" + inkscape:window-width="1920" inkscape:window-height="1176" - inkscape:window-x="49" + inkscape:window-x="1920" inkscape:window-y="24" inkscape:window-maximized="1" showguides="true" @@ -162,10 +162,6 @@ id="grid5704" spacingx="10" spacingy="10" /> - @@ -175,7 +171,7 @@ image/svg+xml - + @@ -422,7 +418,7 @@ height="226.04152" x="-48.598927" y="-3.2517097" /> + fft of at least 3 values x0 x1 x2the fft returns the bias value, phase and amplitudeknown values after fft: xbias, r, φx(t)=xbias+r·cos(ω·t-φ) + φ0 + φ1 + φ2 diff --git a/qt/ESB_MX_exp.ui b/qt/ESB_MX_exp.ui index 350b016..e58b154 100644 --- a/qt/ESB_MX_exp.ui +++ b/qt/ESB_MX_exp.ui @@ -7,7 +7,7 @@ 0 0 776 - 221 + 298 @@ -49,22 +49,24 @@ 20 70 725 - 120 + 168 -P=$(P),M=MOTOR_AX; -P=$(P),M=MOTOR_AZ; -P=$(P),M=MOTOR_ROT_Y1; -P=$(P),M=MOTOR_X1; -P=$(P),M=MOTOR_Y1 +P=$(P),M=MOTOR_CX; +P=$(P),M=MOTOR_CZ; +P=$(P),M=MOTOR_ROT_Y; +P=$(P),M=MOTOR_FX; +P=$(P),M=MOTOR_FY; +P=$(P),M=MOTOR_CAMX; +P=$(P),M=MOTOR_CAMY ESB_MX_motor.ui - 5 + 7