Investigate shorter travel

[drewwhis]

Currently, the wheels have to travel all the way around the circle in some cases.

We have the potential to remove this behavior by having the wheels travel in the opposite direction and reverse the speed.

For the WPI examples, there is an open PR here: https://github.com/wpilibsuite/allwpilib/pull/2248

I don't think it would look exactly the same for us, but it would be close.

[drewwhis]

Looking at what's in the linked PR, I think this is how we'd tackle the problem. This would go in our setState method.

I'm not super confident, though. @HeathHudson and @kjarsty do y'all see anything obviously (or not-obviously) wrong with this approach?

I'd like to clear the rest of our backlog before tackling this in actual robot code, but it doesn't hurt to discuss it now.

Rotation2d angle = state.angle;
double speedMs = state.speedMetersPerSecond;
double angleRadians = angle.getRadians();

double angleDiffRadians = angleRadians - getState().angle.getRadians();
double absDiffRads = Math.abs(angleDiffRadians);

if (absDiffRads > Math.PI / 2 && absDiffRads < 1.5 * Math.PI) {
  angle = state.angle.plus(new Rotation2d(Math.PI));
  speedMs *= -1;
}

drive.setReference(speedMs, Reference.kVelocity);
turn.setReference(angle.toRadians(), Reference.kPosition);