Each wheel of a warehouse robot has an encoder: a disc on the axle that sends a tick every time it turns by a fixed small angle, ticks_per_rev ticks per full revolution. The count goes down when the wheel turns backwards. Between two readings, the left encoder counted left_ticks and the right one right_ticks.
A full revolution rolls a wheel of radius r metres forward by its circumference 2\pi r. From the two distances d_l and d_r the robot (a differential drive with its wheels L metres apart) gets
the distance its midpoint travelled along its path and the change of its heading in radians (positive counter-clockwise, to the left). \Delta\theta is a change, not a heading, so it is not wrapped: a long reading can hold more than a full turn.
Write tick_count(left_ticks, right_ticks, ticks_per_rev, r, L) that returns the tuple (d_left, d_right, d, dtheta), distances in metres.
Examples
Input: left_ticks = 512, right_ticks = 1024, ticks_per_rev = 1024, r = 0.03, L = 0.25
Output: (0.09424778, 0.18849556, 0.14137167, 0.37699112)
Explanation: the left wheel turned half a revolution, 0.5 * 2 * pi * 0.03 = 0.0942 m,
the right one a full revolution, 0.1885 m. The heading grew by 0.0942 / 0.25 = 0.377 rad.
Input: left_ticks = -300, right_ticks = 300, ticks_per_rev = 600, r = 0.05, L = 0.2
Output: (-0.15707963, 0.15707963, 0.0, 1.57079633)
Explanation: half a revolution each, in opposite directions: a quarter turn on the spot.
Constraints
- floats are compared with a tolerance of
1e-6
Goals
- Turn encoder ticks into the distance a wheel rolled
- Combine the two wheel distances into the robot's travel and its change of heading