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()