start imu settig angle while disabled

team581 · Nov 23, 2024 · cbdab4a · cbdab4a
1 parent 2545e98
commit cbdab4a
Showing 1 changed file with 11 additions and 0 deletions.
diff --git 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());
+   }
+    }
   }
 }