Resetting odometry angle breaks heading control.

[UserC2]

Bug Description Resetting odometry to any angle other than 0 causes heading to believe that angle is 0.

Steps to Reproduce

1. Call RobotContainer.drive.drive.resetOdometry() with a Pose2D with a non-zero rotation (e.g. 90 degrees)
2. Attempt to drive the robot with heading or flick stick control (e.g. command it to a heading of 0 degrees).
3. Observe the robot will go to an offset angle (e.g. will go to 90 degrees).

Expected Behavior

• The robot aligns to the desired angle (in the example, 0 degrees).

[newjanson]

Does heading velocity control still work?

[UserC2]

It should be unaffected, as it doesn't take the robot's current angle into account.

[newjanson]

We should perhaps not use resetOdometry(). For localizing with apriltags, we use RobotContainer.drive.drive.swerveDrivePoseEstimator.addVisionMeasurement().

[UserC2]

We should perhaps not use resetOdometry(). For localizing with apriltags, we use RobotContainer.drive.drive.swerveDrivePoseEstimator.addVisionMeasurement().

Reset odometry is used for PathPlanner to reset the pose of the robot when autonomous starts, not for vision measurements.

[newjanson]

ah, i see

[UserC2]

Ok, I've figured out why this happens:

• resetOdometry() resets the odometry and gyro angle assuming the robot is at the provided position.
• If the robot isn't at the provided position (e.g. setting up the robot in autonomous in the wrong position), the odometry will report the robot being in a different place than it actually is, and the gyro angle will be offset.

[UserC2]

We can fix this in two ways:

• Don't call resetOdometry() and use the Limelight to find the robot position.
  • This means auto will fail if the Limelight doesn't find the starting position.
  • Since the Limelight doesn't work in simulation, autos won't be able to be tested in simulation.
• Leave the resetOdometry() call in and ensure the robot is set up in the correct position at the start of auto.
  • Requires the setup team to be precise or position and gyro angle will be off.
  • However, since we use the Limelight to reset odometry anyways, the Limelight will correct for this error when it is working.
  • If the Limelight fails, then the starting position of the auto will be used to reset the odometry, so this method is more reliable.

The second method is ideal, since it still works when the Limelight fails and still works in simulation.

In summary: The current code works as expected. Remember to setup the robot in the correct place during autonomous (especially in simulation or when the Limelight doesn't work).

[UserC2]

In simulation, make sure to zero the gyro before running an autonomous path, or the heading will be offset incorrectly (these instructions have been added to README.md).

[UserC2]

Nevermind!

We are using SwerveDrive::getYaw() to get the heading in most commands (except PathPlanner and AlignToAmp), which never applies the gyro offset.

I will be changing that soon.

[UserC2]

See c98ddf5