feature(intake): operator right trigger overrides spike logic

Valor6800 · Mar 3, 2023 · b9d5a9c · b9d5a9c
1 parent 49625c2
commit b9d5a9c
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 1 deletion.
diff --git a/src/main/cpp/subsystems/Intake.cpp b/src/main/cpp/subsystems/Intake.cpp
@@ -53,6 +53,8 @@ void Intake::init()
     state.coneCacheSize = CONE_CACHE_SIZE;
     state.cubeCacheSize = CUBE_CACHE_SIZE;
 
+    state.intakeOp = false;
+
     prevState = state;
 
     currentSensor.setSpikeSetpoint(state.coneSpikeCurrent);
@@ -93,7 +95,7 @@ void Intake::assessInputs()
     } else if (state.intakeState != SPIKED) {
 
         // Driver or operator ground pickup
-        if (driverGamepad->GetLeftBumper() || driverGamepad->GetRightBumper() || operatorGamepad->leftTriggerActive() ||
+        if (driverGamepad->GetLeftBumper() || driverGamepad->GetRightBumper() || 
             // Operator human player pickup
             operatorGamepad->GetXButton()  || operatorGamepad->DPadLeft()) {
             if (driverGamepad->GetYButton()){
@@ -116,6 +118,12 @@ void Intake::assessInputs()
     } else {
         state.intakeState = DISABLED;
     }
+
+    if(operatorGamepad->leftTriggerActive()){
+        state.intakeOp = true;
+    } else{
+        state.intakeOp = false;
+    }
 }
 
 void Intake::analyzeDashboard()
@@ -137,6 +145,12 @@ void Intake::assignOutputs()
 { 
     if (state.intakeState == DISABLED) {
         intakeMotor.setPower(0);
+    } else if (state.intakeOp){
+         if (state.pieceState == CONE){
+            intakeMotor.setPower(state.intakeConeSpeed);
+        } else{
+            intakeMotor.setPower(state.intakeCubeSpeed);
+        }
     } else if (state.intakeState == SPIKED) {
         if (state.pieceState == CONE){
             intakeMotor.setPower(state.coneHoldSpeed);

diff --git a/src/main/include/subsystems/Intake.h b/src/main/include/subsystems/Intake.h
@@ -92,6 +92,8 @@ class Intake : public ValorSubsystem
          double coneCacheSize;
          double cubeCacheSize;
 
+         bool intakeOp;
+
      }state, prevState;
 
     std::unordered_map<std::string, IntakeStates> stringToStateMap = {