Transferring note in two commands

Iron-Panthers · Sep 20, 2024 · fb07786 · fb07786
1 parent 07be5d2
commit fb07786
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 55 deletions.
diff --git 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
@@ -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
@@ -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
diff --git 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;
     }
   }