might be done

Frc5572 · Jan 7, 2024 · 3eed755 · 3eed755
1 parent f5290f0
commit 3eed755
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 8 deletions.
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java
@@ -7,6 +7,7 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.ControlType;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxPIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -26,6 +27,7 @@ public class SwerveModule {
     private CANSparkMax angleMotor;
     private TalonFX driveMotor;
     private CANCoder angleEncoder;
+    private RelativeEncoder angleEncoderBuiltIn;
     private double lastAngle;
     SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.driveKS,
         Constants.Swerve.driveKV, Constants.Swerve.driveKA);
@@ -54,7 +56,11 @@ public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants
         driveMotor = new TalonFX(constants.driveMotorID);
         configDriveMotor();
 
-
+        angleEncoderBuiltIn = angleMotor.getEncoder();
+        angleEncoderBuiltIn
+            .setPositionConversionFactor(Constants.Swerve.turningDegreesPerEncoderRevolution);
+        angleEncoderBuiltIn
+            .setVelocityConversionFactor(Constants.Swerve.turningDegreesPerEncoderRevolution / 60);
 
         lastAngle = getState().angle.getDegrees();
     }
@@ -107,7 +113,7 @@ private void resetToAbsolute() {
         // getCanCoder().getDegrees() - angleOffset, Constants.Swerve.angleGearRatio);
 
         double absolutePosition = getCanCoder().getDegrees() - angleOffset;
-        angleMotor.setSelectedSensorPosition(absolutePosition);
+        angleEncoderBuiltIn.setPosition(absolutePosition);
     }
 
     // /**
@@ -131,8 +137,7 @@ public SwerveModulePosition getPosition() {
         double position = Conversions.falconToMeters(driveMotor.getSelectedSensorPosition(),
             Constants.Swerve.driveGearRatio, Constants.Swerve.wheelCircumference);
 
-        Rotation2d angle = Rotation2d.fromDegrees(Conversions.falconToDegrees(
-            angleMotor.getSelectedSensorPosition(), Constants.Swerve.angleGearRatio));
+        Rotation2d angle = Rotation2d.fromDegrees(angleEncoderBuiltIn.getPosition());
         return new SwerveModulePosition(position, angle);
     }
 
@@ -191,7 +196,7 @@ public Rotation2d getCanCoder() {
     public SwerveModuleState getState() {
         double velocity = Conversions.falconToMPS(driveMotor.getSelectedSensorVelocity(),
             Constants.Swerve.wheelCircumference, Constants.Swerve.driveGearRatio);
-        Rotation2d angle = Rotation2d.fromDegrees(angleMotor.);
+        Rotation2d angle = Rotation2d.fromDegrees(angleEncoderBuiltIn.getPosition());
         return new SwerveModuleState(velocity, angle);
     }
 

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -72,7 +72,7 @@ public static final class Swerve {
         public static final double closedLoopRamp = 0.4;
 
         public static final double driveGearRatio = (8.14 / 1.0); // 8.14:1
-        public static final double angleGearRatio = (12.8 / 1.0); // 12.8:1
+        public static final double angleGearRatio = ((150 / 7) / 1.0); // 12.8:1
 
         public static final SwerveDriveKinematics swerveKinematics =
             new SwerveDriveKinematics(new Translation2d(wheelBase / 2.0, trackWidth / 2.0),
@@ -92,13 +92,14 @@ public static final class Swerve {
         public static final boolean driveEnableCurrentLimit = true;
 
         /* Angle Motor PID Values */
-        public static final double angleKP = 0.6;
+        public static final double angleKP = 0.0;
         public static final double angleKI = 0.0;
-        public static final double angleKD = 12.0;
+        public static final double angleKD = 0.0;
         public static final double angleKF = 0.0;
 
         public static final double angleMotorEncoderPositionPIDMinInput = 0;
         public static final double angleMotorEncoderPositionPIDMaxInput = 360;
+        public static final double turningDegreesPerEncoderRevolution = 360 / angleGearRatio;
 
         /* Drive Motor PID Values */
         public static final double driveKP = 0.10;