Stopoff on the way toward CTRE-centric swerve

	modified:   src/main/java/frc/robot/Constants.java
	modified:   src/main/java/frc/robot/RobotContainer.java
	modified:   src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java
	modified:   src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java
	modified:   src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java
	modified:   src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java
	new file:   src/main/java/frc/robot/generated/README
	modified:   src/main/java/frc/robot/generated/TunerConstants.java
	new file:   src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
	renamed:    src/main/java/frc/robot/subsystems/swervedrive_phoenix/CommandSwerveDrivetrain.java -> src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java
	renamed:    src/main/java/frc/robot/subsystems/swervedrive_yagsl/SwerveSubsystem.java -> src/main/java/frc/robot/subsystems/swervedrive/underlying/YAGSLSwerve.java
	deleted:    src/main/java/frc/robot/util/YAGSL_AzRBSI/CANCoderSwerve_RBSI.java
	deleted:    src/main/java/frc/robot/util/YAGSL_AzRBSI/DeviceJson_RBSI.java
	deleted:    src/main/java/frc/robot/util/YAGSL_AzRBSI/Pigeon2Swerve_RBSI.java
	deleted:    src/main/java/frc/robot/util/YAGSL_AzRBSI/README
	deleted:    src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveDriveJson_RBSI.java
	deleted:    src/main/java/frc/robot/util/YAGSL_AzRBSI/SwerveParser_RBSI.java
	deleted:    src/main/java/frc/robot/util/YAGSL_AzRBSI/TalonFXSwerve_RBSI.java

src/main/java/frc/robot/Constants.java

@@ -51,6 +51,8 @@ public final class Constants {
    */
   private static RobotType robotType = RobotType.SIMBOT;
 
+  private static SwerveType swerveType = SwerveType.PHOENIX6;
+
   public static boolean disableHAL = false;
 
   /** Enumerate the robot types (add additional bots here) */
@@ -98,6 +100,17 @@ public static void main(String... args) {
     }
   }
 
+  /** Enumerate the supported swerve drive types */
+  public static enum SwerveType {
+    YAGSL, // The generic YAGSL swerve library
+    PHOENIX6 // The all-CTRE Phoenix6 swerve library
+  }
+
+  /** Get the current swerve drive type */
+  public static SwerveType getSwerve() {
+    return swerveType;
+  }
+
   /***************************************************************************/
   /* The remainder of this file contains physical and/or software constants for the various subsystems of the robot */
 
@@ -106,7 +119,7 @@ public static void main(String... args) {
 
   public static final boolean tuningMode = false;
 
-  /** Physical Constants for Robot Operation ************ */
+  /** Physical Constants for Robot Operation ******************************* */
   public static final class PhysicalConstants {
     public static final double kRobotMass = (148 - 20.3) * 0.453592; // 32lbs * kg per pound
     public static final Matter kChassis =

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

@@ -23,7 +23,6 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -36,13 +35,12 @@
 import frc.robot.subsystems.flywheel_example.Flywheel;
 import frc.robot.subsystems.flywheel_example.FlywheelIO;
 import frc.robot.subsystems.flywheel_example.FlywheelIOSim;
-import frc.robot.subsystems.swervedrive_yagsl.SwerveSubsystem;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOPhoton;
 import frc.robot.util.CanDeviceId;
 import frc.robot.util.OverrideSwitches;
-import java.io.File;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 public class RobotContainer {
@@ -78,7 +76,7 @@ public RobotContainer() {
       case REAL:
         // Real robot, instantiate hardware IO implementations
         // YAGSL drivebase, get config from deploy directory
-        m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve"));
+        m_drivebase = new SwerveSubsystem(Constants.getSwerve());
         m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX());
         m_vision =
             new Vision(
@@ -88,14 +86,14 @@ public RobotContainer() {
 
       case SIM:
         // Sim robot, instantiate physics sim IO implementations
-        m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve"));
+        m_drivebase = new SwerveSubsystem(Constants.getSwerve());
         m_flywheel = new Flywheel(new FlywheelIOSim());
         m_vision = new Vision(this::getAprilTagLayoutType);
         break;
 
       default:
         // Replayed robot, disable IO implementations
-        m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve"));
+        m_drivebase = new SwerveSubsystem(Constants.getSwerve());
         m_flywheel = new Flywheel(new FlywheelIO() {});
         m_vision = new Vision(this::getAprilTagLayoutType, new VisionIO() {}, new VisionIO() {});
         break;

src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java

@@ -24,18 +24,18 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.swervedrive_yagsl.SwerveSubsystem;
+import frc.robot.subsystems.swervedrive.underlying.YAGSLSwerve;
 
 /**
  * Auto Balance command using a simple PID controller. Created by Team 3512 <a
  * href="https://github.com/frc3512/Robot-2023/blob/main/src/main/java/frc3512/robot/commands/AutoBalance.java">...</a>
  */
 public class AutoBalanceCommand extends Command {
 
-  private final SwerveSubsystem swerveSubsystem;
+  private final YAGSLSwerve swerveSubsystem;
   private final PIDController controller;
 
-  public AutoBalanceCommand(SwerveSubsystem swerveSubsystem) {
+  public AutoBalanceCommand(YAGSLSwerve swerveSubsystem) {
     this.swerveSubsystem = swerveSubsystem;
     controller = new PIDController(1.0, 0.0, 0.0);
     controller.setTolerance(1);

src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java

@@ -25,7 +25,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.PhysicalConstants;
-import frc.robot.subsystems.swervedrive_yagsl.SwerveSubsystem;
+import frc.robot.subsystems.swervedrive.underlying.YAGSLSwerve;
 import java.util.List;
 import java.util.function.DoubleSupplier;
 import swervelib.SwerveController;
@@ -34,7 +34,7 @@
 /** An example command that uses an example subsystem. */
 public class AbsoluteDrive extends Command {
 
-  private final SwerveSubsystem swerve;
+  private final YAGSLSwerve swerve;
   private final DoubleSupplier vX, vY;
   private final DoubleSupplier headingHorizontal, headingVertical;
   private boolean initRotation = false;
@@ -60,7 +60,7 @@ public class AbsoluteDrive extends Command {
    *     range from -1 to 1 with no deadband. Positive is away from the alliance wall.
    */
   public AbsoluteDrive(
-      SwerveSubsystem swerve,
+      YAGSLSwerve swerve,
       DoubleSupplier vX,
       DoubleSupplier vY,
       DoubleSupplier headingHorizontal,

src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java

@@ -26,7 +26,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.Constants.PhysicalConstants;
-import frc.robot.subsystems.swervedrive_yagsl.SwerveSubsystem;
+import frc.robot.subsystems.swervedrive.underlying.YAGSLSwerve;
 import java.util.List;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
@@ -36,7 +36,7 @@
 /** A more advanced Swerve Control System that has 4 buttons for which direction to face */
 public class AbsoluteDriveAdv extends Command {
 
-  private final SwerveSubsystem swerve;
+  private final YAGSLSwerve swerve;
   private final DoubleSupplier vX, vY;
   private final DoubleSupplier headingAdjust;
   private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight;
@@ -64,7 +64,7 @@ public class AbsoluteDriveAdv extends Command {
    * @param lookRight Face the robot right
    */
   public AbsoluteDriveAdv(
-      SwerveSubsystem swerve,
+      YAGSLSwerve swerve,
       DoubleSupplier vX,
       DoubleSupplier vY,
       DoubleSupplier headingAdjust,

src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java

@@ -25,7 +25,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.PhysicalConstants;
-import frc.robot.subsystems.swervedrive_yagsl.SwerveSubsystem;
+import frc.robot.subsystems.swervedrive.underlying.YAGSLSwerve;
 import java.util.List;
 import java.util.function.DoubleSupplier;
 import swervelib.SwerveController;
@@ -34,7 +34,7 @@
 /** An example command that uses an example subsystem. */
 public class AbsoluteFieldDrive extends Command {
 
-  private final SwerveSubsystem swerve;
+  private final YAGSLSwerve swerve;
   private final DoubleSupplier vX, vY, heading;
 
   /**
@@ -52,7 +52,7 @@ public class AbsoluteFieldDrive extends Command {
    * @param heading DoubleSupplier that supplies the robot's heading angle.
    */
   public AbsoluteFieldDrive(
-      SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) {
+      YAGSLSwerve swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) {
     this.swerve = swerve;
     this.vX = vX;
     this.vY = vY;

src/main/java/frc/robot/generated/README

@@ -0,0 +1,2 @@
+This directory, and in particular the TunerConstants.java file, are the
+Phoenix6 equivalent of the ``src/main/deploy/swerve`` directory for YAGSL.

src/main/java/frc/robot/generated/TunerConstants.java

@@ -11,7 +11,7 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory;
 import edu.wpi.first.math.util.Units;
-import frc.robot.subsystems.swervedrive_phoenix.CommandSwerveDrivetrain;
+import frc.robot.subsystems.swervedrive.underlying.Phoenix6Swerve;
 
 // Generated by the Tuner X Swerve Project Generator
 // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
@@ -182,6 +182,6 @@ public class TunerConstants {
           Units.inchesToMeters(kBackRightYPosInches),
           kInvertRightSide);
 
-  public static final CommandSwerveDrivetrain DriveTrain =
-      new CommandSwerveDrivetrain(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
+  public static final Phoenix6Swerve DriveTrain =
+      new Phoenix6Swerve(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
 }

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -0,0 +1,55 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.swervedrive;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.robot.Constants.SwerveType;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.swervedrive.underlying.Phoenix6Swerve;
+import frc.robot.subsystems.swervedrive.underlying.YAGSLSwerve;
+import java.io.File;
+
+/**
+ * Az-RBSI swerve class that generalizes the swerve subsystem to allow for the use of either
+ * Phoenix6 or YAGSL underlying libraries
+ */
+public class SwerveSubsystem extends Phoenix6Swerve {
+
+  private YAGSLSwerve y_swerve;
+
+  public SwerveSubsystem(Enum<SwerveType> swerveType) {
+
+    if (swerveType == SwerveType.YAGSL) {
+
+      // Load up the YASGL things into this class
+      y_swerve = new YAGSLSwerve(new File(Filesystem.getDeployDirectory(), "swerve"));
+
+    } else if (swerveType == SwerveType.PHOENIX6) {
+
+      // Load up the Phoenix6 things into this class
+      p_swerve = TunerConstants.DriveTrain;
+
+    } else {
+
+      // Raise an error because something unknown was passed
+
+    }
+  }
+
+  //
+}

src/main/java/frc/robot/subsystems/swervedrive_phoenix/CommandSwerveDrivetrain.java → src/main/java/frc/robot/subsystems/swervedrive/underlying/Phoenix6Swerve.java

@@ -18,7 +18,7 @@
 // NOTE: This module based on the CTRE Phoenix6 examples
 //       https://github.com/CrossTheRoadElec/Phoenix6-Examples
 
-package frc.robot.subsystems.swervedrive_phoenix;
+package frc.robot.subsystems.swervedrive.underlying;
 
 import static edu.wpi.first.units.Units.Volts;
 
@@ -49,7 +49,7 @@
  * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used
  * in command-based projects easily.
  */
-public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem {
+public class Phoenix6Swerve extends SwerveDrivetrain implements Subsystem {
   private static final double kSimLoopPeriod = 0.005; // 5 ms
   private Notifier m_simNotifier = null;
   private double m_lastSimTime;
@@ -104,7 +104,7 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst
   /* Change this to the sysid routine you want to test */
   private final SysIdRoutine RoutineToApply = SysIdRoutineTranslation;
 
-  public CommandSwerveDrivetrain(
+  public Phoenix6Swerve(
       SwerveDrivetrainConstants driveTrainConstants,
       double OdometryUpdateFrequency,
       SwerveModuleConstants... modules) {
@@ -115,7 +115,7 @@ public CommandSwerveDrivetrain(
     }
   }
 
-  public CommandSwerveDrivetrain(
+  public Phoenix6Swerve(
       SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) {
     super(driveTrainConstants, modules);
     configurePathPlanner();

src/main/java/frc/robot/subsystems/swervedrive_yagsl/SwerveSubsystem.java → src/main/java/frc/robot/subsystems/swervedrive/underlying/YAGSLSwerve.java

@@ -17,7 +17,7 @@
 //
 // NOTE: This module based on the YAGSL Example Project
 
-package frc.robot.subsystems.swervedrive_yagsl;
+package frc.robot.subsystems.swervedrive.underlying;
 
 import static frc.robot.Constants.DrivebaseConstants.*;
 
@@ -43,7 +43,6 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import frc.robot.Constants.AutonConstants;
 import frc.robot.subsystems.vision.Vision;
-import frc.robot.util.YAGSL_AzRBSI.SwerveParser_RBSI;
 import java.io.File;
 import java.util.Arrays;
 import java.util.function.DoubleSupplier;
@@ -57,11 +56,12 @@
 import swervelib.math.SwerveMath;
 import swervelib.parser.SwerveControllerConfiguration;
 import swervelib.parser.SwerveDriveConfiguration;
+import swervelib.parser.SwerveParser;
 import swervelib.telemetry.SwerveDriveTelemetry;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 
-/** SwerveSubsystem module */
-public class SwerveSubsystem extends SubsystemBase {
+/** YAGSLSwerve module */
+public class YAGSLSwerve extends SubsystemBase {
 
   /** Swerve drive object */
   private final SwerveDrive swerveDrive;
@@ -77,7 +77,7 @@ public class SwerveSubsystem extends SubsystemBase {
    *
    * @param directory Directory of swerve drive config files.
    */
-  public SwerveSubsystem(File directory) {
+  public YAGSLSwerve(File directory) {
 
     // Angle conversion factor is 360 / (GEAR RATIO * ENCODER RESOLUTION)
     double angleConversionFactor = SwerveMath.calculateDegreesPerSteeringRotation(TURN_GEAR_RATIO);
@@ -99,7 +99,7 @@ public SwerveSubsystem(File directory) {
     // created.
     SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
     try {
-      swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(MAX_LINEAR_SPEED);
+      swerveDrive = new SwerveParser(directory).createSwerveDrive(MAX_LINEAR_SPEED);
       // Alternative method if you don't want to supply the conversion factor via JSON files.
       // swerveDrive = new SwerveParser_RBSI(directory).createSwerveDrive(maximumSpeed,
       // angleConversionFactor, driveConversionFactor);
@@ -140,7 +140,7 @@ public SwerveSubsystem(File directory) {
    * @param driveCfg SwerveDriveConfiguration for the swerve.
    * @param controllerCfg Swerve Controller.
    */
-  public SwerveSubsystem(
+  public YAGSLSwerve(
       SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) {
     swerveDrive = new SwerveDrive(driveCfg, controllerCfg, MAX_LINEAR_SPEED);
   }