diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 86c4760..ae73654 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import frc.robot.auto.PhotonRunnable; import frc.robot.constants.OperatorConstants; import frc.robot.elevator.Elevator; +import frc.robot.elevator.commands.AmpScoreKill; import frc.robot.elevator.commands.ElevatorDown; import frc.robot.elevator.commands.ElevatorUp; import frc.robot.elevator.commands.ElevatorSmallUp; @@ -45,7 +46,7 @@ public class RobotContainer { private final Command intakePickup = new IntakePickup(intake); private final Command intakeShooterFeed = new IntakeShooterFeed(intake); private final Command intakeEject = new IntakeAmpScore(intake); - + private final Command ampScoreKill = new AmpScoreKill(elevator); private final Command shooterSpinUp = new ShooterSpinUp(shooter); private final SequentialCommandGroup ampScore = new SequentialCommandGroup( @@ -88,6 +89,7 @@ private void configureSubsystemCommands() { controller.getB().onTrue(speakerScore); controller.getRightTrigger().onTrue(ElevatorSmallUp); + controller.getY().onTrue(ampScoreKill); } private void configureSwerveDriveCommands() { diff --git a/src/main/java/frc/robot/elevator/commands/AmpScoreKill.java b/src/main/java/frc/robot/elevator/commands/AmpScoreKill.java new file mode 100644 index 0000000..5096798 --- /dev/null +++ b/src/main/java/frc/robot/elevator/commands/AmpScoreKill.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.elevator.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.ElevatorConstants; +import frc.robot.elevator.Elevator; + +public class AmpScoreKill extends Command { + private final Elevator elevator; + + public AmpScoreKill(Elevator elevator) { + this.elevator = elevator; + addRequirements(elevator); + + } + + @Override + + public void initialize() { + elevator.run(ElevatorConstants.MOTOR_SLOWFALL_SPEED); + } + + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) { + elevator.run(0); + } + + + @Override + public boolean isFinished() { + double steps = elevator.getEncoderPosition(); + if (steps < ElevatorConstants.MOTOR_MIN_STEPS) { + return true; + } else { + return false; + } + + } +}