diff --git a/Robot2019/src/main/java/frc/robot/commands/KeepClimber.java b/Robot2019/src/main/java/frc/robot/commands/KeepClimber.java new file mode 100644 index 0000000..4412fd6 --- /dev/null +++ b/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(); + } +} diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java index 9e7f995..bd19de9 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java +++ b/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)); } }