From da7a04af85e5fc89feac00cb3277881c25fec154 Mon Sep 17 00:00:00 2001
From: GreenTomato5 <grentomato@gmail.com>
Date: Wed, 6 Nov 2024 18:46:51 -0600
Subject: [PATCH] fixed???????????????????????????

---
 src/main/java/frc/robot/Constants.java                    | 8 ++++----
 .../frc/robot/pioneersLib/bumSwerve/SwerveModule.java     | 8 ++++----
 .../bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java       | 4 ++--
 3 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 2ceed04..a7a2816 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -174,22 +174,22 @@ public static final class Real {
                     new SwerveModule(
                             new SwerveMotorIOTalonFX(59, CAN_BUS, DRIVE_BASE.driveGearing),
                             new SwerveMotorIOTalonFX(8, CAN_BUS, DRIVE_BASE.azimuthGearing),
-                            new SwerveAbsoluteEncoderIOCANcoder(3, 50), 
+                            new SwerveAbsoluteEncoderIOCANcoder(3, 170.156), 
                             "FrontLeft"),
                     new SwerveModule(
                             new SwerveMotorIOTalonFX(56, CAN_BUS, DRIVE_BASE.driveGearing),
                             new SwerveMotorIOTalonFX(3, CAN_BUS, DRIVE_BASE.azimuthGearing),
-                            new SwerveAbsoluteEncoderIOCANcoder(6, 160.313 + 90),
+                            new SwerveAbsoluteEncoderIOCANcoder(6, -18.721),
                             "FrontRight"),
                     new SwerveModule(
                             new SwerveMotorIOTalonFX(11, CAN_BUS, DRIVE_BASE.driveGearing),
                             new SwerveMotorIOTalonFX(2, CAN_BUS, DRIVE_BASE.azimuthGearing),
-                            new SwerveAbsoluteEncoderIOCANcoder(12, 42.012),
+                            new SwerveAbsoluteEncoderIOCANcoder(12, 42.715),
                             "BackLeft"),
                     new SwerveModule(
                             new SwerveMotorIOTalonFX(58, CAN_BUS, DRIVE_BASE.driveGearing),
                             new SwerveMotorIOTalonFX(5, CAN_BUS, DRIVE_BASE.azimuthGearing),
-                            new SwerveAbsoluteEncoderIOCANcoder(9, 289.512),
+                            new SwerveAbsoluteEncoderIOCANcoder(9, 106.084),
                             "BackRight")
             };
         }
diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java
index aa9b358..9b2b0a2 100644
--- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java
+++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveModule.java
@@ -108,6 +108,7 @@ public void antiJitter(
     public SwerveModuleState runState(SwerveModuleState state) {
         // Finds encoder offset that's used for odo calculations
         if (turnRelativeEncoderOffset == null && absoluteEncoder.getRotationDeg() != 0) {
+            turnMotor.setEncoderPosition(absoluteEncoder.getTurnAbsolutePosition().getDegrees());
             turnRelativeEncoderOffset =  absoluteEncoder.getTurnAbsolutePosition().minus(turnMotor.getAngle());
         }
 
@@ -121,7 +122,7 @@ public SwerveModuleState runState(SwerveModuleState state) {
 			lastModuleState = state;
 		}
 
-        //antiJitter(state, lastModuleState, SwerveDrive.maxSpeed);
+        antiJitter(state, lastModuleState, SwerveDrive.maxSpeed);
 
         // Prevents the turn motor from doing unneeded rotations
         var optimizedState = SwerveModuleState.optimize(state, getAngle());
@@ -139,8 +140,7 @@ public SwerveModuleState runState(SwerveModuleState state) {
     public void periodic() {
         // if (moduleName == "FrontLeft" && turnRelativeEncoderOffset != null) {System.out.println(turnRelativeEncoderOffset.getDegrees());}
         if (angleSetPoint != null) {
-            double adjustedAngleSetpoint = Units.radiansToDegrees(MathUtil.angleModulus(Units.degreesToRadians(angleSetPoint + (turnRelativeEncoderOffset != null ? turnRelativeEncoderOffset.getDegrees() : 0))));
-            turnMotor.setPosition(adjustedAngleSetpoint);
+            turnMotor.setPosition(angleSetPoint);
 
             if (speedSetPoint != null) {
                 driveMotor.setVelocity(speedSetPoint);
@@ -182,7 +182,7 @@ public void stop() {
      * @return the rotation of the wheel with 0 being your absolute encoder's 0
      */
     public Rotation2d getAngle() {
-        return turnRelativeEncoderOffset != null ? turnMotor.getAngle().plus(turnRelativeEncoderOffset) : turnMotor.getAngle();
+        return turnMotor.getAngle();
     }
 
     /**
diff --git a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java
index 87aba86..2f0f2a9 100644
--- a/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java
+++ b/src/main/java/frc/robot/pioneersLib/bumSwerve/SwerveMotor/SwerveMotorIOTalonFX.java
@@ -133,7 +133,7 @@ public void updateOutputs(SwerveMotorIOOutputs outputs) {
 
     @Override
     public Rotation2d getAngle() {
-        return Rotation2d.fromRadians(Units.rotationsToRadians(motorPosition.getValueAsDouble()) / gearRatio);
+        return Rotation2d.fromRotations(motorPosition.getValueAsDouble() / gearRatio);
     }
 
     @Override
@@ -143,7 +143,7 @@ public double getVelocity() {
 
     @Override
     public void setEncoderPosition(double positionDeg) {
-        motor.setPosition(positionDeg/360);
+        motor.setPosition((positionDeg/360) * gearRatio);
     }
 
     @Override