diff --git a/Readme.md b/Readme.md index caa8438..f9cb4a4 100644 --- a/Readme.md +++ b/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 ``` diff --git a/python/helicalscan.py b/python/helicalscan.py index c6fe324..06ea081 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -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)