Add passing shot

frc868 · Apr 13, 2024 · 293e171 · 293e171
1 parent c21c083
commit 293e171
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -237,6 +237,7 @@ public static final class Shooter {
         public static final int CURRENT_LIMIT = 70;
 
         public static final double IDLE_RPS = 35;
+        public static final double PASSING_RPS = 50;
         public static final double SUBWOOFER_RPS = 55;
         public static final double PODIUM_RPS = 84;
 
@@ -299,6 +300,7 @@ public static enum ShooterTiltPosition {
             INTAKE(0.55 + 0.035),
             CLIMB(1.176781),
             SUBWOOFER(1.01572),
+            PASS(1.00),
             PODIUM(0.651760);
 
             public final double value;

diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java
@@ -77,6 +77,10 @@ public static void configureDriverControls(int port, Drivetrain drivetrain, Inta
                         RobotCommands.shootOnTheMoveCommand(drivetrain, intake, shooter, shooterTilt),
                         new Trigger(GlobalStates.SUBWOOFER_ONLY::enabled).or(GlobalStates.PODIUM_ONLY::enabled)));
 
+        joystick.flipTriggerIn().and(joystick.triggerSoftPress()).whileTrue(
+                RobotCommands.targetPassCommand(drivetrain, shooter, shooterTilt));
+        joystick.flipTriggerIn().and(joystick.triggerHardPress()).whileTrue(intake.runRollersCommand());
+
         joystick.topRightHatUp()
                 .onTrue(GlobalStates.PODIUM_ONLY.enableCommand().andThen(GlobalStates.SUBWOOFER_ONLY.disableCommand()));
         joystick.topRightHatDown()

diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java
@@ -1,8 +1,11 @@
 package frc.robot;
 
+import static frc.robot.Constants.Shooter.PASSING_RPS;
 import static frc.robot.Constants.Shooter.PODIUM_RPS;
 import static frc.robot.Constants.Shooter.SUBWOOFER_RPS;
 
+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.Commands;
 import edu.wpi.first.wpilibj2.command.ScheduleCommand;
@@ -89,6 +92,16 @@ public static Command targetSpeakerPodiumCommand(Drivetrain drivetrain, Shooter
                 .withName("RobotCommands.targetSpeakerSubwoofer");
     }
 
+    public static Command targetPassCommand(Drivetrain drivetrain, Shooter shooter,
+            ShooterTilt shooterTilt) {
+        return Commands.parallel(
+                drivetrain.controlledRotateCommand(
+                        () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? 2.5 : -2.5),
+                shooterTilt.moveToPositionCommand(() -> ShooterTiltPosition.PASS).asProxy(),
+                shooter.spinAtVelocityCommand(() -> PASSING_RPS).asProxy())
+                .withName("RobotCommands.targetPass");
+    }
+
     public static Command shootCommand(Drivetrain drivetrain, Intake intake, Shooter shooter, ShooterTilt shooterTilt) {
         return Commands.deadline(
                 Commands.waitUntil(() -> shooter.atGoal() && shooterTilt.atGoal())