diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 815d30c..c0c4547 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,13 +5,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Competition2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 105; - public static final String GIT_SHA = "14948704bab3b26f113c92007ec19db04dac36d5"; - public static final String GIT_DATE = "2024-02-19 21:35:51 MST"; + public static final int GIT_REVISION = 110; + public static final String GIT_SHA = "ff2bac4bb1164b7901cc679e47c3ddabd00b2d3d"; + public static final String GIT_DATE = "2024-02-19 22:16:55 MST"; public static final String GIT_BRANCH = "workingongettingready"; - public static final String BUILD_DATE = "2024-02-19 21:36:08 MST"; - public static final long BUILD_UNIX_TIME = 1708403768725L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2024-02-20 14:42:29 MST"; + public static final long BUILD_UNIX_TIME = 1708465349930L; + public static final int DIRTY = 1; private BuildConstants() {} } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3677d7c..192f39e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -279,7 +279,7 @@ private MiscConstants() {} public static final int[] USED_CONTROLLER_PORTS = {0, 1}; public static final boolean TUNING_MODE = true; - public static final int CONFIGURATION_ATTEMPTS = 5; + public static final int CONFIGURATION_ATTEMPTS = 10; } public static final double DT = 0.02; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7aa4b9d..46c9684 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,13 +1,11 @@ package frc.robot; -import static frc.robot.Autos.nearestAmpCommand; import static frc.robot.Constants.ShooterConstants.*; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -148,9 +146,9 @@ private void configureDriverBindings() { // .povLeft() // .and(getDistanceToStaging(DriverStation.getAlliance().get(), driveSubsystem)) // .onTrue(nearestClimberCommand(DriverStation.getAlliance().get(), driveSubsystem)); - driverController - .povRight() - .onTrue(nearestAmpCommand(DriverStation.getAlliance().get(), driveSubsystem)); + // driverController + // .povRight() + // .onTrue(nearestAmpCommand(DriverStation.getAlliance().get(), driveSubsystem)); driverController .povDown() .toggleOnTrue(slapdownSubsystem.setRotationGoalCommand(new Rotation2d(0))); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 349102d..c6f1cb6 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -89,12 +89,16 @@ private void configMotors() { MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> elevatorEncoder.setPositionConversionFactor(METERS_PER_REV), - () -> elevatorEncoder.getPositionConversionFactor() == METERS_PER_REV, + () -> + ConfigurationUtils.fpEqual( + elevatorEncoder.getPositionConversionFactor(), METERS_PER_REV), faultRecorder.run("Position conversion factor"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> elevatorEncoder.setVelocityConversionFactor(METERS_PER_REV / 60), - () -> elevatorEncoder.getVelocityConversionFactor() == METERS_PER_REV / 60, + () -> + ConfigurationUtils.fpEqual( + elevatorEncoder.getVelocityConversionFactor(), METERS_PER_REV / 60), faultRecorder.run("Velocity conversion factor"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 81ba695..57e9b40 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -84,12 +84,16 @@ public void configMotor() { Constants.MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> flywheelEncoder.setPositionConversionFactor(conversionFactor), - () -> flywheelEncoder.getPositionConversionFactor() == conversionFactor, + () -> + ConfigurationUtils.fpEqual( + flywheelEncoder.getPositionConversionFactor(), conversionFactor), faultRecorder.run("Position conversion factor"), Constants.MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> flywheelEncoder.setVelocityConversionFactor(conversionFactor / 60), - () -> flywheelEncoder.getVelocityConversionFactor() == conversionFactor / 60, + () -> + ConfigurationUtils.fpEqual( + flywheelEncoder.getVelocityConversionFactor(), conversionFactor / 60), faultRecorder.run("Velocity conversion factor"), Constants.MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( diff --git a/src/main/java/frc/robot/subsystems/slapdown/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/slapdown/SlapdownSubsystem.java index 6fff186..3c75d03 100644 --- a/src/main/java/frc/robot/subsystems/slapdown/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/slapdown/SlapdownSubsystem.java @@ -97,12 +97,16 @@ private void configMotors() { Constants.MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> rotationEncoder.setPositionConversionFactor(rotationConversionFactor), - () -> rotationEncoder.getPositionConversionFactor() == rotationConversionFactor, + () -> + ConfigurationUtils.fpEqual( + rotationEncoder.getPositionConversionFactor(), rotationConversionFactor), rotationFaultRecorder.run("Position conversion factor"), Constants.MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> rotationEncoder.setVelocityConversionFactor(rotationConversionFactor / 60), - () -> rotationEncoder.getVelocityConversionFactor() == rotationConversionFactor / 60, + () -> + ConfigurationUtils.fpEqual( + rotationEncoder.getVelocityConversionFactor(), rotationConversionFactor / 60), rotationFaultRecorder.run("Velocity conversion factor"), Constants.MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( @@ -147,12 +151,17 @@ private void configMotors() { Constants.MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> feederMotor.getEncoder().setPositionConversionFactor(feederConversionFactor), - () -> feederMotor.getEncoder().getPositionConversionFactor() == feederConversionFactor, + () -> + ConfigurationUtils.fpEqual( + feederMotor.getEncoder().getPositionConversionFactor(), feederConversionFactor), feederFaultRecorder.run("Position conversion factor"), Constants.MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> feederMotor.getEncoder().setVelocityConversionFactor(feederConversionFactor / 60), - () -> feederMotor.getEncoder().getVelocityConversionFactor() == feederConversionFactor / 60, + () -> + ConfigurationUtils.fpEqual( + feederMotor.getEncoder().getVelocityConversionFactor(), + feederConversionFactor / 60), feederFaultRecorder.run("Velocity conversion factor"), Constants.MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java index e5715d5..af5b8ac 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDriveSubsystem.java @@ -100,7 +100,7 @@ enum DriveMode { private final TelemetryPigeon2 pigeon = new TelemetryPigeon2(PIGEON_ID, "/drive/gyro", CAN_BUS, MiscConstants.TUNING_MODE); - private final StatusSignal yawSignal = pigeon.getYaw(); + private StatusSignal yawSignal; private final SwerveDrivePoseEstimator poseEstimator; @@ -156,8 +156,8 @@ public SwerveDriveSubsystem(Function> cameraPos modules[1] = new SwerveModule(FRONT_RIGHT_MODULE_CONFIGURATION, MiscConstants.TUNING_MODE); modules[2] = new SwerveModule(BACK_LEFT_MODULE_CONFIGURATION, MiscConstants.TUNING_MODE); modules[3] = new SwerveModule(BACK_RIGHT_MODULE_CONFIGURATION, MiscConstants.TUNING_MODE); - driveEventLogger.append("Swerve modules initialized"); + configurePigeon(); this.cameraPoseDataSupplier = cameraPoseDataSupplier; @@ -165,8 +165,6 @@ public SwerveDriveSubsystem(Function> cameraPos new SwerveDrivePoseEstimator( kinematics, getGyroRotation(), getModulePositions(), new Pose2d()); - configurePigeon(); - // Start odometry thread Robot.getInstance().addPeriodic(this::updateOdometry, 1.0 / ODOMETRY_FREQUENCY); @@ -175,6 +173,7 @@ public SwerveDriveSubsystem(Function> cameraPos private void configurePigeon() { StringFaultRecorder faultRecorder = new StringFaultRecorder(); + yawSignal = pigeon.getYaw(); ConfigurationUtils.applyCheckRecordCTRE( () -> yawSignal.setUpdateFrequency(ODOMETRY_FREQUENCY), () -> yawSignal.getAppliedUpdateFrequency() == ODOMETRY_FREQUENCY, diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index e9b6d1e..7c14db0 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -181,7 +181,7 @@ private void configDriveMotor(SwerveModuleConfiguration config) { () -> { TalonFXConfiguration appliedConfig = new TalonFXConfiguration(); driveMotor.getConfigurator().refresh(appliedConfig); - return appliedConfig.equals(motorConfiguration); + return ConfigEquality.isTalonConfigurationEqual(motorConfiguration, appliedConfig); }, faultRecorder.run("Motor configuration"), MiscConstants.CONFIGURATION_ATTEMPTS); @@ -260,19 +260,19 @@ private void configSteerMotor(SwerveModuleConfiguration config) { ConfigurationUtils.applyCheckRecordRev( () -> steerController.setP(steerPositionPIDGains.p.get()), - () -> steerController.getP() == steerPositionPIDGains.p.get(), + () -> ConfigurationUtils.fpEqual(steerController.getP(), steerPositionPIDGains.p.get()), faultRecorder.run("P gain"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> steerController.setI(steerPositionPIDGains.i.get()), - () -> steerController.getI() == steerPositionPIDGains.i.get(), + () -> ConfigurationUtils.fpEqual(steerController.getI(), steerPositionPIDGains.i.get()), faultRecorder.run("I gain"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> steerController.setD(steerPositionPIDGains.d.get()), - () -> steerController.getD() == steerPositionPIDGains.d.get(), + () -> ConfigurationUtils.fpEqual(steerController.getD(), steerPositionPIDGains.d.get()), faultRecorder.run("D gain"), MiscConstants.CONFIGURATION_ATTEMPTS); @@ -283,24 +283,29 @@ private void configSteerMotor(SwerveModuleConfiguration config) { MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> steerController.setPositionPIDWrappingMinInput(-Math.PI), - () -> steerController.getPositionPIDWrappingMinInput() == -Math.PI, + () -> + ConfigurationUtils.fpEqual(steerController.getPositionPIDWrappingMinInput(), -Math.PI), faultRecorder.run("PID wrapping min input"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> steerController.setPositionPIDWrappingMaxInput(Math.PI), - () -> steerController.getPositionPIDWrappingMaxInput() == Math.PI, + () -> ConfigurationUtils.fpEqual(steerController.getPositionPIDWrappingMaxInput(), Math.PI), faultRecorder.run("PID wrapping max input"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> steerRelativeEncoder.setPositionConversionFactor(steerPositionConversion), - () -> steerRelativeEncoder.getPositionConversionFactor() == steerPositionConversion, + () -> + ConfigurationUtils.fpEqual( + steerRelativeEncoder.getPositionConversionFactor(), steerPositionConversion), faultRecorder.run("Position conversion factor"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> steerRelativeEncoder.setVelocityConversionFactor(steerVelocityConversion), - () -> steerRelativeEncoder.getVelocityConversionFactor() == steerVelocityConversion, + () -> + ConfigurationUtils.fpEqual( + steerRelativeEncoder.getVelocityConversionFactor(), steerVelocityConversion), faultRecorder.run("Velocity conversion factor"), MiscConstants.CONFIGURATION_ATTEMPTS); @@ -348,7 +353,7 @@ private void configSteerEncoder(SwerveModuleConfiguration config) { () -> { CANcoderConfiguration appliedConfig = new CANcoderConfiguration(); absoluteSteerEncoder.getConfigurator().refresh(appliedConfig); - return appliedConfig.equals(encoderConfiguration); + return ConfigEquality.isCANcoderConfigurationEqual(encoderConfiguration, appliedConfig); }, faultRecorder.run("Encoder configuration"), MiscConstants.CONFIGURATION_ATTEMPTS); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 9877811..ec6a73e 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -86,12 +86,15 @@ private void configMotor() { MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( () -> absoluteEncoder.setPositionConversionFactor(Math.PI * 2), - () -> absoluteEncoder.getPositionConversionFactor() == Math.PI * 2, + () -> + ConfigurationUtils.fpEqual(absoluteEncoder.getPositionConversionFactor(), Math.PI * 2), faultRecorder.run("Position conversion factor"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( - () -> absoluteEncoder.setVelocityConversionFactor(Math.PI * 2), - () -> absoluteEncoder.getVelocityConversionFactor() == Math.PI * 2, + () -> absoluteEncoder.setVelocityConversionFactor(Math.PI * 2 / 60.0), + () -> + ConfigurationUtils.fpEqual( + absoluteEncoder.getVelocityConversionFactor(), Math.PI * 2 / 60.0), faultRecorder.run("Velocity conversion factor"), MiscConstants.CONFIGURATION_ATTEMPTS); // Set the relative encoder too for logging @@ -99,8 +102,9 @@ private void configMotor() { ConfigurationUtils.applyCheckRecordRev( () -> wristMotor.getEncoder().setPositionConversionFactor(relativeEncoderConversionFactor), () -> - wristMotor.getEncoder().getPositionConversionFactor() - == relativeEncoderConversionFactor, + ConfigurationUtils.fpEqual( + wristMotor.getEncoder().getPositionConversionFactor(), + relativeEncoderConversionFactor), faultRecorder.run("Position conversion factor"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( @@ -109,8 +113,9 @@ private void configMotor() { .getEncoder() .setVelocityConversionFactor(relativeEncoderConversionFactor / 60.0), () -> - wristMotor.getEncoder().getVelocityConversionFactor() - == relativeEncoderConversionFactor / 60.0, + ConfigurationUtils.fpEqual( + wristMotor.getEncoder().getVelocityConversionFactor(), + relativeEncoderConversionFactor / 60.0), faultRecorder.run("Velocity conversion factor"), MiscConstants.CONFIGURATION_ATTEMPTS); ConfigurationUtils.applyCheckRecordRev( diff --git a/src/main/java/frc/robot/telemetry/MiscRobotTelemetryAndAlerts.java b/src/main/java/frc/robot/telemetry/MiscRobotTelemetryAndAlerts.java index 602b944..b526c94 100644 --- a/src/main/java/frc/robot/telemetry/MiscRobotTelemetryAndAlerts.java +++ b/src/main/java/frc/robot/telemetry/MiscRobotTelemetryAndAlerts.java @@ -1,11 +1,14 @@ package frc.robot.telemetry; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.CANBus.CANBusStatus; import edu.wpi.first.hal.can.CANStatus; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import frc.robot.BuildConstants; import frc.robot.Constants.MiscConstants; +import frc.robot.Constants.SwerveConstants; import frc.robot.telemetry.types.DoubleTelemetryEntry; import frc.robot.telemetry.types.StructTelemetryEntry; import frc.robot.utils.Alert; @@ -16,8 +19,12 @@ public class MiscRobotTelemetryAndAlerts { private static final String tableName = "/robot/"; - private final Alert highCanUsageAlert = new Alert("High CAN Usage", AlertType.WARNING); - private final LinearFilter highCanUsageFilter = LinearFilter.movingAverage(50); + private final Alert highRIOCanUsageAlert = new Alert("High RIO CAN Usage", AlertType.WARNING); + private final LinearFilter highRIOCanUsageFilter = LinearFilter.movingAverage(50); + + private final Alert highCANivoreCanUsageAlert = + new Alert("High CANivore CAN Usage", AlertType.WARNING); + private final LinearFilter highCANivoreCanUsageFilter = LinearFilter.movingAverage(50); private final Alert[] controllerAlerts = new Alert[MiscConstants.USED_CONTROLLER_PORTS.length]; @@ -27,7 +34,12 @@ public class MiscRobotTelemetryAndAlerts { new DoubleTelemetryEntry(tableName + "inputCurrent", MiscConstants.TUNING_MODE); private final StructTelemetryEntry canStatusEntry = new StructTelemetryEntry<>( - tableName + "canStatus", RaiderStructs.CANStatusStruct, MiscConstants.TUNING_MODE); + tableName + "rioCANStatus", RaiderStructs.CANStatusStruct, MiscConstants.TUNING_MODE); + private final StructTelemetryEntry canivoreStatusEntry = + new StructTelemetryEntry<>( + tableName + "canivoreCANStatus", + RaiderStructs.CANBusStatusStruct, + MiscConstants.TUNING_MODE); public MiscRobotTelemetryAndAlerts() { for (int i = 0; i < controllerAlerts.length; i++) { @@ -58,13 +70,19 @@ public void logValues() { inputVoltageEntry.append(RobotController.getInputVoltage()); inputCurrentEntry.append(RobotController.getInputCurrent()); + // RIO CAN Usage CANStatus canStatus = RobotController.getCANStatus(); canStatusEntry.append(canStatus); - - // CAN Usage double percentBusUsage = canStatus.percentBusUtilization; - double filtered = highCanUsageFilter.calculate(percentBusUsage); - highCanUsageAlert.set(filtered >= 0.9); + double filtered = highRIOCanUsageFilter.calculate(percentBusUsage); + highRIOCanUsageAlert.set(filtered >= 0.9); + + // CANivore CAN Usage + CANBusStatus canivoreStatus = CANBus.getStatus(SwerveConstants.CAN_BUS); + canivoreStatusEntry.append(canivoreStatus); + double percentBusUsageCanivore = canivoreStatus.BusUtilization; + double filteredCanivore = highCANivoreCanUsageFilter.calculate(percentBusUsageCanivore); + highCANivoreCanUsageAlert.set(filteredCanivore >= 0.9); // Joysticks for (int i = 0; i < controllerAlerts.length; i++) { diff --git a/src/main/java/frc/robot/telemetry/wrappers/TelemetryPigeon2.java b/src/main/java/frc/robot/telemetry/wrappers/TelemetryPigeon2.java index e277e03..6b89da6 100644 --- a/src/main/java/frc/robot/telemetry/wrappers/TelemetryPigeon2.java +++ b/src/main/java/frc/robot/telemetry/wrappers/TelemetryPigeon2.java @@ -43,6 +43,7 @@ public TelemetryPigeon2(int deviceId, String telemetryPath, boolean tuningMode) public TelemetryPigeon2(int deviceId, String telemetryPath, String canbus, boolean tuningMode) { super(deviceId, canbus); + // TODO: Check if Device or world yawSignal = super.getYaw(); pitchSignal = super.getPitch(); diff --git a/src/main/java/frc/robot/utils/ConfigEquality.java b/src/main/java/frc/robot/utils/ConfigEquality.java new file mode 100644 index 0000000..d47528b --- /dev/null +++ b/src/main/java/frc/robot/utils/ConfigEquality.java @@ -0,0 +1,194 @@ +package frc.robot.utils; + +import static frc.robot.utils.ConfigurationUtils.fpEqual; + +import com.ctre.phoenix6.configs.*; + +public class ConfigEquality { + private ConfigEquality() {} + + public static boolean isCANcoderConfigurationEqual( + CANcoderConfiguration config1, CANcoderConfiguration config2) { + return isMagnetSensorEqual(config1.MagnetSensor, config2.MagnetSensor); + } + + private static boolean isMagnetSensorEqual(MagnetSensorConfigs obj1, MagnetSensorConfigs obj2) { + return obj1.SensorDirection.value == obj2.SensorDirection.value + && fpEqual(obj1.MagnetOffset, obj2.MagnetOffset) + && obj1.AbsoluteSensorRange.value == obj2.AbsoluteSensorRange.value; + } + + // This class is pain, but necessary because of floating point comparison + public static boolean isTalonConfigurationEqual( + TalonFXConfiguration config1, TalonFXConfiguration config2) { + return isMotorOutputEqual(config1.MotorOutput, config2.MotorOutput) + && isCurrentLimitEqual(config1.CurrentLimits, config2.CurrentLimits) + && isVoltageEqual(config1.Voltage, config2.Voltage) + && isTorqueCurrentEqual(config1.TorqueCurrent, config2.TorqueCurrent) + && isFeedbackEqual(config1.Feedback, config2.Feedback) + && isDifferentialSensorsEqual(config1.DifferentialSensors, config2.DifferentialSensors) + && isDifferentialConstantsEqual( + config1.DifferentialConstants, config2.DifferentialConstants) + && isOpenLoopRampsEqual(config1.OpenLoopRamps, config2.OpenLoopRamps) + && isClosedLoopRampsEqual(config1.ClosedLoopRamps, config2.ClosedLoopRamps) + && isHardwareLimitSwitchEqual(config1.HardwareLimitSwitch, config2.HardwareLimitSwitch) + && isAudioEqual(config1.Audio, config2.Audio) + && isSoftwareLimitSwitchEqual(config1.SoftwareLimitSwitch, config2.SoftwareLimitSwitch) + && isMotionMagicEqual(config1.MotionMagic, config2.MotionMagic) + && isCustomParamsEqual(config1.CustomParams, config2.CustomParams) + && isClosedLoopGeneralEqual(config1.ClosedLoopGeneral, config2.ClosedLoopGeneral) + && isSlot0Equal(config1.Slot0, config2.Slot0) + && isSlot1Equal(config1.Slot1, config2.Slot1) + && isSlot2Equal(config1.Slot2, config2.Slot2); + } + + private static boolean isMotorOutputEqual(MotorOutputConfigs obj1, MotorOutputConfigs obj2) { + return obj1.Inverted.value == obj2.Inverted.value + && obj1.NeutralMode.value == obj2.NeutralMode.value + && fpEqual(obj1.DutyCycleNeutralDeadband, obj2.DutyCycleNeutralDeadband) + && fpEqual(obj1.PeakForwardDutyCycle, obj2.PeakForwardDutyCycle) + && fpEqual(obj1.PeakReverseDutyCycle, obj2.PeakReverseDutyCycle); + } + + private static boolean isCurrentLimitEqual(CurrentLimitsConfigs obj1, CurrentLimitsConfigs obj2) { + return fpEqual(obj1.StatorCurrentLimit, obj2.StatorCurrentLimit) + && obj1.StatorCurrentLimitEnable == obj2.StatorCurrentLimitEnable + && fpEqual(obj1.SupplyCurrentLimit, obj2.SupplyCurrentLimit) + && obj1.SupplyCurrentLimitEnable == obj2.SupplyCurrentLimitEnable + && fpEqual(obj1.SupplyCurrentThreshold, obj2.SupplyCurrentThreshold) + && fpEqual(obj1.SupplyTimeThreshold, obj2.SupplyTimeThreshold); + } + + private static boolean isVoltageEqual(VoltageConfigs obj1, VoltageConfigs obj2) { + return fpEqual(obj1.SupplyVoltageTimeConstant, obj2.SupplyVoltageTimeConstant) + && fpEqual(obj1.PeakForwardVoltage, obj2.PeakForwardVoltage) + && fpEqual(obj1.PeakReverseVoltage, obj2.PeakReverseVoltage); + } + + private static boolean isTorqueCurrentEqual( + TorqueCurrentConfigs obj1, TorqueCurrentConfigs obj2) { + return fpEqual(obj1.PeakForwardTorqueCurrent, obj2.PeakForwardTorqueCurrent) + && fpEqual(obj1.PeakReverseTorqueCurrent, obj2.PeakReverseTorqueCurrent) + && fpEqual(obj1.TorqueNeutralDeadband, obj2.TorqueNeutralDeadband); + } + + private static boolean isFeedbackEqual(FeedbackConfigs obj1, FeedbackConfigs obj2) { + return fpEqual(obj1.FeedbackRotorOffset, obj2.FeedbackRotorOffset) + && fpEqual(obj1.SensorToMechanismRatio, obj2.SensorToMechanismRatio) + && fpEqual(obj1.RotorToSensorRatio, obj2.RotorToSensorRatio) + && obj1.FeedbackSensorSource.value == obj2.FeedbackSensorSource.value + && obj1.FeedbackRemoteSensorID == obj2.FeedbackRemoteSensorID; + } + + private static boolean isDifferentialSensorsEqual( + DifferentialSensorsConfigs obj1, DifferentialSensorsConfigs obj2) { + return obj1.DifferentialSensorSource.value == obj2.DifferentialSensorSource.value + && obj1.DifferentialTalonFXSensorID == obj2.DifferentialTalonFXSensorID + && obj1.DifferentialRemoteSensorID == obj2.DifferentialRemoteSensorID; + } + + private static boolean isDifferentialConstantsEqual( + DifferentialConstantsConfigs obj1, DifferentialConstantsConfigs obj2) { + return fpEqual(obj1.PeakDifferentialDutyCycle, obj2.PeakDifferentialDutyCycle) + && fpEqual(obj1.PeakDifferentialVoltage, obj2.PeakDifferentialVoltage) + && fpEqual(obj1.PeakDifferentialTorqueCurrent, obj2.PeakDifferentialTorqueCurrent); + } + + private static boolean isOpenLoopRampsEqual( + OpenLoopRampsConfigs obj1, OpenLoopRampsConfigs obj2) { + return fpEqual(obj1.DutyCycleOpenLoopRampPeriod, obj2.DutyCycleOpenLoopRampPeriod) + && fpEqual(obj1.VoltageOpenLoopRampPeriod, obj2.VoltageOpenLoopRampPeriod) + && fpEqual(obj1.TorqueOpenLoopRampPeriod, obj2.TorqueOpenLoopRampPeriod); + } + + private static boolean isClosedLoopRampsEqual( + ClosedLoopRampsConfigs obj1, ClosedLoopRampsConfigs obj2) { + return fpEqual(obj1.DutyCycleClosedLoopRampPeriod, obj2.DutyCycleClosedLoopRampPeriod) + && fpEqual(obj1.VoltageClosedLoopRampPeriod, obj2.VoltageClosedLoopRampPeriod) + && fpEqual(obj1.TorqueClosedLoopRampPeriod, obj2.TorqueClosedLoopRampPeriod); + } + + private static boolean isHardwareLimitSwitchEqual( + HardwareLimitSwitchConfigs obj1, HardwareLimitSwitchConfigs obj2) { + return obj1.ForwardLimitType.value == obj2.ForwardLimitType.value + && obj1.ForwardLimitAutosetPositionEnable == obj2.ForwardLimitAutosetPositionEnable + && fpEqual(obj1.ForwardLimitAutosetPositionValue, obj2.ForwardLimitAutosetPositionValue) + && obj1.ForwardLimitEnable == obj2.ForwardLimitEnable + && obj1.ForwardLimitSource.value == obj2.ForwardLimitSource.value + && obj1.ForwardLimitRemoteSensorID == obj2.ForwardLimitRemoteSensorID + && obj1.ReverseLimitType.value == obj2.ReverseLimitType.value + && obj1.ReverseLimitAutosetPositionEnable == obj2.ReverseLimitAutosetPositionEnable + && fpEqual(obj1.ReverseLimitAutosetPositionValue, obj2.ReverseLimitAutosetPositionValue) + && obj1.ReverseLimitEnable == obj2.ReverseLimitEnable + && obj1.ReverseLimitSource.value == obj2.ReverseLimitSource.value + && obj1.ReverseLimitRemoteSensorID == obj2.ReverseLimitRemoteSensorID; + } + + private static boolean isAudioEqual(AudioConfigs obj1, AudioConfigs obj2) { + return obj1.BeepOnBoot == obj2.BeepOnBoot + && obj1.BeepOnConfig == obj2.BeepOnConfig + && obj1.AllowMusicDurDisable == obj2.AllowMusicDurDisable; + } + + private static boolean isSoftwareLimitSwitchEqual( + SoftwareLimitSwitchConfigs obj1, SoftwareLimitSwitchConfigs obj2) { + return obj1.ForwardSoftLimitEnable == obj2.ForwardSoftLimitEnable + && obj1.ReverseSoftLimitEnable == obj2.ReverseSoftLimitEnable + && fpEqual(obj1.ForwardSoftLimitThreshold, obj2.ForwardSoftLimitThreshold) + && fpEqual(obj1.ReverseSoftLimitThreshold, obj2.ReverseSoftLimitThreshold); + } + + private static boolean isMotionMagicEqual(MotionMagicConfigs obj1, MotionMagicConfigs obj2) { + return fpEqual(obj1.MotionMagicCruiseVelocity, obj2.MotionMagicCruiseVelocity) + && fpEqual(obj1.MotionMagicAcceleration, obj2.MotionMagicAcceleration) + && fpEqual(obj1.MotionMagicJerk, obj2.MotionMagicJerk) + && fpEqual(obj1.MotionMagicExpo_kV, obj2.MotionMagicExpo_kV) + && fpEqual(obj1.MotionMagicExpo_kA, obj2.MotionMagicExpo_kA); + } + + private static boolean isCustomParamsEqual(CustomParamsConfigs obj1, CustomParamsConfigs obj2) { + return fpEqual(obj1.CustomParam0, obj2.CustomParam0) + && fpEqual(obj1.CustomParam1, obj2.CustomParam1); + } + + private static boolean isClosedLoopGeneralEqual( + ClosedLoopGeneralConfigs obj1, ClosedLoopGeneralConfigs obj2) { + return obj1.ContinuousWrap == obj2.ContinuousWrap; + } + + private static boolean isSlot0Equal(Slot0Configs obj1, Slot0Configs obj2) { + return fpEqual(obj1.kP, obj2.kP) + && fpEqual(obj1.kI, obj2.kI) + && fpEqual(obj1.kD, obj2.kD) + && fpEqual(obj1.kS, obj2.kS) + && fpEqual(obj1.kV, obj2.kV) + && fpEqual(obj1.kA, obj2.kA) + && fpEqual(obj1.kG, obj2.kG) + && obj1.GravityType.value == obj2.GravityType.value + && obj1.StaticFeedforwardSign.value == obj2.StaticFeedforwardSign.value; + } + + private static boolean isSlot1Equal(Slot1Configs obj1, Slot1Configs obj2) { + return fpEqual(obj1.kP, obj2.kP) + && fpEqual(obj1.kI, obj2.kI) + && fpEqual(obj1.kD, obj2.kD) + && fpEqual(obj1.kS, obj2.kS) + && fpEqual(obj1.kV, obj2.kV) + && fpEqual(obj1.kA, obj2.kA) + && fpEqual(obj1.kG, obj2.kG) + && obj1.GravityType.value == obj2.GravityType.value + && obj1.StaticFeedforwardSign.value == obj2.StaticFeedforwardSign.value; + } + + private static boolean isSlot2Equal(Slot2Configs obj1, Slot2Configs obj2) { + return fpEqual(obj1.kP, obj2.kP) + && fpEqual(obj1.kI, obj2.kI) + && fpEqual(obj1.kD, obj2.kD) + && fpEqual(obj1.kS, obj2.kS) + && fpEqual(obj1.kV, obj2.kV) + && fpEqual(obj1.kA, obj2.kA) + && fpEqual(obj1.kG, obj2.kG) + && obj1.GravityType.value == obj2.GravityType.value + && obj1.StaticFeedforwardSign.value == obj2.StaticFeedforwardSign.value; + } +} diff --git a/src/main/java/frc/robot/utils/ConfigurationUtils.java b/src/main/java/frc/robot/utils/ConfigurationUtils.java index e762ad8..5915ec4 100644 --- a/src/main/java/frc/robot/utils/ConfigurationUtils.java +++ b/src/main/java/frc/robot/utils/ConfigurationUtils.java @@ -52,50 +52,69 @@ public static boolean applyCheckCTRE( public static boolean applyCheckRecord( Runnable apply, BooleanSupplier check, Runnable record, int attempts) { if (applyCheck(apply, check, attempts)) { - record.run(); return true; } + record.run(); return false; } public static boolean applyCheckRecord( BooleanSupplier apply, BooleanSupplier check, Runnable record, int attempts) { if (applyCheck(apply, check, attempts)) { - record.run(); return true; } + record.run(); return false; } public static boolean applyCheckRecordRev( Supplier apply, BooleanSupplier check, Runnable record, int attempts) { if (applyCheckRev(apply, check, attempts)) { - record.run(); return true; } + record.run(); return false; } public static boolean applyCheckRecordCTRE( Supplier apply, BooleanSupplier check, Runnable record, int attempts) { if (applyCheckCTRE(apply, check, attempts)) { - record.run(); return true; } + record.run(); return false; } + /** + * Post a device configuration message to the subsystem event entry + * + * @param fault true if there was a fault + * @param subsystemEventEntry a logger for the subsystem + * @param deviceName the name of the motor + * @param faultString the fault string + */ public static void postDeviceConfig( - boolean fault, Consumer subsystemEventEntry, String motorName, String faultString) { + boolean fault, Consumer subsystemEventEntry, String deviceName, String faultString) { if (fault) { - String message = motorName + " failed to initialize: " + faultString; + String message = deviceName + " failed to initialize: " + faultString; subsystemEventEntry.accept(message); - DriverStation.reportWarning(message, false); + DriverStation.reportWarning(message, true); } else { - subsystemEventEntry.accept(motorName + " initialized"); + subsystemEventEntry.accept(deviceName + " initialized"); } } + /** + * Compare two doubles for equality within a small epsilon + * + * @param d1 the first double + * @param d2 the second double + * @return true if the doubles are equal within a small epsilon + */ + public static boolean fpEqual(double d1, double d2) { + return Math.abs(d1 - d2) < 0.001; + } + public static class StringFaultRecorder { private String faultString = ""; private boolean hasFault = false; diff --git a/src/main/java/frc/robot/utils/RaiderStructs.java b/src/main/java/frc/robot/utils/RaiderStructs.java index c9e1579..2aeb38a 100644 --- a/src/main/java/frc/robot/utils/RaiderStructs.java +++ b/src/main/java/frc/robot/utils/RaiderStructs.java @@ -1,5 +1,7 @@ package frc.robot.utils; +import com.ctre.phoenix6.CANBus.CANBusStatus; +import com.ctre.phoenix6.StatusCode; import edu.wpi.first.hal.can.CANStatus; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.util.struct.Struct; @@ -9,6 +11,53 @@ public class RaiderStructs { private RaiderStructs() {} + public static final CANBusStatusStruct CANBusStatusStruct = new CANBusStatusStruct(); + + private static final class CANBusStatusStruct implements Struct { + + @Override + public Class getTypeClass() { + return CANBusStatus.class; + } + + @Override + public String getTypeString() { + return "struct:CANBusStatus"; + } + + @Override + public int getSize() { + return kSizeInt32 + kSizeFloat + kSizeInt32 + kSizeInt32 + kSizeInt32 + kSizeInt32; + } + + @Override + public String getSchema() { + return "int32 Status;float BusUtilization;int32 BusOffCount;int32 TxFullCount;int32 REC;int32 TEC"; + } + + @Override + public CANBusStatus unpack(ByteBuffer byteBuffer) { + CANBusStatus canBusStatus = new CANBusStatus(); + canBusStatus.Status = StatusCode.valueOf(byteBuffer.getInt()); + canBusStatus.BusUtilization = byteBuffer.getFloat(); + canBusStatus.BusOffCount = byteBuffer.getInt(); + canBusStatus.TxFullCount = byteBuffer.getInt(); + canBusStatus.REC = byteBuffer.getInt(); + canBusStatus.TEC = byteBuffer.getInt(); + return canBusStatus; + } + + @Override + public void pack(ByteBuffer byteBuffer, CANBusStatus canBusStatus) { + byteBuffer.putInt(canBusStatus.Status.value); + byteBuffer.putFloat(canBusStatus.BusUtilization); + byteBuffer.putInt(canBusStatus.BusOffCount); + byteBuffer.putInt(canBusStatus.TxFullCount); + byteBuffer.putInt(canBusStatus.REC); + byteBuffer.putInt(canBusStatus.TEC); + } + } + public static final CANStatusStruct CANStatusStruct = new CANStatusStruct(); private static final class CANStatusStruct implements Struct {