From dbf1ea24e15e52d5dfcb5f37247d0c278f53afe1 Mon Sep 17 00:00:00 2001 From: Marc Pulte Date: Sat, 13 Apr 2024 11:26:49 -0400 Subject: [PATCH] Fixed shoot while move tolerance and moved passing point --- src/main/java/com/team1701/lib/util/GeometryUtil.java | 7 +++++++ src/main/java/com/team1701/robot/FieldConstants.java | 2 +- .../java/com/team1701/robot/commands/ShootAndMove.java | 5 ++--- 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/team1701/lib/util/GeometryUtil.java b/src/main/java/com/team1701/lib/util/GeometryUtil.java index c8507b7e..0d58f952 100644 --- a/src/main/java/com/team1701/lib/util/GeometryUtil.java +++ b/src/main/java/com/team1701/lib/util/GeometryUtil.java @@ -57,6 +57,13 @@ public static Rotation2d angleModulus(Rotation2d rotation, Rotation2d minimumInp MathUtil.inputModulus(rotation.getRadians(), minimumInput.getRadians(), maximumInput.getRadians())); } + public static boolean isBetween(Rotation2d actual, Rotation2d expected1, Rotation2d expected2) { + var maxDiff = Math.abs(MathUtil.angleModulus(expected1.getRadians() - expected2.getRadians())); + var diff1 = Math.abs(MathUtil.angleModulus(actual.getRadians() - expected1.getRadians())); + var diff2 = Math.abs(MathUtil.angleModulus(actual.getRadians() - expected2.getRadians())); + return diff1 < maxDiff && diff2 < maxDiff; + } + public static boolean isNear(Rotation2d expected, Rotation2d actual, Rotation2d tolerance) { var difference = MathUtil.angleModulus(expected.minus(actual).getRadians()); return MathUtil.isNear(difference, 0.0, tolerance.getRadians()); diff --git a/src/main/java/com/team1701/robot/FieldConstants.java b/src/main/java/com/team1701/robot/FieldConstants.java index 3023af1a..a22075fc 100644 --- a/src/main/java/com/team1701/robot/FieldConstants.java +++ b/src/main/java/com/team1701/robot/FieldConstants.java @@ -56,5 +56,5 @@ public final class FieldConstants { public static final Pose2d kBluePassingTarget = new Pose2d(new Translation2d(1.5, 6.5), GeometryUtil.kRotationIdentity); public static final Pose2d kRedPassingTarget = - new Pose2d(new Translation2d(kFieldLongLengthMeters - 1.5, 5.8), GeometryUtil.kRotationIdentity); + new Pose2d(new Translation2d(kFieldLongLengthMeters - 1.5, 6.5), GeometryUtil.kRotationIdentity); } diff --git a/src/main/java/com/team1701/robot/commands/ShootAndMove.java b/src/main/java/com/team1701/robot/commands/ShootAndMove.java index c3385607..0f287bb0 100644 --- a/src/main/java/com/team1701/robot/commands/ShootAndMove.java +++ b/src/main/java/com/team1701/robot/commands/ShootAndMove.java @@ -211,10 +211,9 @@ public void execute() { var atAngle = GeometryUtil.isNear( mShooter.getAngle(), mLastSetpoint.angle(), Rotation2d.fromRadians(kAngleToleranceRadians.get())); - var stationaryTargetHeading = FieldUtil.getHeadingToSpeaker(currentPose); - + var stationaryTargetHeading = FieldUtil.getHeadingToSpeaker(currentPose).minus(shooterSetpoint.releaseAngle()); var atHeading = GeometryUtil.isNear(mLastTargetHeading, mRobotState.getHeading(), headingTolerance) - || GeometryUtil.isNear(stationaryTargetHeading, mRobotState.getHeading(), headingTolerance); + || GeometryUtil.isBetween(mRobotState.getHeading(), mLastTargetHeading, stationaryTargetHeading); var atSpeed = mLastSetpoint .speeds()