diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index d5ec2df..fab3f5f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -3,7 +3,6 @@ import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -26,8 +25,7 @@ public class Swerve extends SubsystemBase { public SwerveDriveOdometry swerveOdometry; public SwerveModule[] swerveMods; - public AHRS gyro = new AHRS(Constants.Swerve.navXID); - private double fieldOffset = gyro.getYaw(); + private double fieldOffset; private SwerveInputsAutoLogged inputs = new SwerveInputsAutoLogged(); private SwerveIO swerveIO; private boolean hasInitialized = false; @@ -38,6 +36,7 @@ public class Swerve extends SubsystemBase { public Swerve(SwerveIO swerveIO) { this.swerveIO = swerveIO; swerveIO.updateInputs(inputs); + fieldOffset = inputs.yaw; swerveMods = new SwerveModule[] { swerveIO.createSwerveModule(0, Constants.Swerve.Mod0.DRIVE_MOTOR_ID, Constants.Swerve.Mod0.ANGLE_MOTOR_ID, Constants.Swerve.Mod0.CAN_CODER_ID, @@ -173,7 +172,7 @@ public Rotation2d getHeading() { * @return Current rotation/yaw of gyro as {@link Rotation2d} */ public Rotation2d getGyroYaw() { - float yaw = gyro.getYaw(); + float yaw = inputs.yaw; return (Constants.Swerve.INVERT_GYRO) ? Rotation2d.fromDegrees(-yaw) : Rotation2d.fromDegrees(yaw); }