Soumik updated auto on develop as well

Author: ExamWeHaltLitMaTowel

src/main/java/frc/robot/HeadHoncho.java

@@ -680,20 +680,37 @@ public Command autonomousScoreCommand() { {
                 END_EFFECTOR_SUBSYSTEM.setState(EndEffectorStates.SCORE_L4);
                 DRIVE_SUBSYSTEM.cancelAutoAlign();
               },
-              () -> {
-                LIFT_SUBSYSTEM.setState(TargetLiftStates.STOW);
-                INTAKE_SUBSYSTEM.startIntake();
-                END_EFFECTOR_SUBSYSTEM.requestIntake();
-              },
+              () -> {},
 		  this
     )
 		.until(() -> {
                 return END_EFFECTOR_SUBSYSTEM.isEmpty();
               })
-          .withTimeout(0.8);
+    .withTimeout(0.8)
+    .andThen(
+      Commands.startEnd(
+                () -> {
+          Logger.recordOutput("Auto/Command", Constants.NamedCommands.AUTO_SCORE_COMMAND_NAME);
+                  LIFT_SUBSYSTEM.setState(TargetLiftStates.PANIC);
+                  END_EFFECTOR_SUBSYSTEM.setState(EndEffectorStates.SCORE_L4);
+                },
+                () -> {
+                  LIFT_SUBSYSTEM.setState(TargetLiftStates.STOW);
+                  INTAKE_SUBSYSTEM.startIntake();
+                  END_EFFECTOR_SUBSYSTEM.requestIntake();
+                },
+        this
+      )
+      .until(() -> {
+        return END_EFFECTOR_SUBSYSTEM.isEmpty();
+      })
+      .withTimeout(0.5));
     }
   }
 
+
+
+
   /**
    * Sets the intake and end effector subsystems to intake state in autonomous
    * @return Command which sets the intake and end-effector to the intake state in autonomous

src/main/java/frc/robot/subsystems/lift/LiftSubsystem.java

@@ -54,6 +54,7 @@ public static enum TargetLiftStates {
     L2,
     L3,
     L4,
+    PANIC,
     A1,
     A2,
     A_SCORE,
@@ -97,6 +98,7 @@ public static enum TargetLiftStates {
   static final Distance CLEAR_HEIGHT = LiftSubsystem.convertToDistance(Rotations.of(3.824)).minus(Inches.of(1.375));
   static final Distance L3_HEIGHT = LiftSubsystem.convertToDistance(Rotations.of(0));
   static final Distance L4_HEIGHT = LiftSubsystem.convertToDistance(Rotations.of(4.49)).minus(Inches.of(1.625));
+  static final Distance PANIC_HEIGHT = LiftSubsystem.convertToDistance(Rotations.of(4.5)).minus(Inches.of(1.125));
   static final Distance TURBO_HEIGHT = L4_HEIGHT;
   static final Distance A1_HEIGHT = LiftSubsystem.convertToDistance(Rotations.of(0.45)).minus(Inches.of(0.375));
   static final Distance A2_HEIGHT = LiftSubsystem.convertToDistance(Rotations.of(2.65678)).minus(Inches.of(0.375));
@@ -1172,6 +1174,42 @@ public SystemState nextState() {
         if (nextState == TargetLiftStates.TURBO) {
           return L4_TURBO_S1;
         }
+        if (nextState == TargetLiftStates.PANIC) {
+          return PANIC;
+        }
+        return this;
+      }
+    },
+    PANIC {
+      @Override
+      public void initialize() {
+        s_liftinstance.setElevatorHeight(PANIC_HEIGHT);
+        s_liftinstance.setArmAngle(SCORING_L4_ANGLE);
+      }
+
+      @Override
+      public void execute() {
+        isLiftReady = true;
+      }
+
+      @Override
+      public SystemState nextState() {
+        curState = TargetLiftStates.PANIC;
+        if (nextState == TargetLiftStates.STOW) {
+          return L4_STOW_S1;
+        }
+        if (nextState == TargetLiftStates.L1) {
+          return L4_L1_S1;
+        }
+        if (nextState == TargetLiftStates.L2) {
+          return L4_L2_S1;
+        }
+        if (nextState == TargetLiftStates.L3) {
+          return L4_L3_S1;
+        }
+        if (nextState == TargetLiftStates.TURBO) {
+          return L4_TURBO_S1;
+        }
         return this;
       }
     },