From 1a675545af1f7de0a4fb237526081fc95611cb04 Mon Sep 17 00:00:00 2001
From: David Tian <dtian.github@outlook.com>
Date: Sat, 9 Nov 2024 09:55:51 -0800
Subject: [PATCH] changes

---
 src/main/deploy/pathplanner/autos/a1.auto     |  6 +++++
 .../team841/calliope/drive/Drivetrain.java    | 25 ++++++++++++-------
 2 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/a1.auto b/src/main/deploy/pathplanner/autos/a1.auto
index b8a79f2..cc3e625 100644
--- a/src/main/deploy/pathplanner/autos/a1.auto
+++ b/src/main/deploy/pathplanner/autos/a1.auto
@@ -11,6 +11,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 10.0
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/java/com/team841/calliope/drive/Drivetrain.java b/src/main/java/com/team841/calliope/drive/Drivetrain.java
index 2e5eaf6..7452f01 100644
--- a/src/main/java/com/team841/calliope/drive/Drivetrain.java
+++ b/src/main/java/com/team841/calliope/drive/Drivetrain.java
@@ -248,21 +248,28 @@ public boolean inRangeToSpeaker(){
         return distance > Swerve.disToRobot - Swerve.disToRobotError && distance < Swerve.disToRobot + Swerve.disToRobotError;
     }
 
+    void configureVision(LimelightHelpers.PoseEstimate PoseEstimate, String name){
+        if (PoseEstimate == null)
+                return;
+        else if (PoseEstimate.tagCount >= 2) {
+            this.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, Math.PI*2));
+            this.addVisionMeasurement(PoseEstimate.pose, PoseEstimate.timestampSeconds);
+            Logger.recordOutput("Drivetrain/" + name, PoseEstimate.pose);
+        }
+    }
+
     @Override
     public void periodic() {
-        /* 
+
         var PoseEstimate =
                 LimelightHelpers.getBotPoseEstimate_wpiBlue(Swerve.Vision.kLimelightFrontName);
-        if (PoseEstimate.tagCount >= 2) {
-            this.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, Math.PI));
-            this.addVisionMeasurement(PoseEstimate.pose, PoseEstimate.timestampSeconds);
-        }
+        configureVision(PoseEstimate, Swerve.Vision.kLimelightFrontName);
+        PoseEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-left");
+        configureVision(PoseEstimate, "limelight-left");
+        PoseEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-right");
+        configureVision(PoseEstimate, "limelight-right");
 
-        ctrePublisher.set(this.getState().Pose);
-        limelightPublisher.set(PoseEstimate.pose);
 
-        Logger.recordOutput("Drivetrain/ctrePose", this.getState().Pose);
-        Logger.recordOutput("Drivetrain/limelightPose", PoseEstimate.pose);
 
         /*
         SmartDashboard.putBoolean("2 tags", PoseEstimate.tagCount >= 2);