Merge branch 'main' into GearFox-AddAdkitAgain

src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java

@@ -12,10 +12,14 @@
 import com.ctre.phoenix6.signals.*;
 import com.gos.lib.properties.GosDoubleProperty;
 import com.gos.lib.properties.HeavyDoubleProperty;
+
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import lib.factories.TalonFXFactory;
 import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
 import lib.properties.phoenix6.PidPropertyPublic;
+
 import frc.robot.Constants.ArmConstants;
 
 public class ArmIOKraken implements ArmIO {
@@ -67,12 +71,7 @@ public class ArmIOKraken implements ArmIO {
 
   public ArmIOKraken() {
     final String CANBUS = "canivore";
-    m_armMaster = new TalonFX(ArmConstants.ARM_MASTER_ID, CANBUS);
-    m_armFollower = new TalonFX(ArmConstants.ARM_FOLLOWER_ID, CANBUS);
     m_armEncoder = new CANcoder(ArmConstants.ARM_ENCODER_ID, CANBUS);
-
-    m_wristMaster = new TalonFX(ArmConstants.WRIST_MASTER_ID, CANBUS);
-    m_wristFollower = new TalonFX(ArmConstants.WRIST_FOLLOWER_ID, CANBUS);
     m_wristEncoder = new CANcoder(ArmConstants.WRIST_ENCODER_ID, CANBUS);
 
     // Arm Configuration
@@ -84,8 +83,8 @@ public ArmIOKraken() {
     armConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
     armConfig.Feedback.SensorToMechanismRatio = ArmConstants.ARM_SENSOR_MECHANISM_RATIO;
 
-    m_armMaster.getConfigurator().apply(armConfig);
-    m_armFollower.getConfigurator().apply(armConfig);
+    m_armMaster = TalonFXFactory.createTalon(ArmConstants.ARM_MASTER_ID, armConfig);
+    m_armFollower = TalonFXFactory.createTalon(ArmConstants.ARM_MASTER_ID, armConfig);
 
     // Wrist Configuration
     TalonFXConfiguration wristConfig = new TalonFXConfiguration();
@@ -95,8 +94,8 @@ public ArmIOKraken() {
     wristConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     wristConfig.Feedback.SensorToMechanismRatio = ArmConstants.WRIST_SENSOR_MECHANISM_RATIO;
 
-    m_wristMaster.getConfigurator().apply(wristConfig);
-    m_wristFollower.getConfigurator().apply(wristConfig);
+    m_wristMaster = TalonFXFactory.createTalon(ArmConstants.WRIST_MASTER_ID, wristConfig);
+    m_wristFollower = TalonFXFactory.createTalon(ArmConstants.WRIST_FOLLOWER_ID, wristConfig);
 
     // Encoder Configuration
     CANcoderConfiguration armEncoderConfig = new CANcoderConfiguration();

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

@@ -10,6 +10,7 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import frc.robot.Constants.ClimberConstants;
+import lib.factories.TalonFXFactory;
 import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
 import lib.properties.phoenix6.PidPropertyPublic;
 
@@ -34,16 +35,15 @@ public class ClimberIOKraken implements ClimberIO {
   private final StatusSignal<Double> m_rightAppliedOutputSignal;
 
   public ClimberIOKraken() {
-    m_leftClimber = new TalonFX(ClimberConstants.LEFT_CLIMBER_ID, "canivore");
-    m_rightClimber = new TalonFX(ClimberConstants.RIGHT_CLIMBER_ID, "canivore");
-
     TalonFXConfiguration climberConfig = new TalonFXConfiguration();
     climberConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
     climberConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-    m_leftClimber.getConfigurator().apply(climberConfig);
+
+    m_leftClimber = TalonFXFactory.createTalon(ClimberConstants.LEFT_CLIMBER_ID, climberConfig);
 
     climberConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    m_rightClimber.getConfigurator().apply(climberConfig);
+
+    m_rightClimber = TalonFXFactory.createTalon(ClimberConstants.RIGHT_CLIMBER_ID, climberConfig);
 
     m_leftProperty = new Phoenix6PidPropertyBuilder("Climber/Left Climber", false, m_leftClimber, 0)
         .addP(ClimberConstants.CLIMBER_KP)

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

@@ -7,6 +7,7 @@
 import frc.robot.Constants.ClimberConstants;
 import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
 import lib.properties.phoenix6.PidPropertyPublic;
+import lib.factories.TalonFXFactory;
 
 public class ClimberIOPrototype implements ClimberIO {
   private final TalonFX m_leftTalon;
@@ -15,15 +16,16 @@ public class ClimberIOPrototype implements ClimberIO {
   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);
+
+    m_rightTalon = TalonFXFactory.createTalon(ClimberConstants.RIGHT_CLIMBER_ID, "canivore", rightTalonConfig);
 
     var leftTalonConfig = new TalonFXConfiguration();
-    m_leftTalon.getConfigurator().apply(leftTalonConfig);
+
+    m_leftTalon = TalonFXFactory.createTalon(ClimberConstants.LEFT_CLIMBER_ID, "canivore", leftTalonConfig);
 
     m_leftClimberPid = new Phoenix6PidPropertyBuilder(
         "Climber/Left PID",

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

@@ -14,6 +14,7 @@
 package frc.robot.subsystems.drive.module;
 
 import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
@@ -28,6 +29,7 @@
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import lib.factories.TalonFXFactory;
 import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
 
 import java.util.Queue;
@@ -76,8 +78,6 @@ public class ModuleIOTalonFX implements ModuleIO {
 
   public ModuleIOTalonFX(ModuleConstants moduleConstants) {
     String canbus = "canivore";
-    m_driveTalon = new TalonFX(moduleConstants.DRIVE_MOTOR_ID(), canbus);
-    m_turnTalon = new TalonFX(moduleConstants.TURN_MOTOR_ID(), canbus);
 
     /*
     * this is technically the proper way of using any class that
@@ -105,7 +105,8 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
             moduleConstants.DRIVE_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive
                     : InvertedValue.CounterClockwise_Positive;
     driveConfig.Feedback.SensorToMechanismRatio = m_moduleConstants.DRIVE_GEAR_RATIO();
-    m_driveTalon.getConfigurator().apply(driveConfig);
+
+    m_driveTalon = TalonFXFactory.createTalon(moduleConstants.DRIVE_MOTOR_ID(), canbus, driveConfig);
     setDriveBrakeMode(true);
 
     // setup pid gains for drive motor
@@ -127,7 +128,8 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
                     : InvertedValue.CounterClockwise_Positive;
     turnConfig.ClosedLoopGeneral.ContinuousWrap = true;
     turnConfig.Feedback.SensorToMechanismRatio = m_moduleConstants.TURNING_GEAR_RATIO();
-    m_turnTalon.getConfigurator().apply(turnConfig);
+
+    m_turnTalon = TalonFXFactory.createTalon(moduleConstants.TURN_MOTOR_ID(), turnConfig);
     setTurnBrakeMode(true);
 
     // setup pid gains for turn motor

src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java

@@ -10,6 +10,7 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.playingwithfusion.TimeOfFlight;
+import lib.factories.TalonFXFactory;
 import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
 import lib.properties.phoenix6.PidPropertyPublic;
 import frc.robot.Constants.ShooterConstants;
@@ -58,10 +59,10 @@ public ShooterIOKraken() {
     m_kicker = new TalonFX(ShooterConstants.KICKER_ID, canbus);
     m_intake = new TalonFX(ShooterConstants.INTAKE_ID, canbus);
     m_indexer = new TalonFX(ShooterConstants.INDEXER_ID, canbus);
-
-//    m_tof = new TimeOfFlight(28);
-//    m_tof.setRangingMode(TimeOfFlight.RangingMode.Short, 25);
-
+    
+    m_tof = new TimeOfFlight(28);
+    m_tof.setRangingMode(TimeOfFlight.RangingMode.Short, 25);
+   
     // general motor configs
     TalonFXConfiguration shooterConfig = new TalonFXConfiguration();
     shooterConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
@@ -72,16 +73,19 @@ public ShooterIOKraken() {
 
     // right shooter isn't inverted
     shooterConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-    m_leftTalon.getConfigurator().apply(shooterConfig);
+
+    m_leftTalon = TalonFXFactory.createTalon(ShooterConstants.TOP_LEFT_ID, shooterConfig);
 
     // everything else is
     shooterConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
-    m_rightTalon.getConfigurator().apply(shooterConfig);
+
+    m_rightTalon = TalonFXFactory.createTalon(ShooterConstants.TOP_RIGHT_ID, shooterConfig);
 
     shooterConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
-    m_indexer.getConfigurator().apply(shooterConfig);
-    m_kicker.getConfigurator().apply(shooterConfig);
-    m_intake.getConfigurator().apply(shooterConfig);
+
+    m_indexer = TalonFXFactory.createTalon(ShooterConstants.INDEXER_ID, shooterConfig);
+    m_intake = TalonFXFactory.createTalon(ShooterConstants.INTAKE_ID, shooterConfig);
+    m_kicker = TalonFXFactory.createTalon(ShooterConstants.KICKER_ID, shooterConfig);
 
     m_leftProperty = new Phoenix6PidPropertyBuilder("Shooter/Left PID", false, m_leftTalon, 0)
         .addP(ShooterConstants.SHOOTER_KP)
@@ -197,21 +201,15 @@ public void updateInputs(ShooterIOInputsAutoLogged inputs) {
     inputs.intakeTemperature = m_intakeTemperatureSignal.getValueAsDouble();
     inputs.indexerTemperature = m_indexerTemperatureSignal.getValueAsDouble();
 
-//    inputs.tofDistanceIn = m_tof.getRange();
+   inputs.tofDistanceIn = m_tof.getRange();
 
     m_leftProperty.updateIfChanged();
     m_rightProperty.updateIfChanged();
   }
 
   @Override
   public boolean hasPiece() {
-    double avgCurrentChange = (
-        m_prevKickerCurrent[0] +
-        m_prevKickerCurrent[1] +
-        m_prevKickerCurrent[2] +
-        m_prevKickerCurrent[3]
-        ) / 4.0;
-    return avgCurrentChange > 20;
+    return m_tof.getRange() < 60.0;
   }
 
   @Override

src/main/java/lib/factories/TalonFXFactory.java

@@ -47,6 +47,16 @@ public static TalonFX createTalon(int id, String canbusName, TalonFXConfiguratio
     return talonFX;
   }
 
+  //METHOD: This allows for the creation of a new Talon Brushless Motor with a preset Config
+  public static TalonFX createTalon(int id, TalonFXConfiguration config) {
+    TalonFX talonFX = new TalonFX(id, "canivore");
+
+    talonFX.getConfigurator().apply(config);
+
+    TalonFxMotors.add(new Pair<>(talonFX, config));
+    return talonFX;
+  }
+
   // METHOD: Checks each motor and handles sticky faults
   public static void handleFaults() {
     TalonFxMotors.forEach((Pair<TalonFX, TalonFXConfiguration> pair) -> {