Add code to keep climber up

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

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.subsystems.Climber;
+
+public class KeepClimber extends Command {
+  private Climber climber;
+
+  private final double retractSpeed = -1; // TODO: Confirm number
+  public KeepClimber(Climber climber) {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    this.climber = climber;
+    requires(climber);
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  protected void initialize() {
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  protected void execute() {
+    if(climber.slipping()) {
+      climber.runClimber(retractSpeed);
+    } else {
+      climber.stopClimber();
+    }
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  protected boolean isFinished() {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  @Override
+  protected void end() {
+    climber.stopClimber();
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

Robot2019/src/main/java/frc/robot/subsystems/Climber.java

@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.VictorSP;
 import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.commands.KeepClimber;
 
 public class Climber extends Subsystem {
   private VictorSP motor; // Mini-CIM
@@ -24,6 +25,7 @@ public class Climber extends Subsystem {
   final private double minDist = 13; // In inches // TODO: Update with actual number
   final private double maxTilt = 30; // In degrees // TODO: Update with actual number
   final private double maxDist = 15; // In inches // TODO: Update with actual number
+  final private double slipTolerance = 0.5; // In inches // TODO: Update with actual number;
 
   public Climber(VictorSP motor, Encoder enc, AHRS ahrs, DoubleSolenoid pistons) {
     this.motor = motor;
@@ -34,6 +36,7 @@ public Climber(VictorSP motor, Encoder enc, AHRS ahrs, DoubleSolenoid pistons) {
     double pulseFraction = 1.0/256;
     double pitchDiameter = 1.790; // https://www.vexrobotics.com/35-sprockets.html#Drawing
     enc.setDistancePerPulse(pulseFraction * Math.PI * pitchDiameter);
+    enc.reset();
   }
 
   public void actuateRails() {
@@ -68,7 +71,12 @@ public void resetEncoder() {
     enc.reset();
   }
 
+  public boolean slipping() {
+    return enc.getDistance() > slipTolerance;
+  }
+
   @Override
   public void initDefaultCommand() {
+    setDefaultCommand(new KeepClimber(this));
   }
 }