Merge branch 'main' of https://github.com/Team3256/Offseason_Bot_2024

Author: realjoshuau

src/main/java/frc/robot/subsystems/climb/ClimbConstants.java

@@ -14,6 +14,7 @@
 public final class ClimbConstants {
 
   public static final int kLeftClimbMotorID = 18;
+  public static final int kRightClimbMotorID = 17;
   public static final double gearRatio = 20; // needs to be tuned
 
   public static final double kClimbUpPosition = 150 / 20;
@@ -29,7 +30,22 @@ public final class ClimbConstants {
   public static double updateFrequency;
   public static boolean kUseMotionMagic = false;
 
-  public static final TalonFXConfiguration motorConfig =
+  public static final TalonFXConfiguration leftClimbConfig =
+      new TalonFXConfiguration()
+          .withSlot0(new Slot0Configs().withKS(0).withKV(0).withKP(1).withKI(0).withKD(0))
+          .withMotorOutput(
+              new MotorOutputConfigs()
+                  .withNeutralMode(NeutralModeValue.Brake)
+                  .withInverted(InvertedValue.Clockwise_Positive))
+          .withMotionMagic(
+              new MotionMagicConfigs()
+                  .withMotionMagicAcceleration(400)
+                  .withMotionMagicCruiseVelocity(100))
+          .withCurrentLimits(
+              new CurrentLimitsConfigs()
+                  .withStatorCurrentLimitEnable(true)
+                  .withStatorCurrentLimit(60));
+  public static final TalonFXConfiguration righClimbConfig =
       new TalonFXConfiguration()
           .withSlot0(new Slot0Configs().withKS(0).withKV(0).withKP(1).withKI(0).withKD(0))
           .withMotorOutput(

src/main/java/frc/robot/subsystems/climb/ClimbIOTalonFX.java

@@ -9,34 +9,40 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
-import com.ctre.phoenix6.controls.NeutralOut;
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.controls.*;
 import com.ctre.phoenix6.hardware.TalonFX;
 import frc.robot.utils.PhoenixUtil;
 import frc.robot.utils.TalonUtil;
 
 public class ClimbIOTalonFX implements ClimbIO {
 
-  private final TalonFX climbMotor = new TalonFX(ClimbConstants.kLeftClimbMotorID);
+  private final TalonFX leftClimbMotor = new TalonFX(ClimbConstants.kLeftClimbMotorID);
   private final PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0);
   private final MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0).withSlot(0);
   private final VoltageOut voltageReq = new VoltageOut(0);
 
-  private final StatusSignal<Double> climbMotorVoltage = climbMotor.getMotorVoltage();
-  private final StatusSignal<Double> climbMotorVelocity = climbMotor.getVelocity();
-  private final StatusSignal<Double> climbMotorPosition = climbMotor.getPosition();
-  private final StatusSignal<Double> climbMotorStatorCurrent = climbMotor.getStatorCurrent();
-  private final StatusSignal<Double> climbMotorSupplyCurrent = climbMotor.getSupplyCurrent();
-  private final StatusSignal<Double> climbMotorTemperature = climbMotor.getDeviceTemp();
+  private final StatusSignal<Double> climbMotorVoltage = leftClimbMotor.getMotorVoltage();
+  private final StatusSignal<Double> climbMotorVelocity = leftClimbMotor.getVelocity();
+  private final StatusSignal<Double> climbMotorPosition = leftClimbMotor.getPosition();
+  private final StatusSignal<Double> climbMotorStatorCurrent = leftClimbMotor.getStatorCurrent();
+  private final StatusSignal<Double> climbMotorSupplyCurrent = leftClimbMotor.getSupplyCurrent();
+  private final StatusSignal<Double> climbMotorTemperature = leftClimbMotor.getDeviceTemp();
   private final StatusSignal<Double> climbMotorReferenceSlope =
-      climbMotor.getClosedLoopReferenceSlope();
+      leftClimbMotor.getClosedLoopReferenceSlope();
+
+  private final TalonFX rightClimbMotor = new TalonFX(ClimbConstants.kRightClimbMotorID);
+
+  private final Follower rightClimbFollowReq = new Follower(leftClimbMotor.getDeviceID(), false);
 
   public ClimbIOTalonFX() {
-    var motorConfig = ClimbConstants.motorConfig;
-    PhoenixUtil.checkErrorAndRetry(() -> climbMotor.getConfigurator().refresh(motorConfig));
-    TalonUtil.applyAndCheckConfiguration(climbMotor, motorConfig);
+    var leftClimbConfig = ClimbConstants.leftClimbConfig;
+    PhoenixUtil.checkErrorAndRetry(() -> leftClimbMotor.getConfigurator().refresh(leftClimbConfig));
+    TalonUtil.applyAndCheckConfiguration(leftClimbMotor, leftClimbConfig);
+
+    var rightClimbConfig = ClimbConstants.righClimbConfig;
+    PhoenixUtil.checkErrorAndRetry(
+        () -> rightClimbMotor.getConfigurator().refresh(rightClimbConfig));
+    TalonUtil.applyAndCheckConfiguration(rightClimbMotor, rightClimbConfig);
 
     BaseStatusSignal.setUpdateFrequencyForAll(
         ClimbConstants.updateFrequency,
@@ -47,7 +53,8 @@ public ClimbIOTalonFX() {
         climbMotorSupplyCurrent,
         climbMotorTemperature,
         climbMotorReferenceSlope);
-    climbMotor.optimizeBusUtilization();
+    leftClimbMotor.optimizeBusUtilization();
+    rightClimbMotor.setControl(rightClimbFollowReq);
   }
 
   @Override
@@ -72,30 +79,30 @@ public void updateInputs(ClimbIOInputs inputs) {
   @Override
   public void setPosition(double position) {
     if (ClimbConstants.kUseMotionMagic) {
-      climbMotor.setControl(motionMagicRequest.withPosition(position));
+      leftClimbMotor.setControl(motionMagicRequest.withPosition(position));
     } else {
-      climbMotor.setControl(positionRequest.withPosition(position));
+      leftClimbMotor.setControl(positionRequest.withPosition(position));
     }
   }
 
   @Override
   public void setVoltage(double voltage) {
-    climbMotor.setVoltage(voltage);
+    leftClimbMotor.setVoltage(voltage);
   }
 
   @Override
   public void off() {
-    climbMotor.setControl(new NeutralOut());
+    leftClimbMotor.setControl(new NeutralOut());
   }
 
   @Override
   public void zero() {
-    climbMotor.setPosition(0);
+    leftClimbMotor.setPosition(0);
   }
 
   @Override
   public TalonFX getMotor() {
-    return climbMotor;
+    return leftClimbMotor;
   }
 
   @Override

src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

@@ -14,7 +14,7 @@
 public final class IntakeConstants {
   /* CAN */
   public static final int kIntakeMotorID = 33;
-  public static final int kSecondaryIntakeMotorID = 999;
+  public static final int kSecondaryIntakeMotorID = 37;
 
   public static final double kPassthroughIntakeVoltage = -8;
   public static final double kIntakeIntakeVoltage = 12;