diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 30a8d7e0..27c5a743 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.wpilibj.ADIS16470_IMU; +import frc.robot.Constants; import frc.robot.Constants.DriveConstants; import frc.utils.SwerveUtils; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -52,10 +53,11 @@ public class DriveSubsystem extends SubsystemBase { private SlewRateLimiter m_rotLimiter = new SlewRateLimiter(DriveConstants.kRotationalSlewRate); private double m_prevTime = WPIUtilJNI.now() * 1e-6; + // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = new SwerveDriveOdometry( DriveConstants.kDriveKinematics, - Rotation2d.fromDegrees(m_gyro.getAngle()), + Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? -1.0 : 1.0)), new SwerveModulePosition[] { m_frontLeft.getPosition(), m_frontRight.getPosition(), @@ -71,7 +73,7 @@ public DriveSubsystem() { public void periodic() { // Update the odometry in the periodic block m_odometry.update( - Rotation2d.fromDegrees(m_gyro.getAngle()), + Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? -1.0 : 1.0)), new SwerveModulePosition[] { m_frontLeft.getPosition(), m_frontRight.getPosition(), @@ -96,7 +98,7 @@ public Pose2d getPose() { */ public void resetOdometry(Pose2d pose) { m_odometry.resetPosition( - Rotation2d.fromDegrees(m_gyro.getAngle()), + Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? -1.0 : 1.0)), new SwerveModulePosition[] { m_frontLeft.getPosition(), m_frontRight.getPosition(), @@ -176,7 +178,7 @@ else if (angleDif > 0.85*Math.PI) { var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(m_gyro.getAngle())) + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? -1.0 : 1.0))) : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond); @@ -229,7 +231,7 @@ public void zeroHeading() { * @return the robot's heading in degrees, from -180 to 180 */ public double getHeading() { - return Rotation2d.fromDegrees(m_gyro.getAngle()).getDegrees(); + return Rotation2d.fromDegrees(m_gyro.getAngle() * (DriveConstants.kGyroReversed ? -1.0 : 1.0)).getDegrees(); } /**