disable subsystems when hardware disconnects

[tildezero]

resolves #149

(currently untested)

[tildezero]

hardware test results:

• chassis only works when CAN is disconnected, not the weird other side port
• yaw should disable when 1 of 2 doubleyawmotors is disconnected, not both

[calebchalmers]

Thanks for putting this together -- great work! I've added some comments for changes throughout the code if you could take a look at those.

With the UTSubsystem parent class, I have an idea which might simplify things a bit: Currently, each UTSubsystem has to run setAmputated(!hardwareOk()) manually on refresh, but really this can be done in the background for every subsystem. I think the best way to do this would actually subclass the CommandScheduler. i.e.

void CommandScheduler::run()
{
#ifndef PLATFORM_HOSTED
    uint32_t runStart = arch::clock::getTimeMicroseconds();
#endif

    // We can insert our custom hardware checks here:
    for (auto it = subMapBegin(); it != subMapEnd(); it++)
    {
        Subsystem *sub = *it;
        sub->setAmputated(sub->hardwareOk());
    }

    // This is the built-in hardware checking system
    if (runningHardwareTests)
    {
        // Call runHardwareTests on all subsystems in the registeredSubsystemBitmap
        // if a hardware test is not already complete
        for (auto it = subMapBegin(); it != subMapEnd(); it++)
        {
            Subsystem *sub = *it;
            if (!sub->isHardwareTestComplete())
            {
                sub->runHardwareTests();
            }
            sub->refresh();
        }
        return;
    }
(...)

Update: nevermind looks like you can't change the CommandScheduler so easily, so let's leave that for later.

[calebchalmers]

@MChung04 this is similar to the hardware checks you're working on. Might be some helpful stuff in here or implementations that can be shared between the tasks

[calebchalmers]

Looks like clang-format is mad about odometry_subsystem.cpp for some reason so this might need formatting again

[tildezero]

hardware test 2: seems like flywheel isn't turning off

[calebchalmers]

Fixed the flywheel and hero build issue. I removed the subsystems:: qualification from all the Subsystem subclass references since the subsystems are already in that namespace, and there is no naming conflict (our parent class is the only one included in each subsystem).

Otherwise looks great -- thanks!