Telling robot to turn to a certain angle

[dltompki]

Proposed Functionality

The robot can turn in place from the current angle to a specified angle, in the more efficient direction.

Suggested Implementation

A command which takes the desired angle as a parameter, and will figure out if it should turn left or right to get there (one way will be shorter than the other, unless they are equal). It will then turn in that direction until it gets to the desired angle.

Resources

https://pdocs.kauailabs.com/navx-mxp/software/roborio-libraries/java/

[jefft138]

I think we should create 2 commands for this, one that turns to a specific field angle (relies on properly calibrated compass), and another that turns a specified number of degrees (or radians), i.e. a relative amount from current angle. We may find both to be useful in these challenges. Higher priority should be on the specified field angle. We should assume that 0 degrees is along the line that goes from start to end, at least for this challenge where they are across the field from one another.

[dltompki]

Sounds good. I would assume the relative command would be relatively easy to implement once the first one is there, since we can just apply the relative change to the current angle, and set that as the desired field angle.

[dltompki]

We should try both compass and pitch/roll/yaw on NavX. We used compass during DeepSpace, but it wasn't perfect (mostly drifting issues), so maybe the pitch/roll/yaw system might be better? Compass uses a magnetometer, which can receive interference from motors, pitch/roll/yaw uses gyro. No idea if it would be better, but worth a shot.

https://pdocs.kauailabs.com/navx-mxp/guidance/terminology/

[dltompki]

@jefft138:

later when we try to turn after picking up the ball, we will also want to see that turning with the ball still in the rollers toward the intake doesn't cause the ball to jam

[jefft138]

Lesson learned from this activity is that the compass heading cannot be read until after the compass is calibrated, therefore the heading can't be read during the command constructor (which is done as part of robotInit), but must be done during the command's initialize method so that the compass has time to calibrate. Also, in looking at execute for TurnToDegrees, I think the logic was incorrect and made some changes for determining the CW/CCW distances. @dylantompkins please look at the changes I made and let me know if you have questions.

[dltompki]

@jefft138 eb9e37f7a8d7f80283f473e5021fb7c2386072e9 looks good. New CW/CCW algorithm makes sense. I only have a couple things:

• I believe that boolean should be lowercase (TurnToDegrees lines 19 and 28). I had to look up the difference.
• the constructor parameter targetHeading should be renamed since it could be a target heading or a relative heading now. Maybe just heading? I think leaving everything else the same is fine though.