SwerveModule drive motor has jittery velocity setpoints

[mjansen4857]

There seems to be some severe jitter in swerve module drive velocity setpoints introduced by various SwerveRequests. We tested ApplyRobotSpeeds and FieldCentric and they both introduced the same jitter. The jitter is in the setpoint itself, not just the output of the motor, which we see by logging the drive motor's getClosedLoopReference() signal.

We have verified that this jitter is not introduced by our code or a poorly tuned PID controller with the following testing:

1. Set the drivetrain control to ApplyChassisSpeeds or FieldCentric with static, zeroed speeds.
2. Notice jitter in drive motor both audibly and through logging the closed loop reference
3. Set the drivetrain control to a custom swerve request that applies NeutralOut to rotation motors and VelocityTorqueCurrentFOC with velocity 0 to drive motors.
4. Notice no jitter in drive motors at all

Our exact setup is Mk4i modules with L2 ratio, KrakenX60s running torque current output for both drive and steer which are set to Velocity and Position control respectively. Using java, latest firmware and Phoenix. I'll be happy to provide further information if needed.

[mjansen4857]

Something else to note is that this jitter is not always present, but once it happens it will not go away on its own. For example, sometimes when applying zero chassis speeds, there will be no jitter. But if we drive around for a second to get things moving, the jitter will usually start happening when we come to a stop. Switching to a static zero speeds swerve request while the jitter is happening does not stop it, and it will continue to jitter indefinitely until we move again.

It also usually only happens on 1 or 2 modules at a time, with the modules that jitter changing seemingly randomly. We have seen all of them jitter together sometimes, however.

[bhall-ctre]

There are two sources of compensation we apply to the drive motors based on the azimuth:

1. Cosine compensation - the drive velocity is scaled by the cosine of (target - measured) azimuth heading. If the azimuth is close (<0.05 rot), then this wouldn't cause any jitter, but if it's sometimes farther away from the setpoint (>0.1 rot), then it's possible you may see a little jitter in the velocity.
2. Coupling ratio - because azimuth velocity causes coupled motion at the drive motor or wheel, we compensate for it by adjusting the drive velocity setpoint based on the azimuth velocity and the coupling ratio.

If the jitter you're seeing is primarily caused by 2, then it should go away by setting the coupling ratio to 0 (note that this will introduce more error into odometry, this is just for testing). Otherwise, you should verify that your module azimuth is tracking well.

Note that none of this has changed from last season, the only thing that has changed is support for module force feedforwards. If you're currently using that, then I would first test without them so there's fewer variables involved. Any jittering from module force feedforwards would likely be the same as cosine compensation, assuming the feedforwards are correct to begin with.

[mjansen4857]

We are not using any force feedforwards at the moment, and our azimuth is tracking very well, down to <0.02 rotations.

I can verify this by setting the coupling ratio to zero next time I have access to the robot, but based on your description, I believe 2 is creating a feedback loop that causes jitter based on the behavior I’ve seen. When the jitter happens, our logging shows very small changes in the steer position and velocity (setpoint remains constant) of the modules that are jittering. This steer velocity leads to drive jitter, which leads to more steer jitter, and so on. We’ve noticed giving the robot a kick can start and stop this jitter as well. This makes sense as it could be stopping or introducing a small amount of steer velocity that starts or stops this feedback loop.

I would argue that these setpoint changes should only be applied if the steer velocity is over a certain threshold (preferably configurable and fairly high by default). I see why coupling ratio helps odometry, but I can’t imagine changing the setpoints based on it making much of a difference that the PID loops can’t compensate for, especially at lower steer velocities. We would like to keep coupling ratio configured for odometry accuracy, but the amount of jitter we’re seeing is pretty excessive and very loud.

[mjansen4857]

Also, we’ve seen this jitter happen with zeroed out PID constants for the steer motors, which is how we discovered it was the drive jittering in the first place. So this endless feedback loop is not really contributed to by the steer motors and their PID tuning.

[mjansen4857]

Changing the coupling ratio to 0 does stop the jitter. Would definitely like the ability to disable those setpoint changes or have a threshold for them.

[bhall-ctre]

We just released Phoenix 6 25.2.2, which addresses this issue: https://api.ctr-electronics.com/changelog#20250206

[mjansen4857]

Tried the latest release and it definitely makes it a lot better, but it can still occur sometimes. It will eventually stop on its own now after a second or two.

[bhall-ctre]

Interesting. Just to confirm, you're seeing this with the robot on carpet? I'm surprised that's not enough friction to stop it after our changes; that sounds more like one of the sets of PID gains (or feedforwards) are a bit too strong.

[mjansen4857]

Yeah. It’s really rare now, so not really something I’m concerned about anymore. We will take another pass at our PID tuning when we get a chance.

The jitter seems to just be in the backlash of the gears, as the wheels don’t actually move. So carpet friction probably doesn’t have much to do with it.

[mjansen4857]

So we just tried a custom swerve request that applies SwerveModuleStates instead of using one of the provided requests and the modules do not jitter at all without even touching our PID tuning. So, this is definitely still an issue with the velocity setpoint changes of the built-in swerve requests. We'd really like the option to just disable this entirely so we can still utilize native swerve requests.