Delta Tau GEO BRICK LV User 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