Implement climber subsystem

Resolves #3

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

@@ -6,4 +6,8 @@ public final class Constants {
   public static final double shooterP = 0.02;
   public static final double shooterI = 0.5;
   public static final double shooterD = 0;
+  public static final int climberPort = 32;
+  public static final double climberP = 0.35;
+  public static final double climberI = 0;
+  public static final double climberD = 0;
 }

src/main/java/frc/robot/Robot.java

@@ -6,12 +6,14 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Shooter;
 
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
   private CommandXboxController m_driver;
   private Shooter m_shooter;
+  private Climber m_climber;
 
   private Command getAutonomousCommand() {
     return null;
@@ -22,10 +24,15 @@ private void configureBindings() {}
   @SuppressWarnings("removal")
   public Robot() {
     m_driver = new CommandXboxController(Constants.driverport);
+
     m_shooter =
         new Shooter(
             new CANSparkMax(Constants.shooterPort, CANSparkMaxLowLevel.MotorType.kBrushless));
 
+    m_climber =
+        new Climber(
+            new CANSparkMax(Constants.climberPort, CANSparkMaxLowLevel.MotorType.kBrushless));
+
     configureBindings();
   }
 

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

@@ -0,0 +1,35 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants;
+
+/** Our Crescendo climber Subsystem */
+public class Climber {
+  private PIDController m_pid;
+  private CANSparkMax m_motor;
+  private RelativeEncoder m_encoder;
+
+  /**
+   * Creates a new {@link Climber} {@link edu.wpi.first.wpilibj2.command.Subsystem}.
+   *
+   * @param motor The motor that the climber controls.
+   */
+  public Climber(CANSparkMax motor) {
+    m_motor = motor;
+    m_encoder = m_motor.getEncoder();
+    m_pid = new PIDController(Constants.climberP, Constants.climberI, Constants.climberD);
+    m_pid.setTolerance(0.2, 0.5);
+  }
+
+  public Command climb() {
+    m_pid.setSetpoint(3.14159 / 2);
+    return Commands.run(
+        () ->
+            m_motor.setVoltage(m_pid.calculate(Units.rotationsToRadians(m_encoder.getPosition()))));
+  }
+}