Programming for Robotics 1, Chapter 5: Challenge 1 solution (measuring a motor: deadband, speed curve, ramp)

Idea: the robot is the rig of section 3 with a deadband of 0.15 and 0.50 m/s at full duty, a thing the software is not told. (a) Sweep the duty with measure_speed_curves (each value on a fresh robot, settle, count ticks) and read the deadband and the slope k from the fit; the full-duty speed is k (1 - deadband). (b) The duty for a wanted speed from the measured curve is deadband + speed / k; the believed curve (no deadband, 0.40 m/s at duty 1.0) gives speed / 0.40, and the robot is then checked at both duties. (c) A slew limit of 3.0 command units per second takes 1.0 / 3.0 s to cross the whole command range, and a command of c takes c / 3.0 s; the passes needed are that time divided by the 20 ms of a pass, rounded up.

CODE:
HIDDEN_DEADBAND, HIDDEN_TOP = 0.15, 0.50                 # what the robot really has; the software is not told
mpt = Config.METRES_PER_TICK
sweep = (0.10, 0.15, 0.20, 0.25, 0.30, 0.40, 0.50, 0.60, 0.80, 1.00)


def make_rig():
    sim = Simulator(World(8.0, 4.0), params=RobotParams(max_speed_mps=HIDDEN_TOP), seed=1, pose=(1.0, 2.0, 0.0))
    return RealMotors(sim, deadband=(HIDDEN_DEADBAND, HIDDEN_DEADBAND))


print("(a) the sweep")
points, (left, right) = measure_speed_curves(make_rig, mpt, mpt, sweep)
for duty, vl, vr in points:
    print("  duty %.2f   left %.4f m/s   right %.4f m/s" % (duty, vl, vr))
print("  fit: k = %.4f m/s per unit duty, deadband = %.4f, full-duty speed %.4f m/s" % (left.k, left.deadband, left.max_speed_mps))
print("  truth: k = %.4f, deadband %.2f, full-duty speed %.2f m/s" % (HIDDEN_TOP / (1 - HIDDEN_DEADBAND), HIDDEN_DEADBAND, HIDDEN_TOP))

print("(b) duty for 0.25 m/s")
want = 0.25
duty = left.duty_for(want)
for name, d in (("measured curve", duty), ("believed curve", want / Config.MAX_SPEED_MPS)):
    rig = make_rig()
    rig.set_duty(d, d)
    for _ in range(100):
        rig.advance(0.02)
    t0 = rig.ticks()[0]
    for _ in range(50):
        rig.advance(0.02)
    speed = (rig.ticks()[0] - t0) * mpt
    print("  %-15s duty %.4f -> %.4f m/s (error %+.1f %%)" % (name, d, speed, 100 * (speed - want) / want))

print("(c) the ramp: slew 3.0 command units per second")
slew = 3.0
m = Motor(deadband=left.deadband, slew_per_s=slew)
passes = 0
while m.cmd < 1.0 - 1e-9:
    m.set(1.0, 0.02)
    passes += 1
print("  predicted 1.0 / %.1f = %.3f s;  measured %d passes = %.2f s" % (slew, 1.0 / slew, passes, passes * 0.02))
m = Motor(deadband=left.deadband, slew_per_s=slew)
cmd_for_speed = (left.duty_for(want) - left.deadband) / (1.0 - left.deadband)
passes = 0
while m.cmd < cmd_for_speed - 1e-9:
    m.set(cmd_for_speed, 0.02)
    passes += 1
print("  the command for 0.25 m/s is %.3f; reaching it takes %.3f / %.1f = %.3f s predicted, %d passes = %.2f s measured"
      % (cmd_for_speed, cmd_for_speed, slew, cmd_for_speed / slew, passes, passes * 0.02))
END CODE
OUTPUT:
(a) the sweep
  duty 0.10   left 0.0000 m/s   right 0.0000 m/s
  duty 0.15   left 0.0000 m/s   right 0.0000 m/s
  duty 0.20   left 0.0293 m/s   right 0.0293 m/s
  duty 0.25   left 0.0585 m/s   right 0.0585 m/s
  duty 0.30   left 0.0881 m/s   right 0.0881 m/s
  duty 0.40   left 0.1470 m/s   right 0.1470 m/s
  duty 0.50   left 0.2059 m/s   right 0.2059 m/s
  duty 0.60   left 0.2648 m/s   right 0.2648 m/s
  duty 0.80   left 0.3825 m/s   right 0.3825 m/s
  duty 1.00   left 0.5000 m/s   right 0.5000 m/s
  fit: k = 0.5886 m/s per unit duty, deadband = 0.1503, full-duty speed 0.5001 m/s
  truth: k = 0.5882, deadband 0.15, full-duty speed 0.50 m/s
(b) duty for 0.25 m/s
  measured curve  duty 0.5750 -> 0.2501 m/s (error +0.1 %)
  believed curve  duty 0.6250 -> 0.2794 m/s (error +11.8 %)
(c) the ramp: slew 3.0 command units per second
  predicted 1.0 / 3.0 = 0.333 s;  measured 17 passes = 0.34 s
  the command for 0.25 m/s is 0.500; reaching it takes 0.500 / 3.0 = 0.167 s predicted, 9 passes = 0.18 s measured
END OUTPUT

Reading the output. (a) The two wheels are identical here, so the two curves agree. Duties 0.10 and 0.15 are inside the deadband: the wheel does not turn, those points are left out of the fit (they would pull the line down), and the fit from the other eight points gives a deadband of 0.1503 against the true 0.15 and a full-duty speed of 0.5001 m/s against 0.50. (b) For 0.25 m/s the measured curve asks for a duty of 0.5750 and the robot does 0.2501 m/s (+0.1 %); the believed curve asks for 0.6250 and the robot does 0.2794 m/s, 11.8 % too fast, which is the same kind of error as the +9.8 % of the page, a little larger because this deadband is a little larger. (c) The ramp takes 17 passes = 0.34 s against a predicted 0.333 s: the prediction is 16.7 passes and the loop can only act on whole passes, so it rounds up. A command of 0.500 (the command for 0.25 m/s here is half of full speed) takes 9 passes = 0.18 s against 0.167 s predicted, for the same reason.

Check on method: the ways to get this wrong are (1) fitting the points inside the deadband as well: the wheel speed there is zero, not "a little", and the line bends, so leave out the points that did not move, as the function does; (2) sweeping with too few points above the deadband, so that the slope rests on two numbers; (3) reading the ramp time off the duty instead of the command: the ramp works on the command (0 to 1), and the deadband map is applied after it, so a duty that jumps from 0 to 0.15 at the start of a ramp is not a failure of the ramp.