Hello! Our team has implemented YAGSL for our swerves, but we are having trouble with PathPlanner autos. Teleop swerve works just fine though. We can do an autonomous routine that goes in a straight line just fine, but when we turn, that’s when the issue arises. We think the issue has something to do with the PID values from YAGSL. The robot rotates when it doesn’t need to rotate at all. The only path that works is a straight line with no turns or rotations. Here is a video of the issue: https://youtu.be/LopeXSlMopQ and here is our code. For the video, the robot is supposed to go in a straight line, and then move right for ≈1 second. There shouldn’t be any rotation involved, but it rotates abnormally. Does anyone have any suggestions for what we can do about this? I would appreciate any help, thank you!
Use 0.01 for PathPlanner PID P config. My team found this out today
Thanks for the suggestion, we will give it a try! @nstrike Did your team have a similar issue?
We (@nstrike 's team) had a similar issue but not totally the same.
However, we did identify that extreme oscillations are likely caused by kP being too high in the AutoBuilder constructor inside of SwerveSubsystem. PR to YAGSL-Example has been submitted to reduce the default, which had been originally pulled from the PathPlanner docs.
This is Team BroncBotz 3481 reaching out to provide an update. We’re the team Nstrike mentioned yesterday, encountering challenges with PathPlanner. We managed to address the initial hurdle where the default PID settings in the PathPlanner setup—specifically the initial kP value of 5.0—were causing erratic oscillations in our robot’s movement. Adjusting the kP value to 0.1 alleviated this issue.
However, we’re now facing a new challenge. Regardless of the path configuration, our robot accelerates continuously in one direction without stopping. Our workshop will be open this afternoon, and until then, I’ll be reviewing the code to identify any potential issues. Our plan is to also collect data from FRC web components to analyze the settings for each module.
In the meantime, I’ll examine your code to compare setups. Could you share details about the hardware you’re using? Thanks.
Sure! We are using MK4i swerves with NEO1.1 & Sparkmax, with CanandCoder encoders connected directly to the SparkMax data port. I hope you fix your issue, would you mind posting an update when you fix it? Maybe we have a similar issue. We are meeting tonight too, I’ll post here again later tonight with an update if we fix our problem. Thanks!
@nstrike We switched the kP value to 0.01, but we are still having problems. We throttled the speed and made sure the offsets were correct, and we put a hard constraint on the entire path, but it still exceeds our 1m/s limit. It appears that the control loop is causing the error to increase, the actual velocity and path inaccuracy graphs look almost exponential in pathplanner. We think an issue could be with the gyro, we’re using a navx on the mxp port. The field widget shows correct rotation of the robot only when the navx is uninverted, which should be the opposite by FRC standards. We are using an Xbox controller too, if that influences how the teleop loops run off of those inputs. Attached is a youtube link with the robot path and pathplanner screen showing the graphs: https://youtu.be/YV2MwknogMQ. Thanks for helping!
Just to confirm what speed MK4is are you using?
It is probably fine. You could probably change around the drive inversion and the NavX inversion, but your NavX could be facing the wrong way to the “front” of the robot.
We are using the L2 version of the Mk4i but we put a hard constraint of 1 meter per second on PathPlanner and in the code.
Just want to chime in and say reducing the P value for the PID controller that much here is not a solution, it is just hiding the problem. You’re essentially making the PID controller do nothing and rely on just feed forward, which will be bad for accuracy. 5.0 is a heavily tested value that should work as a good starting point for most robots, if anything its a bit low (we are running kP = 10.0 ourselves). 0.01 will do nothing, essentially the same as just setting it to 0. For example:
target X = 0 meters
actual X = 1.0 meters
error = -1.0 meters
feedback output = 0.01 * -1.0 = -0.01 m/s
For a more reasonable error you’re likely to see:
target X = 0 meters
actual X = 0.05 meters
error = -0.05 meters
feedback output = 0.01 * -0.05 = -0.0005 m/s
With feedback outputs like that it will never be able to correct for any error.
You guys should look at reverting this PR and find what the actual oscillation problem is, as having no feedback while path following is gonna really come back to haunt you and anyone else using this example later.
Same explanation as to why you shouldn’t do this above.
I’m not sure exactly what your issue is or how the swerve library you are using even works, but the PID values you have here just feel wrong to me. I don’t think I’ve ever seen a working PID controller that had the D term so significantly higher than the P term. I’d recommend setting the D term to 0, then tuning just the P until you’re getting pretty good accuracy out of your modules, then tuning D if needed.
Excessively high D terms can cause some pretty bad oscillations, or just generally weird behavior.
I’ve been experiencing a similar issue with YAGSL and pathplanner. The Robot would rotate the wrong direction during auto (but worked fine in teleop). This is why a P value of 5 caused issues for me, going the wrong way caused the robot to attempt a large correction which simply made it worse since it was already going the wrong way. A P value of 0.01 masked the issue since the robot barely adjusted for the large error in the rotation
To fix this issue I had to edit the YAGSL library. I don’t know what the best way of solving it was but as a quick test I adjusted the setChassisSpeeds function to:
public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
{
SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond);
chassisSpeeds.omegaRadiansPerSecond *= -1;
setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
}
flipping the radiansPerSecond speed fixed the rotation issue. @nstrike will an adjustment be worked into the official YAGSL libraries for this? I don’t think I’m the only one with the issue
I will have to investigate this fix some more but if it comes out how you say it is then i will include it. Can you make a PR to the YAGSL-Examlple dev branch so your team can be credited?
By this logic the passed in ChassisSpeeds is expecting CW+ not CCW+.
I will do that. Thanks
maybe this is a result of incorrect module location configuration. 
Before I made the edit to the YAGSL library, when I manually put in a positive chassis rotation speed the robot rotated CW. Path planner would have expected that to go CCW. My code is here if you want to take a look at my YAGSL config
Have you tried following this page of docs? (Its new)
I think somewhere after step 4 it may work without this patch.
If you flip the modules around (think backwards as forwards and left as right) CW rotation becomes CCW rotation when attempting to rotate in place.
I probably spend about a dozen hours flipping settings in my YAGSL config over the past week in an attempt to solve this issue, nothing worked. I’ll give some of those steps a try to see if that makes a difference rather than the edit I made to the library
Thanks! I would save off your current config and instead of making the change in the library make it in the SwerveSubsystem. Just incase the changes don’t work.
In theory this may be bc your gyro is reading CW+ (somehow) and your motors are doing this bc the locations are incorrect. Flipping them like bellow so your gyro is invertIMU true and swapping ID’s like bellow should work.
