From 0591e799729cf87979d1d86f1461bd4671b2f04f Mon Sep 17 00:00:00 2001
From: Caleb Chalmers <caleb.chalmers1@gmail.com>
Date: Sat, 1 Feb 2025 03:36:06 +0000
Subject: [PATCH] Fix Sentry issue due to no encoder

---
 .../src/subsystems/turret/turret_subsystem.cpp   | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp
index 1c65c39..37f22cf 100644
--- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp
+++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp
@@ -29,15 +29,14 @@ void TurretSubsystem::initialize()
 
 void TurretSubsystem::refresh()
 {
-#if defined(TARGET_STANDARD) || defined(TARGET_HERO)
-    yawEncoder.update();
-#endif
-
     setAmputated(!hardwareOk());
 
     yaw.updateMotorAngle();
     pitch.updateMotorAngle();
 
+#if defined(TARGET_STANDARD) || defined(TARGET_HERO)
+    yawEncoder.update();
+
     if (!isCalibrated && !isAmputated() && yawEncoder.isOnline())
     {
         baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle();
@@ -45,6 +44,15 @@ void TurretSubsystem::refresh()
 
         setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch());
     }
+#else
+    if (!isCalibrated && !isAmputated())
+    {
+        baseYaw = -YAW_OFFSET;
+        isCalibrated = true;
+
+        setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch());
+    }
+#endif
 
     if (isCalibrated && !drivers->isKillSwitched() && !isAmputated())
     {