diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java index a3493411..05c22d00 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java @@ -13,6 +13,7 @@ import com.gos.lib.properties.GosDoubleProperty; import com.gos.lib.properties.HeavyDoubleProperty; import edu.wpi.first.math.util.Units; +import lib.factories.TalonFXFactory; import lib.properties.phoenix6.Phoenix6PidPropertyBuilder; import lib.properties.phoenix6.PidPropertyPublic; import frc.robot.Constants.ArmConstants; @@ -63,12 +64,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 @@ -80,8 +76,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(); @@ -91,8 +87,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(); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java index 45fbca9d..3384ea91 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java +++ b/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 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) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java index 1ebf0d5e..2f5ac8c8 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOPrototype.java +++ b/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", diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index 82aa4b77..ecb97ff5 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/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; @@ -75,8 +77,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 @@ -104,7 +104,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 @@ -126,7 +127,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 diff --git a/src/main/java/lib/factories/TalonFXFactory.java b/src/main/java/lib/factories/TalonFXFactory.java index 86c6b079..af027c15 100644 --- a/src/main/java/lib/factories/TalonFXFactory.java +++ b/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 pair) -> {