From 6d6e582a636dd02e1ef9422f3e33e4513620b6ca Mon Sep 17 00:00:00 2001 From: Gold87 Date: Sun, 31 Mar 2024 17:14:45 -0400 Subject: [PATCH] Removed duplicate implementations of isFacingSpeaker() --- src/main/java/frc/robot/Constants.java | 1 + .../frc/robot/commands/LookAheadSOTM.java | 26 ++------------- .../frc/robot/commands/ShootWhileMoving.java | 25 ++------------ .../frc/robot/util/AutoShootMathUtil.java | 33 +++++++++++++++++++ 4 files changed, 39 insertions(+), 46 deletions(-) create mode 100644 src/main/java/frc/robot/util/AutoShootMathUtil.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4a5083a..826bd4d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -355,6 +355,7 @@ public static class ShooterConstants { } public static final class AutoShootConstants { + public static final double speakerEdgeTolerance = Units.inchesToMeters(2.0); // (distance, angle) // This isn't being used, but the data could still be important at some point public static final Point2D[] autoShootArmAngles = diff --git a/src/main/java/frc/robot/commands/LookAheadSOTM.java b/src/main/java/frc/robot/commands/LookAheadSOTM.java index d796602..c7fcb17 100644 --- a/src/main/java/frc/robot/commands/LookAheadSOTM.java +++ b/src/main/java/frc/robot/commands/LookAheadSOTM.java @@ -19,13 +19,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.AutoShootConstants; -import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Swerve; import frc.robot.util.AllianceUtil; +import frc.robot.util.AutoShootMathUtil; import frc.robot.util.MovingShotData; import frc.robot.util.ShooterState; import java.util.function.DoubleSupplier; @@ -137,27 +137,6 @@ public void initialize() { recalculateSetpoint = true; } - private boolean isFacingSpeaker(Pose2d robotPose, Translation2d virtualGoalLocation) { - if (Math.abs(robotPose.getRotation().getCos()) == 0.0) { - return false; - } - // y = m(goalx) + b - // b = roboty - m(robotx) - double slope = Math.tan(robotPose.getRotation().getRadians()); - if (Double.isInfinite(slope) || Double.isNaN(slope)) { - return false; - } - - double b = robotPose.getY() - slope * robotPose.getX(); - - double yIntersect = slope * virtualGoalLocation.getX() + b; - - double upperBound = virtualGoalLocation.getY() + FieldConstants.speakerWidth / 2; - double lowerBound = virtualGoalLocation.getY() - FieldConstants.speakerWidth / 2; - - return yIntersect >= lowerBound && yIntersect <= upperBound; - } - private MovingShotData updateCurrentState( MovingShotData currentState, Pose2d robotPose, @@ -300,7 +279,8 @@ public void execute() { SmartDashboard.putNumber("Auto Shoot/Drive Angle Error", driveAngleError.getDegrees()); - boolean facingSpeaker = isFacingSpeaker(robotPose, currentState.virtualGoalLocation); + boolean facingSpeaker = + AutoShootMathUtil.isFacingSpeaker(robotPose, currentState.virtualGoalLocation); SmartDashboard.putBoolean("Auto Shoot/Facing Speaker", facingSpeaker); diff --git a/src/main/java/frc/robot/commands/ShootWhileMoving.java b/src/main/java/frc/robot/commands/ShootWhileMoving.java index 79dee0a..f59972f 100644 --- a/src/main/java/frc/robot/commands/ShootWhileMoving.java +++ b/src/main/java/frc/robot/commands/ShootWhileMoving.java @@ -19,13 +19,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.AutoShootConstants; -import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Swerve; import frc.robot.util.AllianceUtil; +import frc.robot.util.AutoShootMathUtil; import frc.robot.util.ShooterState; import java.util.function.DoubleSupplier; @@ -125,27 +125,6 @@ public void initialize() { swerve.setIgnoreArducam(true); } - private boolean isFacingSpeaker(Pose2d robotPose, Translation2d virtualGoalLocation) { - if (Math.abs(robotPose.getRotation().getCos()) == 0.0) { - return false; - } - // y = m(goalx) + b - // b = roboty - m(robotx) - double slope = Math.tan(robotPose.getRotation().getRadians()); - if (Double.isInfinite(slope) || Double.isNaN(slope)) { - return false; - } - - double b = robotPose.getY() - slope * robotPose.getX(); - - double yIntersect = slope * virtualGoalLocation.getX() + b; - - double upperBound = virtualGoalLocation.getY() + FieldConstants.speakerWidth / 2; - double lowerBound = virtualGoalLocation.getY() - FieldConstants.speakerWidth / 2; - - return yIntersect >= lowerBound && yIntersect <= upperBound; - } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { @@ -256,7 +235,7 @@ public void execute() { SmartDashboard.putNumber("Auto Shoot/Drive Angle Error", driveAngleError.getDegrees()); - boolean facingSpeaker = isFacingSpeaker(robotPose, virtualGoalLocation); + boolean facingSpeaker = AutoShootMathUtil.isFacingSpeaker(robotPose, virtualGoalLocation); SmartDashboard.putBoolean("Auto Shoot/Facing Speaker", facingSpeaker); diff --git a/src/main/java/frc/robot/util/AutoShootMathUtil.java b/src/main/java/frc/robot/util/AutoShootMathUtil.java new file mode 100644 index 0000000..70dce18 --- /dev/null +++ b/src/main/java/frc/robot/util/AutoShootMathUtil.java @@ -0,0 +1,33 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.Constants.AutoShootConstants; +import frc.robot.Constants.FieldConstants; + +public class AutoShootMathUtil { + public static boolean isFacingSpeaker(Pose2d robotPose, Translation2d virtualGoalLocation) { + if (Math.abs(robotPose.getRotation().getCos()) == 0.0) { + return false; + } + // y = m(goalx) + b + // b = roboty - m(robotx) + double slope = Math.tan(robotPose.getRotation().getRadians()); + if (Double.isInfinite(slope) || Double.isNaN(slope)) { + return false; + } + + double b = robotPose.getY() - slope * robotPose.getX(); + + double yIntersect = slope * virtualGoalLocation.getX() + b; + + double upperBound = + virtualGoalLocation.getY() + + (FieldConstants.speakerWidth - AutoShootConstants.speakerEdgeTolerance) / 2; + double lowerBound = + virtualGoalLocation.getY() + - (FieldConstants.speakerWidth - AutoShootConstants.speakerEdgeTolerance) / 2; + + return yIntersect >= lowerBound && yIntersect <= upperBound; + } +}