Swerve Odometry Off

michaelyu · March 26, 2023, 11:11pm

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: ChargedUp2023/Swerve.java at master · pittsfordrobotics/ChargedUp2023 · GitHub

Second order kinematics:

Pathing command:

github.com

pittsfordrobotics/ChargedUp2023/blob/master/src/main/java/com/team3181/frc2023/commands/SwervePathing.java

package com.team3181.frc2023.commands;

import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
import com.team3181.frc2023.Constants.AutoConstants;
import com.team3181.frc2023.subsystems.swerve.Swerve;
import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.CommandBase;


public class SwervePathing extends CommandBase {
    private final Swerve swerve = Swerve.getInstance();
    private final PathPlannerTrajectory trajectory;
    private final boolean reset;

This file has been truncated. show original

Wanted Path:

Reality:

Here is the fudged path that ended up working:

EvanM · March 27, 2023, 12:24am

Yup this is a problem pls help

pmxi · March 27, 2023, 2:30am

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

nstrike · March 27, 2023, 6:30pm

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?

michaelyu · March 27, 2023, 6:32pm

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.

nstrike · March 27, 2023, 6:45pm

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?

github.com

BroncBotz3481/YAGSL-Example/blob/module_optimization/src/main/java/swervelib/SwerveModule.java#L156-L158


      
          desiredState = SwerveMath.optimize(desiredState, getState().angle,
                                             Units.radiansToDegrees(lastState.omegaRadPerSecond * configuration.angleKV) *
                                             0.065); // I am unsure of what the 0.065 represents

michaelyu · March 27, 2023, 9:04pm

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!

nstrike · March 27, 2023, 9:54pm

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?

michaelyu · March 28, 2023, 1:31am

Yes, I’m not experiencing any skew.

Hollisms · March 28, 2023, 1:31am

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

michaelyu · March 28, 2023, 1:33am

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.

Hollisms · March 28, 2023, 1:38am

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

michaelyu · March 28, 2023, 1:42am

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.

Rezq · March 28, 2023, 1:46am

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!