Transfer code

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

@@ -15,7 +15,9 @@ import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
+import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.robot.constants.BuildConstants;
@@ -57,6 +59,8 @@ public class Robot extends LoggedRobot {
     // FaultReporter.startThread();
   }
 
+
+
   /**
    * This function is called every robot packet, no matter the mode. Use
    * this for items like diagnostics that you want ran during disabled,
@@ -77,7 +81,6 @@ public class Robot extends LoggedRobot {
     // 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
@@ -125,6 +128,7 @@ public class Robot extends LoggedRobot {
   public void autonomousPeriodic() {
   }
 
+
   @Override
   public void teleopInit() {
     m_robotContainer.stop();
@@ -137,10 +141,7 @@ public class Robot extends LoggedRobot {
       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();
   }

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

@@ -51,11 +51,12 @@ import frc4388.robot.constants.Constants.OIConstants;
 
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
+
+import frc4388.robot.subsystems.Elevator;
 // Subsystems
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.Elevator.CoordinationState;
-import frc4388.robot.subsystems.Elevator;
 // import frc4388.robot.subsystems.Endeffector;
 import frc4388.robot.subsystems.SwerveDrive;
 
@@ -560,7 +561,7 @@ public class RobotContainer {
         NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, 
         new Translation2d(-1,0), new Translation2d(), m_robotMap.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, 
@@ -694,8 +695,8 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
         
         new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
-            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;}))
+            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust *= 2;}))
-            .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;}));
+            .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust /= 2;}));
 
         new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod()))
@@ -890,6 +891,7 @@ public class RobotContainer {
       //  return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
     }
 
+    public boolean autoChooserUpdated = false;
     public void makeAutoChooser() {
         autoChooser = new SendableChooser<String>();
         
@@ -913,6 +915,7 @@ public class RobotContainer {
         }
 
         autoChooser.onChange((filename) -> {
+            autoChooserUpdated = true;
             if (filename.equals("Taxi")) {
                 autoCommand = new SequentialCommandGroup(
                     new MoveForTimeCommand(m_robotSwerveDrive, 

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

@@ -131,7 +131,7 @@ public class DriveToReef extends Command {
             (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
             (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
             (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
-            (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
+            (!waitVelocityZero || swerveDrive.state.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
         );
         // System.out.println(finished);
 

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

@@ -7,12 +7,12 @@ 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 = 169;
+  public static final int GIT_REVISION = 170;
-  public static final String GIT_SHA = "aaef829ad2830d39cc9f9e05e61e973658d7784d";
+  public static final String GIT_SHA = "2a38f94d5eef00a093f47df192f7c5c8a2b8cf8d";
-  public static final String GIT_DATE = "2025-07-15 09:33:40 MDT";
+  public static final String GIT_DATE = "2025-07-15 10:24:11 MDT";
   public static final String GIT_BRANCH = "advantagekit";
-  public static final String BUILD_DATE = "2025-07-15 09:49:13 MDT";
+  public static final String BUILD_DATE = "2025-07-15 11:04:29 MDT";
-  public static final long BUILD_UNIX_TIME = 1752594553460L;
+  public static final long BUILD_UNIX_TIME = 1752599069523L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -4,6 +4,8 @@
 
 package frc4388.robot.subsystems;
 
+import org.littletonrobotics.junction.AutoLog;
+
 import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
@@ -25,24 +27,29 @@ public class Elevator extends SubsystemBase implements Queryable {
   private TalonFX endeffectorMotor;
   private LED led;
 
+  // @AutoLog
+  // private class ElevatorState {
     @SuppressWarnings("unused")
-  private long wait = 0;
+    public long wait = 0;
-  private long maxWait = 1000;
+    public long maxWait = 1000;
 
-  private double elevatorRefrence = 0;
+    public double elevatorRefrence = 0;
-  private double endeffectorRefrence = 0;
+    public double endeffectorRefrence = 0;
 
-  private boolean elevatorManualStop = true;
+    public boolean elevatorManualStop = true;
-  private boolean endefectorManualStop = true;
+    public boolean endefectorManualStop = true;
 
-  private boolean disableAutoIntake = false;
+    public boolean disableAutoIntake = false;
 
-  private boolean seededZeroEndefector = false;
+    public boolean seededZeroEndefector = false;
-  private boolean seededZeroElevator = false;
+    public boolean seededZeroElevator = false;
 
-  private DigitalInput basinBeamBreak;
+    public DigitalInput basinBeamBreak;
-  private DigitalInput endeffectorLimitSwitch;
+    public DigitalInput endeffectorLimitSwitch;
-  private DigitalInput intakeIR;
+    public DigitalInput intakeIR;
+  // }
+
+  // private ElevatorState state = new ElevatorState();
 
   public enum CoordinationState {
     Waiting, // for coral into the though

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

@@ -60,11 +60,6 @@ public class LED extends SubsystemBase implements Queryable {
     return "LEDs";
   }
 
-  // @Override
-  // public void queryStatus() {
-  //   SmartDashboard.putString("LED status", mode.name());
-  // }
-
   @Override
   public Status diagnosticStatus() {
     Status status = new Status();

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

@@ -51,10 +51,6 @@ public class Lidar extends SubsystemBase implements Queryable {
         return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
     }
 
-    ShuffleboardLayout subsystemLayout;
-    GenericEntry sbDistance;
-    GenericEntry sbWithinDistance;
-
     @Override
     public String getName() {
         return "Lidar " + name;

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

@@ -6,6 +6,7 @@ package frc4388.robot.subsystems;
 
 import java.util.Optional;
 
+import org.littletonrobotics.junction.AutoLog;
 import org.littletonrobotics.junction.Logger;
 
 import com.ctre.phoenix6.Utils;
@@ -26,6 +27,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.AutoConstants;
 import frc4388.robot.constants.DriveConstants;
 import frc4388.utility.compute.TimesNegativeOne;
 import frc4388.utility.status.Status;
@@ -40,13 +42,13 @@ import com.pathplanner.lib.config.RobotConfig;
 
 public class SwerveDrive extends SubsystemBase implements Queryable {
     private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
-
     private Vision vision;
 
-    private int gear_index = DriveConstants.STARTING_GEAR;
+    @AutoLog
-    private boolean stopped = false;
+    public class SwerveDriveState {
-    @SuppressWarnings("unused")
+        public int gear_index = DriveConstants.STARTING_GEAR;
-    private boolean robotKnowsWhereItIs = false;
+        public boolean stopped = false;
+        public boolean robotKnowsWhereItIs = false;
     
         public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index];
         public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED;
@@ -61,6 +63,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         public double rotTarget = 0.0;
         public Rotation2d orientRotTarget = new Rotation2d();
         public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+    }
+
+    public SwerveDriveState state = new SwerveDriveState();
 
     /** Creates a new SwerveDrive. */
     public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
@@ -81,7 +86,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         // DoubleSupplier a = () -> 1.d;
         AutoBuilder.configure(
                 () -> {
-                    return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
+                    return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(state.initalPose2d);
                 }, // Robot pose supplier
                 this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
                                              // pose)
@@ -138,7 +143,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
     public void setOdoPose(Pose2d pose) {
         if (pose == null) return;
-        initalPose2d = pose;
+        state.initalPose2d = pose;
         swerveDriveTrain.resetPose(pose);
     }
 
@@ -157,7 +162,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     // }
 
     public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve drive is still going:
             stopModules(); // stop the swerve
 
         if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
@@ -165,7 +170,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
         
-        stopped = false;
+        state.stopped = false;
         if (fieldRelative) {
             
             leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
@@ -173,18 +178,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
             // ! drift correction
             if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) {
-                rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
+                state.rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
                 swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
-                    .withVelocityX(leftStick.getX() * speedAdjust)
+                    .withVelocityX(leftStick.getX() * state.speedAdjust)
-                    .withVelocityY(leftStick.getY() * speedAdjust)
+                    .withVelocityY(leftStick.getY() * state.speedAdjust)
-                    .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+                    .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
                     // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
                 SmartDashboard.putBoolean("drift correction", false);
             } else {
                 var ctrl = new SwerveRequest.FieldCentricFacingAngle()
-                    .withVelocityX(leftStick.getX() * speedAdjust)
+                    .withVelocityX(leftStick.getX() * state.speedAdjust)
-                    .withVelocityY(leftStick.getY() * speedAdjust)
+                    .withVelocityY(leftStick.getY() * state.speedAdjust)
-                    .withTargetDirection(Rotation2d.fromDegrees(rotTarget));
+                    .withTargetDirection(Rotation2d.fromDegrees(state.rotTarget));
                 ctrl.HeadingController.setPID(
                     DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
                     DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
@@ -197,20 +202,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
            
         } else { // Create robot-relative speeds.
             swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
-                    .withVelocityX(leftStick.getX() * speedAdjust)
+                    .withVelocityX(leftStick.getX() * state.speedAdjust)
-                    .withVelocityY(-leftStick.getY() * speedAdjust)
+                    .withVelocityY(-leftStick.getY() * state.speedAdjust)
-                    .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+                    .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
         }
     }
 
     public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
-        stopped = false;
+        state.stopped = false;
         // Create robot-relative speeds.
         if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
         swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
             .withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
             .withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
-            .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+            .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
         
     }
 
@@ -220,7 +225,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
                                                                                                // relitive version of
                                                                                                // this, and no pre
                                                                                                // provided version
-        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve
                                                                                            // drive is still going:
             stopModules(); // stop the swerve
 
@@ -230,8 +235,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
-                .withVelocityX(leftStick.getX() * speedAdjust)
+                .withVelocityX(leftStick.getX() * state.speedAdjust)
-                .withVelocityY(leftStick.getY() * speedAdjust)
+                .withVelocityY(leftStick.getY() * state.speedAdjust)
                 .withTargetDirection(rightStick.getAngle()));
     }
 
@@ -239,8 +244,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
         leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
         var ctrl = new SwerveRequest.FieldCentricFacingAngle()
-            .withVelocityX(leftStick.getX() * speedAdjust)
+            .withVelocityX(leftStick.getX() * state.speedAdjust)
-            .withVelocityY(leftStick.getY() * speedAdjust)
+            .withVelocityY(leftStick.getY() * state.speedAdjust)
             .withTargetDirection(heading);
         ctrl.HeadingController.setPID(
             DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
@@ -254,8 +259,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         leftStick = leftStick.rotateBy(heading);
 
         var ctrl = new SwerveRequest.FieldCentricFacingAngle()
-            .withVelocityX(leftStick.getX() * speedAdjust)
+            .withVelocityX(leftStick.getX() * state.speedAdjust)
-            .withVelocityY(leftStick.getY() * speedAdjust)
+            .withVelocityY(leftStick.getY() * state.speedAdjust)
             .withTargetDirection(heading);
         // ctrl.HeadingController.setPID(
         //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
@@ -298,6 +303,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         return false;
     }
 
+    public boolean isStopped() {
+        return state.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:
@@ -309,8 +318,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
-                .withVelocityX(leftStick.getX() * -speedAdjust)
+                .withVelocityX(leftStick.getX() * -state.speedAdjust)
-                .withVelocityY(leftStick.getY() * speedAdjust)
+                .withVelocityY(leftStick.getY() * state.speedAdjust)
                 .withTargetDirection(rot));
         // double
     }
@@ -320,19 +329,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     public Pose2d getPose2d() {
-        return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d);
+        return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(state.initalPose2d);
     }
 
     public void resetGyro() {
         swerveDriveTrain.tareEverything();
-        robotKnowsWhereItIs = false;
+        state.robotKnowsWhereItIs = false;
-        rotTarget = 0;
+        state.rotTarget = 0;
         // vision.resetRotations();
     }
 
 
     public void softStop() {
-        stopped = true;
+        state.stopped = true;
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
             .withVelocityX(0)
             .withVelocityY(0)
@@ -350,7 +359,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     public void periodic() {
         // This method will be called once per scheduler run\
         SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
-        SmartDashboard.putNumber("RotTartget", rotTarget);
+        SmartDashboard.putNumber("RotTartget", state.rotTarget);
 
         double time = Vision.getTime();
         double freq =  swerveDriveTrain.getOdometryFrequency();
@@ -367,82 +376,80 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     private void reset_index() {
-        gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
+        state.gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
                                                          // robot start in?)
     }
 
     public void shiftDown() {
-        if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
+        if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length)
             reset_index(); // If outof bounds: reset index
-        int i = gear_index - 1;
+        int i = state.gear_index - 1;
         if (i == -1)
             i = 0;
         setPercentOutput(DriveConstants.GEARS[i]);
-        gear_index = i;
+        state.gear_index = i;
     }
 
     public void shiftUp() {
-        if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
+        if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length)
             reset_index(); // If outof bounds: reset index
-        int i = gear_index + 1;
+        int i = state.gear_index + 1;
         if (i == DriveConstants.GEARS.length)
             i = DriveConstants.GEARS.length - 1;
         setPercentOutput(DriveConstants.GEARS[i]);
-        gear_index = i;
+        state.gear_index = i;
     }
 
     public void setPercentOutput(double speed) {
-        speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
+        state.speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
-        gear_index = -1;
+        state.gear_index = -1;
     }
 
     public void setToSlow() {
         setPercentOutput(DriveConstants.SLOW_SPEED);
-        gear_index = 0;
+        state.gear_index = 0;
     }
 
     public void setToFast() {
         setPercentOutput(DriveConstants.FAST_SPEED);
-        gear_index = 1;
+        state.gear_index = 1;
     }
 
     public void setToTurbo() {
         setPercentOutput(DriveConstants.TURBO_SPEED);
-        gear_index = 2;
+        state.gear_index = 2;
     }
 
     public void shiftUpRot() {
-        rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
+        state.rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
     }
 
     public void shiftDownRot() {
-        rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
+        state.rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
     }
 
     private int tmp_gear_index = DriveConstants.STARTING_GEAR;
 
     public void startSlowPeriod() {
-        tmp_gear_index = gear_index;
+        tmp_gear_index = state.gear_index;
         setToSlow();
     }
 
     public void startTurboPeriod() {
-        tmp_gear_index = gear_index;
+        tmp_gear_index = state.gear_index;
         setToTurbo();
     }
 
     public void endSlowPeriod() {
         setPercentOutput(DriveConstants.GEARS[tmp_gear_index]);
-        gear_index = tmp_gear_index;
+        state.gear_index = tmp_gear_index;
     }
 
 
 
     public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
         if(curPose.isPresent() && lastPose.isPresent()){
-            this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
+            state.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
-        
-            SmartDashboard.putNumber("Speed", lastOdomSpeed);
         }
     }
     
@@ -453,28 +460,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         return "Swerve Drive Controller";
     }
 
-    ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
-            .getLayout(getName(), 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();

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

@@ -10,6 +10,7 @@ import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
 
+import org.littletonrobotics.junction.AutoLog;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
@@ -35,44 +36,22 @@ import frc4388.utility.status.FaultReporter;
 import frc4388.utility.status.Queryable;
 
 public class Vision extends SubsystemBase implements Queryable {
-
-    // private PhotonCamera leftCamera;
-    // private PhotonCamera rightCamera;
-
     private PhotonCamera[] cameras;
     private PhotonPoseEstimator[] estimators;
-    private List<EstimatedRobotPose> poses = new ArrayList<>();
 
-    private boolean isTagDetected = false;
+    @AutoLog
-    private boolean isTagProcessed = false;
+    public class VisionState {
-
+        public boolean isTagDetected = false;
-    private double lastLatency = 0;
+        public boolean isTagProcessed = false;
-
+        public List<EstimatedRobotPose> poses = new ArrayList<>();
-    public double getLastLatency() {
+        public double latency = 0;
-        return lastLatency;
+        public Pose2d lastVisionPose = new Pose2d();
+        public Pose2d lastPhysOdomPose = new Pose2d();
     }
 
-    public Pose2d lastVisionPose = new Pose2d();
+    private VisionState state = new VisionState();
-    private Pose2d lastPhysOdomPose = new Pose2d();
-
-    private Matrix<N3, N1> curStdDevs;
 
     private Field2d field = new Field2d();
-    
-    ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
-    .getLayout(getName(), 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();
-    
     public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){
         FaultReporter.register(this);
         SmartDashboard.putData(field);
@@ -98,17 +77,17 @@ public class Vision extends SubsystemBase implements Queryable {
 
 
     private void update() {
-        isTagProcessed = false;
+        state.isTagProcessed = false;
-        isTagDetected = false;
+        state.isTagDetected = false;
 
         // Instant now = Instant.now();
 
         // int cams = 0;
 
-        // double latency = 0;
+        double latency = 0;
 
         // Pose2d pose = null;
-        poses.clear();
+        state.poses.clear();
 
         for(int i = 0; i < cameras.length; i++){
             PhotonCamera camera = cameras[i];
@@ -122,9 +101,9 @@ public class Vision extends SubsystemBase implements Queryable {
 
             
             var result = results.get(results.size()-1);
-            // latency += result.getTimestampSeconds();
+            latency += result.getTimestampSeconds();
 
-            isTagDetected = isTagDetected | result.hasTargets();
+            state.isTagDetected = state.isTagDetected | result.hasTargets();
 
             // If there are no tags
             if(!result.hasTargets())
@@ -136,7 +115,7 @@ public class Vision extends SubsystemBase implements Queryable {
             if(estimatedRobotPose.isEmpty())
                 continue;
             
-            poses.add(estimatedRobotPose.get());
+            state.poses.add(estimatedRobotPose.get());
 
             // if(pose == null)
             //     pose = estimatedRobotPose.get().estimatedPose.toPose2d();
@@ -150,7 +129,7 @@ public class Vision extends SubsystemBase implements Queryable {
             // Yaw += (pose.getRotation().getDegrees() + 180) % 360;
             // cams++;
 
-            isTagProcessed = true;
+            state.isTagProcessed = true;
         
             
         }
@@ -244,22 +223,13 @@ public class Vision extends SubsystemBase implements Queryable {
     //     }
     // }
 
-    /**
-     * 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();
+            state.lastPhysOdomPose = pose.get();
     }
 
     // public double getLastOdomSpeed(){
@@ -267,8 +237,8 @@ public class Vision extends SubsystemBase implements Queryable {
     // }
 
     public Pose2d getPose2d() {
-        if(lastPhysOdomPose != null)
+        if(state.lastPhysOdomPose != null)
-            return lastPhysOdomPose;
+            return state.lastPhysOdomPose;
 
         // if(lastVisionPose != null)
         //     return lastVisionPose;
@@ -281,12 +251,12 @@ public class Vision extends SubsystemBase implements Queryable {
     }
 
     public boolean isTag(){
-        return isTagDetected && isTagProcessed;
+        return state.isTagDetected && state.isTagProcessed;
     }
 
 
     public void addVisionMeasurement( SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
-        for(EstimatedRobotPose pose : poses){
+        for(EstimatedRobotPose pose : state.poses){
             drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds));
         }
     }
@@ -303,19 +273,6 @@ public class Vision extends SubsystemBase implements Queryable {
         return "Vision";
     }
 
-//   GenericEntry sbShiftState = subsystemLayout
-//   .add("Shift State", 0)
-//   .withWidget(BuiltInWidgets.kNumberBar)
-//   .getEntry();
-
-
-    // @Override
-    // public void queryStatus() {
-    //     sbTagDetected.setBoolean(isTagDetected);
-    //     sbTagProcessed.setBoolean(isTagProcessed);
-    //     // field.setRobotPose(getPose2d());
-    // }
-
     @Override
     public Status diagnosticStatus() {
         return new Status();

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

@@ -0,0 +1,14 @@
+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 {
+  private static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError);
+
+  public static void UpdateAlerts(RobotContainer m_RobotContainer) {
+    no_auto.set(!m_RobotContainer.autoChooserUpdated);
+  }
+}