AutoFeedEnable.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/SignalLogger.hpp"
#include "ctre/phoenix6/unmanaged/Unmanaged.hpp"
#include <wpi/driverstation/MatchState.hpp>
#include <wpi/driverstation/RobotState.hpp>
#include <wpi/system/Notifier.hpp>
#include <mutex>
 
namespace ctre {
namespace phoenix6 {
namespace wpiutils {
 
class AutoFeedEnable final {
public:
    static AutoFeedEnable &GetInstance()
    {
        static AutoFeedEnable &autoFeedEnable = *new AutoFeedEnable{};
        return autoFeedEnable;
    }
 
private:
    std::mutex _lck;
    wpi::Notifier _enableNotifier;
    uint32_t _startCount = 0;
 
    AutoFeedEnable() :
        _enableNotifier{[] {
            std::string_view robotMode;
            if (wpi::RobotState::IsEnabled()) {
                ctre::phoenix::unmanaged::FeedEnable(100);
 
                switch (wpi::RobotState::GetRobotMode()) {
                    case wpi::RobotMode::UNKNOWN:
                    default:
                        robotMode = "Unknown";
                        break;
                    case wpi::RobotMode::AUTONOMOUS:
                        robotMode = "Autonomous";
                        break;
                    case wpi::RobotMode::TELEOPERATED:
                        robotMode = "Teleop";
                        break;
                    case wpi::RobotMode::UTILITY:
                        robotMode = "Utility";
                        break;
                }
            } else {
                ctre::phoenix::unmanaged::FeedEnable(0);
 
                if (wpi::RobotState::IsEStopped()) {
                    robotMode = "EStopped";
                } else {
                    robotMode = "Disabled";
                }
            }
 
            std::string_view allianceStation{};
            {
                auto const alliance = wpi::MatchState::GetAlliance();
                auto const location = wpi::MatchState::GetLocation();
                if (alliance && location) {
                    switch (*alliance) {
                        case wpi::Alliance::RED:
                            switch (*location) {
                                case 1:
                                    allianceStation = "Red 1";
                                    break;
                                case 2:
                                    allianceStation = "Red 2";
                                    break;
                                case 3:
                                    allianceStation = "Red 3";
                                    break;
                                default:
                                    break;
                            }
                            break;
                        case wpi::Alliance::BLUE:
                            switch (*location) {
                                case 1:
                                    allianceStation = "Blue 1";
                                    break;
                                case 2:
                                    allianceStation = "Blue 2";
                                    break;
                                case 3:
                                    allianceStation = "Blue 3";
                                    break;
                                default:
                                    break;
                            }
                            break;
                        default:
                            break;
                    }
                }
            }
 
            SignalLogger::WriteString("RobotMode", robotMode);
            SignalLogger::WriteString("AllianceStation", allianceStation);
            SignalLogger::WriteBoolean("DS:IsDSAttached", wpi::RobotState::IsDSAttached());
            SignalLogger::WriteBoolean("DS:IsFMSAttached", wpi::RobotState::IsFMSAttached());
        }}
    {}
 
public:
    /**
     * \brief Starts feeding the enable signal to CTRE actuators.
     */
    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 feeding the enable signal to CTRE actuators.
     * The enable signal will only be stopped when all actuators
     * have requested to stop the enable signal.
     */
    void Stop()
    {
        std::lock_guard<std::mutex> lock{_lck};
        if (_startCount > 0) {
            if (--_startCount == 0) {
                /* stop if we are now at 0 */
                _enableNotifier.Stop();
            }
        }
    }
};
 
}
}
}