First pass at climber subsystem

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

@@ -30,6 +30,7 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.components.rollers.RollerIO;
 import frc.robot.components.rollers.RollerIOSim;
+import frc.robot.subsystems.climber.ClimberIO;
 import frc.robot.subsystems.climber.ClimberSubsystem;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.indexer.LindexerSubsystem;
@@ -260,7 +261,7 @@ public Robot() {
         indexer = new SpindexerSubsystem();
         intake = new LintakeSubsystem();
         shooter = new TurretSubsystem();
-        climber = new ClimberSubsystem(); // TODO climber
+        climber = new ClimberSubsystem();
         break;
     }
     // now that we've assigned the correct subsystems based on robot edition, we can pass them into
@@ -270,7 +271,7 @@ public Robot() {
     // this creates a placeholder "no-operation" climber that will just not do anything, but is not
     // null (and we need it to be not null)
     if (climber == null)
-      climber = new ClimberSubsystem(); // TODO new ClimberSubsystem(new ClimberIO() {}) and such
+      climber = new ClimberSubsystem(new ClimberIO(canivore) {});
 
     DriverStation.silenceJoystickConnectionWarning(true);
     SignalLogger.enableAutoLogging(false);

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

@@ -0,0 +1,137 @@
+package frc.robot.subsystems.climber;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import org.littletonrobotics.junction.AutoLogOutput;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Temperature;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Frequency;
+import frc.robot.subsystems.shooter.HoodIO;
+
+public class ClimberIO {
+
+    @AutoLog
+    public static class ClimberIOInputs {
+        public Rotation2d motorPositionRotations = new Rotation2d();
+        public double motorVelocityMetersPerSec = 0.0;
+        public double motorStatorCurrentAmps = 0.0;
+        public double motorSupplyCurrentAmps = 0.0;
+        public double motorVoltage = 0.0;
+        public double motorTempC = 0.0;
+    }
+
+    protected final TalonFX climberMotor;
+
+    private final StatusSignal<Angle> motorPositionRotations;
+    private final StatusSignal<AngularVelocity> angularVelocityRotsPerSec;
+    private final StatusSignal<Current> statorCurrentAmps;
+    private final StatusSignal<Current> supplyCurrentAmps;
+    private final StatusSignal<Voltage> motorVoltage;
+    private final StatusSignal<Temperature> motorTemp;
+
+    private VoltageOut voltageOut = new VoltageOut(0.0) .withEnableFOC(true);
+    private PositionVoltage positionVoltage = new PositionVoltage(0.0) .withEnableFOC(true);
+    private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0) .withEnableFOC(true);
+
+    private Rotation2d climberSetpoint = Rotation2d.kZero;
+
+
+public ClimberIO(CANBus canBus) {
+    //todo: set correct motor ID
+    climberMotor = new TalonFX(30, canBus);
+    climberMotor.getConfigurator().apply(ClimberIO.getClimberConfiguration());
+
+    angularVelocityRotsPerSec = climberMotor.getVelocity();
+    supplyCurrentAmps = climberMotor.getSupplyCurrent();
+    motorVoltage = climberMotor.getMotorVoltage();
+    statorCurrentAmps = climberMotor.getStatorCurrent();
+    motorTemp = climberMotor.getDeviceTemp();
+    motorPositionRotations = climberMotor.getPosition();
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        50.0,
+        angularVelocityRotsPerSec,
+        supplyCurrentAmps,
+        motorVoltage,
+        statorCurrentAmps,
+        motorTemp,
+        motorPositionRotations);
+    climberMotor.optimizeBusUtilization();
+ }
+
+public static TalonFXConfiguration getClimberConfiguration() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+    //todo: find and make climber gear ratio variable
+    config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO;
+
+    //todo: tune 
+    config.Slot0.kS = 0.0;
+    config.Slot0.kG = 0.0;
+    config.Slot0.kV = 0.0;
+    config.Slot0.kP = 0.0;
+    config.Slot0.kD = 0.0;
+
+    //todo: find actual current limits
+    config.CurrentLimits.StatorCurrentLimit = 50.00;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.CurrentLimits.SupplyCurrentLimit = 40.00;
+
+    return config;
+}
+
+public void setClimberPosition(Rotation2d climberPosition) {
+    climberSetpoint = climberPosition;
+    climberMotor.setControl(positionVoltage.withPosition(climberPosition.getRotations()));
+}
+
+public void setClimberVoltage(double climberVoltage) {
+    climberMotor.setControl(voltageOut.withOutput(climberVoltage));
+}
+
+public void setClimberVelocity(double climberVelocity) {
+    climberMotor.setControl(velocityVoltage.withVelocity(climberVelocity));
+}
+
+public void updateInputs(ClimberIOInputs inputs) {
+  BaseStatusSignal.refreshAll(
+    motorPositionRotations,
+    angularVelocityRotsPerSec,
+    statorCurrentAmps,
+    supplyCurrentAmps,
+    motorVoltage,
+    motorTemp);
+
+  inputs.motorPositionRotations = 
+      Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble());
+  inputs.motorVoltage = motorVoltage.getValueAsDouble();
+  inputs.motorTempC = motorTemp.getValueAsDouble();
+  inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble();
+  inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble();
+  inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble();
+}
+}
+
+
+
+

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

@@ -1,17 +1,55 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
 package frc.robot.subsystems.climber;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.components.rollers.RollerIO;
+import frc.robot.components.rollers.RollerIOInputsAutoLogged;
+import org.littletonrobotics.junction.Logger;
+
+
 
 public class ClimberSubsystem extends SubsystemBase {
-  /** Creates a new ClimberSubsystem. */
-  public ClimberSubsystem() {}
+  //todo: find actual constants
+  public static double GEAR_RATIO = (45.0 / 1.0);
+  public static Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(180);
+  public static Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(0);
+  public static double MAX_ACCELERATION = 10.0;
+  public static double MAX_VELOCITY = 2.0;
+
+ClimberIO climberIO;
+ClimberIOInputsAutoLogged climberInputs = new ClimberIOInputsAutoLogged();
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    climberIO.updateInputs(climberInputs);
+  Logger.processInputs("Climber", climberInputs);
   }
+
+//member variables here?
+
+public ClimberSubsystem(ClimberIO climberIO) {
+  this.climberIO = climberIO;
+}
+
+public Command climbUp() {
+  return this.run(
+    () -> {
+      climberIO.setClimberPosition(MAX_ANGLE);
+    });
+
+}
+
+public Command climbDown() {
+  return this.run(
+    () -> {
+      climberIO.setClimberPosition(MIN_ANGLE);
+    });
+
+}
+
 }