Configuration is so gas

RegisJesuitRobotics · Feb 20, 2024 · cd60a9e · cd60a9e
1 parent ff2bac4
commit cd60a9e
Show file tree

Hide file tree

Showing 14 changed files with 360 additions and 55 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -100,7 +100,7 @@ enum DriveMode {
   private final TelemetryPigeon2 pigeon =
       new TelemetryPigeon2(PIGEON_ID, "/drive/gyro", CAN_BUS, MiscConstants.TUNING_MODE);
 
-  private final StatusSignal<Double> yawSignal = pigeon.getYaw();
+  private StatusSignal<Double> yawSignal;
 
   private final SwerveDrivePoseEstimator poseEstimator;
 
@@ -156,17 +156,15 @@ public SwerveDriveSubsystem(Function<Pose2d, List<EstimatedRobotPose>> 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;
 
     poseEstimator =
         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<Pose2d, List<EstimatedRobotPose>> 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
@@ -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
@@ -86,21 +86,25 @@ 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
     double relativeEncoderConversionFactor = (2 * Math.PI) / WRIST_GEAR_RATIO;
     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
@@ -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<CANStatus> canStatusEntry =
       new StructTelemetryEntry<>(
-          tableName + "canStatus", RaiderStructs.CANStatusStruct, MiscConstants.TUNING_MODE);
+          tableName + "rioCANStatus", RaiderStructs.CANStatusStruct, MiscConstants.TUNING_MODE);
+  private final StructTelemetryEntry<CANBusStatus> 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
@@ -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();