Resetting odometry and gyro on trajectory start

Technical Programming
1

We only use one trajectory for each of our autonomous routines so we reset the odometry position and gyro angle according to the first location from the trajectory. For some reason, the odometry angle successfully resets while the gyro angle doesn’t. The bot follows the trajectory correctly only if it booted in the correct angle, if we change the angle after code initialization the bot will spin while trying to follow the trajectory.

Our gyro reset code:
public void zeroPosition(Pose2d newPose) {

    System.out.println("Zero!");

    System.out.println(m_pigeon.setFusedHeading(newPose.getRotation().getDegrees()));

    // System.out.println(newPose.getRotation());

    // updateGyroAngle();

    // System.out.println(getGyroscopeRotation());

    holdAngleSetpoint = newPose.getRotation().getRadians();

    // m_navx.zeroYaw();

    // Reset odometry angle

    driveOdometry.resetPosition(newPose, getGyroscopeRotation());

}

The Code that calls the reset (during trajectory command initialize):
PathPlannerState headingGoal = (PathPlannerState) trajectory.sample(0);

Pose2d startingPose = new Pose2d(headingGoal.poseMeters.getX(), headingGoal.poseMeters.getY(), headingGoal.holonomicRotation);

driveTrain.zeroPosition(startingPose);

2

When resetting odometry, you don’t need to reset the gyro position (the odometry class tracks and applies an offset based on the pose you give it) but you do need to reset your encoders. Check out the example on this page.

1 Like
3

We reset the gyro as well since we use it for field centric drive so we want to reset it before starting teleop.
We narrowed down our problem to the fact that the setFusedHeading method resets the gyro to the wrong angle by a a factor of about 63. if we just set the gyro to 0 everything works as expected but if we set it to 90 degrees it reports 1.41016 degrees and it scales linearly with other angles.
It looks like radians and degrees problem but our error is not the same ratio as degrees/radians.

1 Like
4

Personally I don’t trust underlying hardware reset functions. CAN encoders have issues too as the reset takes time to propagate back to the get() function. It’s easier and more predictable to store an offset and do the subtraction yourself (abstracted in the subsystem).

3 Likes
5

We’ve seen exactly that. Trying to zero the Pigeon 2.0 gyro with setYaw(0.0) and then immediately doing a getYaw() will almost always return the (non-zero) yaw prior to setting it to zero.

I’ve seen example code for resetting the robot’s pose that zeros the gyro and then immediately uses the gyro angle in a setPose() call. That’s not going to work, unless your yaw was already zero. So if your “resetPose” command rarely works on the first button press, but does on the second or third press. This is probably why.

6

Hi,
I had similar experiences and created a post about a week after you. I believe the scale factor is x 64. I’m also lead to believe the issue is that GetFusedHeading reads the values from the last status frame send out and this is reason that a call to getFusedHeading directly after a call to SetFusedHeading will give you the old gyro value before reseting.
See this post for more info and my explanation of why this causes problem with odometry.

7

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