From 0e22f3a42be3572150ab4ef3a44ae9b8c8095aca Mon Sep 17 00:00:00 2001 From: Kevin Yu Date: Fri, 16 Dec 2022 10:53:23 -0800 Subject: [PATCH] Fixed offset for new encoder shaft --- src/main/java/frc/robot/Constants.java | 4 ++-- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 090f47f8..5a9352f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,13 +46,13 @@ public static final class SwerveConstants { public static final int brDrive = 1; public static final int brSteer = 12; - public static final double brOffsetRads = 5.09281621578; + public static final double brOffsetRads = 3.135456730438264; public static final Translation2d brPos = new Translation2d( Units.inchesToMeters(-13.1365), Units.inchesToMeters(-10.3865) ); } - + public static final class IntakeConstants { public static final int intakeTalonID = 6; public static final int intakeSolID = 4; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index a60a9f14..6ec48962 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -3,6 +3,7 @@ import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Nat; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -123,6 +124,8 @@ public void periodic() { bottomRightModule.getState() ); + System.out.println(MathUtil.angleModulus(bottomRightModule.getState().angle.getRadians())); + // If all commanded velocities are 0, the system is idle (drivers are not supplying input). boolean isIdle = states[0].speedMetersPerSecond == 0.0 && states[1].speedMetersPerSecond == 0.0