From 5ee34c5f1125d0d88f5ad681d28764dd0ff79c60 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Thu, 18 Jan 2018 15:20:52 +0100 Subject: [PATCH] cleanup and commission wedge mover --- Readme.md | 11 +++-- cfg/MX2_home.cfg | 99 +++++++++++++++++++++---------------------- python/helicalscan.py | 38 ++++++++++------- 3 files changed, 77 insertions(+), 71 deletions(-) diff --git a/Readme.md b/Readme.md index 0d44709..9d2706b 100644 --- a/Readme.md +++ b/Readme.md @@ -324,9 +324,12 @@ gpasciiCommander --host $PPMAC -i $$$*** !common() !SAR-EXPMX2() -Motor[1].pLimits=0 -Motor[2].pLimits=0 -Motor[3].pLimits=0 -Motor[4].pLimits=0 ``` + +!!! WATCH OUT !!! +if the send buffer overruns, the programs will abort in an unexplainable way: + send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY +without reading it with sendgetsends -1 + + diff --git a/cfg/MX2_home.cfg b/cfg/MX2_home.cfg index aa67f4b..6f4e0ec 100644 --- a/cfg/MX2_home.cfg +++ b/cfg/MX2_home.cfg @@ -6,65 +6,62 @@ define(PLC_Homing='1') //move both mottors at same time //move to limit switch, gather data, move to other limit switch, gather stop. move back to first limit switch -//motID :P0 : Motor Number -//direction :P1 : Motion direction to first limit switch: -1 -> #1j- ,+1 -> #1j+ //status :P100: local status -//timer :P101: local timer +//timer :L0: local timer open plc PLC_Homing -define(motID='L0',direction='P1',status='P100',timer='P101') -jog-1; jog-2; jog-3; jog-4; -Motor[1].HomeVel=0.5 -Motor[2].HomeVel=0.5 -Motor[3].HomeVel=0.5 -Motor[4].HomeVel=0.5 -status=10 -while(1) -{ - if((Motor[1].MinusLimit==1 || Motor[1].FeFatal==1)){ - if((Motor[2].MinusLimit==1 || Motor[2].FeFatal==1)){ - if((Motor[3].MinusLimit==1 || Motor[3].FeFatal==1)){ - if((Motor[4].MinusLimit==1 || Motor[4].FeFatal==1)){ - status=11 - break - }}}} -} -timer = Sys.RunTime + 2 -status=100 -while (Sys.RunTime < timer){} //wait 2 sec -status=101 -jog1:1000; jog2:1000; jog3:1000; jog4:1000; -timer = Sys.RunTime + 2 -status=200 -while (Sys.RunTime < timer){} //wait 2 sec -status=201 -while(1) -{ - if(Motor[1].DesVelZero==1 && Motor[2].DesVelZero==1 && Motor[3].DesVelZero==1 && Motor[4].DesVelZero==1) + define(status='P100',timer='L0') + jog-1; jog-2; jog-3; jog-4; + Motor[1].HomeVel=0.5 + Motor[2].HomeVel=0.5 + Motor[3].HomeVel=0.5 + Motor[4].HomeVel=0.5 + status=1 + + while(1) { - status=12 + if((Motor[1].MinusLimit==1 || Motor[1].FeFatal==1)){ + if((Motor[2].MinusLimit==1 || Motor[2].FeFatal==1)){ + if((Motor[3].MinusLimit==1 || Motor[3].FeFatal==1)){ + if((Motor[4].MinusLimit==1 || Motor[4].FeFatal==1)){ + status=2 break + }}}} } -} -timer = Sys.RunTime + 2 -while (Sys.RunTime < timer){} -home 1; home 2; home 3; home 4 -status=13 -while(1) //wait homing index found done -{ - if(Motor[1].HomeComplete==1 && Motor[2].HomeComplete==1 && Motor[3].HomeComplete==1 && Motor[4].HomeComplete==1) + timer = Sys.RunTime + 2 + while (Sys.RunTime < timer){} //wait 1 sec + status=3 + jog1:1000; jog2:1000; jog3:1000; jog4:1000; + timer = Sys.RunTime + .1 + while (Sys.RunTime < timer){} //wait 1 sec + status=4 + while(1) { - status=14 - break + if(Motor[1].DesVelZero==1 && Motor[2].DesVelZero==1 && Motor[3].DesVelZero==1 && Motor[4].DesVelZero==1) + { + status=5 + break + } } -} -while(1) //wait moving to zero position done -{ - if(Motor[1].DesVelZero==1 && Motor[2].DesVelZero==1 && Motor[3].DesVelZero==1 && Motor[4].DesVelZero==1) + timer = Sys.RunTime + .1 + while (Sys.RunTime < timer){} + home 1; home 2; home 3; home 4 + status=6 + while(1) //wait homing index found done { - status=15 - break + if(Motor[1].HomeComplete==1 && Motor[2].HomeComplete==1 && Motor[3].HomeComplete==1 && Motor[4].HomeComplete==1) + { + status=7 + break + } } -} -disable plc PLC_Homing + while(1) //wait moving to zero position done + { + if(Motor[1].DesVelZero==1 && Motor[2].DesVelZero==1 && Motor[3].DesVelZero==1 && Motor[4].DesVelZero==1) + { + status=8 + break + } + } + disable plc PLC_Homing close diff --git a/python/helicalscan.py b/python/helicalscan.py index 7f20487..ed803f3 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -689,7 +689,7 @@ open forward 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''') + //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] @@ -711,7 +711,7 @@ open forward DX=qCX-p0_x DZ=qCZ-p0_z Y=qFY - send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y + //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 @@ -732,10 +732,10 @@ open inverse 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 - else - send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y''') + //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 + //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] @@ -770,10 +770,10 @@ open inverse 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 + prg.append('''// 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 + //else + // send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY //P1002+=1 close ''') @@ -880,9 +880,11 @@ close prg.append(' Gather.Enable=2') prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed prg.append(' Coord[1].SegMoveTime=0') #to calculate every 1 ms the inverse kinematics + prg.append(' P2000=0') for i in range(pt.shape[0]): prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :])) - prg.append(' dwell 100') + #prg.append(' P2000=%d'%(i,)) + prg.append(' dwell 10') prg.append(' Gather.Enable=0') # ******** mode 1 ******** @@ -1096,8 +1098,8 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' hs.calcParam() #hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i) #hs.param[1]=(15,4,0,3,0)#(z_i, y_i, x_i, r_i,phi_i) - #hs.param[0]=(-100, 10,0,50,0)#(z_i, y_i, x_i, r_i,phi_i) - #hs.param[1]=(-100,-10,0,70,0)#(z_i, y_i, x_i, r_i,phi_i) + hs.param[0]=(-100, 100,0,50,0)#(z_i, y_i, x_i, r_i,phi_i) + hs.param[1]=(-100,-100,0,70,0)#(z_i, y_i, x_i, r_i,phi_i) hs.test_coord_trf() #hs.interactive_cx_cz_w_fy() @@ -1107,7 +1109,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' #0:1 config simulated motors #1:2 config real motors #2:4 config coord trf - mode=6#6#5#4#0 + mode=6#5#4#0 os.chdir(os.path.join(os.path.dirname(__file__),'../cfg')) if mode&1: hs.download(file='sim_8_motors.cfg') @@ -1123,7 +1125,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' hs.download(['enable plc 0',]) time.sleep(.5) hs.download(['#1..7j/',]) - time.sleep(.5) + time.sleep(.5) #hs.gen_prog(mode=-1) #hs.gen_prog(mode=0,cntHor=1,cntVert=3,wRng=(120000,120000)) @@ -1135,8 +1137,12 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' #hs.gen_prog(mode=1,cntHor=1,cntVert=5,wRng=(0,360000)) #hs.gen_prog(mode=1,cntHor=1,cntVert=5,hRng=(-.3,.3),wRng=(0,360000),smt=1) #hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-3,3),wRng=(120000,120000),smt=0) - hs.gen_prog(mode=1,cntHor=3,cntVert=6,hRng=(-5,5),wRng=(00,120000),smt=0,pt2pt_time=10) - #hs.gen_prog(mode=1, cntHor=1, cntVert=2, wRng=(0, 360000), smt=0) + #hs.gen_prog(mode=1,cntHor=3,cntVert=6,hRng=(-5,5),wRng=(00,120000),smt=0,pt2pt_time=10) + #hs.gen_prog(mode=0,cntHor=3,cntVert=10,hRng=(-5,5),wRng=(0,120000)) + #hs.gen_prog(mode=0,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000)) + hs.gen_prog(mode=1,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000),smt=0,pt2pt_time=300) + #hs.gen_prog(mode=1,cntHor=3,cntVert=20,hRng=(-5,5),wRng=(0,1200),smt=0,pt2pt_time=200) + #hs.gen_prog(mode=1, cntHor=2, cntVert=2, wRng=(0, 360000), smt=0) #hs.gen_prog(mode=1) #hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3)) #hs.gen_prog(mode=1,pt2pt_time=100,cnt=1,cntVert=10,cntHor=3,hRng=(-30,30),wRng=(0,36000),yRng=(-50,-100))