Merge pull request #79 from DeepBlueRobotics/issue-78

Issue 78

Author: mustangzr1

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

@@ -19,11 +19,12 @@ public class Climb extends Command {
   private State state;
 
   private final double backDrive = -0.5; // TODO: Set this to reasonable/tested value;
-  private final double climbUp = 0.5; // TODO: Set this to reasonable/tested value;
-  private final double climbDown = -0.5; // TODO: Set this to reasonable/tested value;
-  private final double retract = -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 offGroundHeight = 10; // TODO: Set this to reasonable/tested value;
 
   public Climb(Climber climber, Drivetrain dt, Joystick joy) {
     // Use requires() here to declare subsystem dependencies
@@ -102,7 +103,7 @@ protected void interrupted() {
    *         overriding the climb
    */
   private boolean robotOnPlatform() {
-    return dt.isStalled() || Math.abs(dtJoy.getY()) > overrideThreshold;
+    return (dt.isStalled() && climber.getEncDistance() > offGroundHeight) || Math.abs(dtJoy.getY()) > overrideThreshold;
   }
 
   private enum State {