diff --git a/src/main/java/frc/robot/command/Climb.java b/src/main/java/frc/robot/command/Climb.java index 15f45b26..1addae37 100644 --- a/src/main/java/frc/robot/command/Climb.java +++ b/src/main/java/frc/robot/command/Climb.java @@ -34,17 +34,6 @@ public void initialize() { setPoint = ClimbConstants.CLIMB_PID_SETPOINT_EXTENDED; } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // use pid to set climber power - double pidOutput = climbController.calculate(climber.getHeight(), setPoint); - climber.setPower(pidOutput); - - LightningShuffleboard.setDouble("Climb", "Climb Height", climber.getHeight()); - LightningShuffleboard.setDouble("Climb", "Climb Power", pidOutput); - - } /** * retracts climb to 0 */ @@ -69,6 +58,9 @@ public void execute() { if (extended() && tipDetection.isTipped()) { retract(); } + + LightningShuffleboard.setDouble("Climb", "Climb Height", climber.getHeight()); + LightningShuffleboard.setDouble("Climb", "Climb Power", pidOutput); } /**