refresh inputs logging

simgui-ds.json

@@ -102,8 +102,9 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0",
-      "name": "Keyboard"
+      "guid": "78696e70757401000000000000000000",
+      "name": "Keyboard",
+      "useGamepad": true
     },
     {
       "useGamepad": true

simgui.json

@@ -12,20 +12,41 @@
       "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d",
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Auto Chooser": "String Chooser",
+      "/SmartDashboard/Blacklist All": "Command",
+      "/SmartDashboard/Blacklist Note": "Command",
+      "/SmartDashboard/Dynamic Auto": "Command",
+      "/SmartDashboard/Dynamic Demo": "Command",
+      "/SmartDashboard/Note Picker": "String Chooser",
+      "/SmartDashboard/Note To Note": "Command",
+      "/SmartDashboard/Note To Shooting": "Command",
       "/SmartDashboard/Run Elevator Sysid": "Command",
       "/SmartDashboard/Run Flywheel Sysid": "Command",
       "/SmartDashboard/Run Pivot Sysid": "Command",
       "/SmartDashboard/Run Swerve Azimuth Sysid": "Command",
       "/SmartDashboard/Run Swerve Drive Sysid": "Command",
+      "/SmartDashboard/SendableChooser[0]": "String Chooser",
+      "/SmartDashboard/Set robot pose center": "Command",
+      "/SmartDashboard/Shoot To Note": "Command",
       "/SmartDashboard/Shooter shoot": "Command",
+      "/SmartDashboard/Start To Note": "Command",
+      "/SmartDashboard/Update Note": "Command",
       "/SmartDashboard/VisionSystemSim-Left Camera Sim System/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Left Camera/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Left_Camera/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Right Camera Sim System/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Right Camera/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Right_Camera/Sim Field": "Field2d",
-      "/SmartDashboard/Zero shooter": "Command",
-      "/SmartDashboard/manual zero shooter": "Command"
+      "/SmartDashboard/Whitelist All": "Command",
+      "/SmartDashboard/Whitelist Note": "Command",
+      "/SmartDashboard/gleeb": "Command",
+      "/SmartDashboard/gloob": "Command"
+    },
+    "windows": {
+      "/SmartDashboard/Auto Chooser": {
+        "window": {
+          "visible": true
+        }
+      }
     }
   },
   "NetworkTables": {

src/main/java/frc/robot/Robot.java

@@ -50,6 +50,7 @@
 import frc.robot.subsystems.swerve.SwerveSubsystem.AutoAimStates;
 import frc.robot.utils.CommandXboxControllerSubsystem;
 import frc.robot.utils.autoaim.AutoAim;
+import frc.robot.utils.logging.TalonFXFaultManager;
 import java.util.List;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.LogFileUtil;
@@ -213,7 +214,9 @@ public void robotInit() {
     shooter.setDefaultCommand(shooter.runFlywheelsCmd(() -> 0.0, () -> 0.0));
     leds.setDefaultCommand(
         leds.defaultStateDisplayCmd(
-            () -> DriverStation.isEnabled(), () -> currentTarget == Target.SPEAKER));
+            () -> DriverStation.isEnabled(),
+            () -> currentTarget == Target.SPEAKER,
+            () -> TalonFXFaultManager.getInstance().isOk()));
 
     controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0));
     operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0));
@@ -401,8 +404,6 @@ public void robotInit() {
     SmartDashboard.putData("Shooter shoot", shootWithDashboard());
     SmartDashboard.putData("Run Swerve Azimuth Sysid", swerve.runModuleSteerCharacterizationCmd());
     SmartDashboard.putData("Run Swerve Drive Sysid", swerve.runDriveCharacterizationCmd());
-    SmartDashboard.putData("Run Elevator Sysid", elevator.runSysidCmd());
-    SmartDashboard.putData("Run Pivot Sysid", shooter.runPivotSysidCmd());
     SmartDashboard.putData("Run Flywheel Sysid", shooter.runFlywheelSysidCmd());
     SmartDashboard.putData(
         "manual zero shooter",
@@ -412,6 +413,8 @@ public void robotInit() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
+    // Update fault checker
+    TalonFXFaultManager.getInstance().periodic();
     // Update ascope mechanism visualization
     Logger.recordOutput(
         "Mechanism Poses",

src/main/java/frc/robot/subsystems/carriage/CarriageIO.java

@@ -4,16 +4,13 @@
 
 package frc.robot.subsystems.carriage;
 
+import frc.robot.utils.logging.TalonFXLogger.TalonFXLog;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface CarriageIO {
   @AutoLog
   public static class CarriageIOInputs {
-    public double velocityRotationsPerSecond = 0.0;
-    public double appliedVolts = 0.0;
-    public double[] currentAmps = new double[] {};
-    public double[] temperatureCelsius = new double[] {};
-    // Not in cad as of 1/28 but requested
+    public TalonFXLog carriage = new TalonFXLog(0, 0, 0, 0, 0, 0);
     public boolean beambreak = false;
   }
 

src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java

@@ -4,26 +4,22 @@
 
 package frc.robot.subsystems.carriage;
 
-import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import frc.robot.utils.components.InvertedDigitalInput;
+import frc.robot.utils.logging.TalonFXLogger;
 
 /** Create a CarriageIO that uses a real TalonFX. */
 public class CarriageIOReal implements CarriageIO {
-  final TalonFX motor = new TalonFX(18, "canivore");
+  private final TalonFX motor = new TalonFX(18, "canivore");
 
-  final InvertedDigitalInput beambreak = new InvertedDigitalInput(2);
+  private final InvertedDigitalInput beambreak = new InvertedDigitalInput(2);
 
-  final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
+  private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
 
-  final StatusSignal<Double> velocity = motor.getVelocity();
-  final StatusSignal<Double> voltage = motor.getMotorVoltage();
-  final StatusSignal<Double> amperage = motor.getStatorCurrent();
-  final StatusSignal<Double> temp = motor.getDeviceTemp();
+  private final TalonFXLogger logger = new TalonFXLogger(motor);
 
   public CarriageIOReal() {
     var config = new TalonFXConfiguration();
@@ -34,18 +30,12 @@ public CarriageIOReal() {
     config.CurrentLimits.SupplyCurrentLimitEnable = true;
 
     motor.getConfigurator().apply(config);
-    BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, amperage, temp);
-    motor.optimizeBusUtilization();
   }
 
   /** Updates the set of loggable inputs. */
   @Override
   public void updateInputs(final CarriageIOInputsAutoLogged inputs) {
-    BaseStatusSignal.refreshAll(velocity, voltage, amperage, temp);
-    inputs.velocityRotationsPerSecond = velocity.getValueAsDouble();
-    inputs.appliedVolts = voltage.getValueAsDouble();
-    inputs.currentAmps = new double[] {amperage.getValueAsDouble()};
-    inputs.temperatureCelsius = new double[] {temp.getValueAsDouble()};
+    inputs.carriage = logger.update();
 
     inputs.beambreak = beambreak.get();
   }

src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.utils.logging.TalonFXFaultManager;
 import org.littletonrobotics.junction.Logger;
 
 /** Drainpipe style amp/trap mechanism on the elevator */
@@ -25,6 +26,7 @@ public CarriageSubsystem(CarriageIO io) {
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("Carriage", inputs);
+    TalonFXFaultManager.getInstance().addLog("Carriage", inputs.carriage);
   }
 
   /** Run the carriage roller at the specified voltage */

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

@@ -10,6 +10,7 @@
 public interface ClimberIO {
   @AutoLog
   public static class ClimberIOInputs {
+    // Not refactored bc removed
     public double climberVelocityRotationsPerSec = 0.0;
     public double climberAppliedVolts = 0.0;
     public double climberCurrentAmps = 0.0;

src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java

@@ -4,16 +4,14 @@
 
 package frc.robot.subsystems.elevator;
 
+import frc.robot.utils.logging.TalonFXLogger.TalonFXLog;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface ElevatorIO {
   @AutoLog
   public static class ElevatorIOInputs {
-    public double elevatorPositionMeters = 0.0;
-    public double elevatorVelocityMetersPerSec = 0.0;
-    public double elevatorAppliedVolts = 0.0;
-    public double[] elevatorCurrentAmps = new double[] {};
-    public double[] elevatorTempCelsius = new double[] {};
+    public TalonFXLog leader;
+    public TalonFXLog follower;
   }
 
   public void updateInputs(final ElevatorIOInputsAutoLogged inputs);

src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java

@@ -4,8 +4,6 @@
 
 package frc.robot.subsystems.elevator;
 
-import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
@@ -15,6 +13,7 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.wpilibj.Servo;
+import frc.robot.utils.logging.TalonFXLogger;
 
 /** Elevator IO using TalonFXs. */
 public class ElevatorIOReal implements ElevatorIO {
@@ -27,11 +26,9 @@ public class ElevatorIOReal implements ElevatorIO {
   private final MotionMagicVoltage positionVoltage =
       new MotionMagicVoltage(0.0).withEnableFOC(true);
 
-  private final StatusSignal<Double> position = motor.getPosition();
-  private final StatusSignal<Double> velocity = motor.getVelocity();
-  private final StatusSignal<Double> voltage = motor.getMotorVoltage();
-  private final StatusSignal<Double> current = motor.getStatorCurrent();
-  private final StatusSignal<Double> temp = motor.getDeviceTemp();
+  private final TalonFXLogger leaderLogger = new TalonFXLogger(motor);
+
+  private final TalonFXLogger followerLogger = new TalonFXLogger(follower);
 
   public ElevatorIOReal() {
     var config = new TalonFXConfiguration();
@@ -64,20 +61,12 @@ public ElevatorIOReal() {
     motor.setPosition(0.0); // Assume we boot 0ed
     follower.getConfigurator().apply(config);
     follower.setControl(new Follower(motor.getDeviceID(), true));
-
-    BaseStatusSignal.setUpdateFrequencyForAll(50.0, position, velocity, voltage, current, temp);
-    motor.optimizeBusUtilization();
-    follower.optimizeBusUtilization();
   }
 
   @Override
   public void updateInputs(final ElevatorIOInputsAutoLogged inputs) {
-    BaseStatusSignal.refreshAll(position, velocity, voltage, current, temp);
-    inputs.elevatorPositionMeters = position.getValueAsDouble();
-    inputs.elevatorVelocityMetersPerSec = velocity.getValueAsDouble();
-    inputs.elevatorAppliedVolts = voltage.getValueAsDouble();
-    inputs.elevatorCurrentAmps = new double[] {current.getValueAsDouble()};
-    inputs.elevatorTempCelsius = new double[] {temp.getValueAsDouble()};
+    inputs.leader = leaderLogger.update();
+    inputs.follower = followerLogger.update();
   }
 
   @Override

src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import frc.robot.utils.logging.TalonFXLogger.TalonFXLog;
 
 /** WPILib physics model based elevator sim. */
 public class ElevatorIOSim implements ElevatorIO {
@@ -36,11 +37,14 @@ public void updateInputs(final ElevatorIOInputsAutoLogged inputs) {
       stop();
     }
     physicsSim.update(0.020);
-    inputs.elevatorPositionMeters = physicsSim.getPositionMeters();
-    inputs.elevatorVelocityMetersPerSec = physicsSim.getVelocityMetersPerSecond();
-    inputs.elevatorAppliedVolts = volts;
-    inputs.elevatorCurrentAmps = new double[] {physicsSim.getCurrentDrawAmps()};
-    inputs.elevatorTempCelsius = new double[] {20.0};
+    inputs.leader =
+        new TalonFXLog(
+            volts,
+            physicsSim.getCurrentDrawAmps(),
+            physicsSim.getCurrentDrawAmps(),
+            0.0,
+            physicsSim.getPositionMeters(),
+            physicsSim.getVelocityMetersPerSecond());
   }
 
   @Override

src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java

@@ -21,7 +21,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.utils.logging.TalonFXFaultManager;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
@@ -77,8 +77,10 @@ public ElevatorSubsystem(ElevatorIO io) {
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("Elevator", inputs);
+    TalonFXFaultManager.getInstance().addLog("Elevator Leader", inputs.leader);
+    TalonFXFaultManager.getInstance().addLog("Elevator Follower", inputs.follower);
 
-    carriage.setLength(inputs.elevatorPositionMeters);
+    carriage.setLength(inputs.leader.position);
     Logger.recordOutput("Elevator/Mechanism2d", mech2d);
 
     Logger.recordOutput("Elevator/Carriage Pose", getCarriagePose());
@@ -109,40 +111,11 @@ public Command unlockClimb() {
 
   public Command runCurrentZeroing() {
     return this.run(() -> io.setVoltage(-1.0))
-        .until(() -> inputs.elevatorCurrentAmps[0] > 40.0)
+        .until(() -> inputs.leader.statorCurrentAmps > 40.0)
         .finallyDo(() -> io.resetEncoder(0.0))
         .beforeStarting(() -> io.setLockServoRotation(0.2));
   }
 
-  public Command runSysidCmd() {
-    return Commands.sequence(
-        runCurrentZeroing(),
-        this.runOnce(() -> SignalLogger.start()),
-        // Stop when we get close to max to avoid hitting hard stop
-        elevatorRoutine
-            .quasistatic(Direction.kForward)
-            .until(() -> inputs.elevatorPositionMeters > MAX_EXTENSION_METERS - 0.2),
-        this.runOnce(() -> io.setVoltage(0.0)),
-        Commands.waitSeconds(1.0),
-        // Stop when we get close to max to avoid hitting hard stop
-        elevatorRoutine
-            .quasistatic(Direction.kReverse)
-            .until(() -> inputs.elevatorPositionMeters < 0.2),
-        this.runOnce(() -> io.setVoltage(0.0)),
-        Commands.waitSeconds(1.0),
-        // Stop when we get close to max to avoid hitting hard stop
-        elevatorRoutine
-            .dynamic(Direction.kForward)
-            .until(() -> inputs.elevatorPositionMeters > MAX_EXTENSION_METERS - 0.2),
-        this.runOnce(() -> io.setVoltage(0.0)),
-        Commands.waitSeconds(1.0),
-        // Stop when we get close to max to avoid hitting hard stop
-        elevatorRoutine
-            .dynamic(Direction.kReverse)
-            .until(() -> inputs.elevatorPositionMeters < 0.2),
-        this.runOnce(() -> SignalLogger.stop()));
-  }
-
   public Pose3d getCarriagePose() {
     return new Pose3d(
         Units.inchesToMeters(4.5) + carriage.getLength() * ELEVATOR_ANGLE.getCos(),
@@ -162,6 +135,6 @@ public Pose3d getFirstStagePose() {
   }
 
   public double getExtensionMeters() {
-    return inputs.elevatorPositionMeters;
+    return inputs.leader.position;
   }
 }

src/main/java/frc/robot/subsystems/feeder/FeederIO.java

@@ -4,15 +4,13 @@
 
 package frc.robot.subsystems.feeder;
 
+import frc.robot.utils.logging.TalonFXLogger.TalonFXLog;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface FeederIO {
   @AutoLog
   public static class FeederIOInputs {
-    public double feederVelocityRotationsPerSec = 0.0;
-    public double feederAppliedVolts = 0.0;
-    public double feederCurrentAmps = 0.0;
-    public double feederTempC = 0.0;
+    public TalonFXLog feeder = new TalonFXLog(0, 0, 0, 0, 0, 0);
 
     public boolean firstBeambreak = false;
     public boolean lastBeambreak = false;