ReplayAutoEnable.hpp

Go to the documentation of this file.

/*
 * Copyright (C) Cross The Road Electronics.  All rights reserved.
 * License information can be found in CTRE_LICENSE.txt
 * For support and suggestions contact support@ctr-electronics.com or file
 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
 */
#pragma once
 
#include "ctre/phoenix6/HootReplay.hpp"
#include <wpi/simulation/DriverStationSim.hpp>
#include <wpi/system/Notifier.hpp>
#include <mutex>
 
namespace ctre {
namespace phoenix6 {
namespace wpiutils {
 
class ReplayAutoEnable final {
public:
    static ReplayAutoEnable &GetInstance()
    {
        static ReplayAutoEnable &replayAutoEnable = *new ReplayAutoEnable{};
        return replayAutoEnable;
    }
 
private:
    std::mutex _lck;
    wpi::Notifier _enableNotifier;
    uint32_t _startCount = 0;
 
    ReplayAutoEnable() :
        _enableNotifier{[] {
            if (HootReplay::IsPlaying()) {
                auto dsAttachedSig = HootReplay::GetBoolean("DS:IsDSAttached");
                if (dsAttachedSig.status.IsOK()) {
                    wpi::sim::DriverStationSim::SetDsAttached(dsAttachedSig.value);
                }
 
                if (wpi::sim::DriverStationSim::GetDsAttached()) {
                    auto fmsAttachedSig = HootReplay::GetBoolean("DS:IsFMSAttached");
                    if (fmsAttachedSig.status.IsOK()) {
                        wpi::sim::DriverStationSim::SetFmsAttached(fmsAttachedSig.value);
                    }
 
                    auto enableSig = HootReplay::GetBoolean("RobotEnable");
                    if (enableSig.status.IsOK()) {
                        wpi::sim::DriverStationSim::SetEnabled(enableSig.value);
                    }
 
                    auto robotModeSig = HootReplay::GetString("RobotMode");
                    if (robotModeSig.status.IsOK()) {
                        if (robotModeSig.value == "Autonomous") {
                            wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_AUTONOMOUS);
                            wpi::sim::DriverStationSim::SetEStop(false);
                        } else if (robotModeSig.value == "Teleop") {
                            wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_TELEOPERATED);
                            wpi::sim::DriverStationSim::SetEStop(false);
                        } else if (robotModeSig.value == "Utility") {
                            wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_UTILITY);
                            wpi::sim::DriverStationSim::SetEStop(false);
                        } else if (robotModeSig.value == "EStopped") {
                            wpi::sim::DriverStationSim::SetEStop(true);
                        } else {
                            wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_UNKNOWN);
                            wpi::sim::DriverStationSim::SetEStop(false);
                        }
                    }
 
                    auto allianceStationSig = HootReplay::GetString("AllianceStation");
                    if (allianceStationSig.status.IsOK()) {
                        if (allianceStationSig.value == "Red 1") {
                            wpi::sim::DriverStationSim::SetAllianceStationId(HAL_ALLIANCE_STATION_RED_1);
                        } else if (allianceStationSig.value == "Red 2") {
                            wpi::sim::DriverStationSim::SetAllianceStationId(HAL_ALLIANCE_STATION_RED_2);
                        } else if (allianceStationSig.value == "Red 3") {
                            wpi::sim::DriverStationSim::SetAllianceStationId(HAL_ALLIANCE_STATION_RED_3);
                        } else if (allianceStationSig.value == "Blue 1") {
                            wpi::sim::DriverStationSim::SetAllianceStationId(HAL_ALLIANCE_STATION_BLUE_1);
                        } else if (allianceStationSig.value == "Blue 2") {
                            wpi::sim::DriverStationSim::SetAllianceStationId(HAL_ALLIANCE_STATION_BLUE_2);
                        } else if (allianceStationSig.value == "Blue 3") {
                            wpi::sim::DriverStationSim::SetAllianceStationId(HAL_ALLIANCE_STATION_BLUE_3);
                        } else {
                            wpi::sim::DriverStationSim::SetAllianceStationId(HAL_ALLIANCE_STATION_UNKNOWN);
                        }
                    }
 
                    wpi::sim::DriverStationSim::NotifyNewData();
                }
            }
        }}
    {}
 
public:
    /**
     * \brief Starts automatically enabling the robot in replay.
     */
    void Start()
    {
        std::lock_guard<std::mutex> lock{_lck};
        if (_startCount < UINT32_MAX) {
            if (_startCount++ == 0) {
                /* start if we were previously at 0 */
                _enableNotifier.StartPeriodic(10_ms);
            }
        }
    }
 
    /**
     * \brief Stops automatically enabling the robot in replay.
     * The replay enable will only be stopped when all actuators
     * have requested to stop the replay enable.
     */
    void Stop()
    {
        std::lock_guard<std::mutex> lock{_lck};
        if (_startCount > 0) {
            if (--_startCount == 0) {
                /* stop if we are now at 0 */
                _enableNotifier.Stop();
            }
        }
    }
};
 
}
}
}