uncomment elevator (untested)

Author: mwitcpalek

src/main/java/frc/robot/commands/auton/DriveAutonServoCommand.java

@@ -169,7 +169,7 @@ public void initialize() {
   public void execute() {
     if (elevatorSubsystem.getState() == ElevatorStates.ZEROED && !hasStaged) {
       hasStaged = true;
-      // robotStateSubsystem.toAutonPrestage();
+      robotStateSubsystem.toAutonPrestage();
     }
     if (isTherePath) {
       if (!isServoing) {
@@ -178,7 +178,7 @@ public void execute() {
 
         if (shouldTransitionToServoing()) {
           isServoing = true;
-          // robotStateSubsystem.toPrepCoral();
+          robotStateSubsystem.toPrepCoral();
           tagAlignSubsystem.startAuto(
               mirrorTrajectory ? Alliance.Red : Alliance.Blue,
               mirrorToProcessor ? !scoreLeft : scoreLeft,
@@ -201,8 +201,8 @@ public boolean isFinished() {
       return true;
     }
     return ((timer.hasElapsed(trajectory.getTotalTime() + AutonConstants.kAutoTimeout)
-        || tagAlignSubsystem.getState() == TagAlignStates.DONE && isServoing))
-    /*  && elevatorSubsystem.isFinished()*/ ;
+            || tagAlignSubsystem.getState() == TagAlignStates.DONE && isServoing))
+        && elevatorSubsystem.isFinished();
     // || (FastMath.sqrt(
     //             FastMath.pow(driveSubsystem.getPoseMeters().getX() - finalPose.getX(), 2)
     //                 + FastMath.pow(

src/main/java/frc/robot/commands/auton/ProcessorShallowAutonCommand.java

@@ -70,7 +70,7 @@ public ProcessorShallowAutonCommand(
         new SequentialCommandGroup(
             new SetGyroOffsetCommand(driveSubsystem, Rotation2d.fromDegrees(180)),
             startPath,
-            // new PlaceCoralAutonCommand(robotStateSubsystem, coralSubsystem)
+            new PlaceCoralAutonCommand(robotStateSubsystem, coralSubsystem),
 
             // new ParallelCommandGroup(
             //     new ZeroElevatorCommand(elevatorSubsystem),

src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java

@@ -211,7 +211,7 @@ private void drivePath() {
             false);
         driveSubsystem.setAutoDebugMsg("Servo Start");
         curState = PathStates.DRIVE_PLACE_SERVO;
-        // robotStateSubsystem.toPrepCoral();
+        robotStateSubsystem.toPrepCoral();
         isServoing = true;
       }
     }
@@ -225,7 +225,7 @@ private void drivePathServo() {
         runningPath = false;
         pathTimer.stop();
         pathTimer.reset();
-        // robotStateSubsystem.toPlaceCoralAuto();
+        robotStateSubsystem.toPlaceCoralAuto();
         curState = PathStates.PLACE;
         driveSubsystem.stopDriving();
         waitingTimer.start();
@@ -381,13 +381,12 @@ public void periodic() {
         }
       }
       case PLACE -> {
-        curState = PathStates.DRIVE_FETCH;
-        // if (!robotStateSubsystem.hasCoral()) {
-        //   // waitingTimer.hasElapsed(PathHandlerConstants.kWaitingTime)
-        //   // proceedToNext) {
-        //   // robotStateSubsystem.toFunnelLoad();
-        //   curState = PathStates.DRIVE_FETCH;
-        // }
+        if (!robotStateSubsystem.hasCoral()) {
+          // waitingTimer.hasElapsed(PathHandlerConstants.kWaitingTime)
+          // proceedToNext) {
+          robotStateSubsystem.toFunnelLoad();
+          curState = PathStates.DRIVE_FETCH;
+        }
       }
       case DONE -> isHandling = false;
       default -> {}