A robot's gyroscope does not tell it which way it faces. It measures how fast it is turning: a rate in radians per second, positive when turning counter-clockwise (left) and negative when turning clockwise (right). The robot's software reads the gyro every dt seconds and keeps its own heading by adding up what it turned:
heading = heading + rate * dt
and then wraps the heading into the range -\pi < heading \le \pi (adding or subtracting 2\pi, a whole turn, does not change the direction the robot faces).
Headings are in radians, counter-clockwise from the world's +x axis. The robot starts at heading start (already in range).
Write gyro_headings(start, rates, dt) that returns the list of wrapped headings after each gyro reading, in order. It has one entry per rate and is empty when there are no readings. Collect them and press Run with plot(headings, kind="step") to see the heading jump from +\pi to -\pi as the robot spins.
Examples
Input: start = 3.0, rates = [0.5, 0.5, -1.0, 0.2], dt = 0.5
Output: [-3.0331853071795862, -2.7831853071795862, 3.0, 3.1]
Explanation: 3.0 + 0.25 = 3.25 is past pi, so it wraps to 3.25 - 2*pi = -3.033. Another 0.25
gives -2.783. Then the robot turns back 0.5: -3.283 wraps to 3.0. Finally 3.0 + 0.1 = 3.1.
Input: start = -1.0, rates = [1.2, 1.2, 1.2, 1.2, 1.2], dt = 0.25
Output: [-0.7, -0.4, -0.1, 0.2, 0.5]
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Turn a turn rate and a time step into an angle turned
- Keep a running heading and wrap it after every update
- Read a negative rate as a clockwise turn