Merge branch 'pre-gkc' of https://github.com/FRCTeam1987/Robot2024 in…

…to pre-gkc
FRCTeam1987 · Mar 29, 2024 · 3e68e26 · 3e68e26
2 parents d6f7fa6 + 29f4207
commit 3e68e26
Show file tree

Hide file tree

Showing 8 changed files with 131 additions and 36 deletions.
diff --git a/build.gradle b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.3.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.2"
     id 'com.diffplug.spotless' version '6.25.0'
 }
 

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.constants.Constants;
 
 public class Robot extends TimedRobot {
   private Command autoCommand;
@@ -28,7 +29,7 @@ public void robotInit() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
-    robotContainer.updatePoseVision();
+    robotContainer.updatePoseVision(Constants.Vision.SPEAKER_LIMELIGHT);
   }
 
   @Override

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -524,41 +525,57 @@ public Pose2d getPose() {
     return DRIVETRAIN.getPose();
   }
 
-  public void updatePoseVision() {
+  // Reference FRC 6391
+  // https://github.com/6391-Ursuline-Bearbotics/2024-6391-Crescendo/blob/4759dfd37c960cf3493b0eafd901519c5c36b239/src/main/java/frc/robot/Vision/Limelight.java#L44-L88
+  public void updatePoseVision(final String limelight) {
     ChassisSpeeds currentSpeed = DRIVETRAIN.getCurrentRobotChassisSpeeds();
-    if (Math.abs(currentSpeed.vxMetersPerSecond) > 1.5
-        || Math.abs(currentSpeed.vyMetersPerSecond) > 1.5) {
+    // Reject pose updates when moving fast.
+    if (Math.abs(currentSpeed.vxMetersPerSecond) > 2.0
+        || Math.abs(currentSpeed.vyMetersPerSecond) > 2.0
+        || Math.abs(currentSpeed.omegaRadiansPerSecond) > Math.PI) {
       return;
     }
-    Limelight.PoseEstimate pose = Limelight.getBotPoseEstimate_wpiBlue(SPEAKER_LIMELIGHT);
-    // TODO throw away any botpose that is far away from previous pose
-    // Sometimes teleports when the ll gets confused
-    // Usually at a longer distance, could filter out long distance from tags
-    if (pose.tagCount >= 2) {
+    Limelight.PoseEstimate botPose = Limelight.getBotPoseEstimate_wpiBlue(limelight);
+
+    // Reject an empty pose.
+    if (botPose.tagCount < 1) {
+      return;
+    }
+
+    // Reject a pose outside of the field.
+    if (!Constants.Vision.fieldBoundary.isPoseWithinArea(botPose.pose)) {
+      return;
+    }
+    // Reject pose from long disance or high ambiguity.
+    if ((botPose.tagCount == 1
+            && (botPose.avgTagDist > Constants.Vision.maxSingleTagDistanceToAccept
+                || botPose.rawFiducials[0].ambiguity >= 0.9))
+        || (botPose.tagCount >= 2
+            && botPose.avgTagDist > Constants.Vision.maxMutiTagDistToAccept)) {
+      return;
+    }
+    // Trust close multi tag pose when disabled with increased confidence.
+    if (DriverStation.isDisabled()
+        && botPose.tagCount >= 2
+        && botPose.avgTagDist < Constants.Vision.maxTagDistToTrust) {
       DRIVETRAIN.addVisionMeasurement(
-          pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, .7));
+          botPose.pose, botPose.timestampSeconds, Constants.Vision.absoluteTrustVector);
+      return;
+    }
+    final double botPoseToPoseDistance =
+        botPose.pose.getTranslation().getDistance(DRIVETRAIN.getPose().getTranslation());
+    // Reject a pose that is far away from the current robot pose.
+    if (botPoseToPoseDistance > 0.5) {
+      return;
+    }
+    double tagDistanceFeet = Units.metersToFeet(botPose.avgTagDist);
+    // Have a lower confidence with single tag pose proportionate to distance.
+    if (botPose.tagCount == 1) {
+      tagDistanceFeet *= 2;
     }
-    // Limelight.PoseEstimate poseAmp = Limelight.getBotPoseEstimate_wpiBlue(AMP_LIMELIGHT);
-    // if (poseAmp.tagCount >= 2) {
-    //   DRIVETRAIN.addVisionMeasurement(
-    //       poseAmp.pose, poseAmp.timestampSeconds, VecBuilder.fill(.9, .9, .9));
-    // }
-    // Limelight.PoseEstimate poseRight = Limelight.getBotPoseEstimate_wpiBlue(RIGHT_LIMEKIGHT);
-    // if (poseRight.tagCount >= 2) {
-    //   DRIVETRAIN.addVisionMeasurement(
-    //       poseRight.pose, poseRight.timestampSeconds, VecBuilder.fill(1.5, 1.5,1.5));
-    // } else {
-    //   if (pose.rawFiducials.length > 0 && pose.rawFiducials[0].ambiguity < 0.07) {
-    //     DriverStation.reportWarning(
-    //         "ADDING POSE BASED ON AMBIGUITY OF "
-    //             + pose.rawFiducials[0].ambiguity
-    //             + " ON TAG "
-    //             + pose.rawFiducials[0].id,
-    //         false);
-    //     DRIVETRAIN.addVisionMeasurement(
-    //         pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, 9999999));
-    //   }
-    // }
+    double confidence = 0.7 + (tagDistanceFeet / 100);
+    DRIVETRAIN.addVisionMeasurement(
+        botPose.pose, botPose.timestampSeconds, VecBuilder.fill(confidence, confidence, 99));
   }
 
   public static RobotContainer get() {

diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
@@ -1,7 +1,13 @@
 package frc.robot.constants;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
 import frc.robot.util.InterpolatingDouble;
 import frc.robot.util.InterpolatingTreeMap;
+import frc.robot.util.RectanglePoseArea;
 
 public class Constants {
 
@@ -297,4 +303,14 @@ public static class Climber {
     // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put(
     //     new InterpolatingDouble(-14.3), new InterpolatingDouble(28.5)); //
   }
+
+  public static class Vision {
+    public static final String SPEAKER_LIMELIGHT = "limelight-speaker";
+    public static final RectanglePoseArea fieldBoundary =
+        new RectanglePoseArea(new Translation2d(0, 0), new Translation2d(16.541, 8.211));
+    public static final double maxMutiTagDistToAccept = Units.feetToMeters(15.0);
+    public static final double maxTagDistToTrust = Units.feetToMeters(10.0);
+    public static final double maxSingleTagDistanceToAccept = Units.feetToMeters(10.0);
+    public static final Vector<N3> absoluteTrustVector = VecBuilder.fill(.2, .2, 99);
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
@@ -153,7 +153,8 @@ public Optional<Rotation2d> getRotationTargetOverride() {
       // drivetrain.setChassisSpeeds(HOLO_CONTROLLER.calculate(current, new
       // Pose2d(current.getTranslation(), new Rotation2d(desiredRotation)), 0, new
       // Rotation2d(desiredRotation)));
-      return Optional.of(new Rotation2d(desiredRotation + 90));
+      // return Optional.of(new Rotation2d(desiredRotation + 90));
+      return Optional.of(Rotation2d.fromDegrees(desiredRotation + 90.0));
     } else {
       // return an empty optional when we don't want to override the path's rotation
       return Optional.empty();

diff --git a/src/main/java/frc/robot/util/RectanglePoseArea.java b/src/main/java/frc/robot/util/RectanglePoseArea.java
@@ -0,0 +1,51 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+public class RectanglePoseArea {
+  private final Translation2d bottomLeft;
+  private final Translation2d topRight;
+
+  /**
+   * Create a 2D rectangular area for pose calculations.
+   *
+   * @param bottomLeft bottom left corner of the rectangle.
+   * @param topRight top right corner of the rectangle.
+   */
+  public RectanglePoseArea(Translation2d bottomLeft, Translation2d topRight) {
+    this.bottomLeft = bottomLeft;
+    this.topRight = topRight;
+  }
+
+  public double getMinX() {
+    return bottomLeft.getX();
+  }
+
+  public double getMaxX() {
+    return topRight.getX();
+  }
+
+  public double getMinY() {
+    return bottomLeft.getY();
+  }
+
+  public double getMaxY() {
+    return topRight.getY();
+  }
+
+  public Translation2d getBottomLeftPoint() {
+    return bottomLeft;
+  }
+
+  public Translation2d getTopRightPoint() {
+    return topRight;
+  }
+
+  public boolean isPoseWithinArea(Pose2d pose) {
+    return pose.getX() >= bottomLeft.getX()
+        && pose.getX() <= topRight.getX()
+        && pose.getY() >= bottomLeft.getY()
+        && pose.getY() <= topRight.getY();
+  }
+}
diff --git a/src/main/java/frc/robot/util/Util.java b/src/main/java/frc/robot/util/Util.java
@@ -23,6 +23,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotContainer;
 import frc.robot.constants.Constants;
+import frc.robot.util.Limelight.RawFiducial;
 import java.util.List;
 
 public class Util {
@@ -203,4 +204,12 @@ public static Pose2d findNearestPose(Pose2d currentPose, Pose2d... otherPoses) {
   public static Pose2d findNearestPoseToTrapClimbs(Pose2d currentPose) {
     return currentPose.nearest(TRAP_TAGS);
   }
+
+  public static double maxFiducialAmbiguity(final RawFiducial[] fiducials) {
+    double maxAmbiguity = 0.0;
+    for (RawFiducial fiducial : fiducials) {
+      maxAmbiguity = Math.max(maxAmbiguity, fiducial.ambiguity);
+    }
+    return maxAmbiguity;
+  }
 }
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.2.7",
+    "version": "2024.2.8",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,15 +12,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.2.7"
+            "version": "2024.2.8"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.2.7",
+            "version": "2024.2.8",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,