Merge pull request #522 from frc-862/520-limelight-update

[#520] - limelight update
frc-862 · Apr 4, 2024 · 78adfe5 · 78adfe5
2 parents 29491ce + e51418c
commit 78adfe5
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -369,6 +369,16 @@ public class VisionConstants {
         public static final Translation3d BLUE_SPEAKER_LOCATION = new Translation3d(0, 5.547593, 1.2);
         public static final Translation3d RED_SPEAKER_LOCATION = new Translation3d(16.4592, 5.547593, 1.2);
 
+        public static final int[] SPEAKER_FILTERS = {4, 7};
+        public static final int[] ALL_TAG_FILTERS  = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};
+
+
+        public static class LimelightOrientation {
+            public static final double STOPME_YAW = 180d;
+            public static final double STOPME_PITCH = 55.5;
+            public static final double STOPME_ROLL = 0d;
+        }
+
         public class Pipelines { // TODO get real
             public static final int TAG_PIPELINE = 0;
             public static final int SPEAKER_PIPELINE = 1;

diff --git a/src/main/java/frc/robot/command/ComboPoint.java b/src/main/java/frc/robot/command/ComboPoint.java
@@ -110,7 +110,8 @@ public void initialize() {
             targetPose = swapAlliance(originalTargetPose);
         }
 
-        stopMe.setPipeline(VisionConstants.Pipelines.SPEAKER_PIPELINE);
+        // stopMe.setPipeline(VisionConstants.Pipelines.SPEAKER_PIPELINE);
+        stopMe.setFiducialIDFiltersOverride(VisionConstants.SPEAKER_FILTERS);
         targetBias = 0;//5.5d;
 
         System.out.println("DRIVE - COMBO POINT START");
@@ -204,7 +205,8 @@ public void updateLogging() {
     @Override
     public void end(boolean interrupted) {
         System.out.println("DRIVE - COMBO POINT END");
-        stopMe.setPipeline(VisionConstants.Pipelines.TAG_PIPELINE);
+        // stopMe.setPipeline(VisionConstants.Pipelines.TAG_PIPELINE);
+        stopMe.setFiducialIDFiltersOverride(VisionConstants.ALL_TAG_FILTERS);
         if (DriverStation.isAutonomous()) {
             drivetrain.setField(0d, 0d, 0d);
         }

diff --git a/src/main/java/frc/robot/subsystems/Limelights.java b/src/main/java/frc/robot/subsystems/Limelights.java
@@ -4,6 +4,7 @@
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.VisionConstants;
+import frc.robot.Constants.VisionConstants.LimelightOrientation;
 import frc.robot.Constants.VisionConstants.Pipelines;
 import frc.thunder.util.Pose4d;
 import frc.thunder.vision.Limelight;
@@ -28,6 +29,8 @@ public Limelights() {
         dust.setPipeline(0);
         champs.setPipeline(0);
 
+        setOrientations();
+
         this.poseProducer = new Thread(() -> {
             while (true) {
                 try {
@@ -97,5 +100,15 @@ public int getChampsPipeline() {
 
     @Override
     public void periodic() {
+        // setOrientations();
     }
+
+    public void setOrientations(){
+        // stopMe.setRobotOrientation(LimelightOrientation.STOPME_YAW,0, 
+        // LimelightOrientation.STOPME_PITCH, 0, 
+        // LimelightOrientation.STOPME_ROLL, 0);
+
+        // champs.setRobotOrientation(0, 0, 0, 0, 0, 0);
+    }
+
 }
diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder