Beginnings of a climber subsystem

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

@@ -30,6 +30,7 @@ import frc.robot.constants.OIConstants;
 import frc.robot.constants.ShooterConstants;
 import frc.robot.constants.IntakePivotConstants.IntakePivotPosition;
 import frc.robot.constants.ShooterConstants.ShooterSpeeds;
+import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Hood;
 import frc.robot.subsystems.IntakePivot;
@@ -48,6 +49,7 @@ public class RobotContainer {
     private IntakePivot intakePivot;
     private IntakeRoller intakeRoller;
     private Spindexer spindexer;
+    private Climber climber;
 
     private CommandXboxController driver;
     private CommandXboxController secondary;
@@ -64,6 +66,7 @@ public class RobotContainer {
         intakePivot = new IntakePivot();
         intakeRoller = new IntakeRoller();
         spindexer = new Spindexer();
+        climber = new Climber();
 
         vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
         vision.addPoseEstimateConsumer((vp) -> {
@@ -143,6 +146,17 @@ public class RobotContainer {
         intakeRoller.setDefaultCommand(intakeRoller.stop());
         hood.setDefaultCommand(hood.stop());
         spindexer.setDefaultCommand(spindexer.stop());
+        climber.setDefaultCommand(climber.stop());
+
+        // While holding POV up of the driver controller, the climber
+        // should move such that its motor moves the climber down with the left
+        // driver controller trigger axis, and up with the right driver controller
+        // trigger axis.
+        // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted
+        // from the constants file for the subsystem having the problem. 
+        driver.povUp().whileTrue(climber.manualSpeed(() -> {
+            return driver.getLeftTriggerAxis() * -1 + driver.getRightTriggerAxis();
+        }));
 
         // While holding the right bumper of the driver controller, the intake rollers
         // and the spindexer and feeder should move such that all motors are moving in such a way
@@ -209,6 +223,7 @@ public class RobotContainer {
         secondary.y().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kUp));
 
         // TODO Some means of testing hood PIDF
+        // TODO Some means of testing climber PIDF
     }
   
     private void configureBindings() {

src/main/java/frc/robot/constants/ClimberConstants.java

@@ -0,0 +1,61 @@
+package frc.robot.constants;
+
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+public class ClimberConstants {
+    // TODO Real values
+    public enum ClimberPositions {
+        kStow(0),
+        kClimbOffGround(0),
+        kUp(0);
+
+        private double positionMeters;
+
+        private ClimberPositions(double positionMeters) {
+            this.positionMeters = positionMeters;
+        }
+
+        public double getPositionMeters() {
+            return positionMeters;
+        }
+    }
+
+    public static final int kMotorCANID = 0;
+
+    public static final double kConversionFactor = 0;
+
+    public static final double kP = 0;
+    public static final double kI = 0;
+    public static final double kD = 0;
+    public static final double kS = 0;
+    public static final double kV = 0;
+    public static final double kA = 0;
+
+    public static final boolean kMotorInverted = false;
+
+    public static final int kCurrentLimit = 40;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig();
+
+    static {
+        kMotorConfig
+            .inverted(kMotorInverted)
+            .smartCurrentLimit(kCurrentLimit)
+            .idleMode(kIdleMode);
+        kMotorConfig.encoder
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
+        kMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .feedForward
+                .sva(kS, kV, kA);
+    }
+}

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

@@ -0,0 +1,74 @@
+package frc.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ClimberConstants;
+import frc.robot.constants.ClimberConstants.ClimberPositions;
+
+public class Climber extends SubsystemBase {
+    private SparkMax motor;
+
+    private RelativeEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    private ClimberPositions targetPosition;
+
+    public Climber() {
+        motor = new SparkMax(ClimberConstants.kMotorCANID, MotorType.kBrushless);
+
+        motor.configure(
+            ClimberConstants.kMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        encoder = motor.getEncoder();
+
+        controller = motor.getClosedLoopController();
+
+        targetPosition = null;
+    }
+
+    @Override
+    public void periodic() {
+        Logger.recordOutput("Climber/TargetPositionMeters", targetPosition == null ? -1 : targetPosition.getPositionMeters());
+        Logger.recordOutput("Climber/CurrentPositionMeters", encoder.getPosition());
+        Logger.recordOutput("Climber/AtSetpoint", controller.isAtSetpoint());
+    }
+
+    public Command maintainPosition(ClimberPositions position) {
+        targetPosition = position;
+
+        return run(() -> {
+            controller.setSetpoint(
+                position.getPositionMeters(), 
+                ControlType.kPosition
+            );
+        });
+    }
+
+    public Command manualSpeed(DoubleSupplier speed) {
+        targetPosition = null;
+
+        return run(() -> {
+            motor.set(speed.getAsDouble());
+        });
+    }
+
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+}