From ed137534d62ea99c7d17a104a3cee707d4d5de42 Mon Sep 17 00:00:00 2001
From: Nick McGill <13747745+nick-mcgill@users.noreply.github.com>
Date: Tue, 27 Feb 2024 20:50:14 -0500
Subject: [PATCH] Draft PR for the Outtake Position to master branch (#26)

* Create outtake position configuration for amp scoring

Authored by Alex and tested by Harsh
---
 src/main/cpp/subsystems/SmartIntake.cpp       |  8 +++----
 .../subsystemManagers/IntakeManager.cpp       | 24 +++++++++++++++++++
 .../subsystemsManagers/IntakeManager.h        |  3 +++
 3 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/src/main/cpp/subsystems/SmartIntake.cpp b/src/main/cpp/subsystems/SmartIntake.cpp
index e44cf10..8386bd8 100644
--- a/src/main/cpp/subsystems/SmartIntake.cpp
+++ b/src/main/cpp/subsystems/SmartIntake.cpp
@@ -121,15 +121,15 @@ void SmartIntake::HandleInput(RobotControlData& input){
         case 0:
         {
                 input.launcherInput.goToStowPos = true;
-                input.intakeInput.goToGroundPos = true;
+                input.intakeInput.goToOutakePos = true;
                 ++m_OutTakeState;
         }
             break;
         case 1:
         {
             input.launcherInput.goToStowPos = false;
-            input.intakeInput.goToGroundPos = false;
-            if (input.intakeOutput.intakePos == IntakePos::GROUND)
+            input.intakeInput.goToOutakePos = false;
+            if (input.intakeOutput.intakePos == IntakePos::OUTAKE)
             {
                 ++m_OutTakeState;
             }
@@ -149,7 +149,7 @@ void SmartIntake::HandleInput(RobotControlData& input){
         }
         case 3:
         {
-            if (m_timer.Get() > units::second_t(0.45))
+            if (m_timer.Get() > units::second_t(0.04))
             {
                 m_SmartOutTakeFlag = false;
                 m_OutTakeState = 0;
diff --git a/src/main/cpp/subsystems/subsystemManagers/IntakeManager.cpp b/src/main/cpp/subsystems/subsystemManagers/IntakeManager.cpp
index 2838e56..0d0b9ac 100644
--- a/src/main/cpp/subsystems/subsystemManagers/IntakeManager.cpp
+++ b/src/main/cpp/subsystems/subsystemManagers/IntakeManager.cpp
@@ -7,6 +7,7 @@ namespace
     const double GROUND_POS = 132.056;
     const double STOW_POS = 68.144;
     const double PSEUDO_STOW_POS = 104.671;
+    const double OUTAKE_POS = 113.4; //change later
 }
 
 void IntakeManager::ResetIntake()
@@ -15,6 +16,7 @@ void IntakeManager::ResetIntake()
     m_goToStowPos = false;
     m_goToAmpPos = false;
     m_goToPseudoStowPos = false;
+    m_goToOutakePos = false;
 }
 
 void IntakeManager::HandleInput(IntakeInput &input, IntakeOutput &output)
@@ -40,6 +42,7 @@ void IntakeManager::HandleInput(IntakeInput &input, IntakeOutput &output)
         m_goToGroundPos = false;
         m_goToStowPos = false;
         m_goToPseudoStowPos = false;
+        m_goToOutakePos = false;
     }
     else if (input.goToStowPos)
     {
@@ -48,6 +51,7 @@ void IntakeManager::HandleInput(IntakeInput &input, IntakeOutput &output)
         m_goToGroundPos = false;
         m_goToStowPos = true;
         m_goToPseudoStowPos = false;
+        m_goToOutakePos = false;
     } 
     else if (input.goToGroundPos)
     {
@@ -56,6 +60,7 @@ void IntakeManager::HandleInput(IntakeInput &input, IntakeOutput &output)
         m_goToGroundPos = true;
         m_goToStowPos = false;
         m_goToPseudoStowPos = false;
+        m_goToOutakePos = false;
     }
     else if (input.goToAmpPos)
     {
@@ -64,6 +69,7 @@ void IntakeManager::HandleInput(IntakeInput &input, IntakeOutput &output)
         m_goToGroundPos = false;
         m_goToStowPos = false;
         m_goToPseudoStowPos = false;
+        m_goToOutakePos = false;
     }
     else if (input.goToPseudoStowPos)
     {
@@ -72,6 +78,16 @@ void IntakeManager::HandleInput(IntakeInput &input, IntakeOutput &output)
         m_goToGroundPos = false;
         m_goToStowPos = false;
         m_goToPseudoStowPos = true;
+        m_goToOutakePos = false;
+    }
+    else if (input.goToOutakePos)
+    {
+        m_intake.ResetProfiledMoveState();
+        m_goToAmpPos = false;
+        m_goToGroundPos = false;
+        m_goToStowPos = false;
+        m_goToPseudoStowPos = false;
+        m_goToOutakePos = true;
     }
    
     if (m_goToAmpPos)
@@ -90,6 +106,10 @@ void IntakeManager::HandleInput(IntakeInput &input, IntakeOutput &output)
     {
         m_intake.ProfiledMoveToAngle(PSEUDO_STOW_POS);
     }
+    if (m_goToOutakePos)
+    {
+        m_intake.ProfiledMoveToAngle(OUTAKE_POS);
+    }
 
     output.intakePos = IntakePos::UNKNOWN;
     double tolerance = 0.5;
@@ -109,4 +129,8 @@ void IntakeManager::HandleInput(IntakeInput &input, IntakeOutput &output)
     {
         output.intakePos = IntakePos::STOW;
     }
+    else if (std::fabs(m_intake.GetAngle() - OUTAKE_POS) < tolerance)
+    {
+        output.intakePos = IntakePos::OUTAKE;
+    }
 }
\ No newline at end of file
diff --git a/src/main/include/subsystems/subsystemsManagers/IntakeManager.h b/src/main/include/subsystems/subsystemsManagers/IntakeManager.h
index 0786f1a..6287733 100644
--- a/src/main/include/subsystems/subsystemsManagers/IntakeManager.h
+++ b/src/main/include/subsystems/subsystemsManagers/IntakeManager.h
@@ -10,6 +10,7 @@ struct IntakeInput
     bool goToStowPos;
     bool goToAmpPos;
     bool goToPseudoStowPos;
+    bool goToOutakePos;
     double manualMove;
 };
 
@@ -19,6 +20,7 @@ enum class IntakePos
     GROUND,
     STOW,
     PSEUDO_STOW,
+    OUTAKE,
     UNKNOWN
 };
 
@@ -40,4 +42,5 @@ class IntakeManager
     bool m_goToStowPos;
     bool m_goToAmpPos;
     bool m_goToPseudoStowPos;
+    bool m_goToOutakePos;
 };
\ No newline at end of file