From b86d1deffc50e8f52fc4dc43024fbd6359f22495 Mon Sep 17 00:00:00 2001
From: NottheIRS <92690588+NottheIRS@users.noreply.github.com>
Date: Thu, 14 Mar 2024 20:01:06 -0400
Subject: [PATCH] Removed reset profiled move state.

---
 src/main/cpp/subsystems/LauncherHAL.cpp                  | 9 ++++++++-
 .../cpp/subsystems/subsystemManagers/LauncherManager.cpp | 1 +
 src/main/include/subsystems/LauncherHAL.h                | 2 +-
 3 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/subsystems/LauncherHAL.cpp b/src/main/cpp/subsystems/LauncherHAL.cpp
index 4998875..13b9f50 100644
--- a/src/main/cpp/subsystems/LauncherHAL.cpp
+++ b/src/main/cpp/subsystems/LauncherHAL.cpp
@@ -118,6 +118,7 @@ LauncherHAL::LauncherHAL()
     m_FlywheelTop.GetConfigurator().Apply(configs);
     m_FlywheelBottom.GetConfigurator().Apply(configs);
 
+    m_currentAngle = -100.0;
 }
 
 void LauncherHAL::SetFlywheelSpeed(double speed)
@@ -172,6 +173,12 @@ void LauncherHAL::SetAngle(double angle)
 
 void LauncherHAL::ProfiledMoveToAngle(double angle)
 {
+    if (std::fabs(m_currentAngle - angle) >= 1e-8)
+    {
+        m_profileState = 0;
+        m_currentAngle = angle;
+    }
+
     switch(m_profileState)
     {
      case 0: 
@@ -234,5 +241,5 @@ double LauncherHAL::GetFlywheelSpeed()
 
 void LauncherHAL::ResetProfiledMoveState()
 {
-    m_profileState = 0;
+    // m_profileState = 0;
 }
\ No newline at end of file
diff --git a/src/main/cpp/subsystems/subsystemManagers/LauncherManager.cpp b/src/main/cpp/subsystems/subsystemManagers/LauncherManager.cpp
index a445d0d..121fe21 100644
--- a/src/main/cpp/subsystems/subsystemManagers/LauncherManager.cpp
+++ b/src/main/cpp/subsystems/subsystemManagers/LauncherManager.cpp
@@ -7,6 +7,7 @@ void LauncherManager::ResetLauncher()
     m_goToSubPos = false;
     m_visionResetProfiledMoveState = false;
 }
+
 void LauncherManager::HandleInput(LauncherInput &launcherInput, LauncherOutput &launcherOutput, IntakeInput &intakeInput, IntakeOutput &intakeOutput)
 {
 
diff --git a/src/main/include/subsystems/LauncherHAL.h b/src/main/include/subsystems/LauncherHAL.h
index c6c862a..34dda6c 100644
--- a/src/main/include/subsystems/LauncherHAL.h
+++ b/src/main/include/subsystems/LauncherHAL.h
@@ -44,6 +44,6 @@ class LauncherHAL
         double m_ProfileStartPos; 
         double m_indexerSpeed;
         int m_profileState = 0;
-
+        double m_currentAngle;
         void SetAngle(double angle);
 };
\ No newline at end of file