limelight done

src/main/java/frc/robot/RobotContainer.java

@@ -25,6 +25,7 @@
 import frc.robot.commands.PivotCommands;
 import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.intake.*;
+import frc.robot.subsystems.limelight.*;
 import frc.robot.subsystems.pivot.*;
 import frc.robot.subsystems.shooter.*;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
@@ -41,6 +42,7 @@ public class RobotContainer {
   private final Intake intake;
   private final Pivot pivot;
   private final Shooter shooter;
+  private final Limelight limelight;
 
   // Controller
   private final GenericHID driver = new GenericHID(0); // hotas
@@ -64,6 +66,8 @@ public RobotContainer() {
         intake = new Intake(new IntakeIOReal());
         pivot = new Pivot(new PivotIOReal());
         shooter = new Shooter(new ShooterIOReal());
+        limelight = new Limelight(new LimelightIOReal(), drive);
+
 
         break;
 
@@ -80,6 +84,7 @@ public RobotContainer() {
         intake = new Intake(new IntakeIOSim());
         pivot = new Pivot(new PivotIOSim());
         shooter = new Shooter(new ShooterIOSim());
+        limelight = new Limelight(new LimelightIO() {}, drive);
 
         break;
 
@@ -96,6 +101,7 @@ public RobotContainer() {
         intake = new Intake(new IntakeIO() {});
         pivot = new Pivot(new PivotIO() {});
         shooter = new Shooter(new ShooterIO() {});
+        limelight = new Limelight(new LimelightIO() {}, drive);
 
         break;
     }
@@ -130,16 +136,6 @@ private void configureButtonBindings() {
     var stopWithXButton = new Trigger(() -> driver.getRawButton(2));
     stopWithXButton.onTrue(Commands.runOnce(drive::stopWithX, drive));
 
-    // driver
-    //     .b()
-    //     .onTrue(
-    //         Commands.runOnce(
-    //                 () ->
-    //                     drive.setPose(
-    //                         new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
-    //                 drive)
-    //             .ignoringDisable(true));
-
     operator.a().toggleOnTrue(intake.onCommand());
     operator.b().toggleOnTrue(intake.offCommand());
     operator.rightBumper().toggleOnTrue(shooter.shootersOnCommand());

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -18,6 +18,8 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
+
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -238,6 +240,10 @@ public SwerveModulePosition[] getModulePositions() {
     return positions;
   }
 
+  public void addVisionMeasurement(double xyStds, double degStds, Pose2d pose, double latency) {
+    poseEstimator.addVisionMeasurement(pose, latency, VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds)));
+  }
+
   /** Returns an array of module translations. */
   public static Translation2d[] getModuleTranslations() {
     return new Translation2d[] {

src/main/java/frc/robot/subsystems/limelight/Limelight.java

@@ -1 +1,47 @@
 package frc.robot.subsystems.limelight;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.drive.Drive;
+
+public class Limelight extends SubsystemBase {
+  LimelightIO io;
+  Drive drive;
+  LimelightIOInputsAutoLogged inputs = new LimelightIOInputsAutoLogged();
+  public Limelight(LimelightIO io, Drive drive) {
+    this.io = io;
+    this.drive = drive;
+  }
+
+  public void periodic() {
+    io.updateInputs(inputs);
+
+    if (inputs.pose.getX() == 0) {
+      return;
+    }
+
+    if (inputs.tagCount > 0) {
+      double poseDifference = drive.getPose().getTranslation().getDistance(inputs.pose);
+      double xyStds, degStds;
+
+      // copied from limelight docs
+      // https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-robot-localization
+      if (inputs.tagCount >= 2) {
+        xyStds = 0.5;
+        degStds = 6;
+      }
+      else if (inputs.averageTagArea > .4 && poseDifference < 0.5) {
+        xyStds = 1.0;
+        degStds = 12;
+      } else if (inputs.averageTagArea > .05 && poseDifference < .5) {
+        xyStds = 2.0;
+        degStds = 30;
+      } else {
+        return;
+      }
+
+      drive.addVisionMeasurement(xyStds, degStds, new Pose2d(inputs.pose, inputs.yaw), inputs.latency);
+    }
+
+  }
+}

src/main/java/frc/robot/subsystems/limelight/LimelightIO.java

@@ -1,19 +1,18 @@
 package frc.robot.subsystems.limelight;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import org.littletonrobotics.junction.AutoLog;
 
-import edu.wpi.first.math.geometry.Translation3d;
-
 public interface LimelightIO {
   @AutoLog
   public static class LimelightIOInputs {
     public double latency = 0.0;
 
-    public Translation3d pose = new Translation3d(); 
-    public double roll = 0.0, pitch = 0.0, yaw = 0.0;
+    public Translation2d pose = new Translation2d();
+    public Rotation2d roll = new Rotation2d(), pitch = new Rotation2d(), yaw = new Rotation2d();
     public double tagCount = 0.0, tagSpan = 0.0, averageTagDistance = 0.0, averageTagArea = 0.0;
   }
 
-  public default void updateInputs(LimelightIOInputs inputs) {
-  }
+  public default void updateInputs(LimelightIOInputs inputs) {}
 }

src/main/java/frc/robot/subsystems/limelight/LimelightIOReal.java

@@ -1,5 +1,7 @@
 package frc.robot.subsystems.limelight;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 
@@ -11,7 +13,21 @@ public LimelightIOReal() {
   }
 
   public void updateInputs(LimelightIOInputs inputs) {
-    var vals = botpose.getDoubleArray(new double[] {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,});
-  }
+    var vals =
+        botpose.getDoubleArray(
+            new double[] {
+              0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+            });
 
+    // ignore z value
+    inputs.pose = new Translation2d(vals[0], vals[1]);
+    inputs.roll = Rotation2d.fromDegrees(vals[3]);
+    inputs.pitch = Rotation2d.fromDegrees( vals[4]);
+    inputs.yaw = Rotation2d.fromDegrees( vals[5]);
+    inputs.latency = vals[6];
+    inputs.tagCount = vals[7];
+    inputs.tagSpan = vals[8];
+    inputs.averageTagDistance = vals[9];
+    inputs.averageTagArea = vals[10];
+  }
 }