Swerve odometry problems

Our team is trying to use the wpi swerve odometry to track our location on the field and possibly use it for autonomous. The problem we are running into is that the output from the swerve odometry is all over the place, we think this is due to how we are inputting our encoder values into the module state. we are using the mk3 modules from swerve drive specialties with the falcon 500’s.

here is how we made the object

robotPosition = new Pose2d(0,0, new Rotation2d());
odometry = new SwerveDriveOdometry(kinematics, gyro.getRotation2d(), robotPosition);

how we are updating our position

  robotPosition = odometry.update(gyro.getRotation2d(), frontLeftModule.getState(), frontRightModule.getState(),backLeftModule.getState(), backRightModule.getState());
  SmartDashboard.putNumber("Robot Position (X)", odometry.getPoseMeters().getX());
  SmartDashboard.putNumber("Robot Position (Y)", odometry.getPoseMeters().getY());
  SmartDashboard.putNumber("Robot Position (Angle)", odometry.getPoseMeters().getRotation().getDegrees());

how we are making the state, this is in our swerve module class

public SwerveModuleState getState() {
  return new SwerveModuleState(nativeUnitsToDistanceMeters(driveMotor.getSensorCollection().getIntegratedSensorVelocity()), new Rotation2d(rotationMotor.getSensorCollection().getIntegratedSensorPosition()));
}

native units

private double nativeUnitsToDistanceMeters(double sensorCounts){
  double motorRotations = (double)sensorCounts / Constants.SENSORS.INTERNAL_ENCODER_RESOLUTION.GetResolution();
  double wheelRotations = motorRotations / Constants.SWERVE.GEAR_RATIO.get();
  double positionMeters = wheelRotations * (2 * Math.PI * Units.inchesToMeters(2));
  return positionMeters;
}

our gear ratio 8.16:1
internal resolution is 2048

You need to convert rotationMotor.getSensorCollection().getIntegratedSensorPosition() into a radian value for the module angle, instead of just plugging in the raw sensor value.

public SwerveModuleState getState() {
  double turnRadians = ((2.0 * Math.PI) / (kTurningMotorGearRatio * kEncoderCPR)) * rotationMotor.getSensorCollection().getIntegratedSensorPosition()
  return new SwerveModuleState(nativeUnitsToDistanceMeters(driveMotor.getSensorCollection().getIntegratedSensorVelocity()), new Rotation2d(turnRadians));
}

Thank you for the reply, but didn’t solve our problem we are still having issues with the numbers being all over the place, for example, if we were to move forward the x and y on the odometry both change and are just nonsense, we get extremely small numbers ((x) -0.02345, (y)-0.02234) and from my understanding on the FRC docs it is supposed to be in meters.

public SwerveModuleState getState() {
  double turnRadians = ((2.0 * Math.PI) / (12.8 * 2048)) * rotationMotor.getSensorCollection().getIntegratedSensorPosition();
  return new SwerveModuleState(nativeUnitsToDistanceMeters(driveMotor.getSensorCollection().getIntegratedSensorVelocity()), new Rotation2d(turnRadians));
}

12.8 is our rotation gear ratio and 8.16 is our drive gear ratio

Velocity given by driveMotor.getSensorCollection().getIntegratedSensorVelocity() is in units per 100ms. If you multiply that value by 10, do the numbers make more sense or do you still have issues?

When you say you got extremely small numbers, what value were you expecting to get? In other words, what position were you commanding the robot to go to?

You should not use .getSensorCollection().getIntegratedSensorVelocity(). This depends on the Status 3 frame which only is updated every 150ms by default. You should set the selected sensor to the integrated sensor and use getSelectedSensorVelocity() and getSelectedSensorPosition().

1 Like

Thank you all for such a quick response, we seem to be getting closer to our desired result.
our numbers are now within 1-0.1 range but when we move the robot a meter it thinks it only moves 1/3 of what it actually moved

When you say you got extremely small numbers, what value were you expecting to get? In other words, what position were you commanding the robot to go to?

We were expecting to get the numbers in meters because that is what the frc docs seem to suggest with how they set up their starting position
“our starting pose is 5 meters along the long end of the field and in the
center of the field along the short end, facing forward.”

You should not use .getSensorCollection().getIntegratedSensorVelocity() . This depends on the Status 3 frame which only is updated every 150ms by default. You should set the selected sensor to the integrated sensor and use getSelectedSensorVelocity() and getSelectedSensorPosition() .

Thank you for reminding us, think we had it that way but just forgot to change it back

Let me rephrase my question: Based on what you just said, the robot should start at (5, 0). Is this what the starting point you use in your trajectory? Or do you use (0, 0)? When you said you got ((x) -0.02345, (y)-0.02234) as your result, where were you trying to do? Were you trying to go from (0, 0) to (5,5)? (0, 5) to (5, 5)? (0, 0) to (50, 50)? I’m trying to figure out the difference between what your inputs are and what you expect to happen vs what is actually happening in order to better locate the issue.

Since you are getting closer to what you expect, the next thing I would check is if you are using the correct gear ratios for throttle and steering and aren’t mixing them up. Next, if you put the robot to the side and manually move one of the wheels, does the module say that it moved the distance of one circumference of your wheel? Or does it return a value 1/3 of that?

Also, how did you initialize your SwerveDriveKinematics kinematics variable? Did you use meters for the module positions or inches? And what order did you place your modules in when initializing the kinematics? Is it in the same order you have have when you update your odometry? (frontLeft, frontRight, backLeft, backRight)

The robot is doing nothing autonomously right now, all we are doing is making sure we are getting the correct values from the odometry before we make the robot move to a specific position, we are starting at 0,0 and are moving the robot by hand to the left one meter, so the expected outcome I believe would be Y 0~ and X 1~

We double-checked the gear ratios and they seem to be fine, we did the test with the one module and the numbers came back fine, so we don’t think that is the issue.

SwerveDriveKinematics was initialized in meters and is in the same order the odometry is updated (FL FR BL BR)

So if you are getting the correct position by manually rolling the module, then your math should be correct. The issue at this point would be either the velocity value you are getting or how the odometry is interpreting the data to get a position return. You noting that the data is ~1/3 off makes me think that you used feet instead of meters in a part of your code related to the velocity or odometry, but none of the code you showed so far indicates as such.

Just to double check, are you using SmartDashboard.putNumber("Robot Position (X)", odometry.getPoseMeters().getX()); to get your distance traveled? Did you change the sensor reading for your velocity to use driveMotor.getSelectedSensorVelocity(). Are you configuring the driveMotor in any specific way after initializing it (Do you call configFactoryDefault() prior to setting any other settings)? When you construct the SwerveDriveKinematics, did you use half the length and width in meters to set their positions? Are you getting any issues related to loop time overruns or any other issues in your code that may be related?

we are using the code below to get our readings.

SmartDashboard.putNumber("Robot Position (X)", odometry.getPoseMeters().getX());

we are using getSelectedSensorVelocity();.

we are not calling configFactoryDefault(); prior but we tested with and without and the results are the same.

we made the swerve drive kinematics in meter, and we tried it with a few other numbers such as 10 and 1 and still got the same result.

below should be everything we are using to do anything with swerve, I hope this help because we’ve done what feels like everything.
https://1drv.ms/u/s!AhKmyPVZBZzKgYo33gl9yKc3J3C_7A?e=IipUK5

Edit: we have updated to the newest version of wpi and we are still having the same issues

So the biggest thing that I see is that you declare your CANCoders and Swerve Motors with overlapping CAN IDs?

enum MOTORID {
        FRONT_LEFT_DRIVE(0),
        FRONT_RIGHT_DRIVE(1),
        BACK_LEFT_DRIVE(2),
        BACK_RIGHT_DRIVE(3),
        FRONT_LEFT_ROTATION(4),
        FRONT_RIGHT_ROTATION(5),    
        BACK_LEFT_ROTATION(6),
        BACK_RIGHT_ROTATION(7),
        ...
}
...

enum SENSORS {
        FRONT_LEFT_ENCODER(0),
        FRONT_RIGHT_ENCODER(1),
        BACK_LEFT_ENCODER(2),
        BACK_RIGHT_ENCODER(3),
        ...
}

If you connect to the robot via USB and use Phoenix Tuner, do you see 12 devices (8 motors, 4 CANCoders)? They should all be different IDs so if you declare a motor with a CAN ID of 1, you can’t have another device with the same address and Phoenix Tuner will show less than 12 devices and give you a warning that you have overlapping device IDs.

If this is actually the case, I’m surprised that the code is able to run at all in this state, as I would have expected the code to crash once it sees a conflict like this. My running theory at the moment is that because you are using multiple IDs that overlap, you are unsetting some of your drive motors, so some of them will not report a velocity, and because they aren’t reporting a velocity, the odometry math will give you a position less than your actual value.

Also you initialize your swerve Motors twice. If you are already initializing them within your SwerveModule class, then you don’t need to initialize them in your SwerveDrive class.

And, if you are using the CANCoder, it should be mounted to where it’s directly tied to the rotation of the module, so you shouldn’t have needed to account for a gear ratio in this. If this still returned a valid rotation, then it looks like your turning motors are still using their internal encoders and not the CANCoders.

Sorry for the late reply, but thank you for all the help.

From my knowledge, we have never had an issue with our IDs being the same as long as they were on separate devices, all of the devices show up on Phoenix Tuner and we dont get an overlapping CAN warning. Maybe we’ve had issues all these times because of that and we just never noticed or found a way around it.

I will make the changes to our SwerveModule, we were trying to use a number system to make a lot of things in our code work better but we can always just convert back.

The CANCoder is in the correct place, and I’ll make sure it’s using the module encoder instead of the motor encoder.

I’ll give you an update hopefully within the next 24 hours on how the changes went.

once again thank you for all the help you have given us.

I hate to say it, but our robot still reports the same numbers, I made the changes so it doesn’t initialize the motors twice, I switched the math to use the module encoder and I change all the ids.

I tried all these changes alone and together, really nothing has changed.

math change

double turnRadians = ((2.0 * Math.PI) / (Constants.SENSORS.EXTERNAL_ENCODER_RESOLUTION.GetResolution())) * moduleEncoder.getPosition();
  return new SwerveModuleState(nativeUnitsToDistanceMeters(driveMotor.getSelectedSensorVelocity()*10), new Rotation2d(turnRadians));

Motor/Encoder Change

  this.driveMotor = driveMotor;
    this.rotationMotor = rotationMotor;

    rotationMotor.configFactoryDefault();
    driveMotor.configFactoryDefault();
    //Module Encoders
    this.moduleEncoder = moduleEncoder;
    moduleEncoder.configFactoryDefault();
    
    rotationConfiguration = new TalonFXConfiguration(){{
      slot0.kP = Constants.SWERVE.P_ROTATION.get();
      slot0.kI = Constants.SWERVE.I_ROTATION.get();
      slot0.kD = Constants.SWERVE.D_ROTATION.get();
      remoteFilter0.remoteSensorDeviceID = ModuleId;
      remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
      primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
    }};
    rotationMotor.configAllSettings(rotationConfiguration);
    rotationMotor.setNeutralMode(NeutralMode.Coast);

    driveConfiguration = new TalonFXConfiguration(){{
      slot0.kP = Constants.SWERVE.P_DRIVE.get();
      slot0.kI = Constants.SWERVE.I_DRIVE.get();
      slot0.kD = Constants.SWERVE.D_DRIVE.get();
      slot0.kF = Constants.SWERVE.F_DRIVE.get();
    }};
    driveMotor.configAllSettings(driveConfiguration);
    driveMotor.setNeutralMode(NeutralMode.Coast);
    
    moduleEncoderConfiguration = new CANCoderConfiguration(){{
      magnetOffsetDegrees = offset.getDegrees();
    }};
    moduleEncoder.configAllSettings(moduleEncoderConfiguration);

Id changes

     int PDP_DEVICE_ID = 16;    

FRONT_LEFT_ENCODER(8),
    FRONT_RIGHT_ENCODER(9),
    BACK_LEFT_ENCODER(10),
    BACK_RIGHT_ENCODER(11),
    FRONT_LEFT_LIMIT(12), 
    FRONT_RIGHT_LIMIT(13), 
    BACK_RIGHT_LIMIT(14),
    BACK_LEFT_LIMIT(15),

FRONT_LEFT_DRIVE(0),
    FRONT_RIGHT_DRIVE(1),
    BACK_LEFT_DRIVE(2),
    BACK_RIGHT_DRIVE(3),
    FRONT_LEFT_ROTATION(4),
    FRONT_RIGHT_ROTATION(5),    
    BACK_LEFT_ROTATION(6),
    BACK_RIGHT_ROTATION(7),

We’re really stumped at this point, should we just multiple the numbers by 3? or does anyone have any idea on whay could be causing this?

Any help would be really appreciated.

The CAN IDs can overlap if the device types are different. There are many things that can be causing odometry issues but having cancoders with the same IDs as the motor controllers isn’t one of them.

You can try multiplying the velocity by 3 to see if it fixes your issue. In this case, I would start with a few simple autos to make sure that this fix will not cause the robot to behave erratically.

Recapping your issue, just so that I’m not missing anything:

  • The position of all four of your drive modules reports a correct distance moved if you move them manually.
  • When you only push the robot physically, without driving it in code, the odometry returns a value ~1/3 of what you expect.

If you put the robot on blocks and command all of the drive modules to move at a specified velocity/power, do all your drive motors report the same velocity?

EDIT: Do any of the modules return a negative velocity/position when moving the swerve drive ‘forward’?

Sorry once again for a late reply, but thank you for pointing that out.

One of our modules was reporting negative numbers and it has been fixed, but we are running into other issues with the robot at the moment and we are currently trying to fix it.

So I thought I would give you an update so you’re not left in the dark for too long.

I’ll get back to you once they are fixed, thank you for all the help you have provided our team so far.

Update, I think we found the problem.

after reverting the code back to a working version we noticed that not just one module was reporting incorrect numbers, but 1-2 were reporting wrong numbers.
(2 were wrong but then it was only 1, it was being weird but seems to be fixed now)

We have fixed the issues and the X now reports the correct numbers and we think so for the Y to but did not have the time/space to test it.

Thanks a million, for all the help everyone has given us, it really help us out and made sure our swerve dreams came to light, I’ll make sure to update you guys on if the Y is reporting correct numbers and will try to show you guys our autonomous in action if we get the chance to work on it.

Once again thank you all so very much. We don’t know what we would have done without you.

1 Like