wip
This commit is contained in:
@@ -664,12 +664,10 @@ class HelicalScan(MotionBase):
|
||||
comm = self.comm
|
||||
gpascii = comm.gpascii
|
||||
param=self.param
|
||||
prg = []
|
||||
prg.append('''
|
||||
prg = '''
|
||||
// Set the motors as inverse kinematic axes in CS 1
|
||||
//motors CX CZ RY FY
|
||||
// 4 5 3 1
|
||||
|
||||
&1
|
||||
a
|
||||
#4->0
|
||||
@@ -681,103 +679,106 @@ a
|
||||
#5->I
|
||||
#3->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
|
||||
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
|
||||
define(DX='C6', DZ='C8', W='C1', Y='C7')
|
||||
//coord X Z B Y
|
||||
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}
|
||||
{W}={qW}
|
||||
{qW}={qW}*{d2r} //scale from 1000*deg to rad
|
||||
|
||||
send 1"fwd_inp(%f) %f %f %f %f\\n",D0,qCX,qCZ,qW,qFY''')
|
||||
for i in range(2):
|
||||
#https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
|
||||
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(" W=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)
|
||||
{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))
|
||||
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)
|
||||
DX=qCX-p0_x
|
||||
DZ=qCZ-p0_z
|
||||
Y=qFY
|
||||
//send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y
|
||||
{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})
|
||||
{DX}={qCX}-{p0_x}
|
||||
{DZ}={qCZ}-{p0_z}
|
||||
{Y}={qFY}
|
||||
//send 1"fwd_res %f %f %f %f\\n",{DX},{DZ},{W},{Y}
|
||||
//P1001+=1
|
||||
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
|
||||
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
|
||||
//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)
|
||||
// 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
|
||||
// send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y''')
|
||||
for i in range(2):
|
||||
# https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
|
||||
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('''
|
||||
// send 1"inv_inp(%f) %f %f %f %f\\n",D0,{DX},{DZ},{W},{Y}
|
||||
{qW}={W}
|
||||
{W}={W}*{d2r} //scale from 1000*deg to rad
|
||||
|
||||
p0_x=x_0+r_0*cos(phi_0+W)
|
||||
p1_x=x_1+r_1*cos(phi_1+W)
|
||||
p0_y=y_0
|
||||
p1_y=y_1
|
||||
p0_z=z_0-r_0*sin(phi_0+W)
|
||||
p1_z=z_1-r_1*sin(phi_1+W)
|
||||
{p0_x}={x_0}+{r_0}*cos({phi_0}+{W})
|
||||
{p1_x}={x_1}+{r_1}*cos({phi_1}+{W})
|
||||
{p0_y}={y_0}
|
||||
{p1_y}={y_1}
|
||||
{p0_z}={z_0}-{r_0}*sin({phi_0}+{W})
|
||||
{p1_z}={z_1}-{r_1}*sin({phi_1}+{W})
|
||||
|
||||
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
|
||||
{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''')
|
||||
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
|
||||
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
|
||||
{{ // calculate velocities for PVT motion
|
||||
{vW}={vW}*{d2r} //scale from 1000*deg to rad
|
||||
{p_x}={sclY}*({p1_x}-{p0_x})
|
||||
{p_z}={sclY}*({p1_z}-{p0_z})
|
||||
{vqFY}={vY}
|
||||
{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
|
||||
}
|
||||
{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
|
||||
// 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
|
||||
// send 1"inv_res %f %f %f %f\\n",{qCX},{qCZ},{qW},{qFY}
|
||||
//P1002+=1
|
||||
close
|
||||
''')
|
||||
'''.format(**subs)
|
||||
|
||||
# vqCX=vDX + (p1_x-p0_x)/(p1_y-p0_y)*vY + vqW*p_x
|
||||
# vDX is in same direction, so add as it is
|
||||
@@ -788,11 +789,10 @@ close
|
||||
time.sleep(.5)
|
||||
gpascii.send_block(prg)
|
||||
if self.verbose & 4:
|
||||
for ln in prg:
|
||||
print(ln)
|
||||
print(prg)
|
||||
if fnCrdTrf is not None :
|
||||
fh=open(fnCrdTrf,'w')
|
||||
fh.write('\n'.join(prg))
|
||||
fh.write(prg)
|
||||
fh.close()
|
||||
time.sleep(.5)
|
||||
gpascii.send_block('enable plc 0')
|
||||
|
||||
Reference in New Issue
Block a user