RamseteCommand sometimes spinning in circles

I’ve been having a weird issue while using path following with RamseteCommand. Sometimes, instead of following the path, the robot will spin in circles. This isn’t very common though, maybe happening 1 in 10 times, which makes it painful to debug. I’ll run the auto, everything works great, then without redeploying code or changing anything, I’ll drive the robot back to the starting spot, put the cargo back, start the auto again, and when it gets to driving it will spin. Most of the time the path following works great, so it’s not something simple like flipped gyro or encoders. This has happened right after turning on with a new battery, on a battery that hasn’t been changed in half an hour, and everything in between.

Any help would be greatly appreciated. I’m at a loss of what could be causing this issue.

All the code running while seeing these issues is here. I’m only using new/ThreeCargoA and new/ThreeCargoB.

Within drivetrain.resetOdometry(), try calling navx.setAngleAdjustment(m_angle), where m_angle is the starting angle (as a double in degrees) from your trajectory.

I’ll try this, thanks!

This did not fix the issue. I’ve started testing only after reboot since I’ve been seeing very different behavior when driving before the auto starts. With and without calling navx.SetAngleAdjustment(m_angle) the robot drives in large one large circle instead of following the path. This happens nearly every time.

In your drivetrain code where you update your odometry, change navx.getRotation2d() to navx.getRotation2d().unaryMinus(). This will invert the angle output. WPILib trajectory following expects a positive angle as counter-clockwise, while the navX returns clockwise as positive by default.