Robotics Fundamentals 1, Chapter 7: Challenge 3 solution (tuning, stability, feedforward and PID in a robot)

Formulas: Ziegler-Nichols  P: Kp = 0.5 Ku ;  PI: Kp = 0.45 Ku, Ti = Pu / 1.2 ;  PID: Kp = 0.6 Ku, Ti = Pu / 2, Td = Pu / 8 ;   Ki = Kp / Ti,  Kd = Kp Td
          overshoot OS = exp( -pi zeta / sqrt( 1 - zeta^2 ) ) ;  zeta = -ln( OS ) / sqrt( pi^2 + ln( OS )^2 )
          Euler loop without delay is stable while Kp < ( 2 tau(m) / dt - 1 ) / K
          P control: omega = ( K Kp r - K d ) / ( 1 + K Kp ) ;  with feedforward: omega = ( K ( u(ff) - d ) + K Kp r ) / ( 1 + K Kp )
          line follower: e = -x, u = Kp e + Kd de/dt, vR = v + u L / 2, vL = v - u L / 2 ;   heading hold: e = wrap( target - heading ), u = Kp e - Kd gyro

(a) Ku = 0.35, Pu = 0.18 s.
    P:    Kp = 0.5 x 0.35 = 0.175.
    PI:   Kp = 0.45 x 0.35 = 0.1575;  Ti = 0.18 / 1.2 = 0.15 s;  Ki = 0.1575 / 0.15 = 1.05.
    PID:  Kp = 0.6 x 0.35 = 0.21;  Ti = 0.18 / 2 = 0.09 s;  Td = 0.18 / 8 = 0.0225 s;  Ki = 0.21 / 0.09 = 2.333;  Kd = 0.21 x 0.0225 = 0.004725.
    These are first guesses: expect an aggressive response (large overshoot, as in the chapter's example) and reduce Kp and Ki until it is acceptable.

(b) zeta = 0.5:   OS = exp( -pi x 0.5 / sqrt( 0.75 ) ) = exp( -1.5708 / 0.8660 ) = exp( -1.8138 ) = 0.163 = 16.3 %.
    For OS = 0.10:   ln( 0.10 ) = -2.3026,   zeta = 2.3026 / sqrt( 9.8696 + 5.3019 ) = 2.3026 / sqrt( 15.1715 ) = 2.3026 / 3.8951 = 0.591.

(c) K = 20, tau(m) = 0.2, Euler with no extra delay:   Kp(max) = ( 2 tau(m) / dt - 1 ) / K.
    dt = 0.05:   ( 2 x 0.2 / 0.05 - 1 ) / 20 = ( 8 - 1 ) / 20 = 7 / 20 = 0.35 V per rad/s.
    dt = 0.02:   ( 20 - 1 ) / 20 = 0.95.
    A slower update (50 ms against 20 ms) cuts the highest usable gain from 0.95 to 0.35, and any extra delay in the loop lowers it further: the update is itself a delay.

(d) K = 25, d = 1.2 V, Kp = 0.16, r = 75:   K Kp = 4, 1 + K Kp = 5.
    P alone:   omega = ( 25 x 0.16 x 75 - 25 x 1.2 ) / 5 = ( 300 - 30 ) / 5 = 54 rad/s,   error 21 rad/s  (also e(ss) = ( 75 + 30 ) / 5 = 21).
    Feedforward u(ff) = r / K = 75 / 25 = 3.0 V (no load assumed):
        omega = ( K ( u(ff) - d ) + K Kp r ) / ( 1 + K Kp ) = ( 25 x ( 3.0 - 1.2 ) + 300 ) / 5 = ( 45 + 300 ) / 5 = 69 rad/s,   error 6 rad/s.
    Feedforward cut the error from 21 to 6 rad/s: the model supplied most of the command, and the feedback only had to deal with the load the model missed.

(e) x = -6.5 mm now, -5.9 mm before, dt = 0.01, Kp = 0.06, Kd = 0.0015, v = 0.25, L = 0.16.
    e = 0 - x = +6.5 mm;  the error one step ago was +5.9 mm.
    de/dt = ( 6.5 - 5.9 ) / 0.01 = 60 mm/s.
    P = 0.06 x 6.5 = 0.39 rad/s;   D = 0.0015 x 60 = 0.09 rad/s;   u = 0.39 + 0.09 = 0.48 rad/s.
    vR = 0.25 + 0.48 x 0.16 / 2 = 0.25 + 0.0384 = 0.2884 m/s.      vL = 0.25 - 0.0384 = 0.2116 m/s.
    u is positive, which is counter-clockwise, so the robot turns LEFT (the right wheel faster): correct, because x is negative, the line is to the left.

(f) Target -170 degrees, heading +175 degrees, gyro +10 degrees/s, Kp = 3, Kd = 0.4.
    Raw error = -170 - 175 = -345 degrees;  wrap( -345 ) = +15 degrees = 0.2618 rad  (a short counter-clockwise turn instead of a long clockwise one).
    P = 3 x 0.2618 = 0.785 rad/s.   D = -0.4 x ( 10 degrees/s = 0.1745 rad/s ) = -0.070 rad/s.   u = 0.785 - 0.070 = 0.716 rad/s.
    u is positive, so the robot turns counter-clockwise, which also means its heading increases through 180 degrees, where the heading wraps to -180 and on to -170 (the target).
    The gyro term reduces the command because the robot is already turning the right way: that is the damping.