Robotics Fundamentals 1, Chapter 7: Challenge 2 solution (PI control, windup, position loop, damping and derivative problems)

Formulas: e = r - omega ;  P = Kp e ;  I = I + Ki e dt ;  u = P + I ;  u sent = u limited to +-5 V
          Euler step: omega(next) = omega + ( dt / tau(m) ) ( K ( u sent - d ) - omega )  with dt / tau(m) = 0.02 / 0.2 = 0.1
          anti-windup (clamping): do not add to I when u is outside the limit and the error pushes it further out
          position loop: wn = sqrt( K Kp / tau(m) ) ;  2 zeta wn = ( 1 + K Kd ) / tau(m) ;  OS = exp( -pi zeta / sqrt( 1 - zeta^2 ) )
          t(peak) = pi / ( wn sqrt( 1 - zeta^2 ) ) ;  T(d) = 2 pi / ( wn sqrt( 1 - zeta^2 ) ) ;  settling (2 %) about 4 / ( zeta wn )

(a) K = 20, tau(m) = 0.2, dt = 0.02, d = 0.5 V, limit +-5 V, r = 40, Kp = 0.1, Ki = 2.0, start at rest.

    Integral always adds (no anti-windup):

    k    omega     e        Kp e     I         u wanted    u sent
    0     0.000   40.000    4.000    1.600     5.600       5.000
    1     9.000   31.000    3.100    2.840     5.940       5.000
    2    17.100   22.900    2.290    3.756     6.046       5.000
    3    24.390   15.610    1.561    4.380     5.941       5.000
    4    30.951    9.049    0.905    4.742     5.647       5.000
    5    36.856    3.144    0.314    4.868     5.183       5.000

    With clamping anti-windup:

    k    omega     e        Kp e     I         u wanted    u sent
    0     0.000   40.000    4.000    0.000     5.600       5.000       (u 5.6 > 5 and e > 0: integral held at 0)
    1     9.000   31.000    3.100    1.240     4.340       4.340       (I = 0 + 2 x 31 x 0.02 = 1.24; now inside the limit, so it integrates)
    2    15.780   24.220    2.422    2.209     4.631       4.631
    3    22.464   17.536    1.754    2.910     4.664       4.664
    4    28.545   11.455    1.145    3.369     4.514       4.514
    5    33.718    6.282    0.628    3.620     4.248       4.248

    Example row (always adding, k = 1):  omega(1) = 0 + 0.1 x ( 20 x ( 5 - 0.5 ) - 0 ) = 9.0;  e = 31;  I = 1.6 + 2 x 31 x 0.02 = 2.84;  u = 3.1 + 2.84 = 5.94, sent as 5.
    At step 5 the integral is 4.868 V with no protection and 3.620 V with clamping: the unprotected integral kept adding while the output was stuck at 5 V, so it is 1.25 V larger.
    The settled value is 2.5 V (below), so both integrals are above what the loop will finally need; the loop must overshoot to unwind them, and the one with more excess overshoots more.
    Run on to the end, the peak speed is 49.24 rad/s (23.1 % overshoot) without anti-windup against 44.70 rad/s (11.7 %) with clamping.
    (The overshoot that remains with clamping comes from the PI gains themselves, not from windup.)
    Settled value of the integral: at rest e = 0, so u = I and the motor needs omega = 40 = K ( u - d ),  u = r / K + d = 40 / 20 + 0.5 = 2.5 V.   The integral settles at I = 2.5 V.

(b) Position loop, K = 20, tau(m) = 0.2, Kp = 0.8.
    Kd = 0:   wn = sqrt( 20 x 0.8 / 0.2 ) = sqrt( 80 ) = 8.944 rad/s.     zeta = 1 / ( 2 sqrt( K Kp tau(m) ) ) = 1 / ( 2 sqrt( 3.2 ) ) = 1 / 3.578 = 0.2795.
              OS = exp( -pi x 0.2795 / sqrt( 1 - 0.0781 ) ) = exp( -0.8780 / 0.9602 ) = exp( -0.9145 ) = 0.401 = 40.1 %.
    Kd = 0.06:  wn is unchanged (the derivative adds damping, not stiffness) = 8.944 rad/s.
              2 zeta wn = ( 1 + K Kd ) / tau(m) = ( 1 + 20 x 0.06 ) / 0.2 = 2.2 / 0.2 = 11.0,   zeta = 11.0 / ( 2 x 8.944 ) = 0.6149.
              OS = exp( -pi x 0.6149 / sqrt( 1 - 0.3781 ) ) = exp( -1.9317 / 0.7886 ) = exp( -2.4496 ) = 0.0863 = 8.6 %.
              t(peak) = pi / ( 8.944 x 0.7886 ) = pi / 7.053 = 0.445 s.
              T(d) = 2 pi / 7.053 = 0.891 s (the period of the ringing).
              Settling (2 %) about 4 / ( zeta wn ) = 4 / ( 0.6149 x 8.944 ) = 4 / 5.5 = 0.73 s.
    The derivative term cut the overshoot from 40.1 % to 8.6 % with the same stiffness.

(c) dt = 0.01 s, Kd = 0.06, the target jumps by 0.5 rad in one step, encoder resolution 0.0105 rad.
    Derivative on the error at the instant of the change:   Kd x 0.5 / 0.01 = 0.06 x 50 = 3.0 V   (one sample of a violent kick, half the 6 V supply).
    Noise from a one-count change in the measurement:       Kd x 0.0105 / 0.01 = 0.06 x 1.05 = 0.063 V   every time the count changes, although the real speed did not.
    Derivative on the measurement:   the measurement has not moved when the target moves, so at that instant y - y(previous) = 0 and the derivative term is 0 V: no kick.
    (The noise from the encoder count remains with either form, which is why the derivative is filtered or taken from a rate sensor such as a gyro.)