Draft PR for the Outtake Position to master branch (#26)

* Create outtake position configuration for amp scoring Authored by Alex and tested by Harsh
FRC830 · Feb 28, 2024 · ed13753 · ed13753
1 parent cc3508b
commit ed13753
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 4 deletions.
diff --git 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
@@ -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;
+    }
 }
diff --git 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;
 };