From 3cc479b2bf863954e36ab1a2eee025e36eae1790 Mon Sep 17 00:00:00 2001
From: HeeistHo <chriskaceli7@gmail.com>
Date: Thu, 4 Apr 2024 09:13:43 -0400
Subject: [PATCH] [#520] - made some constants and orientation changes

---
 src/main/java/frc/robot/Constants.java             |  7 +++++++
 src/main/java/frc/robot/subsystems/Limelights.java | 13 +++++++++++++
 src/main/java/frc/thunder                          |  2 +-
 3 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1f475cb7..8d877b2b 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -369,6 +369,13 @@ 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 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/subsystems/Limelights.java b/src/main/java/frc/robot/subsystems/Limelights.java
index 25c6b386..52a589da 100644
--- 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
index e1d0185f..1b69f038 160000
--- a/src/main/java/frc/thunder
+++ b/src/main/java/frc/thunder
@@ -1 +1 @@
-Subproject commit e1d0185f544e98364a2be749862e9aad1f7139c3
+Subproject commit 1b69f038d4ec259722591b3088cf975493947cfb