climb reset

Author: davidchen20

src/main/java/frc/robot/Robot.java

@@ -18,6 +18,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.Constants.LED_STATE;
+import frc.robot.statemachines.ClimbStateMachine.CLIMB_STATES;
 import frc.robot.util.LocalADStarAK;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
@@ -115,6 +116,7 @@ public void robotPeriodic() {
   @Override
   public void disabledInit() {
     m_robotContainer.getLED().setState(LED_STATE.FIRE);
+    m_robotContainer.getClimbStateMachine().setClimbState(CLIMB_STATES.NONE);
   }
 
   /** This function is called periodically when disabled. */

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

@@ -999,4 +999,8 @@ public LED getLED() {
   public Drive getDrive() {
     return drive;
   }
+
+  public ClimbStateMachine getClimbStateMachine() {
+    return climbStateMachine;
+  }
 }