Program Listing for File command_scheduler.hpp

↰ Return to documentation for file (src/tap/control/command_scheduler.hpp)

/*
 * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington <robomstr@uw.edu>
 *
 * This file is part of Taproot.
 *
 * Taproot is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Taproot is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
 */

#ifndef TAPROOT_COMMAND_SCHEDULER_HPP_
#define TAPROOT_COMMAND_SCHEDULER_HPP_

#include <iterator>

#include "tap/util_macros.hpp"

#include "command_scheduler_types.hpp"

namespace tap
{
class Drivers;
namespace control
{
class Command;
class Subsystem;

class SafeDisconnectFunction
{
public:
    SafeDisconnectFunction(){};
    virtual bool operator()() { return false; }
};

class CommandScheduler
{
public:
    CommandScheduler(
        Drivers* drivers,
        bool masterScheduler = false,
        SafeDisconnectFunction* safeDisconnectFunction =
            &CommandScheduler::defaultSafeDisconnectFunction);
    DISALLOW_COPY_AND_ASSIGN(CommandScheduler)
    mockable ~CommandScheduler();

    mockable void run();

    mockable void addCommand(Command* commandToAdd);

    mockable void removeCommand(Command* command, bool interrupted);

    mockable bool isCommandScheduled(const Command* command) const;

    mockable void registerSubsystem(Subsystem* subsystem);

    mockable void setSafeDisconnectFunction(SafeDisconnectFunction* func);

    mockable bool isSubsystemRegistered(const Subsystem* subsystem) const;

    mockable void runAllHardwareTests();

    mockable void runHardwareTest(const Subsystem* subsystem);

    mockable void stopAllHardwareTests();

    mockable void stopHardwareTest(const Subsystem* subsystem);

    mockable int countRunningHardwareTests();

    mockable bool isRunningTest(const Subsystem* subsystem);

    mockable bool hasPassedTest(const Subsystem* subsystem);

    mockable int subsystemListSize() const;

    mockable int commandListSize() const;

    struct CommandIterator
    {
        using iterator_category = std::forward_iterator_tag;
        using difference_type = std::ptrdiff_t;
        using value_type = Command;
        using pointer = Command*;
        using reference = Command&;

        CommandIterator(CommandScheduler* scheduler, int i);

        pointer operator*();

        // Prefix increment
        CommandIterator& operator++();

        // Postfix increment
        CommandIterator operator++(int);

        friend bool operator==(const CommandIterator& a, const CommandIterator& b);
        friend bool operator!=(const CommandIterator& a, const CommandIterator& b);

    private:
        CommandScheduler* scheduler;
        int currIndex;
    };

    struct SubsystemIterator
    {
        using iterator_category = std::forward_iterator_tag;
        using difference_type = std::ptrdiff_t;
        using value_type = Subsystem;
        using pointer = Subsystem*;
        using reference = Subsystem&;

        SubsystemIterator(CommandScheduler* scheduler, int i);

        pointer operator*();

        // Prefix increment
        SubsystemIterator& operator++();

        // Postfix increment
        SubsystemIterator operator++(int);

        friend bool operator==(const SubsystemIterator& a, const SubsystemIterator& b);
        friend bool operator!=(const SubsystemIterator& a, const SubsystemIterator& b);

    private:
        CommandScheduler* scheduler;
        int currIndex;
    };

    mockable CommandIterator cmdMapBegin();
    mockable CommandIterator cmdMapEnd();
    mockable SubsystemIterator subMapBegin();
    mockable SubsystemIterator subMapEnd();

    mockable subsystem_scheduler_bitmap_t getRegisteredSubsystemBitmap() const
    {
        return registeredSubsystemBitmap;
    }
    mockable command_scheduler_bitmap_t getAddedCommandBitmap() const { return addedCommandBitmap; }

    static int constructCommand(Command* command);
    static int constructSubsystem(Subsystem* subsystem);
    static void destructCommand(Command* command);
    static void destructSubsystem(Subsystem* subsystem);

private:
    static constexpr float MAX_ALLOWABLE_SCHEDULER_RUNTIME = 100;
    static constexpr int MAX_SUBSYSTEM_COUNT = sizeof(subsystem_scheduler_bitmap_t) * 8;
    static constexpr int MAX_COMMAND_COUNT = sizeof(command_scheduler_bitmap_t) * 8;
    static constexpr subsystem_scheduler_bitmap_t LSB_ONE_HOT_SUBSYSTEM_BITMAP = 1;
    static constexpr command_scheduler_bitmap_t LSB_ONE_HOT_COMMAND_BITMAP = 1;
    static constexpr int INVALID_ITER_INDEX = -1;

    static int maxSubsystemIndex;

    static Subsystem* globalSubsystemRegistrar[MAX_SUBSYSTEM_COUNT];

    static int maxCommandIndex;

    static Command* globalCommandRegistrar[MAX_COMMAND_COUNT];

    static bool masterSchedulerExists;

    bool safeDisconnected();

    Drivers* drivers;

    static SafeDisconnectFunction defaultSafeDisconnectFunction;

    SafeDisconnectFunction* safeDisconnectFunction;

    subsystem_scheduler_bitmap_t registeredSubsystemBitmap = 0;

    subsystem_scheduler_bitmap_t subsystemsAssociatedWithCommandBitmap = 0;

    subsystem_scheduler_bitmap_t subsystemsPassingHardwareTests = 0;

    command_scheduler_bitmap_t addedCommandBitmap = 0;

    bool isMasterScheduler = false;
};  // class CommandScheduler

}  // namespace control

}  // namespace tap

#endif  // TAPROOT_COMMAND_SCHEDULER_HPP_