Use cancoder for angle controller

StuyPulse · Jan 26, 2024 · 619c3eb · 619c3eb
1 parent 490cf58
commit 619c3eb
Showing 1 changed file with 15 additions and 7 deletions.
diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveModule.java
@@ -8,6 +8,7 @@
 
 import static com.stuypulse.robot.constants.Settings.Swerve.*;
 
+import com.ctre.phoenix6.hardware.CANcoder;
 import com.stuypulse.stuylib.control.Controller;
 import com.stuypulse.stuylib.control.angle.AngleController;
 import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
@@ -31,6 +32,7 @@
 
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 
@@ -39,12 +41,13 @@ public class SwerveModule extends SubsystemBase {
     private final String id;
     private final Rotation2d angleOffset;
 
-    private final CANSparkMax driveMotor;
+    private final CANSparkFlex driveMotor;
     private final RelativeEncoder driveEncoder;
     private final Controller driveController;
 
     private final CANSparkMax turnMotor;
     private final RelativeEncoder turnEncoder;
+    private final CANcoder turnAbsoluteEncoder;
     private final AngleController turnController;
 
     private final SmartBoolean driveSysID;
@@ -57,7 +60,7 @@ public SwerveModule(String id, Rotation2d angleOffset, int turnID, int driveID,
         this.id = id;
         this.angleOffset = angleOffset;
 
-        driveMotor = new CANSparkMax(driveID, MotorType.kBrushless);
+        driveMotor = new CANSparkFlex(driveID, MotorType.kBrushless);
         driveMotor.setIdleMode(IdleMode.kBrake);
         driveEncoder = driveMotor.getEncoder();
         driveController =
@@ -68,6 +71,7 @@ public SwerveModule(String id, Rotation2d angleOffset, int turnID, int driveID,
         turnMotor = new CANSparkMax(turnID, MotorType.kBrushless);
         turnMotor.setIdleMode(IdleMode.kBrake);
         turnEncoder = turnMotor.getEncoder();
+        turnAbsoluteEncoder = new CANcoder(encoderID);
         turnController =
                 new AnglePIDController(Turn.kP, Turn.kI, Turn.kD)
                         .setSetpointFilter(new ARateLimit(MAX_MODULE_TURN))
@@ -114,7 +118,11 @@ public double getTurnVelocity() {
     }
 
     public Rotation2d getAngle() {
-        return Rotation2d.fromRotations(turnEncoder.getPosition()).minus(angleOffset);
+        return Rotation2d.fromRotations(turnEncoder.getPosition());
+    }
+
+    public Rotation2d getAbsoluteAngle() {
+        return Rotation2d.fromRotations(turnAbsoluteEncoder.getAbsolutePosition().getValueAsDouble()).minus(angleOffset);
     }
 
     public SwerveModulePosition getModulePosition() {
@@ -151,13 +159,13 @@ public void periodic() {
 
             if (driveSysID.get()) {
                 setTurnVoltage(
-                        turnController.update(Angle.kZero, Angle.fromRotation2d(getAngle())));
+                        turnController.update(Angle.kZero, Angle.fromRotation2d(getAbsoluteAngle())));
             } else if (turnSysID.get()) {
                 setDriveVoltage(driveController.update(0, getDriveVelocity()));
             }
 
         } else {
-            setTurnVoltage(turnController.update(Angle.kZero, Angle.fromRotation2d(getAngle())));
+            setTurnVoltage(turnController.update(Angle.kZero, Angle.fromRotation2d(getAbsoluteAngle())));
         }
 
         updateTelemetry();
@@ -169,10 +177,10 @@ private void updateTelemetry() {
                 "Swerve/Modules/" + id + "/Turn Voltage", turnController.getOutput());
         SmartDashboard.putNumber(
                 "Swerve/Modules/" + id + "/Angle Error", turnController.getError().toDegrees());
-        SmartDashboard.putNumber("Swerve/Modules/" + id + "/Angle", getAngle().getDegrees());
+        SmartDashboard.putNumber("Swerve/Modules/" + id + "/Angle", getAbsoluteAngle().getDegrees());
         SmartDashboard.putNumber("Swerve/Modules/" + id + "/Speed", getDriveVelocity());
         SmartDashboard.putNumber(
                 "Swerve/Modules/" + id + "/Raw Encoder Angle",
-                Units.rotationsToDegrees(turnEncoder.getPosition()));
+                Units.rotationsToDegrees(turnAbsoluteEncoder.getAbsolutePosition().getValueAsDouble()));
     }
 }