Speedy pass (#282)

Patribots4738 · Apr 12, 2024 · 67fce33 · 67fce33
2 parents 5160bbd + 6d246ca
commit 67fce33
Show file tree

Hide file tree

Showing 9 changed files with 79 additions and 24 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -51,6 +51,7 @@
 import frc.robot.util.Constants.ShooterConstants;
 import frc.robot.util.auto.PathPlannerStorage;
 import frc.robot.util.calc.LimelightMapping;
+import frc.robot.util.calc.PoseCalculations;
 import frc.robot.util.calc.ShooterCalc;
 import frc.robot.util.custom.PatriBoxController;
 import frc.robot.util.custom.ActiveConditionalCommand;
@@ -433,7 +434,7 @@ private void configureCommonDriverBindings(PatriBoxController controller) {
                         // and shooting-while-still on shooter
                         alignmentCmds.wingRotationalAlignment(controller::getLeftX, controller::getLeftY, robotRelativeSupplier),
                         alignmentCmds.preparePassCommand(controller::getLeftX, controller::getLeftY, robotRelativeSupplier),
-                        alignmentCmds.alignmentCalc::closeToSpeaker)
+                        PoseCalculations::closeToSpeaker)
                     ).finallyDo(
                         () -> 
                             limelight3g.setLEDState(() -> false)

diff --git a/src/main/java/frc/robot/commands/managers/PieceControl.java b/src/main/java/frc/robot/commands/managers/PieceControl.java
@@ -25,6 +25,7 @@
 import frc.robot.subsystems.Ampper;
 import frc.robot.util.Constants.FieldConstants;
 import frc.robot.util.Constants.ShooterConstants;
+import frc.robot.util.calc.PoseCalculations;
 import frc.robot.util.Constants.ElevatorConstants;
 import frc.robot.util.custom.ActiveConditionalCommand;
 import frc.robot.util.custom.SpeedAngleTriplet;
@@ -99,8 +100,15 @@ public Command brakeIntakeAndIndexer() {
 
     // TODO: only run angle reset when we are not using prepareSWDCommand
     public Command shootWhenReady(Supplier<Pose2d> poseSupplier, Supplier<ChassisSpeeds> speedSupplier, BooleanSupplier atDesiredAngle) {
-        return Commands.waitUntil(() -> shooterCmds.shooterCalc.readyToShootSupplier().getAsBoolean() && atDesiredAngle.getAsBoolean())
-                .andThen(noteToShoot(poseSupplier, speedSupplier));
+        return
+            Commands.waitUntil(() -> 
+                    (PoseCalculations.closeToSpeaker()
+                        ? shooterCmds.shooterCalc.readyToShootSupplier().getAsBoolean() 
+                        : shooterCmds.shooterCalc.readyToPassSupplier().getAsBoolean())
+                    && atDesiredAngle.getAsBoolean()
+                )
+            .andThen(noteToShoot(poseSupplier, speedSupplier));
+
     }
 
     public Command shootPreload() {

diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java
@@ -24,6 +24,9 @@ public class Pivot extends SubsystemBase implements Logged {
 	@Log
 	private boolean atDesiredAngle = false;
 
+    @Log
+	private boolean atDesiredPassAngle = false;
+
 	public Pivot() {
 		motor = new Neo(
             ShooterConstants.SHOOTER_PIVOT_CAN_ID,
@@ -48,7 +51,10 @@ public void periodic() {
         realAngle = getAngle();
 
 		atDesiredAngle = 
-            MathUtil.isNear(realAngle, desiredAngle, ShooterConstants.PIVOT_DEADBAND);
+            MathUtil.isNear(realAngle, desiredAngle, ShooterConstants.PIVOT_DEADBAND_DEGREES);
+
+        atDesiredPassAngle = 
+            MathUtil.isNear(realAngle, desiredAngle, ShooterConstants.PIVOT_PASS_DEADBAND_DEGREES);
 
 		RobotContainer.components3d[NTConstants.PIVOT_INDEX] = new Pose3d(
 			NTConstants.PIVOT_OFFSET_METERS.getX(),
@@ -110,4 +116,8 @@ public double getTargetAngle() {
 	public boolean getAtDesiredAngle() {
 		return atDesiredAngle;
 	}
+
+    public boolean getAtDesiredPassAngle() {
+        return atDesiredPassAngle;
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -29,6 +29,9 @@ public class Shooter extends SubsystemBase implements Logged{
     @Log
     private boolean atDesiredRPM = false;
 
+    @Log
+    private boolean atDesiredPassRPM = false;
+
     public Shooter() {
 
         leftMotor = new Neo(ShooterConstants.LEFT_SHOOTER_CAN_ID, true, true);
@@ -60,6 +63,14 @@ public void periodic() {
             && MathUtil.isNear(
                 currentRightSpeed, targetRightSpeed,
                 ShooterConstants.SHOOTER_RPM_DEADBAND);
+
+        atDesiredPassRPM = 
+            MathUtil.isNear( 
+                currentLeftSpeed, targetLeftSpeed,
+                ShooterConstants.SHOOTER_PASS_RPM_DEADBAND)
+            && MathUtil.isNear(
+                currentRightSpeed, targetRightSpeed,
+                ShooterConstants.SHOOTER_PASS_RPM_DEADBAND);
     }
 
     /**
@@ -125,6 +136,10 @@ public boolean getAtDesiredRPM() {
         return atDesiredRPM;
     }
 
+    public boolean getAtDesiredPassRPM() {
+        return atDesiredPassRPM;
+    }
+
     public Pair<Double, Double> getDesiredSpeeds() {
         return new Pair<Double, Double>(targetLeftSpeed, targetRightSpeed);
     }

diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -40,6 +40,7 @@
 import frc.robot.util.Constants.AutoConstants;
 import frc.robot.util.Constants.DriveConstants;
 import frc.robot.util.Constants.FieldConstants;
+import frc.robot.util.calc.PoseCalculations;
 import frc.robot.util.rev.MAXSwerveModule;
 import monologue.Logged;
 import monologue.Annotations.Log;
@@ -452,15 +453,20 @@ public boolean atPose(Pose2d position) {
 		double distance = currentPose.relativeTo(position).getTranslation().getNorm();
         return 
             MathUtil.isNear(0, distance, AutoConstants.AUTO_POSITION_TOLERANCE_METERS)
-            && MathUtil.isNear(0, angleDiff, AutoConstants.AUTO_POSITION_TOLERANCE_RADIANS);
+            && MathUtil.isNear(0, angleDiff, AutoConstants.AUTO_ROTATION_TOLERANCE_RADIANS);
     }
 
     public boolean atHDCPose() {
         return atPose(desiredHDCPose);
     }
 
     public boolean atHDCAngle() {
-        return MathUtil.isNear(desiredHDCPose.getRotation().getRadians(), getPose().getRotation().getRadians(), AutoConstants.AUTO_POSITION_TOLERANCE_RADIANS);
+        return MathUtil.isNear(
+            desiredHDCPose.getRotation().getRadians(), 
+            getPose().getRotation().getRadians(), 
+            PoseCalculations.closeToSpeaker()
+                ? AutoConstants.AUTO_ROTATION_TOLERANCE_RADIANS
+                : AutoConstants.PASS_ROTATION_TOLERANCE_RADIANS);
     }
 
     public boolean isAlignedToAmp() {

diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java
@@ -54,6 +54,8 @@ public static final class DriveConstants {
 
         public static final double MAX_TELEOP_SPEED_METERS_PER_SECOND = 4.7;
 
+        public static final double PASS_ROTATION_DEADBAND = 10;
+
         // Chassis configuration
         // Distance between centers of right and left wheels on robot
         public static final double TRACK_WIDTH = Units.inchesToMeters(25.5);
@@ -140,8 +142,10 @@ public static final class ShooterConstants {
 
         public static final double SHOOTER_BACK_SPEED = -0.5;
 
-        public static final double PIVOT_DEADBAND = 1;
+        public static final double PIVOT_DEADBAND_DEGREES = 1;
+        public static final double PIVOT_PASS_DEADBAND_DEGREES = 4;
         public static final double SHOOTER_RPM_DEADBAND = 50;
+        public static final double SHOOTER_PASS_RPM_DEADBAND = 150;
 
         public static final double PIVOT_LOWER_LIMIT_DEGREES = 21;
         public static final double PIVOT_UPPER_LIMIT_DEGREES = 59;
@@ -333,7 +337,8 @@ public static final class AutoConstants {
         public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED = Units.degreesToRadians(792.90);
 
         public static final double AUTO_POSITION_TOLERANCE_METERS = Units.inchesToMeters(1);
-        public static final double AUTO_POSITION_TOLERANCE_RADIANS = Units.degreesToRadians(3);
+        public static final double AUTO_ROTATION_TOLERANCE_RADIANS = Units.degreesToRadians(3);
+        public static final double PASS_ROTATION_TOLERANCE_RADIANS = Units.degreesToRadians(10);
 
         public static final double AUTO_ALIGNMENT_DEADBAND = Units.inchesToMeters(3);
 

diff --git a/src/main/java/frc/robot/util/calc/AlignmentCalc.java b/src/main/java/frc/robot/util/calc/AlignmentCalc.java
@@ -177,7 +177,7 @@ public ChassisSpeeds getSpeakerRotationalSpeeds(double driverX, double driverY,
 
     public ChassisSpeeds getPassRotationalSpeeds(double driverX, double driverY, ShooterCmds shooterCmds) {
         return 
-            getRotationalSpeeds(driverX, driverY, shooterCmds.shooterCalc.calculateRobotAngleToPass(swerve.getPose()));
+            getRotationalSpeeds(driverX, driverY, shooterCmds.shooterCalc.calculateRobotAngleToPass(swerve.getPose(), swerve.getRobotRelativeVelocity()));
     }
 
     /**
@@ -277,19 +277,4 @@ public Supplier<ChassisSpeeds> getTrapControllerSpeedsSupplier(DoubleSupplier dr
         return () -> getTrapControllerSpeeds(driverX.getAsDouble(), driverY.getAsDouble());
     }
 
-    /**
-     * Detects if the robot is on the opposite side of the field.
-     * Uses the robot's x position to determine if it has crossed the centerline.
-     * 
-     * @return true if the robot is on the opposite side of the field
-     */
-    public boolean onOppositeSide() {
-        return Robot.isRedAlliance() 
-            ? swerve.getPose().getX() < FieldConstants.BLUE_WING_X 
-            : swerve.getPose().getX() > FieldConstants.RED_WING_X;
-    }
-
-    public boolean closeToSpeaker() {
-        return RobotContainer.distanceToSpeakerMeters < 7.3;
-    }
 }
diff --git a/src/main/java/frc/robot/util/calc/PoseCalculations.java b/src/main/java/frc/robot/util/calc/PoseCalculations.java
@@ -3,6 +3,8 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.Robot;
+import frc.robot.RobotContainer;
 import frc.robot.util.Constants.ClimbConstants;
 import frc.robot.util.Constants.FieldConstants;
 import monologue.Logged;
@@ -91,4 +93,20 @@ private static double getChainIntercept(double x) {
         return MathUtil.clamp(calculation, 0.725, ClimbConstants.EXTENSION_LIMIT_METERS);
     }
 
+    /**
+     * Detects if the robot is on the opposite side of the field.
+     * Uses the robot's x position to determine if it has crossed the centerline.
+     * 
+     * @return true if the robot is on the opposite side of the field
+     */
+    public static boolean onOppositeSide(double xPosition) {
+        return Robot.isRedAlliance() 
+            ? xPosition < FieldConstants.BLUE_WING_X 
+            : xPosition > FieldConstants.RED_WING_X;
+    }
+
+    // Note: this method uses a static variable rather than a parameter
+    public static boolean closeToSpeaker() {
+        return RobotContainer.distanceToSpeakerMeters < 7.3;
+    }
 }
diff --git a/src/main/java/frc/robot/util/calc/ShooterCalc.java b/src/main/java/frc/robot/util/calc/ShooterCalc.java
@@ -150,6 +150,13 @@ public BooleanSupplier readyToShootSupplier() {
                 && shooter.getAverageTargetSpeed() != ShooterConstants.DEFAULT_RPM;
     }
 
+    public BooleanSupplier readyToPassSupplier() {
+        return () ->
+            pivot.getAtDesiredPassAngle()
+                && shooter.getAtDesiredPassRPM()
+                && shooter.getAverageTargetSpeed() > 0;
+    }
+
     public SpeedAngleTriplet calculateSpeakerTriplet(Translation2d robotPose) {
         // Get our position relative to the desired field element
         // Use the distance as our key for interpolation