Swerve Odometry Off

We’re experiencing a problem where our odometry, which we’re logging through AdvantageKit, isn’t aligning up with what’s happening in the real world.

This only happens when we’re running an auto where you turn and rotate at the same time.

We’ve tuned our second order kinematics so when you are controlling through teleop there is 0 drift when you drive forward and rotate, but we could’ve made a mistake with that. With AdvantageKit though it is plotting exactly the wanted path.

Is this just wheels slipping and us losing odometry? I’ve also messed with increasing the P for the pathing command, the module angle P, and the module speed P.

Here’s our swerve code: https://github.com/pittsfordrobotics/ChargedUp2023/blob/master/src/main/java/com/team3181/frc2023/subsystems/swerve/Swerve.java

Second order kinematics:

Pathing command:

Wanted Path:

Reality:

Here is the fudged path that ended up working:

5 Likes

Yup this is a problem pls help

1 Like

Our team is also having this problem. I hope others with this can share.

1 Like

How did you tune your Second Order Kinematics? I currently implemented algorithmic generation of the kV and use the onboard controller rather than setting the voltage like you guys do. What does that 0.065 multiplier represent too?

It’s tuning through disabling the lateral movement, which is y in field relative, and then driving back and forth with rotation tuning the value until we get no skew. The weird thing is there doesn’t appear to be any skew from the rotation.

After the robot finishes rotating it goes back up when it isn’t rotating, which is confusing. It could just be some bad carpet at our warehouse.

1 Like

THIS EXPLAINS SO MUCH! I was wondering how to correct for that. For reference I have been developing YAGSL and we used the 2nd order kinematics class which I think your team developed? (It was unattributed from the source we based YAGSL off of) and I have been wondering how to correct for this for a while now. I will be sure to add that as a configurable option then. Is the only thing you tune for 2nd order that differs that kV value for the feedforward?

Yep this was developed by my team. The 0.065 was a fudge factor I put in since the second order kinematics only gives spits out the angular velocity of the module, which we are multiplying by a feedforward, so therefore I didn’t see a straight forward way to find the additional angle offset that the module will be off by.

It was mostly done to make the visualizations look better, but I decided to put it in the swerve optimizer for something to test. I had meant to do some deeper testing on this, but since it worked and I haven’t had a lot of time to test, I’ve left it as is.

Screenshot 2023-03-27 at 4.59.23 PM
Base with only reading pure angle position
Screenshot 2023-03-27 at 5.00.45 PM
With fudged 0.065

steerPID.setReference(state.angle.plus(steerOffset).getRadians(), 
ControlType.kPosition, 
0, 
state.omegaRadPerSecond * (isOpenLoop ? SwerveConstants.MODULE_STEER_FF_OL : SwerveConstants.MODULE_STEER_FF_CL));

I’m running a seperate kV for open and closed loop because I found from my tuning of our swerve (which is a MAXSwerve) there is a different kV needed for the second order kinematics to function.

I hope this helps!

My team and others are experiencing drift towards the direction of the rotation while we spin and translate, did this feedforward fix that for you guys?

Yes, I’m not experiencing any skew.

1 Like

If you cut the velocities in half… how does it impact the error?

I ended up solving this myself. Kinda of a stupid bug, I had wheel base as 28x28 since the chassis was 28x28, but no, the swerve wheels are slightly inset, so in reality it’s closer to 24.6 x 24.6. This solved all of my problems and it runs perfectly now.

1 Like

Interesting! So, that is center of wheel to center of wheel? I bet we are doing the same thing… :slight_smile:

Yep, if your running MAXSwerve I found it’s 1.7 inches offset from the edge of the module. So you’ll want to multiply that by two. I just got that value from REVs CAD on their website.

1 Like

Honestly thanks for this… in our offseason robot I remembered to deal with the fact that the center to center distance isn’t the same as the drivebase width and I even left a comment in our code to remind me:

// Wheel base width in meters (from center of module to center of other module from LEFT TO RIGHT)
  private static final double wheelBaseWidth = Units.inchesToMeters(26);

but I completely just… didn’t bother accounting for that. I just factored this in and i’ll get to test tomorrow, hopefully it makes odometry more accurate!

2 Likes

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