Climb improvements

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);
     }

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<Double> 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;
+    }
+
+
+}