diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8463dcf2..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)); @@ -591,4 +592,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..7db1ff74 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 = 1500; + //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..f0f83030 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 + && 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 9e89cd0e..1b61602d 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.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 11d1c520..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 @@ -20,6 +20,11 @@ public MockSparkMaxIo(String name, MotorLoggableInputs inputs) { public void set(double speed) { } + @Override + public double getEncoderVelocity() { + 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..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 @@ -46,7 +46,12 @@ public boolean isFwdSwitchPressed() { public boolean isRevSwitchPressed() { return getInputs().getRevLimit(); } - + + @Override + public double getEncoderVelocity() { + return getInputs().getEncoderVelocity(); + } + @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..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,6 +8,8 @@ public interface SparkMaxIo extends BaseIo { void set(double speed); + double getEncoderVelocity(); + void setVoltage(double voltage); void stopMotor();