From fb07786458c5d6d0710f390065e3b2a116ab6dfc Mon Sep 17 00:00:00 2001 From: Heggiehog Date: Thu, 19 Sep 2024 20:02:46 -0700 Subject: [PATCH] Transferring note in two commands --- src/main/java/frc/robot/RobotContainer.java | 31 ++++++------------- .../robot/commands/AmpPreparationCommand.java | 12 ++++--- .../frc/robot/commands/IntakeCommand.java | 10 +++--- ...otAndElevatorTransferPositionsCommand.java | 22 ------------- .../robot/commands/UnloadShooterCommand.java | 8 +++-- 5 files changed, 28 insertions(+), 55 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5a26a02..eb3ea61 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,6 @@ import frc.robot.commands.IntakeCommand; import frc.robot.commands.LoadShooterCommand; import frc.robot.commands.OuttakeCommand; -import frc.robot.commands.PivotAndElevatorTransferPositionsCommand; import frc.robot.commands.PivotAngleCommand; import frc.robot.commands.PivotManualCommand; import frc.robot.commands.PivotTargetLockCommand; @@ -339,18 +338,14 @@ private void configureButtonBindings() { anthony .leftBumper() .onTrue( - new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem) - .andThen( - new IntakeCommand( - intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))); + new IntakeCommand( + intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)); jacob .leftBumper() .onTrue( - new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem) - .andThen( - new IntakeCommand( - intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))); + new IntakeCommand( + intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)); // SHOOT anthony @@ -394,25 +389,19 @@ private void configureButtonBindings() { ? -Setpoints.SOURCE_DEGREES : Setpoints.SOURCE_DEGREES) .alongWith( - new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem) - .andThen( - new IntakeCommand( - intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)))); + new IntakeCommand( + intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))); // NOTE TO SHOOTER OR SERIALIZER anthony .b() .onTrue( - new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem) - .andThen( - new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem))); + new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)); jacob .b() .onTrue( - new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem) - .andThen( - new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem))); + new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)); jacob .a() .onTrue( @@ -423,7 +412,7 @@ private void configureButtonBindings() { DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) // FIXME confirm angles .alongWith( new AmpPreparationCommand( - pivotSubsystem, elevatorSubsystem, shooterSubsystem))); + pivotSubsystem, elevatorSubsystem, shooterSubsystem, intakeSubsystem))); // SPEAKER FROM SUBWOOFER anthony @@ -443,7 +432,7 @@ private void configureButtonBindings() { DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) // FIXME confirm angles .alongWith( new AmpPreparationCommand( - pivotSubsystem, elevatorSubsystem, shooterSubsystem))); + pivotSubsystem, elevatorSubsystem, shooterSubsystem, intakeSubsystem))); DoubleSupplier rotation = exponential( diff --git a/src/main/java/frc/robot/commands/AmpPreparationCommand.java b/src/main/java/frc/robot/commands/AmpPreparationCommand.java index 9f8626b..dd55d69 100644 --- a/src/main/java/frc/robot/commands/AmpPreparationCommand.java +++ b/src/main/java/frc/robot/commands/AmpPreparationCommand.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.PivotSubsystem; import frc.robot.subsystems.ShooterSubsystem; @@ -15,15 +16,16 @@ public class AmpPreparationCommand extends SequentialCommandGroup { public AmpPreparationCommand( PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem, - ShooterSubsystem shooterSubsystem) { + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem) { if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()) { addCommands(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)); } else { addCommands( - new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem) - .andThen( - new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)) - .andThen(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT))); + new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem) + .andThen(new IntakeCommand( + intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)) + .andThen(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT))); } } } diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 744005b..ca99496 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -37,15 +37,17 @@ public IntakeCommand( // Called when the command is initially scheduled. @Override public void initialize() { - pivotSubsystem.setTargetDegrees(20); - intakeSubsystem.setIntakeMode(IntakeMode.INTAKE); - shooterSubsystem.setShooterMode(ShooterMode.INTAKE); elevatorSubsystem.setTargetHeight(0); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if(elevatorSubsystem.atTargetHeight()){ + intakeSubsystem.setIntakeMode(IntakeMode.INTAKE); + shooterSubsystem.setShooterMode(ShooterMode.INTAKE); + } + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java b/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java deleted file mode 100644 index 853f013..0000000 --- a/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java +++ /dev/null @@ -1,22 +0,0 @@ -// 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.commands; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.subsystems.ElevatorSubsystem; -import frc.robot.subsystems.PivotSubsystem; - -public class PivotAndElevatorTransferPositionsCommand extends ParallelCommandGroup { - /** Creates a new PivotAndElevatorTransferPositionsCommand. */ - public PivotAndElevatorTransferPositionsCommand( - PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem) { - addCommands( - new PivotAngleCommand(pivotSubsystem, 20), new ElevatorHeightCommand(elevatorSubsystem, 0)); - } -} - - /* combine load and unload commands with this one - - it should move everything into the correct position - and either load or unload based on input from a button */ \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/UnloadShooterCommand.java b/src/main/java/frc/robot/commands/UnloadShooterCommand.java index e8c18ec..9c95f2f 100644 --- a/src/main/java/frc/robot/commands/UnloadShooterCommand.java +++ b/src/main/java/frc/robot/commands/UnloadShooterCommand.java @@ -32,16 +32,18 @@ public UnloadShooterCommand( // Called when the command is initially scheduled. @Override public void initialize() { - pass = shooterSubsystem.isSerializerBeamBreakSensorTriggered(); - shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD); pivotSubsystem.setTargetDegrees(20); elevatorSubsystem.setTargetHeight(0); + pass = shooterSubsystem.isSerializerBeamBreakSensorTriggered(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (shooterSubsystem.isSerializerBeamBreakSensorTriggered() && pass == false) { + if (elevatorSubsystem.atTargetHeight() && pivotSubsystem.atTargetDegrees()) { + shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD); + } + if (shooterSubsystem.isSerializerBeamBreakSensorTriggered() && pass == false) { pass = true; } }