My team 6894 Iced Java Is trying to get into using Path Planner for our autonomous now that we have swerve drive, but it only seems to work the first time that we launch it after restarting everything. I would put some code here, but i genuinely dont even have a lead on what it could be, so i will put the github that only has our swerve drive and autonomous code and pathplanner paths in it, encase this has happened to one of you guys and you want to check if we did the same problem you guys did. Github
Some possible things that could have messed it up is how we use pigeon for our gyro which didnt have a build in getRotation2D, but we are not sure.
When i say that it worked sometimes, i mean the first time we launch it, it will work perfectly, but when we run it a second time from the same spot(moved it back via teleop), it drifts to one direction seemingly randomly and unpredictably. i will upload a video of it here from testing. video(too big to upload so im using google photos)
I haven’t had the chance to look at your code, but my initial thought when reading the problem is that you may not be resetting the pose at the beginning of the auto and are only doing it on initialization of the robot. If that is the case, then I’d say it thinks it is in a different position the second time than it did the first time.
When I get the chance I will look at your code if that isn’t the fix, but in the meantime my team also does open alliance and you can take a look at my team’s code and view how we use pathplanner.
Based on what I’ve seen with your code, I’m unsure of the actual issue but I would begin troubleshooting by printing your robots pose and seeing where it thinks it is in comparison to where it actually should be. If you do that or try any other things for troubleshooting let me know and I can attempt to continue guiding. If you find the solution you may also respond and mark it as the solution on this thread for others to see in case they run into the same issue.
I don’t believe the issue is with the pose not being reset, but rather these lines from your resetPose function:
In this case, I’m pretty sure using the Rotation2d constructor is incorrect, since it takes a rotation value in radians, and gyro.getYaw() seems to return a value in degrees. After first starting the robot, the yaw will be equal to zero, and because 0 degrees is equivalent to 0 radians, everything functions properly. After resetting the robot, the yaw will likely be a bit larger than 0, and so that causes the inaccuracies. I think modifying your code to use Rotation2d.fromDegrees(gyro.getYaw()) instead of new Rotation2d should fix the issue, or it looks like you could replace resetPose with the resetOdometry method currently in your code.