Add a prioritize rotation flag to the swerve drive method

[varun7654]

This should ensure that the outputted rotation speed is equal to what the commanded input is. When this is true we should reduce the commanded robot speed to make sure we hit the commanded angular velocity (if needed).

[Matthew-Kasper]

You might be able to tell if the commanded robot speed needs to be reduced by checking to see if the current to the drive motors is at the current limit and checking to see if the rotation has not met its setpoint. I don't know if this would work properly though.

[varun7654]

I plan to just do a binary search to ensure that no individual wheel is above a max set speed

[Matthew-Kasper]

How would that solve this problem?

[Matthew-Kasper]

I assume the purpose of lowering the allowed speed is so the wheels still have room to spin faster to achieve a specific turn.

[Matthew-Kasper]

I honestly don't know anymore

[varun7654]

We call SwerveDriveKinematics#desaturateWheelSpeeds before setting the wheel speeds. It ensures that no single module is going is set above the max speed that it can achieve. If a wheel is requested to spin faster than the max speed it reduces the speed of all the other wheels so that the ratio of speeds between all the wheels stay the same. The consequence is that you won't be moving at the requested velocity or rotation speed. This may cause issues with our turnpid if we run it while we're moving, so I wanted to make a method that instead sacrifices robot velocity to ensure that we're always turning at our requested rate.

[Matthew-Kasper]

When would we call it? If we see that the turn setpoint isn't being reached? We shouldn't have it active all of the time.

[varun7654]

whenever we're using the turnpid

[Matthew-Kasper]

Hmm ok, I hope this doesn't make the driving feel weird due to the speed changes.