Mr. Matthews, thank you. We are using WPILIB for kinematics and odometry, those are great libraries.
UPDATE:
So… here’s the deal. After checking through source code from swerve-lib, we found some interesting functionality that is not apparent or discussed in their documentation, other than directly looking at their source code.
When creating a SwerveModule object from their library, they use a factory to create a configuration and then apply the configuration and instantiate the TalonFXs (or Neos) and the CANCoders. They control the position of the steering motor using the internal encoder on the falcon and seeding the position using the CANCoder. Their math is correct and nothing is wrong there so what could be the issue? It sporatically would drag a motor and then we would have it enabled, let it sit for ~10 seconds and it would “fix” itself… weird. So here’s the code that is both causing the issue and fixing the issue.
This is inside the setReferenceAngle() method for the Falon500SteerControllerFactoryBuilder
// Reset the NEO's encoder periodically when the module is not rotating.
// Sometimes (~5% of the time) when we initialize, the absolute encoder isn't fully set up, and we don't
// end up getting a good reading. If we reset periodically this won't matter anymore.
if (motor.getSelectedSensorVelocity() * motorEncoderVelocityCoefficient < ENCODER_RESET_MAX_ANGULAR_VELOCITY) {
if (++resetIteration >= ENCODER_RESET_ITERATIONS) {
resetIteration = 0;
double absoluteAngle = absoluteEncoder.getAbsoluteAngle();
motor.setSelectedSensorPosition(absoluteAngle / motorEncoderPositionCoefficient);
currentAngleRadians = absoluteAngle;
}
} else {
resetIteration = 0;
}
At first glance, you can tell they just copied over the code from the Neos Factory Builder and didn’t bother to change it, but to give them credit, it’s the same… for the most part. They explain that because of the initialization process they use, when the robot initially runs the code, it may not have properly seeded the position of the TalonFX using the CANCoder. So, their fix is when you call the set() method with your voltage and your desired rotation angle, it will then call this function down the setReferenceAngle() function, which if your robot is still, it will again seed the position.
Well, if you’re like our team and you do not usually have to tell your motors to go to zero in your disabledPeriodic() function in Robot, then you will not call this enough before you enable and you chance dragging a swerve module or two until you stop and wait 10 seconds (500 iterations if you look at the source code). Very frustrating.
TLDR: call your drive funcion with no velocity during your disabled periodic so i calls the setReferenceAngle function enough before you enable so that your modules have the correctly seeded angle.
