work on helicalscan
This commit is contained in:
22
Readme.md
22
Readme.md
@@ -191,9 +191,23 @@ HelicalScan
|
||||
PPMAC=MOTTEST-CPPM-CRM0485
|
||||
gpasciiCommander --host $PPMAC mx-stage_sim.cfg -i
|
||||
|
||||
Coord[1].SegMoveTime=.05
|
||||
will calculate all 0.05 sec the inverse kinematic.
|
||||
default Coord[1].SegMoveTime=0, calculates inv kin. only at endpoints
|
||||
|
||||
|
||||
ssh root@$PPMAC sendgetsends -1
|
||||
send 1"SampleMessage\n"
|
||||
|
||||
execute helicalscan.py
|
||||
|
||||
```
|
||||
|
||||
HelicalScan SCRATCH
|
||||
-------------------
|
||||
```
|
||||
|
||||
|
||||
&1p ->this will trigger:forward kinematic
|
||||
cpx pmatch ->this will trigger:forward kinematic
|
||||
|
||||
@@ -224,10 +238,12 @@ Motor[1,27,8].JogSpeed=8
|
||||
cpx ;linear abs; X0.2 Z0.3 B0.1 Y00000
|
||||
cpx ;linear abs; X0.2 Z0.3 B0.1 Y10000
|
||||
|
||||
Coord[1].SegMoveTime=.05
|
||||
will calculate all 0.05 sec the inverse kinematic.
|
||||
default Coord[1].SegMoveTime=0, calculates inv kin. only at endpoints
|
||||
input : dx:0.2 dz:0.3 w:5.72958 fy:3.3
|
||||
inv_trf: cx:2.34244 cz:18.2563 w:5.72958 fy:3.3
|
||||
fwd_trf: dx:0.2 dz:0.3 w:5.72958 fy:3.3
|
||||
|
||||
#7j=2.34244;#8j=18.2563;#1j=5729.58;#2j=3.3
|
||||
&1p
|
||||
|
||||
```
|
||||
|
||||
|
||||
@@ -150,11 +150,11 @@ class HelicalScan:
|
||||
def test_coord_trf(self):
|
||||
param = self.param
|
||||
dx, dz, w, y, = (0.2,0.3,0.1,3.3)
|
||||
print 'input : dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)
|
||||
print 'input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r,y)
|
||||
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
|
||||
print 'inv_trf: cx:%.3g cz:%.3g w:%.3g fy:%.3g' % (cx,cz,w/d2r,fy)
|
||||
print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r,fy)
|
||||
(dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy)
|
||||
print 'fwd_trf: dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)
|
||||
print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r,y)
|
||||
|
||||
# plt.ion()
|
||||
# fig = plt.figure()
|
||||
@@ -165,6 +165,18 @@ class HelicalScan:
|
||||
# def my_anim_func3(self,idx):
|
||||
# self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist)
|
||||
|
||||
def show_vel(self):
|
||||
rec=self.rec
|
||||
fig = plt.figure()
|
||||
#y = np.diff(rec[:, 2])
|
||||
y = np.diff(rec,axis=0)
|
||||
mx=y.max(0)
|
||||
mn=y.min(0)
|
||||
y=y/(mx-mn)
|
||||
x = np.arange(0, y.shape[0]);
|
||||
x= x.reshape(-1,1).dot(np.ones((1,y.shape[1])))
|
||||
plt.plot(x, y)
|
||||
|
||||
def interactive_cx_cz_w_fy(self):
|
||||
fig = plt.figure()
|
||||
self.manip=False#True#False
|
||||
@@ -336,6 +348,7 @@ class HelicalScan:
|
||||
animCnt=100
|
||||
self.step=self.rec.shape[0]/animCnt
|
||||
a = anim.FuncAnimation(fig, self.anim_gather_data, animCnt, fargs=(), interval=20, repeat=False, blit=False)
|
||||
self.show_vel()
|
||||
plt.show()
|
||||
|
||||
|
||||
@@ -568,7 +581,7 @@ open forward
|
||||
DZ=qCZ-p0_z
|
||||
Y=qFY
|
||||
send 1"forward result %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
|
||||
close
|
||||
''')
|
||||
@@ -605,7 +618,7 @@ open inverse
|
||||
qCZ=DZ+p0_z
|
||||
qFY=Y
|
||||
send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
|
||||
P1002+=1
|
||||
//P1002+=1
|
||||
|
||||
close
|
||||
''')
|
||||
@@ -676,18 +689,19 @@ close
|
||||
#w=0..3600000 # 10 rev
|
||||
pos=np.array([[0, 0, 0, 2.300],
|
||||
[0, 0, 360000, 6.200],])
|
||||
cnt=13
|
||||
pos=np.zeros((cnt,4))
|
||||
pos[:,2]= np.linspace(0,3600000,cnt)
|
||||
pos[:,3]= np.linspace(2.3,6.2,cnt)
|
||||
numPt=13
|
||||
pos=np.zeros((numPt,4))
|
||||
pos[:,2]= np.linspace(0,3600000,numPt)
|
||||
pos[:,3]= np.linspace(2.3,6.2,numPt)
|
||||
#[0, 0, 3600000, 6.200],])
|
||||
#prg.append('Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics
|
||||
prg.append(' Coord[1].SegMoveTime=.05')
|
||||
prg.append(' linear abs')
|
||||
prg.append(' X(%g) Z(%g) B(%g) Y(%g)' % tuple(pos[0, :]))
|
||||
prg.append(' X%g Z%g B%g Y%g' % tuple(pos[0, :]))
|
||||
prg.append(' dwell 100')
|
||||
prg.append(' Gather.Enable=2')
|
||||
for i in range(pos.shape[0]):
|
||||
prg.append(' X(%g) Z(%g) B(%g) Y(%g)' % tuple(pos[i, :]))
|
||||
prg.append(' X%g Z%g B%g Y%g' % tuple(pos[i, :]))
|
||||
prg.append(' dwell 100')
|
||||
prg.append(' Gather.Enable=0')
|
||||
elif mode==1: #### pvt motion
|
||||
@@ -700,55 +714,56 @@ close
|
||||
cnt=kwargs['cnt'] #move path multiple times
|
||||
except KeyError:
|
||||
cnt=1
|
||||
|
||||
try:
|
||||
pt=self.ptsCorr
|
||||
except AttributeError:
|
||||
pt=self.points
|
||||
numPt=13
|
||||
pt=np.zeros((numPt,4))
|
||||
pt[:,2]= np.linspace(0,3600000,numPt)
|
||||
pt[:,3]= np.linspace(2.3,6.2,numPt)
|
||||
vel=pt[2:,:]-pt[:-2,:]
|
||||
#pv is an array of posx posy velx vely
|
||||
pv=np.ndarray(shape=(pt.shape[0]+2,4),dtype=pt.dtype)
|
||||
pv=np.ndarray(shape=(pt.shape[0]+2,8),dtype=pt.dtype)
|
||||
pv[:]=np.NaN
|
||||
#pv[ 0,(0,1)]=2*pt[0,:]-pt[1,:]
|
||||
pv[ 0,(0,1)]=pt[0,:]
|
||||
pv[ 1:-1,(0,1)]=pt
|
||||
pv[ 0,(0,1,2,3)]=pt[0,:]
|
||||
pv[ 1:-1,(0,1,2,3)]=pt
|
||||
#pv[ -1,(0,1)]=2*pt[-1,:]-pt[-2,:]
|
||||
pv[ -1,(0,1)]=pt[-1,:]
|
||||
pv[(0,0,-1,-1),(2,3,2,3)]=0
|
||||
dist=pv[2:,(0,1)] - pv[:-2,(0,1)]
|
||||
pv[ 1:-1,(2,3)] = 1000.*dist/(2.*pt2pt_time)
|
||||
pv[ -1,(0,1,2,3)]=pt[-1,:]
|
||||
pv[(0,0,0,0,-1,-1,-1,-1),(4,5,6,7,4,5,6,7)]=0
|
||||
dist=pv[2:,(0,1,2,3)] - pv[:-2,(0,1,2,3)]
|
||||
pv[ 1:-1,(4,5,6,7)] = 1000.*dist/(2.*pt2pt_time)
|
||||
|
||||
prg.append(' linear abs')
|
||||
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
|
||||
prg.append('dwell 10')
|
||||
prg.append('Gather.Enable=2')
|
||||
prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0,1,2,3)]))
|
||||
prg.append(' dwell 10')
|
||||
prg.append(' Gather.Enable=2')
|
||||
if cnt>1:
|
||||
prg.append('P100=%d'%cnt)
|
||||
prg.append('N100:')
|
||||
prg.append(' P100=%d'%cnt)
|
||||
prg.append(' N100:')
|
||||
prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position
|
||||
for idx in range(1,pv.shape[0]):
|
||||
prg.append('X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)]))
|
||||
prg.append('X%g Y%g' % tuple(pv[-1, (0,1)]))
|
||||
prg.append(' X%g:%g Z%g:%g B%g:%g Y%g:%g' % tuple(pv[idx, (0,4,1,5,2,6,3,7)]))
|
||||
if cnt>1:
|
||||
prg.append('dwell 10')
|
||||
prg.append('P100=P100-1')
|
||||
prg.append('if(P100>0)')
|
||||
prg.append('{')
|
||||
prg.append(' linear abs')
|
||||
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
|
||||
prg.append('dwell 100')
|
||||
prg.append('goto 100')
|
||||
prg.append('}')
|
||||
prg.append(' dwell 10')
|
||||
prg.append(' P100=P100-1')
|
||||
prg.append(' if(P100>0)')
|
||||
prg.append(' {')
|
||||
prg.append(' linear abs')
|
||||
prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0, 1, 2, 3)]))
|
||||
prg.append(' dwell 100')
|
||||
prg.append(' goto 100')
|
||||
prg.append(' }')
|
||||
else:
|
||||
prg.append('dwell 1000')
|
||||
prg.append('Gather.Enable=0')
|
||||
prg.append(' dwell 1000')
|
||||
prg.append(' Gather.Enable=0')
|
||||
elif mode==2: #### spline motion
|
||||
try:
|
||||
pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
|
||||
except KeyError:
|
||||
print('missing pt2pt_time, use default=100ms')
|
||||
pt2pt_time=100.
|
||||
pos=self.points
|
||||
cnt=13
|
||||
pos=np.zeros((cnt,4))
|
||||
pos[:,2]= np.linspace(0,3600000,cnt)
|
||||
pos[:,3]= np.linspace(2.3,6.2,cnt)
|
||||
pcor=np.ndarray(pos.shape,dtype=pos.dtype);pcor[:]=np.NaN
|
||||
pcor[(0,-1),:]=pos[(0,-1),:]
|
||||
pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6.
|
||||
@@ -826,7 +841,6 @@ class GpasciiCommunicator():
|
||||
if verbose: print(s)
|
||||
return p
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
from optparse import OptionParser, IndentedHelpFormatter
|
||||
class MyFormatter(IndentedHelpFormatter):
|
||||
@@ -865,36 +879,21 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
|
||||
hs=HelicalScan(args)
|
||||
hs.args.verbose = 255
|
||||
#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)
|
||||
return
|
||||
#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.test_coord_trf()
|
||||
#hs.interactive_dx_dz_w_y()
|
||||
#hs.interactive_cx_cz_w_fy()
|
||||
|
||||
|
||||
#hs.interactive_dx_dz_w_y()
|
||||
|
||||
hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485')
|
||||
#hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485')
|
||||
#hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=-1)
|
||||
hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0)
|
||||
#hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1)
|
||||
hs.interactive_anim()
|
||||
return
|
||||
hs.interactive_dx_dz_w_y()
|
||||
hs.interactive_cx_cz_w_fy()
|
||||
|
||||
|
||||
hs.test_coord_trf()
|
||||
|
||||
hs.test_find_rot_ctr()
|
||||
hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
|
||||
hs.interactive_cx_cz_w_fy()
|
||||
hs.interactive_dx_dz_w_y()
|
||||
|
||||
|
||||
#------------------ Main Code ----------------------------------
|
||||
#ssh_test()
|
||||
ret=parse_args()
|
||||
exit(ret)
|
||||
|
||||
Reference in New Issue
Block a user