90/180º heading rotation

[willitcode]

Bind D-Pad buttons to rotate the robot by 90 or 180 degrees.

In field-oriented drive mode, this should rotate the robot relative to the field (duh). Pressing the left d-pad button should rotate the robot so that it faces towards the left side of the field, forward should face opposite DS wall, etc.

In robot-oriented drive mode, up and down should both flip the robot 180 degrees, left should rotate the robot 90 degrees to the left, and right should rotate the robot 90 degrees to the right.

This may require some extra legwork because changing the keymap based off of the drive base mode is a new one for us.

[willitcode]

YAGSL doesn't have built-in functionality for this, so we'll need to spend a bit more time implementing this. It's definitely doable though!

Feel free to brainstorm in this thread about how we could do it. I think we could probably abuse odometry to make it happen, or we could just turn in a direction until we hit the desired angle.

YAGSL has a built-in heading correction PID loop though, so I think a good bet is to grab the current robot pose, transform that pose to the desired rotation, and then ask odometry to move us to that pose. Only problem is that if the driver presses the d-pad while the robot is moving, it might do something weird.

We may also want to consider just forking YAGSL and adding this as a feature. I could open an issue in their repo to gauge interest.