118 lines
3.3 KiB
Python
118 lines
3.3 KiB
Python
def MX3_coordTrf(fileParser,kwargs):
|
|
from math import sqrt
|
|
'''
|
|
--- mandatory ---
|
|
exc : excentricity of cam
|
|
height: Height of the beam above moved plane
|
|
width : x-distance of P2 to P3
|
|
length: y-distance of P1 to P2,P3
|
|
camSf : scale factor to convert cam encoder to rotation angle (360deg=2*pi)
|
|
--- optional ---
|
|
...
|
|
|
|
Documentation:
|
|
'Cam Movers for SLS undulators' (21.5.2013, Calvi, Bruegger, Zimoch)
|
|
home/zamofing_t/Documents/doc-ext/DeltaTau/Training/TrainingSlides/22-Power PMAC Cam Tables 2014-04.ppt
|
|
Ref Manual: 96-106,488, 527-528, 655
|
|
|
|
|
|
Coordinate Transformation
|
|
-------------------------
|
|
L1-5: AA1-5 : Cam Angle = Motor position
|
|
L11-15: S1-5 : Linear shift of the cam S.= E/sqrt(2)*sin(AA.)
|
|
C3-7: X,Y,U,V,W : X,Y positions and rotation around axes X,Y,Z
|
|
|
|
open forward
|
|
L1-L5 are input motor positions, C3-C7 are output coordinate
|
|
X=D^-1*S
|
|
|
|
|
|
|
|
open inverse
|
|
L1-L5 are output motor positions, C3-C7 are input coordinate
|
|
S=D*X
|
|
|
|
'''
|
|
|
|
kw=kwargs
|
|
kw['sqrt2']=sqrt(2.)
|
|
fileParser.parse_txt('''
|
|
// ---------- Coordinate System/Transformation ----------
|
|
// Set the motors as inverse kinematic axes in CS 1
|
|
&1
|
|
#1->I
|
|
#2->I
|
|
#3->I
|
|
#4->I
|
|
#5->I
|
|
|
|
Coord[1].Ta=100
|
|
Coord[1].Td=100
|
|
Coord[1].AltFeedRate=1
|
|
Motor[1].MaxSpeed=Motor[1].JogSpeed
|
|
Motor[2].MaxSpeed=Motor[2].JogSpeed
|
|
Motor[3].MaxSpeed=Motor[3].JogSpeed
|
|
Motor[4].MaxSpeed=Motor[4].JogSpeed
|
|
Motor[5].MaxSpeed=Motor[5].JogSpeed
|
|
|
|
open forward
|
|
define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
|
|
define(r='L6',scl='L7')
|
|
define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
|
|
define(X='C6',Y='C7',U='C3',V='C4',W='C5')
|
|
r=$height/$width
|
|
scl=$exc/$sqrt2
|
|
S1=scl*sin(AA1*$camSf)
|
|
S2=scl*sin(AA2*$camSf)
|
|
S3=scl*sin(AA3*$camSf)
|
|
S4=scl*sin(AA4*$camSf)
|
|
S5=scl*sin(AA5*$camSf)
|
|
|
|
send 1"fwd_inp(%f) %f %f %f %f %f\\n",D0,AA1,AA2,AA3,AA4,AA5
|
|
send 1"fwd_inp(%f) %f %f %f %f %f\\n",D0,S1,S2,S3,S4,S5
|
|
|
|
|
|
//X=D^-1*S
|
|
X=+.5*S1 -.5*S2 +(r+.25)*S3 +(r-.75)*S4 +(.25-r)*$sqrt2*S5
|
|
Y=+.5*S1 +.5*S2 +.25*S3 +.25*S4 +.25*$sqrt2*S5
|
|
U= +1.*S3 +1.*S4 -1.*$sqrt2*S5
|
|
V=-1.*S1 +1.*S2 +1.5*S3 -.5*S4 -.5*$sqrt2*S5
|
|
W=-1.*S1 -1.*S2 +.5*S3 +.5*S4 +.5*$sqrt2*S5
|
|
|
|
send 1"fwd_res %f %f %f %f %f\\n",X,Y,U,V,W
|
|
|
|
D0=$$000000f8; //U=$$8 V=$$10 W=$$20 X=$$40 Y=$$80 hex(8+int('10',16)+int('20',16)+int('40',16)+int('80',16)) -> '0xf8'
|
|
close
|
|
|
|
open inverse
|
|
define(AA1='L1',AA2='L2',AA3='L3',AA4='L4',AA5='L5')
|
|
define(r='L6',scl='L7')
|
|
define(S1='L11',S2='L12',S3='L13',S4='L14',S5='L15')
|
|
define(X='C6',Y='C7',U='C3',V='C4',W='C5')
|
|
|
|
//if(D0>0)
|
|
// send 1"Velocity calculation NOT SUPPORTED\\n"
|
|
|
|
send 1"inv_inp(%f) %f %f %f %f %f\\n",D0,X,Y,U,V,W
|
|
|
|
r=$height/$width
|
|
|
|
S1=+.5*X +.5*Y +(.25-.5*r)*U -.25*V -.25*W
|
|
S2=-.5*X +.5*Y +(.5*r-.25)*U +.25*V -.25*W
|
|
S3=+.5*X +.5*Y +(.25-.5*r)*U +.25*V +.25*W
|
|
S4=-.5*X +.5*Y +(.25+.5*r)*U -.25*V +.25*W
|
|
S5= .5*$sqrt2*Y -.25*$sqrt2*U +.25*$sqrt2*W
|
|
|
|
scl=$sqrt2/$exc
|
|
AA1=asin(S1*scl)/$camSf
|
|
AA2=asin(S2*scl)/$camSf
|
|
AA3=asin(S3*scl)/$camSf
|
|
AA4=asin(S4*scl)/$camSf
|
|
AA5=asin(S5*scl)/$camSf
|
|
|
|
send 1"inv_res %f %f %f %f %f\\n",AA1,AA2,AA3,AA4,AA5
|
|
|
|
close
|
|
|
|
''',kw)
|