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

wip

Browse Source
This commit is contained in:
zamofing_t
2018-10-16 15:42:33 +02:00
parent 6dace17030
commit dc613706d6
1 changed files with 85 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

170
python/helicalscan.py
View File

@@ -664,12 +664,10 @@ class HelicalScan(MotionBase):
comm = self.comm comm = self.comm
gpascii = comm.gpascii gpascii = comm.gpascii
param=self.param param=self.param
prg = [] prg = '''
prg.append('''
// Set the motors as inverse kinematic axes in CS 1 // Set the motors as inverse kinematic axes in CS 1
//motors CX CZ RY FY //motors CX CZ RY FY
// 4 5 3 1 // 4 5 3 1
&1 &1
a a
#4->0 #4->0
@@ -681,103 +679,106 @@ a
#5->I #5->I
#3->I #3->I
#1->I #1->I
'''
s = ('z', 'y', 'x', 'r', 'phi')
a = np.ones(param.shape[1], dtype=np.uint8).reshape(1, -1)
b = np.arange(param.shape[0], dtype=np.uint8).reshape(1, -1)
c = np.matmul(b.T, a).reshape(-1)
subsParam=dict(map(lambda k, i, v: (k + '_' + str(i), v), s * param.shape[0], c, param.reshape(-1)))
subsParam['d2r']=d2r/1000.
subsParam['r2d']=1000./d2r
subs={'qCX':'L4', 'qCZ':'L5', 'qW':'L3', 'qFY':'L1',
'DX':'C6', 'DZ':'C8', 'W':'C1', 'Y':'C7',
#coord X Z B Y
'p0_x':'L10', 'p0_y':'L11', 'p0_z':'L12',
'p1_x':'L13', 'p1_y':'L14', 'p1_z':'L15',
'scale':'L16'}
subs.update(subsParam)
prg+='''
open forward open forward
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1') send 1"fwd_inp(%f) %f %f %f %f\\n",D0,{qCX},{qCZ},{qW},{qFY}
define(DX='C6', DZ='C8', W='C1', Y='C7') {W}={qW}
//coord X Z B Y {qW}={qW}*{d2r} //scale from 1000*deg to rad
define(p0_x='L10', p0_y='L11', p0_z='L12')
define(p1_x='L13', p1_y='L14', p1_z='L15')
define(scale='L16')
send 1"fwd_inp(%f) %f %f %f %f\\n",D0,qCX,qCZ,qW,qFY''') {p0_x}={x_0}+{r_0}*cos({phi_0}+{qW})
for i in range(2): {p1_x}={x_1}+{r_1}*cos({phi_1}+{qW})
#https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list {p0_y}={y_0}
l=[j for i in zip((i,) * param.shape[1], list(param[i])) for j in i] {p1_y}={y_1}
prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)"%tuple(l)) {p0_z}={z_0}-{r_0}*sin({phi_0}+{qW})
prg.append(" W=qW") {p1_z}={z_1}-{r_1}*sin({phi_1}+{qW})
prg.append(" qW=qW*%g"%(d2r/1000.)) #scale from 1000*deg to rad
prg.append('''
p0_x=x_0+r_0*cos(phi_0+qW)
p1_x=x_1+r_1*cos(phi_1+qW)
p0_y=y_0
p1_y=y_1
p0_z=z_0-r_0*sin(phi_0+qW)
p1_z=z_1-r_1*sin(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_x}={p0_x}+{scale}*({p1_x}-{p0_x})
p0_y=p0_y+scale*(p1_y-p0_y) {p0_y}={p0_y}+{scale}*({p1_y}-{p0_y})
p0_z=p0_z+scale*(p1_z-p0_z) {p0_z}={p0_z}+{scale}*({p1_z}-{p0_z})
DX=qCX-p0_x {DX}={qCX}-{p0_x}
DZ=qCZ-p0_z {DZ}={qCZ}-{p0_z}
Y=qFY {Y}={qFY}
//send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y //send 1"fwd_res %f %f %f %f\\n",{DX},{DZ},{W},{Y}
//P1001+=1 //P1001+=1
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2 D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
close close
''') '''.format(**subs)
prg.append(''' #coord X Z B Y
subs={ 'DX':'C6' , 'DZ':'C8' , 'W':'C1', 'Y':'C7',
'vDX':'C38', 'vDZ':'C40', 'vW':'C33', 'vY':'C39',
#motor q4 q5 q3 q1
'qCX':'L4', 'qCZ':'L5', 'qW':'L3', 'qFY':'L1',
'vqCX':'R4', 'vqCZ':'R5', 'vqW':'R3', 'vqFY':'R1',
'p0_x':'L10', 'p0_y':'L11', 'p0_z':'L12',
'p1_x':'L13', 'p1_y':'L14', 'p1_z':'L15',
'p_x':'L16', 'p_y':'L17', 'p_z':'L18',
'sclY':'L19',
'scl':'L20'}
subs.update(subsParam)
prg+='''
open inverse open inverse
//coord X Z B Y
define( DX='C6' , DZ='C8' , W='C1', Y='C7')
define(vDX='C38', vDZ='C40', vW='C33', vY='C39')
//motor q4 q5 q3 q1
define( qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(vqCX='R4', vqCZ='R5', vqW='R3', vqFY='R1')
define(p0_x='L10', p0_y='L11', p0_z='L12')
define(p1_x='L13', p1_y='L14', p1_z='L15')
define(p_x='L16', p_y='L17', p_z='L18')
define(sclY='L19')
define(scl='L20')
//if(D0>0) //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 // send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,{DX},{vDX},{DZ},{vDZ},{W},{vW},{Y},{vY}
//else //else
// send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y''') // send 1"inv_inp(%f) %f %f %f %f\\n",D0,{DX},{DZ},{W},{Y}
for i in range(2): {qW}={W}
# https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list {W}={W}*{d2r} //scale from 1000*deg to rad
l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)" % tuple(l))
prg.append(" qW=W")
prg.append(" W=W*%g"%(d2r/1000.)) #scale from 1000*deg to rad
prg.append('''
p0_x=x_0+r_0*cos(phi_0+W) {p0_x}={x_0}+{r_0}*cos({phi_0}+{W})
p1_x=x_1+r_1*cos(phi_1+W) {p1_x}={x_1}+{r_1}*cos({phi_1}+{W})
p0_y=y_0 {p0_y}={y_0}
p1_y=y_1 {p1_y}={y_1}
p0_z=z_0-r_0*sin(phi_0+W) {p0_z}={z_0}-{r_0}*sin({phi_0}+{W})
p1_z=z_1-r_1*sin(phi_1+W) {p1_z}={z_1}-{r_1}*sin({phi_1}+{W})
sclY=(Y-(y_0))/(y_1-(y_0)) {sclY}=({Y}-({y_0}))/({y_1}-({y_0}))
p_x=p0_x+sclY*(p1_x-p0_x) {p_x}={p0_x}+{sclY}*({p1_x}-{p0_x})
p_y=p0_y+sclY*(p1_y-p0_y) {p_y}={p0_y}+{sclY}*({p1_y}-{p0_y})
p_z=p0_z+sclY*(p1_z-p0_z) {p_z}={p0_z}+{sclY}*({p1_z}-{p0_z})
qCX=DX+p_x {qCX}={DX}+{p_x}
qCZ=DZ+p_z {qCZ}={DZ}+{p_z}
qFY=Y {qFY}={Y}
if(D0>0) if(D0>0)
{ // calculate velocities for PVT motion''') {{ // calculate velocities for PVT motion
prg.append(" vW=vW*%g"%(d2r/1000.)) #scale from 1000*deg to rad {vW}={vW}*{d2r} //scale from 1000*deg to rad
prg.append(''' p_x=sclY*(p1_x-p0_x) {p_x}={sclY}*({p1_x}-{p0_x})
p_z=sclY*(p1_z-p0_z) {p_z}={sclY}*({p1_z}-{p0_z})
vqFY=vY {vqFY}={vY}
vqCX=vDX + (p1_x-p0_x)/(p1_y-p0_y)*vY //+ vqW*p_z {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 {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=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 {vqW}={vW}//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x
''') {vqW}={vqW}*{r2d} //scale from rad to 1000*deg
prg.append(" vqW=vqW*%g"%(1000./d2r)) #scale from rad to 1000*deg // send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",{qCX},{vqCX},{qCZ},{vqCZ},{qW},{vqW},{qFY},{vqFY}
prg.append('''// send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY }}
}
//else //else
// send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY // send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY}
//P1002+=1 //P1002+=1
close close
''') '''.format(**subs)
# vqCX=vDX + (p1_x-p0_x)/(p1_y-p0_y)*vY + vqW*p_x # vqCX=vDX + (p1_x-p0_x)/(p1_y-p0_y)*vY + vqW*p_x
# vDX is in same direction, so add as it is # vDX is in same direction, so add as it is
@@ -788,11 +789,10 @@ close
time.sleep(.5) time.sleep(.5)
gpascii.send_block(prg) gpascii.send_block(prg)
if self.verbose & 4: if self.verbose & 4:
for ln in prg: print(prg)
print(ln)
if fnCrdTrf is not None : if fnCrdTrf is not None :
fh=open(fnCrdTrf,'w') fh=open(fnCrdTrf,'w')
fh.write('\n'.join(prg)) fh.write(prg)
fh.close() fh.close()
time.sleep(.5) time.sleep(.5)
gpascii.send_block('enable plc 0') gpascii.send_block('enable plc 0')