From 487a94ac28063786b9ab8a7d5148828eab6e7684 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Fri, 24 Nov 2023 19:20:01 -0600 Subject: [PATCH 1/2] Fix Gyro Inversion for other Gyros --- .../java/frc/robot/subsystems/DriveSubsystem.java | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 30a8d7e0..f9f6a2b8 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; @@ -42,6 +43,7 @@ public class DriveSubsystem extends SubsystemBase { // The gyro sensor private final ADIS16470_IMU m_gyro = new ADIS16470_IMU(); + private double gyroInversion = 1; // Slew rate filter variables for controlling lateral acceleration private double m_currentRotation = 0.0; @@ -52,10 +54,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 +74,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 +99,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 +179,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 +232,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(); } /** From a9cda1b7b764e1695cfc7d052db9bbfe67571932 Mon Sep 17 00:00:00 2001 From: Technologyman00 Date: Fri, 24 Nov 2023 19:21:42 -0600 Subject: [PATCH 2/2] Removed Unused Variables --- src/main/java/frc/robot/subsystems/DriveSubsystem.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index f9f6a2b8..27c5a743 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -43,7 +43,6 @@ public class DriveSubsystem extends SubsystemBase { // The gyro sensor private final ADIS16470_IMU m_gyro = new ADIS16470_IMU(); - private double gyroInversion = 1; // Slew rate filter variables for controlling lateral acceleration private double m_currentRotation = 0.0;