Janky working prototype, Most code removed, Using phoenix 6

.github/workflows/gradle.yml

@@ -13,10 +13,10 @@ jobs:
     runs-on: ubuntu-latest
     steps:
     - uses: actions/checkout@v1
-    - name: Set up JDK 1.8
+    - name: Set up JDK 17
       uses: actions/setup-java@v1
       with:
-        java-version: 1.12
+        java-version: 17
     - name: Change wrapper permissions
       run: chmod +x ./gradlew
     - name: Build with Gradle

.gitignore

@@ -170,3 +170,11 @@ out/
 
 # Simulation GUI and other tools window save file
 *-window.json
+
+# Simulation data log directory
+logs/
+
+# Folder that has CTRE Phoenix Sim device config storage
+ctre_sim/
+simgui.json
+simgui-ds.json

.wpilib/wpilib_preferences.json

@@ -1,6 +1,6 @@
 {
     "enableCppIntellisense": false,
     "currentLanguage": "java",
-    "projectYear": "2023",
+    "projectYear": "2024",
     "teamNumber": 4388
 }

WPILib-License.md

@@ -1,4 +1,4 @@
-Copyright (c) 2009-2021 FIRST and other WPILib contributors
+Copyright (c) 2009-2023 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

build.gradle

@@ -1,10 +1,12 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.4.3"
+    id "edu.wpi.first.GradleRIO" version "2024.3.1"
 }
 
-sourceCompatibility = JavaVersion.VERSION_11
+java {
-targetCompatibility = JavaVersion.VERSION_11
+    sourceCompatibility = JavaVersion.VERSION_17
+    targetCompatibility = JavaVersion.VERSION_17
+}
 
 def ROBOT_MAIN_CLASS = "frc4388.robot.Main"
 
@@ -65,15 +67,14 @@ dependencies {
     nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
     simulationRelease wpi.sim.enableRelease()
 
-    testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2'
+    //testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2'
+    //testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
-    testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2'
 }
 
-test {
+// test {
-    useJUnitPlatform()
+//     useJUnitPlatform()
-    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+//     systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
-}
+//}
 
 // Simulation configuration (e.g. environment variables).
 wpi.sim.addGui().defaultEnabled = true
@@ -84,6 +85,7 @@ wpi.sim.addDriverstation()
 // knows where to look for our Robot Class.
 jar {
     from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
+    from sourceSets.main.allSource
     manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
     duplicatesStrategy = DuplicatesStrategy.INCLUDE
 }

gradle/wrapper/gradle-wrapper.properties

@@ -1,5 +1,7 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=permwrapper/dists

gradlew

@@ -55,7 +55,7 @@
 #       Darwin, MinGW, and NonStop.
 #
 #   (3) This script is generated from the Groovy template
-#       https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+#       https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
 #       within the Gradle project.
 #
 #       You can find Gradle at https://github.com/gradle/gradle/.
@@ -80,13 +80,11 @@ do
     esac
 done
 
-APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit
+# This is normally unused
-
+# shellcheck disable=SC2034
-APP_NAME="Gradle"
 APP_BASE_NAME=${0##*/}
-
+# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
-# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit
-DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
 
 # Use the maximum available, or set MAX_FD != -1 to use that value.
 MAX_FD=maximum
@@ -133,22 +131,29 @@ location of your Java installation."
     fi
 else
     JAVACMD=java
-    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+    if ! command -v java >/dev/null 2>&1
+    then
+        die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
 
 Please set the JAVA_HOME variable in your environment to match the
 location of your Java installation."
     fi
+fi
 
 # Increase the maximum file descriptors if we can.
 if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
     case $MAX_FD in #(
       max*)
+        # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked.
+        # shellcheck disable=SC2039,SC3045
         MAX_FD=$( ulimit -H -n ) ||
             warn "Could not query maximum file descriptor limit"
     esac
     case $MAX_FD in  #(
       '' | soft) :;; #(
       *)
+        # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked.
+        # shellcheck disable=SC2039,SC3045
         ulimit -n "$MAX_FD" ||
             warn "Could not set maximum file descriptor limit to $MAX_FD"
     esac
@@ -193,11 +198,15 @@ if "$cygwin" || "$msys" ; then
     done
 fi
 
-# Collect all arguments for the java command;
+
-#   * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-#     shell script including quotes and variable substitutions, so put them in
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
-#     double quotes to make sure that they get re-expanded; and
+
-#   * put everything else in single quotes, so that it's not re-expanded.
+# Collect all arguments for the java command:
+#   * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+#     and any embedded shellness will be escaped.
+#   * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
+#     treated as '${Hostname}' itself on the command line.
 
 set -- \
         "-Dorg.gradle.appname=$APP_BASE_NAME" \

gradlew.bat

@@ -26,6 +26,7 @@ if "%OS%"=="Windows_NT" setlocal
 
 set DIRNAME=%~dp0
 if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
 set APP_BASE_NAME=%~n0
 set APP_HOME=%DIRNAME%
 

networktables.json

@@ -0,0 +1 @@
+[]

settings.gradle

@@ -4,7 +4,7 @@ pluginManagement {
     repositories {
         mavenLocal()
         gradlePluginPortal()
-        String frcYear = '2023'
+        String frcYear = '2024'
         File frcHome
         if (OperatingSystem.current().isWindows()) {
             String publicFolder = System.getenv('PUBLIC')
@@ -25,3 +25,6 @@ pluginManagement {
         }
     }
 }
+
+Properties props = System.getProperties();
+props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true");

src/main/java/frc4388/robot/Constants.java

@@ -7,10 +7,10 @@
 
 package frc4388.robot;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
-
-import frc4388.utility.LEDPatterns;
 import frc4388.utility.Gains;
+import frc4388.utility.LEDPatterns;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -23,40 +23,51 @@ import frc4388.utility.Gains;
 public final class Constants {
     public static final class SwerveDriveConstants {
 
-        public static final double MAX_ROT_SPEED        = 1.5;
+        public static final double MAX_ROT_SPEED        = 3.5;
-        public static final double MIN_ROT_SPEED        = 0.8;
+        public static final double AUTO_MAX_ROT_SPEED = 1.5;
+        public static final double MIN_ROT_SPEED        = 1.0;
         public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
+        public static       double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED;
         public static       double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
 
+        public static final String CANBUS_NAME = "IDK";
+    
         public static final double CORRECTION_MIN = 10;
         public static final double CORRECTION_MAX = 50;
     
-        public static final double SLOW_SPEED = 0.8;
+        public static final double SLOW_SPEED = 0.5;
         public static final double FAST_SPEED = 1.0;
         public static final double TURBO_SPEED = 4.0;
     
+        public static final class DefaultSwerveRotOffsets {
+          public static final double FRONT_LEFT_ROT_OFFSET = 220;
+          public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
+          public static final double BACK_LEFT_ROT_OFFSET = -279.08349884;
+          public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656;
+        }
+
         public static final class IDs {
-          public static final int LEFT_FRONT_WHEEL_ID = 2;
+          public static final int RIGHT_FRONT_WHEEL_ID = 0;
-          public static final int LEFT_FRONT_STEER_ID = 3;
+          public static final int RIGHT_FRONT_STEER_ID = 1;
-          public static final int LEFT_FRONT_ENCODER_ID = 10;
+          public static final int RIGHT_FRONT_ENCODER_ID = 12;
           
-          public static final int RIGHT_FRONT_WHEEL_ID = 4;
+        //   public static final int LEFT_FRONT_WHEEL_ID = 4;
-          public static final int RIGHT_FRONT_STEER_ID = 5;
+        //   public static final int LEFT_FRONT_STEER_ID = 5;
-          public static final int RIGHT_FRONT_ENCODER_ID = 11;
+        //   public static final int LEFT_FRONT_ENCODER_ID = 11;
     
-          public static final int LEFT_BACK_WHEEL_ID = 6;
+        //   public static final int LEFT_BACK_WHEEL_ID = 6;
-          public static final int LEFT_BACK_STEER_ID = 7;
+        //   public static final int LEFT_BACK_STEER_ID = 7;
-          public static final int LEFT_BACK_ENCODER_ID = 12;
+        //   public static final int LEFT_BACK_ENCODER_ID = 12;
           
-          public static final int RIGHT_BACK_WHEEL_ID = 8;
+        //   public static final int RIGHT_BACK_WHEEL_ID = 8;
-          public static final int RIGHT_BACK_STEER_ID = 9;
+        //   public static final int RIGHT_BACK_STEER_ID = 9;
-          public static final int RIGHT_BACK_ENCODER_ID = 13;
+        //   public static final int RIGHT_BACK_ENCODER_ID = 13;
         }
     
         public static final class PIDConstants {
             public static final int SWERVE_SLOT_IDX = 0;
             public static final int SWERVE_PID_LOOP_IDX = 1;
-          public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
+            public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
         }
     
         public static final class AutoConstants {
@@ -112,29 +123,40 @@ public final class Constants {
       }
     
     public static final class VisionConstants {
-        public static final String NAME = "photonCamera";
+        // public static final String NAME = "photonCamera";
     
-        public static final int LIME_HIXELS = 640;
+        // public static final int LIME_HIXELS = 640;
-        public static final int LIME_VIXELS = 480;
+        // public static final int LIME_VIXELS = 480;
     
-        public static final double H_FOV = 59.6;
+        // public static final double H_FOV = 59.6;
-        public static final double V_FOV = 45.7;
+        // public static final double V_FOV = 45.7;
     
-        public static final double LIME_HEIGHT = 6.0;
+        // public static final double LIME_HEIGHT = 6.0;
-        public static final double LIME_ANGLE = 55.0;
+        // public static final double LIME_ANGLE = 55.0;
     
-        // public static final double HIGH_TARGET_HEIGHT = 46.0;
+        // // public static final double HIGH_TARGET_HEIGHT = 46.0;
-        public static final double HIGH_TAPE_HEIGHT = 44.0;
+        // public static final double HIGH_TAPE_HEIGHT = 44.0;
         
-        // public static final double MID_TARGET_HEIGHT = 34.0;
+        // // public static final double MID_TARGET_HEIGHT = 34.0;
-        public static final double MID_TAPE_HEIGHT = 24.0;
+        // public static final double MID_TAPE_HEIGHT = 24.0;
     
-        public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
+        // public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
+
+        public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609);
+        public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593);
+
+        public static final double SpeakerBubbleDistance = 3;
+        public static final double targetPosDistance = 1.5;
     
     }
    
+    public static final class AutoAlignConstants {
+        public static final double MoveSpeed = 0.0;
+        public static final double RotSpeed = 0.0;
+    }
+
     public static final class DriveConstants {
-        public static final int DRIVE_PIGEON_ID = 6;
+        public static final int DRIVE_PIGEON_ID = 14;
 
         public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
     }
@@ -145,6 +167,34 @@ public final class Constants {
         public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
     }
 
+    public static final class ShooterConstants {
+        public static final int LEFT_SHOOTER_ID = 15; // final
+        public static final int RIGHT_SHOOTER_ID = 16; // final
+        public static final double SHOOTER_SPEED = 0.50; // final
+        public static final double SHOOTER_IDLE = 0.20; // final
+        public static final double SHOOTER_IDLE_LIMELIGHT = 0.20;
+    }
+
+    public static final class IntakeConstants {
+        public static final int INTAKE_MOTOR_ID = 18;
+        public static final int PIVOT_MOTOR_ID = 17;
+        public static final double INTAKE_SPEED = 0.75;
+        public static final double INTAKE_OUT_SPEED_UNPRESSED =  1.0;       
+        public static final double INTAKE_OUT_SPEED_PRESSED = 0.5;        
+        public static final double HANDOFF_SPEED = 0.4;
+        public static final double PIVOT_SPEED = 0.2;
+
+        public static final class ArmPID {
+            public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0);
+        }
+    }
+
+    public static final class ClimbConstants {
+        public static final int CLIMB_MOTOR_ID = 19;
+        public static final double CLIMB_IN_SPEED = -1.0;
+        public static final double CLIMB_OUT_SPEED = 0.87;
+    }
+
     public static final class OIConstants {
         public static final int XBOX_DRIVER_ID = 0;
         public static final int XBOX_OPERATOR_ID = 1;

src/main/java/frc4388/robot/Robot.java

@@ -7,12 +7,14 @@
 
 package frc4388.robot;
 
+import edu.wpi.first.cameraserver.CameraServer;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.utility.DeferredBlock;
 import frc4388.utility.RobotTime;
-
+//import frc4388.robot.subsystems.LED;
 /**
  * The VM is configured to automatically run this class, and to call the
  * functions corresponding to each mode, as described in the TimedRobot
@@ -25,13 +27,14 @@ public class Robot extends TimedRobot {
 
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
-
+  //private LED mled = new LED();
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
    */
   @Override
   public void robotInit() {
+
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
@@ -40,7 +43,7 @@ public class Robot extends TimedRobot {
   /**
    * This function is called every robot packet, no matter the mode. Use
    * this for items like diagnostics that you want ran during disabled,
-   * autonomous, teleoperated and test.
+   * autonomous, teleoperated and test.doubl
    *
    * <p>This runs after the mode specific periodic functions, but before
    * LiveWindow and SmartDashboard integrated updating.
@@ -48,6 +51,8 @@ public class Robot extends TimedRobot {
   @Override
   public void robotPeriodic() {   
     m_robotTime.updateTimes();
+    //System.out.println(m_robotContainer.limelight.isNearSpeaker());
+    //mled.updateLED();
     // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
     // commands, running already-scheduled commands, removing finished or interrupted commands,
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
@@ -119,7 +124,7 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void teleopPeriodic() {
-    
+   m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -7,20 +7,50 @@
 
 package frc4388.robot;
 
+// Drive Systems
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import frc4388.utility.controller.XboxController;
+import frc4388.utility.controller.DeadbandedXboxController;
+import frc4388.robot.Constants.OIConstants;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+// Commands
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc4388.robot.Constants.*;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+
+// Autos
+import frc4388.robot.commands.Autos.PlaybackChooser;
 import frc4388.robot.commands.Swerve.JoystickPlayback;
 import frc4388.robot.commands.Swerve.JoystickRecorder;
-import frc4388.robot.subsystems.LED;
+import frc4388.utility.controller.VirtualController;
+import frc4388.robot.commands.Swerve.neoJoystickPlayback;
+import frc4388.robot.commands.Swerve.neoJoystickRecorder;
+// import frc4388.robot.commands.Intake.ArmIntakeIn;
+//import frc4388.robot.commands.Autos.AutoAlign;
+
+// Subsystems
+// import frc4388.robot.subsystems.LED;
+// import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.LEDPatterns;
+// import frc4388.robot.subsystems.Shooter;
-import frc4388.utility.controller.DeadbandedXboxController;
+// import frc4388.robot.subsystems.Climber;
-import frc4388.utility.controller.IHandController;
+// import frc4388.robot.subsystems.Intake;
-import frc4388.utility.controller.XboxController;
+
+// Utilites
+import frc4388.utility.DeferredBlock;
+import frc4388.utility.configurable.ConfigurableString;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -31,34 +61,160 @@ import frc4388.utility.controller.XboxController;
  */
 public class RobotContainer {
     /* RobotMap */
-    private final RobotMap m_robotMap = new RobotMap();
+    public final RobotMap m_robotMap = new RobotMap();
     
     /* Subsystems */
-    private final LED m_robotLED = new LED(m_robotMap.LEDController);
+    // private final LED m_robotLED = new LED();
 
-    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
+    // private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor);
-                                                                  m_robotMap.rightFront,
-                                                                  m_robotMap.leftBack,
-                                                                  m_robotMap.rightBack,
-                                                                  m_robotMap.gyro);
 
+    // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
+    //                                                               m_robotMap.rightFront,
+    //                                                               m_robotMap.leftBack,
+    //                                                               m_robotMap.rightBack,
+                                              
+    //                                                               m_robotMap.gyro);
+    /* Limelight */
+    // private final Limelight limelight = new Limelight();
+
+    // private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight);
+
+    // private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor);
 
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox   = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
     private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);    
+    // private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2);
+    
+
+    /* Virtual Controllers */
+    // private final VirtualController m_virtualDriver = new VirtualController(0);
+    // private final VirtualController m_virtualOperator = new VirtualController(1);
+
+    // private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
+    // private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
+
+    // private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
+    //     new InstantCommand(() -> m_robotIntake.PIDIn()),
+    //     new InstantCommand(() -> m_robotShooter.idle())
+    //     // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))),
+    //     // new InstantCommand(() -> m_robotShooter.spin())
+    // );
+
+
+    // ! Teleop Commands
+
+    //private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
+
+    // private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
+    //     // MoveToSpeaker,
+    //     //autoAlign,
+    //     new InstantCommand(() -> m_robotShooter.spin()),
+    //     new WaitCommand(3.0),
+    //     new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake),
+    //     new WaitCommand(3.0),
+    //     new InstantCommand(() -> m_robotShooter.idle())
+    //     // new InstantCommand(() -> autoAlign.reverse()),
+    //    // autoAlign.asProxy()
+    // );
+
+
+    // private SequentialCommandGroup i = new SequentialCommandGroup(
+    //     intakeToShootStuff, intakeToShoot, 
+    //     new InstantCommand(() -> m_robotShooter.idle())
+    // );
+
+    // private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup(
+    //     new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
+    //     new WaitCommand(0.75),
+    //     new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake)
+    // );
+
+    // private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
+    //     new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)
+    //     // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
+    // );
+
+    // private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
+    //     interrupt,
+    //     new InstantCommand(() -> m_robotIntake.PIDIn(), m_robotIntake),
+    //     new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)
+    // );
+
+    // private SequentialCommandGroup ampShoot = new SequentialCommandGroup(
+    //     new InstantCommand(() -> m_robotIntake.ampPosition()),
+    //     new InstantCommand(() -> m_robotIntake.ampOuttake(0.1)) //TODO: Find Actual Speed
+    // );
+
+    // ! /*  Autos */
+    // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
+    // private String lastAutoName = "final_red_center_4note_taxi.auto";
+    // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
+    // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, 
+    // () -> autoplaybackName.get(), // lastAutoName
+    //        new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
+    //        true, false);
+
+
+    // private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
+    //         .addOption("Taxi Auto", taxi.asProxy())
+    //         .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy())
+    //         .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy())
+    //         // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy())
+    //         // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy())
+    //         .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy())
+    //         .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy())
+    //         .buildDisplay();
+    
+    
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
         configureButtonBindings();        
+        configureVirtualButtonBindings();
+        // new Trigger(() -> autoplaybackName.get().equals(lastAutoName)).onTrue(new InstantCommand(() -> changeAuto()));
+        // new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
 
-        /* Default Commands */
+
-        // drives the robot with a two-axis input from the driver controller
+        //     DriverStation.silenceJoystickConnectionWarning(true);
+        //     // CameraServer.startAutomaticCapture();
+
+        //     /* Default Commands */
+        //     // drives the robot with a two-axis input from the driver controller
+            // ! Swerve Drive Default Command (Regular Rotation)
+
+        // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+        //     m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft());
+        // }
+        // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+        //     m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
+        //                                     getDeadbandedDriverController().getRight(),
+        //                         true);
+        // }, m_robotSwerveDrive)
+        // .withName("SwerveDrive DefaultCommand"));
+        // m_robotSwerveDrive.setToSlow();
+
+        // ! Swerve Drive Default Command (Orientation Rotation)
+        // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+        //     m_robotSwerveDrive.driveWithInputOrientation(getDeadbandedDriverController().getLeft(), 
+        //                                                  getDeadbandedDriverController().getRightX(), 
+        //                                                  getDeadbandedDriverController().getRightY(), 
+        //                                                  true);
+        // }, m_robotSwerveDrive)
+        // .withName("SwerveDrive OrientationCommand"));
         // continually sends updates to the Blinkin LED controller to keep the lights on
-        m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+        // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
+
     }
 
+    // private void changeAuto() {
+    //     autoPlayback.unloadAuto();
+    //     autoPlayback.loadAuto();
+    //     lastAutoName = autoplaybackName.get();
+    //     System.out.println("AUTO: Changed auto to; `" + lastAutoName + "`");
+    // }
     /**
      * Use this method to define your button->command mappings. Buttons can be
      * created by instantiating a {@link GenericHID} or one of its subclasses
@@ -66,26 +222,191 @@ public class RobotContainer {
      * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
      */
     private void configureButtonBindings() {
-        /* Driver Buttons */
+
-        // test command to spin the robot while pressing A on the driver controller
+        // ? /* Driver Buttons */
+
+        // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
+
+        // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue()))
+        //     .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180()));
+
+        // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp()))
+        //     .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180()));
+
+       // *  /* D-Pad Stuff */
+    //    new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
+    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
+    //                                                                            new Translation2d(0, 0),
+    //                                                                            true)))
+    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
+    //                                                                             new Translation2d(0, 0), 
+    //                                                                             true)));
+
+    //    new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
+    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1),
+    //                                                                            new Translation2d(0, 0),
+    //                                                                            true)))
+    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
+    //                                                                             new Translation2d(0, 0), 
+    //                                                                             true)));
+
+    //    new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9)
+    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0),
+    //                                                                            new Translation2d(0, 0),
+    //                                                                            true)))
+    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
+    //                                                                             new Translation2d(0, 0), 
+    //                                                                             true)));
+
+    //    new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9)
+    //         .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0),
+    //                                                                            new Translation2d(0, 0),
+    //                                                                            true)))
+    //         .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
+    //                                                                             new Translation2d(0, 0), 
+                                                                                // true)));
+        
+        // ! /* Auto Recording */
+        // new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
+        //    .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
+        //                 new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
+        //                                    () -> autoplaybackName.get()))
+        //    .onFalse(new InstantCommand());
+        
+        // new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
+        //    .onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
+        //    () -> autoplaybackName.get(),
+        //    new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
+        //    true, false))
+        //    .onFalse(new InstantCommand());
+
         // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
                 //    .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
                 //                                    () -> getDeadbandedDriverController().getLeftX(),
                 //                                    () -> getDeadbandedDriverController().getLeftY(),
                 //                                    () -> getDeadbandedDriverController().getRightX(),
                 //                                    () -> getDeadbandedDriverController().getRightY(),
-        //                                     "Blue1Path.txt"))
+                //                                    "Taxi.txt"))
                 //    .onFalse(new InstantCommand());
 
                 // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
-        //     .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
+                //    .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
                 //    .onFalse(new InstantCommand()); 
+        // ! /* Speed */
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
+        //     .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
+        //   // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
+        
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
+        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
+
+        // new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
+        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot()));
+
+        // new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
+            // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
+        
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+        //     .whileTrue(new InstantCommand(() -> 
+        //     m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
+        //                                       new Translation2d(0, 0),
+        //                         true), m_robotSwerveDrive));
+
+        
+       //?  /* Operator Buttons */
+
+        // new Joystick(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotMap.rightFront.go(new Translation2d())));
+
+        // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotIntake.PIDOut()))
+        //     .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
+
+        // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotIntake.handoff()))
+        //     .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors()));
+            
+        // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
+        //      .onTrue(emergencyRetract);
+
+
+        // Override Intake Position encoder: out
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-55), m_robotIntake));
+
+        // // Override Intake Position encoder: in
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-6.2), m_robotIntake));
+
+        // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotShooter.spin(0.5), m_robotShooter))
+        //     .onFalse(turnOffShoot);
+
+
+        // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
+        //     .onTrue(i)
+        //     .onFalse(new InstantCommand(() -> m_robotIntake.PIDIn()));
+        
+        // //spins up shooter, no wind down
+        // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
+        //     .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
+        
+        // // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
+        // //     .onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor(), m_robotIntake))
+        // //     .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake));
+
+        // DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
+        //     .onTrue(emergencyRetract);
+
+        // new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
+        //     .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
+        //     .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
+
+        // new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
+        //     .onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
+        //     .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
+
+        // new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
+        //     .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5)));
+
+    }
+    
+    /**
+     * This method is used to replcate {@link Trigger Triggers} for {@link VirtualController Virtual Controllers}. <p/>
+     * Please use {@link RobotContainer#DualJoystickButton} in {@link RobotContainer#configureButtonBindings} for standard buttons.
+     */
+    private void configureVirtualButtonBindings() {
+
+        // ? /* Driver Buttons */
+        
+        /* Notice: the following buttons have not been replicated
+         * Swerve Drive Slow and Fast mode Gear Shifts : Fast mode is known to cause drift, so we disable that feature in Autoplayback
+         * Swerve Drive Rotation Gear Shifts           : Same reason as Slow and Fast mode.
+         * Auto Recording controls                     : We don't want an Null Ouroboros for an auto.
+         */
+
+        // ? /* Operator Buttons */
+
+        /* Notice: the following buttons have not been replicated
+         * Override Intake Position Encoder : It's an emergancy overide, for when the position of intake when the robot boots, the intake is not inside the robot.
+         *                                    We don't need it in an auto.
+         * Climbing controls                : We don't need to climb in auto.
+         */
+
+        // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
+        //     .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
+        //     .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
+
+        // new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
+        //     .onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
+        //     .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
+
+        // new Trigger(() -> getVirtualOperatorController().getPOV() == 0)
+        //     .onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5)));
 
-        /* Operator Buttons */
-        // activates "Lit Mode"
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
-        //         .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
-        //         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
     }
 
     /**
@@ -95,7 +416,19 @@ public class RobotContainer {
      */
     public Command getAutonomousCommand() {
         //no auto
-        return new InstantCommand();
+        // return autoPlayback;
+        //return playbackChooser.getCommand();
+        return new Command() {};
+    }
+
+    /**
+     * A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller}
+     * @param joystickA A controller
+     * @param joystickB A controller
+     * @param buttonNumber The button to bind to
+     */
+    public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) {
+        return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
     }
 
     /**
@@ -108,4 +441,12 @@ public class RobotContainer {
     public DeadbandedXboxController getDeadbandedOperatorController() {
         return this.m_operatorXbox;
     }
+
+    // public VirtualController getVirtualDriverController() {
+    //     return m_virtualDriver;
+    // }
+
+    // public VirtualController getVirtualOperatorController() {
+    //     return m_virtualOperator;
+    // }
 }

src/main/java/frc4388/robot/RobotMap.java

@@ -7,16 +7,20 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.InvertType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.sensors.CANCoder;
+// import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.PigeonIMU;
+// import com.ctre.phoenix.sensors.WPI_Pigeon2;
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.hardware.CANcoder;
 
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.Constants.ShooterConstants;
+import frc4388.robot.Constants.ClimbConstants;
+import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.subsystems.SwerveModule;
 import frc4388.utility.RobotGyro;
 
@@ -25,8 +29,8 @@ import frc4388.utility.RobotGyro;
  * testing and modularization.
  */
 public class RobotMap {
-    private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
+    // private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
-    public RobotGyro gyro = new RobotGyro(m_pigeon2);
+    // public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
     public SwerveModule leftFront;
     public SwerveModule rightFront;
@@ -39,58 +43,118 @@ public class RobotMap {
     }
 
     /* LED Subsystem */
-    public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+    // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
 
 
+    public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
+    public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
+    public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
+
     /* Swreve Drive Subsystem */
-    public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
+    // public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
-    public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
+    // public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
-    public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
+    // public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
 
-    public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
-    public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
-    public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
 
-    public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
+    // public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
-    public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
+    // public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
-    public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
+    // public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
         
-    public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
+    // public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
-    public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
+    // public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
-    public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
+    // public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
+
+    // public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
+    // public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
+    // public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
+
+    /* Shooter Subsystem */
+    // public final TalonFX leftShooter = new TalonFX(ShooterConstants.LEFT_SHOOTER_ID);
+    // public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID);
+
+    /* Intake Subsystem */
+    // public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID);
+    // public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID);
+
+    /* Climber Subsystem */
+    // public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID);
 
     void configureLEDMotorControllers() {    
     }
 
+    void configureIntakeMotorControllers() {
+        // intakeMotor.getConfigurator().apply(new TalonFXConfiguration());
+        // pivotMotor.getConfigurator().apply(new TalonFXConfiguration());
+    }
+
     void configureDriveMotorControllers() {
         // config factory default
-        leftFrontWheel.configFactoryDefault();
+    //     leftFrontWheel.configFactoryDefault();
-        leftFrontSteer.configFactoryDefault();
+    //     leftFrontSteer.configFactoryDefault();
         
-        rightFrontWheel.configFactoryDefault();
+    //     rightFrontWheel.configFactoryDefault();
-        rightFrontSteer.configFactoryDefault();
+    //     rightFrontSteer.configFactoryDefault();
         
-        leftBackWheel.configFactoryDefault();
+    //     leftBackWheel.configFactoryDefault();
-        leftBackSteer.configFactoryDefault();
+    //     leftBackSteer.configFactoryDefault();
         
-        rightBackWheel.configFactoryDefault();
+    //     rightBackWheel.configFactoryDefault();
-        rightBackSteer.configFactoryDefault();
+    //     rightBackSteer.configFactoryDefault();
 
-        // set neutral mode
+    //     // config open loop ramp
-        leftFrontWheel.setNeutralMode(NeutralMode.Brake);
+    //     leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-        rightFrontWheel.setNeutralMode(NeutralMode.Brake);
+    //     leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-        leftBackWheel.setNeutralMode(NeutralMode.Brake);
-        rightBackWheel.setNeutralMode(NeutralMode.Brake);
         
-        leftFrontSteer.setNeutralMode(NeutralMode.Brake);
+    //     rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-        rightFrontSteer.setNeutralMode(NeutralMode.Brake);
+    //     rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-        leftBackSteer.setNeutralMode(NeutralMode.Brake);
-        rightBackSteer.setNeutralMode(NeutralMode.Brake);
         
-        // initialize SwerveModules
+    //     leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-        this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
+    //     leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-        this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
+        
-        this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
+    //     rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-        this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
+    //     rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+    //     // config closed loop ramp
+    //     leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    //     leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+        
+    //     rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    //     rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+        
+    //     leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    //     leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+        
+    //     rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+    //     rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+    //     // config neutral deadband
+    //     leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+    //     leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+    //     rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+    //     rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+    //     leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+    //     leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+    //     rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+    //     rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+    //     // set neutral mode
+    //     leftFrontWheel.setNeutralMode(NeutralMode.Brake);
+    //     rightFrontWheel.setNeutralMode(NeutralMode.Brake);
+    //     leftBackWheel.setNeutralMode(NeutralMode.Brake);
+    //     rightBackWheel.setNeutralMode(NeutralMode.Brake);
+
+    //     leftFrontSteer.setNeutralMode(NeutralMode.Brake);
+    //     rightFrontSteer.setNeutralMode(NeutralMode.Brake);
+    //     leftBackSteer.setNeutralMode(NeutralMode.Brake);
+    //     rightBackSteer.setNeutralMode(NeutralMode.Brake);
+        
+    //     // initialize SwerveModules
+    //     this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
+        this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
+    //     this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
+    //     this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
     }
 }

src/main/java/frc4388/robot/commands/Autos/AutoAlign.java

@@ -0,0 +1,208 @@
+// package frc4388.robot.commands.Autos;
+// import frc4388.robot.subsystems.Limelight;
+// import frc4388.robot.subsystems.SwerveDrive;
+// import frc4388.robot.Constants.AutoAlignConstants;
+// import frc4388.robot.Constants.VisionConstants;
+// import edu.wpi.first.wpilibj2.command.Command;
+
+// import edu.wpi.first.math.geometry.Translation2d;
+// import edu.wpi.first.math.geometry.Rotation2d;
+
+// import java.util.Optional;
+
+// import org.opencv.core.RotatedRect;
+
+// import edu.wpi.first.math.geometry.Pose2d;
+
+// import edu.wpi.first.wpilibj.DriverStation;
+// import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+// public class AutoAlign extends Command {
+//     private SwerveDrive swerve;
+//     private Limelight limelight;
+//     private Pose2d pose;
+//     private Translation2d targetPos;
+//     private Rotation2d targetRot;
+
+//     private Optional<Alliance> alliance;
+//     private boolean isRed;
+
+//     private boolean isFinished;
+//     private boolean isReverseFinished;
+
+//     private boolean reverseAfterFinish;
+//     private Translation2d[] moveStickReplayArr;
+//     private Translation2d[] rotStickReplayArr;
+//     private int replayIndex;
+
+//     // PID Stuff
+//     private double prevError;
+//     private double cumError;
+
+//     public AutoAlign(SwerveDrive swerve, Limelight limelight) {
+//         this.swerve = swerve;
+//         this.limelight = limelight;
+//     }
+
+//     // Calc the closest point on a circle, to the center of the speaker 
+//     private Translation2d calcTargetPos(){
+//         final double R = VisionConstants.targetPosDistance;
+//         final double cX;
+//         final double cY;
+//         if(isRed){
+//             cX = VisionConstants.RedSpeakerCenter.getX();
+//             cY = VisionConstants.RedSpeakerCenter.getY();
+//         }else{
+//             cX = VisionConstants.BlueSpeakerCenter.getX();
+//             cY = VisionConstants.BlueSpeakerCenter.getY();
+//         }
+//         final double pX = pose.getX();
+//         final double pY = pose.getY();
+
+//         // Code is from https://stackoverflow.com/questions/300871/best-way-to-find-a-point-on-a-circle-closest-to-a-given-point
+//         double vX = pX - cX;
+//         double vY = pY - cY;
+//         double magV = Math.sqrt(vX*vX + vY*vY);
+//         double aX = cX + vX / magV * R;
+//         double aY = cY + vY / magV * R;
+
+//         return new Translation2d(aX, aY);
+//     }
+
+//     // Calculate the angle facing the center, at the target rot
+//     private Rotation2d calcTargetRot() {
+//         final double R = VisionConstants.targetPosDistance;
+//         final double cX;
+//         final double cY;
+//         if(isRed){
+//             cX = VisionConstants.RedSpeakerCenter.getX();
+//             cY = VisionConstants.RedSpeakerCenter.getY();
+//         }else{
+//             cX = VisionConstants.BlueSpeakerCenter.getX();
+//             cY = VisionConstants.BlueSpeakerCenter.getY();
+//         }
+//         final double pX = pose.getX();
+//         final double pY = pose.getY();
+
+//         final double dX = pX - cX;
+//         final double dY = pY - cY;
+
+//         final double yaw = ((Math.atan2(dX, dY)*360/Math.PI) % 360);
+
+//         return Rotation2d.fromDegrees(yaw);
+//     }
+
+//     // Clamp to a circle, like an xbox controller
+//     private Translation2d clamp(double oldX, double oldY){
+//         // Code is from https://stackoverflow.com/questions/74329985/how-can-i-clamp-a-position-to-a-circley
+//         final double alpha = (Math.atan2(oldX, -oldY) * 180 / Math.PI + 360) % 360;
+//         final double maxX = Math.abs(Math.cos(alpha / 180 * Math.PI));
+//         final double maxY = Math.abs(Math.sin(alpha / 180 * Math.PI));
+
+//         final double newX = Math.max(-maxX, Math.min(maxX, oldX));
+//         final double newY = Math.max(-maxY, Math.min(maxY, oldY));
+
+//         return new Translation2d(newX, newY);
+//     }
+
+//     private Translation2d calcMoveStick(){
+//         final double curX = pose.getX();
+//         final double curY = pose.getY();
+
+//         // I think this might work, assuming the constants are good
+//         double stickX = -(curX - targetPos.getX()) * AutoAlignConstants.MoveSpeed;
+//         double stickY = -(curY - targetPos.getY()) * AutoAlignConstants.MoveSpeed;
+
+//         return clamp(stickX, stickY);
+//     }
+
+//     private Translation2d calcRotStick(){
+//         double error = pose.getRotation().getDegrees() - targetRot.getDegrees();
+// 		cumError += error * .02; // 20 ms
+// 		double delta = error - prevError;
+
+//         final double kP = 4;
+//         final double kI = 4;
+//         final double kD = 4;
+//         final double kF = 4;
+
+// 		double output = error * kP;
+// 		output += cumError * kI;
+// 		output += delta * kD;
+// 		output += kF;
+
+//         prevError = error;
+//         return clamp(output, 0);
+//     }
+
+//     public void reverse() {
+//         this.reverseAfterFinish = true;
+//     }
+
+//     // Called when the command is initially scheduled.
+// 	@Override 
+// 	public final void initialize() {
+//         isRed = alliance.get() == DriverStation.Alliance.Red;
+//         if(reverseAfterFinish){
+//             // isReverseFinished = false;
+//             replayIndex = 0;
+//         }else{
+//             moveStickReplayArr = new Translation2d[]{};
+//             rotStickReplayArr = new Translation2d[]{};
+//         }
+// 	}
+
+//     // Called every time the scheduler runs while the command is scheduled.
+// 	@Override
+//     public void execute() {
+//         // Update limelight pos
+//         pose = limelight.getPose();
+
+//         // These must be 0, or it will error
+//         Translation2d moveStick = new Translation2d(0, 0);
+//         Translation2d rotStick = new Translation2d(0, 0);
+
+//         // Regular replay
+//         if(!isFinished){
+//             targetPos = calcTargetPos();
+//             targetRot = calcTargetRot();
+
+//             moveStick = calcMoveStick();
+//             rotStick = calcRotStick();
+
+//             // This is a way of appending...
+//             moveStickReplayArr[moveStickReplayArr.length] = moveStick;
+//             rotStickReplayArr[rotStickReplayArr.length] = rotStick;
+
+//             // if(isFinished != limelight.isNearSpeaker() && isReverseFinished){
+//             //     replayIndex
+//             // }
+//             isFinished = limelight.isNearSpeaker();
+
+//         // If reverseAfterFinish, then loop back over and replay in reverse
+//         }else if(reverseAfterFinish && !isReverseFinished){
+//             // Get reverse direction
+//             moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1];
+//             rotStick = rotStickReplayArr[replayIndex-rotStickReplayArr.length-1];
+
+//             // Invert sticks
+//             moveStick = new Translation2d(moveStick.getX()*-1, moveStick.getY()*-1);
+//             rotStick = new Translation2d(rotStick.getX()*-1, rotStick.getY()*-1);
+
+//             replayIndex++;
+
+//             if(replayIndex >= moveStickReplayArr.length){
+//                 reverseAfterFinish = true;
+//             }
+//         }
+
+//         // This would greatly benifit from having feild Relative implemented.
+//         swerve.driveWithInput(moveStick, rotStick, true);
+//     }
+
+//     // Returns true when the command should end.
+// 	@Override
+// 	public final boolean isFinished() {
+//         return isFinished && (isReverseFinished || !reverseAfterFinish);
+// 	}
+// }

src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java

@@ -42,6 +42,7 @@ public class PlaybackChooser {
         m_playback = m_choosers.get(0);
         nextChooser();
 
+        // ! This does not work, why?
         Shuffleboard.getTab("Auto Chooser")
             .add("Add Sequence", new InstantCommand(() -> nextChooser()))
             .withPosition(4, 0);
@@ -66,9 +67,15 @@ public class PlaybackChooser {
     public void nextChooser() {
         SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
 
-        for (String auto : m_dir.list()) {
+        String[] dirs = m_dir.list();
+
+        if(dirs != null){ // Fix funny error
+            for (String auto : dirs) {
                 chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
             }
+        }
+
+        
         for (var cmd_name : m_commandPool.keySet()) {
             chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
         }

src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt

@@ -0,0 +1,20 @@
+AUTO file format
+
+HEADER static size 0x5
+0x00 BYTE   NUM AXES: defualts to 6
+0x01 BYTE   NUM POV: defualts to 1
+0x02 BYTE   NUM CONTROLLERS: defualts to 2
+0x03 SHORT  FRAMES: any value greator or equal than one.
+
+FRAME PER CONTROLLER: defualt size 0x34
+0x00 DOUBLE AXES[NUM AXES]
+0x30 SHORT  BUTTONS
+0x32 SHORT  POVs[NUM POV]
+
+FRAME: size varrys
+FRAME PER CONTROLLER[NUM CONTROLLERS]
+INT         UNIXTIMESTAMP
+
+FILE:
+HEADER
+FRAME[FRAMES]

src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java

@@ -0,0 +1,107 @@
+// package frc4388.robot.commands.Autos;
+
+// import java.io.File;
+// import java.util.ArrayList;
+// import java.util.HashMap;
+
+// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
+// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+// import edu.wpi.first.wpilibj2.command.Command;
+// import edu.wpi.first.wpilibj2.command.InstantCommand;
+// import frc4388.robot.commands.Swerve.JoystickPlayback;
+// import frc4388.robot.commands.Swerve.neoJoystickPlayback;
+// import frc4388.robot.subsystems.SwerveDrive;
+// import frc4388.utility.controller.VirtualController;
+
+// public class neoPlaybackChooser {
+//     private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();    
+//     private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
+//     private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
+
+//     private final SwerveDrive m_swerve;
+//     private final VirtualController[] m_controllers;
+//     // private final ArrayList<SendableChooser<Command>> m_choosers    = new ArrayList<>();
+//     // private       SendableChooser<Command>            m_playback    = null;
+//     private final ArrayList<ComplexWidget>            m_widgets     = new ArrayList<>();
+//     // private final HashMap<String, Command>            m_commandPool = new HashMap<>();
+    
+//     // private final File        m_dir    = new File("/home/lvuser/autos/");
+//     // private       int         m_cmdNum = 0;
+
+//     // commands
+//     private Command m_noAuto = new InstantCommand();
+    
+//     public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
+//         m_teamChosser.addOption("Red", "red");
+//         m_teamChosser.setDefaultOption("Blue", "blue");
+//         m_teamChosser.addOption("Nuetral", "nuetral");
+//         m_possitionChosser.addOption("AMP", "amp");
+//         m_possitionChosser.setDefaultOption("Center", "center");
+//         m_possitionChosser.addOption("Source", "source");
+//         m_swerve = swerve;
+//         m_controllers = controllers;
+//     }
+
+//     public neoPlaybackChooser addOption(String name, String option) {
+//         m_autoNameChosser.addOption(name, option);
+//         return this;
+//     }
+
+//     // public PlaybackChooser buildDisplay() {
+//     //     for (int i = 0; i < 10; i++) {
+//     //         appendCommand();
+//     //     }
+//     //     m_playback = m_choosers.get(0);
+//     //     nextChooser();
+
+//     //     // ! This does not work, why?
+//     //     Shuffleboard.getTab("Auto Chooser")
+//     //         .add("Add Sequence", new InstantCommand(() -> nextChooser()))
+//     //         .withPosition(4, 0);
+//     //     return this;
+//     // }
+
+//     // This will be bound to a button for the time being
+//     public void render() {
+//         // var chooser = new SendableChooser<Command>();
+//         // // chooser.setDefaultOption("No Auto", m_noAuto);
+
+//         // m_choosers.add(chooser);
+//         ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
+//             .add("Command: " + m_choosers.size(), chooser)
+//             .withSize(4, 1)
+//             .withPosition(0, m_choosers.size() - 1)
+//             .withWidget(BuiltInWidgets.kSplitButtonChooser)
+//             .withWidget(BuiltInWidgets.kComboBoxChooser);
+
+
+//         m_widgets.add(widget);
+//     }
+
+//     // public void nextChooser() {
+//     //     SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
+
+//     //     String[] dirs = m_dir.list();
+
+//     //     if(dirs != null){ // Fix funny error
+//     //         for (String auto : dirs) {
+//     //             chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
+//     //         }
+//     //     }
+
+        
+//     //     for (var cmd_name : m_commandPool.keySet()) {
+//     //         chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
+//     //     }
+//     // }
+    
+//     public String autoName() {
+//         return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
+//     }
+
+//     public Command getCommand() {
+//         return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
+//     }
+// }

src/main/java/frc4388/robot/commands/PID.java

@@ -4,10 +4,10 @@
 
 package frc4388.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.utility.Gains;
 
-public abstract class PID extends CommandBase {
+public abstract class PID extends Command {
 	protected Gains  gains;
 	private   double output    = 0;
 	private   double tolerance = 0;

src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java

@@ -119,7 +119,11 @@ public class JoystickPlayback extends CommandBase {
     //                              new Translation2d(out.rightX, out.rightY),
     //                              true);
     
-    this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
+    // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
+    //                             new Translation2d(lerpRX, lerpRY),
+    //                             true);
+
+    this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY),
                                 new Translation2d(lerpRX, lerpRY),
                                 true);
                              

src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java

@@ -64,11 +64,11 @@ public class JoystickRecorder extends CommandBase {
 
     outputs.add(inputs);
 
-    swerve.driveWithInput(new Translation2d(inputs.leftX,  inputs.leftY),
+    swerve.playbackDriveWithInput(new Translation2d(inputs.leftX,  inputs.leftY),
                           new Translation2d(inputs.rightX, inputs.rightY),
                           true);
     
-    System.out.println("RECORDING");
+    //System.out.println("RECORDING");
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java

@@ -0,0 +1,197 @@
+package frc4388.robot.commands.Swerve;
+
+import java.io.FileInputStream;
+import java.util.ArrayList;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.DataUtils;
+import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
+import frc4388.utility.UtilityStructs.AutoRecordingFrame;
+import frc4388.utility.controller.VirtualController;
+
+
+/**
+ * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s 
+ * @author Zachary Wilke
+ */
+public class neoJoystickPlayback extends Command {
+    private final SwerveDrive                   swerve;
+    private final VirtualController[]           controllers;
+    private final ArrayList<AutoRecordingFrame> frames         = new ArrayList<>();
+    private final Supplier<String>              filenameGetter;
+    private       String                        filename;
+    private       int                           frame_index    = 0;
+    private       long                          startTime      = 0;
+    private       long                          playbackTime   = 0;
+    private       boolean                       m_finished     = false; // ! There is no better way.
+    private       boolean                       m_shouldfree   = false; // should free memory on ending
+    
+    private byte  m_numAxes = 0;
+    private byte  m_numPOVs = 0;
+    private byte  m_numControllers = 0;
+    private short m_numFrames = -1;
+
+    /**
+     * Creates an new NEO Joystick Playback with specifyed pramiters.
+     * @param swerve m_robotSwerveDrive
+     * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. 
+     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
+     * @param shouldfree Unloads the auto on compleation or intruption.
+     * @param instantload Load the auto on object instantiation
+     */
+    public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
+        this.swerve = swerve;
+        this.filenameGetter = filenameGetter;
+        this.controllers = controllers;
+        this.m_shouldfree = shouldfree;
+
+        if (instantload) loadAuto();
+        addRequirements(this.swerve);
+    }
+
+    /**
+     * Creates an new NEO Joystick Playback with specifyed pramiters.
+     * @param swerve m_robotSwerveDrive
+     * @param filename a String containing the name of the auto file you wish to playback. 
+     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
+     * @param shouldfree unloads the auto on compleation or intruption.
+     * @param instantload load the auto on object instantiation
+     */
+    public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
+        this(swerve, () -> filename, controllers, shouldfree, instantload);
+    }
+
+    /**
+     * Creates an new NEO Joystick Playback with specifyed pramiters.
+     * @param swerve m_robotSwerveDrive
+     * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
+     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
+     */
+    public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers) {
+        this(swerve, filenameGetter, controllers, true, false);
+    }
+
+    /**
+     * Creates an new NEO Joystick Playback with specifyed pramiters.
+     * @param swerve m_robotSwerveDrive
+     * @param filename a String containing the name of the auto file you wish to playback. 
+     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
+     */
+    public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
+        this(swerve, () -> filename, controllers, true, false);
+    }
+    
+    /**
+     * Load the auto file from disk into memory
+     * @return Returns true if loading was successful, else wise; return false
+     * @implNote if the auto is already loaded, it will return true. 
+     */
+    public boolean loadAuto() {
+        filename = filenameGetter.get();
+        try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
+            if (m_numFrames != -1 && m_numFrames == frames.size()) {
+                System.out.println("AUTOPLAYBACK: Auto Already loaded.");
+                return true;
+            }
+            
+            m_numAxes = stream.readNBytes(1)[0];
+            m_numPOVs = stream.readNBytes(1)[0];
+            m_numControllers = stream.readNBytes(1)[0];
+            m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2));
+
+            if (m_numControllers > controllers.length) {
+                System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers 
+                + " virtual controllers but only " + controllers.length + " were given");
+                return false;
+            }
+
+            for (int i = 0; i < m_numFrames; i++) {
+                AutoRecordingFrame frame = new AutoRecordingFrame();
+                for (int j = 0; j < m_numControllers; j++) {
+                    AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
+                    double[] axes = new double[m_numAxes];
+                    for (int k = 0; k < m_numAxes; k++) { // we love third level for loops.
+                        axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8));
+                    }
+                    short button = DataUtils.byteArrayToShort(stream.readNBytes(2));
+                    short[] POV = new short[m_numPOVs];
+                    for (int k = 0; k < m_numPOVs; k++) {
+                        POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2));
+                    }
+                    controllerFrame.axes = axes;
+                    controllerFrame.button = button;
+                    controllerFrame.POV = POV;
+                    frame.controllerFrames[j] = controllerFrame;
+                }
+                frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4));
+                frames.add(frame);
+            }
+
+            System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long");
+            return true;
+
+        } catch (Exception e) {
+            e.printStackTrace();
+            System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`');
+            return false;
+        }
+    }
+    
+    /**
+     * Unloads the auto.
+     */
+    public void unloadAuto() {
+        System.out.println("AUTOPLAYBACK: Auto unloaded");
+        frames.clear();
+    }
+
+    @Override
+    public void initialize() {
+        startTime = System.currentTimeMillis();
+        playbackTime = 0;
+        frame_index = 0;
+
+        m_finished = !loadAuto();
+    }
+
+    @Override
+    public void execute() {
+        if (frame_index >= m_numFrames) m_finished = true;
+        if (m_finished) return;
+
+        // if (frame_index == 0) {
+        //    startTime = System.currentTimeMillis();
+        //    playbackTime = 0;
+        // } else {
+        //    playbackTime = System.currentTimeMillis() - startTime;
+        // }
+
+        AutoRecordingFrame frame = frames.get(frame_index);
+        for (int i = 0; i < controllers.length; i++) {
+            AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i];
+            controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
+            if (i == 0) {
+                this.swerve.driveWithInput(
+                    new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
+                    new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
+                    true);
+            }
+        }
+        frame_index++;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        for (VirtualController controller : controllers) controller.zeroControls();
+        swerve.stopModules();
+        if (m_shouldfree) unloadAuto();
+    }
+    
+    @Override
+    public boolean isFinished() {
+        return m_finished;
+    }
+}

src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java

@@ -0,0 +1,129 @@
+package frc4388.robot.commands.Swerve;
+
+import java.io.FileOutputStream;
+import java.util.ArrayList;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.DataUtils;
+import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
+import frc4388.utility.UtilityStructs.AutoRecordingFrame;
+import frc4388.utility.controller.DeadbandedXboxController;
+
+/**
+ * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s 
+ * @author Zachary Wilke
+ */
+public class neoJoystickRecorder extends Command {
+    private final SwerveDrive swerve;
+    private final XboxController[] controllers;
+    private       String filename;
+    private final Supplier<String> filenameGetter;
+    private       long startTime = -1;
+    private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
+
+    /**
+     * Creates an new NEO Joystick Playback with specifyed pramiters.
+     * @param swerve m_robotSwerveDrive
+     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
+     * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
+     */
+    public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier<String> filenameGetter) {
+        this.swerve = swerve;
+        this.controllers = controllers;
+        this.filenameGetter = filenameGetter;
+        this.filename = "";
+
+        addRequirements(this.swerve);
+    }
+
+    /**
+     * Creates an new NEO Joystick Playback with specifyed pramiters.
+     * @param swerve m_robotSwerveDrive
+     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
+     * @param filename a String containing the name of the auto file you wish to playback. 
+     */
+    public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
+        this(swerve, controllers, () -> filename);
+    }
+
+    @Override
+    public void initialize() {
+        frames.clear();
+
+        this.startTime = System.currentTimeMillis();
+        AutoRecordingFrame frame = new AutoRecordingFrame();
+        frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()};
+        frames.add(frame);
+        this.filename = this.filenameGetter.get();
+    }
+
+
+    @Override
+    public void execute() {
+        System.out.println("AUTORECORD: RECORDING");
+        AutoRecordingFrame frame = new AutoRecordingFrame();
+        frame.timeStamp = (int) (System.currentTimeMillis() - startTime);
+        for (int i = 0; i < controllers.length; i++) {
+            XboxController controller = controllers[i];
+            AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
+            double[] axes = {controller.getLeftX(), controller.getLeftY(), 
+                            controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
+                            controller.getRightX(), controller.getRightY()};
+            short button = 0;
+            for (int j = 0; j < 10; j++)
+                if (controller.getRawButton(j+1))
+                    button |= 1 << j;
+            short[] POV = {(short)(controller.getPOV())};
+            controllerFrame.axes = axes;
+            controllerFrame.button = button;
+            controllerFrame.POV = POV;
+            frame.controllerFrames[i] = controllerFrame;
+        }
+
+        frames.add(frame);
+
+        swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]),
+                            new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), 
+                            true); // Really jank way of doing this.
+        
+    }
+    @Override
+    public void end(boolean interrupted) {
+        try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) {
+            // header: size of 0x5
+            // byte Number of axes per controller
+            // byte Number of POVs per controller
+            // byte Number of controllers
+            // short Number of frames 
+            stream.write(new byte[]{6, 1, (byte) controllers.length}); 
+            stream.write(DataUtils.shortToByteArray((short) frames.size())); 
+
+            // frame
+            // controller frame * number of controllers
+            // int unix time stamp.
+            for (AutoRecordingFrame frame : frames) {
+                // controller frame
+                // double axis * Number of axes per controller
+                // short button states
+                // short POV * Number of POVs per controller
+                for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) {
+                    for (double axis: controllerFrame.axes) {
+                        stream.write(DataUtils.doubleToByteArray(axis));
+                    }
+                    stream.write(DataUtils.shortToByteArray(controllerFrame.button));
+                    for (short POV: controllerFrame.POV) {
+                        stream.write(DataUtils.shortToByteArray(POV));
+                    }
+                }
+                stream.write(DataUtils.intToByteArray(frame.timeStamp));
+            }
+        System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long.");
+        } catch (Exception e) {
+            e.printStackTrace();
+        } 
+    }
+}

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -4,15 +4,16 @@
 
 package frc4388.robot.subsystems;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.RobotGyro;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class  SwerveDrive extends SubsystemBase {
   
@@ -24,17 +25,20 @@ public class SwerveDrive extends SubsystemBase {
   private SwerveModule[] modules;
 
   private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
-  private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
-  private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
+  private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
   private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
   
   private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
 
   private RobotGyro gyro;
 
-  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
+  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default
+  public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
+  public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   
   public double rotTarget = 0.0;
+  public Rotation2d orientRotTarget = new Rotation2d();
   public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   /** Creates a new SwerveDrive. */
@@ -51,14 +55,51 @@ public class SwerveDrive extends SubsystemBase {
 
   boolean stopped = false;
   public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+    rightFront.go(leftStick);
+    // if (fieldRelative) {
+
+    //   double rot = 0;
+      
+    //   // ! drift correction
+    //   if (rightStick.getNorm() > 0.05) {
+    //     rotTarget = gyro.getAngle();
+    //     rot = rightStick.getX();
+    //   //  SmartDashboard.putBoolean("drift correction", false);
+    //     stopped = false;
+    //   } else if(leftStick.getNorm() > 0.05) {
+    //     if (!stopped) {
+    //       stopModules();
+    //       stopped = true;
+    //     }
+
+    //  //   SmartDashboard.putBoolean("drift correction", true);
+        
+    //     rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
+
+    //   }
+
+    //   // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
+    //   Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
+    //   // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
+
+    //   // Convert field-relative speeds to robot-relative speeds.
+    //   chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * rotSpeedAdjust, gyro.getRotation2d());//.times(-1));
+    // } else {      // Create robot-relative speeds.
+    //   chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+    // }
+    // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
+  }
+
+  public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
     if (fieldRelative) {
 
       double rot = 0;
       
+      // ! drift correction
       if (rightStick.getNorm() > 0.05) {
         rotTarget = gyro.getAngle();
         rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
-        SmartDashboard.putBoolean("drift correction", false);
+      //  SmartDashboard.putBoolean("drift correction", false);
         stopped = false;
       } else if(leftStick.getNorm() > 0.05) {
         if (!stopped) {
@@ -66,36 +107,63 @@ public class SwerveDrive extends SubsystemBase {
           stopped = true;
         }
 
-        SmartDashboard.putBoolean("drift correction", true);
+     //   SmartDashboard.putBoolean("drift correction", true);
+        
         rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
 
       }
 
       // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
-      Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
+      Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
       // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
-    } else {
+    } else {      // Create robot-relative speeds.
-      // Create robot-relative speeds.
+      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
-      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
     }
-    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
+    // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
+  }
+
+  public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
+
+    Translation2d rightStick = new Translation2d(-rightX, rightY);
+
+    if(fieldRelative) {
+      double rot = 0;
+      if(rightStick.getNorm() > 0.5) {
+        orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
+        
+        Rotation2d tmp =  orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5));
+        double min = tmp.getDegrees();
+        min = Math.max(Math.abs(min), 2);
+        if(tmp.getDegrees() < 0)
+          min*=-1;
+        tmp = new Rotation2d(min * Math.PI / 180);
+        rot = tmp.getRadians(); // x   x - y/x
+      }
+    
+    Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
+
+    chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1);
+    } else {      // Create robot-relative speeds.
+      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+    }
+    // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));  
     }
 
   /**
    * Set each module of the swerve drive to the corresponding desired state.
    * @param desiredStates Array of module states to set.
    */
-  public void setModuleStates(SwerveModuleState[] desiredStates) {
+  // public void setModuleStates(SwerveModuleState[] desiredStates) {
-    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+  //   SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
-    for (int i = 0; i < desiredStates.length; i++) {
+  //   for (int i = 0; i < desiredStates.length; i++) {
-      SwerveModule module = modules[i];
+  //     SwerveModule module = modules[i];
-      SwerveModuleState state = desiredStates[i];
+  //     SwerveModuleState state = desiredStates[i];
-      module.setDesiredState(state);
+  //     module.setDesiredState(state);
-    }
+  //   }
-  }
+  // }
 
   public boolean rotateToTarget(double angle) {
     double currentAngle = getGyroAngle();
@@ -114,9 +182,30 @@ public class SwerveDrive extends SubsystemBase {
     return gyro.getAngle();
   }
 
+  public void add180() {
+    gyro.reset(gyro.getAngle()+180);
+    rotTarget = gyro.getAngle();
+
+  }
+
   public void resetGyro() {
     gyro.reset();
-    rotTarget = 0.0;
+    rotTarget = gyro.getAngle();
+  }
+
+  public void resetGyroFlip() {
+    gyro.resetFlip();
+    rotTarget = gyro.getAngle();
+  }
+
+  public void resetGyroRightBlue() {
+    gyro.resetRightSideBlue();
+    rotTarget = gyro.getAngle();
+  }
+
+  public void resetGyroRightAmp() {
+    gyro.resetAmpSide();
+    rotTarget = gyro.getAngle();
   }
   
   public void stopModules() {
@@ -129,10 +218,15 @@ public class SwerveDrive extends SubsystemBase {
     return this.kinematics;
   }
 
+  public boolean getSpeedState() {
+    
+    return false;
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run\
-    SmartDashboard.putNumber("Gyro", getGyroAngle());
+    // SmartDashboard.putNumber("Gyro", getGyroAngle());
   }
 
   public void shiftDown() {
@@ -142,6 +236,7 @@ public class SwerveDrive extends SubsystemBase {
       this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
     } else {
       this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
+
     }
   }
 
@@ -192,4 +287,14 @@ public class SwerveDrive extends SubsystemBase {
     }
   }
 
+  public void shiftUpRot() {
+    rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
+  }
+
+  public void shiftDownRot() {
+    rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
+  }
+
+  
+
 }

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -4,56 +4,92 @@
 
 package frc4388.robot.subsystems;
 
+// import javax.swing.text.StyleContext.SmallAttributeSet;
+
+// import com.ctre.phoenix.ErrorCode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+// import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+// import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.sensors.CANCoder;
+// import com.ctre.phoenix.sensors.CANCoder;
+// import com.ctre.phoenix.sensors.SensorInitializationStrategy;
 
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.hardware.CANcoder;
+
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
+// import frc4388.utility.configurable.ConfigurableDouble;
 
 public class SwerveModule extends SubsystemBase {
-    private WPI_TalonFX driveMotor;
+    private TalonFX driveMotor;
-    private WPI_TalonFX angleMotor;
+    private TalonFX angleMotor;
-    private CANCoder encoder;
+    private CANcoder encoder;
-
+    // private int selfid;
+    // private ConfigurableDouble offsetGetter;
+    private static int swerveId = 0;
     public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
   
+    
     /** Creates a new SwerveModule. */
-    public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
+    public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
         this.driveMotor = driveMotor;
         this.angleMotor = angleMotor;
         this.encoder = encoder;
+    //     // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset);
+    //     this.selfid = swerveId;
+    //      swerveId++;
+    //     TalonFXConfiguration angleConfig = new TalonFXConfiguration();
+    //     angleConfig.slot0.kP = swerveGains.kP;
+    //     angleConfig.slot0.kI = swerveGains.kI;
+    //     angleConfig.slot0.kD = swerveGains.kD;
 
-        TalonFXConfiguration angleConfig = new TalonFXConfiguration();
+    //     // use the CANcoder as the remote sensor for the primary TalonFX PID
-        angleConfig.slot0.kP = swerveGains.kP;
+    //     angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
-        angleConfig.slot0.kI = swerveGains.kI;
+    //     angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
-        angleConfig.slot0.kD = swerveGains.kD;
+    //     angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
+    //     angleMotor.configAllSettings(angleConfig);
         
-        // use the CANcoder as the remote sensor for the primary TalonFX PID
+    //     //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
-        angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
+    //     reset(0);
-        angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
+    //     encoder.configMagnetOffset(offset);
-        angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
-        angleMotor.configAllSettings(angleConfig);
         
-        encoder.configMagnetOffset(offset);
+    //     driveMotor.setSelectedSensorPosition(0);
-        
+    //     driveMotor.config_kP(0, 0.2);
-        driveMotor.setSelectedSensorPosition(0);
-        driveMotor.config_kP(0, 0.2);
     }
 
+    public void go(Translation2d leftStick){
+        System.out.println(leftStick.getY());
+        driveMotor.set(leftStick.getY());
+    }
+    
+    @Override
+    public void periodic() {
+        //encoder.configMagnetOffset(offsetGetter.get());
+        //SmartDashboard.putString("Error Code: " + selfid, getstuff());
+        // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees());
+        // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel());
+        // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos());
+        // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());
+    }
     /**
      * Get the drive motor of the SwerveModule
      * @return the drive motor of the SwerveModule
      */
-    public WPI_TalonFX getDriveMotor() {
+    public TalonFX getDriveMotor() {
         return this.driveMotor;
     }
 
@@ -61,7 +97,7 @@ public class SwerveModule extends SubsystemBase {
      * Get the angle motor of the SwerveModule
      * @return the angle motor of the SwerveModule
      */
-    public WPI_TalonFX getAngleMotor() {
+    public TalonFX getAngleMotor() {
         return this.angleMotor;
     }
 
@@ -69,7 +105,7 @@ public class SwerveModule extends SubsystemBase {
      * Get the CANcoder of the SwerveModule
      * @return the CANcoder of the SwerveModule
      */
-    public CANCoder getEncoder() {
+    public CANcoder getEncoder() {
         return this.encoder;
     }
 
@@ -79,19 +115,23 @@ public class SwerveModule extends SubsystemBase {
      */
     public Rotation2d getAngle() {
         // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
-        return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
+        // return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
+        return new Rotation2d();
     }
     
     public double getAngularVel() {
-        return this.angleMotor.getSelectedSensorVelocity();
+        // return this.angleMotor.getSelectedSensorVelocity();
+        return 0;
     }
 
     public double getDrivePos() {
-        return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
+        // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
+        return 0;
     }
 
     public double getDriveVel() {
-        return this.driveMotor.getSelectedSensorVelocity(0);
+        // return this.driveMotor.getSelectedSensorVelocity(0);
+        return 0.0;
     }
 
     public void stop() {
@@ -100,62 +140,67 @@ public class SwerveModule extends SubsystemBase {
     }
 
     public void rotateToAngle(double angle) {
-        angleMotor.set(TalonFXControlMode.Position, angle);
+        // angleMotor.set(TalonFXControlMode.Position, angle);
     }
 
     /**
      * Get state of swerve module
      * @return speed in m/s and angle in degrees
      */
-    public SwerveModuleState getState() {
+    // public SwerveModuleState getState() {
-        return new SwerveModuleState(
+    //     return new SwerveModuleState(
-            Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, 
+    //         Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, 
-            getAngle()
+    //         getAngle()
-        );
+    //     );
-    }
+    // }
 
     /**
      * Returns the current position of the SwerveModule
      * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
-     */
+    //  */
-    public SwerveModulePosition getPosition() {
+    // public SwerveModulePosition getPosition() {
-        return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
+    //     return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
-    }
+    // }
 
     /**
      * Set the speed and rotation of the SwerveModule from a SwerveModuleState object
      * @param desiredState a SwerveModuleState representing the desired new state of the module
-     */
+    //  */
-    public void setDesiredState(SwerveModuleState desiredState) {
+    // public void setDesiredState(SwerveModuleState desiredState) {
-        Rotation2d currentRotation = this.getAngle();
+    //     Rotation2d currentRotation = this.getAngle();
 
-        SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
+    //     SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
 
-        // calculate the difference between our current rotational position and our new rotational position
+    //     // calculate the difference between our current rotational position and our new rotational position
-        Rotation2d rotationDelta = state.angle.minus(currentRotation);
+    //     Rotation2d rotationDelta = state.angle.minus(currentRotation);
 
-        // calculate the new absolute position of the SwerveModule based on the difference in rotation
+    //     // calculate the new absolute position of the SwerveModule based on the difference in rotation
-        double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
+    //     double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
 
-        // convert the CANCoder from its position reading to ticks
+    //     // convert the CANCoder from its position reading to ticks
-        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
+    //     double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
 
-        angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
+    //     angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
 
-        double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
+    //     double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
 
-        driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+    //     driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
-    }
+    // }
-
+
-    public void reset(double position) {
+    // public void reset(double position) {
-        encoder.setPositionToAbsolute();
+    //     encoder.setPositionToAbsolute();
-    }
+    // }
-
+
-    public double getCurrent() {
+    // public double getCurrent() {
-        return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
+    //     return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
-    }
+    // }
-
+
-    public double getVoltage() {
+    // public double getVoltage() {
-        return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
+    //     return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
-    }
+    // }
+
+    // public String getstuff() {
+    //     encoder.getPosition();
+    //     return "" + encoder.getLastError().value;
+    // }
 }

src/main/java/frc4388/utility/DataUtils.java

@@ -0,0 +1,35 @@
+package frc4388.utility;
+
+import java.nio.ByteBuffer;
+
+public class DataUtils {
+    public static byte[] doubleToByteArray(double value) {
+        byte[] bytes = new byte[8];
+        ByteBuffer.wrap(bytes).putDouble(value);
+        return bytes;
+    }
+    
+    public static double byteArrayToDouble(byte[] bytes) {
+        return ByteBuffer.wrap(bytes).getDouble();
+    }
+
+    public static byte[] intToByteArray(int value) {
+        byte[] bytes = new byte[4];
+        ByteBuffer.wrap(bytes).putInt(value);
+        return bytes;
+    }
+    
+    public static int byteArrayToInt(byte[] bytes) {
+        return ByteBuffer.wrap(bytes).getInt();
+    }
+
+    public static byte[] shortToByteArray(short value) {
+        byte[] bytes = new byte[2];
+        ByteBuffer.wrap(bytes).putShort(value);
+        return bytes;
+    }
+    
+    public static short byteArrayToShort(byte[] bytes) {
+        return ByteBuffer.wrap(bytes).getShort();
+    }
+}

src/main/java/frc4388/utility/DualJoystickButton.java

@@ -0,0 +1,22 @@
+package frc4388.utility;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+
+/**
+ * A button binding for two controllers, preferably an {@link frc4388.utility.controller.DeadbandedXboxController Xbox Controller} and {@link frc4388.utility.controller.VirtualController Virtual Xbox Controller}
+ * @author Zachary Wilke
+ */
+public class DualJoystickButton extends Trigger {
+
+    /**
+     * Creates an Button binding on two controllers
+     * @param joystickA A controller
+     * @param joystickB A controller
+     * @param buttonNumber The button to bind to
+     */
+    public DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) {
+        super(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
+    }
+}

src/main/java/frc4388/utility/RobotGyro.java

@@ -86,11 +86,12 @@ public class RobotGyro implements Gyro {
      */
     @Override
     public void calibrate() {
-        if (m_isGyroAPigeon) {
+        return;
-            m_pigeon.calibrate();
+        // if (m_isGyroAPigeon) {
-        } else {
+        //     m_pigeon.calibrate();
-            m_navX.calibrate();
+        // } else {
-        }
+        //     m_navX.calibrate();
+        // }
     }
 
     @Override
@@ -102,6 +103,73 @@ public class RobotGyro implements Gyro {
         } else {
             m_navX.reset();
         }
+
+    }
+
+    public void reset(double val) {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(val);
+        } else {
+            m_navX.reset();
+        }
+
+    }
+
+    public void resetFlip() {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(180);
+        } else {
+            m_navX.reset();
+        }
+
+    }
+
+    public void resetNinety() {
+        resetZeroValues();
+        
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(90);
+        } else {
+            m_navX.reset();
+        }
+        
+    }
+
+    public void resetTwoSeventy() {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(270);
+        } else {
+            m_navX.reset();
+        }
+
+    }
+
+    public void resetRightSideBlue() {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(60);
+        } else {
+            m_navX.reset();
+        }   
+
+    }
+
+    public void resetAmpSide() {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(-60);
+        } else {
+            m_navX.reset();
+        }   
+
     }
 
     /**

src/main/java/frc4388/utility/UtilityStructs.java

@@ -7,6 +7,20 @@ public class UtilityStructs {
         public double rightX = 0.0;
         public double rightY = 0.0;
 
+        public boolean OPLB;
+        public boolean OPRB;
+
+    
         public long timedOffset = 0;
     }
+    public static class AutoRecordingControllerFrame {
+        public double[] axes = new double[6];
+        public short button = 0;
+        public short[] POV = new short[1];
+
+    }
+    public static class AutoRecordingFrame {
+        public AutoRecordingControllerFrame[] controllerFrames = new AutoRecordingControllerFrame[2];
+        public int timeStamp;
+    }
 }

src/main/java/frc4388/utility/configurable/ConfigurableDouble.java

@@ -0,0 +1,23 @@
+package frc4388.utility.configurable;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ConfigurableDouble {
+    private double defualtValue;
+    private String name;
+
+    /**
+     * Creates an new ConfigurableDouble through Smart Dashboard.
+     * @param name the name of the Smart Dashboard key.
+     * @param defualtValue the initilization value
+     */
+    public ConfigurableDouble(String name, double defualtValue) {
+        this.name = name;
+        this.defualtValue = defualtValue;
+        SmartDashboard.putNumber(name, defualtValue);
+    }
+
+    public double get() {
+        return SmartDashboard.getNumber(name, defualtValue);
+    }
+}

src/main/java/frc4388/utility/configurable/ConfigurableString.java

@@ -0,0 +1,23 @@
+package frc4388.utility.configurable;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ConfigurableString {
+    private String defualtValue;
+    private String name;
+
+    /**
+     * Creates an new ConfigurableString through Smart Dashboard.
+     * @param name the name of the Smart Dashboard key.
+     * @param defualtValue the initilization value
+     */
+    public ConfigurableString(String name, String defualtValue) {
+        this.name = name;
+        this.defualtValue = defualtValue;
+        SmartDashboard.putString(name, defualtValue);
+    }
+
+    public String get() {
+        return SmartDashboard.getString(name, defualtValue);
+    }
+}

src/main/java/frc4388/utility/controller/VirtualController.java

@@ -0,0 +1,145 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+/**
+ * A virtual controller that can be bound like an standard controller.
+ * @author Zachary Wilke
+ */
+public class VirtualController extends GenericHID {
+    private short m_buttonStates = 0; 
+    private short m_buttonStatesLastFrame = 0; 
+    private double[] m_axes = new double[6];
+    private short[] m_pov = new short[1];
+
+    /**
+     * Create an virtual controller
+     * @param port virtual port (merely a formality).
+     */
+    public VirtualController(int port) {
+        super(port);
+    }
+
+    /**
+     * Set the curent inputs to the new frames.
+     * @param axes joystick axes, (i.e. joysticks and triggers).
+     * @param buttonFlags the bit packed button states.
+     * @param pov the array of dpads.
+     */
+    public void setFrame(double[] axes, short buttonFlags, short[] pov) {
+        m_axes = axes;
+        setOutputs(buttonFlags);
+        m_pov = pov;
+    }
+
+    /**
+     * Zero outs the controls.
+     */
+    public void zeroControls() {
+        m_axes = new double[6];
+        m_buttonStates = 0;
+        m_buttonStatesLastFrame = 0;
+        m_pov = new short[] {-1};
+    }
+
+    /**
+     * Gets the value of a bitflag from an int
+     * @param value int to search
+     * @param index index of bit
+     * @return if the bit is set
+     */
+    public static boolean getFlag(int value, int index) {
+        return ((value & 1 << index) != 0);
+    }
+    
+    @Override
+    public boolean getRawButton(int button) { // man why are buttons indexed at 1.
+        return getFlag(m_buttonStates, button - 1);
+    }
+
+    @Override
+    public boolean getRawButtonPressed(int button) {  
+        return (!getFlag(m_buttonStatesLastFrame, button - 1) && getRawButton(button));
+    }
+    
+    @Override
+    public boolean getRawButtonReleased(int button) {
+        return (getFlag(m_buttonStatesLastFrame, button - 1) && !getRawButton(button));
+    }
+
+    @Override
+    public double getRawAxis(int axis) {
+        return m_axes[axis];
+    }
+    
+    @Override
+    public int getPOV(int pov) {
+        return m_pov[pov];
+    }
+
+    @Override
+    public int getAxisCount() {
+        return m_axes.length;
+    }
+
+    @Override
+    public int getPOVCount() {
+        return m_pov.length;
+    }
+
+    @Override
+    public int getButtonCount() {
+        return 10;
+    }
+
+    @Override
+    public boolean isConnected() {
+        return true;
+    }
+    
+    @Override
+    public HIDType getType() {
+        return HIDType.kXInputGamepad;
+    }
+
+    @Override
+    public String getName() {
+        return "Virtual Controller";
+    }
+
+    @Override
+    public int getAxisType(int axis) {
+        return 1; /* ! Warning, does not return accurate data.
+                  Hopefully this isn't a problem            */
+    }
+
+    /** 
+     * Use {@link VirtualController#setFrame} or {@link VirtualController#setOutputs}.
+     * this is an no-op overide.
+     */
+    @Override
+    public void setOutput(int outputNumber, boolean value) {
+        // do not use
+        //m_buttonStatesLastFrame[outputNumber - 1] = m_buttonStates[outputNumber - 1];
+        //m_buttonStates[outputNumber - 1] = value;
+    }
+
+    /**
+     * Set buttons from a packed int, if you want to set joysticks and dpad use {@link VirtualController#SetFrame} 
+     */    
+    @Override
+    public void setOutputs(int value) {
+        m_buttonStatesLastFrame = m_buttonStates;
+        m_buttonStates = (short) value;
+    }
+
+    /**
+     * Why are you Setting rumble on an virtual controller?
+     * @param type the rumble type (even though it won't do anything)
+     * @param value the rumble strength (always multiplyed by 0.0)
+     */
+    @Override
+    public void setRumble(RumbleType type, double value) {
+        System.out.println("Why are you Setting rumble on an virtual controller?");
+    }
+}

src/main/java/frc4388/utility/controller/XboxTriggerButton.java

@@ -1,13 +1,13 @@
 package frc4388.utility.controller;
 
-import edu.wpi.first.wpilibj2.command.button.Button;
+//import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 /**
  * Mapping for the Xbox controller triggers to allow triggers to be defined as
  * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger 
  * exceeds a tolerance defined in {@link XboxController}.
  */
-public class XboxTriggerButton extends Button {
+public class XboxTriggerButton {//extends Trigger {
 	public static final int RIGHT_TRIGGER = 0;
 	public static final int LEFT_TRIGGER = 1;
 	public static final int RIGHT_AXIS_UP_TRIGGER = 2;

vendordeps/NavX.json

@@ -1,17 +1,18 @@
 {
     "fileName": "NavX.json",
-    "name": "KauaiLabs_navX_FRC",
+    "name": "NavX",
-    "version": "2023.0.4",
+    "version": "2024.1.0",
     "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "frcYear": "2024",
     "mavenUrls": [
-        "https://dev.studica.com/maven/release/2023/"
+        "https://dev.studica.com/maven/release/2024/"
     ],
-    "jsonUrl": "https://dev.studica.com/releases/2023/NavX.json",
+    "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json",
     "javaDependencies": [
         {
             "groupId": "com.kauailabs.navx.frc",
             "artifactId": "navx-frc-java",
-            "version": "2023.0.4"
+            "version": "2024.1.0"
         }
     ],
     "jniDependencies": [],
@@ -19,7 +20,7 @@
         {
             "groupId": "com.kauailabs.navx.frc",
             "artifactId": "navx-frc-cpp",
-            "version": "2023.0.4",
+            "version": "2024.1.0",
             "headerClassifier": "headers",
             "sourcesClassifier": "sources",
             "sharedLibrary": false,

vendordeps/Phoenix5.json

@@ -0,0 +1,151 @@
+{
+    "fileName": "Phoenix5.json",
+    "name": "CTRE-Phoenix (v5)",
+    "version": "5.33.1",
+    "frcYear": 2024,
+    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+    "mavenUrls": [
+        "https://maven.ctr-electronics.com/release/"
+    ],
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json",
+    "requires": [
+        {
+            "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+            "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6.  Please add the Phoenix 6 vendordep before adding Phoenix 5.",
+            "offlineFileName": "Phoenix6.json",
+            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-java",
+            "version": "5.33.1"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-java",
+            "version": "5.33.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.33.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.33.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-cpp",
+            "version": "5.33.1",
+            "libName": "CTRE_Phoenix_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-cpp",
+            "version": "5.33.1",
+            "libName": "CTRE_Phoenix",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.33.1",
+            "libName": "CTRE_PhoenixCCI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "5.33.1",
+            "libName": "CTRE_Phoenix_WPISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "api-cpp-sim",
+            "version": "5.33.1",
+            "libName": "CTRE_PhoenixSim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.33.1",
+            "libName": "CTRE_PhoenixCCISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ]
+}

vendordeps/Phoenix6.json

@@ -0,0 +1,339 @@
+{
+    "fileName": "Phoenix6.json",
+    "name": "CTRE-Phoenix (v6)",
+    "version": "24.3.0",
+    "frcYear": 2024,
+    "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+    "mavenUrls": [
+        "https://maven.ctr-electronics.com/release/"
+    ],
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json",
+    "conflictsWith": [
+        {
+            "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a",
+            "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.",
+            "offlineFileName": "Phoenix6And5.json"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-java",
+            "version": "24.3.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "24.3.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "24.3.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "24.3.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonFX",
+            "version": "24.3.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "24.3.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simPigeonIMU",
+            "version": "24.3.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simCANCoder",
+            "version": "24.3.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "24.3.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANcoder",
+            "version": "24.3.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProPigeon2",
+            "version": "24.3.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-cpp",
+            "version": "24.3.0",
+            "libName": "CTRE_Phoenix6_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "24.3.0",
+            "libName": "CTRE_PhoenixTools",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "24.3.0",
+            "libName": "CTRE_Phoenix6_WPISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "24.3.0",
+            "libName": "CTRE_PhoenixTools_Sim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "24.3.0",
+            "libName": "CTRE_SimTalonSRX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonFX",
+            "version": "24.3.0",
+            "libName": "CTRE_SimTalonFX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "24.3.0",
+            "libName": "CTRE_SimVictorSPX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simPigeonIMU",
+            "version": "24.3.0",
+            "libName": "CTRE_SimPigeonIMU",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simCANCoder",
+            "version": "24.3.0",
+            "libName": "CTRE_SimCANCoder",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "24.3.0",
+            "libName": "CTRE_SimProTalonFX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANcoder",
+            "version": "24.3.0",
+            "libName": "CTRE_SimProCANcoder",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProPigeon2",
+            "version": "24.3.0",
+            "libName": "CTRE_SimProPigeon2",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ]
+}

vendordeps/WPILibNewCommands.json

@@ -3,6 +3,7 @@
   "name": "WPILib-New-Commands",
   "version": "1.0.0",
   "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+  "frcYear": "2024",
   "mavenUrls": [],
   "jsonUrl": "",
   "javaDependencies": [