Swerve PID Continuous input and Swerve State optimization

[Henry_Mahnke]

Hello, I have some questions about swerve code and I’m hoping someone can help simplify it so I can understand. From my understanding SwerveModuleState.optimize() a method from wpilib minimizes the angle needed to be moved by the swerve module by stating that if there is ever a time in which error is greater than pi radians, it should artificially augment the swerve angle(in the eyes of the code) with the quadrants in mind, and then reverse the direction of the “throttle” motor. With that in mind, I don’t see a scenario in which enable continuous input(-pi,pi) is needed, as there should never be a time in which the measurement is greater than pi. I must be missing something! Can anyone help?

[bovlb]

You almost have it, but it’s when the error is more than PI/2. If you ever want to turn the wheels more than a quarter turn, flip it and reverse the drive.

See Source code

And your measurement can still get up to PI, just by turning slowly.

[Quake]

Imagine you are currently in this state, where the blue arrow is the heading of the module and the green arrow is the direction the drive wheel is spinning.

If you want to reach this state:

It will be optimized instead to this state:

In this case, the target heading of the module will change from ~160 degrees to ~-160 degrees as the optimization function uses Rotation2d. If your module heading controller doesn’t have continuous input, it will see -320 degrees of error instead of 40.

[Henry_Mahnke]

just to clarify, this controller will only be utilized when you’re measurement, and setpoint have the same sign prior to optimization, you’re measurement is either <-90 degrees or >90 degrees, and the error between your measurement and setpoint is greater than 90 degrees?

[Quake]

I’m not sure what you mean.

• Measurement: The current measured rotation of the swerve module
• Setpoint: The desired rotation of the swerve module
• Error: Difference between the setpoint and the measurement

The optimization only occurs when you have an absolute error greater than 90 degrees. The incorrect error without continuous input I mentioned above will always occur if the shortest rotation to the optimized state passes over the +/- 180 degree boundary.

[SndMndBdy]

I know this is an outside of the scope of the OP’s specific question, but I find this exact optimization to be a little too aggressive. For example, consider the situation where you are running your translation motor at full speed, and you want to change the module by direction by 91 degrees. The optimization would have you actually only move 89 degrees in the opposite direction and would throw the motor into full speed in reverse. The act of putting a motor from full speed in one direction to full speed in the other has not-great consequences for your electrical system, for the mechanical components on your swerve, and possibly for your robot’s stability.

At some point this optimization is useful, but I don’t think it’s actually 90 degrees. On 3419 we usually use 120 degrees, which is arbitrary but feels reasonable. You could be even smarter and use the speed the translation motor is running at to help make this decision (for example, if it is stopped, go ahead and optimize at 90 degrees, but if it is running full speed only optimize at 150 degrees, and interpolate in between).

[Quake]

Good idea! Haven’t tested this myself but iirc 1323 doesn’t do the angle optimization and just oversteers to the setpoint to maintain tread life. Of course if the speed you rotate the module to its setpoint is slow your ability to accelerate in different directions will suffer, but I don’t know how much of a difference this makes.

[Henry_Mahnke]

This is significantly more concise, Thank you for all of your help!