diff --git a/python/helicalscan.py b/python/helicalscan.py index 7463a2a..eb7f2bd 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -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')