Moving Swerve Code from SDS to WPI

I have been working to convert our Swerve code from using the SDS libraries to the WPI method. Our swerve is using Falcon 500s and trying to use the onboard PID control. I was trying to program it to use the PID constraints from the SDS libraries.

Here is our original code using the SDS libraries:

Here is the new code using WPI:

When we tried it the robot would move in some sort of arc and the modules just locked into a specific position. Any feedback would be helpful.


We are working on a similar project this preseason. We don’t quite have the code working yet, but we found Team 364’s code very helpful:


I liked the method from 364 so I made another version that uses their their setup combined with my code. I had to make a few changes to get rid of a few errors but have hit a road block where the code crashes at startup.

Here is the code: GitHub - smtopps/22-23SwervePractice at 364SwerveModule
Here is a log file: 2022_12_05 22_34_20 (1.5 KB)

It seems to be related to the pose estimator which probably has to do with how I get the module states since it’s the only thing I have changed. Line 97 of the pose estimator seems to be the source of the problem.

The SwerveModule’s getState method is calculating the velocity as NaN. This is because the value of Constants.ModuleConstants.driveGearRatio is 0. This constant is defined as:

public static final double driveGearRatio = (((50/14)*(17/27)*(45/15)) / 1.0); //6.75:1
public static final double angleGearRatio = (((32/15)*(60/10)) / 1.0); //12.8:1

Java will perform integer division if both values are ints instead of doubles. As a result 17/27 evaluates to 0, which results in the entire expression evaluating to 0. Use double literals instead:

public static final double driveGearRatio = (((50.0/14.0)*(17.0/27.0)*(45.0/15.0)) / 1.0); //6.75:1
public static final double angleGearRatio = (((32.0/15.0)*(60.0/10.0)) / 1.0); //12.8:1

This should take care of it.


Looking for some help to understand why sometimes when we start the robot one of the modules ends up 90 out of rotation. When we restart the code it often fixes the issue. It’s hard to diagnose as it only happens sometimes and I think only when we turn on the robot or upload new code but I am not sure.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.