Robotics Fundamentals 1, Chapter 4: Challenge 1 solution (differential-drive kinematics and odometry)

Formulas: v = (vR + vL) / 2          omega = (vR - vL) / L          R = (L/2)(vR + vL)/(vR - vL)
          vR = v + omega L/2         vL = v - omega L/2             d(tick) = 2 pi r / (CPR x gear ratio)
          ds = (dR + dL)/2           dtheta = (dR - dL)/L           x += R (sin(theta+dtheta) - sin theta)    y -= R (cos(theta+dtheta) - cos theta)
Given: L = 0.20 m, wheel diameter 0.08 m (r = 0.04 m), 600 ticks per wheel revolution.

(a) Circumference = 2 pi x 0.04 = 0.25133 m.   d(tick) = 0.25133 / 600 = 0.000419 m = 0.419 mm.

(b) dL = 450 x 0.00041888 = 0.18850 m.      dR = 700 x 0.00041888 = 0.29322 m.
    ds = (0.18850 + 0.29322) / 2 = 0.24086 m.
    dtheta = (0.29322 - 0.18850) / 0.20 = 0.5236 rad = 30.0 degrees (counter-clockwise, because the right wheel went further).
    R = ds / dtheta = 0.24086 / 0.5236 = 0.460 m.

(c) Start (0.50, 0.20), heading 90 degrees (pi/2). New heading = 90 + 30 = 120 degrees.
    Exact arc update:
        x = 0.50 + 0.460 x (sin 120 - sin 90) = 0.50 + 0.460 x (0.86603 - 1.00000) = 0.50 - 0.06162 = 0.4384 m
        y = 0.20 - 0.460 x (cos 120 - cos 90) = 0.20 - 0.460 x (-0.50000 - 0.00000) = 0.20 + 0.2300 = 0.4300 m
    Midpoint shortcut (heading 90 + 15 = 105 degrees for the step):
        x = 0.50 + 0.24086 x cos 105 = 0.50 + 0.24086 x (-0.25882) = 0.50 - 0.06234 = 0.4377 m
        y = 0.20 + 0.24086 x sin 105 = 0.20 + 0.24086 x 0.96593 = 0.20 + 0.23265 = 0.4326 m
    Difference: sqrt(0.0007^2 + 0.0026^2) = 0.0027 m = 2.7 mm, about 1 % of the 0.24 m step.
    The shortcut is very good for steps this short or shorter; the heading after the step is 120 degrees in both cases.

(d) v = 0.30 m/s, omega = -1.50 rad/s (negative = clockwise, so it turns to the right).
        vR = 0.30 + (-1.50)(0.10) = 0.30 - 0.15 = 0.15 m/s.        vL = 0.30 + 0.15 = 0.45 m/s.
        The left wheel is the faster one, so the robot curves to the right, as a clockwise turn should.
        rpm(R) = 60 x 0.15 / (2 pi x 0.04) = 9 / 0.25133 = 35.8 rpm.       rpm(L) = 60 x 0.45 / 0.25133 = 107.4 rpm.
        Tick rates: 0.15 / 0.00041888 = 358 ticks/s (right);   0.45 / 0.00041888 = 1074 ticks/s (left).
        Turning radius: R = v / omega = 0.30 / (-1.50) = -0.20 m, so the centre of the turn is 0.20 m to the right.

(e) 90 degrees per second = pi/2 = 1.5708 rad/s, with v = 0.
        vR = +omega L/2 = 1.5708 x 0.10 = 0.1571 m/s and vL = -0.1571 m/s (opposite directions).
        rpm = 60 x 0.1571 / 0.25133 = 37.5 rpm each.       Tick rate = 0.1571 / 0.00041888 = 375 ticks/s each.
        Full turn: 360 / 90 = 4.0 s.

(f) vR = 0.40, vL = 0.30 m/s:
        v = (0.40 + 0.30) / 2 = 0.350 m/s.      omega = (0.40 - 0.30) / 0.20 = 0.500 rad/s (28.6 deg/s, counter-clockwise).
        R = (0.20 / 2)(0.70 / 0.10) = 0.100 x 7 = 0.700 m (also v / omega = 0.35 / 0.5 = 0.70 m).
        The right wheel is faster, so it is a left turn, and the centre of the circle lies 0.70 m to the LEFT of the robot's centre.

Check on method: the wheels' circles are R - L/2 = 0.60 m and R + L/2 = 0.80 m, and vL / vR = 0.30 / 0.40 = 0.60 / 0.80 = 0.75. Correct.