Climb Buttons

src/main/java/frc/robot/Constants.java

@@ -24,6 +24,13 @@
  */
 public final class Constants {
 
+  //Distance to the Mid Bar
+  public static final double CLIMBER_ENCODER_DISTANCE_MID = 12; 
+  //Distance to the Low Bar
+  public static final double CLIMBER_ENCODER_DISTANCE_LOW = 8;
+  //setpoint for pulling the robot off the ground using random values
+  public static final double RETRACTER_ENCODER_DISTANCE = 0; //random value need to test 
+
   public static final boolean IS_REAL = RobotBase.isReal();
 
   public static final double AIM_SCALING_FACTOR_X = -0.5;
@@ -98,6 +105,12 @@ public static final class RobotDimensions {
   public static final class Input {
 
     //Axis
+    public static final ConstantButton CLIMB_MID_BUTTON = new ConstantButton(1, 1);
+    public static final ConstantButton CLIMB_LOW_BUTTON = new ConstantButton(1, 1);
+    public static final ConstantButton RETRACT_BUTTON = new ConstantButton(1, 1);
+    public static final ConstantButton OVERRIDE_UP_CLIMB = new ConstantButton(1, 1);
+    public static final ConstantButton OVERRIDE_DOWN_CLIMB = new ConstantButton(1, 1);
+
     public static final ConstantAxis X_AXIS = new ConstantAxis(0, 0);
     public static final ConstantAxis Y_AXIS = new ConstantAxis(0, 1);
     public static final ConstantAxis ROTATION = new ConstantAxis(0, 4);
@@ -106,6 +119,7 @@ public static final class Input {
     public static final ConstantButton CLIMB_BUTTON = new ConstantButton(1, 1); 
     public static final ConstantButton INTAKE_BUTTON = new ConstantButton(1, 5); 
     public static final ConstantButton STORAGE_TOGGLE = new ConstantButton(1, 0);
+
   }
 
   public static final class MotorSettings {
@@ -158,6 +172,7 @@ public static final class Motor {
 
     public static final int LEFT_CLIMB_MOTOR = 0; //reset to actual later
     public static final int RIGHT_CLIMB_MOTOR = 1; //reset to actual later
+    public static final double CLIMB_SPEED = 0.5;
   }
 
   public static final class Characteristics { 

src/main/java/frc/robot/RobotContainer.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.commands.ClimbCommand;
 import frc.robot.commands.IndexBall;
 import frc.robot.commands.SwerveDrive;
@@ -26,18 +27,24 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-
-  // Subsystems
-  // private Climb climb = new Climb();
-  // private Intake intake = new Intake();
-  // private Storage storage = new Storage();
-  private Swerve swerve = new Swerve();
   private GenericHID joy = new GenericHID(0);
 
-  // Commands
-  // private IndexBall indexBall = new IndexBall(intake, storage);
-  // private ClimbCommand climbCommand = new ClimbCommand(climb);
+    //Subsystems
+    private Climb climb = new Climb(); 
+    private Intake intake = new Intake();
+    private Storage storage = new Storage();
+    private Swerve swerve = new Swerve();
+
+    private IndexBall indexBall = new IndexBall(intake, storage);
+    private ClimbCommand climbCommand = new ClimbCommand(climb); 
+    private SwerveDrive swerveDrive;
+
+    // Commands
+    // private IndexBall indexBall = new IndexBall(intake, storage);
+    // private ClimbCommand climbCommand = new ClimbCommand(climb);
+
 
+
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
@@ -51,6 +58,10 @@ public RobotContainer() {
    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+    Constants.Input.CLIMB_MID_BUTTON.get().whenPressed(climbCommand.extendMidClimb()); 
+    Constants.Input.CLIMB_LOW_BUTTON.get().whenPressed(climbCommand.extendLowClimb());
+    Constants.Input.RETRACT_BUTTON.get().whenPressed(climbCommand.retract()); 
+    Constants.Input.INTAKE_BUTTON.get().whileHeld(indexBall);
     swerve.setDefaultCommand(new SwerveDrive(swerve, () -> joy.getRawAxis(0),
         () -> joy.getRawAxis(1), () -> joy.getRawAxis(4)));
 

src/main/java/frc/robot/commands/ClimbCommand.java

@@ -1,39 +1,83 @@
 package frc.robot.commands;
 
+import java.util.Set;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.Constants;
 import frc.robot.subsystems.Climb;
 
 public class ClimbCommand extends CommandBase {
+  Climb climb;
+
+  public ClimbCommand(Climb climb) {
+    this.climb = climb;
+    addRequirements(climb);
+  }
+
+  public Command extendLowClimb() {
+    return new Command() {
+      @Override
+      public void execute() {
+        if (climb.getEncoderValue() != Constants.CLIMBER_ENCODER_DISTANCE_LOW) {
+          climb.setSetpoint(Constants.CLIMBER_ENCODER_DISTANCE_LOW);
+        } else {
+          climb.stop();
+        }
+      }
+
+      @Override
+      public Set<Subsystem> getRequirements() {
+        // TODO Auto-generated method stub
+        return null;
+      }
+    }; 
+
+  }
+
+  public Command extendMidClimb() {
+    return new Command() {
+      @Override
+      public void execute() {
+        if (climb.getEncoderValue() != Constants.CLIMBER_ENCODER_DISTANCE_MID) {
+          climb.setSetpoint(Constants.CLIMBER_ENCODER_DISTANCE_MID);
+        } else {
+          climb.stop();
+        }
+      }
+
+      @Override
+      public Set<Subsystem> getRequirements() {
+        // TODO Auto-generated method stub
+        return null;
+      }
+    };
+
+  }
+
+  public Command retract() {
+    return new Command() {
+      @Override
+      public void execute() {
+        climb.setSetpoint(Constants.RETRACTER_ENCODER_DISTANCE);
+      }
+
+      @Override
+      public Set<Subsystem> getRequirements() {
+        // TODO Auto-generated method stub
+        return null;
+      }
+    }; 
+  }
+
+  public void overrideClimb() {
+    // todo ovverride climb retract
+  }
+
+  public void climbExtend() {
+    // todo ovverride climb extend
+  }
+
 
-	private Climb climb;
-
-	public ClimbCommand(Climb climb) {
-		this.climb = climb;
-		addRequirements(climb);
-	}
-
-	// Called when the command is first scheduled.
-	@Override
-	public void initialize() {}
-
-	// Called at 50hz while the command is scheduled.
-	@Override
-	public void execute() {
-		if (climb.getEncoderValue() != Constants.CLIMBER_ENCODER_DISTANCE) {
-			climb.setSetpoint(Constants.CLIMBER_ENCODER_DISTANCE);
-		} else {
-			climb.stop();
-		}
-	}
-
-	// Called once when the command ends or is interrupted.
-	@Override
-	public void end(boolean interrupted) {}
-
-	@Override
-	public boolean isFinished() {
-		return false;
-	}
 
 }

src/main/java/frc/robot/subsystems/Climb.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxPIDController;
@@ -27,6 +28,15 @@ public Climb() {
 		pidController.setD(Constants.PID.D_CLIMB); 
 		followerMotor.follow(mainMotor);
 	}
+	//manually moves the climb bar up
+	public void overrideUp(){
+		mainMotor.set(Constants.Motor.CLIMB_SPEED);
+	}
+
+	//Manually moves the climb bar down
+	public void overrideDown(){
+		mainMotor.set(-Constants.Motor.CLIMB_SPEED);
+	}
 
 	public double getEncoderValue() {
 		return encoder.getPosition();