From cbdab4aa7effa646ff9f1ac992710903850180e2 Mon Sep 17 00:00:00 2001
From: rhetorr <140564619+rhetorr@users.noreply.github.com>
Date: Sat, 23 Nov 2024 11:16:48 -0800
Subject: [PATCH] start imu settig angle while disabled

---
 src/main/java/frc/robot/imu/ImuSubsystem.java | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/src/main/java/frc/robot/imu/ImuSubsystem.java b/src/main/java/frc/robot/imu/ImuSubsystem.java
index 82a5cbe..2d3c7ba 100644
--- a/src/main/java/frc/robot/imu/ImuSubsystem.java
+++ b/src/main/java/frc/robot/imu/ImuSubsystem.java
@@ -3,8 +3,12 @@
 import com.ctre.phoenix6.hardware.Pigeon2;
 import dev.doglog.DogLog;
 import edu.wpi.first.math.MathUtil;
+import frc.robot.arm.ArmState;
+import frc.robot.config.RobotConfig;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.util.state_machines.StateMachine;
+import frc.robot.vision.Limelight;
+import frc.robot.vision.VisionSubsystem;
 
 public class ImuSubsystem extends StateMachine<ImuState> {
   private final Pigeon2 imu;
@@ -18,6 +22,8 @@ public class ImuSubsystem extends StateMachine<ImuState> {
   public ImuSubsystem(Pigeon2 imu) {
     super(SubsystemPriority.IMU, ImuState.DEFAULT_STATE);
     this.imu = imu;
+          setAngle(Limelight.getYaw().getValueAsDouble());
+
   }
 
   @Override
@@ -64,5 +70,10 @@ public void robotPeriodic() {
     DogLog.log("Imu/RobotHeading", robotHeading);
     DogLog.log("Imu/AngularVelocity", angularVelocity);
     DogLog.log("Imu/Pitch", pitch);
+     if (DriverStation.isDisabled()) {
+   if (!MathUtil.isNear(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2.getYaw().getValueAsDouble(),robotHeading,1)){
+setAngle(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2.getYaw().getValueAsDouble());
+   }
+    }
   }
 }