From 5a13dd86dcd68ef704139e9d71fdf4495570a983 Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 2 Mar 2020 20:12:38 +0200 Subject: [PATCH 1/2] Add command to manually adjust the height of the rods --- .../climb/commands/JoystickRelease.java | 75 +++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/climb/commands/JoystickRelease.java diff --git a/src/main/java/frc/robot/subsystems/climb/commands/JoystickRelease.java b/src/main/java/frc/robot/subsystems/climb/commands/JoystickRelease.java new file mode 100644 index 00000000..a1e35396 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/commands/JoystickRelease.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 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.subsystems.climb.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.OI; +import frc.robot.subsystems.climb.Climber; + +import java.util.function.Supplier; + +/** + * This command would allow the driver to modify the robot's angle manually + * with the value of the left xbox's joystick. + */ +public class JoystickRelease extends CommandBase { + private final Climber climber; + private double setpoint; + private Supplier joystickInput = OI::getXboxRY; + + /** + * Creates a new joystick control command. + * + * @param climber The subsystem used by this command. + */ + public JoystickRelease(Climber climber) { + this.climber = climber; + addRequirements(climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + climber.releaseStopper(); + climber.changePIDFSlot(1); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + setpoint = -joystickInput.get() * Constants.Climber.MODIFY_JOYSTICK_RATE; + if (Math.abs((climber.getLeftHeight() + climber.getRightHeight()) / 2 - setpoint) < Constants.Climber.ALLOWED_HEIGHT_TOLERANCE) { + setpoint = 0; + } + + climber.setLeftHeight(setpoint + climber.getLeftHeight()); + climber.setRightHeight(setpoint + climber.getRightHeight()); + + } + + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + + /** + * This command is designed to work only while a button is held, + * so it would terminate itself if the button is released so there is no need for isFinished. + * + * @return false + */ + @Override + public boolean isFinished() { + return false; + } + + +} From e9798a8f3b16691ec8a8355daf387c1c147f513a Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 2 Mar 2020 20:13:24 +0200 Subject: [PATCH 2/2] Add the manual release as a default command at the beginning of endgame --- .../frc/robot/subsystems/climb/Climber.java | 22 ++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climb/Climber.java b/src/main/java/frc/robot/subsystems/climb/Climber.java index d534ba64..03c52c91 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climber.java +++ b/src/main/java/frc/robot/subsystems/climb/Climber.java @@ -20,6 +20,7 @@ import frc.robot.Robot; import frc.robot.UtilityFunctions; import frc.robot.subsystems.UnitModel; +import frc.robot.subsystems.climb.commands.JoystickRelease; import frc.robot.utilities.CustomDashboard; import frc.robot.utilities.TalonConfiguration; import frc.robot.valuetuner.WebConstantPIDTalon; @@ -35,6 +36,7 @@ public class Climber extends SubsystemBase { private final UnitModel unitModel = new UnitModel(Constants.Climber.TICKS_PER_METER); private DoubleSolenoid stopperA = null; private Solenoid stopperB = null; + private boolean addDefault = false; /** * Creates a new climb Subsystem. @@ -91,10 +93,10 @@ public Climber() { rightMotor.config_kD(1, CLIMB_RELEASE_PIDF[2]); rightMotor.config_kF(1, CLIMB_RELEASE_PIDF[3]); - new WebConstantPIDTalon("climbLeftDown", CLIMB_PIDF[0], CLIMB_PIDF[1], CLIMB_PIDF[2], CLIMB_PIDF[3], leftMotor, 0); - new WebConstantPIDTalon("climbRightDown", CLIMB_PIDF[0], CLIMB_PIDF[1], CLIMB_PIDF[2], CLIMB_PIDF[3], rightMotor, 0); - new WebConstantPIDTalon("climbLeftUp", CLIMB_RELEASE_PIDF[0], CLIMB_RELEASE_PIDF[1], CLIMB_RELEASE_PIDF[2], CLIMB_RELEASE_PIDF[3], leftMotor, 1); - new WebConstantPIDTalon("climbRightUp", CLIMB_RELEASE_PIDF[0], CLIMB_RELEASE_PIDF[1], CLIMB_RELEASE_PIDF[2], CLIMB_RELEASE_PIDF[3], rightMotor, 1); + new WebConstantPIDTalon("climbLeftDown", CLIMB_PIDF[0], CLIMB_PIDF[1], CLIMB_PIDF[2], CLIMB_PIDF[3], leftMotor, 0); + new WebConstantPIDTalon("climbRightDown", CLIMB_PIDF[0], CLIMB_PIDF[1], CLIMB_PIDF[2], CLIMB_PIDF[3], rightMotor, 0); + new WebConstantPIDTalon("climbLeftUp", CLIMB_RELEASE_PIDF[0], CLIMB_RELEASE_PIDF[1], CLIMB_RELEASE_PIDF[2], CLIMB_RELEASE_PIDF[3], leftMotor, 1); + new WebConstantPIDTalon("climbRightUp", CLIMB_RELEASE_PIDF[0], CLIMB_RELEASE_PIDF[1], CLIMB_RELEASE_PIDF[2], CLIMB_RELEASE_PIDF[3], rightMotor, 1); if (Robot.isRobotA) stopperA = new DoubleSolenoid(Ports.Climber.STOPPER_FORWARD, Ports.Climber.STOPPER_REVERSE); @@ -202,6 +204,7 @@ public void setLeftHeight(double height, double arbitraryFeedForward) { leftMotor.set(ControlMode.Position, unitModel.toTicks(normalizeSetpoint(height)), DemandType.ArbitraryFeedForward, arbitraryFeedForward); } } + /** * All cases where we want to prevent the drivers from climbing should return true here. whether it's by game time. * It won't allow climbing before the endgame @@ -245,9 +248,18 @@ public void periodic() { CustomDashboard.setClimbLeftHeight(leftHeight); CustomDashboard.setClimbRightHeight(rightHeight); + + if (DriverStation.getInstance().getMatchTime() == 30) { + addDefault = true; + } + + if (addDefault) { + this.setDefaultCommand(new JoystickRelease(this)); + addDefault = false; + } } - public void resetEncoders(){ + public void resetEncoders() { rightMotor.setSelectedSensorPosition(0); leftMotor.setSelectedSensorPosition(0); }