Swerve X and Y Flipped in Odometry

[LSao]

We have been faced with a mystifying issue. We have gotten our MAXSwerve manual field relative driving to work. However, we then tested out path following using pathplanner, the robot follows paths at a 90 degree offset. We troubleshoot further and looked at the field2d in Glass. We found that when we drove forward manually, the robot odometry in Glass showed the robot icon move to the right. When we moved to the side manaully, the robot odometry in Glass showed the robot moving forward and backward. Could this be an issue where CCW rotation is not positive? Has anyone had a similar issue?

Our Swerve drive code is here:
https://github.com/FRC1884/season2024/tree/main/src/main/java/frc/robot/core

Our drivetrain subsystem is here:
https://github.com/FRC1884/season2024/tree/main/src/main/java/frc/robot/subsystems

Our constants related to offsets are stored in Robot.java

[LSao]

Do we need to invert the Gyro? I am not sure this will fix the issue.

[gdefender]

WPILib’s coordinate system is NWU. See the picture below. Positive X is forward, positive Y is to the left, and positive rotation is CCW.

I don’t need to go any further than your kinematics to see that you have this incorrect.

  public static final SwerveDriveKinematics kDriveKinematics =
      new SwerveDriveKinematics(
          new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
          new Translation2d(-kWheelBase / 2, -kTrackWidth / 2),
          new Translation2d(kWheelBase / 2, kTrackWidth / 2),
          new Translation2d(-kWheelBase / 2, kTrackWidth / 2));

Tracking this through your code, the order is FL, FR, BL, BR. The signs on your wheel translations are incorrect. They should be like this:

Once you get the kinematics set up properly, you will notice that you’re not driving the robot in the right direction. DriverMap and TwoJoyStickDriverMap is driving the robot sideways (robot X is forward/backward, joystick X is side-to-side.)

drivetrain.driveCommand(
    this::getSwerveXSpeed, this::getSwerveYSpeed, this::getSwerveRot));

  @Override
  public double getSwerveXSpeed() {
    return -controller.getAxis(Axis.AXIS_LEFT_X) * MaxSwerveConstants.kMaxSpeedMetersPerSecond;
  }

  @Override
  public double getSwerveYSpeed() {
    return controller.getAxis(Axis.AXIS_LEFT_Y) * MaxSwerveConstants.kMaxSpeedMetersPerSecond;
  }

Most likely you’ll find there are other workarounds in the code that made it appear to be working, even though the robot is driving sideways. I would make the changes above, and then do the following:

1. Verify that your drive motors spin forward when you apply positive output.
2. Verify that your encoders read positive when you turn the wheel azimuth CCW.
3. Verify that your azimuth motors rotate CCW when positive out put is applied.
4. Verify that your gyro rotation is positive when you rotate the robot CCW.
5. Verify that your azimuth is consistent:
   a) The units need to be consistent (rotations, degrees, or radians)
   b) The range needs to be consistent ([-180°, 180°], [0°, 360°], [-Pi, Pi] radians, [0, 2Pi] radians, [-.5, .5] rotations, or [0, 1] rotation.

[LSao]

Thank you for this. We will go through this process during our next session.

PS. Thank you for the great videos on Apriltags. We’ll definitely be using your team’s 2023 code to improve our pose estimation.

[Skyehawk]

more here:

Swerve Odometry has Y axis direction reversed

You likely are running into a slightly simpler problem than what this thread discussed (swerve drive module rotation direction). WPLlib uses a coordinate system closely tied to polar coordinates. Specifically, robot-relative coordinates treat “forward” as the positive x axis, and angles increase in the counter-clockwise direction. You will get mystifying problems if you instead think that the robot’s “forward” is the positive y axis and/or angles increase in the clockwise direction.

[gdefender]

I also have a PR open in frc-docs that goes through this in details, that’s where I took the picture from. Hopefully this gets merged in the next week or so.

[LSao]

We tested out the edits, changing xspeeds to be controlled by the joystick y-axis, yspeeds to be controlled by the joystick x-axis, and fixed the SwerveDriveKinematics. However, we are still having a similar issue where the front of the robot on glass is 90 degrees counter clockwise to where it should be. Is there something else you think we could be missing?

[gdefender]

Is your latest code you’d like us to look at still on the main branch? The code in that branch doesn’t start up. I get this error:

Unhandled exception: java.lang.IllegalStateException: A CANSparkMax instance has already been created with this device ID: 8

When I open your code I get this message, and I don’t know exactly why. You might want to figure that out:

EDIT: I think that message is because you have "projectYear": "2024beta" in your wpilib_preferences.json file. Your build.gradle file also has older versions of JUnit. I would just run the project importer to get it fixed up.

I also noticed your CTRE vendor libraries are beta. You should update to the latest (and the latest firmware.)

Now, to your question, why is the robot rotated 90-degrees in Glass. It’s because you have code in your MAXSwerve constructor setting the pose to a heading of 90°. You should just delete the last line in this snippet:

odometry =
    new SwerveDriveOdometry(
        MaxSwerveConstants.kDriveKinematics,
        getYaw(),
        new SwerveModulePosition[] {
          fl.getPosition(), fr.getPosition(), bl.getPosition(), br.getPosition()
        });

this.resetOdometry(new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(90)));

[LSao]

Thank you so much for your help. We were able to get manual driving and odometry working correctly today. We realized we also needed to fix our azimuth motor offsets to match the MAXSwerve template. I saw your PR went through on wpilib: Coordinate System — FIRST Robotics Competition documentation. This is very helpful. I am thinking that we will set our origin to be the blue alliance every time to simplify things, especially since photonvision gives poses in terms of the blue alliance origin. All the best to you and your team.