diff --git a/src/main/java/com/team1701/robot/Configuration.java b/src/main/java/com/team1701/robot/Configuration.java index 9e813e96..2eea6f35 100644 --- a/src/main/java/com/team1701/robot/Configuration.java +++ b/src/main/java/com/team1701/robot/Configuration.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; public final class Configuration { - private static final RobotType kRobot = RobotType.SIMULATION_BOT; + private static final RobotType kRobot = RobotType.COMPETITION_BOT; private static boolean mInvalidRobotAlerted = false; diff --git a/src/main/java/com/team1701/robot/util/TalonFxMotorFactory.java b/src/main/java/com/team1701/robot/util/TalonFxMotorFactory.java index a99baaf4..73b40832 100644 --- a/src/main/java/com/team1701/robot/util/TalonFxMotorFactory.java +++ b/src/main/java/com/team1701/robot/util/TalonFxMotorFactory.java @@ -5,9 +5,11 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.team1701.lib.alerts.Alert; import com.team1701.lib.drivers.motors.MotorIOTalonFX; import com.team1701.lib.drivers.motors.MotorIOTalonFXFOC; import com.team1701.robot.Constants; +import org.littletonrobotics.junction.Logger; public class TalonFxMotorFactory { private static final InvertedValue kInverted = InvertedValue.Clockwise_Positive; @@ -90,10 +92,22 @@ public static MotorIOTalonFXFOC createSteerMotorIOTalonFxFoc(int deviceId) { } private static void configureWithRetry(TalonFX motor, TalonFXConfiguration config) { + StatusCode statusCode = null; for (int i = 0; i < 4; i++) { - if (motor.getConfigurator().apply(config, 0.1) == StatusCode.OK) { + statusCode = motor.getConfigurator().apply(config, 0.1); + if (statusCode == StatusCode.OK) { break; } } + + if (statusCode != StatusCode.OK) { + Alert.error("TalonFX configuration failed: id=" + motor.getDeviceID() + ",status=" + statusCode.toString()) + .enable(); + + var actualConfig = new TalonFXConfiguration(); + motor.getConfigurator().refresh(actualConfig, 0.1); + Logger.recordOutput("Motor/" + motor.getDeviceID() + "/Config", config.toString()); + Logger.recordOutput("Motor/" + motor.getDeviceID() + "/ActualConfig", actualConfig.toString()); + } } }