So we recently were testing out Path Planner with our swerve robot. We we’re able to implement it pretty well but we were confused on how to to tune the PID values for the robot.
For reference here is the command and as you can see there are three PID controllers:
HashMap<String, Command> eventMap = new HashMap<>();
return new SequentialCommandGroup(
new InstantCommand(() -> {
if(true){
m_drivetrainSubsystem.resetOdometry(traj.getInitialHolonomicPose());
}
}),
new PPSwerveControllerCommand(
traj,
m_drivetrainSubsystem::getPose,
m_drivetrainSubsystem.m_kinematics,
new PIDController(0, 0, 0),
new PIDController(0, 0, 0),
new PIDController(0, 0, 0),
(SwerveModuleState[] states) -> {
m_drivetrainSubsystem.m_chassisSpeeds = m_drivetrainSubsystem.m_kinematics.toChassisSpeeds(states);
},
eventMap,
m_drivetrainSubsystem
)
);
We set the values for 0 for now but we were trying to tune it just so that the command can be more accurate when running. For reference, here is our entire code: https://github.com/FRC5549Robotics/BunnyBots_2022
Move the P value up until you hear oscillation (in my experience it’s pretty obvious). Then turn it down until there’s no oscillation. Usually good values land around 3-5 for the translation components iirc. Sometimes you’ll want a D value but I haven’t seen its value. Hopefully the PID won’t be doing much work if your robot is able to follow the path.
You can test each of the controllers separately from the path as well to make it easier to tell what needs to be tuned. Below is an example of that, the path command doesn’t use the profiled PID controllers, but the constants are the same.
public static final double kMaxSpeedPerSecond = 2; // Meters per second
public static final double kMaxSpeedPerSecondSquared = Math.pow(kMaxSpeedPerSecond, 2); // Same as above, but squared
public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(kMaxSpeedPerSecond, kMaxSpeedPerSecondSquared);
public ProfiledPIDController snapController = new ProfiledPIDController(kP, kI, kD, kThetaControllerConstraints);
public ProfiledPIDController xController = new ProfiledPIDController(kP, kI, kD, kThetaControllerConstraints);
public ProfiledPIDController yController = new ProfiledPIDController(kP, kI, kD, kThetaControllerConstraints);
// Returns a ChassisSpeeds to drive the robot to x and y coordinates in meters, ending at the specified angle
// Ths should be run periodicly until the robot reaches it's setpoint
public ChassisSpeeds setPosition(double x, double y, double angle) {
// The second number here is 0 because we want the robot to have no more velocity when it reaches the target
xController.setGoal(new TrapezoidProfile.State(x, 0));
yController.setGoal(new TrapezoidProfile.State(y, 0));
snapController.setGoal(new TrapezoidProfile.State(Math.toRadians(angle), 0.0));
// getPose() should return the robot's position on the field in meters, probably from odometry
// getYaw180 just returns the reading from the gyro
double xAdjustment = xController.calculate(getPose().getY());
double yAdjustment = -yController.calculate(getPose().getX());
double angleAdjustment = snapController.calculate(Math.toRadians(getYaw180()));
return ChassisSpeeds.fromFieldRelativeSpeeds(xAdjustment, yAdjustment, angleAdjustment, Rotation2d.fromDegrees(getYaw180()));
}
Not sure how well they will work for you, but here are our constants if it helps:
Rotation controller:
kP = 5.0;
kI = 0;
kD = 0.0;
Position (x/y) controllers:
kP = 2.5;
kI = 0;
kD = 0.0;
Also, just so you know, putting three of these: ` before and after any code with the language name will keep it formatted nicely which makes it a lot more readable
We were actually wondering the same thing, currently, we have a stop command just so that at the end of the trajectory it has a guaranteed stop no matter the state of the path. Because I don’t believe it will ever stop once the command has ended when the trajectory time has elapsed and a constant motor voltage will be applied. That was a rather unpleasant experience, I’m wondering if tuning the pids will account for this entirely.
I think it would depend on how your code is written. If it’s calling the drive method in a command, it will stop updating when the command ends. Usually, the drive method just sets the robot to the given speeds. If the command ends and the robot is still set to a certain speed, it will just keep driving at that speed. Even if it is getting the speeds from a PID controller, it still needs to be updated so the robot knows to stop when it reaches the right position. However, if you were updating it in the drivetrain subsystem’s periodic method, it would keep updating the robot speeds to get to the set position and should stop when it reaches it. That should only be necessary if you are planning on stopping it in the middle of a trajectory, but just having a separate stop command is fine in that case too.
We are using the path planner swerve drive command which only sets the drive motors to 0, if interrupted. Therefore, unless the PIDS are perfect, it won’t set the drive motors to 0, and the motor watchdog didn’t set them to 0 when not updated so a stop command was the easiest to implement.
