Auto stow after intaking and scoring low

team581 · Nov 1, 2023 · dcbd652 · dcbd652
1 parent f0e62c7
commit dcbd652
Showing 1 changed file with 50 additions and 25 deletions.
diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java
@@ -4,20 +4,17 @@
 
 package frc.robot.managers.superstructure;
 
-import edu.wpi.first.math.trajectory.Trajectory.State;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.imu.ImuSubsystem;
 import frc.robot.intake.HeldGamePiece;
 import frc.robot.intake.IntakeState;
 import frc.robot.intake.IntakeSubsystem;
-import frc.robot.managers.autorotate.AutoRotate;
 import frc.robot.shoulder.ShoulderSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.wrist.WristSubsystem;
 import java.util.function.Supplier;
-
 import org.littletonrobotics.junction.Logger;
 
 public class SuperstructureManager extends LifecycleSubsystem {
@@ -67,10 +64,17 @@ public void enabledPeriodic() {
 
   @Override
   public void robotPeriodic() {
-      Logger.getInstance().recordOutput("Superstructure/GoalState/GoalWristAngle", goalState.position.wristAngle.getDegrees());
-      Logger.getInstance().recordOutput("Superstructure/GoalState/GoalShoulderAngle", goalState.position.shoulderAngle.getDegrees());
-      Logger.getInstance().recordOutput("Superstructure/GoalState/GoalIntakeState", goalState.intakeState.toString());
-      Logger.getInstance().recordOutput("Superstructure/GoalState/GoalIntakeNow", goalState.intakeNow);
+    Logger.getInstance()
+        .recordOutput(
+            "Superstructure/GoalState/GoalWristAngle", goalState.position.wristAngle.getDegrees());
+    Logger.getInstance()
+        .recordOutput(
+            "Superstructure/GoalState/GoalShoulderAngle",
+            goalState.position.shoulderAngle.getDegrees());
+    Logger.getInstance()
+        .recordOutput("Superstructure/GoalState/GoalIntakeState", goalState.intakeState.toString());
+    Logger.getInstance()
+        .recordOutput("Superstructure/GoalState/GoalIntakeNow", goalState.intakeNow);
   }
 
   public void setIntakeOverride(IntakeState intakeState) {
@@ -114,28 +118,43 @@ public ScoringProgress getScoringProgress() {
 
   public Command getIntakeFloorCommand() {
     return setStateCommand(
-        () -> {
-          if (mode == HeldGamePiece.CONE) {
-            return States.INTAKING_CONE_FLOOR;
-          } else {
-            return States.INTAKING_CUBE_FLOOR;
-          }
-        });
+            () -> {
+              if (mode == HeldGamePiece.CONE) {
+                return States.INTAKING_CONE_FLOOR;
+              } else {
+                return States.INTAKING_CUBE_FLOOR;
+              }
+            })
+        .andThen(
+            Commands.runOnce(
+                () -> {
+                  setGoal(States.STOWED);
+                }));
   }
 
   public Command getIntakeShelfCommand() {
     return setStateCommand(
-        () -> {
-          if (mode == HeldGamePiece.CONE) {
-            return States.INTAKING_CONE_SHELF;
-          } else {
-            return States.INTAKING_CUBE_SHELF;
-          }
-        });
+            () -> {
+              if (mode == HeldGamePiece.CONE) {
+                return States.INTAKING_CONE_SHELF;
+              } else {
+                return States.INTAKING_CUBE_SHELF;
+              }
+            })
+        .andThen(
+            Commands.runOnce(
+                () -> {
+                  setGoal(States.STOWED);
+                }));
   }
 
   public Command getIntakeSingleSubstationCommand() {
-    return setStateCommand(States.INTAKING_CONE_SINGLE_SUBSTATION);
+    return setStateCommand(States.INTAKING_CONE_SINGLE_SUBSTATION)
+        .andThen(
+            Commands.runOnce(
+                () -> {
+                  setGoal(States.STOWED);
+                }));
   }
 
   private SuperstructureScoringState getScoringState(NodeHeight height) {
@@ -153,8 +172,8 @@ private SuperstructureScoringState getScoringState(NodeHeight height) {
       //   cubeState = States.CUBE_NODE_LOW_BACK;
       //   coneState = States.CONE_NODE_LOW_BACK;
       // }
-        cubeState = States.CUBE_NODE_LOW_BACK;
-        coneState = States.CONE_NODE_LOW_BACK;
+      cubeState = States.CUBE_NODE_LOW_BACK;
+      coneState = States.CONE_NODE_LOW_BACK;
     } else if (height == NodeHeight.MID) {
       cubeState = States.CUBE_NODE_MID;
       coneState = States.CONE_NODE_MID;
@@ -196,7 +215,13 @@ public Command getScoreFinishCommand(NodeHeight height) {
                 () -> {
                   scoringHeight = null;
                   scoringProgress = ScoringProgress.DONE_SCORING;
-                }));
+                }))
+        .andThen(
+            Commands.runOnce(
+                    () -> {
+                      setGoal(States.STOWED);
+                    })
+                .unless(() -> height != NodeHeight.LOW));
   }
 
   // public Command yeetConeCommand() {