rebels2638 · TheOneAKD · Mar 31, 2026
diff --git a/src/main/deploy/configs/hopper/comp.json b/src/main/deploy/configs/hopper/comp.json
@@ -12,8 +12,11 @@
   "hopperKI": 0.0,
   "hopperKD": 0.0,
   "hopperMotorToOutputShaftRatio": 6.875,
-  "feedingVelocityRPS": 6.3 ,
-  "feedingIdleVoltage": 0.0,
-  "reverseVelocityRPS": 0.0,
-  "hopperVelocityToleranceRPS": 5.0
+  "feedingVelocityRPS": 7.0,
+  "feedingIdleVoltage": 4.0,
+  "reverseVelocityRPS": -7.0,
+  "hopperVelocityToleranceRPS": 5.0,
+  "beltSlipVoltageThreshold": 3.0,
+  "beltSlipVelocityThresholdRPS": 1.0,
+  "beltSlipDetectDurationSec": 0.3
 }
diff --git a/src/main/deploy/configs/intake/comp.json b/src/main/deploy/configs/intake/comp.json
@@ -37,14 +37,10 @@
   "pivotStartingAngleRotations": 0.25,
   "pivotMinAngleRotations": -0.025,
   "pivotMaxAngleRotations": 0.25,
-  "pivotRollerEnableAngleRotations": 0.05,
-  "pivotAngleToleranceRotations": 0.04,
-  "pivotUpAngleRotations": 0.235,
-  "pivotDownAngleRotations": -0.019287,
-  "pivotAlternatingFirstAngleRotations": 0.085,
-  "pivotAlternatingSecondAngleRotations": 0.012,
-  "alternatingTimeoutSeconds": 1.4,
-  "pivotAlternatingMaxVelocityRotationsPerSec": 1.5,
-  "pivotAlternatingMaxAccelerationRotationsPerSec2": 4.2,
-  "pivotAlternatingMaxJerkRotationsPerSec3": 60.0
+  "pivotAngleToleranceRotations": 0.01,
+  "pivotUpAngleRotations": 0.0,
+  "pivotDownAngleRotations": 0.25,
+  "beltSlipVoltageThreshold": 4.0,
+  "beltSlipVelocityThresholdRPS": 2.0,
+  "beltSlipDetectDurationSec": 0.3
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -33,6 +33,7 @@
 import frc.robot.lib.util.ballistics.ProjectileVisualizer;
 import frc.robot.subsystems.Superstructure;
 import frc.robot.subsystems.Superstructure.TargetState;
+import frc.robot.subsystems.SystemsMonitor;
 import frc.robot.subsystems.hopper.Hopper;
 import frc.robot.subsystems.hopper.Hopper.HopperSetpoint;
 import frc.robot.subsystems.intake.Intake;
@@ -65,6 +66,9 @@ public static RobotContainer getInstance() {
     private final Hopper hopper = Hopper.getInstance();
     private final Intake intake = Intake.getInstance();
     private final Superstructure superstructure = Superstructure.getInstance();
+
+    private final SystemsMonitor systemsMonitor = SystemsMonitor.getInstance();
+
     private final SendableChooser<Command> autoChooser;
 
     LoggedNetworkNumber shooterFlywheelSet = new LoggedNetworkNumber("flywheelSet", 0);

diff --git a/src/main/java/frc/robot/configs/HopperConfig.java b/src/main/java/frc/robot/configs/HopperConfig.java
@@ -24,4 +24,8 @@ public class HopperConfig {
     public double reverseVelocityRPS;
 
     public double hopperVelocityToleranceRPS;
+
+    public double beltSlipVoltageThreshold;
+    public double beltSlipVelocityThresholdRPS;
+    public double beltSlipDetectDurationSec;
 }
diff --git a/src/main/java/frc/robot/configs/IntakeConfig.java b/src/main/java/frc/robot/configs/IntakeConfig.java
@@ -62,4 +62,8 @@ public class IntakeConfig {
     public double pivotAlternatingMaxVelocityRotationsPerSec;
     public double pivotAlternatingMaxAccelerationRotationsPerSec2;
     public double pivotAlternatingMaxJerkRotationsPerSec3;
+
+    public double beltSlipVoltageThreshold;
+    public double beltSlipVelocityThresholdRPS;
+    public double beltSlipDetectDurationSec;
 }
diff --git a/src/main/java/frc/robot/subsystems/SystemsMonitor.java b/src/main/java/frc/robot/subsystems/SystemsMonitor.java
@@ -0,0 +1,123 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.hopper.Hopper;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.swerve.SwerveDrive;
+
+public class SystemsMonitor extends SubsystemBase {
+    private static SystemsMonitor instance = null;
+
+    public static SystemsMonitor getInstance() {
+        if (instance == null) {
+            instance = new SystemsMonitor();
+        }
+        return instance;
+    }
+
+    private final SwerveDrive swerve = SwerveDrive.getInstance();
+    private final Shooter shooter = Shooter.getInstance();
+    private final Intake intake = Intake.getInstance();
+    private final Hopper hopper = Hopper.getInstance();
+
+    private boolean intakeWasSlipping = false;
+    private boolean hopperWasSlipping = false;
+
+    // Add with fields at top
+    private static final String AUTO_SHUTOFF_KEY = "Systems/AutoShutoffEnabled";
+
+    private SystemsMonitor() {
+        // Set default — starts as alert-only
+        SmartDashboard.putBoolean(AUTO_SHUTOFF_KEY, false);
+    }
+
+    @Override
+    public void periodic() {
+        // ── DRIVETRAIN ──
+        putStatus("Systems/Drivetrain/Gyro",            swerve.isGyroConnected());
+        putStatus("Systems/Drivetrain/FL_Drive",        swerve.isFrontLeftDriveMotorConnected());
+        putStatus("Systems/Drivetrain/FL_Steer",        swerve.isFrontLeftSteerMotorConnected());
+        putStatus("Systems/Drivetrain/FL_Encoder",      swerve.isFrontLeftSteerEncoderConnected());
+        putStatus("Systems/Drivetrain/FR_Drive",        swerve.isFrontRightDriveMotorConnected());
+        putStatus("Systems/Drivetrain/FR_Steer",        swerve.isFrontRightSteerMotorConnected());
+        putStatus("Systems/Drivetrain/FR_Encoder",      swerve.isFrontRightSteerEncoderConnected());
+        putStatus("Systems/Drivetrain/BL_Drive",        swerve.isBackLeftDriveMotorConnected());
+        putStatus("Systems/Drivetrain/BL_Steer",        swerve.isBackLeftSteerMotorConnected());
+        putStatus("Systems/Drivetrain/BL_Encoder",      swerve.isBackLeftSteerEncoderConnected());
+        putStatus("Systems/Drivetrain/BR_Drive",        swerve.isBackRightDriveMotorConnected());
+        putStatus("Systems/Drivetrain/BR_Steer",        swerve.isBackRightSteerMotorConnected());
+        putStatus("Systems/Drivetrain/BR_Encoder",      swerve.isBackRightSteerEncoderConnected());
+
+        // ── SHOOTER ──
+        putStatus("Systems/Shooter/Hood_Motor",         shooter.isHoodMotorConnected());
+        putStatus("Systems/Shooter/Turret_Motor",       shooter.isTurretMotorConnected());
+        putStatus("Systems/Shooter/Flywheel_Motor",     shooter.isFlywheelMotorConnected());
+        putStatus("Systems/Shooter/Flywheel_Follower",  shooter.isFlywheelFollowerMotorConnected());
+
+        // ── INTAKE ──
+        putStatus("Systems/Intake/Roller_Motor",        intake.isRollerMotorConnected());
+        putStatus("Systems/Intake/Pivot_Motor",         intake.isPivotMotorConnected());
+        putStatus("Systems/Intake/Belt_OK",             !intake.isBeltSlipping());
+
+        // ── HOPPER ──
+        putStatus("Systems/Hopper/Motor",               hopper.isHopperMotorConnected());
+        putStatus("Systems/Hopper/Belt_OK",             !hopper.isBeltSlipping());
+
+        // ── OVERALL ──
+        boolean allOk = swerve.isGyroConnected()
+            && swerve.isFrontLeftDriveMotorConnected()
+            && swerve.isFrontRightDriveMotorConnected()
+            && swerve.isBackLeftDriveMotorConnected()
+            && swerve.isBackRightDriveMotorConnected()
+            && shooter.isHoodMotorConnected()
+            && shooter.isTurretMotorConnected()
+            && shooter.isFlywheelMotorConnected()
+            && intake.isRollerMotorConnected()
+            && intake.isPivotMotorConnected()
+            && hopper.isHopperMotorConnected()
+            && !intake.isBeltSlipping()
+            && !hopper.isBeltSlipping();
+
+        SmartDashboard.putBoolean("Systems/ROBOT_OK", allOk);
+        SmartDashboard.putString("Systems/STATUS", allOk ? "ALL SYSTEMS GO" : "CHECK ALERTS");
+
+        boolean autoShutoff = SmartDashboard.getBoolean(AUTO_SHUTOFF_KEY, false);
+        boolean intakeSlipping = intake.isBeltSlipping();
+        boolean hopperSlipping = hopper.isBeltSlipping();
+
+        if (autoShutoff) {
+            // Only call on the transition, not every loop
+            if (intakeSlipping && !intakeWasSlipping) {
+                intake.enableRollerEStop();
+                intake.enablePivotEStop();
+            } else if (!intakeSlipping && intakeWasSlipping) {
+                intake.disableRollerEStop();
+                intake.disablePivotEStop();
+            }
+
+            if (hopperSlipping && !hopperWasSlipping) {
+                hopper.enableEStop();
+            } else if (!hopperSlipping && hopperWasSlipping) {
+                hopper.disableEStop();
+            }
+        } else {
+            // AutoShutoff just got toggled off — make sure nothing is stuck disabled
+            if (intakeWasSlipping) {
+                intake.disableRollerEStop();
+                intake.disablePivotEStop();
+            }
+            if (hopperWasSlipping) {
+                hopper.disableEStop();
+            }
+        }
+
+        intakeWasSlipping = intakeSlipping;
+        hopperWasSlipping = hopperSlipping;
+    }
+
+    private void putStatus(String key, boolean ok) {
+        SmartDashboard.putBoolean(key, ok);
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java
@@ -13,7 +13,10 @@
 import frc.robot.lib.util.DashboardMotorControlLoopConfigurator;
 
 public class Hopper extends SubsystemBase {
-    private static final double HOPPER_VOLTAGE_SETPOINT_TOLERANCE_VOLTS = 0.25;
+    private double hopperSlipTimer = 0.0;
+    private final Alert hopperBeltSlipAlert = new Alert(
+        "!! HOPPER/DIROTOR BELT SLIP - CHECK MECHANISM !!", AlertType.kError
+    );
 
     private enum HopperControlMode {
         VELOCITY,
@@ -77,6 +80,7 @@ private Hopper() {
     public void periodic() {
         hopperIO.updateInputs(hopperInputs);
         Logger.processInputs("Hopper", hopperInputs);
+        detectBeltSlip();
 
         hopperDisconnectedAlert.set(enableConnectionAlerts && !hopperInputs.hopperMotorConnected);
 
@@ -136,6 +140,28 @@ public void disableEStop() {
         hopperIO.disableEStop();
     }
 
+    private void detectBeltSlip() {
+        boolean motorDemandingTorque = hopperInputs.appliedVolts > config.beltSlipVoltageThreshold
+            && hopperSetpointRPS > 1.0;
+        boolean velocityNearZero = Math.abs(hopperInputs.velocityRotationsPerSec)
+            < config.beltSlipVelocityThresholdRPS;
+
+        if (motorDemandingTorque && velocityNearZero) {
+            hopperSlipTimer += 0.02;
+        } else {
+            hopperSlipTimer = 0.0;
+        }
+
+        boolean slipDetected = hopperSlipTimer >= config.beltSlipDetectDurationSec;
+        hopperBeltSlipAlert.set(slipDetected);
+        Logger.recordOutput("Hopper/beltSlipDetected", slipDetected);
+        Logger.recordOutput("Hopper/beltSlipTimer", hopperSlipTimer);
+    }
+
+    public boolean isBeltSlipping() {
+        return hopperSlipTimer >= config.beltSlipDetectDurationSec;
+    }
+
     @AutoLogOutput(key = "Hopper/isHopperAtSetpoint")
     public boolean isHopperAtSetpoint() {
         if (!hopperInputs.hopperMotorConnected) {
@@ -144,7 +170,7 @@ public boolean isHopperAtSetpoint() {
 
         return hopperControlMode == HopperControlMode.VELOCITY
             ? Math.abs(hopperInputs.velocityRotationsPerSec - hopperSetpointRPS) < config.hopperVelocityToleranceRPS
-            : Math.abs(hopperInputs.appliedVolts - hopperSetpointVolts) < HOPPER_VOLTAGE_SETPOINT_TOLERANCE_VOLTS;
+            : Math.abs(hopperInputs.appliedVolts - hopperSetpointVolts) < hopperSetpointVolts;
     }
 
     @AutoLogOutput(key = "Hopper/isHopperMotorConnected")

diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -15,6 +15,11 @@
 public class Intake extends SubsystemBase {
     private static Intake instance = null;
 
+    private double beltSlipTimer = 0.0;
+    private final Alert beltSlipAlert = new Alert(
+        "!! INTAKE BELT SLIP - CHECK MECHANISM !!", AlertType.kError
+    );
+
     public static Intake getInstance() {
         if (instance == null) {
             instance = new Intake();
@@ -87,6 +92,7 @@ private Intake() {
     public void periodic() {
         intakeIO.updateInputs(intakeInputs);
         Logger.processInputs("Intake", intakeInputs);
+        detectBeltSlip();
 
         rollerDisconnectedAlert.set(enableConnectionAlerts && !intakeInputs.rollerMotorConnected);
         pivotDisconnectedAlert.set(enableConnectionAlerts && !intakeInputs.pivotMotorConnected);
@@ -169,6 +175,28 @@ public void setSetpoint(IntakeSetpoint setpoint) {
         }
     }
 
+    private void detectBeltSlip() {
+        boolean motorDemandingTorque = intakeInputs.rollerAppliedVolts > config.beltSlipVoltageThreshold
+            && rollerSetpointRPS > 1.0;
+        boolean velocityNearZero = Math.abs(intakeInputs.rollerVelocityRotationsPerSec)
+            < config.beltSlipVelocityThresholdRPS;
+
+        if (motorDemandingTorque && velocityNearZero) {
+            beltSlipTimer += 0.02;
+        } else {
+            beltSlipTimer = 0.0;
+        }
+
+        boolean slipDetected = beltSlipTimer >= config.beltSlipDetectDurationSec;
+        beltSlipAlert.set(slipDetected);
+        Logger.recordOutput("Intake/beltSlipDetected", slipDetected);
+        Logger.recordOutput("Intake/beltSlipTimer", beltSlipTimer);
+    }
+
+    public boolean isBeltSlipping() {
+        return beltSlipTimer >= config.beltSlipDetectDurationSec;
+    }
+
     public double getRollerVelocityRotationsPerSec() {
         return intakeInputs.rollerVelocityRotationsPerSec;
     }

diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -28,8 +28,8 @@
 public class Vision extends SubsystemBase {
     private static final double DETAILED_POSE_LOG_PERIOD_SECONDS = 0.1;
     private static final double IMU_MODE_REASSERT_PERIOD_SECONDS = 1.0;
-    private static final boolean ENABLE_DETAILED_POSE_LOGGING = true;
-        // Constants.currentMode == Constants.Mode.REPLAY || Constants.VERBOSE_LOGGING_ENABLED;
+    private static final boolean ENABLE_DETAILED_POSE_LOGGING = 
+        Constants.currentMode == Constants.Mode.REPLAY || Constants.VERBOSE_LOGGING_ENABLED; // Detailed pose logging can consume a lot of bandwidth, so only enable it in replay or verbose logging modes.
 
     private static Vision instance = null;
     private static final VisionConfig config = ConfigLoader.load(
@@ -158,11 +158,10 @@ public void periodic() {
 
         boolean shouldFlushRobotOrientation = false;
         for (VisionIO ioImplementation : io) {
-            shouldFlushRobotOrientation |= ioImplementation.publishRobotOrientation();
-        }
-        if (shouldFlushRobotOrientation) {
-            LimelightHelpers.Flush();
+            ioImplementation.publishRobotOrientation();
         }
+        // NT4 in 2025+ flushes automatically at the loop rate.
+        // Explicit Flush() blocks the main thread waiting for network confirmation — removed.
 
         boolean sawReconnectThisCycle = false;
         for (int i = 0; i < io.length; i++) {