001/*
002 * Copyright (C) Cross The Road Electronics.  All rights reserved.
003 * License information can be found in CTRE_LICENSE.txt
004 * For support and suggestions contact support@ctr-electronics.com or file
005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
006 */
007package com.ctre.phoenix6.wpiutils;
008
009import java.util.concurrent.locks.Lock;
010import java.util.concurrent.locks.ReentrantLock;
011
012import com.ctre.phoenix6.SignalLogger;
013import com.ctre.phoenix6.unmanaged.Unmanaged;
014
015import edu.wpi.first.wpilibj.DriverStation;
016import edu.wpi.first.wpilibj.Notifier;
017
018public class AutoFeedEnable implements AutoCloseable {
019    private static final AutoFeedEnable autoFeedEnable = new AutoFeedEnable();
020
021    private final Notifier _enableNotifier;
022    private final Lock _lck = new ReentrantLock();
023    private int _startCount = 0;
024
025    public static AutoFeedEnable getInstance() {
026        return autoFeedEnable;
027    }
028
029    private AutoFeedEnable() {
030        _enableNotifier = new Notifier(() -> {
031            String robotMode;
032            if (DriverStation.isEnabled()) {
033                Unmanaged.feedEnable(100);
034
035                if (DriverStation.isAutonomous()) {
036                    robotMode = "Autonomous";
037                } else if (DriverStation.isTest()) {
038                    robotMode = "Test";
039                } else {
040                    robotMode = "Teleop";
041                }
042            } else {
043                robotMode = "Disabled";
044            }
045            SignalLogger.writeString("RobotMode", robotMode);
046        });
047    }
048
049    /**
050     * Starts feeding the enable signal to CTRE actuators.
051     */
052    public void start() {
053        _lck.lock();
054        try {
055            if (_startCount < Integer.MAX_VALUE) {
056                if (_startCount++ == 0) {
057                    /* start if we were previously at 0 */
058                    _enableNotifier.startPeriodic(0.02);
059                }
060            }
061        } finally {
062            _lck.unlock();
063        }
064    }
065
066    /**
067     * Stops feeding the enable signal to CTRE actuators. The
068     * enable signal will only be stopped when all actuators
069     * have requested to stop the enable signal.
070     */
071    public void stop() {
072        _lck.lock();
073        try {
074            if (_startCount > 0) {
075                if (--_startCount == 0) {
076                    /* stop if we are now at 0 */
077                    _enableNotifier.stop();
078                }
079            }
080        } finally {
081            _lck.unlock();
082        }
083    }
084
085    @Override
086    public void close() {
087        _enableNotifier.close();
088    }
089}