Add the ability to "square up" with the reef. #24
Description
An aid for the driver, so he can push a button and square up to the face of the reef he is already mostly pointing at.
Rather than fish around for geometry on the field, it's probably easier to take the robot's current angle, mod it with 60 degrees, and then tell the drive subsystem to go to a pose that is on the 60 degree line that it is already closest to pointing at. This would require no other features and could be implemen ted in the absence of vision.