Code builds and deploys; bugs to be fixed

Issues:
  * Power monitoring not working at all
  * CAN issues -- maybe Az-RBSI, maybe my drivebase
  * Add Constant value for whether PhotonVision is enabled

	modified:   build.gradle
	modified:   src/main/java/frc/robot/Constants.java
	modified:   src/main/java/frc/robot/Robot.java
	modified:   src/main/java/frc/robot/RobotContainer.java
	deleted:    src/main/java/frc/robot/commands/swervedrive/auto/YAGSL_AutoBalanceCommand.java
	deleted:    src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDrive.java
	deleted:    src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteDriveAdv.java
	deleted:    src/main/java/frc/robot/commands/swervedrive/drivebase/YAGSL_AbsoluteFieldDrive.java
	modified:   src/main/java/frc/robot/generated/TunerConstants.java
	modified:   src/main/java/frc/robot/subsystems/drive/Drive.java
	modified:   src/main/java/frc/robot/subsystems/drive/GyroIO.java
	modified:   src/main/java/frc/robot/subsystems/drive/GyroIONavX.java
	modified:   src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
	modified:   src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveSubsystem.java
	modified:   src/main/java/frc/robot/subsystems/swervedrive_ignore/SwerveTelemetry.java

build.gradle

@@ -135,7 +135,7 @@ wpi.java.configureTestTasks(test)
 
 // Configure string concat to always inline compile
 tasks.withType(JavaCompile) {
-    options.compilerArgs.add '-XDstringConcat=inline'
+    options.compilerArgs << '-XDstringConcat=inline' << '-Xlint:unchecked'
 }
 
 // Create version file

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

@@ -52,10 +52,9 @@ public final class Constants {
    * Define the various multiple robots that use this same code (e.g., COMPBOT, DEVBOT, SIMBOT,
    * etc.) and the operating modes of the code (REAL, SIM, or REPLAY)
    */
-  private static RobotType robotType = RobotType.SIMBOT;
+  private static RobotType robotType = RobotType.COMPBOT;
 
   private static SwerveType swerveType = SwerveType.PHOENIX6;
-
   private static AutoType autoType = AutoType.PATHPLANNER;
 
   public static boolean disableHAL = false;

src/main/java/frc/robot/Robot.java

@@ -144,8 +144,17 @@ public void disabledPeriodic() {
   @Override
   public void autonomousInit() {
     m_robotContainer.setMotorBrake(true);
-    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
+    switch (Constants.getAutoType()) {
+      case PATHPLANNER:
+        m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+        break;
+      case CHOREO:
+        m_autonomousCommand = m_robotContainer.getAutonomousCommandChoreo();
+        break;
+      default:
+        throw new RuntimeException(
+            "Incorrect AUTO type selected in Constants: " + Constants.getAutoType());
+    }
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();

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

@@ -65,8 +65,9 @@ public class RobotContainer {
   final CommandXboxController operatorXbox = new CommandXboxController(1);
   final OverrideSwitches overrides = new OverrideSwitches(2);
 
+  // Autonomous Things
   Field2d m_field = new Field2d();
-  ChoreoTrajectory traj;
+  ChoreoTrajectory m_traj;
 
   // Declare the robot subsystems here
   private final Drive m_drivebase;
@@ -95,10 +96,6 @@ public static AprilTagLayoutType staticGetAprilTagLayoutType() {
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
-    traj = Choreo.getTrajectory("Trajectory");
-
-    m_field.getObject("traj").setPoses(traj.getInitialPose(), traj.getFinalPose());
-    m_field.getObject("trajPoses").setPoses(traj.getPoses());
 
     // Instantiate Robot Subsystems based on RobotType
     switch (Constants.getMode()) {
@@ -136,14 +133,15 @@ public RobotContainer() {
     //   as that is automatically monitored.
     m_power = new PowerMonitoring(m_flywheel);
 
+    // Set up the SmartDashboard Auto Chooser
+    autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
+
     // Configure the trigger bindings
     configureBindings();
     // Define Auto commands
     defineAutoCommands();
     // Define SysIs Routines
     definesysIdRoutines();
-    // Set up the SmartDashboard Auto Chooser
-    autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
   }
 
   /** Use this method to define your Autonomous commands for use with PathPlanner / Choreo */
@@ -250,10 +248,16 @@ public Command getAutonomousCommand() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommandChoreo() {
+
+    m_traj = Choreo.getTrajectory("Trajectory");
+
+    m_field.getObject("traj").setPoses(m_traj.getInitialPose(), m_traj.getFinalPose());
+    m_field.getObject("trajPoses").setPoses(m_traj.getPoses());
+
     var thetaController = new PIDController(AutoConstants.kPThetaController, 0, 0);
     thetaController.enableContinuousInput(-Math.PI, Math.PI);
 
-    m_drivebase.setPose(traj.getInitialPose());
+    m_drivebase.setPose(m_traj.getInitialPose());
 
     boolean isFlipped =
         DriverStation.getAlliance().isPresent()
@@ -262,7 +266,7 @@ public Command getAutonomousCommandChoreo() {
     Command swerveCommand =
         Choreo.choreoSwerveCommand(
             // Choreo trajectory from above
-            traj,
+            m_traj,
             // A function that returns the current field-relative pose of the robot: your wheel or
             // vision odometry
             m_drivebase::getPose,
@@ -288,7 +292,7 @@ public Command getAutonomousCommandChoreo() {
             m_drivebase);
 
     return Commands.sequence(
-        Commands.runOnce(() -> m_drivebase.setPose(traj.getInitialPose())),
+        Commands.runOnce(() -> m_drivebase.setPose(m_traj.getInitialPose())),
         swerveCommand,
         m_drivebase.run(() -> m_drivebase.stop()));
   }