From 7d284829c29626befef8b6bb51b123e4a67ebb34 Mon Sep 17 00:00:00 2001 From: Raayed Mohammed Date: Sat, 14 Mar 2026 14:17:37 -0400 Subject: [PATCH 1/3] Added checks so the hopper only spins if the shooter is spinning fast enough. --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ src/main/java/frc/robot/constants/GameConstants.java | 3 +++ src/main/java/frc/robot/subsystems/ControllerSubsystem.java | 4 +++- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 4 ++++ .../frc/robot/utils/logging/io/motor/MockSparkMaxIo.java | 5 +++++ .../frc/robot/utils/logging/io/motor/RealSparkMaxIo.java | 6 +++++- .../java/frc/robot/utils/logging/io/motor/SparkMaxIo.java | 2 ++ 7 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8463dcf2..c52bc9fa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -591,4 +591,8 @@ public ShootingState getShootingState() { public IntakeDeployerSubsystem getDeployer(){ return intakeDeployer; } + + public ShooterSubsystem getShooterSubsystem(){ + return shooterSubsystem; + } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 13636392..a507c7ba 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -44,6 +44,9 @@ public enum Mode { public static final int STEER_JOYSTICK_PORT = 1; public static final int XBOX_CONTROLLER_PORT = 2; + // Thresholds + public static final double SHOOTER_THRESHOLD = -2000; + //Speeds public static final double ROLLER_SPEED = 0.25; public static final double TILT_SPEED = -0.5; // Arm motor is inverted - use negative speed diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 8574fe51..46e8b01c 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -243,7 +243,9 @@ public boolean shouldFeederSpin() { } public boolean shouldHopperSpin() { - return activeTargets.hopperSpin; + return activeTargets.hopperSpin + && robotContainer.getShooterSubsystem().getVelocity() < Constants.SHOOTER_THRESHOLD + && activeTargets.shooterVelocityRpm < Constants.SHOOTER_THRESHOLD; } public void setDriverActivatedShooting(boolean set) { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 9e89cd0e..79dbc770 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -108,4 +108,8 @@ public static MotorPair createMotor() { } public record MotorPair(SparkMaxPidMotor mainMotor, SparkMax followerMotor){} public record MotorPairIO(RealSparkMaxIo mainMotor, RealSparkMaxIo followerMotor){} + + public double getVelocity() { + return io.get(); + } } diff --git a/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java index 11d1c520..4f2a8540 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java @@ -20,6 +20,11 @@ public MockSparkMaxIo(String name, MotorLoggableInputs inputs) { public void set(double speed) { } + @Override + public double get() { + return 0; // example + } + @Override public void setVoltage(double voltage) { } diff --git a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java index 33fe540e..1750bd65 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java @@ -46,7 +46,11 @@ public boolean isFwdSwitchPressed() { public boolean isRevSwitchPressed() { return getInputs().getRevLimit(); } - + + public double get() { + return motor.get(); + } + @Override protected void updateInputs(MotorLoggableInputs inputs) { inputs.fromHardware(motor); diff --git a/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java index 85776fc5..452c2140 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java @@ -8,6 +8,8 @@ public interface SparkMaxIo extends BaseIo { void set(double speed); + double get(); + void setVoltage(double voltage); void stopMotor(); From 9cd0e886d0d7b67fb11b3d28d125b3a633c1b3f7 Mon Sep 17 00:00:00 2001 From: Raayed Mohammed Date: Sat, 14 Mar 2026 15:47:06 -0400 Subject: [PATCH 2/3] Added ShootButton to SmartDashboard. --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c52bc9fa..d92f9755 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -408,7 +408,8 @@ public void putShuffleboardCommands() { new RunHopperAndFeeder(hopperSubsystem, feederSubsystem)); SmartDashboard.putData("RunTurret", new TestSetTurretAngle(turretSubsystem)); - + SmartDashboard.putData("Run \"ShootButton\"", + new ShootButton(controllerSubsystem)); SmartDashboard.putData( "test/Run Dashboard Shot Test (30s)", new RunDashboardShotTest(anglerSubsystem, turretSubsystem, shooterSubsystem)); From 3b24540db3122f0260e9912e505601bdf026ec6c Mon Sep 17 00:00:00 2001 From: Samhith Dewal Date: Mon, 16 Mar 2026 20:02:35 -0400 Subject: [PATCH 3/3] Resolved comments --- src/main/java/frc/robot/constants/GameConstants.java | 2 +- src/main/java/frc/robot/subsystems/ControllerSubsystem.java | 4 ++-- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 2 +- .../frc/robot/utils/logging/io/motor/MockSparkMaxIo.java | 2 +- .../frc/robot/utils/logging/io/motor/RealSparkMaxIo.java | 5 +++-- .../java/frc/robot/utils/logging/io/motor/SparkMaxIo.java | 2 +- 6 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index a507c7ba..7db1ff74 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -45,7 +45,7 @@ public enum Mode { public static final int XBOX_CONTROLLER_PORT = 2; // Thresholds - public static final double SHOOTER_THRESHOLD = -2000; + public static final double SHOOTER_THRESHOLD = 1500; //Speeds public static final double ROLLER_SPEED = 0.25; diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 46e8b01c..f0f83030 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -244,8 +244,8 @@ public boolean shouldFeederSpin() { public boolean shouldHopperSpin() { return activeTargets.hopperSpin - && robotContainer.getShooterSubsystem().getVelocity() < Constants.SHOOTER_THRESHOLD - && activeTargets.shooterVelocityRpm < Constants.SHOOTER_THRESHOLD; + && Math.abs(robotContainer.getShooterSubsystem().getVelocity()) > Math.abs(Constants.SHOOTER_THRESHOLD) + && Math.abs(activeTargets.shooterVelocityRpm) > Math.abs(Constants.SHOOTER_THRESHOLD); } public void setDriverActivatedShooting(boolean set) { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 79dbc770..1b61602d 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -110,6 +110,6 @@ public record MotorPair(SparkMaxPidMotor mainMotor, SparkMax followerMotor){} public record MotorPairIO(RealSparkMaxIo mainMotor, RealSparkMaxIo followerMotor){} public double getVelocity() { - return io.get(); + return io.getEncoderVelocity(); } } diff --git a/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java index 4f2a8540..bc567748 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/MockSparkMaxIo.java @@ -21,7 +21,7 @@ public void set(double speed) { } @Override - public double get() { + public double getEncoderVelocity() { return 0; // example } diff --git a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java index 1750bd65..1012cb5c 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java @@ -47,8 +47,9 @@ public boolean isRevSwitchPressed() { return getInputs().getRevLimit(); } - public double get() { - return motor.get(); + @Override + public double getEncoderVelocity() { + return getInputs().getEncoderVelocity(); } @Override diff --git a/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java index 452c2140..4f52c4c1 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/SparkMaxIo.java @@ -8,7 +8,7 @@ public interface SparkMaxIo extends BaseIo { void set(double speed); - double get(); + double getEncoderVelocity(); void setVoltage(double voltage);