refactor: Add comments. Move TalonFX base configurations to superclass.

src/main/java/frc/robot/RobotContainer.java

@@ -1,8 +1,6 @@
 package frc.robot;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.lib.Telemetry;
 import frc.robot.arm.Arm;
@@ -16,6 +14,7 @@
 import frc.robot.swerve.Swerve;
 import frc.robot.vision.Vision;
 
+/** Initializes subsystems and commands. */
 public class RobotContainer {
 
   /** Instance variable for the robot container singleton. */
@@ -63,11 +62,7 @@ private void initializeTelemetry() {
   private void configureBindings() {
     swerve.setDefaultCommand(new Drive(driverController));
 
-    driverController.a().whileTrue(swerve.forwards());
-    driverController.b().whileTrue(swerve.sideways());
-    driverController
-        .y()
-        .onTrue(Commands.runOnce(() -> odometry.setRotation(Rotation2d.fromDegrees(0))));
+    driverController.y().onTrue(odometry.tare());
     driverController.x().whileTrue(swerve.cross());
 
     operatorController.leftBumper().whileTrue(intake.intake()).whileTrue(shooter.intake());

src/main/java/frc/robot/odometry/Odometry.java

@@ -8,6 +8,8 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
 import frc.robot.odometry.GyroscopeIO.GyroscopeIOValues;
@@ -127,6 +129,15 @@ public void setRotation(Rotation2d rotation) {
     setPosition(new Pose2d(position.getTranslation(), rotation));
   }
 
+  /**
+   * Tares the rotation of the robot.
+   *
+   * @return a command that zeroes the rotation of the robot.
+   */
+  public Command tare() {
+    return Commands.runOnce(() -> setRotation(Rotation2d.fromDegrees(0)));
+  }
+
   /**
    * Gets the velocity of the robot on the field.
    *

src/main/java/frc/robot/swerve/DriveMotorIOTalonFX.java

@@ -2,9 +2,12 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import frc.lib.CAN;
+import frc.robot.swerve.SwerveConstants.DriveMotorConstants;
 import frc.robot.swerve.SwerveConstants.MK4iConstants;
 
 /** TalonFX drive motor. */
@@ -13,6 +16,9 @@ public abstract class DriveMotorIOTalonFX implements DriveMotorIO {
   /** Hardware TalonFX. */
   protected final TalonFX talonFX;
 
+  /** Hardware TalonFX base config. */
+  protected final TalonFXConfiguration talonFXBaseConfig;
+
   /** TalonFX's position and velocity status signals. */
   protected final StatusSignal<Double> positionRotations, velocityRotationsPerSecond;
 
@@ -23,6 +29,13 @@ public abstract class DriveMotorIOTalonFX implements DriveMotorIO {
    */
   public DriveMotorIOTalonFX(CAN driveMotorCAN) {
     talonFX = new TalonFX(driveMotorCAN.id(), driveMotorCAN.bus());
+    talonFXBaseConfig = new TalonFXConfiguration();
+
+    talonFXBaseConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+    talonFXBaseConfig.Feedback.SensorToMechanismRatio = MK4iConstants.DRIVE_GEARING;
+
+    talonFXBaseConfig.CurrentLimits = DriveMotorConstants.CURRENT_LIMITS.asCurrentLimitsConfigs();
 
     positionRotations = talonFX.getPosition();
     velocityRotationsPerSecond = talonFX.getVelocity();

src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java

@@ -1,6 +1,5 @@
 package frc.robot.swerve;
 
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.CoastOut;
 import com.ctre.phoenix6.controls.VoltageOut;
 import edu.wpi.first.math.controller.PIDController;
@@ -33,9 +32,7 @@ public DriveMotorIOTalonFXPID(CAN can) {
 
   @Override
   public void configure() {
-    TalonFXConfiguration config = SwerveFactory.createDriveMotorConfig();
-
-    Configurator.configureTalonFX(talonFX.getConfigurator(), config);
+    Configurator.configureTalonFX(talonFX.getConfigurator(), talonFXBaseConfig);
   }
 
   @Override

src/main/java/frc/robot/swerve/SteerMotorIOTalonFX.java

@@ -2,15 +2,21 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
 import frc.lib.CAN;
+import frc.lib.MotorCurrentLimits;
 
 /** TalonFX steer motor. */
 public abstract class SteerMotorIOTalonFX implements SteerMotorIO {
 
   /** Hardware TalonFX. */
   protected final TalonFX talonFX;
 
+  /** Hardware TalonFX base config. */
+  protected final TalonFXConfiguration talonFXBaseConfig;
+
   /** TalonFX"s position and velocity status signals. */
   protected final StatusSignal<Double> positionRotations, velocityRotationsPerSecond;
 
@@ -26,6 +32,15 @@ public abstract class SteerMotorIOTalonFX implements SteerMotorIO {
    */
   public SteerMotorIOTalonFX(CAN steerMotorCAN, CAN azimuthEncoderCAN) {
     talonFX = new TalonFX(steerMotorCAN.id(), steerMotorCAN.bus());
+    talonFXBaseConfig = new TalonFXConfiguration();
+
+    talonFXBaseConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+    talonFXBaseConfig.ClosedLoopGeneral.ContinuousWrap = true;
+
+    // TODO Copied from Nutrons 2023 release
+    talonFXBaseConfig.CurrentLimits =
+        new MotorCurrentLimits(0.0, 40.0, 3.0, 1.0).asCurrentLimitsConfigs();
 
     positionRotations = talonFX.getPosition();
     velocityRotationsPerSecond = talonFX.getVelocity();

src/main/java/frc/robot/swerve/SteerMotorIOTalonFXIntegrated.java

@@ -17,18 +17,23 @@ public SteerMotorIOTalonFXIntegrated(CAN steerMotorCAN, CAN azimuthEncoderCAN) {
 
   @Override
   public void configure() {
-    TalonFXConfiguration config = SwerveFactory.createSteerMotorConfig();
+    TalonFXConfiguration talonFXIntegratedConfig = new TalonFXConfiguration();
 
-    config.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING;
+    // TODO
+    talonFXIntegratedConfig.deserialize(talonFXBaseConfig.serialize());
 
-    config.Slot0 = SwerveConstants.STEER_PIDF_CONSTANTS.asSlot0Configs();
+    talonFXIntegratedConfig.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING;
+
+    talonFXIntegratedConfig.Slot0 = SwerveConstants.STEER_PIDF_CONSTANTS.asSlot0Configs();
 
     // TODO Copied from CTRE swerve library
     // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.123
-    config.MotionMagic.MotionMagicCruiseVelocity = 100.0 / MK4iConstants.STEER_GEARING;
-    config.MotionMagic.MotionMagicAcceleration = config.MotionMagic.MotionMagicCruiseVelocity * 10;
+    talonFXIntegratedConfig.MotionMagic.MotionMagicCruiseVelocity =
+        100.0 / MK4iConstants.STEER_GEARING;
+    talonFXIntegratedConfig.MotionMagic.MotionMagicAcceleration =
+        talonFXIntegratedConfig.MotionMagic.MotionMagicCruiseVelocity * 10;
 
-    Configurator.configureTalonFX(talonFX.getConfigurator(), config);
+    Configurator.configureTalonFX(talonFX.getConfigurator(), talonFXIntegratedConfig);
   }
 
   @Override

src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java

@@ -29,11 +29,14 @@ public SteerMotorIOTalonFXPIDF(CAN steerMotorCAN, CAN azimuthEncoderCAN) {
 
   @Override
   public void configure() {
-    TalonFXConfiguration config = SwerveFactory.createSteerMotorConfig();
+    TalonFXConfiguration talonFXPIDFConfig = new TalonFXConfiguration();
 
-    config.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING;
+    // TODO
+    talonFXPIDFConfig.deserialize(talonFXBaseConfig.serialize());
 
-    Configurator.configureTalonFX(talonFX.getConfigurator(), config);
+    talonFXPIDFConfig.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING;
+
+    Configurator.configureTalonFX(talonFX.getConfigurator(), talonFXPIDFConfig);
   }
 
   @Override

src/main/java/frc/robot/swerve/SwerveConstants.java

@@ -76,12 +76,16 @@ public static class MK4iConstants {
 
   /**
    * Calculates the maximum attainable open loop speed in meters per second.
-   * 
-   * @param rotorVelocityRotationsPerSecondAt12Volts the rotor velocity of the drive motor in rotations per second when the motor is supplied 12 volts.
+   *
+   * @param rotorVelocityRotationsPerSecondAt12Volts the rotor velocity of the drive motor in
+   *     rotations per second when the motor is supplied 12 volts.
    * @return the maximum attainable open loop speed in meters per second.
    */
-  private static double calculateMaximumAttainableSpeed(double rotorVelocityRotationsPerSecondAt12Volts) {
-    return rotorVelocityRotationsPerSecondAt12Volts / MK4iConstants.DRIVE_GEARING * MK4iConstants.WHEEL_CIRCUMFERENCE;
+  private static double calculateMaximumAttainableSpeed(
+      double rotorVelocityRotationsPerSecondAt12Volts) {
+    return rotorVelocityRotationsPerSecondAt12Volts
+        / MK4iConstants.DRIVE_GEARING
+        * MK4iConstants.WHEEL_CIRCUMFERENCE;
   }
 
   /** Maximum attainable speed in meters per second. */
@@ -92,10 +96,10 @@ private static double calculateMaximumAttainableSpeed(double rotorVelocityRotati
 
   /**
    * Calculates an acceleration using a ramp duration.
-   * 
-   * @param maximumSpeed the maximum speed.
+   *
+   * @param maximumSpeed the maximum speed in units per second.
    * @param rampDurationSeconds the desired duration to ramp from no speed to full speed.
-   * @return the acceleration.
+   * @return the acceleration in units per second per second.
    */
   public static double calculateAcceleration(double maximumSpeed, double rampDurationSeconds) {
     return maximumSpeed / rampDurationSeconds;
@@ -112,7 +116,8 @@ public static double calculateAcceleration(double maximumSpeed, double rampDurat
   public static final double MAXIMUM_ROTATION_SPEED = 0.5;
 
   /** Maximum acceleration in rotations per second per second. */
-  public static final double MAXIMUM_ROTATION_ACCELERATION = calculateAcceleration(MAXIMUM_ROTATION_SPEED, 0.1);
+  public static final double MAXIMUM_ROTATION_ACCELERATION =
+      calculateAcceleration(MAXIMUM_ROTATION_SPEED, 0.1);
 
   public static class DriveMotorConstants {
     /** If true, use open-loop control. */

src/main/java/frc/robot/swerve/SwerveFactory.java

@@ -1,12 +1,7 @@
 package frc.robot.swerve;
 
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.signals.InvertedValue;
-import frc.lib.MotorCurrentLimits;
 import frc.robot.Robot;
-import frc.robot.swerve.SwerveConstants.DriveMotorConstants;
-import frc.robot.swerve.SwerveConstants.MK4iConstants;
 
 /** Helper class for creating hardware for the swerve subsystem. */
 public class SwerveFactory {
@@ -57,25 +52,6 @@ public static SteerMotorIO createSteerMotor(SwerveModuleConfig config) {
     return new SteerMotorIOSim();
   }
 
-  /**
-   * Creates a steer motor configuration.
-   *
-   * @return a steer motor configuration.
-   */
-  public static TalonFXConfiguration createSteerMotorConfig() {
-    TalonFXConfiguration steerMotorConfig = new TalonFXConfiguration();
-
-    steerMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-
-    steerMotorConfig.ClosedLoopGeneral.ContinuousWrap = true;
-
-    // TODO Copied from Nutrons 2023 release
-    steerMotorConfig.CurrentLimits =
-        new MotorCurrentLimits(0.0, 40.0, 3.0, 1.0).asCurrentLimitsConfigs();
-
-    return steerMotorConfig;
-  }
-
   /**
    * Creates a drive motor.
    *
@@ -86,21 +62,4 @@ public static DriveMotorIO createDriveMotor(SwerveModuleConfig config) {
 
     return new DriveMotorIOSim();
   }
-
-  /**
-   * Creates a drive motor configuration.
-   *
-   * @return a drive motor configuration.
-   */
-  public static TalonFXConfiguration createDriveMotorConfig() {
-    TalonFXConfiguration driveMotorConfig = new TalonFXConfiguration();
-
-    driveMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-
-    driveMotorConfig.Feedback.SensorToMechanismRatio = MK4iConstants.DRIVE_GEARING;
-
-    driveMotorConfig.CurrentLimits = DriveMotorConstants.CURRENT_LIMITS.asCurrentLimitsConfigs();
-
-    return driveMotorConfig;
-  }
 }

src/main/java/frc/robot/vision/VisionPoseEstimatorIONull.java

@@ -1,22 +1,20 @@
 package frc.robot.vision;
 
-import java.util.Optional;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import java.util.Optional;
 
 /** Vision pose estimator that does nothing. */
 public class VisionPoseEstimatorIONull implements VisionPoseEstimatorIO {
 
-    @Override
-    public void configure() {}
+  @Override
+  public void configure() {}
 
-    @Override
-    public void setReferencePosition(Pose2d referencePosition) {}
+  @Override
+  public void setReferencePosition(Pose2d referencePosition) {}
 
-    @Override
-    public Optional<Pose3d> getPosition() {
-        return Optional.empty();
-    }
-
+  @Override
+  public Optional<Pose3d> getPosition() {
+    return Optional.empty();
+  }
 }