✨[FEATURE] - Turns should default to taking the shortest path

[ssejrog]

Is this feature related to a problem?

It's confusing that heading is absolute and you need to keep track of it, it's very easy to make a mistake and accidentally make a massive turn.

What should be added?

Turns should always take the shortest path.

OPTIONAL: Describe any solutions you've considered

Just like in odometry, a wrap_angle function should be used.

OPTIONAL: Additional context

[ssejrog]

After discussion in Discord, the conclusion on how to implement this is to create new functions with overloads. pid_turn_set(deg, speed) pid_turn_set(deg, speed, slew) pid_turn_set(deg, speed, turn behavior) pid_turn_set_deg, speed, turn behavior, slew)

Possible additional turn behaviors are:

• the current behavior
• always shortest path
• always longest path?
• clockwise
• counter clockwise

Behavior should be able to be defaulted. Turning with a different behavior should override the default, but the next turn should come right back to the previously set default behavior. All of this needs to be replicated for swings too.

This also sets up how #138 will work, and the defaulted behavior there should work the same.

[ssejrog]

This also brings discussion of how pid_wait_until should be changed to handle this. Currently it waits for the robot to pass an absolute target, maybe if that becomes a relative target it would solve everything? Driving and swing inches both already use relative.

To follow semver, this would require that you can change the behavior of wait until to be relative or absolute for angles. I think this is a good solution.