added commands to subsystem classes and changed some booleans to Bool…

…eanSuppliers
Patribots4738 · Jan 24, 2024 · 19da0cf · 19da0cf
1 parent 644db44
commit 19da0cf
Show file tree

Hide file tree

Showing 5 changed files with 54 additions and 43 deletions.
diff --git a/simgui.json b/simgui.json
@@ -5,6 +5,11 @@
         "header": {
           "open": true
         }
+      },
+      "SPARK MAX [2]": {
+        "header": {
+          "open": true
+        }
       }
     }
   },

diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java
@@ -17,12 +17,12 @@ public class ShooterCalc {
 
   private Pivot pivot;
   private Shooter shooter;
-  private BooleanSupplier aiming;
+  private boolean aiming;
 
   public ShooterCalc(Shooter shooter, Pivot pivot) {
     this.pivot = pivot;
     this.shooter = shooter;
-    this.aiming = (() -> false);
+    this.aiming = false;
   }
 
   /**
@@ -37,8 +37,8 @@ public ShooterCalc(Shooter shooter, Pivot pivot) {
    */
   public Command prepareFireCommand(BooleanSupplier shootAtSpeaker, Pose2d robotPose) {
     SpeedAnglePair pair = calculateSpeed(robotPose, shootAtSpeaker.getAsBoolean());
-    return Commands.runOnce(() -> pivotSetAngle(pair.getAngle()))
-            .alongWith(Commands.runOnce(() -> shooterSetSpeed(pair.getSpeed())));
+    return pivot.setAngleCommand(pair.getAngle())
+            .alongWith(shooter.setSpeedCommand(pair.getSpeed()));
   }
 
   /**
@@ -57,13 +57,13 @@ public Command prepareFireCommand(BooleanSupplier shootAtSpeaker, Pose2d robotPo
    * @return The method is returning a Command object.
    */
   public Command prepareFireMovingCommand(BooleanSupplier shootAtSpeaker, Swerve swerve) {
-    if (aiming.getAsBoolean()) {
+    if (aiming) {
       return Commands.runOnce(() -> toggleAiming());
     }
     return Commands.runOnce(() -> toggleAiming())
             .andThen(prepareFireCommand(shootAtSpeaker, swerve.getPose())
                       .repeatedly()
-                      .until(() -> !aiming.getAsBoolean()));
+                      .until(() -> !aiming));
   }
 
   /**
@@ -81,7 +81,7 @@ public Command prepareFireMovingCommand(BooleanSupplier shootAtSpeaker, Swerve s
    */
   public Command prepareShooterCommand(BooleanSupplier shootAtSpeaker, Pose2d robotPose) {
     SpeedAnglePair pair = calculateSpeed(robotPose, shootAtSpeaker.getAsBoolean());
-    return Commands.runOnce(() -> shooterSetSpeed(pair.getSpeed()));
+    return shooter.setSpeedCommand(pair.getSpeed());
   }
 
   /**
@@ -96,44 +96,28 @@ public Command prepareShooterCommand(BooleanSupplier shootAtSpeaker, Pose2d robo
    */
   public Command preparePivotCommand(BooleanSupplier shootAtSpeaker, Pose2d robotPose) {
     SpeedAnglePair pair = calculateSpeed(robotPose, shootAtSpeaker.getAsBoolean());
-    return Commands.runOnce(() -> pivotSetAngle(pair.getAngle()));
+    return pivot.setAngleCommand(pair.getAngle());
   }
 
   public Command sendBackCommand() {
-    return Commands.runOnce(() -> pivotSetRestAngle())
-              .andThen(Commands.waitUntil(() -> pivot.isAtDesiredAngle()))
-              .andThen(() -> shooterSetSpeed(ShooterConstants.SHOOTER_BACK_SPEED));
+    return pivot.setRestAngleCommand()
+              .andThen(Commands.waitUntil(pivot.isAtDesiredAngle()))
+              .andThen(shooter.setSpeedCommand(ShooterConstants.SHOOTER_BACK_SPEED));
   }
 
   public Command resetShooter() {
       return Commands.runOnce(() -> stopAiming())
-              .andThen(shooterStopCommand()
-                        .alongWith(Commands.runOnce(() -> pivotSetRestAngle())));
-  }
-
-  public void shooterSetSpeed(double speed) {
-    shooter.setSpeed(speed);
-  }
-
-  public Command shooterStopCommand() {
-    return shooter.stop();
-  }
-
-  public void pivotSetAngle(double angle) {
-    pivot.setAngle(angle);
-  }
-
-  public void pivotSetRestAngle() {
-    pivot.setRestAngle();
+              .andThen(shooter.stop()
+                        .alongWith(pivot.setRestAngleCommand()));
   }
 
   // Toggles the aiming BooleanSupplier
   private void toggleAiming() {
-    this.aiming = (() -> !aiming.getAsBoolean());
+    this.aiming = !aiming;
   }
 
   private void stopAiming() {
-    this.aiming = (() -> false);
+    this.aiming = false;
   }
 
   // Gets a SpeedAnglePair by interpolating values from a map of already 

diff --git a/src/main/java/frc/robot/commands/handoff/Handoff.java b/src/main/java/frc/robot/commands/handoff/Handoff.java
@@ -54,7 +54,7 @@ public Handoff(
     }
 
     public Trigger readyToShoot() {
-        return new Trigger(() -> pivot.isAtDesiredAngle() && shooter.atDesiredRPM());
+        return new Trigger(() -> pivot.isAtDesiredAngle().getAsBoolean() && shooter.atDesiredRPM().getAsBoolean());
     }
 
     public Command stopAllMotors() {

diff --git a/src/main/java/frc/robot/subsystems/shooter/Pivot.java b/src/main/java/frc/robot/subsystems/shooter/Pivot.java
@@ -1,8 +1,13 @@
 package frc.robot.subsystems.shooter;
 
+import java.util.function.BooleanSupplier;
+
+import com.fasterxml.jackson.databind.ser.std.BooleanSerializer;
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.util.Neo;
 import frc.robot.util.Constants.ShooterConstants;
@@ -38,14 +43,24 @@ public void configMotor() {
     public void setAngle(double angle) {
         pivot.setTargetPosition(angle / ShooterConstants.PIVOT_MAX_ANGLE);
     }
+
+    public Command setAngleCommand(double angle) {
+        return Commands.runOnce(() -> setAngle(angle));
+    }
+
     public void setRestAngle() {
         this.setAngle(ShooterConstants.PIVOT_REST_ANGLE);
     }
 
-    public boolean isAtDesiredAngle() {
-        return MathUtil.applyDeadband(
-            Math.abs(
-                pivot.getPosition() - pivot.getTargetPosition()),
-            ShooterConstants.PIVOT_DEADBAND) == 0;
+    public Command setRestAngleCommand() {
+        return setAngleCommand(ShooterConstants.PIVOT_REST_ANGLE);
+    }
+
+    public BooleanSupplier isAtDesiredAngle() {
+        return () ->
+            (MathUtil.applyDeadband(
+                Math.abs(
+                    pivot.getPosition() - pivot.getTargetPosition()),
+                ShooterConstants.PIVOT_DEADBAND) == 0);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -4,6 +4,8 @@
 
 package frc.robot.subsystems.shooter;
 
+import java.util.function.BooleanSupplier;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -47,15 +49,20 @@ public void setSpeed(double speed) {
         motorLeft.setTargetVelocity(speed);
     }
 
+    public Command setSpeedCommand(double speed) {
+        return Commands.runOnce(() -> setSpeed(speed));
+    }
+
     public Command stop() {
-        return Commands.runOnce(() -> motorLeft.setTargetVelocity(0));
+        return setSpeedCommand(0);
     }
 
     //TODO: Implement a way to get the RPM of the shooter
-    public boolean atDesiredRPM() {
-        return MathUtil.applyDeadband(
-            Math.abs(
-                motorLeft.getVelocity() - motorLeft.getTargetVelocity()),
-            ShooterConstants.SHOOTER_DEADBAND) == 0;
+    public BooleanSupplier atDesiredRPM() {
+        return () ->
+            (MathUtil.applyDeadband(
+                Math.abs(
+                    motorLeft.getVelocity() - motorLeft.getTargetVelocity()),
+                ShooterConstants.SHOOTER_DEADBAND) == 0);
     }
 }