Amping up the Amp button

FRCTeam1987 · May 4, 2024 · 4a55b14 · 4a55b14
1 parent ff51289
commit 4a55b14
Show file tree

Hide file tree

Showing 5 changed files with 49 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -102,8 +102,8 @@ public class RobotContainer {
       new SlewRateLimiter(Constants.translationSlewRate);
   // private final SlewRateLimiter RotationalSlewRate =
   //     new SlewRateLimiter(Constants.rotationSlewRate);
-  public static boolean isForwardAmpPrimed = false;
-  public static boolean isReverseAmpPrimed = false;
+  public static boolean isAmpPrepped = false;
+  public static boolean isAmpShot = false;
   public static boolean isClimbPrimed = false;
   public static boolean aimAtTargetAuto = false;
   private double MaxSpeed = DriveConstants.kSpeedAt12VoltsMps;
@@ -136,19 +136,46 @@ public RobotContainer() {
   private void configureDriverController() {
     DRIVER_CONTROLLER.b().onTrue(new ShootSubwooferFlat(ELEVATOR, WRIST, SHOOTER));
 
+    // DRIVER_CONTROLLER
+    //     .y()
+    //     .onTrue(
+    //         new ConditionalCommand(
+    //             new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE)
+    //                 .andThen(() -> isReverseAmpPrimed = false),
+    //             new PrepRevAmp(ELEVATOR, WRIST)
+    //                 .andThen(new WaitCommand(0.8))
+    //                 .andThen(new FireRevAmp(SHOOTER))
+    //                 .andThen(new WaitCommand(0.1))
+    //                 .andThen(new InstantCommand(() -> ELEVATOR.setLengthInches(4.2)))
+    //                 .andThen(new InstantCommand(() -> isReverseAmpPrimed = true)),
+    //             () -> isReverseAmpPrimed));
+
     DRIVER_CONTROLLER
         .y()
         .onTrue(
             new ConditionalCommand(
-                new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE)
-                    .andThen(() -> isReverseAmpPrimed = false),
+                // Compare prepped to not prepped (If prepped, try shoot. Else, prep)
+                new ConditionalCommand(
+                    // Compare shot to not shot (If shot, go home. If not, try shot)
+                    new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE)
+                        .andThen(
+                            () -> {
+                              isAmpPrepped = false;
+                              isAmpShot = false;
+                            }),
+                    new ConditionalCommand(
+                        // Compare working shot (If working, shoot. Else rumble.)
+                        new FireRevAmp(SHOOTER)
+                            .andThen(new WaitCommand(0.1))
+                            .andThen(new InstantCommand(() -> ELEVATOR.setLengthInches(4.2)))
+                            .andThen(new InstantCommand(() -> isAmpShot = true)),
+                        new AsyncRumble(
+                            DRIVER_CONTROLLER.getHID(), RumbleType.kBothRumble, 1.0, 400L),
+                        () -> AMP_SENSORS.getBothSensors()),
+                    () -> isAmpShot),
                 new PrepRevAmp(ELEVATOR, WRIST)
-                    .andThen(new WaitCommand(0.8))
-                    .andThen(new FireRevAmp(SHOOTER))
-                    .andThen(new WaitCommand(0.1))
-                    .andThen(new InstantCommand(() -> ELEVATOR.setLengthInches(4.2)))
-                    .andThen(new InstantCommand(() -> isReverseAmpPrimed = true)),
-                () -> isReverseAmpPrimed));
+                    .andThen(new InstantCommand(() -> isAmpPrepped = true)),
+                () -> isAmpPrepped));
     DRIVER_CONTROLLER
         .back()
         .onTrue(

diff --git a/src/main/java/frc/robot/commands/control/AimLockWrist.java b/src/main/java/frc/robot/commands/control/AimLockWrist.java
@@ -33,7 +33,7 @@ public void execute() {
     if (RobotContainer.isClimbPrimed) {
       return;
     }
-    if (RobotContainer.isForwardAmpPrimed || RobotContainer.isReverseAmpPrimed) {
+    if (RobotContainer.isAmpPrepped) {
       return;
     }
     // if (Util.canSeeTarget(speakerLimelight)) {

diff --git a/src/main/java/frc/robot/commands/control/GoHome.java b/src/main/java/frc/robot/commands/control/GoHome.java
@@ -6,6 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.RobotContainer;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Shooter;
@@ -23,6 +24,8 @@ public GoHome(Elevator elevator, Wrist wrist, Shooter shooter, Intake intake) {
         new InstantCommand(wrist::goHome),
         new InstantCommand(shooter::stopShooter),
         new InstantCommand(shooter::stopFeeder),
-        new InstantCommand(intake::stopCollecting));
+        new InstantCommand(intake::stopCollecting),
+        new InstantCommand(() -> RobotContainer.isAmpPrepped = false),
+        new InstantCommand(() -> RobotContainer.isAmpShot = false));
   }
 }
diff --git a/src/main/java/frc/robot/commands/control/IdleShooter.java b/src/main/java/frc/robot/commands/control/IdleShooter.java
@@ -39,7 +39,7 @@ public void execute() {
       shooter.stopShooter();
       return;
     }
-    if (RobotContainer.isForwardAmpPrimed) {
+    if (RobotContainer.isAmpPrepped) {
       shooter.setRPMShoot(Constants.Shooter.SHOOTER_AMP_RPM);
       return;
     }

diff --git a/src/main/java/frc/robot/subsystems/AmpSensors.java b/src/main/java/frc/robot/subsystems/AmpSensors.java
@@ -18,15 +18,19 @@ public class AmpSensors extends SubsystemBase {
   public AmpSensors() {
     m_sensor_left = new DigitalInput(Constants.Wrist.PROXIMITY_SENSOR_LEFT_ID);
     m_sensor_right = new DigitalInput(Constants.Wrist.PROXIMITY_SENSOR_RIGHT_ID);
+    Shuffleboard.getTab("MAIN").addBoolean("Amp Left", () -> getSensorLeft());
+    Shuffleboard.getTab("MAIN").addBoolean("Amp Rights", () -> getSensorRight());
     Shuffleboard.getTab("MAIN").addBoolean("Amp Sensor", () -> getBothSensors());
   }
 
   public boolean getSensorLeft() {
-    return m_sensor_left.get();
+    //6.2 inches from wall
+    return !m_sensor_left.get();
   }
 
   public boolean getSensorRight() {
-    return m_sensor_right.get();
+    //6.2 inches from wall
+    return !m_sensor_right.get();
   }
 
   public boolean getBothSensors() {