Merge pull request #18 from FRC830/SmartIntakeChanges

bug fix of smart intake logic
FRC830 · Feb 21, 2024 · c0c4ac0 · c0c4ac0
2 parents cda6896 + 93dab1c
commit c0c4ac0
Showing 1 changed file with 14 additions and 36 deletions.
diff --git a/src/main/cpp/subsystems/SmartIntake.cpp b/src/main/cpp/subsystems/SmartIntake.cpp
@@ -62,20 +62,10 @@ void SmartIntake::HandleInput(RobotControlData& input){
         case 2:
         {
             input.intakeInput.runIntakeIn = true;
-            if (input.smartIntakeInput.laser){
-                input.intakeInput.runIntakeIn = false;
-                input.intakeInput.goToPseudoStowPos = true;
-                ++m_IntakeState;
-            }
-
-        }
-            break;
-        case 3:
-        {
-            input.intakeInput.goToPseudoStowPos = false;
-            if (input.intakeOutput.intakePos == IntakePos::PSEUDO_STOW)
+            if (input.smartIntakeInput.laser)
             {
-                ++m_IntakeState;
+                m_SmartIntakeFlag = false;
+                m_IntakeState = 0;
             }
         }
             break;
@@ -89,10 +79,9 @@ void SmartIntake::HandleInput(RobotControlData& input){
         {
         case 0:
         {
-                /* code */
-                input.intakeInput.runIntakeIn = false;
-                input.intakeInput.goToPseudoStowPos = true;
-                ++m_IntakeState;
+            input.intakeInput.runIntakeIn = false;
+            input.intakeInput.goToPseudoStowPos = true;
+            ++m_IntakeState;
         }
             break;
         case 1:
@@ -133,20 +122,10 @@ void SmartIntake::HandleInput(RobotControlData& input){
         case 2:
         {
             input.intakeInput.runIntakeOut = true;
-            if (input.smartIntakeInput.laser){
-                input.intakeInput.runIntakeOut = false;
-                input.intakeInput.goToPseudoStowPos = true;
-                ++m_OutTakeState;
-            }
-
-        }
-            break;
-        case 3:
-        {
-            input.intakeInput.goToPseudoStowPos = false;
-            if (input.intakeOutput.intakePos == IntakePos::PSEUDO_STOW)
+            if (input.smartIntakeInput.laser)
             {
-                ++m_OutTakeState;
+                m_SmartOutTakeFlag = false;
+                m_OutTakeState = 0;
             }
         }
             break;
@@ -160,11 +139,10 @@ void SmartIntake::HandleInput(RobotControlData& input){
         switch (m_OutTakeState)
         {
         case 0:
-        {
-                /* code */
-                input.intakeInput.runIntakeOut = false;
-                input.intakeInput.goToPseudoStowPos = true;
-                ++m_OutTakeState;
+        {    
+            input.intakeInput.runIntakeOut = false;
+            input.intakeInput.goToPseudoStowPos = true;
+            ++m_OutTakeState;
         }
             break;
         case 1:
@@ -181,7 +159,7 @@ void SmartIntake::HandleInput(RobotControlData& input){
         }
     }
 
-        // needs to go at the bottom
+    // needs to go at the bottom
     m_prevSmartIntake = input.smartIntakeInput.smartIntake;
     m_prevSmartOutTake = input.smartIntakeInput.smartOutTake;
     m_prevSwitchMode = input.smartIntakeInput.switchMode;