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

work on helicalscan

Browse Source
This commit is contained in:
zamofing_t
2017-12-13 10:54:33 +01:00
parent fec82fb362
commit a74197c8ba
2 changed files with 80 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
Readme.md
View File

@@ -191,9 +191,23 @@ HelicalScan
PPMAC=MOTTEST-CPPM-CRM0485 PPMAC=MOTTEST-CPPM-CRM0485
gpasciiCommander --host $PPMAC mx-stage_sim.cfg -i 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 ssh root@$PPMAC sendgetsends -1
send 1"SampleMessage\n" send 1"SampleMessage\n"
execute helicalscan.py
```
HelicalScan SCRATCH
-------------------
```
&1p ->this will trigger:forward kinematic &1p ->this will trigger:forward kinematic
cpx pmatch ->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 Y00000
cpx ;linear abs; X0.2 Z0.3 B0.1 Y10000 cpx ;linear abs; X0.2 Z0.3 B0.1 Y10000
Coord[1].SegMoveTime=.05 input : dx:0.2 dz:0.3 w:5.72958 fy:3.3
will calculate all 0.05 sec the inverse kinematic. inv_trf: cx:2.34244 cz:18.2563 w:5.72958 fy:3.3
default Coord[1].SegMoveTime=0, calculates inv kin. only at endpoints 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
``` ```

123
python/helicalscan.py
View File

@@ -150,11 +150,11 @@ class HelicalScan:
def test_coord_trf(self): def test_coord_trf(self):
param = self.param param = self.param
dx, dz, w, y, = (0.2,0.3,0.1,3.3) 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) (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) (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() # plt.ion()
# fig = plt.figure() # fig = plt.figure()
@@ -165,6 +165,18 @@ class HelicalScan:
# def my_anim_func3(self,idx): # def my_anim_func3(self,idx):
# self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist) # 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): def interactive_cx_cz_w_fy(self):
fig = plt.figure() fig = plt.figure()
self.manip=False#True#False self.manip=False#True#False
@@ -336,6 +348,7 @@ class HelicalScan:
animCnt=100 animCnt=100
self.step=self.rec.shape[0]/animCnt self.step=self.rec.shape[0]/animCnt
a = anim.FuncAnimation(fig, self.anim_gather_data, animCnt, fargs=(), interval=20, repeat=False, blit=False) a = anim.FuncAnimation(fig, self.anim_gather_data, animCnt, fargs=(), interval=20, repeat=False, blit=False)
self.show_vel()
plt.show() plt.show()
@@ -568,7 +581,7 @@ open forward
DZ=qCZ-p0_z DZ=qCZ-p0_z
Y=qFY Y=qFY
send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y 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 D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
close close
''') ''')
@@ -605,7 +618,7 @@ open inverse
qCZ=DZ+p0_z qCZ=DZ+p0_z
qFY=Y qFY=Y
send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
P1002+=1 //P1002+=1
close close
''') ''')
@@ -676,18 +689,19 @@ close
#w=0..3600000 # 10 rev #w=0..3600000 # 10 rev
pos=np.array([[0, 0, 0, 2.300], pos=np.array([[0, 0, 0, 2.300],
[0, 0, 360000, 6.200],]) [0, 0, 360000, 6.200],])
cnt=13 numPt=13
pos=np.zeros((cnt,4)) pos=np.zeros((numPt,4))
pos[:,2]= np.linspace(0,3600000,cnt) pos[:,2]= np.linspace(0,3600000,numPt)
pos[:,3]= np.linspace(2.3,6.2,cnt) pos[:,3]= np.linspace(2.3,6.2,numPt)
#[0, 0, 3600000, 6.200],]) #[0, 0, 3600000, 6.200],])
#prg.append('Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics #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(' 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(' dwell 100')
prg.append(' Gather.Enable=2') prg.append(' Gather.Enable=2')
for i in range(pos.shape[0]): 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(' dwell 100')
prg.append(' Gather.Enable=0') prg.append(' Gather.Enable=0')
elif mode==1: #### pvt motion elif mode==1: #### pvt motion
@@ -700,55 +714,56 @@ close
cnt=kwargs['cnt'] #move path multiple times cnt=kwargs['cnt'] #move path multiple times
except KeyError: except KeyError:
cnt=1 cnt=1
numPt=13
try: pt=np.zeros((numPt,4))
pt=self.ptsCorr pt[:,2]= np.linspace(0,3600000,numPt)
except AttributeError: pt[:,3]= np.linspace(2.3,6.2,numPt)
pt=self.points
vel=pt[2:,:]-pt[:-2,:] vel=pt[2:,:]-pt[:-2,:]
#pv is an array of posx posy velx vely #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[:]=np.NaN
#pv[ 0,(0,1)]=2*pt[0,:]-pt[1,:] #pv[ 0,(0,1)]=2*pt[0,:]-pt[1,:]
pv[ 0,(0,1)]=pt[0,:] pv[ 0,(0,1,2,3)]=pt[0,:]
pv[ 1:-1,(0,1)]=pt pv[ 1:-1,(0,1,2,3)]=pt
#pv[ -1,(0,1)]=2*pt[-1,:]-pt[-2,:] #pv[ -1,(0,1)]=2*pt[-1,:]-pt[-2,:]
pv[ -1,(0,1)]=pt[-1,:] pv[ -1,(0,1,2,3)]=pt[-1,:]
pv[(0,0,-1,-1),(2,3,2,3)]=0 pv[(0,0,0,0,-1,-1,-1,-1),(4,5,6,7,4,5,6,7)]=0
dist=pv[2:,(0,1)] - pv[:-2,(0,1)] dist=pv[2:,(0,1,2,3)] - pv[:-2,(0,1,2,3)]
pv[ 1:-1,(2,3)] = 1000.*dist/(2.*pt2pt_time) pv[ 1:-1,(4,5,6,7)] = 1000.*dist/(2.*pt2pt_time)
prg.append(' linear abs') prg.append(' linear abs')
prg.append('X%g Y%g' % tuple(pv[0, (0,1)])) prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0,1,2,3)]))
prg.append('dwell 10') prg.append(' dwell 10')
prg.append('Gather.Enable=2') prg.append(' Gather.Enable=2')
if cnt>1: if cnt>1:
prg.append('P100=%d'%cnt) prg.append(' P100=%d'%cnt)
prg.append('N100:') prg.append(' N100:')
prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position
for idx in range(1,pv.shape[0]): 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:%g Z%g:%g B%g:%g Y%g:%g' % tuple(pv[idx, (0,4,1,5,2,6,3,7)]))
prg.append('X%g Y%g' % tuple(pv[-1, (0,1)]))
if cnt>1: if cnt>1:
prg.append('dwell 10') prg.append(' dwell 10')
prg.append('P100=P100-1') prg.append(' P100=P100-1')
prg.append('if(P100>0)') prg.append(' if(P100>0)')
prg.append('{') prg.append(' {')
prg.append(' linear abs') prg.append(' linear abs')
prg.append('X%g Y%g' % tuple(pv[0, (0,1)])) prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0, 1, 2, 3)]))
prg.append('dwell 100') prg.append(' dwell 100')
prg.append('goto 100') prg.append(' goto 100')
prg.append('}') prg.append(' }')
else: else:
prg.append('dwell 1000') prg.append(' dwell 1000')
prg.append('Gather.Enable=0') prg.append(' Gather.Enable=0')
elif mode==2: #### spline motion elif mode==2: #### spline motion
try: try:
pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time) pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
except KeyError: except KeyError:
print('missing pt2pt_time, use default=100ms') print('missing pt2pt_time, use default=100ms')
pt2pt_time=100. 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=np.ndarray(pos.shape,dtype=pos.dtype);pcor[:]=np.NaN
pcor[(0,-1),:]=pos[(0,-1),:] pcor[(0,-1),:]=pos[(0,-1),:]
pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6. pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6.
@@ -826,7 +841,6 @@ class GpasciiCommunicator():
if verbose: print(s) if verbose: print(s)
return p return p
if __name__=='__main__': if __name__=='__main__':
from optparse import OptionParser, IndentedHelpFormatter from optparse import OptionParser, IndentedHelpFormatter
class MyFormatter(IndentedHelpFormatter): class MyFormatter(IndentedHelpFormatter):
@@ -865,36 +879,21 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
hs=HelicalScan(args) hs=HelicalScan(args)
hs.args.verbose = 255 hs.args.verbose = 255
#hs.sequencer() #hs.sequencer()
hs.test_find_rot_ctr() #hs.test_find_rot_ctr()
hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6) #hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
return
hs.calcParam() hs.calcParam()
hs.test_coord_trf() hs.test_coord_trf()
#hs.interactive_dx_dz_w_y()
#hs.interactive_cx_cz_w_fy() #hs.interactive_cx_cz_w_fy()
#hs.interactive_dx_dz_w_y() #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=-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=0)
#hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1)
hs.interactive_anim() hs.interactive_anim()
return 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 ---------------------------------- #------------------ Main Code ----------------------------------
#ssh_test()
ret=parse_args() ret=parse_args()
exit(ret) exit(ret)