Fixed offset for new encoder shaft

grt192 · Dec 16, 2022 · 0e22f3a · 0e22f3a
1 parent 4afdf88
commit 0e22f3a
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git 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
@@ -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