Merge pull request #43 from Team4388/2024-changes

rebase from 2024

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

LICENSE

@@ -1,4 +1,4 @@
-Copyright (c) 2009-2018 FIRST
+Copyright (c) 2009-2024 FIRST
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -21,4 +21,4 @@ ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -1,2 +1,2 @@
 # Robot-Essentials
- Basic code for any Ridgebotics robot project
+ Basic code for any Ridgebotics robot project

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
 }
@@ -96,4 +98,4 @@ wpi.java.configureTestTasks(test)
 // Configure string concat to always inline compile
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
-}
+}

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

@@ -8,9 +8,8 @@
 package frc4388.robot;
 
 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,82 +22,98 @@ 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[] GEARS = {0.25, 0.5, 1.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 double SLOW_SPEED = 0.8;
+        public static final class DefaultSwerveRotOffsets {
-        public static final double FAST_SPEED = 1.0;
+          public static final double FRONT_LEFT_ROT_OFFSET  = 0.0; //TODO: per robot swerve module offsets.
-        public static final double TURBO_SPEED = 4.0;
+          public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
-    
+          public static final double BACK_LEFT_ROT_OFFSET   = 0.0; //TODO: per robot swerve module offsets.
+          public static final double BACK_RIGHT_ROT_OFFSET  = 0.0; //TODO: per robot swerve module offsets.
+        }
+
         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_WHEEL_ID = 6;
-          public static final int LEFT_BACK_STEER_ID = 7;
+            public static final int LEFT_BACK_STEER_ID = 7;
-          public static final int LEFT_BACK_ENCODER_ID = 12;
+            public static final int LEFT_BACK_ENCODER_ID = 12;
-          
+            
-          public static final int RIGHT_BACK_WHEEL_ID = 8;
+            public static final int RIGHT_BACK_WHEEL_ID = 8;  
-          public static final int RIGHT_BACK_STEER_ID = 9;
+            public static final int RIGHT_BACK_STEER_ID = 9;
-          public static final int RIGHT_BACK_ENCODER_ID = 13;
+            public static final int RIGHT_BACK_ENCODER_ID = 13;
+
+            public static final int DRIVE_PIGEON_ID = 14;
         }
     
         public static final class PIDConstants {
-          public static final int SWERVE_SLOT_IDX = 0;
+            public static final int SWERVE_SLOT_IDX = 0;
-          public static final int SWERVE_PID_LOOP_IDX = 1;
+            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 {
-          public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+            public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
-          public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+            public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
-          public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
+            public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
-          public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
+            public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
-    
+            
-          public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
+            public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
-          public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
+            public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
         }
     
         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 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 = 0.5;
-    
+            public static final double WHEEL_DIAMETER_INCHES = 3.9;
-          public static final double TICKS_PER_MOTOR_REV = 2048;
+            public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
-          public static final double WHEEL_DIAMETER_INCHES = 3.9;
+        
-          public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
+            public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
-    
+            public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
-          public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
+            public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
-          public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
+            public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
-          public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
+        
-          public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
+            public static final double TICK_TIME_TO_SECONDS = 10;
-    
+            public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
-          public static final double TICK_TIME_TO_SECONDS = 10;
-          public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
         }
     
         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;
@@ -111,31 +126,10 @@ public final class Constants {
         public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
       }
     
-      public static final class VisionConstants {
+    public static final class VisionConstants {
-        public static final String NAME = "photonCamera";
+    }
-    
-        public static final int LIME_HIXELS = 640;
-        public static final int LIME_VIXELS = 480;
-    
-        public static final double H_FOV = 59.6;
-        public static final double V_FOV = 45.7;
-    
-        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 class DriveConstants {
         public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
     }
     
@@ -148,5 +142,8 @@ public final class Constants {
     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
@@ -22,16 +22,17 @@ import frc4388.utility.RobotTime;
  */
 public class Robot extends TimedRobot {
   Command m_autonomousCommand;
-  
+
   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,14 +41,16 @@ 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.
    */
   @Override
-  public void robotPeriodic() {
+  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,18 +7,33 @@
 
 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 frc4388.robot.Constants.*;
+// Autos
-import frc4388.robot.commands.Swerve.JoystickPlayback;
+import frc4388.utility.controller.VirtualController;
-import frc4388.robot.commands.Swerve.JoystickRecorder;
+import frc4388.robot.commands.Swerve.neoJoystickPlayback;
-import frc4388.robot.subsystems.LED;
+import frc4388.robot.commands.Swerve.neoJoystickRecorder;
-import frc4388.utility.LEDPatterns;
+
-import frc4388.utility.controller.IHandController;
+// Subsystems
-import frc4388.utility.controller.XboxController;
+// import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.SwerveDrive;
+
+// Utilites
+import frc4388.utility.DeferredBlock;
+import frc4388.utility.configurable.ConfigurableString;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -29,25 +44,84 @@ import frc4388.utility.controller.XboxController;
  */
 public class RobotContainer {
     /* RobotMap */
-    private final RobotMap m_robotMap = new RobotMap();
+    public final RobotMap m_robotMap = new RobotMap();
-
+    
     /* Subsystems */
-    private final LED m_robotLED = new LED(m_robotMap.LEDController);
+    // private final LED m_robotLED = new LED();
+
+    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
+                                                                  m_robotMap.rightFront,
+                                                                  m_robotMap.leftBack,
+                                                                  m_robotMap.rightBack,
+                                              
+                                                                  m_robotMap.gyro);
 
     /* Controllers */
-    private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
+    private final DeadbandedXboxController m_driverXbox   = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
-    private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
+    private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);    
+    private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID);
 
+    /* Virtual Controllers */
+    private final VirtualController m_virtualDriver = new VirtualController(0);
+    private final VirtualController m_virtualOperator = new VirtualController(1);
+
+    // ! Teleop Commands
+
+    // ! /*  Autos */
+    private String lastAutoName = "defualt.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);
+    
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
-        configureButtonBindings();
+        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));
+
+
+
+
     }
 
     /**
@@ -57,26 +131,68 @@ 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 */
+            
+        // ? /* 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(getOperatorJoystick(), XboxController.A_BUTTON)
-                .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
-                .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
     }
 
     /**
@@ -85,35 +201,32 @@ public class RobotContainer {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        // no auto
+        return autoPlayback;
-        return new InstantCommand();
     }
 
     /**
-     * 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 IHandController getDriverController() {
+    public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) {
-        return m_driverXbox;
+        return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
     }
 
-    /**
+    public DeadbandedXboxController getDeadbandedDriverController() {
-     * Add your docs here.
+        return this.m_driverXbox;
-     */
-    public IHandController getOperatorController() {
-        return m_operatorXbox;
     }
 
-    /**
+    public DeadbandedXboxController getDeadbandedOperatorController() {
-     * Add your docs here.
+        return this.m_operatorXbox;
-     */
-    public Joystick getOperatorJoystick() {
-        return m_operatorXbox.getJoyStick();
     }
 
-    /**
+    public VirtualController getVirtualDriverController() {
-     * Add your docs here.
+        return m_virtualDriver;
-     */
+    }
-    public Joystick getDriverJoystick() {
+
-        return m_driverXbox.getJoyStick();
+    public VirtualController getVirtualOperatorController() {
+        return m_virtualOperator;
     }
 }

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

@@ -7,14 +7,14 @@
 
 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.PigeonIMU;
 
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
+// import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+// import frc4388.robot.Constants.LEDConstants;
-import frc4388.robot.Constants.LEDConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.subsystems.SwerveModule;
 import frc4388.utility.RobotGyro;
 
 /**
@@ -22,19 +22,44 @@ import frc4388.utility.RobotGyro;
  * testing and modularization.
  */
 public class RobotMap {
+    private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON_ID);
+    public RobotGyro gyro = new RobotGyro(m_pigeon2);
+    
+    public SwerveModule leftFront;
+    public SwerveModule rightFront;
+    public SwerveModule leftBack;
+    public SwerveModule rightBack;
 
     public RobotMap() {
-        configureLEDMotorControllers();
         configureDriveMotorControllers();
     }
 
     /* LED Subsystem */
-    public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+    // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
 
-    void configureLEDMotorControllers() {
+    /* Swreve Drive Subsystem */
-        
+    public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_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 TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
+    public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
+    public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
+    
+    public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
+    public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_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);
 
     void configureDriveMotorControllers() {
+        // initialize SwerveModules
+        this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
+        this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
+        this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
+        this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
     }
 }

src/main/java/frc4388/robot/commands/Autos/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();
-            chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
+
+        if(dirs != null){ // Fix funny error
+            for (String auto : dirs) {
+                chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
+            }
         }
+
+        
         for (var cmd_name : m_commandPool.keySet()) {
             chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
         }

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

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

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

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

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

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

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

@@ -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);
                              
@@ -138,4 +142,4 @@ public class JoystickPlayback extends CommandBase {
   public boolean isFinished() {
     return m_finished;
   }
-}
+}

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.
@@ -94,4 +94,4 @@ public class JoystickRecorder extends CommandBase {
   public boolean isFinished() {
     return false;
   }
-}
+}

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/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/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

@@ -1,165 +0,0 @@
-// 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 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.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.VisionConstants;
-
-public class Limelight extends SubsystemBase {
-  
-  private PhotonCamera cam;
-  private PhotonPoseEstimator photonPoseEstimator;
-
-  private boolean lightOn;
-
-  /** Creates a new Limelight. */
-  public Limelight() {
-    cam = new PhotonCamera(VisionConstants.NAME);
-    cam.setDriverMode(false);
-  }
-
-  public void setLEDs(boolean on) {
-    lightOn = on;
-    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
-  }
-
-  public void toggleLEDs() {
-    lightOn = !lightOn;
-    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
-  }
-
-  public void setDriverMode(boolean driverMode) {
-    cam.setDriverMode(driverMode);
-  }
-
-  public void setToLimePipeline() {
-    cam.setPipelineIndex(1);
-    setLEDs(true);
-  }
-
-  public void setToAprilPipeline() {
-    cam.setPipelineIndex(0);
-    setLEDs(false);
-  }
-
-  public PhotonTrackedTarget getAprilPoint() {
-    if (!cam.isConnected()) return null;
-
-    PhotonPipelineResult result = cam.getLatestResult();
-
-    if (!result.hasTargets()) return null;
-
-    return result.getBestTarget();
-  }
-
-  private List<TargetCorner> getAprilCorners() {
-    if (!cam.isConnected()) return null;
-
-    PhotonPipelineResult result = cam.getLatestResult();
-
-    if (!result.hasTargets()) return null;
-
-    return result.getBestTarget().getDetectedCorners();
-  }
-
-  public double getAprilSkew() {
-    List<TargetCorner> corners = getAprilCorners();
-    ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
-
-    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/SwerveDrive.java

@@ -4,17 +4,21 @@
 
 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 {
+public class  SwerveDrive extends SubsystemBase {
   
   private SwerveModule leftFront;
   private SwerveModule rightFront;
@@ -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;
+  
+  private int gear_index;
+  private boolean stopped = false;
 
-  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
+  public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+  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);
         
-        driveMotor.setSelectedSensorPosition(0);
+        TalonFXConfiguration angleConfig = new TalonFXConfiguration()
-        driveMotor.config_kP(0, 0.2);
+            .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)
+            );
+
+        angleConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+        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/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

@@ -6,7 +6,21 @@ public class UtilityStructs {
         public double leftY  = 0.0;
         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/DeadbandedXboxController.java

@@ -0,0 +1,27 @@
+package frc4388.utility.controller;
+
+import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class DeadbandedXboxController extends XboxController {
+  public DeadbandedXboxController(int port) { super(port); }
+  
+  @Override public double getLeftX() { return getLeft().getX(); }
+  @Override public double getLeftY() { return getLeft().getY(); }
+  @Override public double getRightX() { return getRight().getX(); }
+  @Override public double getRightY() { return getRight().getY(); }
+
+  public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
+  public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
+
+  public static Translation2d skewToDeadzonedCircle(double x, double y) {
+    Translation2d translation2d = new Translation2d(x, y);
+    double magnitude = translation2d.getNorm();
+
+    if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
+
+    return translation2d;
+  }
+}

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"
-        }
-    ]
-}