We’ve noticed a probelm with the swerve modules recently where instead of making a short spin from 90 to 0 they would sometimes do a 270 spin or even more (See video).

We tried using different methods of SwerveModuleState optimization, Changing the firmware, Modfying the PID but nothing seems to work.

It should be mentioned it only happens *sometimes* so the code is working fine theoretically and furthermore when we tried using PID on the roborio instead of the motor controller the problem did not reappear. (however that solution isn’t good as PID on the roborio is too slow)

Is the PID controller continuous? Also can you send a link to your code

I’d guess you have an issue with optimization and roll-over.

The first season we did swerve (at home challenge year) we’d find the module doing something similar on some of the paths. We had the CANCoder set to -180 to 180 . When it was at 165 and decided it needed to go to 200, for instance, the way the code was written it would try to go the long way around (instead of just turning 35 degrees). Then when it realize the size of the angle optimization would cut the angle by 180 and reverse the wheel direction. Thank goodness for being able to record on Shuffleboard. It was a simple fix once we know what was happening.

The way my team solved this issue is by calculating the setpoint before setting the reference to our motor controller’s onboard PID, like this:

```
turningEncoder.getPosition() + Math.IEEEremainder(stateAngle - getTurningPosition(), 2 * Math.PI)
```

- The method takes in the optimized current desired angle from the swerveModuleState, in radians (stateAngle)
- The desired angle is subtracted by getTurningPosition, which returns the current angle of the swerve module wrapped between -pi and pi

(Using`Math.IEEEremainder(turningEncoder.getPosition(), 2 * Math.PI)`

), in order to find the distance the motor needs to turn relative to its current position - This difference is then wrapped between between -pi and pi using Math.IEEEremainder
- This is then added to turningEncoder.getPosition(), which returns the raw current position in radians (raw as in not wrapped to any value, could be over pi or less than -pi)
- The final number is then used as the motor’s setpoint

(Note that we use the range [-pi, pi], if you are using [0, 2pi) then use the % operator instead of IEEERemainder)

Hope this helps

Thank you for the replies.

We do optimize the target angle in the code like so:

targetState = SwerveModuleState.optimize(targetState, Rotation2d.fromDegrees(getSteeringAngle());

Yet the issue still occurs at random.

The solution I posted is used in tandem with SwerveModuleState.optimize(), it’s an additional step beyond optimizing the state, that is necessary if you want to run PID onboard your motor controllers