Made the elevator and pivot go down when needed

Iron-Panthers · Sep 19, 2024 · 2c95e5d · 2c95e5d
1 parent f271cba
commit 2c95e5d
Show file tree

Hide file tree

Showing 4 changed files with 88 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -578,6 +578,8 @@ public static final class Ports {
     public static final double MAX_HEIGHT = 50;
     /** FIXME */
     public static final double ELEVATOR_FEEDFORWARD = 0.01;
+    /**FIXME */
+    public static final double AMP_HEIGHT = 20;
 
     public static final class Arm {
       /** FIXME */

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -33,13 +33,15 @@
 import frc.robot.autonomous.HeadingAngle;
 import frc.robot.autonomous.HeadingTargetLock;
 import frc.robot.commands.AccelNoteCommand;
+import frc.robot.commands.AmpPreparationCommand;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
 import frc.robot.commands.ElevatorHeightCommand;
 import frc.robot.commands.HaltDriveCommandsCommand;
 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;
@@ -51,6 +53,7 @@
 import frc.robot.commands.StopIntakeCommand;
 import frc.robot.commands.StopShooterCommand;
 import frc.robot.commands.TargetLockCommand;
+import frc.robot.commands.UnloadShooterCommand;
 import frc.robot.commands.VibrateHIDCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
 import frc.robot.subsystems.DrivebaseSubsystem;
@@ -338,14 +341,18 @@ private void configureButtonBindings() {
     anthony
         .leftBumper()
         .onTrue(
-            new IntakeCommand(
-                intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
+            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+            .andThen(
+                new IntakeCommand(
+                    intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     jacob
         .leftBumper()
         .onTrue(
-            new IntakeCommand(
-                intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
+            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+            .andThen(
+                new IntakeCommand(
+                    intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // SHOOT
     anthony
@@ -412,10 +419,22 @@ private void configureButtonBindings() {
                         intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // NOTE TO SHOOTER OR SERIALIZER
-    anthony.b().onTrue(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
-
-    jacob.b().onTrue(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
-    jacob.a().onTrue(new ShootCommand(shooterSubsystem));
+    anthony.b().onTrue(
+            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+        .andThen(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+
+    jacob.b().onTrue(
+        new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+        .andThen(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+    jacob.a().onTrue(
+        new RotateAngleDriveCommand(
+                drivebaseSubsystem,
+                translationXSupplier,
+                translationYSupplier,
+                DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
+            .alongWith(
+                new AmpPreparationCommand(
+                    pivotSubsystem, elevatorSubsystem, shooterSubsystem)));
 
     // SPEAKER FROM SUBWOOFER
     anthony
@@ -434,8 +453,8 @@ private void configureButtonBindings() {
                     translationYSupplier,
                     DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
                 .alongWith(
-                    new ElevatorHeightCommand(
-                        elevatorSubsystem, 20))); // height should be set to a constant
+                    new AmpPreparationCommand(
+                        pivotSubsystem, elevatorSubsystem, shooterSubsystem))); 
 
     DoubleSupplier rotation =
         exponential(

diff --git a/src/main/java/frc/robot/commands/AmpPreparationCommand.java b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
@@ -0,0 +1,37 @@
+// 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 java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants.Elevator;
+import frc.robot.subsystems.DrivebaseSubsystem;
+import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.PivotSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class AmpPreparationCommand extends SequentialCommandGroup {
+  /** Creates a new AmpPreparationCommand. */
+  public AmpPreparationCommand(
+    PivotSubsystem pivotSubsystem,
+    ElevatorSubsystem elevatorSubsystem,
+    ShooterSubsystem shooterSubsystem) {
+    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)));
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java b/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java
@@ -0,0 +1,20 @@
+// 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.Command;
+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));
+  }
+}