Delta Tau GEO BRICK LV Benutzerhandbuch
Turbo PMAC User Manual
178
Motor Compensation Tables and Constants
In order to start the algorithm correctly, it reads the servo cycle counter and compares it to the counter the
last time the last time this algorithm was executed. If the algorithm was not executed the previous cycle,
it zeros out the “history” values for the algorithm. It also does a saturation check on the commanded
output. This algorithm assumes a standard memory option (5x0) whose data memory ends at
X/Y:$0107FF and a UBUFFER defined of at least 2048 words.
Definitions and Substitutions
last time the last time this algorithm was executed. If the algorithm was not executed the previous cycle,
it zeros out the “history” values for the algorithm. It also does a saturation check on the commanded
output. This algorithm assumes a standard memory option (5x0) whose data memory ends at
X/Y:$0107FF and a UBUFFER defined of at least 2048 words.
Definitions and Substitutions
#define Kp
P(ITOF(MTRNUM*100+30))
; Gain term is Pxx30
#define A
P(ITOF(MTRNUM*100+31))
; A zero is Pxx31
#define B
P(ITOF(MTRNUM*100+32)) ; B zero is Pxx32
#define C
P(ITOF(MTRNUM*100+33)) ; C pole is Pxx33
#define D
P(ITOF(MTRNUM*100+34)) ; D pole is Pxx34
#define E
P42
; Error term e(k) (not saved)
#define Temp1
P45
; Temporary value
#define Temp2
P46
; Temporary value
#define Temp3
P47
; Temporary value
#define U
P48
; Output term u(k) (not saved)
#define LastE
F1[MTRNUM-1]
; e(k-1) is F1[#-1]
F1->L:$010000[32]
; Float reg array in UBUFFER
#define PrevE
F2[MTRNUM-1]
; e(k-2) is F2[#-1]
F2->L:$010020[32]
; Float reg array in UBUFFER
#define LastU
F3[MTRNUM-1]
; u(k-1) is F3[#-1]
F3->L:$010040[32]
; Float reg array in UBUFFER
#define PrevU
F4[MTRNUM-1]
; u(k-2) is F4[#-1]
F4->L:$010060[32]
; Float reg array in UBUFFER
#define ServoCycle
L0
L0->X:$0,0,24,S
; Servo cycle counter
#define LastServoCycle
L1[MTRNUM-1]
L1->X:$010080[32]
; Register array in UBUFFER
#define ServoExtension
L2
L2->Y:(R1-$21)
; Ixx60 register as integer
#define OutputLimit
8388607
Start of Actual Algorithm
OPEN SERVO CLEAR
COPYREG P40
; Following error into P42
; If loop was not closed last cycle, zero out stored values
IF (ServoCycle-LastServoCycle!=ServoExtension+1)
LastU=0
PrevU=0
LastE=0
PrevE=0
ENDIF
LastServoCycle=ServoCycle ; Store for next cycle
Temp1=Kp*(E+(A+C)*LastE+A*C*PrevE) ; Compute TF numerator
Temp1=Kp*(E+(A+C)*LastE+A*C*PrevE) ; Compute TF numerator
Temp2=(B+D)*LastU+B*D*PrevU
; Compute TF denominator
Temp3=Temp1-Temp2
; Combine for net command
U=FLIMIT(Temp3,OutputLimit)
; Saturation check
PrevE=LastE
; Store values for next cycle
LastE=E
PrevU=LastU
LastU=U
RETURN(FTOI(U))
; Return command value as integer
CLOSE