diff --git a/src/main/java/com/techhounds/houndutil/houndlib/robots/HoundRobot.java b/src/main/java/com/techhounds/houndutil/houndlib/robots/HoundRobot.java index 7b09d2c..c0c7add 100644 --- a/src/main/java/com/techhounds/houndutil/houndlib/robots/HoundRobot.java +++ b/src/main/java/com/techhounds/houndutil/houndlib/robots/HoundRobot.java @@ -1,5 +1,6 @@ package com.techhounds.houndutil.houndlib.robots; +import edu.wpi.first.wpilibj.Threads; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -9,7 +10,9 @@ import com.techhounds.houndutil.houndauto.AutoManager; import com.techhounds.houndutil.houndlib.TriConsumer; +import com.techhounds.houndutil.houndlog.FaultLogger; import com.techhounds.houndutil.houndlog.LoggingManager; +import com.techhounds.houndutil.houndlog.SignalManager; public class HoundRobot extends TimedRobot { public HoundRobot() { @@ -34,14 +37,16 @@ public void robotInit() { // sets the LoggingManager to run every 100ms and on an offset of 10ms from the // main thread LoggingManager.getInstance().init(); - addPeriodic(LoggingManager.getInstance()::run, 0.100, 0.010); + addPeriodic(FaultLogger::update, 0.100, 0.010); LiveWindow.disableAllTelemetry(); + SignalManager.registerAll(); + Threads.setCurrentThreadPriority(true, 99); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - // LoggingManager.getInstance().run(); + LoggingManager.getInstance().run(); } @Override diff --git a/src/main/java/com/techhounds/houndutil/houndlib/swerve/KrakenCoaxialSwerveModule.java b/src/main/java/com/techhounds/houndutil/houndlib/swerve/KrakenCoaxialSwerveModule.java index 0fc1aa7..47edd81 100644 --- a/src/main/java/com/techhounds/houndutil/houndlib/swerve/KrakenCoaxialSwerveModule.java +++ b/src/main/java/com/techhounds/houndutil/houndlib/swerve/KrakenCoaxialSwerveModule.java @@ -1,6 +1,7 @@ package com.techhounds.houndutil.houndlib.swerve; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -17,6 +18,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.techhounds.houndutil.houndlib.MotorHoldMode; +import com.techhounds.houndutil.houndlog.SignalManager; import com.techhounds.houndutil.houndlog.interfaces.Log; import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; @@ -50,6 +52,15 @@ public class KrakenCoaxialSwerveModule implements CoaxialSwerveModule { private SwerveModuleState previousState = new SwerveModuleState(); + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAcceleration; + private final StatusSignal driveMotorVoltage; + private final StatusSignal steerPosition; + private final StatusSignal steerVelocity; + private final StatusSignal steerAcceleration; + private final StatusSignal steerMotorVoltage; + /** * Initalizes a SwerveModule. * @@ -137,30 +148,37 @@ public KrakenCoaxialSwerveModule( SWERVE_CONSTANTS.STEER_GEARING, SWERVE_CONSTANTS.STEER_MOI); + drivePosition = driveMotor.getPosition(); + driveVelocity = driveMotor.getVelocity(); + driveAcceleration = driveMotor.getAcceleration(); + driveMotorVoltage = driveMotor.getMotorVoltage(); + steerPosition = steerMotor.getPosition(); + steerVelocity = steerMotor.getVelocity(); + steerAcceleration = steerMotor.getAcceleration(); + steerMotorVoltage = steerMotor.getMotorVoltage(); + BaseStatusSignal.setUpdateFrequencyForAll(250, - driveMotor.getPosition(), - driveMotor.getVelocity(), - driveMotor.getAcceleration(), - driveMotor.getMotorVoltage(), - steerMotor.getPosition(), - steerMotor.getVelocity(), - steerMotor.getAcceleration(), - steerMotor.getMotorVoltage()); + drivePosition, driveVelocity, driveAcceleration, driveMotorVoltage, + steerPosition, steerVelocity, steerAcceleration, steerMotorVoltage); + + SignalManager.register( + drivePosition, driveVelocity, driveAcceleration, driveMotorVoltage, + steerPosition, steerVelocity, steerAcceleration, steerMotorVoltage); } @Override public double getDriveMotorPosition() { - return BaseStatusSignal.getLatencyCompensatedValue(driveMotor.getPosition(), driveMotor.getVelocity()); + return BaseStatusSignal.getLatencyCompensatedValue(drivePosition, driveVelocity); } @Override public double getDriveMotorVelocity() { - return BaseStatusSignal.getLatencyCompensatedValue(driveMotor.getVelocity(), driveMotor.getAcceleration()); + return BaseStatusSignal.getLatencyCompensatedValue(driveVelocity, driveAcceleration); } @Override public double getDriveMotorVoltage() { - return driveMotor.getMotorVoltage().getValue(); + return driveMotorVoltage.getValue(); } public TalonFX getDriveMotor() { @@ -169,17 +187,17 @@ public TalonFX getDriveMotor() { @Override public double getSteerMotorPosition() { - return BaseStatusSignal.getLatencyCompensatedValue(steerMotor.getPosition(), steerMotor.getVelocity()); + return BaseStatusSignal.getLatencyCompensatedValue(steerPosition, steerVelocity); } @Override public double getSteerMotorVelocity() { - return BaseStatusSignal.getLatencyCompensatedValue(steerMotor.getVelocity(), steerMotor.getAcceleration()); + return BaseStatusSignal.getLatencyCompensatedValue(steerVelocity, steerAcceleration); } @Override public double getSteerMotorVoltage() { - return steerMotor.getMotorVoltage().getValue(); + return steerMotorVoltage.getValue(); } public TalonFX getSteerMotor() { @@ -187,8 +205,7 @@ public TalonFX getSteerMotor() { } public BaseStatusSignal[] getSignals() { - return new BaseStatusSignal[] { driveMotor.getPosition(), driveMotor.getVelocity(), - steerMotor.getPosition(), steerMotor.getVelocity() }; + return new BaseStatusSignal[] { drivePosition, driveVelocity, steerPosition, steerVelocity }; } @Override diff --git a/src/main/java/com/techhounds/houndutil/houndlog/FaultLogger.java b/src/main/java/com/techhounds/houndutil/houndlog/FaultLogger.java index 668ab4d..7098b9b 100644 --- a/src/main/java/com/techhounds/houndutil/houndlog/FaultLogger.java +++ b/src/main/java/com/techhounds/houndutil/houndlog/FaultLogger.java @@ -11,10 +11,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringArrayPublisher; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.RobotBase; - import java.lang.reflect.Field; import java.util.ArrayList; import java.util.HashSet; @@ -266,6 +263,8 @@ public static void register(TalonFX talon) { talon.getStickyFault_UnstableSupplyV(), talon.getStickyFault_UsingFusedCANcoderWhileUnlicensed()); + faultSignals.forEach((s) -> SignalManager.register(s)); + for (StatusSignal signal : faultSignals) { register(signal::getValue, "Talon FX [" + talon.getDeviceID() + "]", signal.getName(), FaultType.ERROR); } @@ -294,6 +293,8 @@ public static void register(CANcoder cancoder) { cancoder.getStickyFault_Undervoltage(), cancoder.getStickyFault_UnlicensedFeatureInUse()); + faultSignals.forEach((s) -> SignalManager.register(s)); + for (StatusSignal signal : faultSignals) { register(signal::getValue, "CANcoder [" + cancoder.getDeviceID() + "]", signal.getName(), FaultType.ERROR); @@ -334,6 +335,8 @@ public static void register(Pigeon2 pigeon) { pigeon.getStickyFault_Undervoltage(), pigeon.getStickyFault_UnlicensedFeatureInUse()); + faultSignals.forEach((s) -> SignalManager.register(s)); + for (StatusSignal signal : faultSignals) { register(signal::getValue, "Pigeon 2 [" + pigeon.getDeviceID() + "]", signal.getName(), FaultType.ERROR); diff --git a/src/main/java/com/techhounds/houndutil/houndlog/LogProfileBuilder.java b/src/main/java/com/techhounds/houndutil/houndlog/LogProfileBuilder.java index b60552e..8cdf31b 100644 --- a/src/main/java/com/techhounds/houndutil/houndlog/LogProfileBuilder.java +++ b/src/main/java/com/techhounds/houndutil/houndlog/LogProfileBuilder.java @@ -2,6 +2,7 @@ import java.lang.reflect.Field; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; @@ -18,6 +19,7 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticHub; import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; @@ -28,7 +30,6 @@ import com.techhounds.houndutil.houndlog.logitems.DoubleArrayLogItem; import com.techhounds.houndutil.houndlog.logitems.DoubleLogItem; import com.techhounds.houndutil.houndlog.logitems.FloatLogItem; -import com.techhounds.houndutil.houndlog.logitems.IntegerLogItem; import com.techhounds.houndutil.houndlog.logitems.StringLogItem; import com.techhounds.houndutil.houndlog.logitems.TunableDouble; @@ -80,24 +81,41 @@ public static String ctreFaultsToString(int value) { * @return the array of LogItems */ public static AbstractLogItem[] buildTalonFXLogItems(TalonFX obj) { + StatusSignal position = obj.getPosition(); + StatusSignal velocity = obj.getVelocity(); + StatusSignal acceleration = obj.getAcceleration(); + StatusSignal temp = obj.getDeviceTemp(); + StatusSignal outputVoltage = obj.getMotorVoltage(); + StatusSignal outputCurrent = obj.getTorqueCurrent(); + + SignalManager.register(position, velocity, acceleration, temp, outputVoltage, outputCurrent); FaultLogger.register(obj); return new AbstractLogItem[] { - new DoubleLogItem("position", () -> obj.getPosition().getValue(), LogType.NT), - new DoubleLogItem("velocity", () -> obj.getVelocity().getValue(), LogType.NT), - new DoubleLogItem("acceleration", () -> obj.getAcceleration().getValue(), LogType.NT), - new DoubleLogItem("temperature", () -> obj.getDeviceTemp().getValue(), LogType.NT), - new DoubleLogItem("speed", () -> obj.getDutyCycle().getValue() / 2.0, LogType.NT), // [-2, 2) - new DoubleLogItem("outputVoltage", () -> obj.getMotorVoltage().getValue(), LogType.NT), - new DoubleLogItem("busVoltage", () -> obj.getSupplyVoltage().getValue(), LogType.NT), - new DoubleLogItem("outputCurrent", () -> obj.getTorqueCurrent().getValue(), LogType.NT), - new DoubleLogItem("supplyCurrent", () -> obj.getSupplyCurrent().getValue(), LogType.NT), - new StringLogItem("bridgeOutput", () -> obj.getBridgeOutput().getValue().toString(), LogType.NT), - new DoubleLogItem("closedLoopReference", () -> obj.getClosedLoopReference().getValue(), LogType.NT), - new DoubleLogItem("closedLoopOutput", () -> obj.getClosedLoopOutput().getValue(), LogType.NT), - new DoubleLogItem("closedLoopError", () -> obj.getClosedLoopError().getValue(), LogType.NT), - new StringLogItem("faults", () -> ctreFaultsToString(obj.getFaultField().getValue()), LogType.NT), - new StringLogItem("stickyFaults", () -> ctreFaultsToString(obj.getStickyFaultField().getValue()), - LogType.NT) + new DoubleLogItem("position", () -> position.getValue(), LogType.NT), + new DoubleLogItem("velocity", () -> velocity.getValue(), LogType.NT), + new DoubleLogItem("acceleration", () -> acceleration.getValue(), LogType.NT), + new DoubleLogItem("temperature", () -> temp.getValue(), LogType.NT), + // new DoubleLogItem("speed", () -> obj.getDutyCycle().getValue() / 2.0, + // LogType.DEBUG), // [-2, 2) + new DoubleLogItem("outputVoltage", () -> outputVoltage.getValue(), LogType.NT), + // new DoubleLogItem("busVoltage", () -> obj.getSupplyVoltage().getValue(), + // LogType.DEBUG), + new DoubleLogItem("outputCurrent", () -> outputCurrent.getValue(), LogType.NT), + // new DoubleLogItem("supplyCurrent", () -> obj.getSupplyCurrent().getValue(), + // LogType.DEBUG), + // new StringLogItem("bridgeOutput", () -> + // obj.getBridgeOutput().getValue().toString(), LogType.DEBUG), + // new DoubleLogItem("closedLoopReference", () -> + // obj.getClosedLoopReference().getValue(), LogType.DEBUG), + // new DoubleLogItem("closedLoopOutput", () -> + // obj.getClosedLoopOutput().getValue(), LogType.DEBUG), + // new DoubleLogItem("closedLoopError", () -> + // obj.getClosedLoopError().getValue(), LogType.DEBUG), + // new StringLogItem("faults", () -> + // ctreFaultsToString(obj.getFaultField().getValue()), LogType.DEBUG), + // new StringLogItem("stickyFaults", () -> + // ctreFaultsToString(obj.getStickyFaultField().getValue()), + // LogType.DEBUG) }; } @@ -114,26 +132,19 @@ public static AbstractLogItem[] buildCANSparkMaxLogItems(CANSparkMax obj) { private static AbstractLogItem[] buildCANSparkBaseLogItems(CANSparkBase obj) { return new AbstractLogItem[] { new DoubleLogItem("encoderPosition", obj.getEncoder()::getPosition, LogType.NT), - // new DoubleLogItem("encoderPositionConversionFactor", - // obj.getEncoder()::getPositionConversionFactor, LogType.DATALOG), new DoubleLogItem("encoderVelocity", obj.getEncoder()::getVelocity, LogType.NT), - // new DoubleLogItem("encoderVelocityConversionFactor", - // obj.getEncoder()::getVelocityConversionFactor, LogType.DATALOG), new DoubleLogItem("speed", obj::get, LogType.NT), - new DoubleLogItem("outputVoltage", obj::getAppliedOutput, LogType.NT), - new DoubleLogItem("busVoltage", obj::getBusVoltage, LogType.DATALOG), + new DoubleLogItem("outputVoltage", + () -> RobotBase.isReal() ? obj.getAppliedOutput() * obj.getBusVoltage() + : obj.getAppliedOutput(), + LogType.NT), + // new DoubleLogItem("busVoltage", obj::getBusVoltage, LogType.DEBUG), new DoubleLogItem("motorTemperature", obj::getMotorTemperature, LogType.NT), new DoubleLogItem("outputCurrent", obj::getOutputCurrent, LogType.NT), - new IntegerLogItem("deviceId", obj::getDeviceId, LogType.DATALOG), - new StringLogItem("firmwareVersion", obj::getFirmwareString, LogType.DATALOG), - // new BooleanLogItem("brakeMode", () -> obj.getIdleMode() == - // CANSparkBase.IdleMode.kBrake, LogType.NT), - // new BooleanLogItem("isInverted", obj::getInverted, LogType.DATALOG), - new BooleanLogItem("isFollower", obj::isFollower, LogType.DATALOG), - new StringLogItem("faults", - () -> revFaultsToString(obj.getFaults()), LogType.NT), - new StringLogItem("stickyFaults", - () -> revFaultsToString(obj.getStickyFaults()), LogType.NT), + // new StringLogItem("faults", + // () -> revFaultsToString(obj.getFaults()), LogType.DEBUG), + // new StringLogItem("stickyFaults", + // () -> revFaultsToString(obj.getStickyFaults()), LogType.DEBUG), }; } @@ -144,17 +155,26 @@ private static AbstractLogItem[] buildCANSparkBaseLogItems(CANSparkBase obj) * @return the array of LogItems */ public static AbstractLogItem[] buildCANcoderLogItems(CANcoder obj) { + StatusSignal absolutePosition = obj.getAbsolutePosition(); + StatusSignal position = obj.getPosition(); + StatusSignal velocity = obj.getVelocity(); + + SignalManager.register(absolutePosition, position, velocity); FaultLogger.register(obj); return new AbstractLogItem[] { - new DoubleLogItem("absolutePosition", () -> obj.getAbsolutePosition().getValue(), LogType.NT), - new DoubleLogItem("position", () -> obj.getPosition().getValue(), LogType.NT), - new DoubleLogItem("velocity", () -> obj.getVelocity().getValue(), LogType.NT), - new DoubleLogItem("busVoltage", () -> obj.getSupplyVoltage().getValue(), LogType.DATALOG), - new IntegerLogItem("deviceId", obj::getDeviceID, LogType.DATALOG), - new StringLogItem("magnetHealth", () -> obj.getMagnetHealth().getValue().name(), LogType.DATALOG), - new StringLogItem("faults", () -> ctreFaultsToString(obj.getFaultField().getValue()), LogType.NT), - new StringLogItem("stickyFaults", () -> ctreFaultsToString(obj.getStickyFaultField().getValue()), - LogType.NT) + new DoubleLogItem("absolutePosition", () -> absolutePosition.getValue(), LogType.NT), + new DoubleLogItem("position", () -> position.getValue(), LogType.NT), + new DoubleLogItem("velocity", () -> velocity.getValue(), LogType.NT), + // new DoubleLogItem("busVoltage", () -> obj.getSupplyVoltage().getValue(), + // LogType.DEBUG), + // new IntegerLogItem("deviceId", obj::getDeviceID, LogType.DEBUG), + // new StringLogItem("magnetHealth", () -> + // obj.getMagnetHealth().getValue().name(), LogType.DEBUG), + // new StringLogItem("faults", () -> + // ctreFaultsToString(obj.getFaultField().getValue()), LogType.DEBUG), + // new StringLogItem("stickyFaults", () -> + // ctreFaultsToString(obj.getStickyFaultField().getValue()), + // LogType.DEBUG) }; } @@ -169,11 +189,6 @@ public static AbstractLogItem[] buildSparkAbsoluteEncoderLogItems(SparkAbsolu new DoubleLogItem("position", obj::getPosition, LogType.NT), new DoubleLogItem("velocity", obj::getVelocity, LogType.NT), new DoubleLogItem("zeroOffset", obj::getZeroOffset, LogType.NT), - // new BooleanLogItem("isInverted", obj::getInverted, LogType.DATALOG), - // new DoubleLogItem("positionConversionFactor", - // obj::getPositionConversionFactor, LogType.DATALOG), - // new DoubleLogItem("velocityConversionFactor", - // obj::getVelocityConversionFactor, LogType.DATALOG), }; } @@ -214,20 +229,31 @@ public static AbstractLogItem[] buildNavXLogItems(AHRS obj) { * @return the array of LogItems */ public static AbstractLogItem[] buildPigeon2LogItems(Pigeon2 obj) { + StatusSignal pitch = obj.getPitch(); + StatusSignal roll = obj.getRoll(); + StatusSignal yaw = obj.getYaw(); + + SignalManager.register(pitch, roll, yaw); FaultLogger.register(obj); return new AbstractLogItem[] { - new DoubleLogItem("pitch", () -> obj.getPitch().getValue(), LogType.NT), - new DoubleLogItem("roll", () -> obj.getRoll().getValue(), LogType.NT), - new DoubleLogItem("yaw", () -> obj.getYaw().getValue(), LogType.NT), - new DoubleLogItem("yawRad", () -> Units.degreesToRadians(obj.getYaw().getValue()), LogType.NT), - new DoubleLogItem("xAcceleration", () -> obj.getAccelerationX().getValue(), LogType.DATALOG), - new DoubleLogItem("yAcceleration", () -> obj.getAccelerationY().getValue(), LogType.DATALOG), - new DoubleLogItem("zAcceleration", () -> obj.getAccelerationZ().getValue(), LogType.DATALOG), - new DoubleLogItem("temperature", () -> obj.getTemperature().getValue(), LogType.DATALOG), - new IntegerLogItem("deviceId", obj::getDeviceID, LogType.DATALOG), - new StringLogItem("faults", () -> ctreFaultsToString(obj.getFaultField().getValue()), LogType.NT), - new StringLogItem("stickyFaults", () -> ctreFaultsToString(obj.getStickyFaultField().getValue()), - LogType.NT) + new DoubleLogItem("pitch", () -> pitch.getValue(), LogType.NT), + new DoubleLogItem("roll", () -> roll.getValue(), LogType.NT), + new DoubleLogItem("yaw", () -> yaw.getValue(), LogType.NT), + new DoubleLogItem("yawRad", () -> Units.degreesToRadians(yaw.getValue()), LogType.NT), + // new DoubleLogItem("xAcceleration", () -> obj.getAccelerationX().getValue(), + // LogType.DEBUG), + // new DoubleLogItem("yAcceleration", () -> obj.getAccelerationY().getValue(), + // LogType.DEBUG), + // new DoubleLogItem("zAcceleration", () -> obj.getAccelerationZ().getValue(), + // LogType.DEBUG), + // new DoubleLogItem("temperature", () -> obj.getTemperature().getValue(), + // LogType.DEBUG), + // new IntegerLogItem("deviceId", obj::getDeviceID, LogType.DEBUG), + // new StringLogItem("faults", () -> + // ctreFaultsToString(obj.getFaultField().getValue()), LogType.DEBUG), + // new StringLogItem("stickyFaults", () -> + // ctreFaultsToString(obj.getStickyFaultField().getValue()), + // LogType.DEBUG) }; } @@ -259,113 +285,117 @@ public static AbstractLogItem[] buildPDHLogItems(PowerDistribution obj) { new DoubleLogItem("voltage", obj::getVoltage, LogType.NT), new DoubleLogItem("temperature", obj::getTemperature, LogType.NT), new DoubleLogItem("totalCurrentAmps", obj::getTotalCurrent, LogType.NT), - new DoubleLogItem("totalPowerWatts", obj::getTotalPower, LogType.NT), - new DoubleLogItem("totalEnergyJoules", obj::getTotalEnergy, LogType.NT), - new DoubleArrayLogItem("channelCurrents", () -> new double[] { - obj.getCurrent(0), - obj.getCurrent(1), - obj.getCurrent(2), - obj.getCurrent(3), - obj.getCurrent(4), - obj.getCurrent(5), - obj.getCurrent(6), - obj.getCurrent(7), - obj.getCurrent(8), - obj.getCurrent(9), - obj.getCurrent(10), - obj.getCurrent(11), - obj.getCurrent(12), - obj.getCurrent(13), - obj.getCurrent(14), - obj.getCurrent(15), - obj.getCurrent(16), - obj.getCurrent(17), - obj.getCurrent(18), - obj.getCurrent(19), - obj.getCurrent(20), - obj.getCurrent(21), - obj.getCurrent(22), - obj.getCurrent(23), - }, LogType.DATALOG), - new StringLogItem("firmwareVersion", - () -> obj.getVersion().firmwareMajor + "." - + obj.getVersion().firmwareMinor + "." - + obj.getVersion().firmwareFix, - LogType.DATALOG), - new StringLogItem("type", - () -> obj.getType().toString(), - LogType.DATALOG), - new BooleanLogItem("isSwitchableChannelActive", obj::getSwitchableChannel, - LogType.NT), - new BooleanArrayLogItem("faults/breakerFaults", - () -> new boolean[] { - obj.getFaults().Channel0BreakerFault, - obj.getFaults().Channel1BreakerFault, - obj.getFaults().Channel2BreakerFault, - obj.getFaults().Channel3BreakerFault, - obj.getFaults().Channel4BreakerFault, - obj.getFaults().Channel5BreakerFault, - obj.getFaults().Channel6BreakerFault, - obj.getFaults().Channel7BreakerFault, - obj.getFaults().Channel8BreakerFault, - obj.getFaults().Channel9BreakerFault, - obj.getFaults().Channel10BreakerFault, - obj.getFaults().Channel11BreakerFault, - obj.getFaults().Channel12BreakerFault, - obj.getFaults().Channel13BreakerFault, - obj.getFaults().Channel14BreakerFault, - obj.getFaults().Channel15BreakerFault, - obj.getFaults().Channel16BreakerFault, - obj.getFaults().Channel17BreakerFault, - obj.getFaults().Channel18BreakerFault, - obj.getFaults().Channel19BreakerFault, - obj.getFaults().Channel20BreakerFault, - obj.getFaults().Channel21BreakerFault, - obj.getFaults().Channel22BreakerFault, - obj.getFaults().Channel23BreakerFault, - }, - LogType.DATALOG), - new BooleanLogItem("faults/brownout", () -> obj.getFaults().Brownout, LogType.DATALOG), - new BooleanLogItem("faults/canWarning", - () -> obj.getFaults().CanWarning, LogType.DATALOG), - new BooleanLogItem("faults/hardwareFault", () -> obj.getFaults().HardwareFault, - LogType.DATALOG), - new BooleanArrayLogItem("faults/sticky/breakerFaults", - () -> new boolean[] { - obj.getStickyFaults().Channel0BreakerFault, - obj.getStickyFaults().Channel1BreakerFault, - obj.getStickyFaults().Channel2BreakerFault, - obj.getStickyFaults().Channel3BreakerFault, - obj.getStickyFaults().Channel4BreakerFault, - obj.getStickyFaults().Channel5BreakerFault, - obj.getStickyFaults().Channel6BreakerFault, - obj.getStickyFaults().Channel7BreakerFault, - obj.getStickyFaults().Channel8BreakerFault, - obj.getStickyFaults().Channel9BreakerFault, - obj.getStickyFaults().Channel10BreakerFault, - obj.getStickyFaults().Channel11BreakerFault, - obj.getStickyFaults().Channel12BreakerFault, - obj.getStickyFaults().Channel13BreakerFault, - obj.getStickyFaults().Channel14BreakerFault, - obj.getStickyFaults().Channel15BreakerFault, - obj.getStickyFaults().Channel16BreakerFault, - obj.getStickyFaults().Channel17BreakerFault, - obj.getStickyFaults().Channel18BreakerFault, - obj.getStickyFaults().Channel19BreakerFault, - obj.getStickyFaults().Channel20BreakerFault, - obj.getStickyFaults().Channel21BreakerFault, - obj.getStickyFaults().Channel22BreakerFault, - obj.getStickyFaults().Channel23BreakerFault, - }, - LogType.DATALOG), - new BooleanLogItem("faults/sticky/brownout", () -> obj.getStickyFaults().Brownout, - LogType.DATALOG), - new BooleanLogItem("faults/sticky/canBusOff", - () -> obj.getStickyFaults().CanBusOff, LogType.DATALOG), - new BooleanLogItem("faults/sticky/canWarning", - () -> obj.getStickyFaults().CanWarning, LogType.DATALOG), - new BooleanLogItem("faults/sticky/hasReset", () -> obj.getStickyFaults().HasReset, - LogType.DATALOG) + // new DoubleLogItem("totalPowerWatts", obj::getTotalPower, LogType.DEBUG), + // new DoubleLogItem("totalEnergyJoules", obj::getTotalEnergy, LogType.DEBUG), + // new DoubleArrayLogItem("channelCurrents", () -> new double[] { + // obj.getCurrent(0), + // obj.getCurrent(1), + // obj.getCurrent(2), + // obj.getCurrent(3), + // obj.getCurrent(4), + // obj.getCurrent(5), + // obj.getCurrent(6), + // obj.getCurrent(7), + // obj.getCurrent(8), + // obj.getCurrent(9), + // obj.getCurrent(10), + // obj.getCurrent(11), + // obj.getCurrent(12), + // obj.getCurrent(13), + // obj.getCurrent(14), + // obj.getCurrent(15), + // obj.getCurrent(16), + // obj.getCurrent(17), + // obj.getCurrent(18), + // obj.getCurrent(19), + // obj.getCurrent(20), + // obj.getCurrent(21), + // obj.getCurrent(22), + // obj.getCurrent(23), + // }, LogType.DATALOG), + // new StringLogItem("firmwareVersion", + // () -> obj.getVersion().firmwareMajor + "." + // + obj.getVersion().firmwareMinor + "." + // + obj.getVersion().firmwareFix, + // LogType.DEBUG), + // new StringLogItem("type", + // () -> obj.getType().toString(), + // LogType.DEBUG), + // new BooleanLogItem("isSwitchableChannelActive", obj::getSwitchableChannel, + // LogType.DEBUG), + // new BooleanArrayLogItem("faults/breakerFaults", + // () -> new boolean[] { + // obj.getFaults().Channel0BreakerFault, + // obj.getFaults().Channel1BreakerFault, + // obj.getFaults().Channel2BreakerFault, + // obj.getFaults().Channel3BreakerFault, + // obj.getFaults().Channel4BreakerFault, + // obj.getFaults().Channel5BreakerFault, + // obj.getFaults().Channel6BreakerFault, + // obj.getFaults().Channel7BreakerFault, + // obj.getFaults().Channel8BreakerFault, + // obj.getFaults().Channel9BreakerFault, + // obj.getFaults().Channel10BreakerFault, + // obj.getFaults().Channel11BreakerFault, + // obj.getFaults().Channel12BreakerFault, + // obj.getFaults().Channel13BreakerFault, + // obj.getFaults().Channel14BreakerFault, + // obj.getFaults().Channel15BreakerFault, + // obj.getFaults().Channel16BreakerFault, + // obj.getFaults().Channel17BreakerFault, + // obj.getFaults().Channel18BreakerFault, + // obj.getFaults().Channel19BreakerFault, + // obj.getFaults().Channel20BreakerFault, + // obj.getFaults().Channel21BreakerFault, + // obj.getFaults().Channel22BreakerFault, + // obj.getFaults().Channel23BreakerFault, + // }, + // LogType.DEBUG), + // new BooleanLogItem("faults/brownout", () -> obj.getFaults().Brownout, + // LogType.DEBUG), + // new BooleanLogItem("faults/canWarning", + // () -> obj.getFaults().CanWarning, LogType.DEBUG), + // new BooleanLogItem("faults/hardwareFault", () -> + // obj.getFaults().HardwareFault, + // LogType.DEBUG), + // new BooleanArrayLogItem("faults/sticky/breakerFaults", + // () -> new boolean[] { + // obj.getStickyFaults().Channel0BreakerFault, + // obj.getStickyFaults().Channel1BreakerFault, + // obj.getStickyFaults().Channel2BreakerFault, + // obj.getStickyFaults().Channel3BreakerFault, + // obj.getStickyFaults().Channel4BreakerFault, + // obj.getStickyFaults().Channel5BreakerFault, + // obj.getStickyFaults().Channel6BreakerFault, + // obj.getStickyFaults().Channel7BreakerFault, + // obj.getStickyFaults().Channel8BreakerFault, + // obj.getStickyFaults().Channel9BreakerFault, + // obj.getStickyFaults().Channel10BreakerFault, + // obj.getStickyFaults().Channel11BreakerFault, + // obj.getStickyFaults().Channel12BreakerFault, + // obj.getStickyFaults().Channel13BreakerFault, + // obj.getStickyFaults().Channel14BreakerFault, + // obj.getStickyFaults().Channel15BreakerFault, + // obj.getStickyFaults().Channel16BreakerFault, + // obj.getStickyFaults().Channel17BreakerFault, + // obj.getStickyFaults().Channel18BreakerFault, + // obj.getStickyFaults().Channel19BreakerFault, + // obj.getStickyFaults().Channel20BreakerFault, + // obj.getStickyFaults().Channel21BreakerFault, + // obj.getStickyFaults().Channel22BreakerFault, + // obj.getStickyFaults().Channel23BreakerFault, + // }, + // LogType.DEBUG), + // new BooleanLogItem("faults/sticky/brownout", () -> + // obj.getStickyFaults().Brownout, + // LogType.DEBUG), + // new BooleanLogItem("faults/sticky/canBusOff", + // () -> obj.getStickyFaults().CanBusOff, LogType.DEBUG), + // new BooleanLogItem("faults/sticky/canWarning", + // () -> obj.getStickyFaults().CanWarning, LogType.DEBUG), + // new BooleanLogItem("faults/sticky/hasReset", () -> + // obj.getStickyFaults().HasReset, + // LogType.DEBUG) }; } @@ -466,10 +496,12 @@ public static AbstractLogItem[] buildPIDControllerLogItems(PIDController obj) LogType.NT), new BooleanLogItem("atSetpoint", () -> obj.atSetpoint(), LogType.NT), - new BooleanLogItem("isContinuousInputEnabled", () -> obj.isContinuousInputEnabled(), - LogType.NT), + // new BooleanLogItem("isContinuousInputEnabled", () -> + // obj.isContinuousInputEnabled(), + // LogType.DEBUG), new DoubleLogItem("positionError", () -> obj.getPositionError(), LogType.NT), - new DoubleLogItem("velocityError", () -> obj.getVelocityError(), LogType.DATALOG), + // new DoubleLogItem("velocityError", () -> obj.getVelocityError(), + // LogType.DEBUG), new TunableDouble("tunables/kP", obj.getP(), (d) -> obj.setP(d)), new TunableDouble("tunables/kI", obj.getI(), (d) -> obj.setI(d)), new TunableDouble("tunables/kD", obj.getD(), (d) -> obj.setD(d)), @@ -514,8 +546,8 @@ public static AbstractLogItem[] buildProfiledPIDControllerLogItems(ProfiledPI LogType.NT), new DoubleLogItem("positionError", () -> obj.getPositionError(), LogType.NT), - new DoubleLogItem("velocityError", () -> obj.getVelocityError(), - LogType.DATALOG), + // new DoubleLogItem("velocityError", () -> obj.getVelocityError(), + // LogType.DEBUG), new TunableDouble("tunables/kP", obj.getP(), (d) -> obj.setP(d)), new TunableDouble("tunables/kI", obj.getI(), (d) -> obj.setI(d)), new TunableDouble("tunables/kD", obj.getD(), (d) -> obj.setD(d)), diff --git a/src/main/java/com/techhounds/houndutil/houndlog/LoggingManager.java b/src/main/java/com/techhounds/houndutil/houndlog/LoggingManager.java index de0c64a..53f9c01 100644 --- a/src/main/java/com/techhounds/houndutil/houndlog/LoggingManager.java +++ b/src/main/java/com/techhounds/houndutil/houndlog/LoggingManager.java @@ -134,7 +134,6 @@ public void run() { for (Loggable loggable : loggables) { loggable.run(); } - FaultLogger.update(); loggingLoopTimeMs = (Timer.getFPGATimestamp() - startTime) * 1000; } } diff --git a/src/main/java/com/techhounds/houndutil/houndlog/SignalManager.java b/src/main/java/com/techhounds/houndutil/houndlog/SignalManager.java new file mode 100644 index 0000000..27ab368 --- /dev/null +++ b/src/main/java/com/techhounds/houndutil/houndlog/SignalManager.java @@ -0,0 +1,28 @@ +package com.techhounds.houndutil.houndlog; + +import java.util.ArrayList; + +import com.ctre.phoenix6.BaseStatusSignal; + +public class SignalManager { + private static ArrayList statusesList = new ArrayList<>(); + private static BaseStatusSignal[] statuses; + + public static void register(BaseStatusSignal... statuses) { + for (var status : statuses) { + statusesList.add(status); + } + } + + public static void registerAll() { + statuses = new BaseStatusSignal[statusesList.size()]; + + statusesList.toArray(statuses); + + statusesList.clear(); + } + + public static void refresh() { + BaseStatusSignal.waitForAll(0, statuses); + } +} \ No newline at end of file diff --git a/src/main/java/com/techhounds/houndutil/houndlog/enums/LogType.java b/src/main/java/com/techhounds/houndutil/houndlog/enums/LogType.java index bb09d31..54a0212 100644 --- a/src/main/java/com/techhounds/houndutil/houndlog/enums/LogType.java +++ b/src/main/java/com/techhounds/houndutil/houndlog/enums/LogType.java @@ -9,7 +9,7 @@ public enum LogType { /** * LogValues set at this type will only be available via the console (over NT). */ - CONSOLE(2), + DEBUG(2), /** * LogValues set at this level will be available via DataLog, but not NT. */