intake assist toggle in auto

Author: IsaacThoman

src/main/java/frc/robot/RobotContainer.java

@@ -91,7 +91,9 @@ private void configureBindings() {
     driver.a().onTrue(new InstantCommand(stateController::stowClimbPressed));
     driver.y().onTrue(new InstantCommand(stateController::raiseClimbPressed));
     driver.x().onTrue(new InstantCommand(stateController::climbPressed));
+    driver.b().onTrue(new InstantCommand(stateController::resetPressed));
 
+    operator.leftBumper().onTrue(new InstantCommand(stateController::resetPressed));
 
    // operator.leftBumper().onTrue(new InstantCommand(stateController::stowPressed));
    // operator.rightBumper().onTrue(new InstantCommand(stateController::raiseClimbPressed));

src/main/java/frc/robot/StateControllerSub.java

@@ -32,6 +32,7 @@ public enum Objective{SPEAKER,AMP,SOURCE,TRAP}
     public enum SelectedTrap{AMP,SOURCE,REAR}
 
     public enum UseFedPoseIntention{YES,NO}
+    boolean intakeAssistPPEnabled = false;
 
     public enum DuckMode {UNDUCK, DOWN}
 
@@ -71,7 +72,7 @@ public Optional<Rotation2d> getRotationTargetOverride(){
             return Optional.of(AllianceFlipUtil.apply(Rotation2d.fromDegrees(180)));
 
 
-        if(armState ==ArmState.INTAKE)
+        if(armState ==ArmState.INTAKE && intakeAssistPPEnabled)
             return Optional.of(getRobotPose().getRotation().minus(Rotation2d.fromRadians(noteAlignAngleDiff)));
 
         if(armState ==ArmState.SPEAKER  )
@@ -82,6 +83,9 @@ public Optional<Rotation2d> getRotationTargetOverride(){
         return Optional.empty();
     }
 
+    public void setIntakeAssistPPEnabled(boolean intakeAssistPPEnabled){
+        this.intakeAssistPPEnabled = intakeAssistPPEnabled;
+    }
 
 
     public void scheduleAlignmentCommand(){
@@ -220,6 +224,8 @@ public ClimbState getClimbStateConsideringDuckMode(){
     return climbState;
 }
 
+
+
 public void toggleClimbed(){
         if(climbState == ClimbState.DOWN) //local variable climbState should never be down. (that's what the climbStateConsideringDuckMode() method is for)
             climbState = ClimbState.CLIMBED;
@@ -409,6 +415,13 @@ public void publishTableEntries(){
         table.getEntry("loweredMode").setString(duckMode.toString());
     }
 
+    public void resetPressed(){
+        armState = ArmState.HOLD;
+        efState = EFState.HOLD;
+        climbState = ClimbState.DOWN;
+        useFedPoseIntention = UseFedPoseIntention.NO;
+    }
+
     public void intakePressed(){
 
         armState = ArmState.INTAKE;