Initial commit

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;
+    }
+}