diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 23130163..43e73e99 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -342,6 +342,8 @@ public static class ShooterConstants { public static final double RADIUS_UPPER_HUB = 0.61; // in m public static final double SHOOTER_HEIGHT = 0.51; // in m public static final double UPPER_HUB_AIMING_HEIGHT = 2.725427; // in m + public static final double SHOOTER_SLOW_PERCENT = 0.10; + // Tuning Section public static final double DELTA_AIM_HEIGHT_FACTOR = 0.0; // TODO: Set delta aim height factor from tuning diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e1092d4b..4926ac68 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -28,6 +28,7 @@ import frc.robot.commands.shooter.*; import frc.robot.commands.transfer.TransferIndexForward; import frc.robot.commands.transfer.TransferManualReverse; +import frc.robot.commands.transfer.TransferOff; import frc.robot.hardware.Limelight; import frc.robot.helper.ControllerUtil; import frc.robot.helper.DPadButton; @@ -127,7 +128,7 @@ private void initializeDrivetrain() { private void initializeShooter() { this.shooterSubsystem = new ShooterSubsystem(); - TransferSubsystem.flywheelSubsystem = shooterSubsystem; + TransferSubsystem.shooterSubsystem = shooterSubsystem; } private void initializeTransfer() { @@ -218,6 +219,14 @@ private void configureShooter() { // Vibrations if (TRANSFER) { new Button(() -> transferSubsystem.getCurrentBallCount() >= MAX_BALL_COUNT).whenPressed(new WaitAndVibrateCommand(driverController, 0.1, 0.1)); + + new Trigger(()-> transferSubsystem.shouldOuttake(1)) + .whileActiveContinuous( + new ParallelCommandGroup( + new OutakeShooter(shooterSubsystem), + new TransferIndexForward(transferSubsystem) + ) + ); } // Flywheel Vibration from the SetShooterPIDVelocityFromStateCommand @@ -240,13 +249,22 @@ private void configureIntake() { driverRightBumper.whenHeld(new IntakeOn(intakeSubsystem)); // TODO: bad - if (TRANSFER) + if (TRANSFER) { operatorBButton.whenHeld( new ParallelCommandGroup( new IntakeReverse(intakeSubsystem), new TransferManualReverse(transferSubsystem) ) ); + new Trigger(()-> transferSubsystem.shouldOuttake(0)) + .whileActiveContinuous( + new ParallelCommandGroup( + new IntakeReverse(intakeSubsystem), + new TransferManualReverse(transferSubsystem) + ) + ); + } + diff --git a/src/main/java/frc/robot/commands/shooter/OutakeShooter.java b/src/main/java/frc/robot/commands/shooter/OutakeShooter.java new file mode 100644 index 00000000..7a923a67 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/OutakeShooter.java @@ -0,0 +1,30 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ShooterSubsystem; + +public class OutakeShooter extends CommandBase { + private ShooterSubsystem shooterSubsystem; + + public OutakeShooter(ShooterSubsystem flywheelSubsystem) { + this.shooterSubsystem = flywheelSubsystem; + + addRequirements(flywheelSubsystem); + } + + @Override + public void initialize() { + shooterSubsystem.slowFlywheel(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + shooterSubsystem.stopFlywheel(); + } + + @Override + public boolean isFinished() { + return shooterSubsystem.didBallOutake(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 019b777d..6f9df2af 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -31,7 +31,6 @@ public enum ShooterLocationPreset { private final TalonFX hoodAngleMotor; private final DigitalInput limitSwitch; private ShooterLocationPreset shooterLocationPreset = ShooterLocationPreset.TARMAC_VERTEX; - public ShooterSubsystem() { TalonConfiguration MASTER_CONFIG = new TalonConfiguration(); MASTER_CONFIG.NEUTRAL_MODE = NeutralMode.Coast; @@ -97,9 +96,17 @@ public void zeroHoodMotorSensor(){ public boolean isHoodLimitSwitchPressed(){ return !limitSwitch.get(); } + + public boolean didBallOutake(){ + return TransferSubsystem.isTransferEndIRBroken(); + } /** * Disables powers to flywheel motor, motors change to neutral/coast mode */ + public void slowFlywheel(){ + setPercentSpeed(SHOOTER_SLOW_PERCENT); + } + public void stopFlywheel() { masterLeftShooterMotor.neutralOutput(); } diff --git a/src/main/java/frc/robot/subsystems/TransferSubsystem.java b/src/main/java/frc/robot/subsystems/TransferSubsystem.java index 197f8168..40e4f392 100644 --- a/src/main/java/frc/robot/subsystems/TransferSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TransferSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.intake.IntakeOn; import frc.robot.hardware.MuxedColorSensor; import frc.robot.hardware.TalonConfiguration; import frc.robot.hardware.TalonFXFactory; @@ -30,8 +31,7 @@ import static frc.robot.Constants.LEDConstants.BALL_PATTERN; import static frc.robot.Constants.IDConstants; -import static frc.robot.Constants.SubsystemEnableFlags.BALL_COLOR_SENSOR; -import static frc.robot.Constants.SubsystemEnableFlags.IR_SENSORS; +import static frc.robot.Constants.SubsystemEnableFlags.*; import static frc.robot.Constants.TransferConstants; import static frc.robot.Constants.TransferConstants.*; @@ -39,9 +39,9 @@ public class TransferSubsystem extends SubsystemBase { private static final RobotLogger logger = new RobotLogger(TransferSubsystem.class.getCanonicalName()); private final TalonFX transferMotor; - private final DigitalInput transferStartIRSensor; - private final DigitalInput transferStopIRSensor; - private final DigitalInput transferEndIRSensor; + private static DigitalInput transferStartIRSensor = null; + private static DigitalInput transferStopIRSensor = null; + private static DigitalInput transferEndIRSensor = null; // Counts how many times what color is being detected // To avoid single bad reading skewing data @@ -55,9 +55,10 @@ public class TransferSubsystem extends SubsystemBase { // Linked list for FIFO queue LinkedList ballColorIndex = new LinkedList<>(); + BallColor wrongBallColor; // For Tracking Ball RPM - public static ShooterSubsystem flywheelSubsystem; + public static ShooterSubsystem shooterSubsystem; private double currentBallCount; @@ -122,7 +123,7 @@ public void off(){ /** * @return Returns whether the IR sensor at the front of the transfer's line of sight is broken. */ - public boolean isTransferStartIRBroken() { + public static boolean isTransferStartIRBroken() { return !transferStartIRSensor.get(); } @@ -130,7 +131,7 @@ public boolean isTransferStartIRBroken() { * @return Returns whether the IR sensor in the middle of the transfer's line of sight is broken, * which signifies when the ball should stop. */ - public boolean isTransferStopIRBroken() { + public static boolean isTransferStopIRBroken() { return !transferStopIRSensor.get(); } @@ -138,7 +139,7 @@ public boolean isTransferStopIRBroken() { * @return Returns whether the IR sensor at the end of the transfer's line of sight is broken, * which signifies when the ball leaves the transfer via the shooter */ - public boolean isTransferEndIRBroken() { + public static boolean isTransferEndIRBroken() { return !transferEndIRSensor.get(); } @@ -146,6 +147,10 @@ public boolean isFull(){ return currentBallCount >= TransferConstants.MAX_BALL_COUNT; } + public boolean shouldOuttake(int index){ + return wrongBallColor == ballColorIndex.get(index); + } + public void transferIndexSetup(){ // If no IR Sensors, Disable all Sensors @@ -153,7 +158,7 @@ public void transferIndexSetup(){ return; // Starts Index / Counting Process when First Detecting Ball - new Trigger(this::isTransferStartIRBroken).and(new Trigger(()->!isReversed)) + new Trigger((() -> isTransferStartIRBroken())).and(new Trigger(()->!isReversed)) .whenActive(new TransferIndexForward(this)) .whenInactive(new TransferOff(this)); @@ -162,11 +167,11 @@ public void transferIndexSetup(){ );*/ // Subtract Balls shot out of shooter - new Trigger(this::isTransferEndIRBroken).and(new Trigger(()->!isReversed)) + new Trigger(() -> isTransferEndIRBroken()).and(new Trigger(()->!isReversed)) .whenInactive(new InstantCommand(this::removeShotBallFromIndex)); // When Reversed, Subtract Balls that leave - new Trigger(this::isTransferStartIRBroken).and(new Trigger(()-> isReversed)) + new Trigger(() -> isTransferStartIRBroken()).and(new Trigger(()-> isReversed)) .whenActive(new InstantCommand(this::removeBallEjectedOutOfIntake)); } @@ -208,6 +213,7 @@ private void ballIndexEnd(){ logger.warning("No Ball Detected in Index!"); else if (redColorCountVote == blueColorCountVote) + logger.warning("Blue and Red Ball Counts are the same!\nCount: " + redColorCountVote); else if (redColorCountVote > blueColorCountVote) { @@ -249,8 +255,8 @@ private void addBallToIndex(BallColor ballColor){ private void removeShotBallFromIndex(){ logger.info("Ball Leaving Transfer by Shooting"); - if (flywheelSubsystem != null) - logger.info("Ball Leaving At RPM: " + flywheelSubsystem.getFlywheelRPM() + " rpm"); + if (shooterSubsystem != null) + logger.info("Ball Leaving At RPM: " + shooterSubsystem.getFlywheelRPM() + " rpm"); currentBallCount--; if (currentBallCount < 0) { @@ -262,6 +268,7 @@ private void removeShotBallFromIndex(){ if (ballColorIndex.getLast() == BallColor.NONE) logger.warning("No Ball Color At end of index!"); + ballColorIndex.removeLast(); ballColorIndex.addFirst(BallColor.NONE); @@ -292,11 +299,13 @@ private void updateBallLEDPattern(){ BALL_PATTERN.update(firstBallColor, secondBallColor); } + private void wrongBallColorDetected(BallColor ballColorDetected){ logger.warning("Intaked Wrong Ball Color! " + "(Alliance: " + alliance + ", Ball Color Intaken: " + ballColorDetected + ")"); + wrongBallColor = ballColorDetected; } @Override