Merge pull request #11 from Team4388/kcmt-changes

Kcmt changes

.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
 }

README.md

@@ -1,2 +1,34 @@
-# Robot-Essentials
+# FIRST FRC 2024 Season (Crescendo) "Kickdrum" Robot Code.
- Basic code for any Ridgebotics robot project
+Code for our robot named "Kickdrum"
+
+## Driving instructions
+
+### Driver (Port 0)
+**A**: Reset the gryo. 
+
+**Left Bumper**: Gear shift down.
+
+**Right Bumper**: Gear shift up.
+
+### Operator (Port 1)
+
+**Right Bumper**: Moves out the intake and starts intaking a note, when the note enters the intake move the intake back into the robot, then idles the shooter.
+
+**Left Bumper**: Spins up the shooter to speaker speed.
+
+**A**: *Outtakes* the *intake*. If the intake is in the robot then it will pass it off to the shooter (and its recomended that the shooter is at speaker speed when it gets handed off), if its outside the robot it will just eject it onto the floor.
+
+**Right Trigger**: While held it moves the climbers up.
+
+**Left Trigger**: While held it moves the climbers down.
+
+### Programer (Port 2)
+
+**Left Bumper**: While pressed it records using the new autonomus recording and playback system, and when released it saves the autonomus to the robot under the name that you put into shuffleboard in the "Auto Playback Name" section. (**Warning**: If the robot gets reimaged (Not redeployed) it will lose all of the auto files and you are stuck using a tool like SCP to send the autos back to the robot from this repository)
+
+**Right Bumper**: When pressed it loads the auto from the "Auto Playback Name" in shuffleboard with the new autonomus recording and playback system, and plays it back. If the file you put in doesn't exist in autos/ in the home directory of the robot it will print the error to console but the robot will remain functional. (**Warning**: If the robot gets reimaged (Not redeployed) it will lose all of the auto files and you are stuck using a tool like SCP to send the autos back to the robot from this repository)
+
+### Shuffleboard
+
+**Auto Playback Name**: a string value of the auto name you wish to load or save to, this gets read at the start of the atonomus phase and loads the corisponding auto from ~/autos/ on the robot
+

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.2"
 }
 
-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,26 +23,39 @@ 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[] GEARS = {0.25, 0.5, 1.0};
-        public static final double FAST_SPEED = 1.0;
+
-        public static final double TURBO_SPEED = 4.0;
+        public static final double SLOW_SPEED = 0.25;
+        public static final double FAST_SPEED = 0.5;
+        public static final double TURBO_SPEED = 1.0;
+    
+        public static final class DefaultSwerveRotOffsets {
+          public static final double FRONT_LEFT_ROT_OFFSET  =  0.36962890625  + 0.5;
+          public static final double FRONT_RIGHT_ROT_OFFSET =  0.61474609375  + 0.5;
+          public static final double BACK_LEFT_ROT_OFFSET   = -0.227294921875 + 0.5;
+          public static final double BACK_RIGHT_ROT_OFFSET  =  0.60595703125  + 0.5;
+        }
 
         public static final class IDs {
-          public static final int LEFT_FRONT_WHEEL_ID = 2;
+            public static final int RIGHT_FRONT_WHEEL_ID = 2;
-          public static final int LEFT_FRONT_STEER_ID = 3;
+            public static final int RIGHT_FRONT_STEER_ID = 3;
-          public static final int LEFT_FRONT_ENCODER_ID = 10;
+            public static final int RIGHT_FRONT_ENCODER_ID = 10;
             
-          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_STEER_ID = 7;
@@ -51,12 +64,17 @@ public final class Constants {
             public static final int RIGHT_BACK_WHEEL_ID = 8;  
             public static final int RIGHT_BACK_STEER_ID = 9;
             public static final int RIGHT_BACK_ENCODER_ID = 13;
+
+            public static final int DRIVE_PIGEON_ID = 14;
         }
     
         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(50, 0.0, 0.32, 0.0, 0, 0.0);
+
+            public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
+
         }
     
         public static final class AutoConstants {
@@ -70,15 +88,13 @@ public final class Constants {
         }
     
         public static final class Conversions {
-          public static final int CANCODER_TICKS_PER_ROTATION = 4096;
+            public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22;
-    
+            public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.5;
-          public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
-          public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
         
             public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
             public static final double MOTOR_REV_PER_STEER_REV = 12.8;
         
-          public static final double TICKS_PER_MOTOR_REV = 2048;
+            public static final double TICKS_PER_MOTOR_REV = 0.5;
             public static final double WHEEL_DIAMETER_INCHES = 3.9;
             public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
         
@@ -92,13 +108,13 @@ public final class Constants {
         }
     
         public static final class Configurations {
-          public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
+            public static final double OPEN_LOOP_RAMP_RATE = 0.2;
-          public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
+            public static final double CLOSED_LOOP_RAMP_RATE = 0.2;
-          public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
+            public static final double NEUTRAL_DEADBAND = 0.04;
         }
     
-        public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
+        public static final double MAX_SPEED_FEET_PER_SECOND = 20.4;
-        public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
+        public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
     
         // dimensions
         public static final double WIDTH = 18.5;
@@ -112,29 +128,18 @@ public final class Constants {
       }
     
     public static final class VisionConstants {
-        public static final String NAME = "photonCamera";
+        // public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
 
-        public static final int LIME_HIXELS = 640;
+        public static final Translation2d RedSpeakerCenter = new Translation2d(7.914259, -3.221609);
-        public static final int LIME_VIXELS = 480;
+        public static final Translation2d BlueSpeakerCenter = new Translation2d(-8.308975, 1.442593);
 
-        public static final double H_FOV = 59.6;
+        public static final double SpeakerBubbleDistance = 3;
-        public static final double V_FOV = 45.7;
+        public static final double targetPosDistance = 1.5;
-    
-        public static final double LIME_HEIGHT = 6.0;
-        public static final double LIME_ANGLE = 55.0;
-    
-        // public static final double HIGH_TARGET_HEIGHT = 46.0;
-        public static final double HIGH_TAPE_HEIGHT = 44.0;
-        
-        // public static final double MID_TARGET_HEIGHT = 34.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 class DriveConstants {
-        public static final int DRIVE_PIGEON_ID = 6;
+        
 
         public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
     }
@@ -145,9 +150,38 @@ 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;
+        public static final int XBOX_PROGRAMMER_ID = 2;
         public static final double LEFT_AXIS_DEADBAND = 0.1;
 
     }

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

@@ -12,7 +12,7 @@ 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 +25,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 +41,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 +49,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 +122,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,39 @@
 
 package frc4388.robot;
 
-import edu.wpi.first.wpilibj.Joystick;
+// Drive Systems
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.wpilibj.GenericHID;
+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;
+
+// Commands
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 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 frc4388.robot.commands.Swerve.JoystickPlayback;
+// Autos
-import frc4388.robot.commands.Swerve.JoystickRecorder;
+import frc4388.robot.commands.Intake.ArmIntakeIn;
-import frc4388.robot.subsystems.LED;
+import frc4388.utility.controller.VirtualController;
+import frc4388.robot.commands.Swerve.neoJoystickPlayback;
+import frc4388.robot.commands.Swerve.neoJoystickRecorder;
+
+// Subsystems
+import frc4388.robot.subsystems.Climber;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Limelight;
+import frc4388.robot.subsystems.Shooter;
+// import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.LEDPatterns;
+
-import frc4388.utility.controller.DeadbandedXboxController;
+// Utilites
-import frc4388.utility.controller.IHandController;
+import frc4388.utility.DeferredBlock;
-import frc4388.utility.controller.XboxController;
+import frc4388.utility.configurable.ConfigurableString;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -31,32 +50,120 @@ 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();
+
+    private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor);
 
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
                                                                   m_robotMap.rightFront,
                                                                   m_robotMap.leftBack,
                                                                   m_robotMap.rightBack,
-                                                                  m_robotMap.gyro);
                                               
+                                                                  m_robotMap.gyro);
 
     /* 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(OIConstants.XBOX_PROGRAMMER_ID);
+    
+    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);
+
+    /* 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);
+
+    // ! Teleop Commands
+
+    private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
+        new InstantCommand(() -> m_robotIntake.PIDIn()),
+        new InstantCommand(() -> m_robotShooter.idle())
+    );
+    
+    private SequentialCommandGroup intakeNotePullInIdle = new SequentialCommandGroup(
+        intakeToShootStuff, intakeToShoot, 
+        new InstantCommand(() -> m_robotShooter.idle())
+    );
+
+    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)
+    );
+
+    // ! /*  Autos */
+    private String lastAutoName = "four_note_taxi_kracken.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 neoJoystickPlayback ampShoot = new neoJoystickPlayback(m_robotSwerveDrive, "Amp_shoot.auto",
+        new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
+        false, true);
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
         configureButtonBindings();        
+        configureVirtualButtonBindings();
+        new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip());
+        DriverStation.silenceJoystickConnectionWarning(true);
+        // CameraServer.startAutomaticCapture();
 
         /* Default Commands */
+        // ! Swerve Drive Default Command (Regular Rotation)
         // drives the robot with a two-axis input from the driver controller
+        m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+            m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
+                                            getDeadbandedDriverController().getRight(),
+                                true);
+        }, m_robotSwerveDrive)
+        .withName("SwerveDrive DefaultCommand"));
+        m_robotSwerveDrive.setToSlow();
+
+        // ! Swerve Drive One Module Test
+        // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+        //     m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft());
+        // }
+
+        // ! 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));
+
+        // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+        //     m_robotSwerveDrive.driveWithInput(
+        //                                     getDeadbandedDriverController().getLeft(), 
+        //                                     getDeadbandedDriverController().getRight(),
+        //                                     true);
+        // }, m_robotSwerveDrive));
+
+
+
+
     }
 
     /**
@@ -66,26 +173,123 @@ 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
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
-        //     .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
-        //                                     () -> getDeadbandedDriverController().getLeftX(),
-        //                                     () -> getDeadbandedDriverController().getLeftY(),
-        //                                     () -> getDeadbandedDriverController().getRightX(),
-        //                                     () -> getDeadbandedDriverController().getRightY(),
-        //                                     "Blue1Path.txt"))
-        //     .onFalse(new InstantCommand());
 
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+        // ? /* Driver Buttons */
-        //     .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
+
-        //     .onFalse(new InstantCommand()); 
+        DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip()));
+            
+        // ! /* Speed */
+        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
+            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
+        
+        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()));
+          
+        // ?  /* Operator Buttons */
+            
+        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
+        .onTrue(new InstantCommand(() -> m_robotIntake.PIDIn()))
+            .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
+            
+        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(intakeNotePullInIdle)
+            .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(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)));
+        
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() != -1)
+            .onTrue(ampShoot)
+            .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter, m_robotSwerveDrive));
+        
+        // ? /* Programer Buttons (Controller 3)*/
+
+        // * /* 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());
+    }
+    
+    /**
+     * 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.
+         */
+        
+         // ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto.
 
-        /* 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));
     }
 
     /**
@@ -94,13 +298,22 @@ public class RobotContainer {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        // no auto
+        //no auto
-        return new InstantCommand();
+        return autoPlayback;
+        //return playbackChooser.getCommand();
+        // return new Command() {};
     }
 
     /**
-     * Add your docs here.
+     * 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)));
+    }
+
     public DeadbandedXboxController getDeadbandedDriverController() {
         return this.m_driverXbox;
     }
@@ -108,4 +321,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,16 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.InvertType;
+import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix6.hardware.Pigeon2;
-import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.PigeonIMU;
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
+// import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import frc4388.robot.Constants.LEDConstants;
+// 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,7 +25,7 @@ import frc4388.utility.RobotGyro;
  * testing and modularization.
  */
 public class RobotMap {
-    private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
+    private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON_ID);
     public RobotGyro gyro = new RobotGyro(m_pigeon2);
     
     public SwerveModule leftFront;
@@ -39,58 +39,48 @@ 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);
-
 
     /* Swreve Drive Subsystem */
-    public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
+    public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
-    public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
+    public final TalonFX leftFrontSteer = new 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 TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
-    public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
+    public final TalonFX rightFrontSteer = new 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 TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
-    public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
+    public final TalonFX leftBackSteer = new 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 TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
+    public final TalonFX rightBackSteer = new 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() {
+    }
+
     void configureDriveMotorControllers() {
-        // config factory default
-        leftFrontWheel.configFactoryDefault();
-        leftFrontSteer.configFactoryDefault();
-        
-        rightFrontWheel.configFactoryDefault();
-        rightFrontSteer.configFactoryDefault();
-        
-        leftBackWheel.configFactoryDefault();
-        leftBackSteer.configFactoryDefault();
-        
-        rightBackWheel.configFactoryDefault();
-        rightBackSteer.configFactoryDefault();
-
-        // 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, -181.230469);
+        this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
-        this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
+        this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
-        this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
+        this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
-        this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
+        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/Intake/ArmIntakeIn.java

@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Shooter;
+
+public class ArmIntakeIn extends Command {
+  /** Creates a new ArmIntakeIn. */
+  private Intake robotIntake;
+  private Shooter robotShooter;
+
+  public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.robotIntake = robotIntake;
+    this.robotShooter = robotShooter;
+
+    addRequirements(this.robotIntake, this.robotShooter);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    robotIntake.PIDOut();
+    robotIntake.spinIntakeMotor();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return robotIntake.getIntakeLimitSwitchState();
+    // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) 
+    // {
+    //   return !true==true;
+    // } 
+    // else 
+    // {
+    //   return !false==!(!(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

@@ -9,11 +9,11 @@ import java.io.FileNotFoundException;
 import java.util.ArrayList;
 import java.util.Scanner;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.UtilityStructs.TimedOutput;
 
-public class JoystickPlayback extends CommandBase {
+public class JoystickPlayback extends Command {
   private final SwerveDrive            swerve;
   private       String                 filename;
   private       int                    mult       = 1;
@@ -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

@@ -11,11 +11,11 @@ import java.util.ArrayList;
 import java.util.function.Supplier;
 
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.UtilityStructs.TimedOutput;
 
-public class JoystickRecorder extends CommandBase {
+public class JoystickRecorder extends Command {
   public  final SwerveDrive            swerve;
 
   public  final Supplier<Double>       leftX;
@@ -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/Climber.java

@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.ClimbConstants;
+
+public class Climber extends SubsystemBase {
+  /** Creates a new Climber. */
+  TalonFX climbMotor;
+
+  public Climber(TalonFX climbMotor) {
+    this.climbMotor = climbMotor;
+    this.climbMotor.setInverted(true);
+
+    climbMotor.setNeutralMode(NeutralModeValue.Brake);
+    
+    var slot0Configs = new Slot0Configs();
+    slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output
+    slot0Configs.kI = 0.0; // no output for integrated error
+    slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output
+
+    climbMotor.getConfigurator().apply(slot0Configs);
+  }
+
+  public void climbOut() {
+    //PositionVoltage request = new PositionVoltage(0);
+    //climbMotor.setControl(request.withPosition(-520)); 
+    
+    climbMotor.set(ClimbConstants.CLIMB_OUT_SPEED);
+  }
+
+  public void climbIn() {
+    //PositionVoltage request = new PositionVoltage(-520);
+    //climbMotor.setControl(request.withPosition(0)); 
+    climbMotor.set(ClimbConstants.CLIMB_IN_SPEED);
+  }
+
+  public void stopClimb() {
+    climbMotor.set(0.d);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    //SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue());
+  }
+}

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

@@ -7,7 +7,8 @@
 
 package frc4388.robot.subsystems;
 
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.hardware.TalonFX;
 
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -26,23 +27,25 @@ public class DiffDrive extends SubsystemBase {
 
   private RobotTime m_robotTime = RobotTime.getInstance();
 
-  private WPI_TalonFX m_leftFrontMotor;
+  private TalonFX m_leftFrontMotor;
-  private WPI_TalonFX m_rightFrontMotor;
+  private TalonFX m_rightFrontMotor;
-  private WPI_TalonFX m_leftBackMotor;
+  private TalonFX m_leftBackMotor;
-  private WPI_TalonFX m_rightBackMotor;
+  private TalonFX m_rightBackMotor;
   private DifferentialDrive m_driveTrain;
   private RobotGyro m_gyro;
 
   /**
    * Add your docs here.
    */
-  public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor,
+  public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
-      WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
+      TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
 
     m_leftFrontMotor = leftFrontMotor;
     m_rightFrontMotor = rightFrontMotor;
     m_leftBackMotor = leftBackMotor;
     m_rightBackMotor = rightBackMotor;
+    m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
+    m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false));
     m_driveTrain = driveTrain;
     m_gyro = gyro;
   }

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

@@ -0,0 +1,110 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.IntakeConstants;
+import frc4388.utility.Gains;
+
+public class Intake extends SubsystemBase {
+  private TalonFX intakeMotor;
+  private TalonFX pivotMotor;
+  public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS;
+
+  /** Creates a new Intake. */
+  public Intake(TalonFX intakeMotor, TalonFX pivotMotor) {
+    this.intakeMotor = intakeMotor;
+    this.pivotMotor = pivotMotor;
+
+    intakeMotor.getConfigurator().apply(new TalonFXConfiguration());
+    pivotMotor.getConfigurator().apply(new TalonFXConfiguration());
+
+    intakeMotor.setNeutralMode(NeutralModeValue.Brake);
+    pivotMotor.setNeutralMode(NeutralModeValue.Brake);
+
+    // in init function, set slot 0 gains
+    var slot0Configs = new Slot0Configs();
+    slot0Configs.kP = 1.3; // An error of 0.5 rotations results in 12 V output
+    slot0Configs.kI = 0.0; // no output for integrated error
+    slot0Configs.kD = 0.21; // A velocity of 1 rps results in 0.1 V output
+
+    pivotMotor.getConfigurator().apply(slot0Configs);
+  }
+
+  // ! Talon Methods
+  public void PIDIn() {
+    PIDPosition(0);
+  }
+
+  public void PIDOut() {
+    PIDPosition(-53);
+  }
+
+  public void PIDPosition(double pos) {
+    PositionVoltage request = new PositionVoltage(pos);
+    pivotMotor.setControl(request);
+  }
+
+  public void handoff() {
+    intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
+  }
+
+  public void spinIntakeMotor() {
+    intakeMotor.set(IntakeConstants.INTAKE_SPEED);
+  }
+
+  public void spinIntakeMotor(double speed) {
+    intakeMotor.set(speed);
+  }
+
+  public boolean getIntakeLimitSwitchState() {
+    return intakeMotor.getForwardLimit().getValue().value == 0;
+  }
+
+  public void setPivotEncoderPosition(double val) {
+    pivotMotor.setPosition(val);
+  }
+  
+  public void stopIntakeMotors() {
+    intakeMotor.set(0);
+  }
+
+  public void stopArmMotor() {
+    pivotMotor.set(0);
+  }
+
+  public void stop() {
+    intakeMotor.set(0);
+    pivotMotor.set(0);
+  }
+
+  public double getArmPos() {
+    return pivotMotor.getPosition().getValue();
+  }
+
+  public void resetArmPosition() {
+    if (getIntakeLimitSwitchState()) {
+     // talonPivot.setPosition(0);
+    }
+  }
+
+  public void ampPosition() {
+    PIDPosition(-59); //TODO: Find actual value
+  } 
+
+  public void ampOuttake(double speed) {
+    spinIntakeMotor(speed);
+  }
+
+  @Override
+  public void periodic() {
+    resetArmPosition();
+  }
+}

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

@@ -7,6 +7,8 @@
 
 package frc4388.robot.subsystems;
 
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -20,44 +22,69 @@ import frc4388.utility.LEDPatterns;
  */
 public class LED extends SubsystemBase {
 
-  private LEDPatterns m_currentPattern;
+  static AddressableLED m_led;
-  private Spark m_LEDController;
+  static AddressableLEDBuffer m_ledBuffer;
-
+  static LED m_self;
   /**
    * Add your docs here.
    */
-  public LED(Spark LEDController){
+
-    m_LEDController = LEDController;
+  public LED(){
-    setPattern(LEDConstants.DEFAULT_PATTERN);
+    if(m_self != null)
-    updateLED();
+      return;
+    m_led = new AddressableLED(9);
+    m_ledBuffer = new AddressableLEDBuffer(10);
+    m_led.setLength(m_ledBuffer.getLength());
+    m_led.setData(m_ledBuffer);
+    m_led.start();
     System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
   }
-
+  public static LED getInstance() {
+    if(m_self == null)
+      m_self = new LED();
+    return m_self;
+  }
   @Override
   public void periodic(){
-    SmartDashboard.putNumber("LED", m_currentPattern.getValue());
+    //gamermode();
+    //SmartDashboard.putNumber("LED", m_currentPattern.getValue());
+    return;
+  }
+  static int firstcolor = 0;
+  static void gamermode() {
+      for(int i = 0; i < m_ledBuffer.getLength(); i++) {
+          final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
+          setLEDHSV(i, hue, 255, 128);
+      }
+      firstcolor +=3;
+      firstcolor %= 180;
+  }
+  /**
+   * Add your docs here.
+   */
+  public static void updateLED (){
+    gamermode();
+   // m_LEDController.set(m_currentPattern.getValue());
   }
 
   /**
    * Add your docs here.
    */
-  public void updateLED(){
+  public static void setLEDRGB(int lednum, int r, int g, int b){
-    m_LEDController.set(m_currentPattern.getValue());
+    m_ledBuffer.setRGB(lednum, r, g, b);
+    //m_currentPattern = pattern;
+   // m_LEDController.set(m_currentPattern.getValue());
   }
-
+  public static void setLEDHSV(int lednum, int hue, int sat, int val){
-  /**
+    m_ledBuffer.setRGB(lednum, hue, sat, val);
-   * Add your docs here.
+    //m_currentPattern = pattern;
-   */
+   // m_LEDController.set(m_currentPattern.getValue());
-  public void setPattern(LEDPatterns pattern){
-    m_currentPattern = pattern;
-    m_LEDController.set(m_currentPattern.getValue());
   }
-
   /**
    * Add your docs here.
    * @return
    */
-  public LEDPatterns getPattern() {
+  public AddressableLEDBuffer getBuffer() {
-    return m_currentPattern;
+    return m_ledBuffer;
   }
 }

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

@@ -3,163 +3,80 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc4388.robot.subsystems;
-
-import java.io.IOException;
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Optional;
-
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.common.hardware.VisionLEDMode;
-import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
-import org.photonvision.targeting.TargetCorner;
-
-import edu.wpi.first.apriltag.AprilTag;
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
 import frc4388.robot.Constants.VisionConstants;
 
+// Look at vvv for networktables stuff
+// https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data
+
 public class Limelight extends SubsystemBase {
 
-  private PhotonCamera cam;
+  // // [X, Y, Z, Roll, Pitch, Yaw]
-  private PhotonPoseEstimator photonPoseEstimator;
+  // private double[] cameraPose;
+  // private boolean isTag;
 
-  private boolean lightOn;
+  // private Pose2d pose;
+  // private boolean isNearSpeaker;
 
-  /** Creates a new Limelight. */
+  // public boolean getIsTag() {
-  public Limelight() {
+  //   return isTag;
-    cam = new PhotonCamera(VisionConstants.NAME);
+  // }
-    cam.setDriverMode(false);
-  }
 
-  public void setLEDs(boolean on) {
+  // private void update() {
-    lightOn = on;
+  //   SmartDashboard.putBoolean("Apriltag", isTag);
-    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+  //   if(!isTag){
-  }
+  //     return;
+  //   }
 
-  public void toggleLEDs() {
+  //   double x = cameraPose[0];
-    lightOn = !lightOn;
+  //   double y = cameraPose[1];
-    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+  //   double yaw = cameraPose[5];
-  }
 
-  public void setDriverMode(boolean driverMode) {
+  //   Rotation2d rot = Rotation2d.fromDegrees(yaw);
-    cam.setDriverMode(driverMode);
-  }
 
-  public void setToLimePipeline() {
+  //   pose = new Pose2d(x, y, rot);
-    cam.setPipelineIndex(1);
-    setLEDs(true);
-  }
 
-  public void setToAprilPipeline() {
+  //   boolean isRed = DriverStation.getAlliance().get() == Alliance.Red;
-    cam.setPipelineIndex(0);
-    setLEDs(false);
-  }
     
-  public PhotonTrackedTarget getAprilPoint() {
+  //   double distance;
-    if (!cam.isConnected()) return null;
 
-    PhotonPipelineResult result = cam.getLatestResult();
+  //   if(isRed){
+  //     distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter);
+  //   }else{
+  //     distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter);
+  //   }
     
-    if (!result.hasTargets()) return null;
+  //   isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance;
 
-    return result.getBestTarget();
+  //   //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
-  }
+  //   //SmartDashboard.putNumber("speakerDistance", distance);
+  // }
 
-  private List<TargetCorner> getAprilCorners() {
+  // public Pose2d getPose() {
-    if (!cam.isConnected()) return null;
+  //   return pose;
+  // }
 
-    PhotonPipelineResult result = cam.getLatestResult();
+  // public boolean isNearSpeaker() {
+  //   return isNearSpeaker;
+  // }
 
-    if (!result.hasTargets()) return null;
+  // @Override
+  // public void periodic() {
+  //   // This method will be called once per scheduler run
 
-    return result.getBestTarget().getDetectedCorners();
+  //   //isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0;
-  }
+  //   //double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]);
 
-  public double getAprilSkew() {
+  //   //if(newPose != cameraPose){
-    List<TargetCorner> corners = getAprilCorners();
+  //   //  cameraPose = newPose;
-    ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
+  //     //update();
-
+  //   //}
-    if (bottomSide == null) return 0;
+  // }
-
-    TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
-    TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
-
-    return bottomLeft.y - bottomRight.y;
-  }
-
-  private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
-    if (box == null) return null;
-
-    ArrayList<TargetCorner> bottomSide = new ArrayList<>();
-
-    TargetCorner l1 = new TargetCorner(-1, -1);
-    TargetCorner l2 = new TargetCorner(-1, -1);
-    
-    for (TargetCorner c : box) {
-      if (c.y > l1.y) l1 = c;
-    }
-
-    for (TargetCorner c : box) {
-      if (c.y == l1.y) continue;
-      if (c.y > l2.y) l2 = c;
-    }
-
-    bottomSide.add(l1);
-    bottomSide.add(l2);
-
-    return bottomSide;
-  }
-
-  public double getDistanceToApril() {    
-    PhotonTrackedTarget aprilPoint = getAprilPoint();
-    if (aprilPoint == null) return -1;
-
-    double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
-    double theta = 35.0 + aprilPoint.getPitch();
-
-    double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
-    return distanceToApril;
-  }
-
-  public PhotonTrackedTarget getLowestTape() {
-    if (!cam.isConnected()) return null;
-
-    PhotonPipelineResult result = cam.getLatestResult();
-
-    if (!result.hasTargets()) return null;
-
-    ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
-
-    PhotonTrackedTarget lowest = points.get(0);
-    for (PhotonTrackedTarget point : points) {
-      if (point.getPitch() < lowest.getPitch()) {
-        lowest = point;
-      }
-    }
-
-    return lowest;
-  }
-
-  public double getDistanceToTape() {    
-    PhotonTrackedTarget tapePoint = getLowestTape();
-    if (tapePoint == null) return -1;
-
-    double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
-    double theta = 35.0 + tapePoint.getPitch();
-
-    double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
-    return distanceToTape;
-  }
-
-  @Override
-  public void periodic() {}
 }

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

@@ -0,0 +1,114 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.motorcontrol.Talon;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.IntakeConstants;
+import frc4388.robot.Constants.ShooterConstants;
+
+import frc4388.robot.subsystems.Limelight;
+
+// import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+// import com.ctre.phoenix.motorcontrol.NeutralMode;
+
+
+public class Shooter extends SubsystemBase {
+  
+  private TalonFX leftShooter;
+  private TalonFX rightShooter;
+
+  private Limelight limelight;
+
+  private int spinMode = 0;
+  // 0 = Stop / Coast
+  // 1 = Idle
+  // 2 = Idle Near Speaker
+  // 3 = Spin
+  // 4 = SingleSpin
+
+  private double smartDashboardShooterSpeed;
+
+  /** Creates a new Shooter. */
+  public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) {
+    leftShooter  = leftTalonFX;
+    rightShooter = rightTalonFX;
+
+    limelight = tmplimelight;
+
+    leftShooter.setNeutralMode(NeutralModeValue.Coast);
+    rightShooter.setNeutralMode(NeutralModeValue.Coast);
+    SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
+
+  }
+
+  public Shooter(TalonFX leftShooter) {
+    this.leftShooter = leftShooter;
+    leftShooter.setNeutralMode(NeutralModeValue.Coast);
+  }
+
+  public void singleSpin() {
+    leftShooter.set(1.0);
+    spinMode = 4;
+  }
+
+  public void singleSpin(double speed) {
+    leftShooter.set(speed);
+    spinMode = 4;
+  }
+
+  public void spin() {
+    spin(smartDashboardShooterSpeed);
+    spinMode = 3;
+  }
+
+  public void spin(double speed) {
+    leftShooter.set(-speed);    
+    rightShooter.set(-speed);
+    spinMode = 3;
+  }
+
+  public void spin(double leftSpeed, double rightSpeed) {
+    leftShooter.set(leftSpeed);    
+    rightShooter.set(-rightSpeed);
+    spinMode = 3;
+  }
+
+  public void stop() {
+    spin(0.d);
+    spinMode = 0;
+  }
+
+  public void idle() {
+    spin(ShooterConstants.SHOOTER_IDLE);
+    spinMode = 1;
+  }
+
+  public int getMode(){
+    return spinMode;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  //  SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
+    //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
+    //smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
+
+    // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed.
+    // Else if the robot is not near the speaker, then set the speed back to idle.
+    // if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){
+    //   leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);    
+    //   rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
+    //   spinMode = 2;
+    // }else if(!limelight.isNearSpeaker() && spinMode == 2){
+    //   idle();
+    // }
+  }
+}

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

@@ -4,15 +4,19 @@
 
 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.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.Constants.SwerveDriveConstants.Conversions;
 import frc4388.utility.RobotGyro;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.utility.RobotUnits;
 
 public class  SwerveDrive extends SubsystemBase {
   
@@ -24,17 +28,23 @@ 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
+  private int gear_index;
+  private boolean stopped = false;
+
+  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+  public double rotSpeedAdjust = SwerveDriveConstants.MAX_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. */
@@ -45,20 +55,37 @@ public class SwerveDrive extends SubsystemBase {
     this.rightBack = rightBack;
     
     this.gyro = gyro;
-
+    reset_index();
     this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
   }
 
-  boolean stopped = false;
+  public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
+    // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
+    // rightStick.getAngle()
+    double speed = Math.sqrt(Math.pow(leftStick.getX(), 2) + Math.pow(leftStick.getY(), 2));
+    // System.out.println(ang);
+    // module.go(ang);
+    // Rotation2d rot = Rotation2d.fromRadians(ang);
+    Rotation2d rot = new Rotation2d(rightStick.getX(), rightStick.getY());
+    SwerveModuleState state = new SwerveModuleState(speed, rot);
+    module.setDesiredState(state);
+  }
+
   public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+
+    double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0;
+    SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction));
+
     if (fieldRelative) {
 
       double rot = 0;
       
+      // ! drift correction
       if (rightStick.getNorm() > 0.05) {
         rotTarget = gyro.getAngle();
-        rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
+        rot_correction = 0;
-        SmartDashboard.putBoolean("drift correction", false);
+        // rot = rightStick.getX();
+      //  SmartDashboard.putBoolean("drift correction", false);
         stopped = false;
       } else if(leftStick.getNorm() > 0.05) {
         if (!stopped) {
@@ -66,8 +93,9 @@ 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;
+        
+        // rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
 
       }
 
@@ -76,14 +104,82 @@ public class SwerveDrive extends SubsystemBase {
       // 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.
-    } else {
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1));
-      // Create robot-relative speeds.
+    } else {      // Create robot-relative speeds.
-      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+      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);
+        stopped = false;
+      } else if(leftStick.getNorm() > 0.05) {
+        if (!stopped) {
+          stopModules();
+          stopped = true;
+        }
+
+     //   SmartDashboard.putBoolean("drift correction", true);
+        // double rot_correction = ((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() * 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(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
+    } else {      // Create robot-relative speeds.
+      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
+    }
+    // setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
+  }
+
+  public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+
+    // Translation2d rightStick = new Translation2d(-rightX, rightY);
+    double rightX = rightStick.getX();
+    double rightY = rightStick.getY();
+
+    double rot_correction = 0;
+
+    // double rot_correction = ((rightStick.getAngle().getDegrees() - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
+
+    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) + rot_correction, 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.
@@ -111,12 +207,33 @@ public class SwerveDrive extends SubsystemBase {
   }
 
   public double getGyroAngle() {
-    return gyro.getAngle();
+    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,24 +246,45 @@ 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("RotTartget", rotTarget);
+  }
+
+  private void reset_index() {
+    gear_index = 0; // however we wish to initialize the gear (What gear does the robot start in?)
   }
 
   public void shiftDown() {
-    if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
+    if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
-      
+    int i = gear_index - 1;
-    } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
+    if (i == -1) i = 0;
-      this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
+    setPercentOutput(SwerveDriveConstants.GEARS[i]);
-    } else {
+    gear_index = i;
-      this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
   }
+
+  public void shiftUp() {
+    if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length) reset_index(); // If outof bounds: reset index
+    int i = gear_index + 1;
+    if (i == SwerveDriveConstants.GEARS.length) i = SwerveDriveConstants.GEARS.length - 1;
+    setPercentOutput(SwerveDriveConstants.GEARS[i]);
+    gear_index = i;
+  }
+
+  public void setPercentOutput(double speed) {
+    speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * speed;
+    gear_index = -1;
   }
 
   public void setToSlow() {
-    this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
+    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED;
     System.out.println("SLOW");
     System.out.println("SLOW");
     System.out.println("SLOW");
@@ -155,7 +293,7 @@ public class SwerveDrive extends SubsystemBase {
   }
 
   public void setToFast() {
-    this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
+    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED;
     System.out.println("FAST");
     System.out.println("FAST");
     System.out.println("FAST");
@@ -164,7 +302,7 @@ public class SwerveDrive extends SubsystemBase {
   }
 
   public void setToTurbo() {
-    this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
+    this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED;
     System.out.println("TURBO");
     System.out.println("TURBO");
     System.out.println("TURBO");
@@ -172,16 +310,6 @@ public class SwerveDrive extends SubsystemBase {
     System.out.println("TURBO");
   }
 
-  public void shiftUp() {
-    if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
-      this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
-    } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
-      this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
-    } else {
-      
-    }
-  }
-
   public void toggleGear(double angle) {
     if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
       this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
@@ -192,4 +320,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,126 @@
 
 package frc4388.robot.subsystems;
 
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix6.Utils;
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+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;
 
 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 final StatusSignal<Double> cc_pos;
+    // private final StatusSignal<Double> cc_vel;
+    // 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;
 
-        TalonFXConfiguration angleConfig = new TalonFXConfiguration();
+        var motorCfg = new TalonFXConfiguration()
-        angleConfig.slot0.kP = swerveGains.kP;
+            .withOpenLoopRamps(
-        angleConfig.slot0.kI = swerveGains.kI;
+                new OpenLoopRampsConfigs()
-        angleConfig.slot0.kD = swerveGains.kD;
+                    .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
+            ).withClosedLoopRamps(
+                new ClosedLoopRampsConfigs()
+                    .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
+            ).withMotorOutput(
+                new MotorOutputConfigs()
+                    .withNeutralMode(NeutralModeValue.Brake)
+                    .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
+            ).withCurrentLimits(
+                new CurrentLimitsConfigs()
+                    .withStatorCurrentLimit(100)
+                    .withStatorCurrentLimitEnable(true)
+                    .withSupplyCurrentLimit(100)
+                    .withSupplyCurrentLimitEnable(true)
+            );
 
-        // use the CANcoder as the remote sensor for the primary TalonFX PID
+        driveMotor.getConfigurator().apply(motorCfg);
-        angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
-        angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
-        angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
-        angleMotor.configAllSettings(angleConfig);
         
-        encoder.configMagnetOffset(offset);
+        TalonFXConfiguration angleConfig = new TalonFXConfiguration()
+            .withOpenLoopRamps(
+                new OpenLoopRampsConfigs()
+                    .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
+            ).withClosedLoopRamps(
+                new ClosedLoopRampsConfigs()
+                    .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
+            ).withMotorOutput(
+                new MotorOutputConfigs()
+                    .withNeutralMode(NeutralModeValue.Brake)
+                    .withDutyCycleNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
+            );
 
-        driveMotor.setSelectedSensorPosition(0);
+        angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-        driveMotor.config_kP(0, 0.2);
+
+        angleConfig.Slot0.kP = swerveGains.kP;
+        angleConfig.Slot0.kI = swerveGains.kI;   
+        angleConfig.Slot0.kD = swerveGains.kD;
+
+        angleConfig.Feedback.FeedbackRemoteSensorID = encoder.getDeviceID();
+        angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
+        angleMotor.getConfigurator().apply(angleConfig);
+
+        CANcoderConfiguration canconfig = new CANcoderConfiguration();
+        canconfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
+        canconfig.MagnetSensor.MagnetOffset = offset;
+        encoder.getConfigurator().apply(canconfig);
+
+        rotateToAngle(0);
     }
 
+    // public void go(double ang){
+    //     // double curang = this.encoder.getAbsolutePosition().getValue();
+    //     System.out.println(getAngle().getDegrees());
+    //     rotateToAngle(ang);
+    // }
+    
+    @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 +131,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 +139,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 +149,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 Rotation2d.fromRotations(encoder.getPosition().getValue());
     }
     
     public double getAngularVel() {
-        return this.angleMotor.getSelectedSensorVelocity();
+        // return this.angleMotor.getSelectedSensorVelocity();
+        return angleMotor.getVelocity().getValueAsDouble();
     }
 
     public double getDrivePos() {
-        return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
+        // return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
+        return driveMotor.getPosition().getValueAsDouble();
     }
 
     public double getDriveVel() {
-        return this.driveMotor.getSelectedSensorVelocity(0);
+        // return this.driveMotor.getSelectedSensorVelocity(0);
+        return driveMotor.getVelocity().getValueAsDouble();
     }
 
     public void stop() {
@@ -100,7 +174,8 @@ public class SwerveModule extends SubsystemBase {
     }
 
     public void rotateToAngle(double angle) {
-        angleMotor.set(TalonFXControlMode.Position, angle);
+        final PositionVoltage m_request = new PositionVoltage(angle);
+        angleMotor.setControl(m_request);
     }
 
     /**
@@ -109,23 +184,30 @@ public class SwerveModule extends SubsystemBase {
      */
     public SwerveModuleState getState() {
         return new SwerveModuleState(
-            Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, 
+            Units.inchesToMeters(driveMotor.getVelocity().getValue() * 
+            SwerveDriveConstants.Conversions.INCHES_PER_WHEEL_REV * 
+            SwerveDriveConstants.Conversions.WHEEL_REV_PER_MOTOR_REV), 
             getAngle()
         );
     }
 
+    // private SwerveModuleState optimizeState(SwerveModuleState desiredState) {
+    //     Rotation2d curRot = this.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) {
         Rotation2d currentRotation = this.getAngle();
 
@@ -134,28 +216,27 @@ public class SwerveModule extends SubsystemBase {
         // calculate the difference between our current rotational position and our new rotational position
         Rotation2d rotationDelta = state.angle.minus(currentRotation);
 
-        // calculate the new absolute position of the SwerveModule based on the difference in rotation
+        double speed = Units.metersToFeet(state.speedMetersPerSecond) / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND;
-        double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
 
-        // convert the CANCoder from its position reading to ticks
+        rotateToAngle(rotationDelta.getRotations() + currentRotation.getRotations());
-        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
 
-        angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
+        driveMotor.set(Math.max(Math.min(speed, 1.), -1.));
-
-        double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-
-        driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
     }
 
-    public void reset(double position) {
+    public void reset() {
-        encoder.setPositionToAbsolute();
+        // encoder.setPosition(0);
     }
 
-    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

@@ -7,20 +7,22 @@
 
 package frc4388.utility;
 
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
+// import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.ctre.phoenix6.hardware.Pigeon2;
 import com.kauailabs.navx.frc.AHRS;
 
 // import edu.wpi.first.wpilibj.GyroBase;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
+// import edu.wpi.first.wpilibj.interfaces.Gyro;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 /**
  * Gyro class that allows for interchangeable use between a pigeon and a navX
  */
-public class RobotGyro implements Gyro {
+public class RobotGyro {
     private RobotTime m_robotTime = RobotTime.getInstance();
 
-    private WPI_Pigeon2 m_pigeon = null;
+    private Pigeon2 m_pigeon = null;
     private AHRS m_navX = null;
     public boolean m_isGyroAPigeon; //true if pigeon, false if navX
 
@@ -34,7 +36,7 @@ public class RobotGyro implements Gyro {
      * Creates a Gyro based on a pigeon
      * @param gyro the gyroscope to use for Gyro
      */
-    public RobotGyro(WPI_Pigeon2 gyro) {
+    public RobotGyro(Pigeon2 gyro) {
         m_pigeon = gyro;
         m_isGyroAPigeon = true;
     }
@@ -54,8 +56,8 @@ public class RobotGyro implements Gyro {
     public void resetZeroValues() {
         if (!m_isGyroAPigeon) return;
 
-        pitchZero = m_pigeon.getPitch();
+        // pitchZero = m_pigeon.getPitch();
-        rollZero =  m_pigeon.getRoll();
+        // rollZero =  m_pigeon.getRoll();
     }
 
     /**
@@ -84,16 +86,15 @@ public class RobotGyro implements Gyro {
      * is typically done when the robot is first turned on while it's sitting at rest before the
      * competition starts.
      */
-    @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
     public void reset() {
         resetZeroValues();
 
@@ -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();
+        }   
+
     }
 
     /**
@@ -113,16 +181,19 @@ public class RobotGyro implements Gyro {
 	 *					Roll is within [-90,+90] degrees.
      */
     private double[] getPigeonAngles() {
-        double[] ypr = new double[3];
+        m_pigeon.getAngle();
-        m_pigeon.getYawPitchRoll(ypr);
+        var rotation = m_pigeon.getRotation3d();
 
-        return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
+        return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
+    }
+
+    public Rotation2d getRotation2d() {
+        return m_pigeon.getRotation2d();
     }
 
-    @Override
     public double getAngle() {
         if (m_isGyroAPigeon) {
-            return getPigeonAngles()[0];
+            return getPigeonAngles()[2];
         } else {
             return m_navX.getAngle();
         }
@@ -176,7 +247,6 @@ public class RobotGyro implements Gyro {
         }
     }
 
-    @Override
     public double getRate() {
         if (m_isGyroAPigeon) {
             return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
@@ -185,7 +255,7 @@ public class RobotGyro implements Gyro {
         }
     }
 
-    public WPI_Pigeon2 getPigeon(){
+    public Pigeon2 getPigeon(){
         return m_pigeon;
     }
 
@@ -193,7 +263,6 @@ public class RobotGyro implements Gyro {
         return m_navX;
     }
 
-    @Override
     public void close() throws Exception {
 
     }

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/Phoenix6.json

@@ -1,56 +1,32 @@
 {
-    "fileName": "Phoenix.json",
+    "fileName": "Phoenix6.json",
-    "name": "CTRE-Phoenix (v5)",
+    "name": "CTRE-Phoenix (v6)",
-    "version": "5.31.0+23.2.2",
+    "version": "24.3.0",
-    "frcYear": 2023,
+    "frcYear": 2024,
-    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+    "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
         "https://maven.ctr-electronics.com/release/"
     ],
-    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json",
+    "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.phoenix",
+            "groupId": "com.ctre.phoenix6",
-            "artifactId": "api-java",
-            "version": "5.31.0"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
             "artifactId": "wpiapi-java",
-            "version": "5.31.0"
+            "version": "24.3.0"
         }
     ],
     "jniDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.31.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.31.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -63,7 +39,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -76,7 +52,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -89,7 +65,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonFX",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -102,7 +78,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -115,7 +91,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -128,7 +104,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -141,7 +117,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -154,7 +130,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -167,7 +143,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,40 +156,10 @@
     ],
     "cppDependencies": [
         {
-            "groupId": "com.ctre.phoenix",
+            "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "5.31.0",
+            "version": "24.3.0",
-            "libName": "CTRE_Phoenix_WPI",
+            "libName": "CTRE_Phoenix6_WPI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-cpp",
-            "version": "5.31.0",
-            "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.31.0",
-            "libName": "CTRE_PhoenixCCI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,
@@ -227,7 +173,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -240,40 +186,10 @@
             "simMode": "hwsim"
         },
         {
-            "groupId": "com.ctre.phoenix.sim",
+            "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "5.31.0",
+            "version": "24.3.0",
-            "libName": "CTRE_Phoenix_WPISim",
+            "libName": "CTRE_Phoenix6_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.31.0",
-            "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.31.0",
-            "libName": "CTRE_PhoenixCCISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,
@@ -287,7 +203,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -302,7 +218,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -317,7 +233,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonFX",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "libName": "CTRE_SimTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -332,7 +248,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -347,7 +263,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -362,7 +278,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -377,7 +293,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -392,7 +308,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -407,7 +323,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "23.2.2",
+            "version": "24.3.0",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,

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": [

vendordeps/photonlib.json

@@ -1,42 +0,0 @@
-{
-    "fileName": "photonlib.json",
-    "name": "photonlib",
-    "version": "v2024.1.1-beta-3.2",
-    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
-    "frcYear": "2024",
-    "mavenUrls": [
-        "https://maven.photonvision.org/repository/internal",
-        "https://maven.photonvision.org/repository/snapshots"
-    ],
-    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
-    "jniDependencies": [],
-    "cppDependencies": [
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "PhotonLib-cpp",
-            "version": "v2024.1.1-beta-3.2",
-            "libName": "Photon",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxathena",
-                "linuxx86-64",
-                "osxuniversal"
-            ]
-        }
-    ],
-    "javaDependencies": [
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "PhotonLib-java",
-            "version": "v2024.1.1-beta-3.2"
-        },
-        {
-            "groupId": "org.photonvision",
-            "artifactId": "PhotonTargeting-java",
-            "version": "v2024.1.1-beta-3.2"
-        }
-    ]
-}