Kraken swerve based on Tyler's original work

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -2,6 +2,12 @@ package frc.robot.constants;
 
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 public class ModuleConstants {
@@ -20,52 +26,73 @@ public class ModuleConstants {
     public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
         / kDrivingMotorReduction;
 
-    public static final int kDriveMotorCurrentLimit = 40;
+    public static final double kDrivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+    public static final double kTurningFactor = 2 * Math.PI;
+    public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
+    
+    public static final double kDriveP = .04;
+    public static final double kDriveI = 0;
+    public static final double kDriveD = 0;
+    public static final double kDriveS = 0;
+    public static final double kDriveV = kDrivingVelocityFeedForward;
+    public static final double kDriveA = 0;
+    
+    public static final double kTurnP = 1;
+    public static final double kTurnI = 0;
+    public static final double kTurnD = 0;
+
+    public static final int kDriveMotorStatorCurrentLimit = 120;
     public static final int kTurnMotorCurrentLimit = 20;
 
+    public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
+
+    public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
+    public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
+
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM 
 
-    public static final SparkMaxConfig drivingConfig = new SparkMaxConfig();
     public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
 
-    static {
+    public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
-        // Use module constants to calculate conversion factors and feed forward gain.
+    public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
-        double drivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+    public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
-        double turningFactor = 2 * Math.PI;
+    public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
-        double drivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
 
-        drivingConfig
+    static {
-            .idleMode(IdleMode.kBrake)
+        kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
-            .smartCurrentLimit(kDriveMotorCurrentLimit);
+
-        drivingConfig.encoder
+        kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true;
-            .positionConversionFactor(drivingFactor) // meters
+        kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit;
-            .velocityConversionFactor(drivingFactor / 60.0); // meters per second
+
-        drivingConfig.closedLoop
+        kDriveMotorConfig.Inverted = kDriveInversionState;
-            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+        kDriveMotorConfig.NeutralMode = kDriveIdleMode;
-            // These are example gains you may need to them for your own robot!
+
-            .pid(0.04, 0, 0)
+        kDriveSlot0Config.kP = kDriveP;
-            .velocityFF(drivingVelocityFeedForward)
+        kDriveSlot0Config.kI = kDriveI;
-            .outputRange(-1, 1);
+        kDriveSlot0Config.kD = kDriveD;
+        kDriveSlot0Config.kS = kDriveS;
+        kDriveSlot0Config.kV = kDriveV;
+        kDriveSlot0Config.kA = kDriveA;
 
         turningConfig
-            .idleMode(IdleMode.kBrake)
+            .idleMode(kTurnIdleMode)
-            .smartCurrentLimit(20);
+            .smartCurrentLimit(kTurnMotorCurrentLimit);
         turningConfig.absoluteEncoder
             // Invert the turning encoder, since the output shaft rotates in the opposite
             // direction of the steering motor in the MAXSwerve Module.
             .inverted(true)
-            .positionConversionFactor(turningFactor) // radians
+            .positionConversionFactor(kTurningFactor) // radians
-            .velocityConversionFactor(turningFactor / 60.0); // radians per second
+            .velocityConversionFactor(kTurningFactor / 60.0); // radians per second
         turningConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             // These are example gains you may need to them for your own robot!
-            .pid(1, 0, 0)
+            .pid(kTurnP, kTurnI, kTurnD)
             .outputRange(-1, 1)
             // Enable PID wrap around for the turning motor. This will allow the PID
             // controller to go through 0 to get to the setpoint i.e. going from 350 degrees
             // to 10 degrees will go through 0 rather than the other direction which is a
             // longer route.
             .positionWrappingEnabled(true)
-            .positionWrappingInputRange(0, turningFactor);
+            .positionWrappingInputRange(0, kTurningFactor);
     }
 }

src/main/java/frc/robot/subsystems/MAXSwerveModule.java

@@ -15,21 +15,22 @@ import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.AbsoluteEncoder;
-import com.revrobotics.RelativeEncoder;
 
 import frc.robot.constants.ModuleConstants;
 
 public class MAXSwerveModule {
-  private final SparkMax m_drivingSpark;
+  private final TalonFX m_drive;
   private final SparkMax m_turningSpark;
 
-  private final RelativeEncoder m_drivingEncoder;
   private final AbsoluteEncoder m_turningEncoder;
 
-  private final SparkClosedLoopController m_drivingClosedLoopController;
   private final SparkClosedLoopController m_turningClosedLoopController;
 
+  private final VelocityVoltage driveVelocityRequest;
+
   private double m_chassisAngularOffset = 0;
   private SwerveModuleState m_desiredState = new SwerveModuleState(0.0, new Rotation2d());
 
@@ -40,26 +41,29 @@ public class MAXSwerveModule {
    * Encoder.
    */
   public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) {
-    m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless);
+    m_drive = new TalonFX(drivingCANId);
     m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless);
 
-    m_drivingEncoder = m_drivingSpark.getEncoder();
     m_turningEncoder = m_turningSpark.getAbsoluteEncoder();
 
-    m_drivingClosedLoopController = m_drivingSpark.getClosedLoopController();
     m_turningClosedLoopController = m_turningSpark.getClosedLoopController();
 
+    driveVelocityRequest = new VelocityVoltage(0).withSlot(0);
+
     // Apply the respective configurations to the SPARKS. Reset parameters before
     // applying the configuration to bring the SPARK to a known good state. Persist
     // the settings to the SPARK to avoid losing them on a power cycle.
-    m_drivingSpark.configure(ModuleConstants.drivingConfig, ResetMode.kResetSafeParameters,
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
-        PersistMode.kPersistParameters);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
+    m_drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+
     m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
         PersistMode.kPersistParameters);
 
     m_chassisAngularOffset = chassisAngularOffset;
     m_desiredState.angle = new Rotation2d(m_turningEncoder.getPosition());
-    m_drivingEncoder.setPosition(0);
+    m_drive.setPosition(0);
   }
 
   /**
@@ -70,7 +74,7 @@ public class MAXSwerveModule {
   public SwerveModuleState getState() {
     // Apply chassis angular offset to the encoder position to get the position
     // relative to the chassis.
-    return new SwerveModuleState(m_drivingEncoder.getVelocity(),
+    return new SwerveModuleState(m_drive.getVelocity().getValueAsDouble(),
         new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
   }
 
@@ -82,8 +86,7 @@ public class MAXSwerveModule {
   public SwerveModulePosition getPosition() {
     // Apply chassis angular offset to the encoder position to get the position
     // relative to the chassis.
-    return new SwerveModulePosition(
+    return new SwerveModulePosition(m_drive.getPosition().getValueAsDouble(),
-        m_drivingEncoder.getPosition(),
         new Rotation2d(m_turningEncoder.getPosition() - m_chassisAngularOffset));
   }
 
@@ -102,14 +105,21 @@ public class MAXSwerveModule {
     correctedDesiredState.optimize(new Rotation2d(m_turningEncoder.getPosition()));
 
     // Command driving and turning SPARKS towards their respective setpoints.
-    m_drivingClosedLoopController.setReference(correctedDesiredState.speedMetersPerSecond, ControlType.kVelocity);
+    m_drive.setControl(
+      driveVelocityRequest.withVelocity(
+        correctedDesiredState.speedMetersPerSecond
+      ).withFeedForward(
+        correctedDesiredState.speedMetersPerSecond
+      )
+    );
+
     m_turningClosedLoopController.setReference(correctedDesiredState.angle.getRadians(), ControlType.kPosition);
 
     m_desiredState = desiredState;
   }
 
   public void setVoltageDrive(double voltage){
-    m_drivingSpark.setVoltage(voltage);
+    m_drive.setVoltage(voltage);
   }
 
   public void setVoltageTurn(double voltage) {
@@ -117,7 +127,7 @@ public class MAXSwerveModule {
   }
 
   public double getVoltageDrive() {
-    return m_drivingSpark.get() * RobotController.getBatteryVoltage();
+    return m_drive.get() * RobotController.getBatteryVoltage();
   }
 
   public double getVoltageTurn() {
@@ -126,6 +136,6 @@ public class MAXSwerveModule {
 
   /** Zeroes all the SwerveModule encoders. */
   public void resetEncoders() {
-    m_drivingEncoder.setPosition(0);
+    m_drive.setPosition(0);
   }
 }

src/main/java/frc/robot/subsystems/sysid/ArmSysID.java

@@ -4,11 +4,8 @@ import static edu.wpi.first.units.Units.Volts;
 import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 
-import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.MutAngle;
 import edu.wpi.first.units.measure.MutAngularVelocity;
-import edu.wpi.first.units.measure.MutDistance;
-import edu.wpi.first.units.measure.MutLinearVelocity;
 import edu.wpi.first.units.measure.MutVoltage;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;