Field Oriented Scoring

[jkleiber]

Summary Whenever the driver/operator wants to score a game piece, they should not have to think about which side of the robot to move the arm to. Instead, the robot should move to the correct side automatically based on the robot's knowledge of its orientation on the field.

The objective of this issue is to add field oriented scoring to our teleop capability. Field oriented meaning knowledge of which way the robot is facing relative to the field based on its gyro/IMU heading

Work to Do

• add a command for field oriented scoring
• use the navX to decide which preset to use
• map buttons so scoring is only 3 buttons: high, mid, low, and they all use this new command
• handle the edge case where the arm gets extended but the driver turns around 180 degrees by not changing directions unless there is a mode change in the arm to idle (by releasing the command)

[danielbrownmsm]

@jkleiber this should be done now, yeah?

[jkleiber]

This is done and we can close it with the understandimg that we need to merge #64