add 5CAM mover from git@git.psi.ch:epics_ioc_modules/5CAM_SYSTEM.git

2018-01-22 10:41:35 +01:00
parent 5ee34c5f11
commit 13686b03b7
3 changed files with 146 additions and 1 deletions
--- a/cfg/MX3_coordTrf.py
+++ b/cfg/MX3_coordTrf.py
@@ -0,0 +1,132 @@
+def 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
+
+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)
+
+//debug code
+P1=S1
+P2=S2
+P3=S3
+P4=S4
+P5=S5
+P6=P6+1
+
+//X=D^-1*S
+X=+.5*S1     -.5*S2   +(r+.5)*S3    +(r-.5)*S4    +(.5-r)*$sqrt2*S5
+Y=+.5*S1     +.5*S2      +.25*S3       +.25*S4       +.25*$sqrt2*S5
+U=                        -.5*S3        -.5*S4         +.5$sqrt2*S5
+V=-.5*S1     +1.*S2      +1.5*S3        -.5*S4        -.5*$sqrt2*S5
+W=-.5*S1     -1.*S2       +.5*S3        +.5*S4        +.5*$sqrt2*S5
+
+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')
+
+r=$height/$width
+
+S1=+.5*X          +.5*Y    +(0)       *U    -.5*V          +.5*W
+S2=-.5*X          +.5*Y    +(0)       *U    +.5*V          +.5*W
+S3=+.5*X          +.5*Y    -(.5)      *U    +.5*V          -.5*W
+S4=-.5*X          +.5*Y    -(.5)      *U    -.5*V          -.5*W
+S5=         .5*$sqrt2*Y     +.5*$sqrt2*U            -.5*$sqrt2*W
+
+//debug code
+P11=S1
+P12=S2
+P13=S3
+P14=S4
+P15=S5
+P16=P16+1
+
+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
+
+//debug code
+P21=AA1
+P22=AA2
+P23=AA3
+P24=AA4
+P25=AA5
+
+close
+
+//#1..5j/
+//#1..5hmz
+&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
+''',kw)