Delta Tau GEO BRICK LV 사용자 설명서
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.
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:
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)
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
#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