Climber Subsytem

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

@@ -59,6 +59,14 @@ public static final class Intake {
             public static final int INTAKE_MOTOR_ID = -1;
             public static final int INDEXER_MOTOR_ID = -1;
         }
+
+        /**
+         * Climber motor constants
+         */
+        public static final class Climber {
+            public static final int LEFT_MOTOR_ID = -1;
+            public static final int RIGHT_MOTOR_ID = -1;
+        }
     }
 
     /**
@@ -195,6 +203,26 @@ public static final class Mod3 {
                 maxSpeed, MOD0_MODOFFSET.getNorm(), new ReplanningConfig());
     }
 
+    /**
+     * Climber constants
+     */
+    public static final class ClimberConstants {
+        public static final double CLIMBER_KP = 0;
+        public static final double CLIMBER_KI = 0;
+        public static final double CLIMBER_KD = 0;
+        public static final double CLIMBER_MAX_VELOCITY = 0;
+        public static final double CLIMBER_MAX_ACCELERATION = 0;
+        public static final double CLIMBER_KS = 0;
+        public static final double CLIMBER_KG = 0;
+        public static final double CLIMBER_KV = 0;
+
+        public static final double CLIMBING_DISTANCE = Units.inchesToMeters(15);
+        public static final double MAX_CLIMBING_DISTANCE = Units.inchesToMeters(21);
+
+        // 2pi * radius
+        public static final double LINEAR_DISTANCE = Units.inchesToMeters(2 * Math.PI * 1);
+    }
+
     /**
      * Auto constants
      */
@@ -265,15 +293,12 @@ public static final class SetPoints {
             public static final Rotation2d AMP_ANGLE = Rotation2d.fromDegrees(-10);
             public static final double TRAP_HEIGHT = Units.inchesToMeters(40);
             public static final Rotation2d TRAP_ANGLE = Rotation2d.fromDegrees(30);
-
-            public static final double CLIMBING_HEIGHT = Units.inchesToMeters(0);
+            public static final double MAX_EXTENSION = Units.inchesToMeters(48);
+            public static final double CLIMBING_HEIGHT = Units.inchesToMeters(15);
             public static final Rotation2d CLIMBING_ANGLE = Rotation2d.fromDegrees(0);
 
-            public static final double MAX_EXTENSION = Units.inchesToMeters(48);
-            public static final Rotation2d MAX_ANGLE_UP_HOME = Rotation2d.fromDegrees(85);
-            public static final Rotation2d MAX_ANGLE_DOWN_HOME = Rotation2d.fromDegrees(-15);
-            public static final Rotation2d MAX_ANGLE_UP_EXTENDED = Rotation2d.fromDegrees(85);
-            public static final Rotation2d MAX_ANGLE_DOWN_EXTENDED = Rotation2d.fromDegrees(-15);
+            public static final double LINEAR_DISTANCE = Units.inchesToMeters(2 * Math.PI * 659);
+
         }
 
 

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

@@ -3,6 +3,7 @@
 import java.util.function.BooleanSupplier;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
+import frc.robot.subsystems.climber.Climber;
 import frc.robot.subsystems.elevator_wrist.ElevatorWrist;
 import frc.robot.subsystems.intake.Intake;
 
@@ -27,4 +28,26 @@ public static Command runIntake(Intake intake, ElevatorWrist elevatorWrist) {
             intake.runIntakeMotor(Constants.IntakeConstants.INDEX_MOTOR_FORWARD);
         return moveElevatorWrist.andThen(runIntakeIndexer).unless(sensor);
     }
+
+    /**
+     * Command to climb robot, then set up position to score.
+     *
+     * @param climber Climber subsystem
+     * @param elevatorWrist Elevator and Wrist subsystem
+     * @return Returns auto climb command
+     */
+    public static Command autoClimb(Climber climber, ElevatorWrist elevatorWrist) {
+        Command initialExtension =
+            elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.CLIMBING_HEIGHT,
+                Constants.ElevatorWristConstants.SetPoints.HOME_ANGLE);
+        Command hooksAttach =
+            elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT,
+                Constants.ElevatorWristConstants.SetPoints.HOME_ANGLE);
+        Command climb = climber.getToPosition(Constants.ClimberConstants.CLIMBING_DISTANCE);
+        Command extendToTrap =
+            elevatorWrist.goToPosition(Constants.ElevatorWristConstants.SetPoints.TRAP_HEIGHT,
+                Constants.ElevatorWristConstants.SetPoints.TRAP_ANGLE);
+        return initialExtension.andThen(hooksAttach).andThen(climb).andThen(extendToTrap);
+
+    }
 }

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

@@ -0,0 +1,116 @@
+package frc.robot.subsystems.climber;
+
+import java.util.function.BooleanSupplier;
+import org.littletonrobotics.junction.Logger;
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+/**
+ * Climber subsystem.
+ */
+public class Climber extends SubsystemBase {
+    public ClimberIO io;
+    public ClimberInputsAutoLogged inputs = new ClimberInputsAutoLogged();
+
+    PIDController leftClimberPIDController =
+        new PIDController(Constants.ClimberConstants.CLIMBER_KP,
+            Constants.ClimberConstants.CLIMBER_KI, Constants.ClimberConstants.CLIMBER_KD);
+    PIDController rightClimberPIDController =
+        new PIDController(Constants.ClimberConstants.CLIMBER_KP,
+            Constants.ClimberConstants.CLIMBER_KI, Constants.ClimberConstants.CLIMBER_KD);
+
+    private ElevatorFeedforward climberFeedforward =
+        new ElevatorFeedforward(Constants.ClimberConstants.CLIMBER_KS,
+            Constants.ClimberConstants.CLIMBER_KG, Constants.ClimberConstants.CLIMBER_KV);
+
+    public Climber(ClimberIO io) {
+        this.io = io;
+        io.updateInputs(inputs);
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Climber", inputs);
+        double leftClimberPIDValue =
+            leftClimberPIDController.calculate(leftClimberDistanceTraveled());
+        double rightClimberPIDValue =
+            rightClimberPIDController.calculate(rightClimberDistanceTraveled());
+        double climberFeedForwardValue =
+            climberFeedforward.calculate(0, 0, leftClimberPIDController.getPeriod());
+        io.setLeftClimberVoltage(climberFeedForwardValue + leftClimberPIDValue);
+        io.setRightClimberVoltage(climberFeedForwardValue + rightClimberPIDValue);
+        Logger.recordOutput("/Climber/VoltageFromFeedForward/LeftClimber", climberFeedForwardValue);
+    }
+
+    /**
+     * Sets voltage for left side climber system
+     *
+     * @param power Sets power for climbing motors.
+     */
+    public void setLeftClimberVoltage(double power) {
+        Logger.recordOutput("/Climber/Voltage", power);
+        io.setLeftClimberVoltage(power);
+
+    }
+
+    /**
+     * Sets voltage for right side climber system
+     *
+     * @param power Sets power for climbing motors.
+     */
+    public void setRightClimberVoltage(double power) {
+        Logger.recordOutput("/Climber/Voltage", power);
+        io.setRightClimberVoltage(power);
+
+    }
+
+    /**
+     * Climbs to designated position.
+     *
+     * @param distance Distance to climb
+     * @return Returns a usable command
+     */
+    public Command getToPosition(double distance) {
+        BooleanSupplier end =
+            () -> leftClimberPIDController.atSetpoint() && rightClimberPIDController.atSetpoint();
+        return Commands.runOnce(() -> {
+            leftClimberPIDController.setSetpoint(distance);
+            rightClimberPIDController.setSetpoint(distance);
+        }).andThen(Commands.run(() -> {
+            leftClimberPIDController.calculate(leftClimberDistanceTraveled());
+            rightClimberPIDController.calculate(leftClimberDistanceTraveled());
+            double leftClimberPIDValue =
+                leftClimberPIDController.calculate(leftClimberDistanceTraveled());
+            double rightClimberPIDValue =
+                rightClimberPIDController.calculate(rightClimberDistanceTraveled());
+            Logger.recordOutput("/Climber/VoltageFromPID/LeftClimber", leftClimberPIDValue);
+            Logger.recordOutput("/Climber/VoltageFromPID/RightClimber", rightClimberPIDValue);
+        }).until(end));
+
+    }
+
+    /**
+     * Get the height in meters of the elevator based on the rotations of the motor
+     *
+     * @return Height of elevator in meters
+     */
+    public double leftClimberDistanceTraveled() {
+        return inputs.leftMotorEncoderValue * Constants.ClimberConstants.LINEAR_DISTANCE;
+    }
+
+    /**
+     * Get the height in meters of the elevator based on the rotations of the motor
+     *
+     * @return Height of elevator in meters
+     */
+    public double rightClimberDistanceTraveled() {
+        return inputs.rightMotorEncoderValue * Constants.ClimberConstants.LINEAR_DISTANCE;
+    }
+
+
+}

src/main/java/frc/robot/subsystems/climber/ClimberIO.java

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.climber;
+
+import org.littletonrobotics.junction.AutoLog;
+
+/**
+ * Subsystem for climber IO
+ */
+public interface ClimberIO {
+
+    /**
+     * Auto logs designated climber inputs
+     */
+    @AutoLog
+    public static class ClimberInputs {
+        public double climberLeftMotorVoltage;
+        public double climberLeftMotorAmp;
+        public double climberRightMotorVoltage;
+        public double climberRightMotorAmp;
+        public double leftMotorEncoderValue;
+        public double rightMotorEncoderValue;
+
+    }
+
+    public default void updateInputs(ClimberInputs inputs) {}
+
+    public default void setLeftClimberVoltage(double volts) {}
+
+    public default void setRightClimberVoltage(double volts) {}
+}

src/main/java/frc/robot/subsystems/climber/ClimberNEO.java

@@ -0,0 +1,49 @@
+package frc.robot.subsystems.climber;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import frc.robot.Constants;
+
+/**
+ * Subsystem for physical climber.
+ */
+public class ClimberNEO implements ClimberIO {
+    public final CANSparkMax leftMotor =
+        new CANSparkMax(Constants.Motors.Climber.LEFT_MOTOR_ID, MotorType.kBrushless);
+    public final CANSparkMax rightMotor =
+        new CANSparkMax(Constants.Motors.Climber.RIGHT_MOTOR_ID, MotorType.kBrushless);
+    public final RelativeEncoder leftRelativeEncoder = leftMotor.getEncoder();
+    public final RelativeEncoder rightRelativeEncoder = leftMotor.getEncoder();
+
+
+    /**
+     * Constructor for climberNEO subsystem.
+     */
+    public ClimberNEO() {
+        rightMotor.follow(leftMotor, true);
+        leftRelativeEncoder.setPositionConversionFactor(25);
+        rightRelativeEncoder.setPositionConversionFactor(25);
+    }
+
+    @Override
+    public void updateInputs(ClimberInputs inputs) {
+        inputs.climberLeftMotorVoltage = leftMotor.getBusVoltage();
+        inputs.climberLeftMotorAmp = leftMotor.getOutputCurrent();
+        inputs.climberRightMotorVoltage = rightMotor.getBusVoltage();
+        inputs.climberRightMotorAmp = rightMotor.getOutputCurrent();
+        inputs.leftMotorEncoderValue = leftRelativeEncoder.getPosition();
+        inputs.rightMotorEncoderValue = rightRelativeEncoder.getPosition();
+
+    }
+
+    @Override
+    public void setLeftClimberVoltage(double voltage) {
+        leftMotor.setVoltage(voltage);
+    }
+
+    @Override
+    public void setRightClimberVoltage(double voltage) {
+        leftMotor.setVoltage(voltage);
+    }
+}

src/main/java/frc/robot/subsystems/elevator_wrist/ElevatorWrist.java

@@ -61,7 +61,7 @@ public void periodic() {
         double wristPIDValue = wristPIDController.calculate(inputs.wristAbsoluteEncRawValue);
 
         double elevatorFeedForwardValue =
-            elevatorFeedForward.calculate(0, 0, wristPIDController.getPeriod());
+            elevatorFeedForward.calculate(0, 0, elevatorPIDController.getPeriod());
 
         double wristFeedForwardValue =
             wristFeedForward.calculate(0, 0, wristPIDController.getPeriod());
@@ -120,6 +120,7 @@ public Command followPosition(DoubleSupplier height, Supplier<Rotation2d> angle)
      * @return Height of elevator in meters
      */
     public double elevatorDistanceTraveled() {
-        return inputs.elevatorRelativeEncRawValue * 0.111715034761762;
+        return inputs.elevatorRelativeEncRawValue
+            * Constants.ElevatorWristConstants.SetPoints.LINEAR_DISTANCE;
     }
 }