PMAC Servo Control Algorithm

PMAC PID and Notch Filter

1− z −1 Kaff

+ +
1− z −1 Kvff Ʃ
Notch filter

+ +
Reference + 1+ n z + n z
−1 −2

Position Ʃ Ʃ Kp 1 2
Output to DAC
1+ d z + d z
−1 −2

- + - 1 2

IM

Ki Kd

1− z −1
1− z −1
Velocity Loop Feedback

Position Loop Feedback

PID Parameters
Notch Filter

Kp Ixx30, Proportional gain Coefficients
Kd Ixx31, Derivative gain n1 Ixx36
Kvff Ixx32, Velocity feedforward gain n2 Ixx37
Ki Ixx33, Integral gain d1 Ixx38
IM Ixx34, Integration mode d2 Ixx39
Kaff Ixx35, Acceleration feedforward gain
142 Turbo PMAC Training PMAC Servo Control Algorithm
 PMAC Extended Servo Algorithm
1− z−1 Ixx68

R(z)
KS(k 0 + L + k 3z −3 )
Input ref. h0 + h1(1− z−1)
1 + 4( L1z −1 + L + L 3z −3 )

+ + + + +
TS(t 0 + L + t 4 z −4 ) 1 Ixx69
32 ⋅ Ixx08 Ʃ s0 Ʃ Ʃ Ʃ Ʃ U(z)
- - 1 + 8( r1z −1 + L + r4 z − 4 ) 1 + d1 z + d 2 z − 2
−1
+ 215
+ +
Output
s1(1− z−1)

Enc. #1
32 ⋅ Ixx08
Feedback #1
+ Enc. #2
+
Ʃ f0 Ʃ GS(g0 + g1 (1 − z −1 ))
Extended Servo
- -
Algorithm Parameters
f1(1− z−1)
s0 ~ s1 L1 ~ L3
f0 ~ f1 k0 ~ k3
Enc. #1
h0 ~ h 1 KS
32 ⋅ Ixx08
Enc. #2 r1 ~ r4 d1 ~ d2
t0 ~ t4 g0 ~ g2
Feedback #2 TS GS

Note: Must be tuned by an external algorithm (e.g. pole placement).

143 Turbo PMAC Training PMAC Servo Control Algorithm
 Proportional Control
K
Fixed Lumped mass with
moment of inertia J1
Torsional
Spring T Θ J
ζ : Damping Ratio
ω0 : Natural Frequency
d 2Θ K Θ : Angular Displacement
T = K • Θ = − J 2 , ω 0= , ζ =0 Τ : Input Torque
dt J K: Spring Constant
J: Moment of Inertia
P: Period of Oscillation
Θ [rad]] t: Time

Example Response: t [sec]

P
=1/ ω0
144 Turbo PMAC Training PMAC Servo Control Algorithm
 Proportional Control
e(t) T(t)
Ref. + + 1 Θ(t)
KP KC KA KT
J ∫ ∫
- -
Kd

d 2Θ
T (t ) = K p • K C • K A • KT • e(t ) = − K p • K C • K A • KT • Θ(t ) = J • 2
dt
KC: DAC Conversion Gain
KA: Amplifier Gain
K p • K C • K A • KT
KT: Motor Torque Constant ω0 = , ζ =0
J

This is an undamped Simple Harmonic Oscillator (SHO)

Thus, the proportional gain KP correlates with spring stiffness.

Higher KP → higher stiffness

145 Turbo PMAC Training PMAC Servo Control Algorithm

 Derivative Control
Torsional
Spring K J

Lumped mass with

Θ moment of inertia J
Fixed
T
Torsional c
Damper

ζ : Damping Ratio
dΘ d 2Θ K c ω0 : Natural Frequency
T = K •Θ+c• = − J • 2 , ω 0= , ζ= KJ
dt dt J 2 Θ : Angular Displacement
Τ : Input Torque
C: Damping Coefficient
K: Spring Constant
Θ [rad]
J: Moment of Inertia
t: Time

Example Response:
t [sec]

146 Turbo PMAC Training PMAC Servo Control Algorithm

 Ref. Input

e(t) T(t)
+ + 1 Θ
J ∫∫
KP KC KA KT
- -
d
Kd
dt

dΘ d 2Θ
T (t ) = − K p • K C • K A • KT • Θ(t ) − K p • K C • K A • K d • =J• 2
dt dt
This is a damped SHO.

K p • K C • K A • KT Kd K p • K C • K A • KT
ω0 = , ζ =
J 2 J
KC: DAC Conversion Gain
KA: Amplifier Gain
KT: Motor Torque Constant

147 Turbo PMAC Training PMAC Servo Control Algorithm

 Integral Control
Moment of
Pulley Attached to Inertia J
Motor Shaft
Θ
Without the integral, the governing equation is:
Rope in
r Tension
T (t ) = ( K P • K C • K A • K T ) • e(t ) = m • g • r

T(t)
Lumped
mass

mg
Ref.
Input
e(t) T(t)
+ + 1
J ∫∫
+
KP KC KA KT +
- - m•g•r Θ
d
Kd
KC: DAC Conversion Gain dt
KA: Amplifier Gain
KT: Motor Torque Constant
148 Turbo PMAC Training PMAC Servo Control Algorithm
 Adding the Integral

Ref.
Input e(t) T(t)
+ + 1 Θ
J ∫∫
+
KP KC KA KT
- + +
-
Ki ∫
m•g•r

d
Kd
dt

dΘ(t )
T (t ) = ( K P • K C • K A • K T ) • [ K i ∫ e(t )dt + e(t ) + K d ]= m• g •r
dt

Therefore as t → ∞, e(t)→
→0

KC: DAC Conversion Gain

KA: Amplifier Gain
KT: Motor Torque Constant

149 Turbo PMAC Training PMAC Servo Control Algorithm

 Steady State Errors Due to Constant
Speed Trajectory Tracking
dΘ d 2Θ
Θ d friction
Response (assuming = Vd =•0):t , = Vd , 2
=0
dt dt
Position

Vd

Reference trajectory Actual

(ramp input), Θd=Vdt Trajectory

ess = KdΘ

Steady state error

Time
150 Turbo PMAC Training PMAC Servo Control Algorithm
 Block Diagram without Kvff

dΘ
ess= Kd
e(t) dt T(t)
Θd=Vdt Θ
+ + 1
J ∫∫
KP KAKcKT
- -

d
Kd
dΘ dt
Kd
dt

KC: DAC Conversion Gain

KA: Amplifier Gain
KT: Motor Torque Constant

151 Turbo PMAC Training PMAC Servo Control Algorithm

 Purpose of Velocity Feedforward
Selecting Kvff = Kd  ess= 0

No steady state error due to constant velocity tracking

Position

Vd

ess = 0

Steady state error

Reference
trajectory (ramp
input), Θd=Vdt

Actual
Trajectory
Time

152 Turbo PMAC Training PMAC Servo Control Algorithm

 Block Diagram with Kvff

d
Kvff
dt
T(t)
Θd=Vdt ess= 0
Θ
+ + 1
J ∫∫
KP KAKcKT
- -

d
Kd
dΘ dt
Kd
dt KC: DAC Conversion Gain
KA: Amplifier Gain
KT: Motor Torque Constant

Typically, setting Kvff = Kd is a good starting place for tuning the

servo loop.
Note

153 Turbo PMAC Training PMAC Servo Control Algorithm

 Purpose of Acceleration Feedforward

In general, the trajectories contain higher order time functions:

Θ d (t ) = c 0 + c1 • t + c 2 • t 2

Example: Constant Jerk Trajectory
By choosing:

J1
K aff = → e(t ) = 0
K P • K C • K A • KT

J1 d 2Θd J1 • d 2Θ d
T (t ) = ( ) 2
(K P • KC • K A • KT ) =
K P • K C • K A • K T dt dt 2

This results in no following error for the Ideal system; i.e.:

d 2Θ d d 2Θ

Since
2
= 2 → Θ d = Θ → No Following Error
dt dt

154 Turbo PMAC Training PMAC Servo Control Algorithm

 Acceleration Feedforward: Kaff

d2
K af f
dt 2

d
K vff
dt

Θd=Vdt T(t) Θ
+ + 1
J ∫∫
KP KAKcKT
- -
d
Kd
dΘ dt
Kd
dt

KC: DAC Conversion Gain

KA: Amplifier Gain
KT: Motor Torque Constant

155 Turbo PMAC Training PMAC Servo Control Algorithm

 Servo Loop Modifier I-Variables
Ixx59: Motor xx User-Written Servo/Phase Enable
= 0: Use standard PID phase algorithms
= 1: Use custom servo, standard phase algorithms
= 2: Use standard PID, custom phase algorithms
= 3: Use custom servo, phase algorithms

Ixx60: Motor xx Servo Cycle Extension Period [Servo Cycles]

Loop closed every (Ixx60+1) servo interrupts
Useful for slow, low-resolution axes
Useful for process control “axes”

Ixx63: Motor xx Integration Limit [(Ixx33 / 219) counts * servo cycles]

Maximum integrated error accumulated
When negative, killed on saturation

Ixx64: Motor xx Deadband Gain Factor

Controls the effective gain within the deadband zone

Ixx65: Motor xx Deadband Size [1/16 count]

Defines the size of the position error band, measured from zero error, within which there
will be changed or no control effort, for the PMAC feature known as deadband
compensation.

156 Turbo PMAC Training PMAC Servo Control Algorithm

 Deadband Compensation

Deadband
Deadband width set by Ixx65 in units of 1/16 count

Output of

Filter
Proportional Gain Set by
value of Ixx30 n > 0 provides stiffer servo when
near in-position
n = 0 disables any deadband effect
Desired Position n < 0 reduces gain when near in-position
(n=-16 provides true deadband)
n<-16 provides negative gain when near
Two stable in-position (This is of no known use)
positions only n=-16
e(t) [Following Error]
Dead Band Gain Control:

 n + 16 
Deadband Gain =  Ixx30 , where n = Ixx64
 16 
DEAD BAND
DEAD BAND
Deadband
Gain

NO GAIN DISCONTINUITIES
n=16

n=0 Ixx30
n=-16
e(t) [Following Error]
157 Turbo PMAC Training PMAC Servo Control Algorithm
 Servo Loop Modifier I-Variables (cont.)
Ixx67: Motor xx Position Error Limit [1/16 Count]
Limits error that filter “sees”

Ixx68: Motor xx Friction Feedforward [16-bit DAC Bits]

Compensates for dry (Coulumb) friction

Ixx69: Output Command (DAC) Limit [16-bit DAC Bits]

Limits output of filter
Acts as torque (current) limit if commutating

158 Turbo PMAC Training PMAC Servo Control Algorithm

 Servo Loop Modifiers
α: Phase angle in Phase A
β: Phase angle in Phase B

"Friction Sin(α)
Phase A Bias
Command Feedforward"
Velocity
1
1-Z-1
-1 Ixx29
Sin(α−β)
Phase B Bias
Position Ixx68
Error Limit

Input
Deadband "Big Step” Output
Ixx79
Compensation Limit Limit DAC Bias
Commanded (Ixx01=1)
Position +
+ + KP
+
- + - +
Ixx02
Ixx64,65 Ixx67 Ixx69 Bit 16 Ixx29
Actual Integration =1
Position Mode
KD
Integration
Cmd Vel = 0? Ixx79
Limit
Ixx34
Ixx29
Ki
Motor Position

Ixx63

Load Position

159 Turbo PMAC Training PMAC Servo Control Algorithm

 Closing a Force Loop around a
Position Loop

Force Loop
Output Command
(to open register) Motor 2 (Position Loop)
Commanded (this is usually a
Force + velocity command)
Trajectory
PID Σdt
- Master Position Loop
Position Output Command
Actual Signal (to open register)
Force Commanded +
Signal Position +
Motor 1 (Force Loop) PID DAC AMP
Trajectory
- M Motor
Actual
Position
Signal ENC1
E Encoder

SG Strain Gage
ADC1

See the “Cascading Servo Loops” section of the Turbo PMAC User
Manual for detailed information on how to configure this type of
hybrid control.
Note

160 Turbo PMAC Training PMAC Servo Control Algorithm