Merge branch 'main' into ShooterFinish

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

@@ -101,6 +101,7 @@ private DriveConstants() {
     );
   }
 
+
   public static class ShooterConstants {
     private ShooterConstants() {
       throw new IllegalStateException("Static classes should not be constructed");
@@ -122,4 +123,24 @@ private ShooterConstants() {
     public static final boolean BOTTOM_RIGHT_INVERTED = false;
     public static final boolean KICKER_INVERTED = true;
   }
+
+  public class ClimberConstants {
+    private ClimberConstants() {
+      throw new IllegalStateException("Static class should not be constructed");
+    }
+
+    public static final int LEFT_CLIMBER_ID = 18;
+    public static final int RIGHT_CLIMBER_ID = 19;
+
+    public static final double CLIMBER_KP = 0.0;
+    public static final double CLIMBER_KI = 0.0;
+    public static final double CLIMBER_KD = 0.0;
+
+    public static final double CLIMBER_KV = 0.0;
+    public static final double CLIMBER_KS = 0.0;
+    public static final double CLIMBER_KA = 0.0;
+
+    public static final double CLIMBER_GEAR_RATIO = 0.0;
+    public static final double CLIMBER_CONVERSION_FACTOR_INCHES = 0.0;
+  }
 }

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

@@ -1,7 +1,105 @@
 package frc.robot.subsystems.climber;
 
+import org.littletonrobotics.junction.AutoLog;
+
 public interface ClimberIO {
+  @AutoLog
+  class ClimberIOInputs {
+    protected double leftClimberPosition = 0.0;
+    protected double rightClimberPosition = 0.0;
+    protected double leftClimberVelocity = 0.0;
+    protected double rightClimberVelocity = 0.0;
+    protected double leftClimberCurrentDraw = 0.0;
+    protected double rightClimberCurrentDraw = 0.0;
+    protected double leftClimberCurrentSetpoint = 0.0;
+    protected double rightClimberCurrentSetpoint = 0.0;
+    protected double leftClimberAppliedOutput = 0.0;
+    protected double rightClimberAppliedOutput = 0.0;
+
+    public double getLeftClimberPosition() {
+      return leftClimberPosition;
+    }
+
+    public void setLeftClimberPosition(double leftClimberPosition) {
+      this.leftClimberPosition = leftClimberPosition;
+    }
+
+    public double getRightClimberPosition() {
+      return rightClimberPosition;
+    }
+
+    public void setRightClimberPosition(double rightClimberPosition) {
+      this.rightClimberPosition = rightClimberPosition;
+    }
+
+    public double getLeftClimberVelocity() {
+      return leftClimberVelocity;
+    }
+
+    public void setLeftClimberVelocity(double leftClimberVelocity) {
+      this.leftClimberVelocity = leftClimberVelocity;
+    }
+
+    public double getRightClimberVelocity() {
+      return rightClimberVelocity;
+    }
+
+    public void setRightClimberVelocity(double rightClimberVelocity) {
+      this.rightClimberVelocity = rightClimberVelocity;
+    }
+
+    public double getLeftClimberCurrentDraw() {
+      return leftClimberCurrentDraw;
+    }
+
+    public void setLeftClimberCurrentDraw(double leftClimberCurrentDraw) {
+      this.leftClimberCurrentDraw = leftClimberCurrentDraw;
+    }
+
+    public double getRightClimberCurrentDraw() {
+      return rightClimberCurrentDraw;
+    }
+
+    public void setRightClimberCurrentDraw(double rightClimberCurrentDraw) {
+      this.rightClimberCurrentDraw = rightClimberCurrentDraw;
+    }
+
+    public double getLeftClimberCurrentSetpoint() {
+      return leftClimberCurrentSetpoint;
+    }
+
+    public void setLeftClimberCurrentSetpoint(double leftClimberCurrentSetpoint) {
+      this.leftClimberCurrentSetpoint = leftClimberCurrentSetpoint;
+    }
+
+    public double getRightClimberCurrentSetpoint() {
+      return rightClimberCurrentSetpoint;
+    }
+
+    public void setRightClimberCurrentSetpoint(double rightClimberCurrentSetpoint) {
+      this.rightClimberCurrentSetpoint = rightClimberCurrentSetpoint;
+    }
+
+    public double getLeftClimberAppliedOutput() {
+      return leftClimberAppliedOutput;
+    }
+
+    public void setLeftClimberAppliedOutput(double leftClimberAppliedOutput) {
+      this.leftClimberAppliedOutput = leftClimberAppliedOutput;
+    }
+
+    public double getRightClimberAppliedOutput() {
+      return rightClimberAppliedOutput;
+    }
+
+    public void setRightClimberAppliedOutput(double rightClimberAppliedOutput) {
+      this.rightClimberAppliedOutput = rightClimberAppliedOutput;
+    }
+  }
 
-    default void setLeftVoltage(double voltage) {}
-    default void setRightVoltage(double voltage) {}
+  default void setRightVoltage(double volts) {}
+  default void setLeftVoltage(double volts) {}
+  default void setRightPosition(double degrees) {}
+  default void setLeftPosition(double degrees) {}
+  default void updateInputs(ClimberIOInputsAutoLogged inputs) {}
 }

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

@@ -1,36 +1,88 @@
 package frc.robot.subsystems.climber;
 
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
-import edu.wpi.first.math.MathUtil;
+import frc.robot.Constants.ClimberConstants;
+import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
+import lib.properties.phoenix6.PidPropertyPublic;
 
 public class ClimberIOPrototype implements ClimberIO {
-    private final TalonFX m_leftTalon;
-    private final TalonFX m_rightTalon;
-
-    public ClimberIOPrototype() {
-        m_leftTalon = new TalonFX(18);
-        m_rightTalon = new TalonFX(19);
-
-        var rightTalonConfig = new TalonFXConfiguration();
-        rightTalonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-        m_rightTalon.getConfigurator().apply(rightTalonConfig);
-
-        var leftTalonConfig = new TalonFXConfiguration();
-        m_leftTalon.getConfigurator().apply(leftTalonConfig);
-    }
-
-    @Override
-    public void setRightVoltage(double voltage) {
-        double clampedVoltage = MathUtil.clamp(voltage, -12, 12);
-        m_rightTalon.setVoltage(clampedVoltage);
-    }
-
-    @Override
-    public void setLeftVoltage(double voltage) {
-        double clampedVoltage = MathUtil.clamp(voltage, -12, 12);
-        m_leftTalon.setVoltage(clampedVoltage);
-    }
+  private final TalonFX m_leftTalon;
+  private final TalonFX m_rightTalon;
+  private final PidPropertyPublic m_leftClimberPid;
+  private final PidPropertyPublic m_rightClimberPid;
+
+  public ClimberIOPrototype() {
+    m_leftTalon = new TalonFX(ClimberConstants.LEFT_CLIMBER_ID);
+    m_rightTalon = new TalonFX(ClimberConstants.RIGHT_CLIMBER_ID);
+
+    var rightTalonConfig = new TalonFXConfiguration();
+    rightTalonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    m_rightTalon.getConfigurator().apply(rightTalonConfig);
+
+    var leftTalonConfig = new TalonFXConfiguration();
+    m_leftTalon.getConfigurator().apply(leftTalonConfig);
+
+    m_leftClimberPid = new Phoenix6PidPropertyBuilder(
+        "Climber/Left PID",
+        false, m_leftTalon, 0)
+        .addP(ClimberConstants.CLIMBER_KP)
+        .addI(ClimberConstants.CLIMBER_KI)
+        .addD(ClimberConstants.CLIMBER_KD)
+        .build();
+
+    m_rightClimberPid = new Phoenix6PidPropertyBuilder(
+        "Climber/Right PID",
+        false, m_rightTalon, 0)
+        .addP(ClimberConstants.CLIMBER_KP)
+        .addI(ClimberConstants.CLIMBER_KI)
+        .addD(ClimberConstants.CLIMBER_KD)
+        .build();
+
+  }
+
+  @Override
+  public void setLeftVoltage(double volts) {
+    m_leftTalon.setVoltage(volts);
+  }
+
+  @Override
+  public void setRightVoltage(double volts) {
+    m_rightTalon.setVoltage(volts);
+  }
+
+  @Override
+  public void setLeftPosition(double degrees) {
+    final PositionVoltage request = new PositionVoltage(0).withSlot(0);
+    m_leftTalon.setControl(request.withPosition(degrees / 360.0));
+  }
+
+  @Override
+  public void setRightPosition(double degrees) {
+    final PositionVoltage request = new PositionVoltage(0).withSlot(0);
+    m_rightTalon.setControl(request.withPosition(degrees / 360.0));
+  }
+
+  @Override
+  public void updateInputs(ClimberIOInputsAutoLogged inputs) {
+    m_leftClimberPid.updateIfChanged();
+    m_rightClimberPid.updateIfChanged();
+
+    inputs.setLeftClimberPosition(m_leftTalon.getPosition().getValueAsDouble());
+    inputs.setRightClimberPosition(m_rightTalon.getPosition().getValueAsDouble());
+
+    inputs.setLeftClimberVelocity(m_leftTalon.getVelocity().getValueAsDouble());
+    inputs.setRightClimberVelocity(m_rightTalon.getVelocity().getValueAsDouble());
+
+    inputs.setLeftClimberCurrentDraw(m_leftTalon.getSupplyCurrent().getValueAsDouble());
+    inputs.setRightClimberCurrentDraw(m_rightTalon.getSupplyCurrent().getValueAsDouble());
+
+    inputs.setLeftClimberCurrentSetpoint(m_leftTalon.getClosedLoopReference().getValueAsDouble());
+    inputs.setRightClimberCurrentSetpoint(m_rightTalon.getClosedLoopReference().getValueAsDouble());
+
+    inputs.setLeftClimberAppliedOutput(m_leftTalon.getBridgeOutput().getValueAsDouble());
+    inputs.setRightClimberAppliedOutput(m_rightTalon.getBridgeOutput().getValueAsDouble());
+  }
 }

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

@@ -3,20 +3,29 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
 
 public class ClimberSubsystem extends SubsystemBase {
-    private final ClimberIO m_io;
-    public ClimberSubsystem(ClimberIO io) {
-        m_io = io;
-    }
+  private final ClimberIO m_io;
+  private final ClimberIOInputsAutoLogged m_inputs;
+  public ClimberSubsystem(ClimberIO io) {
+    m_io = io;
+    m_inputs = new ClimberIOInputsAutoLogged();
+  }
 
-    public void setClimberPower(double power) {
-        m_io.setLeftVoltage(power * 12.0);
-        m_io.setRightVoltage(power * 12.0);
-    }
+  @Override
+  public void periodic() {
+    m_io.updateInputs(m_inputs);
+    Logger.processInputs("Climber", m_inputs);
+  }
 
-    public Command setClimberPowerFactory(double power) {
-        return run(() -> setClimberPower(power));
-    }
+  public void setClimberPower(double power) {
+    m_io.setRightVoltage(power * 12.0);
+    m_io.setLeftVoltage(power * 12.0);
+  }
+
+  public Command setClimberPowerFactory(double power) {
+    return run(() -> setClimberPower(power));
+  }
 }
 

src/main/java/frc/robot/subsystems/drive/module/Module.java

@@ -90,7 +90,7 @@ public void periodic() {
 
     // Run closed loop turn control
     if (m_angleSetpoint != null) {
-      m_io.setTurnVoltage(
+      m_io.setTurnPositionDegs(
           m_turnFeedback.calculate(getAngle().getRadians(), m_angleSetpoint.getRadians()));
 
       // Run closed loop drive control
@@ -105,7 +105,7 @@ public void periodic() {
 
         // Run drive controller
         double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS;
-        m_io.setDriveVoltage(
+        m_io.setDriveVelocityMPS(
             m_driveFeedforward.calculate(velocityRadPerSec)
                 + m_driveFeedback.calculate(m_inputs.getDriveVelocityRadPerSec(), velocityRadPerSec));
       }
@@ -144,14 +144,14 @@ public void runCharacterization(double volts) {
     m_angleSetpoint = new Rotation2d();
 
     // Open loop drive control
-    m_io.setDriveVoltage(volts);
+    m_io.setDriveVelocityMPS(volts);
     m_speedSetpoint = null;
   }
 
   /** Disables all outputs to motors. */
   public void stop() {
-    m_io.setTurnVoltage(0.0);
-    m_io.setDriveVoltage(0.0);
+    m_io.setTurnPositionDegs(0.0);
+    m_io.setDriveVelocityMPS(0.0);
 
     // Disable closed loop control for turn and drive
     m_angleSetpoint = null;

src/main/java/frc/robot/subsystems/drive/module/ModuleIO.java

@@ -126,10 +126,10 @@ public void setOdometryTurnPositions(Rotation2d[] odometryTurnPositions) {
   default void updateInputs(ModuleIOInputs inputs) {}
 
   /** Run the drive motor at the specified voltage. */
-  default void setDriveVoltage(double volts) {}
+  default void setDriveVelocityMPS(double mps) {}
 
   /** Run the turn motor at the specified voltage. */
-  default void setTurnVoltage(double volts) {}
+  default void setTurnPositionDegs(double degrees) {}
 
   /** Enable or disable brake mode on the drive motor. */
   default void setDriveBrakeMode(boolean enable) {}