User's Manual
Turbo PMAC User Manual
Motor Compensation Tables and Constants 179
Example 3: PMAC’s PID Filter
The following example mimics the action of Turbo PMAC’s basic PID loop (without the notch, deadband
compensation, position-error limiting, or friction feedforward terms), but with floating-point calculations.
It uses the PID’s own I-variables, accessed as L-variables for speed, then converted to floating-point
values.
This program is a user written servo that replicates the PMAC PID loop. It uses the following equations
defined in the PMAC User 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