I found myself in a situation where polar coordinates are used in determining position and location of a small robot and several objects of interest. In order change the heading of a robot to correctly point to one of the objects, I needed to calculate the most efficient direction to turn and when to stop.
The heading of the robot is derived by an overhead webcam running myrobotlab. It is converted into degrees from the value Java's Math.arctan2 (x,y) function. I didn't like my first couple attempts at deriving the appropriate code for this. It was frustrating and maybe someone here has already come up with the solution (or a better one). I did get a little relief when I finally drew it out.
The polar coordinates returned from arctan2 are a little unusual.
NORTH -179 OR 179 degrees depending on your heading direction, it never gets to 180
EAST -90 degrees
WEST 90 degrees
SOUTH 0 degrees
So if your bot's heading is -90 and the target is at 146, should your bot turn LEFT or RIGHT? This drove me a little crazy trying to find all the cases of heading, positive, negative, within the fixed coordinate system. I did not like all the different cases (it makes very sloppy code). After drawing it, I came to an easier way to visualize it. A sliding band with two colors (RED for RIGHT, GREEN for LEFT). It's important to keep track of the anti-target (AT) which is 180 degrees away from the target.
If you can imagine an armband which has 2 colors. Where the 2 colors meet is the TARGET. The HEADING can be anywhere, but if its on RED it needs to turn RIGHT and if its on GREEN it needs to turn left.
The possible locations of the three points (HEADING, TARGET, & ANTI-TARGET) can create N! (factorial) or 6 cases , but they all resolve down to two states (LEFT or RIGHT).
Here are the possible states in graphical form. If you stare at it long enough, you'll see the pattern.
The tracking is distilled down to a single if statement.
It's in Java, but you can change it to your favorite language flavor.
while (isTracking) |
Here Bug Toy markings are being tracked with LKOpticalTracking. The coordinates of the markings are used to calculate heading. The yellow block's position is determined with the inRange and FindContours OpenCV filter. The yellow block's target heading is calculated, then the Bug Toy figures out it needs to turn RIGHT ! A Graphics Service is used to display the filtered information on the right pane. Turn RIGHT Bug Toy, Turn RIGHT ! Processing speed for all filtering is about 140 ms.
