diff --git a/src/main/java/frc/robot/OperatorJoystick.java b/src/main/java/frc/robot/OperatorJoystick.java index 5d65e6dc..08261722 100644 --- a/src/main/java/frc/robot/OperatorJoystick.java +++ b/src/main/java/frc/robot/OperatorJoystick.java @@ -11,6 +11,7 @@ import frc.robot.commands.UnacquireCommand; import frc.robot.commands.ScoringElevatorPositionCommand.FinishActions; import frc.robot.commands.ScoringElevatorPositionCommand; +import frc.robot.commands.ShootPasserCommand; import frc.robot.commands.ScoreNoteCommand; import frc.robot.subsystems.ScoringElevator; import frc.robot.subsystems.Storage; @@ -62,6 +63,9 @@ private OperatorJoystick() { new Trigger(() -> operatorJoystick.getPOVPosition() == PovPositions.Right) .and(() -> !scoringElevatorLock) .toggleOnTrue(new ScoringElevatorPositionCommand(ElevatorSetpoints.Amp, FinishActions.NoFinish)); + new Trigger(() -> operatorJoystick.getPOVPosition() == PovPositions.Up) + .and(() -> !scoringElevatorLock) + .toggleOnTrue(new ScoringElevatorPositionCommand(ElevatorSetpoints.Passer, FinishActions.NoFinish)); // Amp buttons rightTrigger @@ -79,6 +83,11 @@ private OperatorJoystick() { .and(() -> scoringElevator.getSetpoint() == ElevatorSetpoints.Trap.value) .whileTrue(new FeedNoteCommand(ScoringLocation.Trap)); + // Passer button + leftTrigger + .and(() -> scoringElevator.getSetpoint() == ElevatorSetpoints.Passer.value) + .whileTrue(new ShootPasserCommand()); + leftBumper.onTrue(new DrawbridgeUpCommand()); rightBumper.onTrue(new DrawbridgeDownCommand()); } diff --git a/src/main/java/frc/robot/commands/FeedNoteCommand.java b/src/main/java/frc/robot/commands/FeedNoteCommand.java index bd585ac9..f22d08a0 100644 --- a/src/main/java/frc/robot/commands/FeedNoteCommand.java +++ b/src/main/java/frc/robot/commands/FeedNoteCommand.java @@ -46,7 +46,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - scoring.stopRoller(); + scoring.stop(); timer.stop(); timer.reset(); } diff --git a/src/main/java/frc/robot/commands/FeedNoteFineAdjCommand.java b/src/main/java/frc/robot/commands/FeedNoteFineAdjCommand.java index 0ef013d5..7c13956b 100644 --- a/src/main/java/frc/robot/commands/FeedNoteFineAdjCommand.java +++ b/src/main/java/frc/robot/commands/FeedNoteFineAdjCommand.java @@ -28,6 +28,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - scoring.stopRoller(); + scoring.stop(); } } diff --git a/src/main/java/frc/robot/commands/ScoreNoteCommand.java b/src/main/java/frc/robot/commands/ScoreNoteCommand.java index 728b1f40..db350654 100644 --- a/src/main/java/frc/robot/commands/ScoreNoteCommand.java +++ b/src/main/java/frc/robot/commands/ScoreNoteCommand.java @@ -61,7 +61,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - scoring.stopRoller(); + scoring.stop(); storage.stopStorage(); timer.stop(); timer.reset(); diff --git a/src/main/java/frc/robot/commands/ScoringElevatorPositionCommand.java b/src/main/java/frc/robot/commands/ScoringElevatorPositionCommand.java index b1a335d7..11bc6fe8 100644 --- a/src/main/java/frc/robot/commands/ScoringElevatorPositionCommand.java +++ b/src/main/java/frc/robot/commands/ScoringElevatorPositionCommand.java @@ -32,17 +32,7 @@ public ScoringElevatorPositionCommand(ElevatorSetpoints scoringState) { @Override public void initialize() { scoring.enable(); - switch (scoringState) { - case Amp: - scoring.setSetpoint(ElevatorSetpoints.Amp); - break; - case Ground: - scoring.setSetpoint(ElevatorSetpoints.Ground); - break; - default: - scoring.setSetpoint(ElevatorSetpoints.Trap); - break; - } + scoring.setSetpoint(scoringState); } @Override diff --git a/src/main/java/frc/robot/commands/ShootPasserCommand.java b/src/main/java/frc/robot/commands/ShootPasserCommand.java new file mode 100644 index 00000000..a864e03c --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootPasserCommand.java @@ -0,0 +1,54 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Passer; +import frc.robot.subsystems.Scoring; +import frc.robot.subsystems.Storage; + +public class ShootPasserCommand extends Command { + private Scoring scoring = Scoring.getInstance(); + private Storage storage = Storage.getInstance(); + private Passer passer = Passer.getInstance(); + private Timer timer = new Timer(); + private double secondsToScore = 0; + + public ShootPasserCommand(double secondsToScore) { + this.addRequirements(scoring); + this.secondsToScore = secondsToScore; + } + + public ShootPasserCommand() { + this.addRequirements(scoring); + } + + @Override + public void initialize() { + timer.start(); + } + + @Override + public void execute() { + scoring.feedForPasser(); + passer.shoot(); + if (!storage.noteExitingStorage()) { + storage.stopStorage(); + } else { + storage.runStorage(); + } + } + + @Override + public boolean isFinished() { + return this.secondsToScore != 0 ? timer.get() > this.secondsToScore : false; + } + + @Override + public void end(boolean interrupted) { + scoring.stop(); + storage.stopStorage(); + passer.stop(); + timer.stop(); + timer.reset(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/PasserConstants.java b/src/main/java/frc/robot/constants/PasserConstants.java new file mode 100644 index 00000000..18ca60ce --- /dev/null +++ b/src/main/java/frc/robot/constants/PasserConstants.java @@ -0,0 +1,7 @@ +package frc.robot.constants; + +public class PasserConstants { + public static final int passerMotorPort = 11; + + public static final double shooterSpeed = 1.0; +} diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index bef23096..ccdf1a24 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -1,11 +1,12 @@ package frc.robot.constants; public class ScoringConstants { - public static final int rollerMotorPort = 12; + public static final int scoringMotorPort = 12; public static final double feedTrapSpeed = -0.32; public static final double scoreTrapSpeed = 1.0; public static final double feedAmpSpeed = -0.32; public static final double scoreAmpSpeed = 0.75; + public static final double feedPasserSpeed = -0.32; public enum ScoringLocation { Amp, diff --git a/src/main/java/frc/robot/constants/ScoringElevatorConstants.java b/src/main/java/frc/robot/constants/ScoringElevatorConstants.java index 48968af4..c6fbe02b 100644 --- a/src/main/java/frc/robot/constants/ScoringElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ScoringElevatorConstants.java @@ -10,6 +10,7 @@ public class ScoringElevatorConstants { public static final double groundSetpoint = 0; public static final double ampSetpoint = 4.0; + public static final double passerSetpoint = 6.0; public static final double trapSetpoint = 15.0; public static final double maxDistance = 0; diff --git a/src/main/java/frc/robot/constants/StorageConstants.java b/src/main/java/frc/robot/constants/StorageConstants.java index 1bb59076..76e9f8a8 100644 --- a/src/main/java/frc/robot/constants/StorageConstants.java +++ b/src/main/java/frc/robot/constants/StorageConstants.java @@ -3,8 +3,7 @@ public class StorageConstants { public static final int transitionMotorPort = 7; - public static final int leftStoragePort = 11; - public static final int rightStoragePort = 8; + public static final int storagePort = 8; public static final int enterStorageLaserPort = 1; public static final int exitStorageLaserPort = 0; diff --git a/src/main/java/frc/robot/subsystems/Passer.java b/src/main/java/frc/robot/subsystems/Passer.java new file mode 100644 index 00000000..ade03cc8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Passer.java @@ -0,0 +1,61 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.RelativeEncoder; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.NeoMotorConstants; +import frc.robot.constants.PasserConstants; + +public class Passer extends SubsystemBase { + private final CANSparkFlex passerMotor = new CANSparkFlex(PasserConstants.passerMotorPort, MotorType.kBrushless); + private final RelativeEncoder passerEncoder = passerMotor.getEncoder(); + + private static Passer instance; + + /** + * Creates an instance of the Passer subsystem if it does not already exist. + * + * @return An instance of the Passer subsystem. + */ + public static Passer getInstance() { + if (instance == null) { + instance = new Passer(); + } + return instance; + } + + private Passer() { + passerMotor.restoreFactoryDefaults(); + + passerMotor.setIdleMode(IdleMode.kBrake); + passerMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxVortexCurrent); + passerMotor.setInverted(true); + + passerMotor.burnFlash(); + passerEncoder.setPosition(0); + } + + /** + * Get the encoder value of the passer encoder + */ + public double getPosition() { + return passerEncoder.getPosition(); + } + + /** + * Runs the passer at a constant speed + */ + public void shoot() { + passerMotor.set(PasserConstants.shooterSpeed); + } + + /** + * Stops the passer + */ + public void stop() { + passerMotor.stopMotor(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Scoring.java b/src/main/java/frc/robot/subsystems/Scoring.java index c1fc4eda..36f6a0ba 100644 --- a/src/main/java/frc/robot/subsystems/Scoring.java +++ b/src/main/java/frc/robot/subsystems/Scoring.java @@ -11,8 +11,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Scoring extends SubsystemBase { - private final CANSparkFlex rollerMotor = new CANSparkFlex(ScoringConstants.rollerMotorPort, MotorType.kBrushless); - private final RelativeEncoder rollerEncoder = rollerMotor.getEncoder(); + private final CANSparkFlex scoringMotor = new CANSparkFlex(ScoringConstants.scoringMotorPort, MotorType.kBrushless); + private final RelativeEncoder scoringEncoder = scoringMotor.getEncoder(); private double feedAmpSpeed = ScoringConstants.feedAmpSpeed; private static Scoring instance; @@ -26,21 +26,21 @@ public static Scoring getInstance() { } public Scoring() { - rollerMotor.restoreFactoryDefaults(); + scoringMotor.restoreFactoryDefaults(); - rollerMotor.setIdleMode(IdleMode.kBrake); - rollerMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxVortexCurrent); - rollerMotor.setInverted(false); + scoringMotor.setIdleMode(IdleMode.kBrake); + scoringMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxVortexCurrent); + scoringMotor.setInverted(false); - rollerMotor.burnFlash(); - rollerEncoder.setPosition(0); + scoringMotor.burnFlash(); + scoringEncoder.setPosition(0); } /** - * Get the encoder value of the roller encoder + * Get the encoder value of the scoring encoder */ public double getPosition() { - return rollerEncoder.getPosition(); + return scoringEncoder.getPosition(); } /** @@ -57,14 +57,14 @@ public void setFeedAmpSpeed(double speed) { * ready to score in amp. */ public void feedForAmp() { - rollerMotor.set(feedAmpSpeed); + scoringMotor.set(feedAmpSpeed); } /** * Runs the scoring roller at the constant speed designated for scoring in amp. */ public void scoreAmp() { - rollerMotor.set(ScoringConstants.scoreAmpSpeed); + scoringMotor.set(ScoringConstants.scoreAmpSpeed); } /** @@ -72,20 +72,28 @@ public void scoreAmp() { * ready to score in trap. */ public void feedForTrap() { - rollerMotor.set(feedAmpSpeed); + scoringMotor.set(feedAmpSpeed); } /** * Runs the scoring roller at the constant speed designated for scoring in amp. */ public void scoreTrap() { - rollerMotor.set(ScoringConstants.scoreTrapSpeed); + scoringMotor.set(ScoringConstants.scoreTrapSpeed); + } + + /** + * Runs the scoring roller at the constant speed designated for getting the note + * ready to shoot in passer. + */ + public void feedForPasser() { + scoringMotor.set(ScoringConstants.feedPasserSpeed); } /** * Stops the scoring roller. */ - public void stopRoller() { - rollerMotor.stopMotor(); + public void stop() { + scoringMotor.stopMotor(); } } diff --git a/src/main/java/frc/robot/subsystems/ScoringElevator.java b/src/main/java/frc/robot/subsystems/ScoringElevator.java index 37fd2e89..de776ecc 100644 --- a/src/main/java/frc/robot/subsystems/ScoringElevator.java +++ b/src/main/java/frc/robot/subsystems/ScoringElevator.java @@ -41,6 +41,7 @@ public class ScoringElevator extends SubsystemBase { public enum ElevatorSetpoints { Ground(ScoringElevatorConstants.groundSetpoint), Amp(ScoringElevatorConstants.ampSetpoint), + Passer(ScoringElevatorConstants.passerSetpoint), Trap(ScoringElevatorConstants.trapSetpoint); public final double value; diff --git a/src/main/java/frc/robot/subsystems/Storage.java b/src/main/java/frc/robot/subsystems/Storage.java index 78814968..aecefdf8 100644 --- a/src/main/java/frc/robot/subsystems/Storage.java +++ b/src/main/java/frc/robot/subsystems/Storage.java @@ -13,10 +13,8 @@ public class Storage extends SubsystemBase { private final CANSparkMax transitionMotor = new CANSparkMax(StorageConstants.transitionMotorPort, MotorType.kBrushless); - private final CANSparkMax leftStorageMotor = new CANSparkMax( - StorageConstants.leftStoragePort, MotorType.kBrushless); - private final CANSparkMax rightStorageMotor = new CANSparkMax( - StorageConstants.rightStoragePort, MotorType.kBrushless); + private final CANSparkMax storageMotor = new CANSparkMax( + StorageConstants.storagePort, MotorType.kBrushless); private final DigitalInput enterStorageLaser = new DigitalInput(StorageConstants.enterStorageLaserPort); private final DigitalInput exitStorageLaser = new DigitalInput(StorageConstants.exitStorageLaserPort); @@ -37,26 +35,19 @@ public static Storage getInstance() { private Storage() { transitionMotor.restoreFactoryDefaults(); - leftStorageMotor.restoreFactoryDefaults(); - rightStorageMotor.restoreFactoryDefaults(); + storageMotor.restoreFactoryDefaults(); - leftStorageMotor.setIdleMode(IdleMode.kBrake); - rightStorageMotor.setIdleMode(IdleMode.kBrake); + storageMotor.setIdleMode(IdleMode.kBrake); transitionMotor.setIdleMode(IdleMode.kBrake); - leftStorageMotor.setInverted(false); - rightStorageMotor.setInverted(false); + storageMotor.setInverted(true); transitionMotor.setInverted(false); - rightStorageMotor.follow(leftStorageMotor, true); - - rightStorageMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxNeo550Current); - leftStorageMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxNeo550Current); + storageMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxNeo550Current); transitionMotor.setSmartCurrentLimit(NeoMotorConstants.kMaxNeo550Current); transitionMotor.burnFlash(); - leftStorageMotor.burnFlash(); - rightStorageMotor.burnFlash(); + storageMotor.burnFlash(); } public void runTransition() { @@ -68,15 +59,15 @@ public void reverseTransition() { } public void runStorage() { - leftStorageMotor.set(StorageConstants.storageSpeed); + storageMotor.set(StorageConstants.storageSpeed); } public void reverseStorage() { - leftStorageMotor.set(StorageConstants.reverseStorageSpeed); + storageMotor.set(StorageConstants.reverseStorageSpeed); } public void stopStorage() { - leftStorageMotor.stopMotor(); + storageMotor.stopMotor(); } public void stopTransition() {