SwerveDriveOdometry Keeps Approaching Zero


I’m having trouble with my swerve’s odometry. Its value keeps approaching zero, and from what I can tell, it’s because our SwerveModulePosition getPosition() method isn’t working right. The value for distance that it is returning also keeps approaching zero (When it’s driving, it has a value, maybe its speed? but when you stop it goes back to zero).

Here is the code for getting the swerve module position:

public SwerveModulePosition getPosition() {
    return new SwerveModulePosition(
      ModuleConstants.drivetoMetersPerSecond * driveMotor.getSelectedSensorVelocity(), 

Code for ModuleConstants.drivetoMetersPerSecond

public static final double wheelDiameterMeters = Units.inchesToMeters(4); // 4 inches
    public static final double wheelCircumferenceMeters =
        wheelDiameterMeters * Math.PI; // C = D * pi
    public static final double drivetoMetersPerSecond =
        (10 * wheelCircumferenceMeters) / (driveGearRatio * driveFXEncoderCPR);

That’s because ur calculating position based off of u velocity. So it’s a snapshot in time as opposed to a continuous calculation. Here is our code, FRC2023/SwerveModule.java at main · Frc5572/FRC2023 · GitHub. we use the continuous encoder position, not speed.

1 Like