Merge pull request #143 from Robocubs/patches

Fixed shoot while move tolerance and moved passing point
Robocubs · Apr 13, 2024 · d21c3e0 · d21c3e0
2 parents 3aef8be + dbf1ea2
commit d21c3e0
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 4 deletions.
diff --git 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
@@ -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
@@ -212,10 +212,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()