Merge pull request #55 from flamingchickens1540/zhangal/limelightUpdate

Update LimelightLib, use better latency compensation for gyro measurements
flamingchickens1540 · Sep 27, 2024 · 89b2806 · 89b2806
2 parents fd8cebf + ae7ba1a
commit 89b2806
Show file tree

Hide file tree

Showing 8 changed files with 377 additions and 94 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
@@ -106,9 +106,6 @@ public void robotInit() {
         PathfindingCommand.warmupCommand().schedule();
 
         AprilTagsCrescendo.getInstance().getTag(1);
-
-        // Init driver cam
-        LimelightHelpers.setCameraMode_Driver("limelight-driver");
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -42,7 +42,6 @@
 
 import java.util.function.BooleanSupplier;
 
-import static org.team1540.robot2024.Constants.SwerveConfig;
 import static org.team1540.robot2024.Constants.isTuningMode;
 
 public class RobotContainer {
@@ -62,6 +61,8 @@ public class RobotContainer {
 
     public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
 
+
+
     public boolean isBrakeMode;
     /**
      * The container for the robot. Contains subsystems, IO devices, and commands.
@@ -80,7 +81,8 @@ public RobotContainer() {
                     aprilTagVision = AprilTagVision.createReal(
                             drivetrain::addVisionMeasurement,
                             elevator::getPosition,
-                            drivetrain::getRotation);
+                            drivetrain::getRotation,
+                            drivetrain::getAngularVelocityRadPerSec);
                     noteVision = NoteVision.createReal();
                 } else {
                     elevator = Elevator.createDummy();

diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
@@ -1,7 +1,6 @@
 package org.team1540.robot2024.subsystems.drive;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.commands.FollowPathHolonomic;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.pathfinding.Pathfinding;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
@@ -192,7 +191,6 @@ public void periodic() {
         // Update odometry
         poseEstimator.update(rawGyroRotation, getModulePositions());
         visionPoseEstimator.update(rawGyroRotation, getModulePositions());
-
     }
 
     /**
@@ -285,6 +283,7 @@ public double getCharacterizationVelocity() {
         }
         return driveVelocityAverage / 4.0;
     }
+
     @AutoLogOutput(key = "Odometry/ChassisSpeeds")
     public ChassisSpeeds getChassisSpeeds() {
         return kinematics.toChassisSpeeds(getModuleStates());
@@ -331,6 +330,10 @@ public Rotation2d getRawGyroRotation() {
         return rawGyroRotation;
     }
 
+    public double getAngularVelocityRadPerSec() {
+        return gyroInputs.yawVelocityRadPerSec;
+    }
+
     public void zeroFieldOrientationManual() {
         fieldOrientationOffset = rawGyroRotation;
     }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVision.java
@@ -12,6 +12,7 @@
 import org.team1540.robot2024.util.vision.EstimatedVisionPose;
 
 import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
 import static org.team1540.robot2024.Constants.Vision.AprilTag.*;
@@ -47,13 +48,15 @@ private AprilTagVision(
 
     public static AprilTagVision createReal(Consumer<EstimatedVisionPose> visionPoseConsumer,
                                             Supplier<Double> elevatorHeightSupplierMeters,
-                                            Supplier<Rotation2d> headingSupplier) {
+                                            Supplier<Rotation2d> headingSupplier,
+                                            DoubleSupplier headingVelocitySupplierRadPerSec) {
         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 AprilTagVisionIOMegaTag2(REAR_CAMERA_NAME, REAR_CAMERA_POSE, headingSupplier),
+                new AprilTagVisionIOMegaTag2(
+                        REAR_CAMERA_NAME, REAR_CAMERA_POSE, headingSupplier, headingVelocitySupplierRadPerSec),
                 visionPoseConsumer,
                 elevatorHeightSupplierMeters);
     }

diff --git a/...ain/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOLimelight.java b/...ain/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOLimelight.java
@@ -10,7 +10,6 @@ public class AprilTagVisionIOLimelight implements AprilTagVisionIO {
 
     public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) {
         this.name = name;
-        LimelightHelpers.setCameraMode_Processor(name);
         LimelightHelpers.setLEDMode_PipelineControl(name);
         setPoseOffset(cameraOffsetMeters);
     }

diff --git a/...main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java b/...main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java
@@ -6,23 +6,32 @@
 import org.team1540.robot2024.util.vision.LimelightHelpers;
 
 import java.util.Arrays;
+import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
 public class AprilTagVisionIOMegaTag2 implements AprilTagVisionIO {
     private final String name;
     private final Supplier<Rotation2d> heading;
+    private final DoubleSupplier headingVelocityRadPerSec;
 
-    public AprilTagVisionIOMegaTag2(String name, Pose3d cameraOffsetMeters, Supplier<Rotation2d> heading) {
+    public AprilTagVisionIOMegaTag2(String name,
+                                    Pose3d cameraOffsetMeters,
+                                    Supplier<Rotation2d> heading,
+                                    DoubleSupplier headingVelocityRadPerSec) {
         this.name = name;
         this.heading = heading;
-        LimelightHelpers.setCameraMode_Processor(name);
+        this.headingVelocityRadPerSec = headingVelocityRadPerSec;
         LimelightHelpers.setLEDMode_PipelineControl(name);
         setPoseOffset(cameraOffsetMeters);
     }
 
     @Override
     public void updateInputs(AprilTagVisionIOInputs inputs) {
-        LimelightHelpers.SetRobotOrientation(name, heading.get().getDegrees(), 0, 0, 0, 0, 0);
+        LimelightHelpers.SetRobotOrientation(
+                name,
+                heading.get().getDegrees(),
+                Math.toDegrees(headingVelocityRadPerSec.getAsDouble()),
+                0, 0, 0, 0);
         LimelightHelpers.PoseEstimate measurement =
                 DriverStation.isDisabled()
                         ? LimelightHelpers.getBotPoseEstimate_wpiBlue(name)

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/gamepiece/NoteVisionIOLimelight.java
@@ -10,7 +10,6 @@ public class NoteVisionIOLimelight implements NoteVisionIO {
 
     public NoteVisionIOLimelight(String name) {
         this.name = name;
-        LimelightHelpers.setCameraMode_Processor(name);
         LimelightHelpers.setLEDMode_PipelineControl(name);
         LimelightHelpers.setPipelineIndex(name, PIPELINE_INDEX);
     }