Fixes the resetOdometry method

[MarshallTappen]

The call to swerveDrivePoseEstimator.resetPosition should pass in the current gyro angle as the first argument.
Without this fix, it is impossible to start the robot in autonomous with a non-zero pose angle.

Thanks for YAGSL!

[thenetworkgrinch]

If i remember correctly i use the Pose2d rotation because i wanted to be able to test in sim. I will have to look into wpilib docs about this tomorrow. Thank you for your contribution!

[MarshallTappen]

Sounds good! I found the problem using sim. If you start with a trajectory where the robot heading is at 180 degrees, that initial pose gets removed because the gyro offset does not get set right in the SwervePoseEstimator.

[thenetworkgrinch]

Hmmm, i think i need a SwerveDrive.setYaw or SwerveDrive.setRobotHeading function then

[thenetworkgrinch]

That way i can set the current orientation of the robot without it actually shifting it's 0 to it. Can you add that to this PR and I will merge it then?

[thenetworkgrinch]

This removes the ability to set rotation in odometry so we should ensure that the ability is maintained by adding a new function SwerveDrive.setHeading.

[MarshallTappen]

The SwerveDrivePoseEstimator is designed to not require the user to manually offset the gyro The docs comment for resetPosition says

The gyroscope angle does not need to be reset in the user's robot code. The library automatically takes care of offsetting the gyro angle.

I dug through it last night and when you call resetPosition it takes the current gyro angle and the desired pose. At that point, it calculates an offset between the gyro and the desired position. The third argument to resetPosition contains both the desired translation and rotation.

If you pass in the desired pose to the first argument, then it assumes that the offset is 0 because the heading from the gyro and desired pose are the same.

[MarshallTappen]

Quick follow-up, I think I misunderstood your thinking. You would like to have an explicit method to set the yaw for testing, like in sim?

[thenetworkgrinch]

The heading from the gyro and desired pose arent always the same? Especially if the robot was turned on while 0 was not correct. I have a function which resets the odometry using the first pose.

[MarshallTappen]

Yes, I think swerveDrivePoseEstimator.resetPosition already does exactly that. SwerveDrive.resetOdometry (with the patch) reads the current gyro angle, then calls swerveDrivePoseEstimator.resetPosition with the current gyro position and the new pose. The pose estimator then calculates an offset that maps the gyro angle to the desired heading passed into resetPosition. Any future calls to get the new position will use that offset to correct the gyro heading. So, the user never has to worry about correcting the gyro - if they just use odometry.

Any calls to getHeading or getYaw will be uncorrected, though, which could cause confusion. One option would be to make getHeading in SwerveSubsystem just return the heading from the pose estimator, instead of the direct gyro reading. I think that is probably better for less-experienced teams who may not realize that the headings from getPose and getHeading could return different values.

We could add a getIMU() method if the user really wants to directly read the gyro directly. Or, keep getYaw() in SwerveDrive as a direct gyro reading.

[thenetworkgrinch]

I agree, could you make that change for SwerveSubsystem thank you for the debate! This will be tested tomorrow

[MarshallTappen]

I added a second commit that modifies getHeading() to use the odometry pose..

If this ends up testing correctly, I can also do a quick PR to change getHeading to getOdometryHeading . It could break existing users' code, but it would make sure they are intentional about using odometry vs raw gyro.

[thenetworkgrinch]

I just tested and did the same. It doesnt fix what I would like it to however it is a good change. The original resetOdometry function (from a year ago) did this and set the yaw of the gyro which should be considered too. At this point the benefit i can see of setting the gyro yaw while using the current method is logging but you could argue otherwise as well if you log getPose().

[MarshallTappen]

Quick question, what other issues are you seeing? I can be on the lookout for them in our testing.