Initializing Climber Class

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

@@ -1,6 +1,7 @@
 package frc.robot;
 
 import com.revrobotics.spark.config.SparkMaxConfig;
+
 import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -280,21 +281,67 @@ public static class ManipulatorConstants { // CAN ID range 21-24
 	}
 
 	public static class ClimberConstants { // CAN ID range 25-29
+
+		/*
+		 * To Find:
+		 * initialEncoderValue
+		 * setEncoderValue
+		 * minPos
+		 * maxPos
+		 * kSetG
+		 * kSetP
+		 * kLightG
+		 * downVolts
+		 * upVolts
+		 * kClimbG
+		 * minVel
+		 * maxVel
+		 */
+
 		public static final int kClimberMotorCANID = 25;
 
-		public static final SparkMaxConfig climberConfig = new SparkMaxConfig();
+		public static final double upVolts = 0;
+		public static final double downVolts = 0;
+
+		public static final SparkMaxConfig climberSetConfig = new SparkMaxConfig();
+
+		// public static final double kEncoderConversionFactor = 1 * 2 * Math.PI;
+
+		public static final double kSetP = 0;
+		public static final double kSetI = 0;
+		public static final double kSetD = 0;
+
+		public static final double kSetG = 0;
+		public static final double kClimbG = 0;
+		public static final double kLightG = 0;
+
+		public static final double setEncoderValue = 0;
+		public static final double initialEncoderValue = 0;
+
+		public static final double minPos = 0;
+		public static final double maxPos = 0;
+
+		public static final double maxVel = 0;
+		public static final double minVel = 0;
 
 		static {
-			climberConfig
+			climberSetConfig
 				.idleMode(IdleMode.kBrake)
 				.smartCurrentLimit(30)
-				.voltageCompensation(12)
-			;
+				.voltageCompensation(12);
+			// climberSetConfig.encoder
+				// .positionConversionFactor(kEncoderConversionFactor);
+			climberSetConfig.closedLoop
+				.feedbackSensor((FeedbackSensor.kPrimaryEncoder))
+				.p(kSetP)
+				.i(kSetP)
+				.d(kSetD);
 		}
 	}
 
 	public static class OperatorConstants {
 		public static final int kDriverControllerPort = 0;
 		public static final int kAuxControllerPort = 1;
 	}
+
 }

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

@@ -4,12 +4,13 @@
 
 package frc.robot;
 
-import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.OperatorConstants;
+import frc.robot.subsystems.climber.Climber;
+import frc.robot.subsystems.climber.ClimberIOSpark;
 import frc.robot.subsystems.elevator.Elevator;
 import frc.robot.subsystems.elevator.ElevatorIO;
 import frc.robot.subsystems.elevator.ElevatorIOSpark;
@@ -26,11 +27,15 @@ public class RobotContainer {
     private SwerveDrive swerve;
     private Elevator elevator;
 
+    private Climber climber;
+
     public RobotContainer() {
         Preferences.removeAll();
         driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort);
         auxController = new CommandXboxController(OperatorConstants.kAuxControllerPort);
 
+        climber = new Climber(new ClimberIOSpark());
+
         switch(Constants.currentMode) {
             case REAL:
             case SIM: // no sim classes for now
@@ -72,6 +77,19 @@ private void configureBindings() {
         driverController.x().onTrue(swerve.runOpenTestDrive());
         driverController.x().onFalse(swerve.runStopDrive());
         driverController.b().onTrue(swerve.runUpdateControlConstants());
+
+        // climber.setDefaultCommand(climber.runClimber(
+        //     auxController.b()::getAsBoolean,
+        //     auxController.a()::getAsBoolean
+        // ));
+
+        auxController.a().onTrue(climber.runUp());
+        auxController.b().onTrue(climber.runDown());
+        auxController.a().onFalse(climber.stop());
+        auxController.b().onFalse(climber.stop());
+
+        auxController.x().onTrue(climber.gravity());
+        auxController.x().onFalse(climber.stop());
     }
 
     public Command getAutonomousCommand() {

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

@@ -0,0 +1,57 @@
+package frc.robot.subsystems.climber;
+
+import java.util.function.BooleanSupplier;
+
+import edu.wpi.first.wpilibj.Preferences;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Climber extends SubsystemBase{
+
+    private ClimberIO climber;
+
+    public Climber(ClimberIO climberIO) {
+        Preferences.initDouble("volts",0);
+        Preferences.initDouble("kG", 0);
+        Preferences.initDouble("P",0.1);
+        this.climber = climberIO;  
+    }
+
+    public Command runClimber(BooleanSupplier set, BooleanSupplier up, BooleanSupplier down, BooleanSupplier key) {
+        // return runOnce(() -> climberIO.setClimberVolts(low.getAsBoolean(), high.getAsBoolean()));
+        boolean bKey = key.getAsBoolean();
+        boolean bSet = set.getAsBoolean() && bKey;
+        boolean bUp = up.getAsBoolean() && bKey;
+        boolean bDown = down.getAsBoolean() && bKey;
+
+        return run(() -> climber.climberInputs(bSet, bUp, bDown));
+    }
+
+    public Command runUp() {
+        return runOnce( () -> climber.setVolts(Preferences.getDouble("volts",0)));
+    }
+
+    public Command runDown() {
+        return runOnce(() -> climber.setVolts(-Preferences.getDouble("volts",0)));
+    }
+
+    public Command stop() {
+        return runOnce(() -> climber.setVolts(0));
+    }
+
+    public Command gravity() {
+        return runOnce(() -> climber.setVolts(Preferences.getDouble("kG",0)));
+    }
+
+    public Command pid() {
+        return runOnce(() -> climber.setReference(Preferences.getDouble("P", 0)));
+    }
+
+    @Override
+    public void periodic() {
+        climber.update();
+        SmartDashboard.putNumber("encoder position", climber.getPos());
+
+    }
+}

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

@@ -0,0 +1,23 @@
+package frc.robot.subsystems.climber;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ClimberIO {
+    @AutoLog
+    public static class ClimberIOInputs {
+    }
+
+    public default void setClimberVolts(boolean low, boolean high) {}
+
+    public default void climberInputs(boolean set, boolean up, boolean down){}
+
+    public default void update() {}
+
+    public default double getPos() {return 0;}
+
+    public default void setVolts(double volts) {}
+
+    public default void setPos(double encoderValue, double kG) {}
+
+    public default void setReference(double p) {}
+}

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

@@ -0,0 +1,160 @@
+package frc.robot.subsystems.climber;
+
+import frc.robot.Constants.ClimberConstants;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+
+public class ClimberIOSpark implements ClimberIO {
+    private SparkMax climberMotor;
+    private RelativeEncoder encoder;
+    private SparkClosedLoopController pid;
+
+    private boolean prevSet;
+    private boolean prevUp;
+    private boolean prevDown;
+
+    private boolean up;
+    private boolean down;
+
+    private double kG;
+    private boolean isLocked;
+
+    public SparkMaxConfig climberSetConfig;
+
+    public ClimberIOSpark() {
+        climberMotor = new SparkMax(ClimberConstants.kClimberMotorCANID, MotorType.kBrushless);
+        climberMotor.setCANTimeout(0);
+        encoder = climberMotor.getEncoder();
+        pid = climberMotor.getClosedLoopController();
+        // climberMotor.configure(ClimberConstants.climberSetConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        // pid.setReference(ClimberConstants.initialEncoderValue, ControlType.kPosition, ClosedLoopSlot.kSlot0, ClimberConstants.kSetG, ArbFFUnits.kVoltage);
+
+        prevSet = true;
+        prevUp = false;
+        prevDown = false;
+
+        up = false;
+        down = false;
+
+        kG = ClimberConstants.kClimbG;
+        isLocked = true;
+
+        climberSetConfig = new SparkMaxConfig();
+        climberSetConfig
+				.idleMode(IdleMode.kBrake)
+				.smartCurrentLimit(30)
+				.voltageCompensation(12);
+			// climberSetConfig.encoder
+				// .positionConversionFactor(kEncoderConversionFactor);
+			climberSetConfig.closedLoop
+				.feedbackSensor((FeedbackSensor.kPrimaryEncoder))
+				.p(0)
+				.i(0)
+				.d(0);
+
+        climberMotor.configure(climberSetConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    @Override
+    public void climberInputs(boolean set, boolean up, boolean down) {
+        if(set && !prevSet && !up && !down) {
+            isLocked = false;
+            pid.setReference(
+                ClimberConstants.setEncoderValue,
+                ControlType.kPosition,
+                ClosedLoopSlot.kSlot0,
+                ClimberConstants.kSetG,
+                ArbFFUnits.kVoltage
+            );
+        }
+        if(up && !this.up) {
+            isLocked = false;
+            climberMotor.setVoltage(ClimberConstants.upVolts);
+        }
+        if(!up && down && !this.down) {
+            isLocked = false;
+            climberMotor.setVoltage(ClimberConstants.downVolts);
+        }
+        if(!this.up && ((!up && prevUp) || (!up && !down && prevDown))) {
+            isLocked = true;
+            pid.setReference(
+                encoder.getPosition(),
+                ControlType.kPosition,
+                ClosedLoopSlot.kSlot0,
+                kG,
+                ArbFFUnits.kVoltage
+            );
+        }
+        prevSet = set;
+        prevUp = up;
+        prevDown = down;
+    }
+
+    @Override
+    public void update() {
+        if(encoder.getPosition() > ClimberConstants.maxPos) {
+            up = true;
+            pid.setReference(
+                encoder.getPosition(),
+                ControlType.kPosition,
+                ClosedLoopSlot.kSlot0,
+                ClimberConstants.kSetG,
+                ArbFFUnits.kVoltage
+            );
+        }
+
+        if(encoder.getPosition() < ClimberConstants.minPos) {
+            down = true;
+            pid.setReference(
+                encoder.getPosition(),
+                ControlType.kPosition,
+                ClosedLoopSlot.kSlot0,
+                kG,
+                ArbFFUnits.kVoltage
+            );
+        }
+
+        if(isLocked && (encoder.getVelocity() > ClimberConstants.maxVel)) {
+            kG = ClimberConstants.kClimbG;
+        }
+
+        if(isLocked && (encoder.getVelocity() < ClimberConstants.minVel)) {
+            kG = ClimberConstants.kLightG;
+        }
+    }
+
+    @Override
+    public void setVolts(double volts) {
+        climberMotor.setVoltage(volts);
+    }
+
+    @Override
+    public double getPos() {
+        return encoder.getPosition();
+    }
+
+    @Override
+    public void setPos(double volts, double kG) {
+        pid.setReference(volts, ControlType.kPosition, ClosedLoopSlot.kSlot0, kG, ArbFFUnits.kVoltage);
+    }
+
+    @Override
+    public void setReference(double p) {
+        climberSetConfig.closedLoop
+            .p(p);
+        climberMotor.configure(climberSetConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        pid.setReference(0,ControlType.kPosition, ClosedLoopSlot.kSlot0, 0, ArbFFUnits.kVoltage);
+    }
+}