From ec7f3dc2184e5833e4324fa38953724f4e176da5 Mon Sep 17 00:00:00 2001 From: David Fang Date: Mon, 18 Feb 2019 16:00:20 -0800 Subject: [PATCH 1/2] Add code to keep climber up --- .../java/frc/robot/commands/KeepClimber.java | 53 +++++++++++++++++++ .../java/frc/robot/subsystems/Climber.java | 10 ++++ 2 files changed, 63 insertions(+) create mode 100644 Robot2019/src/main/java/frc/robot/commands/KeepClimber.java 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..73b448d --- /dev/null +++ b/Robot2019/src/main/java/frc/robot/commands/KeepClimber.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* 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); + } + + // 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..ebc4e0f 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java +++ b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java @@ -13,27 +13,32 @@ 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 private Encoder enc; private AHRS ahrs; private DoubleSolenoid pistons; + public boolean climbing; final private double minTilt = 0; // In degrees // TODO: Update wtih actual number 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; this.enc = enc; this.ahrs = ahrs; this.pistons = pistons; + climbing = false; 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 +73,12 @@ public void resetEncoder() { enc.reset(); } + public boolean slipping() { + return enc.getDistance() > slipTolerance; + } + @Override public void initDefaultCommand() { + setDefaultCommand(new KeepClimber(this)); } } From 40e4844b82fd2464fffe7f9efe5146788a2e2fe6 Mon Sep 17 00:00:00 2001 From: David Fang Date: Mon, 18 Feb 2019 17:09:11 -0800 Subject: [PATCH 2/2] Remove extraneous variable, add to keepclimber logic --- Robot2019/src/main/java/frc/robot/commands/KeepClimber.java | 6 +++++- Robot2019/src/main/java/frc/robot/subsystems/Climber.java | 2 -- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/Robot2019/src/main/java/frc/robot/commands/KeepClimber.java b/Robot2019/src/main/java/frc/robot/commands/KeepClimber.java index 73b448d..4412fd6 100644 --- a/Robot2019/src/main/java/frc/robot/commands/KeepClimber.java +++ b/Robot2019/src/main/java/frc/robot/commands/KeepClimber.java @@ -29,7 +29,11 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if(climber.slipping()) climber.runClimber(retractSpeed); + if(climber.slipping()) { + climber.runClimber(retractSpeed); + } else { + climber.stopClimber(); + } } // Make this return true when this Command no longer needs to run execute() diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java index ebc4e0f..bd19de9 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java +++ b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java @@ -20,7 +20,6 @@ public class Climber extends Subsystem { private Encoder enc; private AHRS ahrs; private DoubleSolenoid pistons; - public boolean climbing; final private double minTilt = 0; // In degrees // TODO: Update wtih actual number final private double minDist = 13; // In inches // TODO: Update with actual number @@ -33,7 +32,6 @@ public Climber(VictorSP motor, Encoder enc, AHRS ahrs, DoubleSolenoid pistons) { this.enc = enc; this.ahrs = ahrs; this.pistons = pistons; - climbing = false; double pulseFraction = 1.0/256; double pitchDiameter = 1.790; // https://www.vexrobotics.com/35-sprockets.html#Drawing