Turbo PMAC Users Manual Manual

Delta Tau Turbo PMAC User Manual
;***************************************************************
; This program is a user written servo
; that replicates the PMAC PID loop.
; It uses the following equations defined
; in the PMAC Users Manual but modified for PMAC passed terms:
; K16 = 2**-16
; K128 = 1/128
; K23 = 2**-23
; FE = DPOS - APOS - From PMAC in Ix08 * 32 counts
; AVEL, DVEL - From PMAC in Ix09 * 32 counts/servo period
; IPOS = ITOF(Ix33) * FE * K23 + IPOS
; DACEL = DVEL - PDVEL
; PDVEL = DVEL
; DACOUT = ITOF(Ix30)*K16 * ( FE + K128 *(ITOF(Ix32) * DVEL +
; ITOF(Ix35) * DACEL - ITOF(Ix31) *AVEL) + IPOS)
#define K16 0.000015259 ; Constant of 2^-16
#define K128 0.0078125 ; Constant of 1/128
#define K23 0.000000119 ; Constant of 2^-23
; Use L-variables
for quick access to PID I-variable registers
#define Ix08 L1
L1->Y:(R1-$27)
#define Ix09 L2
L2->Y:(R1-$14)
#define Ix30 L3
L3->Y:(R1-$17)
#define Ix31 L4
L4->X:(R1-$1E)
#define Ix32 L5
L5->X:(R1-$21)
#define Ix33 L6
L6->X:(R1-$11)
#define Ix35 L7
L7->Y:(R1-$1D)
#define Ix63 L8
L8->Y:(R1-$11)
#define Ix69 L9 ; Note that this is 24-bit value, not 16
L9->Y:(R1-2)
#define STATUS L10
L10->X:(R1+0)
#define IPOS F1 ; Integrated position error register
F1->L:(R1-$12) ; Automatically zeroed on motor open loop
#define PDVEL F2 ; Previous desired velocity register
F2->L:(R1-$29) ; Automatically zeroed on motor open loop
#define AVEL P0 ; Floating-point actual velocity
#define DVEL P1 ; Floating-point net desired velocity
#define FE P2 ; Floating-point following error
#define APOS P3 ; Floating-point actual position
#define DPOS P4 ; Floating-point net desired position
#define DACOUT P5 ; Floating-point commanded output
OPEN SERVO CLEAR
COPYREG P0 ; Copy Motor AVEL,DVEL,FE,APOS,DPOS to float P0..4
; FE, APOS, & DPOS in 1 / [Ix08* 32] counts
; AVEL in 1 / [Ix09*32] counts/servo period
Setting Up the Servo Loop 8-35