Merge pull request #42 from Team4388/advantagekit

Advantagekit merged into  master

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Main",
+      "request": "launch",
+      "mainClass": "frc4388.robot.Main",
+      "projectName": "2025RidgeScape"
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

build.gradle

@@ -1,6 +1,7 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.2.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.2"
+    id "com.peterabeles.gversion" version "1.10"
 }
 
 java {
@@ -72,6 +73,9 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 }
 
 test {
@@ -102,3 +106,19 @@ wpi.java.configureTestTasks(test)
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
 }
+
+task(replayWatch, type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.ReplayWatch"
+    classpath = sourceSets.main.runtimeClasspath
+}
+
+// Create version file
+project.compileJava.dependsOn(createVersionFile)
+gversion {
+    srcDir       = "src/main/java/"
+    classPackage = "frc4388.robot.constants"
+    className    = "BuildConstants"
+    dateFormat   = "yyyy-MM-dd HH:mm:ss z"
+    timeZone     = "America/Denver"
+    indent       = "  "
+}

simgui-ds.json

@@ -96,7 +96,17 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0"
+      "guid": "030000005e040000ea0200000b050000",
+      "useGamepad": true
+    },
+    {
+      "useGamepad": true
+    },
+    {},
+    {},
+    {},
+    {
+      "useGamepad": true
     }
   ]
 }

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

@@ -1,472 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 static edu.wpi.first.units.Units.Inches;
-import static edu.wpi.first.units.Units.Rotations;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-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.apriltag.AprilTag;
-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.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
-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.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.Distance;
-import frc4388.utility.CanDevice;
-import frc4388.utility.Gains;
-import frc4388.utility.LEDPatterns;
-import frc4388.utility.ReefPositionHelper;
-import frc4388.utility.Trim;
-import frc4388.utility.configurable.ConfigurableDouble;
-
-/**
- * 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 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(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND)
-                ).withOpenLoopRamps(
-                    new OpenLoopRampsConfigs()
-                        .withDutyCycleOpenLoopRampPeriod(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE)
-                ).withClosedLoopRamps(
-                    new ClosedLoopRampsConfigs()
-                        .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.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(SwerveDriveConstants.Configurations.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??? 
-        }
-
-        // No mans land
-        // Beware, there be dragons.
-        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;
-      }
-    
-    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 Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
-        public static final Trim ARM_OFFSET_TRIM =      new Trim("ARM Offset Trim",      -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 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;
-                
-        // X is tangent to reef side
-        // Y is normal to reef side
-        public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field
-        public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
-        public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
-
-        public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
-        public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
-        // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
-        
-        public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
-        public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1);
-        
-        public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6);
-        public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2);
-
-        public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE;
-        
-        // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
-        // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
-        // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
-        
-        // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
-        // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
-        
-        // public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
-        // public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
-
-        // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
-
-        public static final int L4_DRIVE_TIME = 250; //Milliseconds
-        // public static final int L3_DRIVE_TIME = 500;
-        public static final int L2_DRIVE_TIME = 250; //Milliseconds
-        public static final int ALGAE_DRIVE_TIME = 500;
-        public static final double STOP_VELOCITY = 0.0;
-    }
-
-    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.k2025Reefscape);
-
-        // 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 DriveConstants {
-        public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
-    }
-    
-    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;
-
-    }
-
-    public static final class ElevatorConstants {
-        public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
-        public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
-
-        public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75;
-        public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20;
-
-        // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
-        
-        public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
-        public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
-        public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND
-
-        
-        public static final double GEAR_RATIO_ELEVATOR = -9.0;
-        //Max for elevator = 50%
-        
-        public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
-        public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
-        public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
-        public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
-        public static final double SCORING_THREE_ELEVATOR = -9.25;
-        public static final double DEALGAE_L2_ELEVATOR = -4;
-        public static final double DEALGAE_L3_ELEVATOR = -26.5;
-        public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
-        
-        public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
-        public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
-        //Max for endefector = 60%
-        public static final double L2_SCORE_ELEVATOR = -5;
-        public static final double L2_LEAVE_ELEVATOR = -11;
-
-        public static final double L2_SCORE_ENDEFFECTOR = -19;
-
-        public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
-        public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR;
-        public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
-        public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
-        public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR;
-        public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR;
-        public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
-
-        public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
-            .withKP(1)
-            .withKI(0)
-            .withKD(0);
-        
-        public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs()
-            .withKP(1)
-            .withKI(0)
-            .withKD(0);
-    }
-}

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

@@ -7,28 +7,24 @@
 
 package frc4388.robot;
 
-import java.util.ArrayList;
+// Advantagekit
-import java.util.Base64;
+import org.littletonrobotics.junction.LogFileUtil;
-import java.util.List;
+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 com.ctre.phoenix6.CANBus;
+import edu.wpi.first.wpilibj.RobotController;
-import com.ctre.phoenix6.CANBus.CANBusStatus;
-
-import edu.wpi.first.util.sendable.Sendable;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc4388.utility.CanDevice;
+import frc4388.robot.constants.BuildConstants;
+import frc4388.robot.constants.Constants.SimConstants;
 import frc4388.utility.DeferredBlock;
-import frc4388.utility.DeferredBlockMulti;
+import frc4388.utility.compute.RobotTime;
-import frc4388.utility.RobotTime;
+import frc4388.utility.compute.Trim;
-import frc4388.utility.Status;
+import frc4388.utility.status.FaultReporter;
-import frc4388.utility.Subsystem;
+
-import frc4388.utility.Trim;
-import frc4388.utility.Status.Report;
-import frc4388.utility.Status.ReportLevel;
 //import frc4388.robot.subsystems.LED;
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -37,7 +33,7 @@ import frc4388.utility.Status.ReportLevel;
  * creating this project, you must also update the build.gradle file in the
  * project.
  */
-public class Robot extends TimedRobot {
+public class Robot extends LoggedRobot {
   Command m_autonomousCommand;
 
   private RobotTime m_robotTime = RobotTime.getInstance();
@@ -49,31 +45,19 @@ public class Robot extends TimedRobot {
    */
   @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
-    new Thread() {
+    // FaultReporter.startThread();
-      public void run() {
-        try{
-        while(!this.isInterrupted() && this.isAlive()){
-          Thread.sleep(500);
-          for(int i=0;i<Subsystem.subsystems.size(); i++){
-            Subsystem.subsystems.get(i).queryStatus();
   }
 
-          // System.out.println("Updated statuses!");
 
-        }
-        }catch(Exception e){
-          e.printStackTrace();
-        }
-      }
-    }.start();
-  }
 
   /**
    * This function is called every robot packet, no matter the mode. Use
@@ -95,7 +79,6 @@ public class Robot extends TimedRobot {
     // 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
@@ -113,7 +96,6 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledExit() {
     DeferredBlock.execute();
-    DeferredBlockMulti.execute();
   }
 
   /**
@@ -144,6 +126,7 @@ public class Robot extends TimedRobot {
   public void autonomousPeriodic() {
   }
 
+
   @Override
   public void teleopInit() {
     m_robotContainer.stop();
@@ -156,10 +139,7 @@ public class Robot extends TimedRobot {
       CommandScheduler.getInstance().cancel(m_autonomousCommand);
       m_autonomousCommand.cancel();
       m_autonomousCommand.end(true);
-      System.out.println("NOT Null!!");
 
-    } else {
-      System.out.println("Null!!");
     }
     m_robotTime.startMatchTime();
   }
@@ -182,70 +162,103 @@ public class Robot extends TimedRobot {
 
   @Override
   public void testInit() {
-
+    FaultReporter.printReport();
-    List<String> errors = new ArrayList<>();
-
-    // Subsystems header
-    System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWluKWl+KWliDilpfilpbilpfiloTiloTilpYgIOKWl+KWhOKWhOKWluKWl+KWliAg4paX4paW4paX4paE4paE4paW4paX4paE4paE4paE4paW4paX4paE4paE4paE4paW4paX4paWICDilpfilpYg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMICAgIOKWneKWmuKWnuKWmOKWkOKWjCAgICAg4paIICDilpDilowgICDilpDilpvilprilp7ilpzilozilpDilowgICAKIOKWneKWgOKWmuKWluKWkOKWjCDilpDilozilpDilpviloDilprilpYg4pad4paA4paa4paWICDilpDilowgIOKWneKWgOKWmuKWliAg4paIICDilpDilpviloDiloDilpjilpDilowgIOKWkOKWjCDilp3iloDilprilpYK4paX4paE4paE4pae4paY4pad4paa4paE4pae4paY4paQ4paZ4paE4pae4paY4paX4paE4paE4pae4paYICDilpDilowg4paX4paE4paE4pae4paYICDiloggIOKWkOKWmeKWhOKWhOKWluKWkOKWjCAg4paQ4paM4paX4paE4paE4pae4paY")));
-
-    for(int i=0;i< Subsystem.subsystems.size();i++){
-
-      Subsystem subsystem = Subsystem.subsystems.get(i);
-      System.out.println("** Subsystem diagnostic report for " + subsystem.getName() + ":");
-      Status status = subsystem.diagnosticStatus();
-
-      for(int a=0;a<status.reports.size();a++){
-        Report r = status.reports.get(a);
-        if(r.reportLevel == ReportLevel.ERROR)
-          errors.add(subsystem.getName() + " - " + r.toString());
-        subsystem.Log(r.toString());
-      }
   }
 
 
-    // CAN header
-    System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWliDilpfiloTilpYg4paX4paWICDilpfilpYK4paQ4paMICAg4paQ4paMIOKWkOKWjOKWkOKWm+KWmuKWluKWkOKWjArilpDilowgICDilpDilpviloDilpzilozilpDilowg4pad4pac4paMCuKWneKWmuKWhOKWhOKWluKWkOKWjCDilpDilozilpDilowgIOKWkOKWjCh0KQ==")));
 
-    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);
+  // VisionSystemSim visionSim;
-      System.out.println("** CAN diagnostic report for " + device.name + ":");
+  // RobotMapSim robotMapSim;
-      Status status = device.diagnosticStatus();
 
-      for(int a=0;a<status.reports.size();a++){
+  // @Override
-        Report r = status.reports.get(a);
+  // public void simulationInit() {
-        if(r.reportLevel == ReportLevel.ERROR)
+  //   visionSim = new VisionSystemSim("main");
-          errors.add(device.getName() + " - " + r.toString());
+  //   robotMapSim = m_robotContainer.m_robotMap.configureSim();
-        device.Log(r.toString());
+
-      }
+
+  //   // 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;
     }
 
-    // System.out.println("Found CAN devices: " + new DeviceFinder().Find());
+    // 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;
 
-    if(errors.size() > 0) {
+      case SIM:
-      // Errors header
+        // Running a physics simulator, log to NT
-      System.out.println(new String(Base64.getDecoder().decode("4paX4paE4paE4paE4paW4paX4paE4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilpDilowgICAK4paQ4pab4paA4paA4paY4paQ4pab4paA4paa4paW4paQ4pab4paA4paa4paW4paQ4paMIOKWkOKWjOKWkOKWm+KWgOKWmuKWliDilp3iloDilprilpYK4paQ4paZ4paE4paE4paW4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilp3ilpriloTilp7ilpjilpDilowg4paQ4paM4paX4paE4paE4pae4paY")));
+        Logger.addDataReceiver(new NT4Publisher());
-      for(int i=0;i<errors.size(); i++){
+        break;
-        System.out.println(errors.get(i));
+
-      }
+      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

@@ -11,34 +11,22 @@ package frc4388.robot;
 import edu.wpi.first.wpilibj.DriverStation;
 
 import java.io.File;
-import java.util.ArrayList;
-import java.util.List;
-import java.util.function.BooleanSupplier;
-import java.util.function.Supplier;
-
-import edu.wpi.first.cameraserver.CameraServer;
 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.Joystick;
+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 frc4388.robot.Constants.FieldConstants;
-import frc4388.robot.Constants.LiDARConstants;
-import frc4388.robot.Constants.OIConstants;
-import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.robot.Constants.AutoConstants;
 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.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -47,72 +35,63 @@ import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 // Autos
 import frc4388.utility.controller.VirtualController;
-import frc4388.robot.commands.DriveUntilLiDAR;
-import frc4388.robot.commands.GotoLastApril;
-import frc4388.robot.commands.LidarAlign;
 import frc4388.robot.commands.MoveForTimeCommand;
 import frc4388.robot.commands.MoveUntilSuply;
-import frc4388.robot.commands.WhileTrueCommand;
+import frc4388.robot.commands.alignment.DriveToReef;
-import frc4388.robot.commands.waitElevatorRefrence;
+import frc4388.robot.commands.alignment.DriveUntilLiDAR;
-import frc4388.robot.commands.waitEndefectorRefrence;
+import frc4388.robot.commands.alignment.LidarAlign;
-import frc4388.robot.commands.waitFeedCoral;
+import frc4388.robot.commands.wait.waitElevatorRefrence;
-import frc4388.robot.commands.waitSupplier;
+import frc4388.robot.commands.wait.waitEndefectorRefrence;
-import frc4388.robot.commands.Swerve.neoJoystickPlayback;
+import frc4388.robot.commands.wait.waitFeedCoral;
-import frc4388.robot.commands.Swerve.neoJoystickRecorder;
+import frc4388.robot.commands.wait.waitSupplier;
-
+import frc4388.robot.constants.Constants;
-import com.pathplanner.lib.auto.AutoBuilder;
+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.Vision;
+import frc4388.robot.subsystems.elevator.Elevator;
-import frc4388.robot.subsystems.Elevator.CoordinationState;
+import frc4388.robot.subsystems.elevator.Elevator.CoordinationState;
-import frc4388.robot.subsystems.Lidar;
+import frc4388.robot.subsystems.lidar.LiDAR;
-import frc4388.robot.subsystems.Elevator;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
-// import frc4388.robot.subsystems.Endeffector;
+import frc4388.robot.subsystems.vision.Vision;
-import frc4388.robot.subsystems.SwerveDrive;
-
 // Utilites
 import frc4388.utility.DeferredBlock;
-import frc4388.utility.DeferredBlockMulti;
+import frc4388.utility.compute.TimesNegativeOne;
-import frc4388.utility.ReefPositionHelper;
+import frc4388.utility.compute.ReefPositionHelper.Side;
-import frc4388.utility.Subsystem;
-import frc4388.utility.TimesNegativeOne;
-import frc4388.utility.ReefPositionHelper.Side;
-import frc4388.utility.configurable.ConfigurableString;
 
 /**
  * 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 (including subsystems,
+ * 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();
+    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 Lidar m_reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef");
+    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
-    public final Lidar m_reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse");
-    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, 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);
-    private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(OIConstants.XBOX_PROGRAMMER_ID);
-
-    /* Virtual Controllers */
-    private final VirtualController m_virtualDriver = new VirtualController(0);
-    private final VirtualController m_virtualOperator = new VirtualController(1);
 
     // public List<Subsystem> subsystems = new ArrayList<>();
 
@@ -124,7 +103,6 @@ public class RobotContainer {
     }
 
     // ! /*  Autos */
-    private String lastAutoName = "defualt.auto";
     private SendableChooser<String> autoChooser;
     private Command autoCommand;
 
@@ -144,7 +122,7 @@ public class RobotContainer {
         
         new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
+            new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
         ), () -> m_robotElevator.isL4Primed()),
 
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -153,9 +131,9 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
         waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
         new ParallelRaceGroup(
             new WaitCommand(1),
@@ -186,7 +164,7 @@ public class RobotContainer {
         
         new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
+            new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true)
         ), () -> m_robotElevator.isL4Primed()),
 
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -195,9 +173,9 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
         waitDebuger.asProxy(),
-        // new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        // new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
         // waitDebuger.asProxy(),
         // new ParallelRaceGroup(
         //     new WaitCommand(1),
@@ -243,7 +221,7 @@ public class RobotContainer {
 
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
         waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
         waitDebuger.asProxy(),
 
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -279,7 +257,7 @@ public class RobotContainer {
 
         new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
         waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
         waitDebuger.asProxy(),
 
         new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -307,7 +285,7 @@ public class RobotContainer {
 
         new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
+            new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
         ), () -> m_robotElevator.isL4Primed()),
         
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -316,9 +294,9 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
         waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
         new ParallelRaceGroup(
             new WaitCommand(1),
@@ -354,7 +332,7 @@ public class RobotContainer {
 
         new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
+            new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true)
         ), () -> m_robotElevator.isL4Primed()),
         
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator),
@@ -363,9 +341,9 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
         waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         // waitDebuger.asProxy(),
         // new ParallelRaceGroup(
         //     new WaitCommand(1),
@@ -398,15 +376,15 @@ public class RobotContainer {
         new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
         // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true)
+            new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.LEFT, true)
         ), () -> m_robotElevator.isL3Primed()),
 
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true),
         waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
@@ -423,7 +401,7 @@ public class RobotContainer {
         new ConditionalCommand(Commands.none(), new SequentialCommandGroup(
         // new IfCommand(() -> m_robotElevator.isL3Primed(), new SequentialCommandGroup(
             new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
-            new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true)
+            new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_PREP, Side.RIGHT, true)
         ),() -> m_robotElevator.isL3Primed()),
         
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator),
@@ -432,10 +410,10 @@ public class RobotContainer {
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
 
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true),
         waitDebuger.asProxy(),
         
-        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
 
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
@@ -450,12 +428,12 @@ public class RobotContainer {
     
     private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT, true),
         waitDebuger.asProxy(),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true),
         waitDebuger.asProxy(),
         
-        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
@@ -471,12 +449,12 @@ public class RobotContainer {
 
     private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT, true),
         waitDebuger.asProxy(),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true),
         waitDebuger.asProxy(),
 
-        new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
@@ -495,7 +473,7 @@ public class RobotContainer {
     //     new waitEndefectorRefrence(m_robotElevator),
     //     new waitElevatorRefrence(m_robotElevator),
     //     new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
-    //     new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+    //     new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
     //     new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
     //     new MoveForTimeCommand(m_robotSwerveDrive, 
     //     new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
@@ -506,10 +484,10 @@ public class RobotContainer {
     private Command lowerAlgaeRemove = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
         waitDebuger.asProxy(),
         // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
@@ -526,7 +504,7 @@ public class RobotContainer {
     //     new waitEndefectorRefrence(m_robotElevator),
     //     new waitElevatorRefrence(m_robotElevator),
     //     new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false),
-    //     new LidarAlign(m_robotSwerveDrive, m_reefLidar),
+    //     new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
     //     new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
     //     new MoveForTimeCommand(m_robotSwerveDrive, 
     //     new Translation2d(1,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
@@ -536,10 +514,10 @@ public class RobotContainer {
     private Command upperAlgaeRemove = new SequentialCommandGroup(
         new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true),
         new waitEndefectorRefrence(m_robotElevator),
         new waitElevatorRefrence(m_robotElevator),
-        new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
+        new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true),
         waitDebuger.asProxy(),
         // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
         new MoveForTimeCommand(m_robotSwerveDrive, 
@@ -584,9 +562,9 @@ public class RobotContainer {
         NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
 
         NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, 
-        new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
+        new Translation2d(-1,0), new Translation2d(), reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE));
         // NamedCommands.registerCommand("feed-driveback", Commands.none());
-        NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY));
+        NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.isStopped()));
 
         NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
             new MoveForTimeCommand(m_robotSwerveDrive, 
@@ -616,12 +594,14 @@ public class RobotContainer {
 
         configureButtonBindings();        
         configureVirtualButtonBindings();
-        new DeferredBlock(() -> { // Called on first robot enable
+        
+        DeferredBlock.addBlock(() -> { // Called on first robot enable
             m_robotSwerveDrive.resetGyro();
-        });
+        }, false);
-        new DeferredBlockMulti(() -> { // Called on every robot enable
+        DeferredBlock.addBlock(() -> { // Called on every robot enable
             TimesNegativeOne.update();
-        });
+        }, true);
+
         DriverStation.silenceJoystickConnectionWarning(true);
         // CameraServer.startAutomaticCapture();
 
@@ -688,7 +668,7 @@ public class RobotContainer {
     private void configureButtonBindings() {
         // ? /* Driver Buttons */
 
-        DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
 
         // ! /* Speed */
@@ -739,21 +719,21 @@ public class RobotContainer {
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
         //        .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, 
-        //        new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
+        //        new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true));
             .onTrue(WannaSeeMeDunk.asProxy());
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
             .onTrue(thrustIntake.asProxy());
           
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_reefLidar)); 
+            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, reefLidar)); 
         
             
         // ?  /* Operator Buttons */
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
         
-        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
         
         // Button box
@@ -914,11 +894,19 @@ public class RobotContainer {
       //  return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
     }
 
+    public boolean autoChooserUpdated = false;
     public void makeAutoChooser() {
         autoChooser = new SendableChooser<String>();
         
-        File dir = new File("/home/lvuser/deploy/pathplanner/autos/");
+        File dir;
-        // File dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\");
+
+        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;
@@ -930,6 +918,7 @@ public class RobotContainer {
         }
 
         autoChooser.onChange((filename) -> {
+            autoChooserUpdated = true;
             if (filename.equals("Taxi")) {
                 autoCommand = new SequentialCommandGroup(
                     new MoveForTimeCommand(m_robotSwerveDrive, 
@@ -966,12 +955,4 @@ public class RobotContainer {
     public ButtonBox getButtonBox() {
         return this.m_buttonBox;
     }
-
-    public VirtualController getVirtualDriverController() {
-        return m_virtualDriver;
-    }
-
-    public VirtualController getVirtualOperatorController() {
-        return m_virtualOperator;
-    }
 }

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

@@ -10,22 +10,31 @@ 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.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain;
-import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import edu.wpi.first.math.geometry.Rotation2d;
-import com.ctre.phoenix6.swerve.SwerveModuleConstants;
-import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
-
 import edu.wpi.first.wpilibj.DigitalInput;
-import frc4388.robot.Constants.ElevatorConstants;
+import frc4388.robot.constants.Constants.ElevatorConstants;
-// import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import frc4388.robot.constants.Constants.LiDARConstants;
-// import frc4388.robot.Constants.LEDConstants;
+import frc4388.robot.constants.Constants.SimConstants;
-import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.constants.Constants.VisionConstants;
-import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.elevator.ElevatorIO;
-// import frc4388.robot.subsystems.SwerveModule;
+import frc4388.robot.subsystems.elevator.ElevatorReal;
-import frc4388.utility.RobotGyro;
+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
@@ -35,34 +44,132 @@ public class RobotMap {
     // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
     // public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
-    public PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
+    public final VisionIO leftCamera;
-    public PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
+    public final VisionIO rightCamera;
+
+    // public final LiDAR lidar = new 
+
+    public final LidarIO reefLidar;
+    public final LidarIO reverseLidar;
 
-    public RobotMap() {
-        configureDriveMotorControllers();
-    }
 
     /* LED Subsystem */
     // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
     
     /* Swreve Drive Subsystem */
-    public final SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrain = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new, 
+    public final SwerveIO swerveDrivetrain;
-        Constants.SwerveDriveConstants.DrivetrainConstants, 
-        Constants.SwerveDriveConstants.FRONT_LEFT, Constants.SwerveDriveConstants.FRONT_RIGHT,
-        Constants.SwerveDriveConstants.BACK_LEFT, Constants.SwerveDriveConstants.BACK_RIGHT
-    );
 
     /* Elevator Subsystem */
-    public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
+    public final ElevatorIO elevatorIO;
-    public final TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
+
+    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);
                 
 
-    public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
+                DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
-    public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
+                DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
-    public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
+                DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
 
-    void configureDriveMotorControllers() {
+                elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam);
-        // endeffector.saf
+
+
+
+                // 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/MoveForTimeCommand.java

@@ -4,8 +4,7 @@ import java.time.Instant;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
-import frc4388.utility.TimesNegativeOne;
 
 // Command to repeat a joystick movement for a specific time.
 public class MoveForTimeCommand extends Command {
@@ -23,6 +22,7 @@ public class MoveForTimeCommand extends Command {
         Translation2d rightStick, 
         long millis,
         boolean robotRelative) {
+        
         addRequirements(swerveDrive);
 
         this.swerveDrive = swerveDrive;

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

@@ -1,12 +1,10 @@
 package frc4388.robot.commands;
 
-import java.time.Instant;
 import java.util.function.Supplier;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
-import frc4388.utility.TimesNegativeOne;
 
 // Command to repeat a joystick movement for a specific time.
 public class MoveUntilSuply extends Command {

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

@@ -5,7 +5,7 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.utility.Gains;
+import frc4388.utility.structs.Gains;
 
 public abstract class PID extends Command {
 	protected Gains  gains;

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

@@ -8,7 +8,6 @@ import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 import java.util.function.BooleanSupplier;
 

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

@@ -1,29 +1,19 @@
-package frc4388.robot.commands;
+package frc4388.robot.commands.alignment;
-
-import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
-
-import java.util.Optional;
 
 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 edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.Constants.AutoConstants;
+import frc4388.robot.constants.Constants.AutoConstants;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
-import frc4388.robot.subsystems.Vision;
+import frc4388.robot.subsystems.vision.Vision;
-import frc4388.utility.Gains;
+import frc4388.utility.compute.ReefPositionHelper;
-import frc4388.utility.ReefPositionHelper;
+import frc4388.utility.compute.TimesNegativeOne;
-import frc4388.utility.TimesNegativeOne;
+import frc4388.utility.compute.ReefPositionHelper.Side;
-import frc4388.utility.ReefPositionHelper.Side;
+import frc4388.utility.structs.Gains;
-import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
-import frc4388.utility.UtilityStructs.AutoRecordingFrame;
-import frc4388.utility.controller.VirtualController;
 
-public class GotoLastApril extends Command {
+public class DriveToReef extends Command {
     
 
     // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
@@ -45,7 +35,7 @@ public class GotoLastApril extends Command {
      * @param SwerveDrive m_robotSwerveDrive
      */
 
-    public GotoLastApril(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
+    public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
         this.swerveDrive = swerveDrive;
         this.vision = vision;
         this.distance = distance;
@@ -62,23 +52,6 @@ public class GotoLastApril extends Command {
 
     @Override
     public void initialize() {
-        // double kP = AutoConstants.P_XY_GAINS.get();
-        // double kI = AutoConstants.I_XY_GAINS.get();
-        // double kD = AutoConstants.D_XY_GAINS.get();
-        // xPID = new PID(new Gains(
-        //     kP, 
-        //     kI, 
-        //     kD), 
-        // 0);
-        // yPID = new PID(new Gains(
-        //     kP, 
-        //     kI, 
-        //     kD),
-        // 0);
-
-        // System.out.println("kP: "+kP);
-        // System.out.println("kI: "+kI);
-        // System.out.println("kD: "+kD);
         xPID.initialize();
         yPID.initialize();
         this.targetpos = ReefPositionHelper.getNearestPosition(
@@ -169,33 +142,6 @@ public class GotoLastApril extends Command {
                 // this statement is a boolean in and of itself
     }
 
-    // @Override
-    // public void end(boolean interrupted) {
-       
-    // }
-    
-
-
-    // @Override
-    // public double getError() {
-    //     return e; 
-    // }
-
-
-
-    // @Override
-    // public void runWithOutput(double output) {
-    //     // TODO Auto-generated method stub
-    //     Translation2d leftStick = new Translation2d(Math.max(Math.min(output, 1), -1),0);
-    //     Translation2d rightStick = new Translation2d();
-    //     // System.out.println("Output = " + -output);
-    //     SmartDashboard.putNumber("PID Output", output);
-    //     swerveDrive.driveWithInput(leftStick, rightStick, true);
-    // }
-
-
-
-
 
 
 
@@ -212,17 +158,11 @@ public class GotoLastApril extends Command {
     private class PID {
         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;
         }
 
         // Called when the command is initially scheduled.
@@ -244,10 +184,5 @@ public class GotoLastApril extends Command {
 
             return output;
         }
-
-        // // Returns true when the command should end.
-        // public final boolean isFinished() {
-        //     return Math.abs(getError()) < tolerance;
-        // }
     }
 }

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

@@ -1,29 +1,24 @@
-package frc4388.robot.commands;
+package frc4388.robot.commands.alignment;
-
-import java.time.Instant;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.Lidar;
+import frc4388.robot.subsystems.lidar.LiDAR;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
-import frc4388.utility.TimesNegativeOne;
 
 // 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 LiDAR m_lidar;
     private final double mindistance;
-    private final boolean robotRelative;
 
     public DriveUntilLiDAR(
         SwerveDrive swerveDrive, 
         Translation2d leftStick, 
         Translation2d rightStick, 
-        Lidar lidar,
+        LiDAR lidar,
-        double mindistance,
+        double mindistance) {
-        boolean robotRelative) {
             addRequirements(swerveDrive);
 
         this.swerveDrive = swerveDrive;
@@ -31,7 +26,6 @@ public class DriveUntilLiDAR extends Command {
         this.rightStick = rightStick;
         this.m_lidar = lidar;
         this.mindistance = mindistance;
-        this.robotRelative = robotRelative;
     }
 
     @Override

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

@@ -2,19 +2,19 @@
 // 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;
+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;
+import frc4388.robot.subsystems.lidar.LiDAR;
-import frc4388.robot.subsystems.SwerveDrive;
+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 LiDAR lidar;  
 
   private int currentFinderTick;
   // private int tickFoundPipe;
@@ -26,7 +26,7 @@ public class LidarAlign extends Command {
   // private final boolean constructedHeadedRight;
 
   /** Creates a new LidarAlign. */
-  public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) {
+  public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     this.swerveDrive = swerveDrive;
@@ -41,7 +41,7 @@ public class LidarAlign extends Command {
     this.currentFinderTick = 0;
     this.speed = 0.4; // TODO: find good speed for this
     this.foundReef = false;
-    this.headedRight = (GotoLastApril.tagRelativeXError < 0);
+    this.headedRight = (DriveToReef.tagRelativeXError < 0);
     this.additionalDistance = 0;
     this.bounces = 0;
   }

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

@@ -1,3 +1,4 @@
+package frc4388.robot.commands.autos;
 // package frc4388.robot.commands.Autos;
 
 // import java.io.File;

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

@@ -2,11 +2,11 @@
 // 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;
+package frc4388.robot.commands.swerve;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import frc4388.robot.commands.PID;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 public class RotateToAngle extends PID {
 

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

@@ -1,4 +1,4 @@
-package frc4388.robot.commands.Swerve;
+package frc4388.robot.commands.swerve;
 
 import java.io.FileInputStream;
 import java.util.ArrayList;
@@ -6,11 +6,11 @@ import java.util.function.Supplier;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
-import frc4388.utility.DataUtils;
+import frc4388.utility.compute.DataUtils;
-import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
-import frc4388.utility.UtilityStructs.AutoRecordingFrame;
 import frc4388.utility.controller.VirtualController;
+import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
+import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
 
 
 /**
@@ -24,8 +24,8 @@ public class neoJoystickPlayback extends Command {
     private final Supplier<String>              filenameGetter;
     private       String                        filename;
     private       int                           frame_index    = 0;
-    private       long                          startTime      = 0;
+    // private       long                          startTime      = 0;
-    private       long                          playbackTime   = 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
     
@@ -150,8 +150,8 @@ public class neoJoystickPlayback extends Command {
 
     @Override
     public void initialize() {
-        startTime = System.currentTimeMillis();
+        // startTime = System.currentTimeMillis();
-        playbackTime = 0;
+        // playbackTime = 0;
         frame_index = 0;
 
         m_finished = !loadAuto();

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

@@ -1,4 +1,4 @@
-package frc4388.robot.commands.Swerve;
+package frc4388.robot.commands.swerve;
 
 import java.io.FileOutputStream;
 import java.util.ArrayList;
@@ -7,11 +7,11 @@ import java.util.function.Supplier;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
-import frc4388.utility.DataUtils;
+import frc4388.utility.compute.DataUtils;
-import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
-import frc4388.utility.UtilityStructs.AutoRecordingFrame;
 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 

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

@@ -2,10 +2,10 @@
 // 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;
+package frc4388.robot.commands.wait;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.Elevator;
+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 {

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

@@ -2,10 +2,10 @@
 // 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;
+package frc4388.robot.commands.wait;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.Elevator;
+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 {

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

@@ -2,10 +2,10 @@
 // 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;
+package frc4388.robot.commands.wait;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.Elevator;
+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 {

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

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.robot.commands.wait;
 
 import java.util.function.Supplier;
 

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,234 @@
+/*----------------------------------------------------------------------------*/
+/* 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 Trim ELEVATOR_OFFSET_TRIM = new Trim("Elevator Offset Trim", -ElevatorConstants.MAX_POSITION_ELEVATOR, ElevatorConstants.MAX_POSITION_ELEVATOR, 1, 0);
+        public static final Trim ARM_OFFSET_TRIM =      new Trim("ARM Offset Trim",      -ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR, 1, 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;
+                
+        // X is tangent to reef side
+        // Y is normal to reef side
+        public static final double X_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5+1); // This is from the field
+        public static final double Y_SCORING_POSITION_OFFSET = Units.inchesToMeters(16+1);
+        public static final double HALF_ROBOT_SIZE = Units.inchesToMeters(18);
+
+        public static final double L4_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
+        public static final double L4_DISTANCE_SCORE = L4_DISTANCE_PREP;
+        // public static final double L4_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(4.5);
+        
+        public static final double L3_DISTANCE_PREP = HALF_ROBOT_SIZE + Units.inchesToMeters(15);
+        public static final double L3_DISTANCE_SCORE = HALF_ROBOT_SIZE + Units.inchesToMeters(5+1);
+        
+        public static final double L2_PREP_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(6);
+        public static final double L2_SCORE_DISTANCE = HALF_ROBOT_SIZE + Units.inchesToMeters(0.5-2);
+
+        public static final double ALGAE_REMOVAL_DISTANCE = HALF_ROBOT_SIZE;
+        
+        // public static final double L4_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
+        // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5.5);
+        // // public static final double L4_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(4.5);
+        
+        // public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(15);
+        // public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(5+1);
+        
+        // public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
+        // public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(0.5);
+
+        // public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
+
+        public static final int L4_DRIVE_TIME = 250; //Milliseconds
+        // public static final int L3_DRIVE_TIME = 500;
+        public static final int L2_DRIVE_TIME = 250; //Milliseconds
+        public static final int ALGAE_DRIVE_TIME = 500;
+        public static final double STOP_VELOCITY = 0.0;
+    }
+
+    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;
+
+    }
+
+    public static final class ElevatorConstants {
+        public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
+        public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
+
+        public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75;
+        public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20;
+
+        // public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
+        
+        public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
+        public static final int ENDEFFECTOR_LIMIT_SWITCH = 9; // TODO: FIND
+        public static final int INTAKE_LIMIT_SWITCH = 6; // TODO: FIND
+
+        
+        public static final double GEAR_RATIO_ELEVATOR = -9.0;
+        //Max for elevator = 50%
+        
+        public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR;
+        public static final double WAITING_POSITION_ELEVATOR = -9.5; // TODO: find 2-4 in off the pipe
+        public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe
+        public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe
+        public static final double SCORING_THREE_ELEVATOR = -9.25;
+        public static final double DEALGAE_L2_ELEVATOR = -4;
+        public static final double DEALGAE_L3_ELEVATOR = -26.5;
+        public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position
+        
+        public static final double GEAR_RATIO_ENDEFECTOR = -100.0;
+        public static final double ENDEFECTOR_DRIVE_SLOW = -0.08;
+        //Max for endefector = 60%
+        public static final double L2_SCORE_ELEVATOR = -5;
+        public static final double L2_LEAVE_ELEVATOR = -11;
+
+        public static final double L2_SCORE_ENDEFFECTOR = -19;
+
+        public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR;
+        public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR;
+        public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR;
+        public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR;
+        public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR;
+        public static final double PRIMED_FOUR_ENDEFFECTOR = 0.44 * GEAR_RATIO_ENDEFECTOR;
+        public static final double COMPLETLY_TOP_ENDEFFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR;
+
+        public static final Slot0Configs ELEVATOR_PID = new Slot0Configs()
+            .withKP(1)
+            .withKI(0)
+            .withKD(0);
+        
+        public static final Slot0Configs ENDEFFECTOR_PID = new Slot0Configs()
+            .withKP(1)
+            .withKI(0)
+            .withKD(0);
+    }
+
+    // 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/DiffDrive.java

@@ -1,108 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.hardware.TalonFX;
-
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import frc4388.robot.Constants.DriveConstants;
-import frc4388.utility.RobotGyro;
-import frc4388.utility.RobotTime;
-import frc4388.utility.Status;
-import frc4388.utility.Subsystem;
-
-/**
- * Add your docs here.
- */
-public class DiffDrive extends Subsystem {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
-  private RobotTime m_robotTime = RobotTime.getInstance();
-
-  private TalonFX m_leftFrontMotor;
-  private TalonFX m_rightFrontMotor;
-  private TalonFX m_leftBackMotor;
-  private TalonFX m_rightBackMotor;
-  private DifferentialDrive m_driveTrain;
-  private RobotGyro m_gyro;
-
-  /**
-   * Add your docs here.
-   */
-  public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
-      TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
-    
-        super();
-
-    m_leftFrontMotor = leftFrontMotor;
-    m_rightFrontMotor = rightFrontMotor;
-    m_leftBackMotor = leftBackMotor;
-    m_rightBackMotor = rightBackMotor;
-    m_leftBackMotor .setControl(new Follower(m_leftFrontMotor.getDeviceID(), false));
-    m_rightBackMotor.setControl(new Follower(m_rightBackMotor.getDeviceID(), false));
-    m_driveTrain = driveTrain;
-    m_gyro = gyro;
-  }
-
-  @Override
-  public void periodic() {
-    m_gyro.updatePigeonDeltas();
-
-    if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
-      updateSmartDashboard();
-    }
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public void driveWithInput(double move, double steer) {
-    m_driveTrain.arcadeDrive(move, steer);
-  }
-
-  /**
-   * Add your docs here.
-   */
-  public void tankDriveWithInput(double leftMove, double rightMove) {
-    m_leftFrontMotor.set(leftMove);
-    m_rightFrontMotor.set(rightMove);
-  }
-
-  /**
-   * Add your docs here.
-   */
-  private void updateSmartDashboard() {
-
-    // Examples of the functionality of RobotGyro
-    SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
-    SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
-    SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
-    //SmartDashboard.putData(m_gyro);
-  }
-
-
-  @Override
-  public String getSubsystemName() {
-      return "Diff Drive";
-  }
-
-  @Override
-  public void queryStatus() {
-      // TODO: Add Stuff
-  }
-
-  @Override
-  public Status diagnosticStatus() {
-      Log("Diagnostic info for this has not been inplemented!"); //TODO
-      return new Status();
-  }
-}

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

@@ -1,402 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.subsystems;
-
-import java.time.Instant;
-
-import org.opencv.ml.RTrees;
-
-import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.controls.PositionDutyCycle;
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.units.measure.Current;
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.ElevatorConstants;
-import frc4388.robot.Constants.LEDConstants;
-import frc4388.robot.Constants.AutoConstants;
-import frc4388.robot.subsystems.LED;
-import frc4388.utility.LEDPatterns;
-import frc4388.utility.Status;
-import frc4388.utility.Subsystem;
-import frc4388.utility.TimesNegativeOne;
-import frc4388.utility.Status.ReportLevel;
-
-public class Elevator extends Subsystem {
-  /** Creates a new Elevator. */
-  private TalonFX elevatorMotor;
-  private TalonFX endeffectorMotor;
-  private LED led;
-
-  private long wait = 0;
-  private long maxWait = 1000;
-
-  private double elevatorRefrence = 0;
-  private double endeffectorRefrence = 0;
-
-  private boolean elevatorManualStop = true;
-  private boolean endefectorManualStop = true;
-
-  private boolean disableAutoIntake = false;
-
-  private boolean seededZeroEndefector = false;
-  private boolean seededZeroElevator = false;
-
-  private DigitalInput basinBeamBreak;
-  private DigitalInput endeffectorLimitSwitch;
-  private DigitalInput intakeIR;
-
-  public enum CoordinationState {
-    Waiting, // for coral into the though
-    WatingBeamTripped, //once the beam break trips
-    Ready, // Has coral in endefector
-    Hovering, // Has coral in endefector
-    L2Score,
-    L2ScoreLeave,
-    PrimedThree, // Arm and elevator Waiting to score in the level 3 position
-    ScoringThree, // Arm and elevator in the level three position
-    PrimedFour, // Arm and elevator Waiting to score in the level 4 position
-    ScoringFour, // Arm and elevator in the level four position
-    BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef.
-    BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef.
-    BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef.
-    BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef.
-  }
-
-  private CoordinationState currentState;
-
-  // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
-  public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeDigitalInput, LED led) {
-    elevatorMotor = elevatorTalonFX;
-    endeffectorMotor = endeffectorTalonFX;
-    this.led = led;
-
-    this.basinBeamBreak = basinLimitSwitch;
-    this.endeffectorLimitSwitch = endeffectorLimitSwitch;
-    this.intakeIR = intakeDigitalInput;
-
-    elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
-    endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
-    
-    elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
-    endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
-    currentState = CoordinationState.Ready;
-  }
-
-  //PID methods
-
-  private void PIDPosition(TalonFX motor, double position) {
-    if (motor == elevatorMotor) elevatorRefrence = position;
-    else endeffectorRefrence = position;
-
-    var request = new PositionDutyCycle(position);
-    motor.setControl(request);
-  }
-
-  public void elevatorStop() {
-    elevatorMotor.set(0);
-  }
-
-  public void endeffectorStop() {
-    endeffectorMotor.set(0);
-  }
-
-
-  public void transitionState(CoordinationState state) {
-    // elevatorMotor.enable();
-    
-
-    currentState = state;
-    switch (currentState) {
-      case Waiting: {
-        wait = System.currentTimeMillis() + maxWait;
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR);
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
-        led.setMode(LEDConstants.WAITING_PATTERN);
-        break;
-      }
-
-      case WatingBeamTripped: {
-        PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
-        led.setMode(LEDConstants.DOWN_PATTERN);
-        break;
-      }
-
-      case Ready: {
-        PIDPosition(elevatorMotor, ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
-        led.setMode(LEDConstants.DOWN_PATTERN);
-        break;
-      }
-
-      case Hovering: {
-        PIDPosition(elevatorMotor, ElevatorConstants.HOVERING_POSITION_ELEVATOR);
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
-        led.setMode(LEDConstants.READY_PATTERN);
-        break;
-      }
-
-      case L2Score: {
-        PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(LEDConstants.SCORING_PATTERN);
-        break;
-      }
-
-      case L2ScoreLeave: {
-        PIDPosition(elevatorMotor, ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(LEDConstants.SCORING_PATTERN);
-        break;
-      }
-      
-      case PrimedFour: {
-        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
-        led.setMode(LEDConstants.SCORING_PATTERN);
-        break;
-      }
-
-      case ScoringFour: {
-        PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(LEDConstants.SCORING_PATTERN);
-        break;
-      }
-
-      case PrimedThree: {
-        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
-        led.setMode(LEDConstants.SCORING_PATTERN);
-        break;
-      }
-      
-      case ScoringThree: {
-        PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(LEDConstants.SCORING_PATTERN);
-        break;
-      }
-
-      case BallRemoverL2Primed: {
-        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
-        led.setMode(LEDConstants.SCORING_PATTERN);
-        break;
-      }
-
-      case BallRemoverL2Go: {
-        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(LEDConstants.SCORING_PATTERN);
-        break;
-      }
-
-      case BallRemoverL3Primed: {
-        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
-        break;
-      }
-
-      case BallRemoverL3Go: {
-        PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
-        PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
-        led.setMode(LEDConstants.SCORING_PATTERN);
-        break;
-      }
-
-      default: {
-        assert false;
-      }
-    }
-
-  }
-
-  public void togggleAutoIntaking() {
-    disableAutoIntake = !disableAutoIntake;
-  }
-
-  public boolean elevatorAtReference() {
-    // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
-    double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
-    double diffrence = elevatorRefrence - elevatorPosition;
-
-    boolean headedUp = diffrence < 0;
-    boolean forwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
-    boolean reverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
-
-    return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
-  }
-
-  public boolean endeffectorAtReference() {
-    // double elevatorRefrence = endefectorMotor.getClosedLoopReference().getValueAsDouble();
-    double endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
-    double diffrence = endeffectorRefrence - endeffectorPosition;
-
-    boolean headedUp = diffrence < 0;
-    boolean forwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0;
-    boolean reverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0;
-
-    return (Math.abs(diffrence) <= 0.5 || (reverseLimit && headedUp) || (forwardLimit && !headedUp));
-  }
-  // public void driveElevatorStick(Translation2d stick) {
-  //   if (stick.getNorm() > 0.05) {
-  //     elevatorMotor.set(stick.getY());
-  //   }
-  // }
-
-  public boolean getEndeffectorLimit() {
-    return endeffectorLimitSwitch.get();
-  }
-
-  private void periodicWaiting() {
-    if (!basinBeamBreak.get()) 
-      transitionState(CoordinationState.Ready);
-    // if(!endeffectorLimitSwitch.get())
-    //   transitionState(CoordinationState.Hovering);
-  }
-
-  // private void periodicWaitingTripped() {
-  //   if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) 
-  //     transitionState(CoordinationState.Ready);
-  // }
-  
-  private void periodicReady() {
-    if (elevatorAtReference() && !endeffectorLimitSwitch.get())
-      transitionState(CoordinationState.Hovering);
-    if(elevatorAtReference() && endeffectorLimitSwitch.get())
-      transitionState(CoordinationState.Hovering);
-  }
-
-  private void periodicScoring() {
-    if (!endeffectorLimitSwitch.get()) 
-      transitionState(CoordinationState.Waiting);
-  }
-
-  public void manualElevatorVel(double velocity) {
-    if (Math.abs(velocity) > 0.1) {
-      elevatorMotor.set(velocity);
-      elevatorManualStop = false;
-      return;
-    }
-    if (!elevatorManualStop) {
-      elevatorManualStop = true;
-      elevatorMotor.set(0);
-    }
-  }
-
-  public void manualEndeffectorVel(double velocity) {
-    if (Math.abs(velocity) > 0.1) {
-      endeffectorMotor.set(velocity);
-      endefectorManualStop = false;
-      return;
-    }
-    if (!endefectorManualStop) {
-      endefectorManualStop = true;
-      endeffectorMotor.set(0);
-    }
-  }
-
-  @Override
-  public void periodic() {
-
-    // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble();
-    // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble();
-    // double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble();
-    // double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble();
-
-
-    // if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
-    //   PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
-    // }
-
-    // This method will be called once per scheduler run
-    // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
-    // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
-    SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
-    SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
-    SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
-    SmartDashboard.putString("State", currentState.toString());
-
-    if (!seededZeroEndefector && endeffectorMotor.getForwardLimit().asSupplier().get().value == 0) {
-      endeffectorMotor.setPosition(0);
-      seededZeroEndefector = !seededZeroEndefector;
-    }
-
-    if (!seededZeroElevator && elevatorMotor.getForwardLimit().asSupplier().get().value == 0) {
-      elevatorMotor.setPosition(0);
-      seededZeroElevator = !seededZeroElevator;
-    }
-    
-    if (disableAutoIntake) return;
-
-    if (currentState == CoordinationState.Waiting) {
-      periodicWaiting();
-    } else if (currentState == CoordinationState.WatingBeamTripped) {
-      // periodicWaitingTripped();
-    } else if (currentState == CoordinationState.Ready) {
-      periodicReady();
-    }
-
-    if(!intakeIR.get()){
-      led.setMode(LEDConstants.DOWN_PATTERN);
-    }
-
-    
-    // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
-    //   periodicScoring();
-    // }
-  }
-
-  public boolean isL4Primed() {
-    return currentState == CoordinationState.PrimedFour;
-  }
-
-  public boolean isL3Primed() {
-    return currentState == CoordinationState.PrimedThree;
-  }
-
-  public boolean hasCoral() {
-    return elevatorAtReference() && currentState == CoordinationState.Hovering || !endeffectorLimitSwitch.get();
-  }
-
-  public boolean readyToMove() {
-    return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get();
-    // return hasCoral();
-  }
-
-  public void armShuffle(){
-    if(!basinBeamBreak.get()){
-      //shuffle the coral with the arm until coral hits beam break
-    }
-  }
-
-  @Override
-  public String getSubsystemName() {
-    return "Elevator";
-  }
-
-  @Override
-  public void queryStatus() {}
-
-  @Override
-  public Status diagnosticStatus() {
-    Status status = new Status();
-
-    status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name());
-    status.diagnoseHardwareCTRE("Elevator Motor", elevatorMotor);
-    status.diagnoseHardwareCTRE("Endeffector Motor", endeffectorMotor);
-
-    return status;
-  }
-}

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

@@ -7,24 +7,26 @@
 
 package frc4388.robot.subsystems;
 
-import edu.wpi.first.wpilibj.AddressableLED;
+import org.littletonrobotics.junction.AutoLogOutput;
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
+import frc4388.robot.constants.Constants.LEDConstants;
-import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.status.Status;
-import frc4388.utility.LEDPatterns;
+import frc4388.utility.status.FaultReporter;
-import frc4388.utility.Status;
+import frc4388.utility.status.Queryable;
-import frc4388.utility.Subsystem;
+import frc4388.utility.status.Status.ReportLevel;
-import frc4388.utility.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 Subsystem {
+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;
@@ -47,23 +49,21 @@ public class LED extends Subsystem {
       LEDController.set(mode.getValue());
   }
 
-  @Override
+  @AutoLogOutput
-  public String getSubsystemName() {
+  public String state() {
-    return "LEDs";
+    return mode.getClass().toString();
   }
 
   @Override
-  public void queryStatus() {
+  public String getName() {
-    SmartDashboard.putString("LED status", mode.name());
+    return "LEDs";
   }
 
   @Override
   public Status diagnosticStatus() {
     Status status = new Status();
 
-    if(LEDController.isAlive())
+    if(!LEDController.isAlive())
-      status.addReport(ReportLevel.INFO, "LED is CONNECTED");
-    else
       status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
     
     status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());

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

@@ -1,92 +0,0 @@
-package frc4388.robot.subsystems;
-
-import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.wpilibj.PWM;
-import edu.wpi.first.wpilibj.Counter;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
-import frc4388.robot.Constants.LiDARConstants;
-import frc4388.utility.Status;
-import frc4388.utility.Subsystem;
-import frc4388.utility.Status.ReportLevel;
-
-// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
-public class Lidar extends Subsystem {
-
-    private Counter LidarPWM;
-    private String name = "Lidar";
-
-    private double distance = -1;
-    public Lidar(int port, String name) {
-        this.name = name;
-        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();
-
-        
-    subsystemLayout = Shuffleboard.getTab("Subsystems")
-    .getLayout(getSubsystemName(), BuiltInLayouts.kList)
-    .withSize(2, 2);
-
-    sbDistance = subsystemLayout
-    .add("Distance", 0)
-    .withWidget(BuiltInWidgets.kGraph)
-    .getEntry();
-
-    sbWithinDistance = subsystemLayout
-    .   add("Within Distance", 0)
-    .withWidget(BuiltInWidgets.kBooleanBox)
-    .getEntry();
-    }
-
-    @Override
-    public void periodic() {
-        if(LidarPWM.get() < 1)
-            distance = -1;
-        else
-            distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
-    }
-
-    public double getDistance(){
-        return distance;
-    }
-
-    public boolean withinDistance(){
-        if(distance == -1) return false;
-        return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
-    }
-
-    ShuffleboardLayout subsystemLayout;
-    GenericEntry sbDistance;
-    GenericEntry sbWithinDistance;
-
-    @Override
-    public String getSubsystemName() {
-        return "Lidar " + name;
-    }
-
-    @Override
-    public void queryStatus() {
-        sbDistance.setDouble(distance);
-        sbWithinDistance.setBoolean(withinDistance());
-    }
-
-    @Override
-    public Status diagnosticStatus() {
-        Status s = new Status();
-
-        if(distance == -1){
-            s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
-        }else{
-            s.addReport(ReportLevel.INFO, "LiDAR Connected");
-        }
-
-        s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance);
-
-        return s;
-    }
-    
-}

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

@@ -1,385 +0,0 @@
-package frc4388.robot.subsystems;
-
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
-
-import java.time.Instant;
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Optional;
-
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.simulation.PhotonCameraSim;
-import org.photonvision.simulation.SimCameraProperties;
-import org.photonvision.simulation.VisionSystemSim;
-import org.photonvision.targeting.MultiTargetPNPResult;
-import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
-
-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.networktables.GenericEntry;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc4388.robot.Constants.FieldConstants;
-import frc4388.robot.Constants.VisionConstants;
-import frc4388.utility.Status;
-import frc4388.utility.Subsystem;
-import frc4388.utility.Status.ReportLevel;
-
-public class Vision extends Subsystem {
-
-    // private PhotonCamera leftCamera;
-    // private PhotonCamera rightCamera;
-
-    private PhotonCamera[] cameras;
-    private PhotonPoseEstimator[] estimators;
-    private List<EstimatedRobotPose> poses = new ArrayList<>();
-
-    private boolean isTagDetected = false;
-    private boolean isTagProcessed = false;
-
-    private double lastLatency = 0;
-
-    public double getLastLatency() {
-        return lastLatency;
-    }
-
-    public Pose2d lastVisionPose = new Pose2d();
-    private Pose2d lastPhysOdomPose = new Pose2d();
-
-    private Matrix<N3, N1> curStdDevs;
-
-    private Field2d field = new Field2d();
-    
-    ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
-    .getLayout(getSubsystemName(), BuiltInLayouts.kList)
-    .withSize(2, 2);
-
-    GenericEntry sbTagDetected = subsystemLayout
-    .add("Tag Detected", false)
-    .withWidget(BuiltInWidgets.kBooleanBox)
-    .getEntry();
-
-    GenericEntry sbTagProcessed = subsystemLayout
-    .add("Tag Processed", false)
-    .withWidget(BuiltInWidgets.kBooleanBox)
-    .getEntry();
-    
-    GenericEntry sbLeftCamConnected = subsystemLayout
-    .add("Left Camera Connnected", false)
-    .withWidget(BuiltInWidgets.kBooleanBox)
-    .getEntry();
-
-    GenericEntry sbRightCamConnected = subsystemLayout
-    .add("Right Camera Connnected", false)
-    .withWidget(BuiltInWidgets.kBooleanBox)
-    .getEntry();
-    
-    public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){
-        SmartDashboard.putData(field);
-
-        this.cameras = new PhotonCamera[]{leftCamera, rightCamera};
-
-        PhotonPoseEstimator photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS);
-        PhotonPoseEstimator photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS);
-
-        photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
-        photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
-
-        this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight};
-        // resetRotations();
-    }
-
-    @Override
-    public void periodic() {
-        update();
-        field.setRobotPose(getPose2d());
-        // cameras[0].
-    }
-
-    // public int[] rotations;
-    // public Instant[] lastUpdateTimes;
-
-    // public void resetRotations(){
-    //     rotations = new int[cameras.length];
-    //     lastUpdateTimes = new Instant[cameras.length];
-    // }
-
-    private Instant lastVisionTime = null;
-
-
-    private void update() {
-        isTagProcessed = false;
-        isTagDetected = false;
-
-        Instant now = Instant.now();
-
-        int cams = 0;
-
-        // double X = 0;
-        // double Y = 0;
-        // double Yaw = 0;
-        double latency = 0;
-
-        // Pose2d pose = null;
-        poses.clear();
-
-        for(int i = 0; i < cameras.length; i++){
-            PhotonCamera camera = cameras[i];
-            PhotonPoseEstimator estimator = estimators[i];
-
-            var results = camera.getAllUnreadResults();
-
-            // If there are no more updates from the camera
-            if (results.size() == 0) 
-                continue;
-
-            
-            var result = results.get(results.size()-1);
-            latency += result.getTimestampSeconds();
-
-            isTagDetected = isTagDetected | result.hasTargets();
-
-            // If there are no tags
-            if(!result.hasTargets())
-                continue;
-
-            Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
-
-            // If the tag was failed to be processed
-            if(estimatedRobotPose.isEmpty())
-                continue;
-            
-            poses.add(estimatedRobotPose.get());
-
-            // if(pose == null)
-            //     pose = estimatedRobotPose.get().estimatedPose.toPose2d();
-            // else
-            //     pose = pose.interpolate(pose, 0.5);
-            // X += pose.getX();
-            // Y += pose.getY();
-
-            // if(X > 6)
-
-            // Yaw += (pose.getRotation().getDegrees() + 180) % 360;
-            // cams++;
-
-            isTagProcessed = true;
-        
-            
-        }
-
-        // lastLatency = latency / cams;
-
-        // if(isTagProcessed){
-
-
-        //     lastVisionPose = pose;
-        //     // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle));
-        //     // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360));
-
-        //     // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations());
-        //     // SmartDashboard.putNumber("Rotations", rotations);
-
-        //     lastVisionTime = now;
-        // }
-    }
-
-
-    /**
-     * 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;
-    }
-
-
-    // /**
-    //  * 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;
-    //         }
-    //     }
-    // }
-
-    /**
-     * Returns the latest standard deviations of the estimated pose from {@link
-     * #getEstimatedGlobalPose()}, for use with {@link
-     * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
-     * only be used when there are targets visible.
-     */
-    public Matrix<N3, N1> getEstimationStdDevs() {
-        return curStdDevs;
-    }
-
-
-
-
-    public void setLastOdomPose(Optional<Pose2d> pose){
-        if(pose.isPresent())
-            lastPhysOdomPose = pose.get();
-    }
-
-    // public double getLastOdomSpeed(){
-    //     return lastOdomSpeed;
-    // }
-
-    public Pose2d getPose2d() {
-        if(lastPhysOdomPose != null)
-            return lastPhysOdomPose;
-        return new Pose2d();
-        // if(isTagDetected && isTagProcessed)
-        //     // return lastVisionPose;
-        // else
-        //     return lastPhysOdomPose;
-    }
-
-    public static double getTime() {
-        return Utils.getCurrentTimeSeconds();
-    }
-
-    public boolean isTag(){
-        return isTagDetected && isTagProcessed;
-    }
-
-
-    public void addVisionMeasurement( SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
-        for(EstimatedRobotPose pose : poses){
-            drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds));
-        }
-    }
-
-
-
-
-
-
-
-
-    @Override
-    public String getSubsystemName() {
-        return "Vision";
-    }
-
-//   GenericEntry sbShiftState = subsystemLayout
-//   .add("Shift State", 0)
-//   .withWidget(BuiltInWidgets.kNumberBar)
-//   .getEntry();
-
-
-    @Override
-    public void queryStatus() {
-        sbTagDetected.setBoolean(isTagDetected);
-        sbTagProcessed.setBoolean(isTagProcessed);
-        sbLeftCamConnected.setBoolean(cameras[0].isConnected());
-        sbRightCamConnected.setBoolean(cameras[1].isConnected());
-        // field.setRobotPose(getPose2d());
-    }
-
-    @Override
-    public Status diagnosticStatus() {
-        Status status = new Status();
-
-        if(cameras[0].isConnected())
-            status.addReport(ReportLevel.INFO, "Left Camera Connected");
-        else
-            status.addReport(ReportLevel.ERROR, "Left Camera DISCONNECTED");
-
-        if(cameras[1].isConnected())
-            status.addReport(ReportLevel.INFO, "Right Camera Connected");
-        else
-            status.addReport(ReportLevel.ERROR, "Right Camera DISCONNECTED");
-
-
-        return status;
-    }
-    
-}

src/main/java/frc4388/robot/subsystems/elevator/Elevator.java

@@ -0,0 +1,379 @@
+// 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.elevator;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.constants.Constants.ElevatorConstants;
+import frc4388.robot.constants.Constants.LEDConstants;
+import frc4388.robot.subsystems.LED;
+import frc4388.robot.subsystems.elevator.ElevatorIO.ElevatorState;
+import frc4388.utility.status.Status;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status.ReportLevel;
+
+public class Elevator extends SubsystemBase implements Queryable {
+  ElevatorIO io;
+  ElevatorStateAutoLogged state = new ElevatorStateAutoLogged();
+
+  /** Creates a new Elevator. */
+  private LED led;
+
+  @SuppressWarnings("unused")
+  public long wait = 0;
+  public long maxWait = 1000;
+
+  public boolean elevatorManualStop = true;
+  public boolean endefectorManualStop = true;
+
+  public boolean disableAutoIntake = false;
+
+  public boolean seededZeroEndefector = false;
+  public boolean seededZeroElevator = false;
+
+  // private ElevatorState state = new ElevatorState();
+
+  public enum CoordinationState {
+    Waiting, // for coral into the though
+    WatingBeamTripped, //once the beam break trips
+    Ready, // Has coral in endefector
+    Hovering, // Has coral in endefector
+    L2Score,
+    L2ScoreLeave,
+    PrimedThree, // Arm and elevator Waiting to score in the level 3 position
+    ScoringThree, // Arm and elevator in the level three position
+    PrimedFour, // Arm and elevator Waiting to score in the level 4 position
+    ScoringFour, // Arm and elevator in the level four position
+    BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef.
+    BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef.
+    BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef.
+    BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef.
+  }
+
+  private CoordinationState currentState;
+
+  // public Elevator(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, LED led) {
+  public Elevator(ElevatorIO io, LED led) {
+    this.io = io;
+    this.led = led;
+
+    currentState = CoordinationState.Ready;
+
+    FaultReporter.register(this);
+  }
+
+
+  public void transitionState(CoordinationState state) {
+    // elevatorMotor.enable();
+    
+
+    currentState = state;
+    switch (currentState) {
+      case Waiting: {
+        wait = System.currentTimeMillis() + maxWait;
+        io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_ELEVATOR);
+        io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + (!seededZeroEndefector ? 10 : 0));
+        led.setMode(LEDConstants.WAITING_PATTERN);
+        break;
+      }
+
+      case WatingBeamTripped: {
+        io.elevatorToPosition(ElevatorConstants.WAITING_POSITION_BEAM_BREAK_ELEVATOR);
+        io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        led.setMode(LEDConstants.DOWN_PATTERN);
+        break;
+      }
+
+      case Ready: {
+        io.elevatorToPosition(ElevatorConstants.GROUND_POSITION_ELEVATOR + (!seededZeroElevator ? 10 : 0));
+        io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        led.setMode(LEDConstants.DOWN_PATTERN);
+        break;
+      }
+
+      case Hovering: {
+        io.elevatorToPosition(ElevatorConstants.HOVERING_POSITION_ELEVATOR);
+        io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR);
+        led.setMode(LEDConstants.READY_PATTERN);
+        break;
+      }
+
+      case L2Score: {
+        io.elevatorToPosition(ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case L2ScoreLeave: {
+        io.elevatorToPosition(ElevatorConstants.L2_LEAVE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+      
+      case PrimedFour: {
+        io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.PRIMED_FOUR_ENDEFFECTOR);
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case ScoringFour: {
+        io.elevatorToPosition(ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case PrimedThree: {
+        io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.PRIMED_THREE_ENDEFFECTOR);
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+      
+      case ScoringThree: {
+        io.elevatorToPosition(ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case BallRemoverL2Primed: {
+        io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case BallRemoverL2Go: {
+        io.elevatorToPosition(ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      case BallRemoverL3Primed: {
+        io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR);
+        break;
+      }
+
+      case BallRemoverL3Go: {
+        io.elevatorToPosition(ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get());
+        io.endeffectorToPosition(ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get());
+        led.setMode(LEDConstants.SCORING_PATTERN);
+        break;
+      }
+
+      default: {
+        assert false;
+      }
+    }
+
+  }
+
+  public void togggleAutoIntaking() {
+    disableAutoIntake = !disableAutoIntake;
+  }
+
+  public boolean elevatorAtReference() {
+    double diffrence = state.elevatorRefrence - state.elevatorPosition;
+
+    boolean headedUp = diffrence < 0;
+
+    return (Math.abs(diffrence) <= 0.5 
+      || (state.elevatorReverseLimit && headedUp) 
+      || (state.elevatorForwardLimit && !headedUp)
+    );
+  }
+
+  public boolean endeffectorAtReference() {
+    double diffrence = state.endeffectorRefrence - state.endeffectorPosition;
+
+    boolean headedUp = diffrence < 0;
+
+    return (Math.abs(diffrence) <= 0.5 
+      || (state.elevatorReverseLimit && headedUp) 
+      || (state.endeffectorForwardLimit && !headedUp)
+    );
+  }
+  // public void driveElevatorStick(Translation2d stick) {
+  //   if (stick.getNorm() > 0.05) {
+  //     elevatorMotor.set(stick.getY());
+  //   }
+  // }
+
+  public boolean getEndeffectorLimit() {
+    return state.endeffectorLimitSwitch;
+  }
+
+  private void periodicWaiting() {
+    if (!state.basinBeamBreak) 
+      transitionState(CoordinationState.Ready);
+    // if(!endeffectorLimitSwitch.get())
+    //   transitionState(CoordinationState.Hovering);
+  }
+
+  // private void periodicWaitingTripped() {
+  //   if (!basinBeamBreak.get() && System.currentTimeMillis() > wait) 
+  //     transitionState(CoordinationState.Ready);
+  // }
+  
+  private void periodicReady() {
+    if (elevatorAtReference() && !state.endeffectorLimitSwitch)
+      transitionState(CoordinationState.Hovering);
+    if(elevatorAtReference() && state.endeffectorLimitSwitch)
+      transitionState(CoordinationState.Hovering);
+  }
+
+  @SuppressWarnings("unused")
+  private void periodicScoring() {
+    if (!state.endeffectorLimitSwitch) 
+      transitionState(CoordinationState.Waiting);
+  }
+
+  public void manualElevatorVel(double velocity) {
+    if (Math.abs(velocity) > 0.1) {
+      io.elevatorToVelocity(velocity);
+      elevatorManualStop = false;
+      return;
+    }
+    if (!elevatorManualStop) {
+      elevatorManualStop = true;
+      io.elevatorToVelocity(0);
+    }
+  }
+
+  public void manualEndeffectorVel(double velocity) {
+    if (Math.abs(velocity) > 0.1) {
+      io.endeffectorToVelocity(velocity);
+      endefectorManualStop = false;
+      return;
+    }
+    if (!endefectorManualStop) {
+      endefectorManualStop = true;
+      io.endeffectorToVelocity(0);
+    }
+  }
+
+  @Override
+  public void periodic() {
+
+    // double elevatorVelocity = elevatorMotor.getVelocity().getValueAsDouble();
+    // double elevatorTorque = elevatorMotor.getTorqueCurrent().getValueAsDouble();
+    // double endeffectorVelocity = endeffectorMotor.getVelocity().getValueAsDouble();
+    // double endeffectorTorque = endeffectorMotor.getTorqueCurrent().getValueAsDouble();
+
+
+    // if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
+    //   PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
+    // }
+
+    // This method will be called once per scheduler run
+    // SmartDashboard.putNumber("Velocity Endeffector", endeffectorVelocity);
+    // SmartDashboard.putNumber("Torque Endeffector", endeffectorTorque);
+    // SmartDashboard.putNumber("Basin", basinBeamBreak.get() ? 1 : 0);
+    // SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
+    // SmartDashboard.putNumber("intake", intakeIR.get() ? 1 : 0);
+    // SmartDashboard.putString("State", currentState.toString());
+
+    io.updateInputs(state);
+    Logger.processInputs("Elevator", state);
+    
+    if (!seededZeroEndefector && state.endeffectorForwardLimit) {
+      io.endeffectorToPosition(0);
+      seededZeroEndefector = !seededZeroEndefector;
+    }
+
+    if (!seededZeroElevator && state.elevatorReverseLimit) {
+      io.endeffectorToPosition(0);
+      seededZeroElevator = !seededZeroElevator;
+    }
+    
+    if (disableAutoIntake) return;
+
+    if (currentState == CoordinationState.Waiting) {
+      periodicWaiting();
+    } else if (currentState == CoordinationState.WatingBeamTripped) {
+      // periodicWaitingTripped();
+    } else if (currentState == CoordinationState.Ready) {
+      periodicReady();
+    }
+
+    if(!state.intakeIR){
+      led.setMode(LEDConstants.DOWN_PATTERN);
+    }
+
+    
+    // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
+    //   periodicScoring();
+    // }
+  }
+
+  @AutoLogOutput(key="Elevator/state")
+  public String getState() {
+    return currentState.toString();
+  }
+
+  public boolean isL4Primed() {
+    return currentState == CoordinationState.PrimedFour;
+  }
+
+  public boolean isL3Primed() {
+    return currentState == CoordinationState.PrimedThree;
+  }
+
+  public boolean hasCoral() {
+    return elevatorAtReference() && currentState == CoordinationState.Hovering || !state.endeffectorLimitSwitch;
+  }
+
+  public void elevatorStop() {
+    io.elevatorToVelocity(0);
+  }
+
+  public void endeffectorStop() {
+    io.endeffectorToVelocity(0);
+  }
+
+  public boolean readyToMove() {
+    return !state.intakeIR || hasCoral() || !state.endeffectorLimitSwitch;
+    // return hasCoral();
+  }
+
+  public void armShuffle(){
+    if(!state.basinBeamBreak){
+      //shuffle the coral with the arm until coral hits beam break
+    }
+  }
+
+  @Override
+  public String getName() {
+    return "Elevator";
+  }
+
+  // @Override
+  // public void queryStatus() {}
+
+  @Override
+  public Status diagnosticStatus() {
+    Status status = new Status();
+
+    status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name());
+
+    return status;
+  }
+}

src/main/java/frc4388/robot/subsystems/elevator/ElevatorIO.java

@@ -0,0 +1,34 @@
+package frc4388.robot.subsystems.elevator;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import frc4388.robot.subsystems.lidar.LidarIO.LidarState;
+
+public interface ElevatorIO {
+    @AutoLog
+    public class ElevatorState {
+        public double elevatorRefrence;
+        public double elevatorPosition;
+        public boolean elevatorForwardLimit;
+        public boolean elevatorReverseLimit;
+
+        public double endeffectorRefrence;
+        public double endeffectorPosition;
+        public boolean endeffectorForwardLimit;
+        public boolean endeffectorReverseLimit;
+
+        
+        public boolean basinBeamBreak;
+        public boolean endeffectorLimitSwitch;
+        public boolean intakeIR;
+
+    }
+
+    public default void elevatorToPosition(double position) {}
+    public default void endeffectorToPosition(double position) {}
+    public default void elevatorToVelocity(double velocity) {}
+    public default void endeffectorToVelocity(double velocity) {}
+
+    public default void updateInputs(ElevatorState state) {}
+}

src/main/java/frc4388/robot/subsystems/elevator/ElevatorReal.java

@@ -0,0 +1,80 @@
+package frc4388.robot.subsystems.elevator;
+
+import com.ctre.phoenix6.controls.PositionDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import frc4388.robot.constants.Constants.ElevatorConstants;
+
+public class ElevatorReal implements ElevatorIO {
+    TalonFX elevatorMotor;
+    TalonFX endeffectorMotor;
+    
+    DigitalInput basinLimitSwitch;
+    DigitalInput endeffectorLimitSwitch;
+    DigitalInput intakeIR;
+
+    
+    double elevatorRefrence = 0;
+    double endeffectorRefrence = 0;
+
+    public ElevatorReal(TalonFX elevatorTalonFX, TalonFX endeffectorTalonFX, DigitalInput basinLimitSwitch, DigitalInput endeffectorLimitSwitch, DigitalInput intakeIR) {
+        this.elevatorMotor = elevatorTalonFX;
+        this.endeffectorMotor = endeffectorTalonFX;
+
+        this.basinLimitSwitch = basinLimitSwitch;
+        this.endeffectorLimitSwitch = endeffectorLimitSwitch;
+        this.intakeIR = intakeIR;
+
+        
+        elevatorMotor.setNeutralMode(NeutralModeValue.Brake);
+        endeffectorMotor.setNeutralMode(NeutralModeValue.Brake);
+        
+        elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
+        endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
+    }
+
+    @Override
+    public void updateInputs(ElevatorState state) {
+        state.elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
+        state.elevatorRefrence = elevatorRefrence;
+        state.elevatorForwardLimit = elevatorMotor.getForwardLimit().asSupplier().get().value == 0;
+        state.elevatorReverseLimit = elevatorMotor.getReverseLimit().asSupplier().get().value == 0;
+
+        state.endeffectorPosition = endeffectorMotor.getPosition().getValueAsDouble();
+        state.endeffectorRefrence = endeffectorRefrence;
+        state.endeffectorForwardLimit = endeffectorMotor.getForwardLimit().asSupplier().get().value == 0;
+        state.endeffectorReverseLimit = endeffectorMotor.getReverseLimit().asSupplier().get().value == 0;
+
+
+        state.basinBeamBreak = basinLimitSwitch.get();
+        state.endeffectorLimitSwitch = endeffectorLimitSwitch.get();
+        state.intakeIR = intakeIR.get();
+    }
+
+    @Override
+    public void elevatorToPosition(double position) {
+        elevatorRefrence = position; 
+        var request = new PositionDutyCycle(position);
+        elevatorMotor.setControl(request);
+    }
+
+    @Override
+    public void endeffectorToPosition(double position) {
+        endeffectorRefrence = position;
+        var request = new PositionDutyCycle(position);
+        endeffectorMotor.setControl(request);
+    }
+
+    @Override
+    public void elevatorToVelocity(double velocity) {
+        elevatorMotor.set(velocity);
+    }
+
+    
+    @Override
+    public void endeffectorToVelocity(double velocity) {
+        endeffectorMotor.set(velocity);
+    }
+}

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

@@ -2,56 +2,44 @@
 // 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;
+package frc4388.robot.subsystems.swerve;
 
-import java.util.Optional;
+import org.littletonrobotics.junction.AutoLogOutput;
-import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.Logger;
 
-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 com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue;
-
 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.math.kinematics.SwerveModuleState;
-import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.constants.Constants.AutoConstants;
-import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.vision.Vision;
-import frc4388.robot.commands.GotoLastApril;
+import frc4388.utility.compute.TimesNegativeOne;
-import frc4388.robot.commands.LidarAlign;
+import frc4388.utility.status.Status;
-import frc4388.utility.Status;
+import frc4388.utility.status.FaultReporter;
-import frc4388.utility.Subsystem;
+import frc4388.utility.status.Queryable;
-import frc4388.utility.TimesNegativeOne;
-import frc4388.utility.Status.ReportLevel;
 
 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.commands.PathPlannerAuto;
 import com.pathplanner.lib.config.RobotConfig;
 
-public class SwerveDrive extends Subsystem {
+public class SwerveDrive extends SubsystemBase implements Queryable {
-    private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+    // private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+
+    private SwerveIO io;
+    private SwerveStateAutoLogged state;
 
     private Vision vision;
 
-    private int gear_index = SwerveDriveConstants.STARTING_GEAR;
+
-    private boolean stopped = false;
+
-    private boolean robotKnowsWhereItIs = false;
+    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;
@@ -60,7 +48,7 @@ public class SwerveDrive extends Subsystem {
 
     public double lastOdomSpeed;
 
-    public Pose2d initalPose2d = null;
+    public Pose2d initalPose2d = new Pose2d();
 
 
     public double rotTarget = 0.0;
@@ -68,12 +56,14 @@ public class SwerveDrive extends Subsystem {
     public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
     /** Creates a new SwerveDrive. */
-    public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
+    public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
         // public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
         // swerveDriveTrain) {
-        super();
+        FaultReporter.register(this);
+
+        this.io = swerveDriveTrain;
+        this.state = new SwerveStateAutoLogged();
 
-        this.swerveDriveTrain = swerveDriveTrain;
         this.vision = vision;
 
         RobotConfig config;
@@ -86,12 +76,12 @@ public class SwerveDrive extends Subsystem {
         // DoubleSupplier a = () -> 1.d;
         AutoBuilder.configure(
                 () -> {
-                    return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
+                    return getPose2d();
                 }, // Robot pose supplier
                 this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
                                              // pose)
-                () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                () -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-                (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
+                (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
@@ -115,12 +105,36 @@ public class SwerveDrive extends Subsystem {
                 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;
-        swerveDriveTrain.resetPose(pose);
+        io.resetPose(pose);
     }
 
     // public void oneModuleTest(SwerveModule module, Translation2d leftStick,
@@ -154,11 +168,14 @@ public class SwerveDrive extends Subsystem {
 
             // ! drift correction
             if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
-                rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
+
-                swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
+                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 {
@@ -171,13 +188,13 @@ public class SwerveDrive extends Subsystem {
                     SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
                     SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
                 );
-                swerveDriveTrain.setControl(ctrl);
+                io.setControl(ctrl);
                 SmartDashboard.putBoolean("drift correction", true);
             }
 
            
         } else { // Create robot-relative speeds.
-            swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
+            io.setControl(new SwerveRequest.RobotCentric()
                     .withVelocityX(leftStick.getX() * speedAdjust)
                     .withVelocityY(-leftStick.getY() * speedAdjust)
                     .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
@@ -188,7 +205,7 @@ public class SwerveDrive extends Subsystem {
         stopped = false;
         // Create robot-relative speeds.
         if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
-        swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
+        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));
@@ -210,7 +227,7 @@ public class SwerveDrive extends Subsystem {
 
         leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
-        swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+        io.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * speedAdjust)
                 .withVelocityY(leftStick.getY() * speedAdjust)
                 .withTargetDirection(rightStick.getAngle()));
@@ -228,7 +245,7 @@ public class SwerveDrive extends Subsystem {
             SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
             SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
         );
-        swerveDriveTrain.setControl(ctrl);
+        io.setControl(ctrl);
     }
 
     public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
@@ -239,35 +256,23 @@ public class SwerveDrive extends Subsystem {
             .withVelocityY(leftStick.getY() * speedAdjust)
             .withTargetDirection(heading);
         // ctrl.HeadingController.setPID(
-        //     SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
+        //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
-        //     SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
+        //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
-        //     SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
+        //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
         // );
-        swerveDriveTrain.setControl(ctrl);
+        io.setControl(ctrl);
-    }
-
-    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);
-        }
     }
 
     public void activateLuigiMode() {
-        setLimits(20);
+        io.setLimits(20);
     }
 
     public void deactivateLuigiMode() {
-        setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
+        io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
     }
 
     public boolean rotateToTarget(double angle) {
-        swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+        io.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(0)
                 .withVelocityY(0)
                 .withTargetDirection(Rotation2d.fromDegrees(angle)));
@@ -279,6 +284,10 @@ public class SwerveDrive extends Subsystem {
         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:
@@ -289,7 +298,7 @@ public class SwerveDrive extends Subsystem {
 
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
-        swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+        io.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * -speedAdjust)
                 .withVelocityY(leftStick.getY() * speedAdjust)
                 .withTargetDirection(rot));
@@ -301,11 +310,13 @@ public class SwerveDrive extends Subsystem {
     }
 
     public Pose2d getPose2d() {
-        return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d);
+        if(state.currentPose == null)
+            return initalPose2d;
+        return state.currentPose;
     }
 
     public void resetGyro() {
-        swerveDriveTrain.tareEverything();
+        io.tareEverything();
         robotKnowsWhereItIs = false;
         rotTarget = 0;
         // vision.resetRotations();
@@ -314,7 +325,7 @@ public class SwerveDrive extends Subsystem {
 
     public void softStop() {
         stopped = true;
-        swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
+        io.setControl(new SwerveRequest.FieldCentric()
             .withVelocityX(0)
             .withVelocityY(0)
             .withRotationalRate(0)
@@ -333,27 +344,21 @@ public class SwerveDrive extends Subsystem {
         SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
         SmartDashboard.putNumber("RotTartget", rotTarget);
 
-        double time = Vision.getTime();
+        io.updateInputs(state);
-        double freq =  swerveDriveTrain.getOdometryFrequency();
+        Logger.processInputs("SwerveDrive", state);
         
-        Optional<Pose2d> curpose = swerveDriveTrain.samplePoseAt(time);
+        vision.setLastOdomPose(state.currentPose);
-        Optional<Pose2d> lastPose = swerveDriveTrain.samplePoseAt(time - freq);
+        setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
-        
-        vision.setLastOdomPose(curpose);
-        setLastOdomSpeed(curpose, lastPose, freq);
 
         if (vision.isTag()) {
             Pose2d pose = vision.getPose2d();
             if (!robotKnowsWhereItIs) {
                 robotKnowsWhereItIs = true;
-                Pose2d currPose = getPose2d();
+                Pose2d curPose = getPose2d();
-                double deltaAngle = pose.getRotation().getDegrees() - currPose.getRotation().getDegrees();
+                rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees();
-                rotTarget += deltaAngle;
             }
 
-            vision.addVisionMeasurement(swerveDriveTrain);
+            io.addVisionMeasurement(vision.getPosesToAdd());
-            // swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
-            //back to the ~~future~~ *past*
         }
 
         // if(e.isPresent())
@@ -431,57 +436,32 @@ public class SwerveDrive extends Subsystem {
 
 
 
-    public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
+    public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){
-        if(curPose.isPresent() && lastPose.isPresent()){
+        if(curPose != null && lastPose != null){
-            this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
+            lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq;
-        
-
-            // double rotDiff = curPose.get().getRotation().getDegrees() - lastPose.get().getRotation().getDegrees();
-    
-            // if(rotDiff >= 180){
-            //     vision.subtractRotation();
-            // }else if(rotDiff <= -180){
-            //     vision.addRotation();
-            // }
-            SmartDashboard.putNumber("Speed", lastOdomSpeed);
         }
     }
 
+    @AutoLogOutput(key="SwerveDrive/speed ")
+    public double getOdometrySpeed() {
+        return lastOdomSpeed;
+    }
+    
+
 
     @Override
-    public String getSubsystemName() {
+    public String getName() {
         return "Swerve Drive Controller";
     }
 
-    ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
-            .getLayout(getSubsystemName(), BuiltInLayouts.kList)
-            .withSize(2, 2);
-
-    GenericEntry sbGyro = subsystemLayout
-            .add("Gyro angle", 0)
-            .withWidget(BuiltInWidgets.kGyro)
-            .getEntry();
-
-    GenericEntry sbShiftState = subsystemLayout
-            .add("Shift State", 0)
-            .withWidget(BuiltInWidgets.kNumberBar)
-            .getEntry();
-
-    @Override
-    public void queryStatus() {
-        sbGyro.setDouble(getGyroAngle());
-        sbShiftState.setDouble(this.speedAdjust);
-
-        // TODO: Add more status things
-    }
-
     @Override
     public Status diagnosticStatus() {
         Status status = new Status();
 
-        status.addReport(ReportLevel.INFO,
+        // 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.");
+        //         "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/Alliance.java

@@ -1,5 +0,0 @@
-package frc4388.utility;
-
-public class Alliance {
-
-}

src/main/java/frc4388/utility/AprilTag.java

@@ -1,13 +0,0 @@
-package frc4388.utility;
-
-// This is a seperate class in case I want to encode rotation or other
-// information about the tag
-public class AprilTag {
-    public final double x, y, z;
-
-    public AprilTag(double _x, double _y, double _z) {
-        x = _x;
-        y = _y;
-        z = _z;
-    }
-}

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

@@ -2,22 +2,40 @@ 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 = new ArrayList<>();
+    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 DeferredBlock(Runnable block) {
+    public static void addBlock(Runnable block) {
-        m_blocks.add(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() {
-        if (m_hasRun) return;
 
-        for (Runnable block : m_blocks) {
+        // Run blocks that run multiple times.
+        for (Runnable block : m_blocks_rerun) {
             block.run();
         }
 
-        m_blocks.clear(); // for garbage collection
+        // 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/DeferredBlockMulti.java

@@ -1,18 +0,0 @@
-package frc4388.utility;
-
-import java.util.ArrayList;
-
-public class DeferredBlockMulti {
-    private static ArrayList<Runnable> m_blocks = new ArrayList<>();
-
-    public DeferredBlockMulti(Runnable block) {
-        m_blocks.add(block);
-    }
-
-    public static void execute() {
-
-        for (Runnable block : m_blocks) {
-            block.run();
-        }
-    }
-}

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

@@ -1,269 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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;
-
-// import com.ctre.phoenix.sensors.WPI_Pigeon2;
-import com.ctre.phoenix6.hardware.Pigeon2;
-import com.kauailabs.navx.frc.AHRS;
-
-// import edu.wpi.first.wpilibj.GyroBase;
-// import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Rotation2d;
-
-/**
- * Gyro class that allows for interchangeable use between a pigeon and a navX
- */
-public class RobotGyro {
-    private RobotTime m_robotTime = RobotTime.getInstance();
-
-    private Pigeon2 m_pigeon = null;
-    private AHRS m_navX = null;
-    public boolean m_isGyroAPigeon; //true if pigeon, false if navX
-
-    private double m_lastPigeonAngle;
-    private double m_deltaPigeonAngle;
-
-    private double pitchZero = 0;
-    private double rollZero = 0;
-
-    /**
-     * Creates a Gyro based on a pigeon
-     * @param gyro the gyroscope to use for Gyro
-     */
-    public RobotGyro(Pigeon2 gyro) {
-        m_pigeon = gyro;
-        m_isGyroAPigeon = true;
-    }
-
-    /**
-     * Creates a Gyro based on a navX
-     * @param gyro the gyroscope to use for Gyro
-     */
-    public RobotGyro(AHRS gyro){
-        m_navX = gyro;
-        m_isGyroAPigeon = false;
-    }
-
-    /**
-     * Resets yaw, pitch, and roll.
-     */
-    public void resetZeroValues() {
-        if (!m_isGyroAPigeon) return;
-
-        // pitchZero = m_pigeon.getPitch();
-        // rollZero =  m_pigeon.getRoll();
-    }
-
-    /**
-     * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
-     * that the getRate() method for a navX will likely be much more accurate than for a pigeon.
-     */
-    public void updatePigeonDeltas() {
-        double currentPigeonAngle = getAngle();
-        m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
-        m_lastPigeonAngle = currentPigeonAngle;
-    }
-
-    /**
-     * <p>NavX:
-     * <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
-     * the center value as the Accumulator center value for subsequent measurements. It's important to
-     * make sure that the robot is not moving while the centering calculations are in progress, this
-     * is typically done when the robot is first turned on while it's sitting at rest before the
-     * competition starts.
-     * 
-     * <p>Pigeon:
-     * <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
-     * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
-     * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
-     * make sure that the robot is not moving while the tempurature calculations are in progress, this
-     * is typically done when the robot is first turned on while it's sitting at rest before the
-     * competition starts.
-     */
-    public void calibrate() {
-        return;
-        // if (m_isGyroAPigeon) {
-        //     m_pigeon.calibrate();
-        // } else {
-        //     m_navX.calibrate();
-        // }
-    }
-
-    public void reset() {
-        resetZeroValues();
-
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(0);
-        } else {
-            m_navX.reset();
-        }
-
-    }
-
-    public void reset(double val) {
-        resetZeroValues();
-
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(val);
-        } else {
-            m_navX.reset();
-        }
-
-    }
-
-    public void resetFlip() {
-        resetZeroValues();
-
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(180);
-        } else {
-            m_navX.reset();
-        }
-
-    }
-
-    public void resetNinety() {
-        resetZeroValues();
-        
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(90);
-        } else {
-            m_navX.reset();
-        }
-        
-    }
-
-    public void resetTwoSeventy() {
-        resetZeroValues();
-
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(270);
-        } else {
-            m_navX.reset();
-        }
-
-    }
-
-    public void resetRightSideBlue() {
-        resetZeroValues();
-
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(60);
-        } else {
-            m_navX.reset();
-        }   
-
-    }
-
-    public void resetAmpSide() {
-        resetZeroValues();
-
-        if (m_isGyroAPigeon) {
-            m_pigeon.setYaw(-60);
-        } else {
-            m_navX.reset();
-        }   
-
-    }
-
-    /**
-	 * Get Yaw, Pitch, and Roll data.
-	 *
-	 * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
-	 *					Yaw is within [-368,640, +368,640] degrees.
-	 *					Pitch is within [-90,+90] degrees.
-	 *					Roll is within [-90,+90] degrees.
-     */
-    private double[] getPigeonAngles() {
-        //m_pigeon.getAngle(); // This appeared to not do anything?
-        var rotation = m_pigeon.getRotation3d();
-
-        return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
-    }
-
-    public Rotation2d getRotation2d() {
-        return m_pigeon.getRotation2d();
-    }
-
-    public double getAngle() {
-        if (m_isGyroAPigeon) {
-            return getPigeonAngles()[2];
-        } else {
-            return m_navX.getAngle();
-        }
-    }
-
-    public double getYaw() {
-       return this.getAngle();
-    }
-
-    /**
-     * Gets an absolute heading of the robot
-     * @return heading from -180 to 180 degrees
-     */
-    public double getHeading() {
-        return getHeading(getAngle());
-    }
-
-    /**
-     * Gets an absolute heading of the robot
-     * @return heading from -180 to 180 degrees
-     */
-    public double getHeading(double angle) {
-        return Math.IEEEremainder(angle, 360);
-    }
-
-    /**
-     * Returns the current pitch value (in degrees, from -90 to 90)
-     * reported by the sensor.  Pitch is a measure of rotation around
-     * the Y Axis.
-     * @return The current pitch value in degrees (-90 to 90).
-     */
-    public double getPitch() {
-        if (m_isGyroAPigeon) {
-            return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
-        } else {
-            return MathUtil.clamp(m_navX.getPitch(), -90, 90);
-        }
-    }
-
-    /**
-     * Returns the current roll value (in degrees, from -90 to 90)
-     * reported by the sensor.  Roll is a measure of rotation around
-     * the X Axis.
-     * @return The current roll value in degrees (-90 to 90).
-     */
-    public double getRoll() {
-        if (m_isGyroAPigeon) {
-            return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
-        } else {
-            return MathUtil.clamp(m_navX.getRoll(), -90, 90);
-        }
-    }
-
-    public double getRate() {
-        if (m_isGyroAPigeon) {
-            return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
-        } else {
-            return m_navX.getRate();
-        }
-    }
-
-    public Pigeon2 getPigeon(){
-        return m_pigeon;
-    }
-
-    public AHRS getNavX(){
-        return m_navX;
-    }
-
-    public void close() throws Exception {
-
-    }
-}

src/main/java/frc4388/utility/Subsystem.java

@@ -1,25 +0,0 @@
-package frc4388.utility;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public abstract class Subsystem extends SubsystemBase {
-    public static List<Subsystem> subsystems = new ArrayList<>();
-
-    public Subsystem(){
-        subsystems.add(this);
-    }
-
-    public void Log(String str) {
-        System.out.println(getSubsystemName() + " - " + str);
-    }
-
-    // Get name of subsystem, for use in log.
-    public abstract String getSubsystemName();
-    // Get what the subystem is currently doing, such as "Shooter spun up". This should post to SmartDashboard
-    public abstract void queryStatus();
-    // Proactivly search for any errors in each subsystem
-    public abstract Status diagnosticStatus(); 
-}

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

@@ -1,4 +1,4 @@
-package frc4388.utility;
+package frc4388.utility.compute;
 
 import java.nio.ByteBuffer;
 

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

@@ -1,15 +1,11 @@
-package frc4388.utility;
+package frc4388.utility.compute;
-
-import java.util.Optional;
 
 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 edu.wpi.first.wpilibj.DriverStation;
+import frc4388.robot.constants.Constants.AutoConstants;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc4388.robot.constants.Constants.FieldConstants;
-import frc4388.robot.Constants.AutoConstants;
-import frc4388.robot.Constants.FieldConstants;
 
 public class ReefPositionHelper {
     public enum Side {

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

@@ -5,7 +5,7 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc4388.utility;
+package frc4388.utility.compute;
 
 /**
  * <p>Keeps track of Robot times like time passed, delta time, etc

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

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.utility.compute;
 
 /** Aarav's good units class (better than WPILib)
  * @author Aarav Shah */

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

@@ -1,4 +1,4 @@
-package frc4388.utility;
+package frc4388.utility.compute;
 
 import java.util.Optional;
 
@@ -7,7 +7,7 @@ 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.Constants.SwerveDriveConstants;
+import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
 
 // Class that holds weather the drivers sticks should be inverted
 public class TimesNegativeOne {

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

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.utility.compute;
 
 import java.io.FileInputStream;
 import java.io.FileOutputStream;
@@ -27,11 +27,34 @@ public class Trim {
 
     private boolean modified = false;
     private double currentValue;
+    private boolean persistant = false;
 
     private GenericEntry trimElement = null;
 
     /**
-     * Creates a Trim with a given name, upper and lower bounds, step size and intial value
+     * 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
@@ -81,22 +104,25 @@ public class Trim {
     }
 
     public boolean load() {
-        // try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
+        if(!persistant)
-        //     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;
-        // }
             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() {

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

@@ -1,6 +1,7 @@
+
 package frc4388.utility.controller;
 
-import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
+import static frc4388.robot.constants.Constants.OIConstants.LEFT_AXIS_DEADBAND;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.XboxController;

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

@@ -1,13 +1,9 @@
-package frc4388.utility;
+package frc4388.utility.status;
 
 import java.util.ArrayList;
 import java.util.List;
 
-import edu.wpi.first.hal.CANData;
+import frc4388.utility.status.Status.ReportLevel;
-import edu.wpi.first.hal.can.CANJNI;
-import edu.wpi.first.wpilibj.CAN;
-import frc4388.utility.Status.Report;
-import frc4388.utility.Status.ReportLevel;
 
 public class CanDevice {
     public static List<CanDevice> devices = new ArrayList<>();

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

@@ -1,4 +1,4 @@
-package frc4388.utility;
+package frc4388.utility.status;
 
 import java.util.ArrayList;
 import java.util.List;
@@ -44,6 +44,10 @@ public class Status {
     }
 
     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!");
     }

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

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.utility.structs;
 
 /** Add your docs here. */
 public class Gains {

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

@@ -1,4 +1,4 @@
-package frc4388.utility;
+package frc4388.utility.structs;
 
 /**
  * Add your docs here.

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

@@ -1,4 +1,4 @@
-package frc4388.utility;
+package frc4388.utility.structs;
 
 public class UtilityStructs {
     public static class TimedOutput {

vendordeps/AdvantageKit.json

@@ -0,0 +1,35 @@
+{
+    "fileName": "AdvantageKit.json",
+    "name": "AdvantageKit",
+    "version": "4.1.2",
+    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
+    ],
+    "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+    "javaDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-java",
+            "version": "4.1.2"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-wpilibio",
+            "version": "4.1.2",
+            "skipInvalidPlatforms": false,
+            "isJar": false,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ],
+    "cppDependencies": []
+}

vendordeps/PathplannerLib-2025.2.7.json

@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib-2025.2.4.json",
+    "fileName": "PathplannerLib-2025.2.7.json",
     "name": "PathplannerLib",
-    "version": "2025.2.4",
+    "version": "2025.2.7",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.2.4"
+            "version": "2025.2.7"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.2.4",
+            "version": "2025.2.7",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,

vendordeps/Phoenix6-frc2025-latest.json

@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.1.0.json",
+    "fileName": "Phoenix6-frc2025-latest.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.1.0",
+    "version": "25.4.0",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.1.0"
+            "version": "25.4.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.4.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +194,35 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.4.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdi",
+            "version": "25.4.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -196,7 +238,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -212,7 +254,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -228,7 +270,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -244,7 +286,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -260,7 +302,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -276,7 +318,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -292,7 +334,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -308,7 +350,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -324,7 +366,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -337,10 +379,26 @@
             ],
             "simMode": "swsim"
         },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.4.0",
+            "libName": "CTRE_SimProTalonFXS",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -356,7 +414,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -372,7 +430,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -384,6 +442,38 @@
                 "osxuniversal"
             ],
             "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdi",
+            "version": "25.4.0",
+            "libName": "CTRE_SimProCANdi",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "25.4.0",
+            "libName": "CTRE_SimProCANdle",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
         }
     ]
 }

vendordeps/photonlib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "photonlib.json",
     "name": "photonlib",
-    "version": "v2025.0.0-beta-8",
+    "version": "v2025.3.1",
     "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
     "frcYear": "2025",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-cpp",
-            "version": "v2025.0.0-beta-8",
+            "version": "v2025.3.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -28,7 +28,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photonlib-cpp",
-            "version": "v2025.0.0-beta-8",
+            "version": "v2025.3.1",
             "libName": "photonlib",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -43,7 +43,7 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-cpp",
-            "version": "v2025.0.0-beta-8",
+            "version": "v2025.3.1",
             "libName": "photontargeting",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -60,12 +60,12 @@
         {
             "groupId": "org.photonvision",
             "artifactId": "photonlib-java",
-            "version": "v2025.0.0-beta-8"
+            "version": "v2025.3.1"
         },
         {
             "groupId": "org.photonvision",
             "artifactId": "photontargeting-java",
-            "version": "v2025.0.0-beta-8"
+            "version": "v2025.3.1"
         }
     ]
 }