From c0cbf17912539102cb6fc442d899e62b42d5d2f6 Mon Sep 17 00:00:00 2001 From: "a.the.one" Date: Tue, 31 Mar 2026 16:01:05 -0400 Subject: [PATCH] feat(hopper, intake): add belt slip detection parameters and implement detection logic fix loop overrun error --- src/main/deploy/configs/hopper/comp.json | 11 +- src/main/deploy/configs/intake/comp.json | 16 +-- src/main/java/frc/robot/RobotContainer.java | 4 + .../java/frc/robot/configs/HopperConfig.java | 4 + .../java/frc/robot/configs/IntakeConfig.java | 4 + .../frc/robot/subsystems/SystemsMonitor.java | 123 ++++++++++++++++++ .../frc/robot/subsystems/hopper/Hopper.java | 30 ++++- .../frc/robot/subsystems/intake/Intake.java | 28 ++++ .../frc/robot/subsystems/vision/Vision.java | 11 +- 9 files changed, 209 insertions(+), 22 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/SystemsMonitor.java diff --git a/src/main/deploy/configs/hopper/comp.json b/src/main/deploy/configs/hopper/comp.json index 177b61c..f96a7a7 100644 --- 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 index dfae08e..8ccefc8 100644 --- 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 index e01b3e3..33ec2a6 100644 --- 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 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 index e357503..b8345cd 100644 --- 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 index fcffb4c..3fa522c 100644 --- 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 new file mode 100644 index 0000000..9de9fc3 --- /dev/null +++ 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); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 8850390..e8e21b4 100644 --- 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 index e90e64d..4ac0a52 100644 --- 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 index 704af76..36061ab 100644 --- 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++) {