From ac79bb363b999019a9a75f6f9ea870b36aba99a9 Mon Sep 17 00:00:00 2001
From: NottheIRS <92690588+NottheIRS@users.noreply.github.com>
Date: Sun, 18 Feb 2024 15:50:10 -0500
Subject: [PATCH] Set conversion factor, zero offest, inversion for launcher
 pvt abs enc; set max/min angles for launcher.

---
 src/main/cpp/subsystems/LauncherHAL.cpp       | 13 +++++++++++++
 src/main/include/subsystems/SubSystemConfig.h | 10 ++++++++--
 2 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/subsystems/LauncherHAL.cpp b/src/main/cpp/subsystems/LauncherHAL.cpp
index a1bf22e..728cf94 100644
--- a/src/main/cpp/subsystems/LauncherHAL.cpp
+++ b/src/main/cpp/subsystems/LauncherHAL.cpp
@@ -13,6 +13,10 @@ LauncherHAL::LauncherHAL()
 
     m_PvtPID.SetPositionPIDWrappingEnabled(false);
 
+    m_PvtAbsEncoder.SetInverted(true);
+    m_PvtAbsEncoder.SetPositionConversionFactor(LAUNCHER_PVT_ABS_ENC_CONVERSION_FACTOR);
+    m_PvtAbsEncoder.SetZeroOffset(ZERO_OFFSET);
+
     m_PvtPID.SetFeedbackDevice(m_PvtAbsEncoder);
 
     m_PvtMotor.SetSmartCurrentLimit(LAUNCHER_PVT_CURRENT_LIMIT);
@@ -61,6 +65,15 @@ void LauncherHAL::SetIndexerSpeed(double speed)
 
 void LauncherHAL::SetAngle(double angle)
 {
+    if (angle > MAX_PIVOT_ANGLE)
+    {
+        angle = MAX_PIVOT_ANGLE;
+    }
+    else if (angle < MIN_PIVOT_ANGLE)
+    {
+        angle = MIN_PIVOT_ANGLE;
+    }
+
     m_PvtPID.SetReference(angle, rev::CANSparkMax::ControlType::kPosition);
 }
 
diff --git a/src/main/include/subsystems/SubSystemConfig.h b/src/main/include/subsystems/SubSystemConfig.h
index d4c55e9..6f5b341 100644
--- a/src/main/include/subsystems/SubSystemConfig.h
+++ b/src/main/include/subsystems/SubSystemConfig.h
@@ -46,7 +46,7 @@ const bool LFT_PVT_MTR_INVERTED = true;
 // ###########################################################
 const int FLYWHEEL_TOP_ID = 22;
 const int FLYWHEEL_BOTTOM_ID = 23;
-const int LAUNCHER_PVT_MTR_ID = 6;
+const int LAUNCHER_PVT_MTR_ID = 21;
 const int LAUNCHER_IND_MTR_ID = 7;
 
 const int LAUNCHER_PVT_ENCODER_ID = 9;
@@ -63,4 +63,10 @@ const int LAUNCHER_PVT_CURRENT_LIMIT = 20;
 const int FLYWHEEL_ACT_CURRENT_LIMIT = 20;
 const int LAUNCHER_IND_CURRENT_LIMIT = 20;
 
-const bool INVERT_FLYWHEEL = true;
\ No newline at end of file
+const bool INVERT_FLYWHEEL = true;
+
+const double LAUNCHER_PVT_ABS_ENC_CONVERSION_FACTOR = 80.0;
+const double ZERO_OFFSET = 36.221;
+
+const double MAX_PIVOT_ANGLE = 75.0;
+const double MIN_PIVOT_ANGLE = 5.0;
\ No newline at end of file