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

work on fwd/inv kinematics for PVT motion

Browse Source
This commit is contained in:
zamofing_t
2018-01-05 15:48:06 +01:00
parent e84c2020cd
commit f3e01ce33c
4 changed files with 127 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
python/coordTrfTest.py
View File

@@ -258,32 +258,6 @@ open prog 2
Gather.Enable=0
P1000=1
close
open prog 3
P1000=0
nofrax
Coord[1].AltFeedRate=0
Coord[1].SegMoveTime=0
linear abs
X0 Z0 B120000 Y2.3
dwell 10
Gather.Enable=2
Coord[1].AltFeedRate=0
Coord[1].SegMoveTime=0
pvt100 abs
X0:0 Z0:0 B120000:0 Y2.3:4.875
X0:0 Z0:0 B120000:0 Y3.275:9.75
X0:0 Z0:0 B120000:0 Y4.25:9.75
X0:0 Z0:0 B120000:0 Y5.225:9.75
X0:0 Z0:0 B120000:0 Y6.2:4.875
X0:0 Z0:0 B120000:0 Y6.2:0
dwell 1000
Gather.Enable=0
P1000=1
close
''')
if mode==2:
gather=False
@@ -292,15 +266,20 @@ close
// 4 5 3 1
&1
a
#1->0
#2->0
#3->0
#4->0
#5->0
#3->0
#1->0
#6->0
#7->0
#8->0
#4->X
#5->Z
#3->B
#1->Y
open forward
close
open inverse
close
''')
if mode==3:
gather=False
@@ -310,42 +289,47 @@ a
// 4 5 3 1
&1
a
#1->0
#2->0
#3->0
#4->0
#5->0
#3->0
#1->0
#6->0
#7->0
#8->0
#4->I
#5->I
#3->I
#1->I
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(q1='L1')
define(Y='C7')
//coord Y
send 1"forward D0: %f \\n",D0
if(D0>0) callsub 100
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
D0=$00000080; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
N100:
DX=qCX
DZ=qCZ
W=qW
Y=qFY
send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y
Y=q1
send 1"forward result %f\\n",Y
P1001+=1
close
open inverse
define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y
define(q1='L1', vq1='R1')
define(Y='C7',vY='C39')
//coord Y
send 1"inverse D0:%f _Y:%f:%f\\n",D0,Y,vY
//D0 is set to $000001c2
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
qCX=DX
qCZ=DZ
qW=W
qFY=Y
send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
define(q1='L1')
q1=Y
if(D0>0)
{
vq1=vY // THIS LINE IS USED FOR PVT MOVE !!!
send 1"inverse result _q1:%f:%f\\n",q1,vq1
}
else
{
send 1"inverse result %f\\n",q1
}
P1002+=1
close
''')
@@ -353,7 +337,7 @@ close
def test(self):
file='/tmp/prg.cfg'
self.gen_code(0) #motor config
#self.gen_code(0) #motor config
self.gen_code(1) #program code
self.gen_code(2) #simple coord trf
@@ -365,10 +349,17 @@ close
#prg.append(' Coord[1].SegMoveTime=0') # turn off segmented mode
#self.download_code(['&1;b1r',], file, True)
self.download_code(['&1;b2r',], file, True)
#self.download_code(['Coord[1].AltFeedRate=100','Coord[1].SegMoveTime=0','&1;b2r',], file, True)
self.download_code(['Coord[1].AltFeedRate=0','Coord[1].SegMoveTime=0','&1;b2r',], file, True)
#self.download_code(['Coord[1].AltFeedRate=1','Coord[1].SegMoveTime=.1','&1;b2r',], file, True)
#self.download_code(['Coord[1].AltFeedRate=0','Coord[1].SegMoveTime=.1','&1;b2r',], file, True)
#self.download_code(['&1;b3r',], file, True)
# *** READ USER MANUAL P 455 ***
# Inverse-Kinematic Program for PVT Mode, No Segmentation
#
if __name__=='__main__':
ct=CoordTrf(verbose=255,host='MOTTEST-CPPM-CRM0485')
ct.test()

71
python/helicalscan.py
View File

@@ -689,7 +689,7 @@ open forward
define(p1_x='L13', p1_y='L14', p1_z='L15')
define(scale='L16')
//send 1"forward kinematic %f %f %f %f\\n",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"forward result %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
@@ -719,14 +719,22 @@ close
prg.append('''
open inverse
define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y
//D0 is set to $000001c2
//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(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
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"inverse kinematic %f %f %f %f\\n",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]
@@ -749,7 +757,16 @@ open inverse
qCX=DX+p0_x
qCZ=DZ+p0_z
qFY=Y
//send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
if(D0>0)
{ // calculate velocities for PVT motion
vqCX=vDX
vqCZ=vDZ
vqW=vW
vqFY=vY
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
//P1002+=1
close
''')
@@ -806,10 +823,11 @@ a
#1->I
open forward
//define(KinVelEna='D0',KinAxisUsed='D0')
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y
send 1"forward D0: %f \\n",D0
send 1"fwd_inp(%f) %f %f %f %f\\n",D0,qCX,qCZ,qW,qFY
if(D0>0) callsub 100
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
N100:
@@ -817,20 +835,38 @@ N100:
DZ=qCZ
W=qW
Y=qFY
send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y
send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y
P1001+=1
close
open inverse
define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y
//D0 is set to $000001c2
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
//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')
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
qCX=DX
qCZ=DZ
qW=W
qFY=Y
send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
if(D0>0)
{ // calculate velocities for PVT motion
vqCX=vDX
vqCZ=vDZ
vqW=vW
vqFY=vY
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
P1002+=1
close
''')
@@ -943,7 +979,7 @@ close
prg.append(' dwell 100')
prg.append(' Gather.Enable=2')
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
prg.append(' Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics
prg.append(' Coord[1].SegMoveTime=0') #to calculate every 1 ms the inverse kinematics
for i in range(pt.shape[0]):
prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :]))
prg.append(' dwell 100')
@@ -959,6 +995,7 @@ close
wRng = kwargs.get('wRng', (0,360000))
yRng = kwargs.get('yRng', (2.3,6.2))
pt2pt_time = kwargs.get('pt2pt_time', 100)
smt = kwargs.get('smt', 1) # SegMoveTime, default = 1ms -> velocity calc not yet 100% correct (smt=0 not 100% working)
numPt=cntVert*cntHor
pt=np.zeros((numPt,4))
if cntHor>1:
@@ -991,7 +1028,7 @@ close
prg.append(' P100=%d'%cnt)
prg.append(' N100:')
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
prg.append(' Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics
prg.append(' Coord[1].SegMoveTime=%d'%smt) #to calculate every 1 ms the inverse kinematics
prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position
for idx in range(1,pv.shape[0]):
#prg.append(' P2000=%d'%idx)
@@ -1140,7 +1177,9 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1,cntVert=5,wRng=(120000,120000))
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100,smt=0)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(0,360000))
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(0,360000),smt=0)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1)
hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,
pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3))