changes

Team841 · Nov 9, 2024 · 1a67554 · 1a67554
1 parent 4be4f1b
commit 1a67554
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 9 deletions.
diff --git 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
@@ -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);