Sensors, rates & delay

The control loop

A real controller is a loop: read the sensors, compute a torque, command the motor, wait, repeat. It runs at a fixed rate $f$ (updates per second, Hz), so it acts every $T = 1/f$ seconds. In between it is blind, and the motor keeps the last torque.

Here the controller is step 4's PD law on the base, torque $\tau = k_p(\theta_{\text{target}} - \theta) - k_d\,\omega$: $\theta$ is the base angle, $\omega$ its angular velocity, and $k_p$ and $k_d$ the gains.

The simulator takes steps of $\Delta t = 2$ ms, so one update every $T$ seconds means $T/\Delta t$ physics steps per update: 1 at 500 Hz, 25 at 20 Hz.

A late, coarse sensor

This step's sensor is imperfect, like a real one:

• Latency: every reading of arm.q, arm.qd and arm.state() is 20 ms old.
• Quantisation: the angle comes from a 4096-tick encoder, so it moves in steps of $2\pi/4096$ rad. The velocity is the change over the last 10 ms (step 1's finite difference), so it jitters.

Why that hurts

Feedback corrects what it saw. At 20 Hz with 20 ms latency, the controller acts on where the arm was 20 ms ago and keeps that torque for 50 ms more: by the next update, its picture is 70 ms old. A high gain pushes hard on that stale error, overshoots, pushes hard back... The loop can oscillate, or even grow until the motor saturates.

Step 4's gains ($k_p = 40$, $k_d = 4.6$)	Settles to ±1°
500 Hz	in 0.4 s
50 Hz	in 0.8 s, 3 % overshoot
20 Hz	never: it keeps swinging

The cure is gentler gains: a slower controller only needs its information to be fresh on its own, slower timescale. (Or run faster and buy a better sensor.) Keep $k_d$ near critical damping, $2\sqrt{k_p I}$ with $I \approx 0.133$ kg·m².

Latency comes back in Noisy Sensors & Kalman Filters, step 3, where predicting ahead makes up for it.

Your task

1. Write run_at_rate(kp, kd, rate, duration, label): round(duration * rate) times, read arm.state(), send pd_torque to the base, call show(...), then step the simulation $1/(f\,\Delta t)$ times.
2. The program runs step 4's gains at 500, 50 and 20 Hz. Compare them in Plots.
3. Choose KP and KD for 20 Hz: the step must settle within ±1° in 1.5 s with under 5 % overshoot.