Driving a wheeled robot

A robot on wheels

The rover in the viewer has two wheels on one axle, each with its own motor. Spin them at the same speed and it drives straight; spin them at different speeds and it turns. This is differential drive. You don't command the wheels one by one: you give a forward speed $v$ (m/s) and a turn rate $\omega$ (rad/s, anticlockwise positive), and the rover works out the wheel speeds.

The rover's pose $(x, y, \theta)$ says where it is: its position on the table in metres, and its heading $\theta$ in radians, measured anticlockwise from the $+x$ axis. rover.pose reads it.

Arcs

Hold $v$ and $\omega$ steady and the rover drives round a circle. In $t$ seconds it covers $vt$ metres and turns $\omega t$ radians, so a full turn ($2\pi$) takes $2\pi/\omega$ seconds and covers the circumference $2\pi R$. That gives the radius:

$$R = \frac{v}{\omega}$$

$\omega = 0$ drives straight ($R$ is infinite) and $v = 0$ spins on the spot.

Go to a goal

To reach a point $(x_g, y_g)$, keep steering towards it:

• the direction to the goal is $\theta_g = \operatorname{atan2}(y_g - y,\ x_g - x)$, and its distance is $d$;
• the heading error is $e = \theta_g - \theta$, wrapped into $(-\pi, \pi]$ with rover.wrap so the rover turns the short way;
• turn in proportion to it: $\omega = K_\text{turn}\, e$;
• drive at $v = \min(v_\text{max},\ K_\text{dist}\, d) \cdot \max(0, \cos e)$: slow down on arrival, and don't drive forwards while facing away.

Every 50 ms, read the pose and recompute. This is a feedback loop: the rover corrects its own mistakes. Foundations: Angles, atan2 & 2-D rotation covers atan2 and wrapping.

Your task

1. Set W and T so rover.run(V, W, T) drives one full circle of radius $R$ = 10 cm and comes back to the start.
2. Finish go_to(goal): compute the heading error and $(v, \omega)$ each time round the loop. It stops within 1 cm of the goal.

The program then visits the three beacons in turn and plots the distance and heading error. The checks want the circle within 5 mm, each goal reached within 2 cm, no bumps and the tour in under 30 s.