From f7563af7c79427f659f5030bed59c2facfc5deca Mon Sep 17 00:00:00 2001
From: NottheIRS <92690588+NottheIRS@users.noreply.github.com>
Date: Fri, 16 Feb 2024 20:18:15 -0500
Subject: [PATCH] Changed SetFlywheelSpeed().

---
 src/main/cpp/subsystems/LauncherHAL.cpp       | 14 +++++++++++---
 src/main/include/subsystems/SubSystemConfig.h |  2 +-
 2 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/src/main/cpp/subsystems/LauncherHAL.cpp b/src/main/cpp/subsystems/LauncherHAL.cpp
index 8ef9536..928e1c7 100644
--- a/src/main/cpp/subsystems/LauncherHAL.cpp
+++ b/src/main/cpp/subsystems/LauncherHAL.cpp
@@ -37,9 +37,17 @@ LauncherHAL::LauncherHAL()
 
 void LauncherHAL::SetFlywheelSpeed(double speed)
 {   
-    units::angular_velocity::turns_per_second_t speed_tps = units::angular_velocity::turns_per_second_t(speed);
-    ctre::phoenix6::controls::VelocityVoltage m_request{speed_tps};
-    m_FlywheelActMotorA.SetControl(m_request.WithVelocity(speed_tps));
+    if (speed > 1.0)
+    {
+        units::angular_velocity::turns_per_second_t speed_tps = units::angular_velocity::turns_per_second_t(speed);
+        ctre::phoenix6::controls::VelocityVoltage m_request{speed_tps};
+        m_FlywheelActMotorA.SetControl(m_request.WithVelocity(speed_tps));
+    }
+    else
+    {
+        ctre::phoenix6::controls::DutyCycleOut m_request{0.0};
+        m_FlywheelActMotorA.SetControl(m_request.WithOutput(0.0));
+    }
 }
 
 void LauncherHAL::SetIndexerSpeed(double speed)
diff --git a/src/main/include/subsystems/SubSystemConfig.h b/src/main/include/subsystems/SubSystemConfig.h
index 1af4dd3..7dd37f2 100644
--- a/src/main/include/subsystems/SubSystemConfig.h
+++ b/src/main/include/subsystems/SubSystemConfig.h
@@ -51,7 +51,7 @@ const int LAUNCHER_IND_MTR_ID = 7;
 
 const int LAUNCHER_PVT_ENCODER_ID = 9;
 
-const double FLYWHEEL_P = 1.0;
+const double FLYWHEEL_P = 0.25;
 const double FLYWHEEL_I = 0.0;
 const double FLYWHEEL_D = 0.0;