Replace timer with encoder to finish climb

Author: SaltyJoke

Robot2019/src/main/java/frc/robot/commands/Climb.java

@@ -11,28 +11,25 @@
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.Timer;
 
 public class Climb extends Command {
   private Climber climber;
   private Drivetrain dt;
   private Joystick dtJoy;
   private State state;
-  private Timer timer;
 
   private final double backDrive = -0.5; // TODO: Set this to reasonable/tested value;
   private final double climbUp = 1;
   private final double climbDown = -1;
   private final double retract = -1;
   private final double overrideThreshold = 0.1; // TODO: Set this to reasonable/tested value;
   private final double retractGoal = 0; // TODO: Set this to reasonable/tested value;
-  private final double startTime = 2; // Time to lift off the floor. TODO: Set this to reasonable/tested value
+  private final double offGroundHeight = 15; // TODO: Set this to reasonable/tested value;
 
   public Climb(Climber climber, Drivetrain dt, Joystick joy) {
     // Use requires() here to declare subsystem dependencies
     this.climber = climber;
     this.dt = dt;
-    timer = new Timer();
     dtJoy = joy;
     requires(climber);
     requires(dt);
@@ -107,7 +104,7 @@ protected void interrupted() {
    *         overriding the climb
    */
   private boolean robotOnPlatform() {
-    return timer.get() > startTime && (dt.isStalled() || Math.abs(dtJoy.getY()) > overrideThreshold);
+    return (dt.isStalled() && climber.getEncDistance() > offGroundHeight) || Math.abs(dtJoy.getY()) > overrideThreshold;
   }
 
   private enum State {