Add Monitor and MonitoredSubystem to WPILIb Interface

[aidnem]

Add a Monitor class to monitor whether a certain state is unacceptable for an unacceptable period of time and take action if it is. Also add a MonitoredSubsystem class to check each monitor every time periodic is called.

This has already been described and implemented in 2024-Robot-Code:199.

Scope

• Add Monitor class
• Add MonitoredSubsystem class
• Test it out in sim to see if it works.

[jkleiber]

I think the callback approach is the way to go (see this comment).

The monitored subsystem would live in wpi_interface, but I think the monitor class itself should live in its own package (maybe called "monitors")

[aidnem]

I'm going to add the option for a building architecture for the Monitor class because I think it will be more readable and clear than having a giant constructor with a bunch of unnamed suppliers.

/* BEFORE */
MonitorManager.addMonitor(
    new CustomMonitor(
        "armEncoderUnplugged", // Name to log callback under
        true, // Sticky fault (remain faulted even after conditions become acceptable again
        () -> !(DriverStation.isEnabled()
                && (aimerInputs.aimAppliedVolts > ScoringConstants.aimerkS * 2
                        && aimerInputs.aimVelocityRadPerSec
                                < ScoringConstants.aimerMovementThresholdRadPerSec)), // isStateValid function to check whether the state is currently valid or not
        ScoringConstants.maxAimUnresponsiveTimeSeconds, // timeToFault (how long can state be invalid before a fault occurs)
        () -> { // faultCallback, the function run every tick when the monitor has detected a fault.
            ScoringSubsystem.setOverrideMode(true);
            ScoringSubsystem.setOverrideVolts(0.0);
        }));
/* AFTER */
MonitorManager.addMonitor(
    new CustomMonitor("armEncoderUnplugged") // Name of Monitor. This will be the name it's logged under.
        .withStickyness(true) // Sticky fault (remain faulted even after conditions become acceptable again
        .withIsStateValid(() -> !(DriverStation.isEnabled()
                && (aimerInputs.aimAppliedVolts > ScoringConstants.aimerkS * 2
                        && aimerInputs.aimVelocityRadPerSec
                                < ScoringConstants.aimerMovementThresholdRadPerSec))) // isStateValid function to check whether the state is currently valid or not
        .withTimeToFault(ScoringConstants.maxAimUnresponsiveTimeSeconds) // timeToFault (how long can state be invalid before a fault occurs)
        .withFaultCallback(() -> { // faultCallback, the function run every tick when the monitor has detected a fault.
            ScoringSubsystem.setOverrideMode(true);
            ScoringSubsystem.setOverrideVolts(0.0);
        }));

[avidraccoon]

Where will the monitors be added to the MonitorManager?

[aidnem]

Monitor is under its own package and MonitoredSubsystem is under wpi interface.

[avidraccoon]

Where in the robot code would we want them to be used we might want to limit them to prevent hacky solutions and make sure the are added in a consistent manor.

[aidnem]

Subsystems should use them by extending MonitoredSubsystem. They will register their monitors in their constructor and then the monitors will automatically be run every periodic by the MonitoredSubsystem; no other action needs to be taken. I think this solution is fairly consistent.

[avidraccoon]

So should only MonitoredSubsytem's child classes be able to add the monitors?

[aidnem]

No I believe they should be able to be created anywhere, because if we create a monitor for battery voltage it might live in robot container.

[avidraccoon]

Where would that monitor even be added? It isn't a MonitoredSubsystem where they are meant to be stored.

[aidnem]

Just manually store the monitor as a variable and run its periodic in the periodic of RobotContainer.