A robot vacuum has finished a room. It stands with heading theta and must turn on the spot until it faces its charging dock, at heading target. Headings are in radians, measured counter-clockwise from the +x axis; either may lie outside (-\pi, \pi] (the robot does not tidy its own angle), and headings that differ by a whole number of turns are the same direction.
It turns the shorter way round: the turn is the difference target - theta brought into the range (-\pi, \pi] by adding or subtracting whole turns 2\pi. Positive turns are counter-clockwise. When the dock is exactly behind it (a half turn either way), the turn is +\pi, counter-clockwise.
The vacuum is a differential drive: two wheels of radius r metres, L metres apart. Turning on the spot, the wheels roll in opposite directions along a circle of radius L/2 around the robot's midpoint. Each wheel has an encoder with ticks_per_rev ticks per revolution, counting down when the wheel rolls backwards.
Write face_dock(theta, target, ticks_per_rev, r, L) that returns the tuple (left_ticks, right_ticks) the motor controller should count for the turn, each rounded to the nearest whole tick with Python's round.
Examples
Input: theta = 0, target = 1.5707963267948966, ticks_per_rev = 360, r = 0.05, L = 0.3
Output: (-270, 270)
Explanation: a quarter turn to the left. The right wheel rolls forward
pi/2 * 0.15 = 0.2356 m, which is 0.75 of its 0.3142 m circumference.
Input: theta = 3.0, target = -3.0, ticks_per_rev = 360, r = 0.05, L = 0.3
Output: (-49, 49)
Explanation: -6 rad the long way round is 2 * pi - 6 = 0.2832 rad the short way, to the left.
Input: theta = -1.5707963267948966, target = 1.5707963267948966, ticks_per_rev = 360, r = 0.05, L = 0.3
Output: (-540, 540)
Explanation: a half turn, which this robot always makes counter-clockwise.
Constraints
- the tests avoid counts that end in exactly half a tick
Goals
- Find the shorter way round between two headings by wrapping the difference
- Turn an on-the-spot rotation into the arc each wheel rolls, and the arc into encoder ticks