[kylelanman]

The trick to a smooth swerve and optimal control is to limit rotating the module as much as possible. If you think about it you should never have to rotate a swerve module more than 90 deg for a given action. The exception is for a smooth change over time like a tornado maneuver where the robot is spinning while traveling in a given direction. At any rate from a single step perspective a 90 degree limit is key. If the desired angle is more than 90 degrees and less than 270 degrees than it is going to be optimal to command the module to the desired angle + 180 and then reverse your motor speed.

We have handled this in the past using the following code.

//C++
if (fabs(desiredAngle - currentAngle) > 90 && fabs(desiredAngle - currentAngle) < 270) {
    desiredAngle = ((int)desiredAngle + 180) % 360;
    speed = -speed;
}

Here is our entire SwerveModule implementation from 2015.
https://github.com/Frc2481/frc-2015/...erveModule.cpp