PathPlanner Auto Going Off Path When Rotating

I am a programming mentor for 1714 MORE Robotics. I am hoping you all can help me with an issue with our swerve autonomous. We use the BaseTalonFXSwerve library that used to be Team364’s BaseFalconSwerve. We are using TTB Absolute Mag Encoders for which we have written our own class for. We are using PathPlanner to run our autonomous routines.

Everything works perfectly in teleop. Everything works pretty well in autonomous when the path HAS NO ROTATION, however we do notice some rotational drift that IMO should be handled by the rotation PID so that is certainly strange. The bigger issue is when we have rotation in our path, the robot refuses to rotate at all and goes completely off the path, as shown below (solid is actual and transparent is target).

odom_replay

We have tried a few different path shapes and they all do completely unexpected things. I have been working for a few days to try and track down the source of the error and I have tracked it as far as I can go without additional assistance. In pathplanner.lib.commands.FollowPathCommand.execute() there is a line where a ChassisSpeeds object called targetSpeeds is created and eventually passed into our supplied setChassisSpeeds() method to drive the robot. To create that object, the PathFollowingController.calculateRobotRelativeSpeeds() method is called and passed the current pose and target state. The same current pose and target state objects are then logged a few lines later and are what are shown in the attached replay. From what I can gather, the current pose is accurately reflected and the target state seems to be what we expect from our path. However, the targetSpeeds object is where things go wrong.

In the above image is the omega (rotational velocity) as well as the x and y velocity collected from our setChasisSpeeds() method that targetSpeeds is passed into through a Consumer . As you can see the set omega seems to be increasing at a reasonable amount to run a path that is very close to a turn in place, however, the set x and y velocity are going nearly max acceleration. Based on the current pose and target state passed into the calculateRobotRelativeSpeeds() method, this doesn’t seem to make a lot of sense.

I have not deep dived into the calculateRobotRelativeSpeeds() method to determine how the math might be going wrong but I really find it hard to believe that this method could have a bug in it. I have attached the log file and have more from this past meeting if anyone wants more data. Could someone help me sort this out?

Our Code (links to specifically relevant branch)

Any chance the robot is trying to get to a start pose of (0, 0)?

Nope, very good question to ask though.

What is the intended PathPlanner path that you are trying to run?

Try a Auto turning P value of like 5

Was like that from the start.

It is a bit whack but it is intended to be as close to a rotate in place as possible. Trust me, we tried it with a path that goes forward and rotates at the same time and got the same results.

path_demo

Ah ok so Constants.java isn’t used

Not yet :wink:

Working out the major kinks then I will enforce a coding standard.

Does reading the gyro work? Because you could use get2Drotation instead of getYaw

Gyro works

I wonder if it’s the alliance color of the driver station that could be influencing it. I’m sure you must have already checked that but just adding it onto here as something to check, because the translation makes it seem like the robot wants to go to the other side of the field. Also, does your gyro have a wrap that forces it to be within [-180, 180)? You might also consider writing a more basic path that just goes straight and rotates with a holonomic rotation of 90 degrees at the end; I think that is a simpler path to analyze than a “close enough” rotation in place.

I agree that it seems like the robot wants to go way across the field.

However, it does stop about a meter or two away from the original location as if it found it’s location, so that is strange. We did actually have a problem with driver station flipping but fixed that by putting a path in an auto.

We should look into the gyro wrapping. We did try the below path. Instead of the quarter circle, it went a full half circle (about twice the X distance at about half the Y distance) without rotating. It also is definitely drifting in rotation along long translational paths so just whack stuff all around.

One thing that helped us debug our PathPlanner issues is just zeroing the modules with the absolute encoders (just doing it again for the sake of starting from ground 0), and then running paths on a cart/place where the wheels cannot come into contact. Doing that for each auto routine (starting from a basic routine → driving + rotating) can allow you to take a step-by-step approach into understanding what’s going wrong. After running it on a cart we then ran it on the ground and compared the results. It might be useful in terms of debugging.

You can also use the IEEERemainder(angle, 360) functionality in the math library to enable gyro wrapping.

PathPlanner is not meant for doing turn in place. That will create a pretty busted spline and cause a bunch of issues. If you want to turn in place, you need to create a separate command to do that yourself.

2 Likes

I understand that. We have tried some other paths with strange results. The below path was where we found the problem and the red line is approximately what the robot did without rotating.

image

And just to confirm … Where does the Field2d widget show the robot in shuffleboard or whatever dashboard…?

You have a very short control point length on that path as well. That is what can break things. Change it to something like this:
image

Now this is something we have not tried. Not sure why they were like that in the first place but we will try that.

Back when we ran that path was early in our 2024 auto testing so we weren’t logging anything yet and didn’t look close at the PPTelemetry so I can’t tell you for sure. But I can say that our SwerveOdometry object has been keeping track of our robot’s actual position nearly perfectly through all problems so I would assume that it would work.