Initial commit

src/main/deploy/example.txt

@@ -0,0 +1,3 @@
+Files placed in this directory will be deployed to the RoboRIO into the
+'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function
+to get a proper path relative to the deploy directory.

src/main/java/frc4388/robot/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

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

@@ -0,0 +1,264 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+// Advantagekit
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.robot.constants.BuildConstants;
+import frc4388.robot.constants.Constants.SimConstants;
+import frc4388.utility.DeferredBlock;
+import frc4388.utility.compute.RobotTime;
+import frc4388.utility.compute.Trim;
+import frc4388.utility.status.FaultReporter;
+
+//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
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends LoggedRobot {
+  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() {
+    // Start logging with AdvantageKit
+    startLogging();
+
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+
+
+    // // Create a shuffleboard update thread, that will periodically update the values on shuffleboard
+    // FaultReporter.startThread();
+  }
+
+
+
+  /**
+   * 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.doubl
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {   
+    m_robotTime.updateTimes();
+    // SmartDashboard.putNumber("Time", System.currentTimeMillis());
+    
+    m_robotContainer.m_robotLED.update();
+    // 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
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+  /**
+   * This function is called once each time the robot enters Disabled mode.
+   * You can use it to reset any subsystem information you want to clear when
+   * the robot is disabled.
+   */
+  @Override
+  public void disabledInit() {
+    m_robotTime.endMatchTime();
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  @Override
+  public void disabledExit() {
+    DeferredBlock.execute();
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    /*
+     * String autoSelected = SmartDashboard.getString("Auto Selector",
+     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+     * = new MyAutoCommand(); break; case "Default Auto": default:
+     * autonomousCommand = new ExampleCommand(); break; }
+     */
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+    m_robotTime.startMatchTime();
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+
+  @Override
+  public void teleopInit() {
+    m_robotContainer.stop();
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+
+    if (m_autonomousCommand != null) {
+      CommandScheduler.getInstance().cancel(m_autonomousCommand);
+      m_autonomousCommand.cancel();
+      m_autonomousCommand.end(true);
+
+    }
+    m_robotTime.startMatchTime();
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+  //  m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopExit() { // the only OTHER mode that teleop can enter into is disabled.
+    Trim.dumpAll();
+  }
+
+  @Override
+  public void testInit() {
+    FaultReporter.printReport();
+  }
+
+
+
+
+
+
+
+  // VisionSystemSim visionSim;
+  // RobotMapSim robotMapSim;
+
+  // @Override
+  // public void simulationInit() {
+  //   visionSim = new VisionSystemSim("main");
+  //   robotMapSim = m_robotContainer.m_robotMap.configureSim();
+
+
+  //   // A 0.5 x 0.25 meter rectangular target
+  //   TargetModel targetModel = new TargetModel(0.5, 0.25);
+  //   // The pose of where the target is on the field.
+  //   // Its rotation determines where "forward" or the target x-axis points.
+  //   // Let's say this target is flat against the far wall center, facing the blue driver stations.
+  //   Pose3d targetPose = new Pose3d(16, 4, 2, new Rotation3d(0, 0, Math.PI));
+  //   // The given target model at the given pose
+  //   VisionTargetSim visionTarget = new VisionTargetSim(targetPose, targetModel);
+
+  //   // Add this vision target to the vision system simulation to make it visible
+  //   visionSim.addVisionTargets(visionTarget);
+
+  //   // The layout of AprilTags which we want to add to the vision system
+  //   AprilTagFieldLayout tagLayout = Constants.FieldConstants.kTagLayout;
+
+  //   visionSim.addAprilTags(tagLayout);
+
+
+  //   visionSim.addCamera(robotMapSim.leftCamera, Constants.VisionConstants.LEFT_CAMERA_POS);
+  //   visionSim.addCamera(robotMapSim.rightCamera, Constants.VisionConstants.RIGHT_CAMERA_POS);
+
+  //   SmartDashboard.putData(visionSim.getDebugField());
+  // }
+
+
+  // @Override
+  // public void simulationPeriodic() {
+  //   m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage());
+  //   // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d());
+
+  //   // m_robotContainer.m_robotSwerveDrive.
+  // }
+
+
+
+// Start AdvantageKit logging / replay and record metadata
+// Refrence: https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/template_projects/sources/skeleton/src/main/java/frc/robot/Robot.java
+  public void startLogging() {
+    // Record metadata
+    Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+    Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+    Logger.recordMetadata("Git+SHA", BuildConstants.GIT_SHA);
+    Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+    Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+    switch (BuildConstants.DIRTY) {
+      case 0:
+        Logger.recordMetadata("GitDirty", "All changes committed");
+        break;
+      case 1:
+        Logger.recordMetadata("GitDirty", "Uncomitted changes");
+        break;
+      default:
+        Logger.recordMetadata("GitDirty", "Unknown");
+        break;
+    }
+
+    // Set up data receivers & replay source
+    switch (SimConstants.currentMode) {
+      case REAL:
+        // Running on a real robot, log to a USB stick ("/U/logs")
+        Logger.addDataReceiver(new WPILOGWriter());
+        Logger.addDataReceiver(new NT4Publisher());
+        break;
+
+      case SIM:
+        // Running a physics simulator, log to NT
+        Logger.addDataReceiver(new NT4Publisher());
+        break;
+
+      case REPLAY:
+        // Replaying a log, set up replay source
+        setUseTiming(false); // Run as fast as possible
+        String logPath = LogFileUtil.findReplayLog();
+        Logger.setReplaySource(new WPILOGReader(logPath));
+        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+        break;
+    }
+
+    // Start AdvantageKit logger
+    Logger.start();
+  }
+}

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

@@ -0,0 +1,221 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+// Drive Systems
+import edu.wpi.first.wpilibj.DriverStation;
+
+import java.io.File;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.utility.controller.XboxController;
+import frc4388.utility.controller.ButtonBox;
+import frc4388.utility.controller.DeadbandedXboxController;
+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.ParallelRaceGroup;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+// Autos
+import frc4388.utility.controller.VirtualController;
+import frc4388.robot.commands.MoveForTimeCommand;
+import frc4388.robot.commands.MoveUntilSuply;
+import frc4388.robot.commands.alignment.DriveToReef;
+import frc4388.robot.commands.alignment.DriveUntilLiDAR;
+import frc4388.robot.commands.alignment.LidarAlign;
+import frc4388.robot.commands.wait.waitElevatorRefrence;
+import frc4388.robot.commands.wait.waitEndefectorRefrence;
+import frc4388.robot.commands.wait.waitFeedCoral;
+import frc4388.robot.commands.wait.waitSupplier;
+import frc4388.robot.constants.Constants;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.constants.Constants.LiDARConstants;
+import frc4388.robot.constants.Constants.OIConstants;
+import frc4388.robot.constants.Constants.SimConstants.Mode;
+
+import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
+// Subsystems
+import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.elevator.Elevator;
+import frc4388.robot.subsystems.elevator.Elevator.CoordinationState;
+import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+import frc4388.robot.subsystems.vision.Vision;
+// Utilites
+import frc4388.utility.DeferredBlock;
+import frc4388.utility.compute.TimesNegativeOne;
+import frc4388.utility.compute.ReefPositionHelper.Side;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (2including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+    /* RobotMap */
+    
+    public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
+    
+    /* Subsystems */
+    public final LED m_robotLED = new LED();
+    public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
+    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
+    public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
+    // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
+
+    public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
+    public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse");
+
+
+    /* Controllers */
+    private final DeadbandedXboxController m_driverXbox   = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
+    private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
+    private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
+
+    // public List<Subsystem> subsystems = new ArrayList<>();
+
+    // ! Teleop Commands
+    public void stop() {
+        new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
+        m_robotSwerveDrive.stopModules();
+        Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
+    }
+
+    // ! /*  Autos */
+    private SendableChooser<String> autoChooser;
+    private Command autoCommand;
+
+    
+    /**
+     * 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.
+
+    }
+
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+
+
+        //return autoPlayback;
+        //return new GotoPositionCommand(m_robotSwerveDrive, m_vision)
+        //return autoChooser.getSelected();
+	// try{
+	// //     // Load the path you want to follow using its name in the GUI
+    //     return autoCommand;
+	// } catch (Exception e) {
+	//     DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
+	    return autoCommand;
+	// }
+    // return new PathPlannerAuto("Line-up-no-arm");
+	// zach told me to do the below comment
+	//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
+      //  return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
+    }
+
+    public boolean autoChooserUpdated = false;
+    public void makeAutoChooser() {
+        autoChooser = new SendableChooser<String>();
+        
+        File dir;
+
+        if(RobotBase.isReal()) {
+            dir = new File("/home/lvuser/deploy/pathplanner/autos/");
+        } else {
+            // dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
+            dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos");
+        }
+
+        String[] autos = dir.list();
+
+        if(autos == null) return;
+
+        for (String auto : autos) {
+            if (auto.endsWith(".auto"))
+                autoChooser.addOption(auto.replaceAll(".auto", ""), auto.replaceAll(".auto", ""));
+            // System.out.println(auto);
+        }
+
+        autoChooser.onChange((filename) -> {
+            autoChooserUpdated = true;
+            if (filename.equals("Taxi")) {
+                autoCommand = new SequentialCommandGroup(
+                    new MoveForTimeCommand(m_robotSwerveDrive, 
+                        new Translation2d(0, -1), 
+                        new Translation2d(), 1000, true
+                ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
+            } else {
+                autoCommand = new PathPlannerAuto(filename);
+            }
+            System.out.println("Robot Auto Changed " + filename);
+        });
+        // SmartDashboard.putData(autoChooser);
+
+    }
+
+    /**
+     * A button binding for two controllers, preferably an {@link DeadbandedXboxController Xbox Controller} and {@link VirtualController Virtual Xbox Controller}
+     * @param joystickA A controller
+     * @param joystickB A controller
+     * @param buttonNumber The button to bind to
+     */
+    public Trigger DualJoystickButton(GenericHID joystickA, GenericHID joystickB, int buttonNumber) {
+        return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
+    }
+
+    public DeadbandedXboxController getDeadbandedDriverController() {
+        return this.m_driverXbox;
+    }
+
+    public DeadbandedXboxController getDeadbandedOperatorController() {
+        return this.m_operatorXbox;
+    }
+
+    public ButtonBox getButtonBox() {
+        return this.m_buttonBox;
+    }
+}

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

@@ -0,0 +1,175 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import org.photonvision.PhotonCamera;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DigitalInput;
+import frc4388.robot.constants.Constants.ElevatorConstants;
+import frc4388.robot.constants.Constants.LiDARConstants;
+import frc4388.robot.constants.Constants.SimConstants;
+import frc4388.robot.constants.Constants.VisionConstants;
+import frc4388.robot.subsystems.elevator.ElevatorIO;
+import frc4388.robot.subsystems.elevator.ElevatorReal;
+import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.lidar.LidarIO;
+import frc4388.robot.subsystems.lidar.LidarReal;
+import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
+import frc4388.robot.subsystems.swerve.SwerveIO;
+import frc4388.robot.subsystems.swerve.SwerveReal;
+import frc4388.robot.subsystems.vision.VisionIO;
+import frc4388.robot.subsystems.vision.VisionReal;
+import frc4388.utility.status.FaultCANCoder;
+import frc4388.utility.status.FaultPhotonCamera;
+import frc4388.utility.status.FaultPidgeon2;
+import frc4388.utility.status.FaultTalonFX;
+
+/**
+ * Defines and holds all I/O objects on the Roborio. This is useful for unit
+ * 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 final VisionIO leftCamera;
+    public final VisionIO rightCamera;
+
+    // public final LiDAR lidar = new 
+
+    public final LidarIO reefLidar;
+    public final LidarIO reverseLidar;
+
+
+    /* LED Subsystem */
+    // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+    
+    /* Swreve Drive Subsystem */
+    public final SwerveIO swerveDrivetrain;
+
+    /* Elevator Subsystem */
+    public final ElevatorIO elevatorIO;
+
+    public RobotMap(SimConstants.Mode mode) {
+        switch (mode) {
+            case REAL:
+                // Configure cameras
+                PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
+                PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
+
+                leftCamera =  new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS);                ;
+                rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
+
+                FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
+                FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
+
+                // Configure LiDAR
+                reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
+                reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
+
+                // Configure swerve drive train
+                SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new, 
+                    SwerveDriveConstants.DrivetrainConstants, 
+                    SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
+                    SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
+                );
+
+                swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
+
+                // Configure elevator
+
+                TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
+                TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
+                
+
+                DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
+                DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
+                DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
+
+                elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam);
+
+
+
+                // Fault
+                FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
+
+                FaultTalonFX.addDevice(elevator, "Elevator");
+                FaultTalonFX.addDevice(endeffector, "Endeffector");
+                
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
+
+                break;
+            // case SIM:
+            //     break;
+            default:
+                leftCamera = new VisionIO() {};
+                rightCamera = new VisionIO() {};
+                reefLidar = new LidarIO() {};
+                reverseLidar = new LidarIO() {};
+                swerveDrivetrain = new SwerveIO() {};
+                elevatorIO = new ElevatorIO() {};
+                break;
+        }
+    }
+
+    // public class RobotMapSim {
+    //     public PhotonCameraSim leftCamera;
+    //     public PhotonCameraSim rightCamera;
+    // }
+
+    // public RobotMapSim configureSim() {
+    //     RobotMapSim sim = new RobotMapSim();
+
+    //     // The simulated camera properties
+    //     SimCameraProperties cameraProp = new SimCameraProperties();
+    //     // A 640 x 480 camera with a 100 degree diagonal FOV.
+    //     cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100));
+    //     // Approximate detection noise with average and standard deviation error in pixels.
+    //     cameraProp.setCalibError(0.25, 0.08);
+    //     // Set the camera image capture framerate (Note: this is limited by robot loop rate).
+    //     cameraProp.setFPS(20);
+    //     // The average and standard deviation in milliseconds of image data latency.
+    //     cameraProp.setAvgLatencyMs(35);
+    //     cameraProp.setLatencyStdDevMs(5);
+
+    //     // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
+    //     // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
+
+        
+    //     sim.leftCamera.enableRawStream(true);
+    //     sim.leftCamera.enableProcessedStream(true);
+    //     sim.leftCamera.enableDrawWireframe(true);
+
+
+    //     sim.rightCamera.enableRawStream(true);
+    //     sim.rightCamera.enableProcessedStream(true);
+    //     sim.rightCamera.enableDrawWireframe(true);
+
+    //     return sim;
+
+    // }
+   
+}

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,108 @@
+package frc4388.robot.commands.autos;
+// 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/MoveForTimeCommand.java

@@ -0,0 +1,49 @@
+package frc4388.robot.commands;
+
+import java.time.Instant;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+// Command to repeat a joystick movement for a specific time.
+public class MoveForTimeCommand extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final long duration;
+    private final boolean robotRelative;
+
+    private Instant startTime;
+
+    public MoveForTimeCommand(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        long millis,
+        boolean robotRelative) {
+        
+        addRequirements(swerveDrive);
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.duration = millis;
+        this.robotRelative = robotRelative;
+    }
+
+    @Override
+    public void initialize() {
+        startTime = Instant.now();
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
+    }
+}

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

@@ -0,0 +1,45 @@
+package frc4388.robot.commands;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+// Command to repeat a joystick movement for a specific time.
+public class MoveUntilSuply extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final Supplier<Boolean> truth;
+    private final boolean robotRelative;
+
+    public MoveUntilSuply(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        Supplier<Boolean> truth,
+        boolean robotRelative) {
+            addRequirements(swerveDrive);
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.truth = truth;
+        this.robotRelative = robotRelative;
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return truth.get();
+    }
+}

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

@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.utility.structs.Gains;
+
+public abstract class PID extends Command {
+	protected Gains  gains;
+	private   double output    = 0;
+	private   double tolerance = 0;
+
+	/** Creates a new PelvicInflammatoryDisease. */
+	public PID(double kp, double ki, double kd, double kf, double tolerance) {
+		gains          = new Gains(kp, ki, kd, kf, 0);
+		this.tolerance = tolerance;
+	}
+
+	public PID(Gains gains, double tolerance) {
+		this.gains     = gains;
+		this.tolerance = tolerance;
+	}
+
+	/** produces the error from the setpoint */
+	public abstract double getError();
+	
+	/** todo: javadoc */
+	public abstract void runWithOutput(double output);
+
+	// Called when the command is initially scheduled.
+	@Override 
+	public final void initialize() {
+		output = 0;
+	}
+
+	private double prevError, cumError = 0;
+	
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override 
+	public final void execute() {
+		double error = getError();
+		cumError += error * .02; // 20 ms
+		double delta = error - prevError;
+
+		output = error * gains.kP;
+		output += cumError * gains.kI;
+		output += delta * gains.kD;
+		output += gains.kF;
+
+		runWithOutput(output);
+	}
+
+	// Returns true when the command should end.
+	@Override
+	public final boolean isFinished() {
+		return Math.abs(getError()) < tolerance;
+	}
+}

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

@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.swerve;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PID;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+public class RotateToAngle extends PID {
+
+  SwerveDrive drive;
+  double targetAngle;
+
+  /** Creates a new RotateToAngle. */
+  public RotateToAngle(SwerveDrive drive, double targetAngle) {
+    super(0.3, 0.0, 0.0, 0.0, 1);
+    
+    this.drive = drive;
+    this.targetAngle = targetAngle;
+
+    addRequirements(drive);
+  }
+
+  @Override
+  public double getError() {
+    return targetAngle - drive.getGyroAngle();
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+    drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);    
+  }
+}

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.swerve.SwerveDrive;
+import frc4388.utility.compute.DataUtils;
+import frc4388.utility.controller.VirtualController;
+import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
+import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
+
+
+/**
+ * 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.swerve.SwerveDrive;
+import frc4388.utility.compute.DataUtils;
+import frc4388.utility.controller.DeadbandedXboxController;
+import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
+import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
+
+/**
+ * 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/commands/WhileTrueCommand.java

@@ -0,0 +1,104 @@
+package frc4388.robot.commands;
+
+// 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.
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import java.util.function.BooleanSupplier;
+
+/**
+ * A command composition that runs one of two commands, depending on the value of the given
+ * condition when this command is initialized.
+ *
+ * <p>The rules for command compositions apply: command instances that are passed to it cannot be
+ * added to any other composition or scheduled individually, and the composition requires all
+ * subsystems its components require.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class WhileTrueCommand extends Command {
+  private final Command m_whileTrue;
+  private final BooleanSupplier m_condition;
+
+  /**
+   * Creates a new WhileTrueCommand.
+   *
+   * @param whileTrue the command to run while the condition is true
+   * @param condition the condition to determine which command to run
+   */
+  @SuppressWarnings("this-escape")
+  public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
+    m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
+    m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
+
+    //CommandScheduler.getInstance().registerComposedCommands(whileTrue);
+
+    // addRequirements(whileTrue.getRequirements());
+  }
+
+  @Override
+  public void initialize() {
+    if(m_condition.getAsBoolean())
+      m_whileTrue.initialize();
+  }
+
+  @Override
+  public void execute() {
+    m_whileTrue.execute();
+
+    System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " +  m_condition.getAsBoolean());
+
+    if(!m_whileTrue.isFinished())
+      return;
+
+    if(m_condition.getAsBoolean()){
+      m_whileTrue.end(false);
+      m_whileTrue.initialize();
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_whileTrue.end(interrupted);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return !m_condition.getAsBoolean() && m_whileTrue.isFinished();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return m_whileTrue.runsWhenDisabled();
+  }
+
+  @Override
+  public InterruptionBehavior getInterruptionBehavior() {
+    if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
+      return InterruptionBehavior.kCancelSelf;
+    } else {
+      return InterruptionBehavior.kCancelIncoming;
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addStringProperty("whileTrue", m_whileTrue::getName, null);
+    builder.addStringProperty(
+        "selected",
+        () -> {
+          if (m_whileTrue == null) {
+            return "null";
+          } else {
+            return m_whileTrue.getName();
+          }
+        },
+        null);
+  }
+}

src/main/java/frc4388/robot/commands/alignment/DriveToReef.java

@@ -0,0 +1,188 @@
+package frc4388.robot.commands.alignment;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+import frc4388.robot.subsystems.vision.Vision;
+import frc4388.utility.compute.ReefPositionHelper;
+import frc4388.utility.compute.TimesNegativeOne;
+import frc4388.utility.compute.ReefPositionHelper.Side;
+import frc4388.utility.structs.Gains;
+
+public class DriveToReef extends Command {
+    
+
+    // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
+    // private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
+
+    private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
+    private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
+    // private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
+    private Pose2d targetpos;
+
+    SwerveDrive swerveDrive;
+    Vision vision;
+    double distance;
+    Side side;
+    boolean waitVelocityZero;
+
+    /**
+     * Command to drive robot to position.
+     * @param SwerveDrive m_robotSwerveDrive
+     */
+
+    public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
+        this.swerveDrive = swerveDrive;
+        this.vision = vision;
+        this.distance = distance;
+        this.side = side;
+        this.waitVelocityZero = waitVelocityZero && false;
+        addRequirements(swerveDrive);
+    }
+
+
+    public static double tagRelativeXError = -1;
+    private static void setTagRelativeXError(double val){
+        tagRelativeXError = val;
+    }
+
+    @Override
+    public void initialize() {
+        xPID.initialize();
+        yPID.initialize();
+        this.targetpos = ReefPositionHelper.getNearestPosition(
+            this.vision.getPose2d(), 
+            side, 
+            Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), 
+            distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())
+        );
+    }
+    
+    double xerr;
+    double yerr;
+    double roterr;
+
+    double xoutput;
+    double youtput;
+    double rotoutput;
+
+    @Override
+    public void execute() {
+        xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
+        yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
+        // xerr = targetpos.getX() - vision.getPose2d().getX();
+        // yerr = targetpos.getX() - vision.getPose2d().getY();
+
+        // roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
+
+        roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
+
+        if(roterr > 180){
+            roterr -= 360;
+        }else if(roterr < -180){
+            roterr += 360;
+        }
+
+        // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
+        // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
+        // SmartDashboard.putNumber("Rotational PID error", roterr);
+
+        SmartDashboard.putNumber("PID X Error", xerr);
+        SmartDashboard.putNumber("PID Y Error", yerr);
+        SmartDashboard.putNumber("PID Rot Error", roterr);
+
+        xoutput = xPID.update(xerr);
+        youtput = yPID.update(yerr);
+        // rotoutput = rotPID.update(roterr);
+
+        xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
+        youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
+        // rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
+        
+
+
+        Translation2d leftStick = new Translation2d(
+            Math.max(Math.min(-youtput, 1), -1),
+            Math.max(Math.min(-xoutput, 1), -1)
+            // 0,0
+        );
+
+        // Translation2d rightStick = new Translation2d(
+        //     Math.max(Math.min(rotoutput, 1), -1), 
+        //    0
+        // );
+
+        setTagRelativeXError(
+            new Translation2d(xerr, yerr).getAngle()
+            .rotateBy(targetpos.getRotation())
+            .getCos());
+
+        swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
+        // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
+    }
+
+    @Override
+    public final boolean isFinished() {
+        boolean finished = (
+            (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+            (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+            (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
+            (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
+        );
+        // System.out.println(finished);
+
+        if(finished)
+            swerveDrive.softStop();
+
+        return finished;
+                // this statement is a boolean in and of itself
+    }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    private class PID {
+        protected Gains  gains;
+        private   double output    = 0;
+
+
+        /** Creates a new PelvicInflammatoryDisease. */
+        public PID(Gains gains, double tolerance) {
+            this.gains     = gains;
+        }
+
+        // Called when the command is initially scheduled.
+        public final void initialize() {
+            output = 0;
+        }
+
+        private double prevError, cumError = 0;
+        
+        // Called every time the scheduler runs while the command is scheduled.
+        public double update(double error) {
+            cumError += error * .02; // 20 ms
+            double delta = error - prevError;
+
+            output = error * gains.kP;
+            output += cumError * gains.kI;
+            output += delta * gains.kD;
+            output += gains.kF;
+
+            return output;
+        }
+    }
+}

src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java

@@ -0,0 +1,48 @@
+package frc4388.robot.commands.alignment;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+// Command to repeat a joystick movement for a specific time.
+public class DriveUntilLiDAR extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final LiDAR m_lidar;
+    private final double mindistance;
+
+    public DriveUntilLiDAR(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        LiDAR lidar,
+        double mindistance) {
+            addRequirements(swerveDrive);
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.m_lidar = lidar;
+        this.mindistance = mindistance;
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveFine(leftStick, rightStick, 0.3);
+    }
+
+    @Override
+    public boolean isFinished() {
+        if (Math.abs(m_lidar.getDistance()) < mindistance) {
+            swerveDrive.softStop();
+            return true;
+        }
+        return false;
+    }
+}

src/main/java/frc4388/robot/commands/alignment/LidarAlign.java

@@ -0,0 +1,107 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.alignment;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class LidarAlign extends Command {
+  private SwerveDrive swerveDrive;
+  private LiDAR lidar;  
+
+  private int currentFinderTick;
+  // private int tickFoundPipe;
+  private boolean foundReef;
+  private boolean headedRight;
+  private double speed;
+  private int bounces;
+  private double additionalDistance = 0;
+  // private final boolean constructedHeadedRight;
+
+  /** Creates a new LidarAlign. */
+  public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.swerveDrive = swerveDrive;
+    this.lidar = lidar;
+
+    addRequirements(swerveDrive, lidar);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.currentFinderTick = 0;
+    this.speed = 0.4; // TODO: find good speed for this
+    this.foundReef = false;
+    this.headedRight = (DriveToReef.tagRelativeXError < 0);
+    this.additionalDistance = 0;
+    this.bounces = 0;
+  }
+
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (lidar.withinDistance()) {
+      swerveDrive.softStop(); 
+      foundReef = true;
+      return;
+    }
+
+    if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
+      headedRight = !headedRight;
+      currentFinderTick *= -1;
+      bounces++;
+      additionalDistance += 5;
+      if (bounces == 5) return;
+    }
+    double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
+    double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
+    SmartDashboard.putNumber("Rel Angle", relAngle);
+    SmartDashboard.putNumber("heading", currentHeading);
+    if (!headedRight) {
+      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
+    } else {
+      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
+    }
+
+    currentFinderTick++;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (lidar.getDistance() < 4) {
+      swerveDrive.stopModules();
+      return true;
+    } else if (foundReef && lidar.withinDistance()) { // spot on
+      swerveDrive.stopModules();
+      return true;
+    } else if (foundReef && !lidar.withinDistance()) { // over shot
+      speed = speed / 2;
+      headedRight = !headedRight;
+      currentFinderTick = 0;
+      foundReef = false;
+      return false;
+    } else if (bounces >= 3) {
+      swerveDrive.stopModules();
+      return true;
+    } else {
+      return false;
+    }
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitElevatorRefrence.java

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.wait;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.elevator.Elevator;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitElevatorRefrence extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitElevatorRefrence(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevator.elevatorAtReference();
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitEndefectorRefrence.java

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.wait;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.elevator.Elevator;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitEndefectorRefrence extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitEndefectorRefrence(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevator.endeffectorAtReference();
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitFeedCoral.java

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.wait;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.elevator.Elevator;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitFeedCoral extends Command {
+  /** Creates a new waitElevatorRefrence. */
+  private Elevator elevator;
+  public waitFeedCoral(Elevator elevator) {
+    this.elevator = elevator;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevator.hasCoral();
+  }
+}

src/main/java/frc4388/robot/commands/wait/waitSupplier.java

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.wait;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class waitSupplier extends Command {
+  /** Creates a new waitSupplier. */
+  private final Supplier<Boolean> truth;
+  public waitSupplier(Supplier<Boolean> truth) {
+    this.truth = truth;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return truth.get();
+  }
+}

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -0,0 +1,19 @@
+package frc4388.robot.constants;
+
+/**
+ * Automatically generated file containing build version information.
+ */
+public final class BuildConstants {
+  public static final String MAVEN_GROUP = "";
+  public static final String MAVEN_NAME = "2025RidgeScape";
+  public static final String VERSION = "unspecified";
+  public static final int GIT_REVISION = 173;
+  public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513";
+  public static final String GIT_DATE = "2025-07-17 09:15:19 MDT";
+  public static final String GIT_BRANCH = "advantagekit";
+  public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT";
+  public static final long BUILD_UNIX_TIME = 1752774931371L;
+  public static final int DIRTY = 1;
+
+  private BuildConstants(){}
+}

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

@@ -0,0 +1,146 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.constants;
+
+import com.ctre.phoenix6.configs.Slot0Configs;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotBase;
+import frc4388.utility.compute.Trim;
+import frc4388.utility.status.CanDevice;
+import frc4388.utility.structs.Gains;
+import frc4388.utility.structs.LEDPatterns;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+    public static final String CANBUS_NAME = "rio";
+
+    public static final class LiDARConstants {
+        public static final int REEF_LIDAR_DIO_CHANNEL = 7;
+        public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
+
+        public static final int HUMAN_PLAYER_STATION_DISTANCE = 40;
+
+        public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
+        public static final int LIDAR_MICROS_TO_CM = 10;
+        public static final int SECONDS_TO_MICROS = 1000000;
+    }
+
+    public static final class AutoConstants {
+        // public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
+        public static final Gains XY_GAINS = new Gains(8,0,0.0);
+        // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
+        // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
+        // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
+       // public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
+
+        // public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
+        // public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
+
+        public static final Trim X_OFFSET_TRIM =        new Trim("X Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
+        // public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 1.5);
+        public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
+         
+        public static final double XY_TOLERANCE = 0.07; // Meters
+        public static final double ROT_TOLERANCE = 5; // Degrees
+
+        public static final double MIN_XY_PID_OUTPUT = 0.0;
+        public static final double MIN_ROT_PID_OUTPUT = 0.0;
+
+        public static final double VELOCITY_THRESHHOLD = 0.01;
+    }
+  
+
+    public static final class VisionConstants { 
+        public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT";
+        public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT";
+
+        public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
+        public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(Units.inchesToMeters(4.547), -Units.inchesToMeters(8.031), Units.inchesToMeters(8.858)), new Rotation3d(0,0.0,0.0));
+        
+        public static final double MIN_ESTIMATION_DISTANCE = 2; // Meters
+
+        // Photonvision thing
+        // The standard deviations of our vision estimated poses, which affect correction rate
+        // X, Y, Theta
+        // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2
+        public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4);
+        public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1);
+    }
+
+    public static final class FieldConstants {
+        public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
+
+        // Test april tag field layout
+        // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
+        //     Arrays.asList(new AprilTag[] {
+        //         new AprilTag(1, new Pose3d(
+        //             new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
+        //         )),
+        //     }), 100, 100);
+
+    }
+
+    public static final class LEDConstants {
+        public static final int LED_SPARK_ID = 9;
+
+        public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
+
+        public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED;
+        public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW;
+        public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK;
+        public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW;
+
+        public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES;
+        public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES;
+    }
+
+    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 BUTTONBOX_ID = 2;
+        public static final int XBOX_PROGRAMMER_ID = 3;
+        public static final double LEFT_AXIS_DEADBAND = 0.1;
+
+    }
+
+
+
+    // Logging constants
+    public static class SimConstants {
+        public static final Mode simMode = Mode.SIM;
+        public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode;
+
+        public static enum Mode {
+            /** Running on a real robot. */
+            REAL,
+
+            /** Running a physics simulator. */
+            SIM,
+
+            /** Replaying from a log file. */
+            REPLAY
+        }
+    }
+}

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

@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.subsystems;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.LEDConstants;
+import frc4388.utility.status.Status;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status.ReportLevel;
+import frc4388.utility.structs.LEDPatterns;
+
+/**
+ * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
+ * Driver
+ */
+public class LED extends SubsystemBase implements Queryable {
+  public LED() {
+    FaultReporter.register(this);
+  }
+
+  private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+  private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN;
+
+  public void setMode(LEDPatterns pattern){
+    this.mode = pattern;
+  }
+
+  @Override
+  public void periodic() {
+    update();
+  }
+
+  public void update() {
+    if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return;
+
+    if(DriverStation.isDisabled()){
+      LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue());
+    }else
+      LEDController.set(mode.getValue());
+  }
+
+  @AutoLogOutput
+  public String state() {
+    return mode.getClass().toString();
+  }
+
+  @Override
+  public String getName() {
+    return "LEDs";
+  }
+
+  @Override
+  public Status diagnosticStatus() {
+    Status status = new Status();
+
+    if(!LEDController.isAlive())
+      status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
+    
+    status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());
+
+    return status;
+  }
+  
+
+}

src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java

@@ -0,0 +1,58 @@
+package frc4388.robot.subsystems.lidar;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.LiDARConstants;
+import frc4388.utility.status.Status;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status.ReportLevel;
+
+public class LiDAR extends SubsystemBase implements Queryable {
+    LidarIO io;
+    LidarStateAutoLogged state = new LidarStateAutoLogged();
+
+    private String name = "Lidar";
+
+    public LiDAR(LidarIO device, String name) {
+        FaultReporter.register(this);
+
+        this.io = device;
+        this.name = name;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(state);
+        Logger.processInputs("LiDAR/"+name, state);
+    }
+
+    // @AutoLogOutput(key = "Lidar/{name}")
+    public double getDistance(){
+        return state.distance;
+    }
+
+    public boolean withinDistance(){
+        if(state.distance == -1) return false;
+        return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
+    }
+
+    @Override
+    public String getName() {
+        return "Lidar " + name;
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status s = new Status();
+
+        if(state.distance == -1)
+            s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
+        
+        s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance);
+
+        return s;
+    }
+    
+}

src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java

@@ -0,0 +1,13 @@
+package frc4388.robot.subsystems.lidar;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface LidarIO {
+    @AutoLog
+    public class LidarState {
+        public boolean connected;
+        public double distance;
+    }
+    
+    public default void updateInputs(LidarState state) {}
+}

src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java

@@ -0,0 +1,27 @@
+package frc4388.robot.subsystems.lidar;
+
+import edu.wpi.first.wpilibj.Counter;
+import frc4388.robot.constants.Constants.LiDARConstants;
+
+// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
+public class LidarReal implements LidarIO {
+
+    
+    private Counter LidarPWM;
+
+    public LidarReal(int port) {
+        LidarPWM = new Counter(port);
+        LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
+        LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
+        LidarPWM.reset();
+    }
+
+    @Override
+    public void updateInputs(LidarState state) {
+        
+        if(LidarPWM.get() < 1)
+            state.distance = -1;
+        else
+            state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
+    }
+}

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

@@ -0,0 +1,467 @@
+// 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.swerve;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.swerve.SwerveRequest;
+import edu.wpi.first.math.geometry.Pose2d;
+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.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.subsystems.vision.Vision;
+import frc4388.utility.compute.TimesNegativeOne;
+import frc4388.utility.status.Status;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.util.PathPlannerLogging;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.RobotConfig;
+
+public class SwerveDrive extends SubsystemBase implements Queryable {
+    // private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+
+    private SwerveIO io;
+    private SwerveStateAutoLogged state;
+
+    private Vision vision;
+
+
+
+    public int gear_index = SwerveDriveConstants.STARTING_GEAR;
+    public boolean stopped = false;
+    public boolean robotKnowsWhereItIs = false;
+
+    public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
+    public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
+    public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
+                                                                                            // 25%
+
+    public double lastOdomSpeed;
+
+    public Pose2d initalPose2d = new Pose2d();
+
+
+    public double rotTarget = 0.0;
+    public Rotation2d orientRotTarget = new Rotation2d();
+    public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+
+    /** Creates a new SwerveDrive. */
+    public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
+        // public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
+        // swerveDriveTrain) {
+        FaultReporter.register(this);
+
+        this.io = swerveDriveTrain;
+        this.state = new SwerveStateAutoLogged();
+
+        this.vision = vision;
+
+        RobotConfig config;
+        try {
+            config = RobotConfig.fromGUISettings();
+        } catch (Exception e) {
+            // Handle exception as needed
+            config = null;
+        }
+        // DoubleSupplier a = () -> 1.d;
+        AutoBuilder.configure(
+                () -> {
+                    return getPose2d();
+                }, // Robot pose supplier
+                this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
+                                             // pose)
+                () -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
+                        .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
+                                              // Also optionally outputs individual module feedforwards
+                new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
+                                                // holonomic drive trains
+                        new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
+                        new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
+                ),
+                config, // The robot configuration
+                () -> {
+                    // Boolean supplier that controls when the path will be mirrored for the red
+                    // alliance
+                    // This will flip the path being followed to the red side of the field.
+                    // THE ORIGIN WILL REMAIN ON THE BLUE SIDE
+
+                    // var alliance = DriverStation.getAlliance();
+                    // if (alliance.isPresent()) {
+                    //     return alliance.get() == DriverStation.Alliance.Red;
+                    // }
+                    return TimesNegativeOne.isRed;
+                },
+                this // Reference to this subsystem to set requirements
+        );
+
+        PathPlannerLogging.setLogActivePathCallback(
+        (activePath) -> {
+          Logger.recordOutput(
+              "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()]));
+        });
+        
+        PathPlannerLogging.setLogTargetPoseCallback(
+        (targetPose) -> {
+          Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose);
+        });
+
+
+
+        // // Configure SysId
+        // sysId =
+        //     new SysIdRoutine(
+        //         new SysIdRoutine.Config(
+        //             null,
+        //             null,
+        //             null,
+        //             (state) -> Logger.recordOutput("Drive/SysIdState", toString())),
+        //         new SysIdRoutine.Mechanism(
+        //             (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
+
+    }
+
+    public void setOdoPose(Pose2d pose) {
+        if (pose == null) return;
+        initalPose2d = pose;
+        io.resetPose(pose);
+    }
+
+    // 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) {
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
+            stopModules(); // stop the swerve
+
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
+            return; // don't bother doing swerve drive math and return early.
+
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+        
+        stopped = false;
+        if (fieldRelative) {
+            
+            leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
+            rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);    
+
+            // ! drift correction
+            if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
+
+                rotTarget = state.currentPose.getRotation().getDegrees();
+
+                io.setControl(new SwerveRequest.FieldCentric()
+                    .withVelocityX(leftStick.getX() * speedAdjust)
+                    .withVelocityY(leftStick.getY() * speedAdjust)
+                    .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+
+                    // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
+                SmartDashboard.putBoolean("drift correction", false);
+            } else {
+                var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+                    .withVelocityX(leftStick.getX() * speedAdjust)
+                    .withVelocityY(leftStick.getY() * speedAdjust)
+                    .withTargetDirection(Rotation2d.fromDegrees(rotTarget));
+                ctrl.HeadingController.setPID(
+                    SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
+                    SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
+                    SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
+                );
+                io.setControl(ctrl);
+                SmartDashboard.putBoolean("drift correction", true);
+            }
+
+           
+        } else { // Create robot-relative speeds.
+            io.setControl(new SwerveRequest.RobotCentric()
+                    .withVelocityX(leftStick.getX() * speedAdjust)
+                    .withVelocityY(-leftStick.getY() * speedAdjust)
+                    .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+        }
+    }
+
+    public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
+        stopped = false;
+        // Create robot-relative speeds.
+        if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
+        io.setControl(new SwerveRequest.RobotCentric()
+            .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
+            .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
+            .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+        
+    }
+
+
+    public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
+                                                                                               // reason to have a robot
+                                                                                               // relitive version of
+                                                                                               // this, and no pre
+                                                                                               // provided version
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve
+                                                                                           // drive is still going:
+            stopModules(); // stop the swerve
+
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
+            return; // don't bother doing swerve drive math and return early.
+
+        leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+
+        io.setControl(new SwerveRequest.FieldCentricFacingAngle()
+                .withVelocityX(leftStick.getX() * speedAdjust)
+                .withVelocityY(leftStick.getY() * speedAdjust)
+                .withTargetDirection(rightStick.getAngle()));
+    }
+
+    public void driveRelativeAngle(Translation2d leftStick, Rotation2d heading) {
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+        leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
+        var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+            .withVelocityX(leftStick.getX() * speedAdjust)
+            .withVelocityY(leftStick.getY() * speedAdjust)
+            .withTargetDirection(heading);
+        ctrl.HeadingController.setPID(
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
+        );
+        io.setControl(ctrl);
+    }
+
+    public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
+        leftStick = leftStick.rotateBy(heading);
+
+        var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+            .withVelocityX(leftStick.getX() * speedAdjust)
+            .withVelocityY(leftStick.getY() * speedAdjust)
+            .withTargetDirection(heading);
+        // ctrl.HeadingController.setPID(
+        //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
+        //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
+        //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
+        // );
+        io.setControl(ctrl);
+    }
+
+    public void activateLuigiMode() {
+        io.setLimits(20);
+    }
+
+    public void deactivateLuigiMode() {
+        io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
+    }
+
+    public boolean rotateToTarget(double angle) {
+        io.setControl(new SwerveRequest.FieldCentricFacingAngle()
+                .withVelocityX(0)
+                .withVelocityY(0)
+                .withTargetDirection(Rotation2d.fromDegrees(angle)));
+
+        if (Math.abs(angle - getGyroAngle()) < 5.0) {
+            return true;
+        }
+
+        return false;
+    }
+
+    public boolean isStopped() {
+        return lastOdomSpeed < AutoConstants.STOP_VELOCITY;
+    }
+
+    public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
+        // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the
+        // swerve drive is still going:
+        // stopModules(); // stop the swerve
+
+        // if (leftStick.getNorm() < 0.05) //if no imput
+        // return; // don't bother doing swerve drive math and return early.
+
+        leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
+
+        io.setControl(new SwerveRequest.FieldCentricFacingAngle()
+                .withVelocityX(leftStick.getX() * -speedAdjust)
+                .withVelocityY(leftStick.getY() * speedAdjust)
+                .withTargetDirection(rot));
+        // double
+    }
+
+    public double getGyroAngle() {
+        return getPose2d().getRotation().getRadians();
+    }
+
+    public Pose2d getPose2d() {
+        if(state.currentPose == null)
+            return initalPose2d;
+        return state.currentPose;
+    }
+
+    public void resetGyro() {
+        io.tareEverything();
+        robotKnowsWhereItIs = false;
+        rotTarget = 0;
+        // vision.resetRotations();
+    }
+
+
+    public void softStop() {
+        stopped = true;
+        io.setControl(new SwerveRequest.FieldCentric()
+            .withVelocityX(0)
+            .withVelocityY(0)
+            .withRotationalRate(0)
+        ); // stop the modules without breaking
+    }
+
+    public void stopModules() {
+        // stopped = true;
+        // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());
+        softStop();
+    }
+
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run\
+        SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
+        SmartDashboard.putNumber("RotTartget", rotTarget);
+
+        io.updateInputs(state);
+        Logger.processInputs("SwerveDrive", state);
+        
+        vision.setLastOdomPose(state.currentPose);
+        setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
+
+        if (vision.isTag()) {
+            Pose2d pose = vision.getPose2d();
+            if (!robotKnowsWhereItIs) {
+                robotKnowsWhereItIs = true;
+                Pose2d curPose = getPose2d();
+                rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees();
+            }
+
+            io.addVisionMeasurement(vision.getPosesToAdd());
+        }
+
+        // if(e.isPresent())
+    }
+
+    private void reset_index() {
+        gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
+                                                         // robot start in?)
+    }
+
+    public void shiftDown() {
+        if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
+            reset_index(); // If outof bounds: reset index
+        int i = gear_index - 1;
+        if (i == -1)
+            i = 0;
+        setPercentOutput(SwerveDriveConstants.GEARS[i]);
+        gear_index = i;
+    }
+
+    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 = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
+        gear_index = -1;
+    }
+
+    public void setToSlow() {
+        setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
+        gear_index = 0;
+    }
+
+    public void setToFast() {
+        setPercentOutput(SwerveDriveConstants.FAST_SPEED);
+        gear_index = 1;
+    }
+
+    public void setToTurbo() {
+        setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
+        gear_index = 2;
+    }
+
+    public void shiftUpRot() {
+        rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
+    }
+
+    public void shiftDownRot() {
+        rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
+    }
+
+    private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR;
+
+    public void startSlowPeriod() {
+        tmp_gear_index = gear_index;
+        setToSlow();
+    }
+
+    public void startTurboPeriod() {
+        tmp_gear_index = gear_index;
+        setToTurbo();
+    }
+
+    public void endSlowPeriod() {
+        setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
+        gear_index = tmp_gear_index;
+    }
+
+
+
+    public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){
+        if(curPose != null && lastPose != null){
+            lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq;
+        }
+    }
+
+    @AutoLogOutput(key="SwerveDrive/speed ")
+    public double getOdometrySpeed() {
+        return lastOdomSpeed;
+    }
+    
+
+
+    @Override
+    public String getName() {
+        return "Swerve Drive Controller";
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status status = new Status();
+
+        // status.addReport(ReportLevel.INFO,
+        //         "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there.");
+
+        return status;
+    }
+}
+

src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java

@@ -0,0 +1,246 @@
+package frc4388.robot.subsystems.swerve;
+
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Rotations;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+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.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
+import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Distance;
+import frc4388.utility.status.CanDevice;
+import frc4388.utility.structs.Gains;
+
+
+// No mans land
+// Beware, there be dragons.
+public final class SwerveDriveConstants {
+    public static final double MAX_ROT_SPEED        = Math.PI * 2;
+    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 double CORRECTION_MIN = 10;
+    public static final double CORRECTION_MAX = 50;
+    
+    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[] GEARS = {SLOW_SPEED, FAST_SPEED, TURBO_SPEED};
+    public static final int STARTING_GEAR = 0;
+    // Dimensions
+    public static final double WIDTH = 18.5; // TODO: validate
+    public static final double HEIGHT = 18.5; // TODO: validate
+    public static final double HALF_WIDTH = WIDTH / 2.d;
+    public static final double HALF_HEIGHT = HEIGHT / 2.d;
+
+    // Mechanics
+    private static final double COUPLE_RATIO = 3; //todo: find
+    private static final double DRIVE_RATIO = 6.12;
+    private static final double STEER_RATIO = (150/7);
+    private static final Distance WHEEL_RADIUS = Inches.of(2);
+    
+    public static final double MAX_SPEED_MEETERS_PER_SEC = 6.22; // TODO: Validate
+
+    public static final double MAX_SPEED_FEET_PER_SECOND = MAX_SPEED_MEETERS_PER_SEC * 3.28084;
+    public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI;
+
+    // Operation
+    public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
+
+    public static final boolean DRIFT_CORRECTION_ENABLED = true;
+    public static final boolean INVERT_X = false;
+    public static final boolean INVERT_Y = true;
+    public static final boolean INVERT_ROTATION = false;
+
+    // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
+
+    private static final class ModuleSpecificConstants { //2025
+        //Front Left
+        private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375);
+        private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
+        private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
+        private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
+        private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
+        private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
+        
+        //Front Right
+        private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375);
+        private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
+        private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
+        private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
+        private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
+        private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
+
+        //Back Left
+        private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5);
+        private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
+        private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
+        private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
+        private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
+        private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
+        
+        //Back Right
+        private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5);
+        private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
+        private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
+        private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
+        private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
+        private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
+    }
+
+    public static final class IDs {
+        public static final CanDevice RIGHT_FRONT_WHEEL   = new CanDevice("RIGHT_FRONT_WHEEL", 4);
+        public static final CanDevice RIGHT_FRONT_STEER   = new CanDevice("RIGHT_FRONT_STEER", 5);
+        public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 11);
+        
+        public static final CanDevice LEFT_FRONT_WHEEL    = new CanDevice("LEFT_FRONT_WHEEL", 2);
+        public static final CanDevice LEFT_FRONT_STEER    = new CanDevice("LEFT_FRONT_STEER", 3);
+        public static final CanDevice LEFT_FRONT_ENCODER  = new CanDevice("LEFT_FRONT_ENCODER", 10);
+        
+        public static final CanDevice LEFT_BACK_WHEEL     = new CanDevice("LEFT_BACK_WHEEL", 6);
+        public static final CanDevice LEFT_BACK_STEER     = new CanDevice("LEFT_BACK_STEER", 7);
+        public static final CanDevice LEFT_BACK_ENCODER   = new CanDevice("LEFT_BACK_ENCODER", 12);
+        
+        public static final CanDevice RIGHT_BACK_WHEEL    = new CanDevice("RIGHT_BACK_WHEEL", 8);  
+        public static final CanDevice RIGHT_BACK_STEER    = new CanDevice("RIGHT_BACK_STEER", 9);
+        public static final CanDevice RIGHT_BACK_ENCODER  = new CanDevice("RIGHT_BACK_ENCODER", 13);
+
+        public static final CanDevice DRIVE_PIGEON        = new CanDevice("DRIVE_PIGEON", 14);
+    }
+
+    public static final class PIDConstants {
+        public static final int SWERVE_SLOT_IDX = 0;
+        public static final int SWERVE_PID_LOOP_IDX = 1;
+        public static final Gains SWERVE_GAINS = new Gains(50, 0.0, 0.32, 0.0, 0, 0.0);
+
+        public static final Slot0Configs CURRENT_SWERVE_ROT_GAINS = new Slot0Configs()
+            .withKP(50).withKI(0).withKD(0.32)
+            .withKS(0).withKV(0).withKA(0);
+        
+            public static final Slot0Configs TEST_SWERVE_ROT_GAINS = new Slot0Configs()
+            .withKP(10).withKI(0).withKD(0.3)
+            .withKS(0).withKV(0).withKA(0);
+
+        public static final Gains TEST_SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0);
+
+        // The steer motor uses any SwerveModule.SteerRequestType control request with the
+        // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+        public static final Slot0Configs PREPROVIDED_STEER_GAINS = new Slot0Configs()
+            .withKP(100).withKI(0).withKD(0.6)
+            .withKS(0.1).withKV(1.91).withKA(0)
+            .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+        // When using closed-loop control, the drive motor uses the control
+        // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+        public static final Slot0Configs PREPROVIDED_DRIVE_GAINS = new Slot0Configs()
+            .withKP(0.1).withKI(0).withKD(0)
+            .withKS(0).withKV(0.124);
+        
+        public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1);
+        public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1);
+    }
+
+    public static final class Configurations {
+        public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
+        public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help.
+        public static final double NEUTRAL_DEADBAND = 0.04;
+
+        // POWER! (limiting)
+        private static final TalonFXConfiguration DRIVE_MOTOR_CONFIGS = new TalonFXConfiguration()
+            .withMotorOutput(
+                new MotorOutputConfigs()
+                    .withNeutralMode(NeutralModeValue.Brake)
+                    .withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND)
+            ).withOpenLoopRamps(
+                new OpenLoopRampsConfigs()
+                    .withDutyCycleOpenLoopRampPeriod(OPEN_LOOP_RAMP_RATE)
+            ).withClosedLoopRamps(
+                new ClosedLoopRampsConfigs()
+                    .withDutyCycleClosedLoopRampPeriod(CLOSED_LOOP_RAMP_RATE)
+        );
+        private static final TalonFXConfiguration STEER_MOTOR_CONFIGS = new TalonFXConfiguration()
+            .withCurrentLimits(
+                new CurrentLimitsConfigs()
+                    .withStatorCurrentLimit(40) // TODO: tune???
+                    .withStatorCurrentLimitEnable(true)
+                ).withMotorOutput(
+                    new MotorOutputConfigs()
+                        .withNeutralMode(NeutralModeValue.Brake)
+                        .withDutyCycleNeutralDeadband(NEUTRAL_DEADBAND)
+                // ).withOpenLoopRamps(
+                //     new OpenLoopRampsConfigs()
+                //         .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
+                // ).withClosedLoopRamps(
+                //     new ClosedLoopRampsConfigs()
+                //         .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE)
+        );
+        public static final double SLIP_CURRENT = 60; // TODO: Tune??? 
+    }
+
+    public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
+        .withPigeon2Id(IDs.DRIVE_PIGEON.id);
+
+    private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
+        new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() // holy verbosity batman.
+            .withDriveMotorGearRatio(DRIVE_RATIO)
+            .withSteerMotorGearRatio(STEER_RATIO)
+            .withCouplingGearRatio(COUPLE_RATIO)
+            .withWheelRadius(WHEEL_RADIUS)
+            .withSteerMotorGains(PIDConstants.PREPROVIDED_STEER_GAINS) // ?
+            .withDriveMotorGains(PIDConstants.PREPROVIDED_DRIVE_GAINS) // ?
+            .withSteerMotorClosedLoopOutput(ClosedLoopOutputType.Voltage)
+            .withDriveMotorClosedLoopOutput(ClosedLoopOutputType.Voltage)
+            .withSlipCurrent(Configurations.SLIP_CURRENT)
+            .withSpeedAt12Volts(MAX_SPEED_MEETERS_PER_SEC)
+            .withDriveMotorType(DriveMotorArrangement.TalonFX_Integrated)
+            .withSteerMotorType(SteerMotorArrangement.TalonFX_Integrated)
+            .withFeedbackSource(SteerFeedbackType.RemoteCANcoder)
+            .withDriveMotorInitialConfigs(Configurations.DRIVE_MOTOR_CONFIGS)
+            .withSteerMotorInitialConfigs(Configurations.STEER_MOTOR_CONFIGS);
+
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_LEFT =
+        ConstantCreator.createModuleConstants(
+            IDs.LEFT_FRONT_STEER.id, IDs.LEFT_FRONT_WHEEL.id, IDs.LEFT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_LEFT_ENCODER_OFFSET,
+            ModuleSpecificConstants.FRONT_LEFT_XPOS, ModuleSpecificConstants.FRONT_LEFT_YPOS, 
+            ModuleSpecificConstants.FRONT_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_LEFT_ENCODER_INVERTED
+    );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_RIGHT =
+        ConstantCreator.createModuleConstants(
+            IDs.RIGHT_FRONT_STEER.id, IDs.RIGHT_FRONT_WHEEL.id, IDs.RIGHT_FRONT_ENCODER.id, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_OFFSET,
+            ModuleSpecificConstants.FRONT_RIGHT_XPOS, ModuleSpecificConstants.FRONT_RIGHT_YPOS, 
+            ModuleSpecificConstants.FRONT_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.FRONT_RIGHT_ENCODER_INVERTED
+    );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_LEFT =
+        ConstantCreator.createModuleConstants(
+            IDs.LEFT_BACK_STEER.id, IDs.LEFT_BACK_WHEEL.id, IDs.LEFT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_LEFT_ENCODER_OFFSET,
+            ModuleSpecificConstants.BACK_LEFT_XPOS, ModuleSpecificConstants.BACK_LEFT_YPOS, 
+            ModuleSpecificConstants.BACK_LEFT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_LEFT_ENCODER_INVERTED
+    );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_RIGHT =
+        ConstantCreator.createModuleConstants(
+            IDs.RIGHT_BACK_STEER.id, IDs.RIGHT_BACK_WHEEL.id, IDs.RIGHT_BACK_ENCODER.id, ModuleSpecificConstants.BACK_RIGHT_ENCODER_OFFSET,
+            ModuleSpecificConstants.BACK_RIGHT_XPOS, ModuleSpecificConstants.BACK_RIGHT_YPOS, 
+            ModuleSpecificConstants.BACK_RIGHT_DRIVE_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_STEER_MOTOR_INVERTED, ModuleSpecificConstants.BACK_RIGHT_ENCODER_INVERTED
+    );
+    
+    // misc
+    public static final int TIMEOUT_MS = 30;
+    public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+}

src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java

@@ -0,0 +1,33 @@
+package frc4388.robot.subsystems.swerve;
+
+import java.util.List;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+
+public interface SwerveIO {
+    @AutoLog
+    public class SwerveState {
+        public Pose2d currentPose = null;
+        public Pose2d lastPose = null;
+        public ChassisSpeeds speeds = null;
+        public double odometryRate = 1;
+    }
+
+    public default void setControl(SwerveRequest ctrl) {}
+
+    public default void setLimits(double limitInAmps) {}
+
+    public default void tareEverything() {}
+
+    public default void resetPose(Pose2d pose) {}
+
+    public default void addVisionMeasurement(List<PoseObservation> poses) {}
+
+    public default void updateInputs(SwerveState state) {}
+}

src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java

@@ -0,0 +1,63 @@
+package frc4388.robot.subsystems.swerve;
+
+import java.util.List;
+
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+import com.ctre.phoenix6.swerve.SwerveModule;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import frc4388.robot.subsystems.vision.Vision;
+import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+
+public class SwerveReal implements SwerveIO {
+    SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+
+    public SwerveReal(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
+        this.swerveDriveTrain = swerveDriveTrain;
+        swerveDriveTrain.getOdometryFrequency();
+    }
+
+    @Override
+    public void updateInputs(SwerveState state) {
+        double time = Vision.getTime();
+        state.odometryRate = 1 / swerveDriveTrain.getOdometryFrequency();
+        state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null);
+        state.lastPose = swerveDriveTrain.samplePoseAt(time - state.odometryRate).orElse(null);
+        state.speeds = swerveDriveTrain.getState().Speeds;
+    }
+
+    @Override
+    public void setControl(SwerveRequest ctrl) {
+        swerveDriveTrain.setControl(ctrl);
+    }
+
+    @Override
+    public void tareEverything() {
+        swerveDriveTrain.tareEverything();
+    }
+
+    @Override
+    public void setLimits(double limitInAmps) {
+        for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {
+            var talonFXConfigurator = module.getDriveMotor().getConfigurator();
+            var talonFXConfigs = new TalonFXConfiguration();
+
+            talonFXConfigurator.refresh(talonFXConfigs);
+            talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps;
+            talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10;
+            talonFXConfigurator.apply(talonFXConfigs);
+        }
+    }
+
+    @Override
+    public void addVisionMeasurement(List<PoseObservation> poses) {
+        for(PoseObservation pose : poses) {
+            swerveDriveTrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp()));
+        }
+    }
+
+}

src/main/java/frc4388/robot/subsystems/vision/Vision.java

@@ -0,0 +1,95 @@
+package frc4388.robot.subsystems.vision;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status;
+
+public class Vision extends SubsystemBase implements Queryable {
+    VisionIO[] io;
+    VisionStateAutoLogged[] state;
+
+    
+    public Pose2d lastVisionPose = new Pose2d();
+    public Pose2d lastPhysOdomPose = new Pose2d();
+
+    public Vision(VisionIO... devices) {
+        FaultReporter.register(this);
+        io = devices;
+        state = new VisionStateAutoLogged[io.length];
+
+        for(int i = 0; i < io.length; i++) {
+            state[i] = new VisionStateAutoLogged();
+        }
+    }
+
+    @Override
+    public void periodic() {
+        for(int i = 0; i < io.length; i++) {
+            io[i].updateInputs(state[i]);
+            Logger.processInputs("Vision/Camera" + i , state[i]);
+        }
+    }
+
+    public List<PoseObservation> getPosesToAdd(){
+        List<PoseObservation> poses = new ArrayList<>();
+        for(int i = 0; i < state.length; i++) {
+            if(state[i].lastEstimatedPose != null) {
+                poses.add(state[i].lastEstimatedPose);
+            }
+        }
+
+        return poses;
+    }
+
+    public void setLastOdomPose(Pose2d pose){
+        if(pose != null)
+            lastPhysOdomPose = pose;
+    }
+
+    public boolean isTag(){
+        for(int i = 0; i < state.length; i++){
+            if(state[i].isTagDetected && state[i].isTagProcessed)
+                return true;
+        }
+        return false;
+    }
+
+    @AutoLogOutput
+    public Pose2d getPose2d() {
+        if(lastPhysOdomPose != null)
+            return lastPhysOdomPose;
+
+        // if(lastVisionPose != null)
+        //     return lastVisionPose;
+        return new Pose2d();
+
+    }
+
+    public static double getTime() {
+        return Utils.getCurrentTimeSeconds();
+    }
+
+
+    @Override
+    public Status diagnosticStatus() {
+        return new Status();
+        // // TODO Auto-generated method stub
+        // throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'");
+    }
+    
+}

src/main/java/frc4388/robot/subsystems/vision/VisionIO.java

@@ -0,0 +1,22 @@
+package frc4388.robot.subsystems.vision;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public interface VisionIO {
+    @AutoLog
+    public class VisionState {
+        public boolean isTagDetected = false;
+        public boolean isTagProcessed = false;
+        // public double latency = 0;
+        public PoseObservation lastEstimatedPose = null;
+    }
+
+    public static record PoseObservation(
+        Pose2d pose,
+        double timestamp
+    ) {}
+
+    public default void updateInputs(VisionState state) {}
+}

src/main/java/frc4388/robot/subsystems/vision/VisionReal.java

@@ -0,0 +1,158 @@
+package frc4388.robot.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonPipelineResult;
+import frc4388.robot.constants.Constants.FieldConstants;
+import frc4388.robot.constants.Constants.VisionConstants;
+
+public class VisionReal implements VisionIO {
+    // private PhotonCamera[] cameras;
+    // private PhotonPoseEstimator[] estimators;
+    
+    private PhotonCamera camera;
+    private PhotonPoseEstimator estimator;
+
+    // public List<EstimatedRobotPose> poses = new ArrayList<>();
+
+    
+    public VisionReal(PhotonCamera camera, Transform3d position){
+        this.camera = camera;
+        estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position);
+        estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+    }
+
+    // private Instant lastVisionTime = null;
+
+
+    public void updateInputs(VisionState state) {
+        state.isTagProcessed = false;
+        state.isTagDetected = false;
+
+        state.lastEstimatedPose = null;
+
+        var results = camera.getAllUnreadResults();
+
+        // If there are no more updates from the camera
+        if (results.size() == 0) 
+            return;
+
+        
+        var result = results.get(results.size()-1);
+
+        state.isTagDetected = state.isTagDetected | result.hasTargets();
+
+        // If there are no tags
+        if(!result.hasTargets())
+            return;
+
+        Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
+
+        // If the tag was failed to be processed
+        if(estimatedRobotPose.isEmpty())
+            return;
+        
+        EstimatedRobotPose pose = estimatedRobotPose.get();
+
+        state.lastEstimatedPose = new PoseObservation(pose.estimatedPose.toPose2d(), pose.timestampSeconds);
+
+        state.isTagProcessed = true;
+    }
+
+
+    /**
+     * The latest estimated robot pose on the field from vision data. This may be empty. This should
+     * only be called once per loop.
+     *
+     * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
+     * {@link getEstimationStdDevs}
+     *
+     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
+     *     used for estimation.
+     */
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) {
+        Optional<EstimatedRobotPose> visionEst = Optional.empty();
+
+        var targets = change.getTargets();
+        for(int i = targets.size()-1; i >= 0; i--){
+            Transform3d pos = targets.get(i).getBestCameraToTarget();
+            double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
+            if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
+                change.targets.remove(i);
+            }
+        }
+
+        if(targets.size() <= 0)
+            return visionEst; // Will be empty
+
+        visionEst = estimator.update(change);
+        // updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
+
+        return visionEst;
+    }
+
+    public String getName() {
+        return camera.getName();
+    }
+
+
+    // /**
+    //  * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
+    //  * deviations based on number of tags, estimation strategy, and distance from the tags.
+    //  *
+    //  * @param estimatedPose The estimated pose to guess standard deviations for.
+    //  * @param targets All targets in this camera frame
+    //  */
+    // private void updateEstimationStdDevs(
+    //         Optional<EstimatedRobotPose> estimatedPose, 
+    //         List<PhotonTrackedTarget> targets,
+    //         PhotonPoseEstimator estimator) {
+    //     if (estimatedPose.isEmpty()) {
+    //         // No pose input. Default to single-tag std devs
+    //         curStdDevs = VisionConstants.kSingleTagStdDevs;
+
+    //     } else {
+    //         // Pose present. Start running Heuristic
+    //         var estStdDevs = VisionConstants.kSingleTagStdDevs;
+    //         int numTags = 0;
+    //         double avgDist = 0;
+
+    //         // Precalculation - see how many tags we found, and calculate an average-distance metric
+    //         for (var tgt : targets) {
+    //             var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
+    //             if (tagPose.isEmpty()) continue;
+                
+    //             double distance = tagPose
+    //             .get()
+    //             .toPose2d()
+    //             .getTranslation()
+    //             .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+                
+    //             if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
+    //                 numTags++;
+    //                 avgDist += distance;
+    //             }
+    //         }
+
+    //         if (numTags == 0) {
+    //             // No tags visible. Default to single-tag std devs
+    //             curStdDevs = VisionConstants.kSingleTagStdDevs;
+    //         } else {
+    //             // One or more tags visible, run the full heuristic.
+    //             avgDist /= numTags;
+    //             // Decrease std devs if multiple targets are visible
+    //             if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
+    //             // Increase std devs based on (average) distance
+    //             if (numTags == 1 && avgDist > 4)
+    //                 estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    //             else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+    //             curStdDevs = estStdDevs;
+    //         }
+    //     }
+    // }
+}

src/main/java/frc4388/utility/DeferredBlock.java

@@ -0,0 +1,41 @@
+package frc4388.utility;
+
+import java.util.ArrayList;
+
+// Class for running code snippets whenever the robot is enabled.
+public class DeferredBlock {
+    private static ArrayList<Runnable> m_blocks_norerun = new ArrayList<>();
+    private static ArrayList<Runnable> m_blocks_rerun = new ArrayList<>();
+    private static boolean             m_hasRun = false;
+
+    public static void addBlock(Runnable block) {
+        addBlock(block, false);
+    }
+
+
+    public static void addBlock(Runnable block, boolean rerun) {
+        if(rerun) {
+            m_blocks_rerun.add(block);
+        } else {
+            m_blocks_norerun.add(block);
+        }
+    }
+
+    public static void execute() {
+
+        // Run blocks that run multiple times.
+        for (Runnable block : m_blocks_rerun) {
+            block.run();
+        }
+
+        // Run blocks that only run once
+        if (m_hasRun) return;
+
+        for (Runnable block : m_blocks_norerun) {
+            block.run();
+        }
+
+        m_blocks_norerun.clear(); // for garbage collection
+        m_hasRun = true;
+    }
+}

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

@@ -0,0 +1,35 @@
+package frc4388.utility.compute;
+
+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/compute/ReefPositionHelper.java

@@ -0,0 +1,106 @@
+package frc4388.utility.compute;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.constants.Constants.FieldConstants;
+
+public class ReefPositionHelper {
+    public enum Side {
+        LEFT,
+        RIGHT,
+        CENTER,
+        FAR_LEFT
+    }
+
+    public static final Pose2d[] RED_TAGS = {
+        FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(11).get().toPose2d()
+    };
+
+    public static final Pose2d[] BLUE_TAGS = {
+        FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(22).get().toPose2d()
+    };
+
+    public static double distanceTo(Pose2d first, Pose2d second){
+        return Math.sqrt(Math.pow(first.getX()  -  second.getX(),2) + Math.pow(first.getY()  -  second.getY(),2));
+    }
+
+
+    /*
+    * Function to loop through a list of tag locations to figure out closest one
+    */
+    public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){
+        if(locations.length <= 0) return new Pose2d();
+
+        Pose2d minPos = locations[0];
+        double minDistance = distanceTo(position,minPos);
+
+        for(int i = 1; i < locations.length; i++){
+            double dist = distanceTo(locations[i],position);
+            if(dist < minDistance){
+                minPos = locations[i];
+                minDistance = dist;
+            }
+        }
+        
+        System.out.println(minPos.getRotation().getDegrees());
+
+        return minPos;
+    }
+
+    /*
+     * Function to find closest tag location based on side
+     */
+    public static Pose2d getNearestTag(Pose2d position) {
+
+        if(TimesNegativeOne.isRed)
+            return getNearestTag(RED_TAGS, position);
+        else
+            return getNearestTag(BLUE_TAGS, position);
+    }
+
+    public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
+        return offset(getNearestTag(position), 
+        getSide(side) + xtrim,
+        ydistance); 
+    }
+
+    public static double getSide(Side side){
+        switch(side) {
+            case LEFT:
+                return -(AutoConstants.X_SCORING_POSITION_OFFSET);
+            case FAR_LEFT:
+                return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8));
+            case RIGHT:
+                return (AutoConstants.X_SCORING_POSITION_OFFSET);
+            case CENTER:
+                return 0;
+        }
+        assert false;
+        return 0;
+    }
+
+
+    public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){
+        Translation2d oldTranslation = oldPose.getTranslation();
+        
+        double rot = oldPose.getRotation().getRadians();
+
+        return new Pose2d(new Translation2d(
+            oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset,
+            oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset
+        ), oldPose.getRotation().rotateBy(Rotation2d.k180deg));
+    }
+}

src/main/java/frc4388/utility/compute/RobotTime.java

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.utility.compute;
+
+/**
+ * <p>Keeps track of Robot times like time passed, delta time, etc
+ * <p>All times are in milliseconds
+ */
+public class RobotTime {
+    private long m_currTime = System.currentTimeMillis();
+    public long m_deltaTime = 0;
+
+    private long m_startRobotTime = m_currTime;
+    public long m_robotTime = 0;
+    public long m_lastRobotTime = 0;
+
+    private long m_startMatchTime = 0;
+    public  long m_matchTime = 0;
+    public long m_lastMatchTime = 0;
+
+    public long m_frameNumber = 0;
+
+    /**
+     * Private constructor prevents other classes from instantiating
+     */
+    private RobotTime(){}
+
+    private static RobotTime instance = null;
+
+    /**
+     * Gets the instance of Robot Time. If there is no instance running one will be created.
+     * @return instance of Robot Time
+     */
+    public static RobotTime getInstance() {
+        if (instance == null) {
+            instance = new RobotTime();
+        }
+        return instance;
+    }
+
+    /**
+     * Call this once per periodic loop.
+     */
+    public void updateTimes() {
+        m_lastRobotTime = m_robotTime;
+        m_lastMatchTime = m_matchTime;
+
+        m_currTime = System.currentTimeMillis();
+        m_robotTime = m_currTime - m_startRobotTime;
+        m_deltaTime = m_robotTime - m_lastRobotTime;
+        m_frameNumber++;
+
+        if (m_startMatchTime != 0) {
+            m_matchTime = m_currTime - m_startMatchTime;
+        }
+    }
+
+    /**
+     * Call this in both the auto and periodic inits
+     */
+    public void startMatchTime() {
+        if (m_startMatchTime == 0) {
+            m_startMatchTime = m_currTime;
+        }
+    }
+
+    /**
+     * Call this in disabled init
+     */
+    public void endMatchTime() {
+        m_startMatchTime = 0;
+        m_matchTime = 0;
+    }
+}

src/main/java/frc4388/utility/compute/RobotUnits.java

@@ -0,0 +1,27 @@
+// 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.utility.compute;
+
+/** Aarav's good units class (better than WPILib)
+ * @author Aarav Shah */
+
+public class RobotUnits {
+  // constants
+
+  // angle conversions
+  public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
+
+  public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
+
+  // falcon conversions
+  public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
+
+  public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
+
+  // distance conversions
+  public static double metersToFeet(final double meters) {return meters * 3.28084;}
+
+  public static double feetToMeters(final double feet) {return feet / 3.28084;}
+}

src/main/java/frc4388/utility/compute/TimesNegativeOne.java

@@ -0,0 +1,51 @@
+package frc4388.utility.compute;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
+
+// Class that holds weather the drivers sticks should be inverted
+public class TimesNegativeOne {
+
+    public static boolean XAxis = SwerveDriveConstants.INVERT_X;
+    public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
+    public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
+    public static boolean isRed = false;
+    public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET);
+
+    private static boolean isRed() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        if(alliance.isEmpty()) return false;
+        return (alliance.get() == Alliance.Red);
+    }
+
+    public static void update(){
+        isRed = isRed();
+        XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
+        YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
+        RotAxis = SwerveDriveConstants.INVERT_ROTATION;
+        ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
+        SmartDashboard.putBoolean("Is red alliance", isRed);
+    }
+
+    public static double invert(double num, boolean invert){
+        return invert ? -num : num;
+    }
+
+    public static Translation2d invert(Translation2d stick, boolean invertXY){
+        if(invertXY) return stick.times(-1);
+        else return stick;
+    }
+
+    public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){
+        return new Translation2d(
+            invert(stick.getX(), invertX), 
+            invert(stick.getY(), invertY)
+        );
+    }
+}

src/main/java/frc4388/utility/compute/Trim.java

@@ -0,0 +1,145 @@
+// 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.utility.compute;
+
+import java.io.FileInputStream;
+import java.io.FileOutputStream;
+import java.util.ArrayList;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+
+/**
+ * Reboot persistant Trims.
+ * @author Zachary Wilke
+ */
+public class Trim {
+    private static ArrayList<Trim> trims = new ArrayList<Trim>();
+    private static ShuffleboardTab trimTab = Shuffleboard.getTab("Trims");
+
+    private String trimName; 
+    private double upperBound;
+    private double lowerBound;
+    private double step;
+
+    private boolean modified = false;
+    private double currentValue;
+    private boolean persistant = false;
+
+    private GenericEntry trimElement = null;
+
+    /**
+     * Creates a variably Trim with a given name, upper and lower bounds, step size and intial value
+     * @param trimName please keep the trim name without special symbols
+     * @param upperBound the upper limit inclusive
+     * @param lowerBound the lower limit inclusive
+     * @param step the step size
+     * @param inital the inital value, will get overridden if the persistant trim exists on disk.
+     * @param persistnat Weather the trim is persistant or not
+     */
+    public Trim(String trimName, double upperBound, double lowerBound, double step, double inital, boolean persistant) {
+        this.trimName = trimName;
+        this.upperBound = upperBound;
+        this.lowerBound = lowerBound;
+        this.step = step;
+        this.persistant = persistant;
+        currentValue = inital;
+        load();
+        trimElement = trimTab.add(trimName, currentValue).getEntry();
+
+        trims.add(this);
+    }
+
+    /**
+     * Creates a non-Trim with a given name, upper and lower bounds, step size and intial value
+     * @param trimName please keep the trim name without special symbols
+     * @param upperBound the upper limit inclusive
+     * @param lowerBound the lower limit inclusive
+     * @param step the step size
+     * @param inital the inital value, will get overridden if the persistant trim exists on disk.
+     */
+    public Trim(String trimName, double upperBound, double lowerBound, double step, double inital) {
+        this.trimName = trimName;
+        this.upperBound = upperBound;
+        this.lowerBound = lowerBound;
+        this.step = step;
+        currentValue = inital;
+        load();
+        trimElement = trimTab.add(trimName, currentValue).getEntry();
+
+        trims.add(this);
+    }
+
+    private void clampModify() {
+        currentValue = Math.min(upperBound, Math.max(currentValue, lowerBound));
+        if (trimElement != null) 
+            trimElement.setValue(currentValue);
+        modified = true;
+    }
+
+    public void stepUp() {
+        this.currentValue += step;
+        clampModify();
+    }
+
+    public void stepDown() {
+        this.currentValue -= step;
+        clampModify();
+    }
+    
+    public void set(double value) {
+        this.currentValue = value;
+        clampModify();
+    }
+
+    public double get() {
+        return this.currentValue;
+    }
+
+    public boolean isModified() {
+        return modified;
+    }
+
+    public boolean load() {
+        if(!persistant)
+            return false;
+
+        try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
+            double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8));
+            currentValue = fileValue;
+            clampModify();
+            modified = false;
+            if (fileValue != currentValue) {
+                System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
+                modified = true;
+            }
+            return true;
+        } catch (Exception e) {
+            // e.printStackTrace();
+            System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value...");
+            return false;
+        }
+        
+    }
+
+    public void dump() {
+        try (FileOutputStream stream = new FileOutputStream("/home/lvuser/trims/" + trimName)) {
+            stream.write(DataUtils.doubleToByteArray(currentValue));
+            modified = false;
+        } catch (Exception e) {
+            // e.printStackTrace();
+            System.out.println("TRIMS: Unable to write to trim file `" + trimName + "`!?!");
+        }
+    }
+
+    public static void dumpAll() {
+        for (int i = 0; i < trims.size(); i++) {
+            Trim trim = trims.get(i);
+            if (trim.isModified()) 
+                trim.dump();
+        }
+    }
+}

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/ButtonBox.java

@@ -0,0 +1,19 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+public class ButtonBox extends GenericHID {
+    public static final int White = 1;
+    public static final int One = 2;
+    public static final int Two = 3;
+    public static final int Three = 4;
+    public static final int Four = 5;
+    public static final int Five = 6;
+    public static final int Six = 7;
+    public static final int Seven = 8;
+    public static final int Eight = 9;
+
+    public ButtonBox(int ID){
+        super(ID);
+    }
+}

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

@@ -0,0 +1,28 @@
+
+package frc4388.utility.controller;
+
+import static frc4388.robot.constants.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/IHandController.java

@@ -0,0 +1,21 @@
+package frc4388.utility.controller;
+
+/**
+ * Add your docs here.
+ */
+public interface IHandController {
+
+	public double getLeftXAxis();
+
+	public double getLeftYAxis();
+
+	public double getRightXAxis();
+	
+	public double getRightYAxis();
+
+	public double getLeftTriggerAxis();
+
+	public double getRightTriggerAxis();
+
+	public int getDpadAngle();
+}

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/XboxController.java

@@ -0,0 +1,218 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj.Joystick;
+
+/**
+ * This is a wrapper for the WPILib Joystick class that represents an XBox 
+ * controller.
+ * @author frc1675
+ */
+public class XboxController implements IHandController
+{
+	public static final int LEFT_X_AXIS = 0;
+	public static final int LEFT_Y_AXIS = 1;
+	public static final int LEFT_TRIGGER_AXIS = 2;
+	public static final int RIGHT_TRIGGER_AXIS = 3;
+	public static final int RIGHT_X_AXIS = 4;
+	public static final int RIGHT_Y_AXIS = 5;
+	public static final int LEFT_RIGHT_DPAD_AXIS = 6;
+	public static final int TOP_BOTTOM_DPAD_AXIS = 6;
+
+	public static final int A_BUTTON = 1;
+	public static final int B_BUTTON = 2;
+	public static final int X_BUTTON = 3;
+	public static final int Y_BUTTON = 4;
+	public static final int LEFT_BUMPER_BUTTON = 5;
+	public static final int RIGHT_BUMPER_BUTTON = 6;
+	public static final int BACK_BUTTON = 7;
+	public static final int START_BUTTON = 8;
+
+	public static final int LEFT_JOYSTICK_BUTTON = 9;
+	public static final int RIGHT_JOYSTICK_BUTTON = 10;
+
+	private static final double LEFT_DPAD_TOLERANCE = -0.9;
+	private static final double RIGHT_DPAD_TOLERANCE = 0.9;
+	private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
+	private static final double TOP_DPAD_TOLERANCE = 0.9;
+
+	private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
+	private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
+
+	private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
+	private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
+	private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
+	private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
+
+	private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
+	private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
+	private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
+	private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
+
+	private static final double DEADZONE = 0.1;
+
+	private Joystick m_stick;
+
+	/**
+     * Add your docs here.
+     */
+	public XboxController(int portNumber){
+		m_stick = new Joystick(portNumber);
+	}
+
+	/**
+     * Add your docs here.
+     */
+	public Joystick getJoyStick() {
+		return m_stick;
+	}
+	
+	/**
+	 * Checks if the input falls within the deadzone.
+	 * @param input from an axis on the controller
+	 * @return true if input falls in deadzone, false if input falls outside deadzone
+	 */
+	private boolean inDeadZone(double input){
+		return (Math.abs(input) < DEADZONE);
+	}
+
+	/**
+	 * Updates an input to have a deadzone around the 0 position
+	 * @param input from an axis on the controller
+	 * @return updated input
+	 */
+	private double getAxisWithDeadZoneCheck(double input){
+		if(inDeadZone(input)){
+			return 0.0;       
+		} else {
+			return input;
+		}
+	}
+
+	public boolean getAButton(){
+		return m_stick.getRawButton(A_BUTTON);
+	}
+
+	public boolean getXButton(){
+		return m_stick.getRawButton(X_BUTTON);    
+	}
+
+	public boolean getBButton(){
+		return m_stick.getRawButton(B_BUTTON);
+	}
+
+	public boolean getYButton(){
+		return m_stick.getRawButton(Y_BUTTON);
+	} 
+
+	public boolean getBackButton(){
+		return m_stick.getRawButton(BACK_BUTTON);
+	}
+
+	public boolean getStartButton(){
+		return m_stick.getRawButton(START_BUTTON);
+	} 
+
+	public boolean getLeftBumperButton(){
+		return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
+	}
+
+	public boolean getRightBumperButton(){
+		return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
+	}
+
+	public boolean getLeftJoystickButton(){
+		return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
+	}
+
+	public boolean getRightJoystickButton(){
+		return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
+	}
+
+	public double getLeftXAxis(){         
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); 
+	}
+
+	public double getLeftYAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
+	}
+
+	public double getRightXAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
+	}
+
+	public double getRightYAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); 
+	}
+
+	public double getLeftTriggerAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); 
+	}
+
+	public double getRightTriggerAxis(){
+		return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); 
+	}
+
+	/**
+	 * Get the angle input from the dpad.
+	 * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
+	 */
+	public int getDpadAngle() {
+		return m_stick.getPOV(0);
+	}
+
+	public boolean getDPadLeft(){
+      return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
+  	}
+
+  	public boolean getDPadRight(){
+      return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
+  	}
+
+  	public boolean getDPadTop(){
+      return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
+  	}
+
+  	public boolean getDPadBottom(){
+      return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
+  	}
+
+	public boolean getLeftTrigger(){
+		return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
+	}
+
+	public boolean getRightTrigger(){
+		return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
+	}   
+
+	public boolean getRightAxisUpTrigger(){
+		return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
+	}   
+
+	public boolean getRightAxisDownTrigger(){
+		return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
+	}   
+
+	public boolean getRightAxisLeftTrigger(){
+		return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
+	}   
+
+	public boolean getRightAxisRightTrigger(){
+		return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisUpTrigger(){
+		return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisDownTrigger(){
+		return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisLeftTrigger(){
+		return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
+	}   
+
+	public boolean getLeftAxisRightTrigger(){
+		return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
+	}
+}

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

@@ -0,0 +1,68 @@
+package frc4388.utility.controller;
+
+//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 Trigger {
+	public static final int RIGHT_TRIGGER = 0;
+	public static final int LEFT_TRIGGER = 1;
+	public static final int RIGHT_AXIS_UP_TRIGGER = 2;
+	public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
+	public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
+	public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
+	public static final int LEFT_AXIS_UP_TRIGGER = 6;
+	public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
+	public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
+	public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
+
+	private XboxController m_controller;
+	private int m_trigger;
+
+	/**
+	 * Creates a Trigger-Button mapped to a specific Xbox controller and trigger
+	 */
+	public XboxTriggerButton(XboxController controller, int trigger) {
+		m_controller = controller;
+		m_trigger = trigger;
+	}
+
+	/** {@inheritDoc} */
+	// @Override
+	public boolean get() {
+		if (m_trigger == RIGHT_TRIGGER) {
+			return m_controller.getRightTrigger();
+		}
+		else if (m_trigger == LEFT_TRIGGER) {
+			return m_controller.getLeftTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
+			return m_controller.getRightAxisUpTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
+			return m_controller.getRightAxisDownTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
+			return m_controller.getRightAxisRightTrigger();
+		}
+		else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
+			return m_controller.getRightAxisLeftTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
+			return m_controller.getLeftAxisUpTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
+			return m_controller.getLeftAxisDownTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
+			return m_controller.getLeftAxisRightTrigger();
+		}
+		else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
+			return m_controller.getLeftAxisLeftTrigger();
+		}
+		return false;
+	}
+}

src/main/java/frc4388/utility/status/Alerts.java

@@ -0,0 +1,10 @@
+package frc4388.utility.status;
+
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import frc4388.robot.RobotContainer;
+
+// Class to update a series of WPILIB Alerts
+public class Alerts {
+  public static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError);
+}

src/main/java/frc4388/utility/status/CanDevice.java

@@ -0,0 +1,53 @@
+package frc4388.utility.status;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import frc4388.utility.status.Status.ReportLevel;
+
+public class CanDevice {
+    public static List<CanDevice> devices = new ArrayList<>();
+
+    public String name;
+    public int id;
+    
+    public CanDevice(String name, int id) {
+        this.name = name;
+        this.id = id;
+
+        devices.add(this);
+    }
+
+
+    private boolean isAlive() {
+        return true; //TODO: Link this with Device Finder
+    }
+
+    public String getName() {
+        return "CAN ID " + this.id + " ( " + this.name + " ) ";
+    }
+
+    public void Log(String str){
+        System.out.println(getName() + " - " + str);
+    }
+
+    // public Status queryStatus() {
+    //      Status s = new Status();
+
+    //     s.addReport(ReportLevel.INFO, "TODO");
+
+    //     return s;
+    // }
+
+    public Status diagnosticStatus() {
+        Status s = new Status();
+        //TODO
+        s.addReport(ReportLevel.INFO, "Add CAN magic here");
+        boolean isAlive = isAlive();
+        s.addReport(isAlive ? ReportLevel.INFO : ReportLevel.ERROR, "Is Alive: " + isAlive);
+
+        return s;
+    }
+
+    
+}

src/main/java/frc4388/utility/status/FaultCANCoder.java

@@ -0,0 +1,76 @@
+package frc4388.utility.status;
+
+import com.ctre.phoenix6.controls.EmptyControl;
+import com.ctre.phoenix6.hardware.CANcoder;
+
+import frc4388.utility.status.Status.ReportLevel;
+
+public class FaultCANCoder implements Queryable {
+    private String name;
+    private CANcoder cancoder;
+
+    public static void addDevice(CANcoder cancoder, String name) {
+        FaultCANCoder p = new FaultCANCoder();
+
+        p.name = name;
+        p.cancoder = cancoder;
+
+        FaultReporter.register(p);
+    }
+
+    @Override
+    public String getName() {
+        return name;
+    }
+    
+    @Override
+    public Status diagnosticStatus() {
+        Status s = new Status();
+
+        boolean debounceBad = !QueryUtils.isDebounceOk(cancoder.getSupplyVoltage());
+        boolean emptyControlBad = cancoder.setControl(new EmptyControl()).value != 0;
+
+        if(debounceBad || emptyControlBad) {
+            s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + 
+            (debounceBad ? " Failed debounce test" : "") + 
+            (emptyControlBad ? " Failed empty control test" : "")
+            );
+        }
+
+        // faults
+        if (cancoder.getFault_Hardware().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Hardware fault detected");
+        }
+        if (cancoder.getFault_BootDuringEnable().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Device booted while enabled");
+        }
+        if (cancoder.getFault_BadMagnet().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Bad magnet");
+        }
+        if (cancoder.getFault_Undervoltage().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Device supply voltage near brownout");
+        }
+        if (cancoder.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Unlicensed feature in use");
+        }
+    
+        // sticky faults
+        if (cancoder.getStickyFault_Hardware().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Hardware fault detected");
+        }
+        if (cancoder.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Device booted while enabled");
+        }
+        if (cancoder.getStickyFault_BadMagnet().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Bad magnet");
+        }
+        if (cancoder.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Device supply voltage near brownout");
+        }
+        if (cancoder.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Unlicensed feature in use");
+        }
+
+        return s;
+    }
+}

src/main/java/frc4388/utility/status/FaultPhotonCamera.java

@@ -0,0 +1,35 @@
+package frc4388.utility.status;
+
+import org.photonvision.PhotonCamera;
+
+import frc4388.utility.status.Status.ReportLevel;
+
+public class FaultPhotonCamera implements Queryable {
+    private String name;
+    private PhotonCamera cam;
+
+    public static void addDevice(PhotonCamera cam, String name) {
+        FaultPhotonCamera p = new FaultPhotonCamera();
+
+        p.name = name;
+        p.cam = cam;
+
+        FaultReporter.register(p);
+    }
+
+    @Override
+    public String getName() {
+        return name;
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status s = new Status();
+
+        if(!cam.isConnected())
+            s.addReport(ReportLevel.ERROR, "Not Connected!");
+
+        return s;
+    }
+}
+

src/main/java/frc4388/utility/status/FaultPidgeon2.java

@@ -0,0 +1,168 @@
+package frc4388.utility.status;
+
+import com.ctre.phoenix6.controls.EmptyControl;
+import com.ctre.phoenix6.hardware.Pigeon2;
+
+import frc4388.utility.status.Status.ReportLevel;
+
+public class FaultPidgeon2 implements Queryable {
+    private String name;
+    private Pigeon2 pigeon2;
+
+    public static void addDevice(Pigeon2 pigeon2, String name) {
+        FaultPidgeon2 p = new FaultPidgeon2();
+
+        p.name = name;
+        p.pigeon2 = pigeon2;
+
+        FaultReporter.register(p);
+    }
+
+    @Override
+    public String getName() {
+        return name;
+    }
+    
+    @Override
+    public Status diagnosticStatus() {
+        Status s = new Status();
+
+
+
+        boolean debounceBad = !QueryUtils.isDebounceOk(pigeon2.getSupplyVoltage());
+        boolean emptyControlBad = pigeon2.setControl(new EmptyControl()).value != 0;
+
+        if(debounceBad || emptyControlBad) {
+            s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + 
+            (debounceBad ? " Failed debounce test" : "") + 
+            (emptyControlBad ? " Failed empty control test" : "")
+            );
+        }
+
+
+        s.addReport(ReportLevel.INFO, "Voltage: " + pigeon2.getSupplyVoltage());
+
+        s.addReport(ReportLevel.INFO, "Pitch: " + pigeon2.getPitch());
+        s.addReport(ReportLevel.INFO, "Yaw: " + pigeon2.getYaw());
+        s.addReport(ReportLevel.INFO, "Roll: " + pigeon2.getRoll());
+
+        s.addReport(ReportLevel.INFO, "Acceleration X: " + pigeon2.getAccelerationX());
+        s.addReport(ReportLevel.INFO, "Acceleration Y: " + pigeon2.getAccelerationY());
+        s.addReport(ReportLevel.INFO, "Acceleration Z: " + pigeon2.getAccelerationZ());
+
+        s.addReport(ReportLevel.INFO, "Magnomiter X: " + pigeon2.getMagneticFieldX());
+        s.addReport(ReportLevel.INFO, "Magnomiter Y: " + pigeon2.getMagneticFieldY());
+        s.addReport(ReportLevel.INFO, "Magnomiter Z: " + pigeon2.getMagneticFieldZ());
+
+
+        // faults
+        if (pigeon2.getFault_BootDuringEnable().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Device booted while enabled");
+        }
+        if (pigeon2.getFault_BootIntoMotion().getValue() == Boolean.TRUE) {
+        s.addReport(ReportLevel.ERROR, "Device booted while in motion");
+        }
+        if (pigeon2.getFault_BootupAccelerometer().getValue() == Boolean.TRUE) {
+        s.addReport(ReportLevel.ERROR, "Accelerometer fault detected");
+        }
+        if (pigeon2.getFault_BootupGyroscope().getValue() == Boolean.TRUE) {
+        s.addReport(ReportLevel.ERROR, "Gyro fault detected");
+        }
+        if (pigeon2.getFault_BootupMagnetometer().getValue() == Boolean.TRUE) {
+        s.addReport(ReportLevel.ERROR, "Magnetometer fault detected");
+        }
+        if (pigeon2.getFault_DataAcquiredLate().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, 
+                "Motion stack data acquisition slower than expected");
+        }
+        if (pigeon2.getFault_Hardware().getValue() == Boolean.TRUE) {
+        s.addReport(ReportLevel.ERROR, "Hardware fault detected");
+        }
+        if (pigeon2.getFault_LoopTimeSlow().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, 
+                "Motion stack loop time was slower than expected");
+        }
+        if (pigeon2.getFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, "Accelerometer values are saturated");
+        }
+        if (pigeon2.getFault_SaturatedGyroscope().getValue() == Boolean.TRUE) {
+        s.addReport(ReportLevel.ERROR, "Gyro values are saturated");
+        }
+        if (pigeon2.getFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, "Magnetometer values are saturated");
+        }
+        if (pigeon2.getFault_Undervoltage().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, "Device supply voltage near brownout");
+        }
+        if (pigeon2.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
+        s.addReport(ReportLevel.ERROR, "Unlicensed feature in use");
+        }
+
+        // sticky faults
+        if (pigeon2.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, "[STICKY] Device booted while enabled");
+        }
+        if (pigeon2.getStickyFault_BootIntoMotion().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, "[STICKY] Device booted while in motion");
+        }
+        if (pigeon2.getStickyFault_BootupAccelerometer().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, "[STICKY] Accelerometer fault detected");
+        }
+        if (pigeon2.getStickyFault_BootupGyroscope().getValue() == Boolean.TRUE) {
+        s.addReport(ReportLevel.ERROR, "[STICKY] Gyro fault detected");
+        }
+        if (pigeon2.getStickyFault_BootupMagnetometer().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, "[STICKY] Magnetometer fault detected");
+        }
+        if (pigeon2.getStickyFault_DataAcquiredLate().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, 
+                String.format(
+                    "[STICKY] Motion stack data acquisition slower than expected"));
+        }
+        if (pigeon2.getStickyFault_Hardware().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, "[STICKY] Hardware fault detected");
+        }
+        if (pigeon2.getStickyFault_LoopTimeSlow().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, 
+                String.format(
+                    "[STICKY] Motion stack loop time was slower than expected"));
+        }
+        if (pigeon2.getStickyFault_SaturatedAccelerometer().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, 
+                "[STICKY] Accelerometer values are saturated");
+        }
+        if (pigeon2.getStickyFault_SaturatedGyroscope().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, "[STICKY] Gyro values are saturated");
+        }
+        if (pigeon2.getStickyFault_SaturatedMagnetometer().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, 
+                "[STICKY] Magnetometer values are saturated");
+        }
+        if (pigeon2.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, 
+                "[STICKY] Device supply voltage near brownout");
+        }
+        if (pigeon2.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
+        s.addReport(
+            ReportLevel.ERROR, "[STICKY] Unlicensed feature in use");
+        }
+      
+        return s;
+    }
+}

src/main/java/frc4388/utility/status/FaultReporter.java

@@ -0,0 +1,133 @@
+package frc4388.utility.status;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.CANBus.CANBusStatus;
+
+import frc4388.robot.constants.Constants;
+import frc4388.utility.status.Status.Report;
+import frc4388.utility.status.Status.ReportLevel;
+
+public class FaultReporter {
+
+    private static final String REPORTS_HEADER = 
+    "###############\n" + //
+    "#.............#\n" + //
+    "#...Reports...#\n" + //
+    "#.............#\n" + //
+    "###############\n";
+
+
+    private static final String CAN_HEADER = 
+    "###############\n" + //
+    "#.............#\n" + //
+    "#....CAN(t)...#\n" + //
+    "#.............#\n" + //
+    "###############\n";
+
+    private static final String ERROR_HEADER = 
+    "###############\n" + //
+    "#.............#\n" + //
+    "#....ERRORS...#\n" + //
+    "#.............#\n" + //
+    "###############\n";
+
+    private static List<Queryable> queryables = new ArrayList<>();
+
+    // public static void startThread() {
+    //     new Thread() {
+    //         public void run() {
+    //           try{
+    //           while(!this.isInterrupted() && this.isAlive()){
+    //             Thread.sleep(500);
+    //             for(int i=0;i<queryables.size(); i++){
+    //               queryables.get(i).queryStatus();
+    //             }
+      
+    //             // System.out.println("Updated statuses!");
+                
+    //           }
+    //           }catch(Exception e){
+    //               e.printStackTrace();
+    //           }
+    //         }
+    //     }.start();
+    // }
+
+    public static void register(Queryable q) {
+        queryables.add(q);
+    }
+
+    private static void Log(Queryable q, String s){
+        System.out.println(q.getName() + " - " + s);
+    }
+
+    public static void printReport() {
+
+        List<String> errors = new ArrayList<>();
+
+        // Subsystems header
+        System.out.println(REPORTS_HEADER);
+
+        for(int i=0;i< queryables.size();i++){
+
+            Queryable q = queryables.get(i);
+            System.out.println("** Subsystem diagnostic report for " + q.getName() + ":");
+            Status status = q.diagnosticStatus();
+
+            for(int a=0;a<status.reports.size();a++){
+                Report r = status.reports.get(a);
+                if(r.reportLevel == ReportLevel.ERROR)
+                errors.add(q.getName() + " - " + r.toString());
+                Log(q, r.toString());
+            }
+        }
+
+        
+        // CAN header
+        System.out.println(CAN_HEADER);
+
+        CANBus canBus = new CANBus(Constants.CANBUS_NAME);
+        
+        CANBusStatus canInfo = canBus.getStatus();
+        
+        System.out.println("CANInfo BusOffCount     - " + canInfo.BusOffCount);
+        System.out.println("CANInfo BusUtilization  - " + canInfo.BusUtilization);
+        System.out.println("CANInfo RX Errors count - " + canInfo.REC);
+        System.out.println("CANInfo TX Errors count - " + canInfo.TEC);
+        System.out.println("CANInfo Transmit buffer full count - " + canInfo.TxFullCount);
+        // Broken turniary operator
+        ReportLevel canReportLevel = canInfo.Status.isOK() ? (canInfo.Status.isWarning() ? ReportLevel.WARNING : ReportLevel.ERROR) : ReportLevel.INFO;
+        String canStatus = "CAN " + canReportLevel.name() + " - " + canInfo.Status.getName() + " (" + canInfo.Status.getDescription() + ")";
+        if(canReportLevel == ReportLevel.ERROR) {
+            errors.add(canStatus);
+        }
+        System.out.println(canStatus);
+
+        // for(int i=0;i<CanDevice.devices.size();i++){
+
+        //     CanDevice device = CanDevice.devices.get(i);
+        //     System.out.println("** CAN diagnostic report for " + device.name + ":");
+        //     Status status = device.diagnosticStatus();
+
+        //     for(int a=0;a<status.reports.size();a++){
+        //         Report r = status.reports.get(a);
+        //         if(r.reportLevel == ReportLevel.ERROR)
+        //         errors.add(device.getName() + " - " + r.toString());
+        //         device.Log(r.toString());
+        //     }
+        // }
+
+            // System.out.println("Found CAN devices: " + new DeviceFinder().Find());
+            
+        if(errors.size() > 0) {
+            // Errors header
+            System.out.println(ERROR_HEADER);
+            for(int i=0;i<errors.size(); i++){
+                System.out.println(errors.get(i));
+            }
+        }
+    }
+}

src/main/java/frc4388/utility/status/FaultTalonFX.java

@@ -0,0 +1,154 @@
+package frc4388.utility.status;
+
+import com.ctre.phoenix6.controls.EmptyControl;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import frc4388.utility.status.Status.ReportLevel;
+
+public class FaultTalonFX implements Queryable {
+    private String name;
+    private TalonFX motor;
+
+    public static void addDevice(TalonFX motor, String name) {
+        FaultTalonFX p = new FaultTalonFX();
+
+        p.name = name;
+        p.motor = motor;
+
+        FaultReporter.register(p);
+    }
+
+    @Override
+    public String getName() {
+        return name;
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status s = new Status();
+
+
+        boolean debounceBad = !QueryUtils.isDebounceOk(motor.getSupplyVoltage());
+        boolean emptyControlBad = motor.setControl(new EmptyControl()).value != 0;
+
+        if(debounceBad || emptyControlBad) {
+            s.addReport(ReportLevel.ERROR, "device is unreachable - Failed" + 
+            (debounceBad ? " Failed debounce test" : "") + 
+            (emptyControlBad ? " Failed empty control test" : "")
+            );
+        }
+
+
+        s.addReport(ReportLevel.INFO, "Voltage: " + motor.getSupplyVoltage());
+        s.addReport(ReportLevel.INFO, "Current: " + motor.getSupplyCurrent());
+        s.addReport(ReportLevel.INFO, "Device temp: " + motor.getDeviceTemp());
+        s.addReport(ReportLevel.INFO, "Processor temp: " + motor.getProcessorTemp());
+        s.addReport(ReportLevel.INFO, "Position: " + motor.getPosition());
+        s.addReport(ReportLevel.INFO, "Velocity: " + motor.getVelocity());
+        s.addReport(ReportLevel.INFO, "Acceleration: " + motor.getAcceleration());
+
+        // faults<
+        if (motor.getFault_BootDuringEnable().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Device booted while enabled");
+        }
+        if (motor.getFault_BridgeBrownout().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Bridge was disabled most likely due to supply voltage dropping too low");
+        }
+        if (motor.getFault_DeviceTemp().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Device temperature exceeded limit");
+        }
+        if (motor.getFault_FusedSensorOutOfSync().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Remote sensor is out of sync");
+        }
+        if (motor.getFault_Hardware().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Hardware failure detected");
+        }
+        if (motor.getFault_MissingDifferentialFX().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "The remote Talon used for differential control is not present");
+        }
+        if (motor.getFault_MissingHardLimitRemote().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR,"The remote limit switch device is not present");
+        }
+        if (motor.getFault_MissingSoftLimitRemote().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "The remote soft limit device is not present");
+        }
+        if (motor.getFault_OverSupplyV().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Supply voltage exceeded limit");
+        }
+        if (motor.getFault_ProcTemp().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Processor temperature exceeded limit");
+        }
+        if (motor.getFault_RemoteSensorDataInvalid().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "The remote sensor's data is no longer trusted");
+        }
+        if (motor.getFault_RemoteSensorPosOverflow().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "remote sensor position has overflowed");
+        }
+        if (motor.getFault_RemoteSensorReset().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "The remote sensor has reset");
+        }
+        if (motor.getFault_Undervoltage().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, " Device supply voltage near brownout");
+        }
+        if (motor.getFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Unlicensed feature in use");
+        }
+        if (motor.getFault_UnstableSupplyV().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "Supply voltage is unstable");
+        }
+  
+
+        // sticky faults
+        if (motor.getStickyFault_BootDuringEnable().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Device booted while enabled");
+        }
+        if (motor.getStickyFault_BridgeBrownout().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Bridge was disabled most likely due to supply voltage dropping too low");
+        }
+        if (motor.getStickyFault_DeviceTemp().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Device temperature exceeded limit");
+        }
+        if (motor.getStickyFault_FusedSensorOutOfSync().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Remote sensor is out of sync");
+        }
+        if (motor.getStickyFault_Hardware().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Hardware failure detected");
+        }
+        if (motor.getStickyFault_MissingDifferentialFX().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] The remote Talon used for differential control is not present");
+        }
+        if (motor.getStickyFault_MissingHardLimitRemote().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] The remote limit switch device is not present");
+        }
+        if (motor.getStickyFault_MissingSoftLimitRemote().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] The remote soft limit device is not present");
+        }
+        if (motor.getStickyFault_OverSupplyV().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Supply voltage exceeded limit");
+        }
+        if (motor.getStickyFault_ProcTemp().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Processor temperature exceeded limit");
+        }
+        if (motor.getStickyFault_RemoteSensorDataInvalid().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] The remote sensor's data is no longer trusted");
+        }
+        if (motor.getStickyFault_RemoteSensorPosOverflow().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] The remote sensor position has overflowed");
+        }
+        if (motor.getStickyFault_RemoteSensorReset().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] The remote sensor has reset");
+        }
+        if (motor.getStickyFault_Undervoltage().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Device supply voltage near brownout");
+        }
+        if (motor.getStickyFault_UnlicensedFeatureInUse().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Unlicensed feature in use");
+        }
+        if (motor.getStickyFault_UnstableSupplyV().getValue() == Boolean.TRUE) {
+            s.addReport(ReportLevel.ERROR, "[STICKY] Supply voltage is unstable");
+        }
+
+
+        return s;
+    }
+}

src/main/java/frc4388/utility/status/QueryUtils.java

@@ -0,0 +1,13 @@
+package frc4388.utility.status;
+
+import com.ctre.phoenix6.StatusSignal;
+
+import edu.wpi.first.math.filter.Debouncer;
+
+public class QueryUtils {
+    public static boolean isDebounceOk(@SuppressWarnings("rawtypes") StatusSignal status) {
+        Debouncer connectedDebounce = new Debouncer(0.5);
+        status.refresh();
+        return connectedDebounce.calculate(status.getStatus().isOK());
+    };
+}

src/main/java/frc4388/utility/status/Queryable.java

@@ -0,0 +1,8 @@
+package frc4388.utility.status;
+
+public interface Queryable {
+    // Get name of subsystem, for use in log.
+    String getName();
+    // Proactivly search for any errors in each subsystem
+    Status diagnosticStatus(); 
+}

src/main/java/frc4388/utility/status/Status.java

@@ -0,0 +1,85 @@
+package frc4388.utility.status;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.controls.EmptyControl;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+public class Status {
+    public enum ReportLevel {
+        INFO,
+        WARNING,
+        ERROR
+    }
+
+    public class Report {
+        public ReportLevel reportLevel;
+        public String description;
+
+        @Override
+        public String toString() {
+            return this.reportLevel.name() + ": " + this.description;
+        }
+    }
+
+    public List<Report> reports;
+
+    public Status() {
+        this.reports = new ArrayList<>();
+    }
+
+    public void addReport(ReportLevel level, String description) {
+        Report r = new Report();
+        r.reportLevel = level;
+        r.description = description;
+        this.reports.add(r);
+    }
+
+    private String printStatusCode(StatusCode status){
+        return status.getName() + " (" + status.value + ")";
+    }
+
+    public void diagnoseHardwareCTRE(String deviceName, TalonFX motor) {
+        addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: " + (motor.isAlive() ? "Alive." : "Dead!"));
+
+
+
+        if (motor.isAlive()) addReport(ReportLevel.INFO, deviceName + " Motor (TalonFX) Alive?: Alive.");
+        else addReport(ReportLevel.ERROR, deviceName + " Motor (TalonFX) Alive?: Dead!");
+    }
+
+    public void diagnoseHardwareCTRE(String deviceName, CANcoder coder) {
+        // Because the Cancoder has no method to check its alive, we send it a empty control which it should return a zero when it gets the control.
+        // If its not zero, that means that most likely that it had some communication error, I.e. It actually is powered off or not connected at all.
+        // TODO: validate that a CANCoder can actually do `EmptyControl`s
+        StatusCode status = coder.setControl(new EmptyControl()); 
+        if (status.value == 0) addReport(ReportLevel.INFO, deviceName + " Cancoder Alive?: Alive. " + printStatusCode(status));
+        else addReport(ReportLevel.ERROR, deviceName + " Cancoder Alive?: Dead! " + printStatusCode(status));
+
+        
+        
+        // StatusSignal<MagnetHealthValue> -> MagnetHealthValue -> int
+        int coderMagHealth = coder.getMagnetHealth().getValue().value;
+        if (coderMagHealth == 3) addReport(ReportLevel.INFO, deviceName + " Cancoder Magnet Strength?: Ideal."); // why is 3 the 'good value'?
+        if (coderMagHealth == 2) addReport(ReportLevel.WARNING, deviceName + " Cancoder Magnet Strength?: Subpar.");
+        if (coderMagHealth == 1) addReport(ReportLevel.ERROR, deviceName + " Cancoder Magnet Strength?: Too Close or Far!");
+        if (coderMagHealth == 0) addReport(ReportLevel.ERROR, deviceName + " Cancoder Magnet Strength?: Unkown!");
+    }
+
+    public void diagnoseHardwareCTRE(String deviceName, Pigeon2 pigeon) {
+        // Because the Pigeon has no method to check its alive, we send it a empty control which it should return a zero when it gets the control.
+        // If its not zero, that means that most likely that it had some communication error, I.e. It actually is powered off or not connected at all.
+        // TODO: validate that a Pigeon2 can actually do `EmptyControl`s
+        StatusCode status = pigeon.setControl(new EmptyControl()); 
+        if (status.value == 0) addReport(ReportLevel.INFO, deviceName + " Pigeon2 Alive?: Alive. " + printStatusCode(status));
+        else addReport(ReportLevel.ERROR, deviceName + " Pigeon2 Alive?: Dead! " + printStatusCode(status));
+    }
+    
+    public boolean hasReport() {
+        return reports.size() == 0;
+    }
+}

src/main/java/frc4388/utility/structs/Gains.java

@@ -0,0 +1,83 @@
+// 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.utility.structs;
+
+/** Add your docs here. */
+public class Gains {
+    public double kP;
+    public double kI;
+    public double kD;
+    public double kF;
+    public int kIZone;
+    public double kPeakOutput;
+    public double kMaxOutput;
+    public double kMinOutput;
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kPeakOutput = kPeakOutput;
+        this.kMaxOutput = kPeakOutput;
+        this.kMinOutput = -kPeakOutput;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kPeakOutput = 1.0;
+        this.kMaxOutput = 1.0;
+        this.kMinOutput = -1.0;
+    }
+
+    public Gains(double kP, double kI, double kD) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+    }
+
+    /**
+     * Creates Gains object for PIDs
+     * @param kP The P value.
+     * @param kI The I value.
+     * @param kD The D value.
+     * @param kF The F value.
+     * @param kIZone The zone of the I value.
+     * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
+     * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
+     */
+    public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.kF = kF;
+        this.kIZone = kIZone;
+        this.kMaxOutput = kMaxOutput;
+        this.kMinOutput = kMinOutput;
+        this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
+    }    
+}

src/main/java/frc4388/utility/structs/LEDPatterns.java

@@ -0,0 +1,45 @@
+package frc4388.utility.structs;
+
+/**
+ * Add your docs here.
+ */
+public enum LEDPatterns {
+  /* PALLETTE PATTERNS */
+  RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
+    RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
+    PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
+    PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
+    RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
+    RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
+    RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
+    BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
+    GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
+  
+  /* COLOR 1 PATTERNS */
+  C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
+    C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
+    
+  /* COLOR 2 PATTERNS */
+  C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), 
+    C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
+    
+  /* COLOR 1 AND 2 PATTERNS */
+  C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
+    C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
+     
+  /* SOLID COLORS */
+  SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
+    SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
+    SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
+    SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
+    SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
+  
+  /* GETTERS/SETTERS */
+  private final float id;
+  LEDPatterns(float id) {
+    this.id = id;
+  }
+  public float getValue() {
+    return id;
+  }
+}

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

@@ -0,0 +1,26 @@
+package frc4388.utility.structs;
+
+public class UtilityStructs {
+    public static class TimedOutput {
+        public double leftX  = 0.0;
+        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;
+    }
+}