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

first steps to commission wedge mover: all motors + encoder ok.

Browse Source 
Homing limit switch and coorTrf to be done.
This commit is contained in:
zamofing_t
2018-01-15 11:28:14 +01:00
parent c2695e0ebf
commit 4d197eb204
12 changed files with 941 additions and 364 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
python/helicalscan.py
View File

@@ -30,7 +30,7 @@ verbose bits:
'''
import os, sys, json
import os, sys, json,re
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
@@ -275,7 +275,7 @@ class HelicalScan:
param=self.param
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
ctr=param[:,0:3].mean(0)[::-1]
self.axSetCenter(ctr,10)
self.axSetCenter(ctr,param[0,3]+param[1,3])
axCx=plt.axes([0.1, 0.01, 0.8, 0.02])
axCz=plt.axes([0.1, 0.04, 0.8, 0.02])
@@ -348,7 +348,7 @@ class HelicalScan:
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
ctr=(0,0,0)
self.axSetCenter(ctr,10)
self.axSetCenter(ctr,param[0,3]+param[1,3])
axDx=plt.axes([0.1, 0.01, 0.8, 0.02])
axDz=plt.axes([0.1, 0.04, 0.8, 0.02])
@@ -404,7 +404,7 @@ class HelicalScan:
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
ctr=(0,0,0)
self.axSetCenter(ctr,10)
self.axSetCenter(ctr,param[0,3]+param[1,3])
axFrm=plt.axes([0.1, 0.01, 0.8, 0.02])
@@ -704,7 +704,7 @@ open forward
p0_z=z_0+r_0*cos(phi_0+qW)
p1_z=z_1+r_1*cos(phi_1+qW)
scale=(qFY-y_0)/(y_1-y_0)
scale=(qFY-(y_0))/(y_1-(y_0))
p0_x=p0_x+scale*(p1_x-p0_x)
p0_y=p0_y+scale*(p1_y-p0_y)
p0_z=p0_z+scale*(p1_z-p0_z)
@@ -727,10 +727,11 @@ open inverse
define( qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(vqCX='R4', vqCZ='R5', vqW='R3', vqFY='R1')
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(p0_x='L10', p0_y='L11', p0_z='L12')
define(p1_x='L13', p1_y='L14', p1_z='L15')
define(scale='L16')
define(p_x='L16', p_y='L17', p_z='L18')
define(sclY='L19')
define(scl='L20')
if(D0>0)
send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,DX,vDX,DZ,vDZ,W,vW,Y,vY
else
@@ -750,20 +751,26 @@ open inverse
p0_z=z_0+r_0*cos(phi_0+W)
p1_z=z_1+r_1*cos(phi_1+W)
scale=(Y-y_0)/(y_1-y_0)
p0_x=p0_x+scale*(p1_x-p0_x)
p0_y=p0_y+scale*(p1_y-p0_y)
p0_z=p0_z+scale*(p1_z-p0_z)
qCX=DX+p0_x
qCZ=DZ+p0_z
sclY=(Y-(y_0))/(y_1-(y_0))
p_x=p0_x+sclY*(p1_x-p0_x)
p_y=p0_y+sclY*(p1_y-p0_y)
p_z=p0_z+sclY*(p1_z-p0_z)
qCX=DX+p_x
qCZ=DZ+p_z
qFY=Y
if(D0>0)
{ // calculate velocities for PVT motion
vqCX=vDX
vqCZ=vDZ
vqW=vW
{ // calculate velocities for PVT motion''')
prg.append(" vW=vW*%g"%(d2r/1000.)) #scale from 1000*deg to rad
prg.append(''' p_x=sclY*(p1_x-p0_x)
p_z=sclY*(p1_z-p0_z)
vqFY=vY
send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY
vqCX=vDX + (p1_x-p0_x)/(p1_y-p0_y)*vY //+ vqW*p_z
vqCZ=vDZ + (p1_z-p0_z)/(p1_y-p0_y)*vY //+ vqW*p_x
//vqW=vqW+(vqCX+(p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+(vqCZ+(p1_z-p0_z)/(p1_y-p0_y)*vY)*p_x
vqW=vW//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x
''')
prg.append(" vqW=vqW*%g"%(1000./d2r)) #scale from rad to 1000*deg
prg.append(''' send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY
}
else
send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY
@@ -771,6 +778,11 @@ open inverse
close
''')
# vqCX=vDX + (p1_x-p0_x)/(p1_y-p0_y)*vY + vqW*p_x
# vDX is in same direction, so add as it is
# (p1_x-p0_x)/(p1_y-p0_y)*vY velocity part in vqCX direction, when moving in vY
# vqW*p_x velocity part of the rotation (vqW is in rad)
self.download(prg,mode=0,file='/tmp/coordTrf.cfg')
@@ -845,7 +857,7 @@ close
cntHor = kwargs.get('cntHor', 4)
hRng = kwargs.get('hRng', (-.2,.2))
wRng = kwargs.get('wRng', (0,360000))
yRng = kwargs.get('yRng', (2.3,6.2))
yRng = kwargs.get('yRng', self.param[:,1])
numPt=cntVert*cntHor
pt=np.zeros((numPt,4))
@@ -881,7 +893,7 @@ close
cntHor = kwargs.get('cntHor', 4)
hRng = kwargs.get('hRng', (-.2,.2))
wRng = kwargs.get('wRng', (0,360000))
yRng = kwargs.get('yRng', (2.3,6.2))
yRng = kwargs.get('yRng', self.param[:,1])
pt2pt_time = kwargs.get('pt2pt_time', 100)
smt = kwargs.get('smt', 1) # SegMoveTime, default = 1ms -> velocity calc not yet 100% correct (smt=0 not 100% working)
numPt=cntVert*cntHor
@@ -1001,6 +1013,18 @@ close
stderr=sprc.PIPE)
res = p.wait()
self.rec = np.genfromtxt(fnLoc, delimiter=' ')
com=GpasciiCommunicator().connect(host,prompt='# ')
ack=GpasciiCommunicator.gpascii_ack
channels=["Motor[4].HomePos","Motor[5].HomePos","Motor[3].HomePos","Motor[1].HomePos"]
ofs=np.ndarray(len(channels))
for i,v in enumerate(channels):
com.write(v+'\n')
val=com.read_until(ack)
#print val
#regexp: https://docs.python.org/2/library/re.html -> %e, %E, %f, %g
ofs[i]=float(re.search('=([-+]?(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?)',val).group(1))
self.rec-=ofs
self.save_rec()
@@ -1062,19 +1086,45 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
hs=HelicalScan(args)
hs.args.verbose = 255
hs.args.host='MOTTEST-CPPM-CRM0485'
#hs.args.host='MOTTEST-CPPM-CRM0485'
hs.args.host='SAR-CPPM-EXPMX1'
#SAR-CPPM-EXPMX1 MOTTEST-CPPM-CRM0485
#hs.sequencer()
#hs.test_find_rot_ctr()
#hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
hs.calcParam()
#hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
#hs.param[1]=(15,4,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
#hs.param[0]=(-100, 10,0,50,0)#(z_i, y_i, x_i, r_i,phi_i)
#hs.param[1]=(-100,-10,0,70,0)#(z_i, y_i, x_i, r_i,phi_i)
hs.test_coord_trf()
#hs.interactive_cx_cz_w_fy()
#hs.interactive_dx_dz_w_y()
hs.download(file='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg/sim_8_motors.cfg')
hs.gen_coord_trf_code()
#mode bits:
#0:1 config simulated motors
#1:2 config real motors
#2:4 config coord trf
mode=6#6#5#4#0
os.chdir(os.path.join(os.path.dirname(__file__),'../cfg'))
if mode&1:
hs.download(file='sim_8_motors.cfg')
if mode&2:
hs.download(['$$$***','!common()','!SAR-EXPMX1()','#1..7j/','enable plc 1','Motor[1].MaxSpeed=25','Motor[2].MaxSpeed=25'])
raw_input('press return when homed')
if mode&4:
hs.download(['disable plc 0',])
time.sleep(.5)
hs.gen_coord_trf_code()
print ('gen_coord_trf_code')
time.sleep(.5)
hs.download(['enable plc 0',])
time.sleep(.5)
hs.download(['#1..7j/',])
time.sleep(.5)
#hs.gen_prog(mode=-1)
#hs.gen_prog(mode=0,cntHor=1,cntVert=3,wRng=(120000,120000))
#hs.gen_prog(mode=0,cntHor=1)
@@ -1083,9 +1133,14 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100)
#hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100,smt=0)
#hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(0,360000))
#hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(0,360000),smt=0)
#hs.gen_prog(mode=1,cntHor=1,cntVert=5,hRng=(-.3,.3),wRng=(0,360000),smt=1)
#hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-3,3),wRng=(120000,120000),smt=0)
hs.gen_prog(mode=1,cntHor=3,cntVert=6,hRng=(-5,5),wRng=(00,120000),smt=0,pt2pt_time=10)
#hs.gen_prog(mode=1, cntHor=1, cntVert=2, wRng=(0, 360000), smt=0)
#hs.gen_prog(mode=1)
hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3))
#hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3))
#hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=10,cntHor=3,hRng=(-30,30),wRng=(0,36000),yRng=(-50,-100))
#hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-100,50),wRng=(000,10000),smt=0)
hs.load_rec()
hs.show_pos();hs.show_vel()
hs.interactive_anim()