Hello, I’m a mentor and my team’s been having issues with the 2024 version of PathplannerLib. Whenever an Auto or Path is run, it frantically rotates left and right before coming to a stop far short of the intended end point. Our swerve drive code is based off the framework provided in https://github.com/dirtbikerxz/BaseTalonFXSwerve/. I thought at first it might be a odometry issue, but using Pathplanner’s Telemetry mode showed that the Robot was aware of how off-course it was. I’m unsure now as to what the problem would be. Here’s some hopefully relevant code.
In Swerve’s constructor:
swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions());
AutoBuilder.configureHolonomic(
this::getPose, // Robot pose supplier
this::setPose, // Method to reset odometry (will be called if your auto has a starting pose)
this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
(speeds) ->driveRobotRelative(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
Constants.Swerve.pathFollowerConfig,
() -> {
// Boolean supplier that controls when the path will be mirrored for the red alliance
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE
Optional<Alliance> alliance = DriverStation.getAlliance();
if (alliance.isPresent()) {
return alliance.get() == DriverStation.Alliance.Red;
}
return false;
},
this // Reference to this subsystem to set requirements
);
The setPose function:
public void setPose(Pose2d pose) {
swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), pose);
}
The driveRobotRelative and setModuleStates functions:
public void driveRobotRelative(ChassisSpeeds speeds) {
speeds = ChassisSpeeds.discretize(speeds, 0.02);
setModuleStates(Constants.Swerve.swerveKinematics.toSwerveModuleStates(speeds));
}
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed);
for(SwerveModule mod : mSwerveMods){
mod.setDesiredState(desiredStates[mod.moduleNumber], true);
}
}