I pulled the code down, and the first thing I noticed is that you’re still using WPILib 2023. You should update ASAP. You have the 2024 version of Phoenix, and that won’t work with WPILib 2024. Besides, you’ll need to upgrade before competition, and the sooner you do it, the less code you’ll have to upgrade.
I see a couple things in the lines you’re asking about:
var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed.getX(), ySpeed.getY(), rot, m_gyro.getYaw())
: new ChassisSpeeds(xSpeed.getX(), ySpeed.getY(), rot));
First, you changed the variables that the code is using for X, Y, and rotation speed. I didn’t dive into the code, but I think it’s safe to change it back to using the same values as REV’s template.
Second, the gyro rotation argument needs to be a Rotation2d object. Pigeon2 has a getRotation2d() method, you used it other places.
Try this:
DriveConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
I also noticed the code in getHeading() doesn’t match the comment. Rotation returned from the Pigeon will be continuous, meaning after 180° it returns 181° not -179°.
/**
* Returns the heading of the robot.
*
* @return the robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
return m_gyro.getRotation2d().getDegrees();
}
You can solve this with this code:
public double getHeading() {
return Math.IEEEremainder(m_gyro.getRotation2d().getDegrees(), 360);
}