feat: initial mt2 code

flamingchickens1540 · Apr 4, 2024 · 1065c06 · 1065c06
1 parent b05021e
commit 1065c06
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 7 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -111,7 +111,7 @@ public static class Vision {
         public static final String REAR_CAMERA_NAME = "limelight-rear";
 
         public static final Pose3d FRONT_CAMERA_POSE = new Pose3d(0.086018, 0, 0.627079, new Rotation3d(0, Math.toRadians(-40.843), 0));
-        public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(0, Math.toRadians(10), Math.PI));
+        public static final Pose3d REAR_CAMERA_POSE = new Pose3d(0.046049, 0, 0.540510, new Rotation3d(Math.PI, Math.toRadians(10), Math.PI));
 
         public static final boolean TAKE_SNAPSHOTS = true;
         public static final double SNAPSHOT_PERIOD_SECS = 1;

diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -74,7 +74,8 @@ public RobotContainer() {
                     indexer = Indexer.createReal();
                     aprilTagVision = AprilTagVision.createReal(
                             drivetrain::addVisionMeasurement,
-                            elevator::getPosition);
+                            elevator::getPosition,
+                            drivetrain::getRotation);
                 } else {
                     elevator = Elevator.createDummy();
                     drivetrain = Drivetrain.createReal(odometrySignalRefresher, () -> 0.0);

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.Timer;
@@ -45,13 +46,14 @@ private AprilTagVision(
     }
 
     public static AprilTagVision createReal(Consumer<EstimatedVisionPose> visionPoseConsumer,
-                                            Supplier<Double> elevatorHeightSupplierMeters) {
+                                            Supplier<Double> elevatorHeightSupplierMeters,
+                                            Supplier<Rotation2d> headingSupplier) {
         if (Constants.currentMode != Constants.Mode.REAL) {
             DriverStation.reportWarning("Using real vision on simulated robot", false);
         }
         return new AprilTagVision(
                 new AprilTagVisionIOPhoton(FRONT_CAMERA_NAME, FRONT_CAMERA_POSE),
-                new AprilTagVisionIOLimelight(REAR_CAMERA_NAME, REAR_CAMERA_POSE),
+                new AprilTagVisionIOLimelight(REAR_CAMERA_NAME, REAR_CAMERA_POSE, headingSupplier),
                 visionPoseConsumer,
                 elevatorHeightSupplierMeters);
     }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java
@@ -1,23 +1,29 @@
 package org.team1540.robot2024.subsystems.vision;
 
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import org.team1540.robot2024.util.vision.LimelightHelpers;
 
+import java.util.function.Supplier;
+
 public class AprilTagVisionIOLimelight implements AprilTagVisionIO {
     private final String name;
+    private final Supplier<Rotation2d> heading;
 
-    public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) {
+    public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters, Supplier<Rotation2d> heading) {
         this.name = name;
+        this.heading = heading;
         LimelightHelpers.setCameraMode_Processor(name);
         LimelightHelpers.setLEDMode_PipelineControl(name);
         setPoseOffset(cameraOffsetMeters);
     }
 
     @Override
     public void updateInputs(AprilTagVisionIOInputs inputs) {
-        LimelightHelpers.PoseEstimate measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(name);
+        LimelightHelpers.SetRobotOrientation(name, heading.get().getDegrees(), 0, 0, 0, 0, 0);
+        LimelightHelpers.PoseEstimate measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name);
         if (measurement.tagCount > 1) {
-            Pose3d limelightPose = LimelightHelpers.getBotPose3d_wpiBlue(name);
+            Pose3d limelightPose = new Pose3d(measurement.pose);
             inputs.estimatedPoseMeters = new Pose3d(
                     limelightPose.getX(),
                     limelightPose.getY() + 0.105,