fixed???????????????????????????

FRC-7525 · Nov 7, 2024 · da7a04a · da7a04a
1 parent bbf0c1d
commit da7a04a
Show file tree

Hide file tree

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