diff --git a/wpilibc/src/main/native/cpp/DSControlWord.cpp b/wpilibc/src/main/native/cpp/DSControlWord.cpp new file mode 100644 index 0000000000..15d209aca4 --- /dev/null +++ b/wpilibc/src/main/native/cpp/DSControlWord.cpp @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/DSControlWord.h" + +#include + +using namespace frc; + +DSControlWord::DSControlWord() { + HAL_GetControlWord(&m_controlWord); +} + +bool DSControlWord::IsEnabled() const { + return m_controlWord.enabled && m_controlWord.dsAttached; +} + +bool DSControlWord::IsDisabled() const { + return !(m_controlWord.enabled && m_controlWord.dsAttached); +} + +bool DSControlWord::IsEStopped() const { + return m_controlWord.eStop; +} + +bool DSControlWord::IsAutonomous() const { + return m_controlWord.autonomous; +} + +bool DSControlWord::IsAutonomousEnabled() const { + return m_controlWord.autonomous && m_controlWord.enabled && + m_controlWord.dsAttached; +} + +bool DSControlWord::IsTeleop() const { + return !(m_controlWord.autonomous || m_controlWord.test); +} + +bool DSControlWord::IsTeleopEnabled() const { + return !m_controlWord.autonomous && !m_controlWord.test && + m_controlWord.enabled && m_controlWord.dsAttached; +} + +bool DSControlWord::IsTest() const { + return m_controlWord.test; +} + +bool DSControlWord::IsDSAttached() const { + return m_controlWord.dsAttached; +} + +bool DSControlWord::IsFMSAttached() const { + return m_controlWord.fmsAttached; +} diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp index 55c4dfafc9..9474936c80 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp @@ -8,6 +8,7 @@ #include #include +#include "frc/DSControlWord.h" #include "frc/Errors.h" #include "frc/livewindow/LiveWindow.h" #include "frc/shuffleboard/Shuffleboard.h" @@ -102,14 +103,15 @@ void IterativeRobotBase::LoopFunc() { m_watchdog.Reset(); // Get current mode + DSControlWord word; Mode mode = Mode::kNone; - if (IsDisabled()) { + if (word.IsDisabled()) { mode = Mode::kDisabled; - } else if (IsAutonomous()) { + } else if (word.IsAutonomous()) { mode = Mode::kAutonomous; - } else if (IsOperatorControl()) { + } else if (word.IsTeleop()) { mode = Mode::kTeleop; - } else if (IsTest()) { + } else if (word.IsTest()) { mode = Mode::kTest; } diff --git a/wpilibc/src/main/native/include/frc/DSControlWord.h b/wpilibc/src/main/native/include/frc/DSControlWord.h new file mode 100644 index 0000000000..fb0709f342 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/DSControlWord.h @@ -0,0 +1,102 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace frc { + +/** + * A wrapper around Driver Station control word. + */ +class DSControlWord { + public: + /** + * DSControlWord constructor. + * + * Upon construction, the current Driver Station control word is read and + * stored internally. + */ + DSControlWord(); + + /** + * Check if the DS has enabled the robot. + * + * @return True if the robot is enabled and the DS is connected + */ + bool IsEnabled() const; + + /** + * Check if the robot is disabled. + * + * @return True if the robot is explicitly disabled or the DS is not connected + */ + bool IsDisabled() const; + + /** + * Check if the robot is e-stopped. + * + * @return True if the robot is e-stopped + */ + bool IsEStopped() const; + + /** + * Check if the DS is commanding autonomous mode. + * + * @return True if the robot is being commanded to be in autonomous mode + */ + bool IsAutonomous() const; + + /** + * Check if the DS is commanding autonomous mode and if it has enabled the + * robot. + * + * @return True if the robot is being commanded to be in autonomous mode and + * enabled. + */ + bool IsAutonomousEnabled() const; + + /** + * Check if the DS is commanding teleop mode. + * + * @return True if the robot is being commanded to be in teleop mode + */ + bool IsTeleop() const; + + /** + * Check if the DS is commanding teleop mode and if it has enabled the robot. + * + * @return True if the robot is being commanded to be in teleop mode and + * enabled. + */ + bool IsTeleopEnabled() const; + + /** + * Check if the DS is commanding test mode. + * + * @return True if the robot is being commanded to be in test mode + */ + bool IsTest() const; + + /** + * Check if the DS is attached. + * + * @return True if the DS is connected to the robot + */ + bool IsDSAttached() const; + + /** + * Is the driver station attached to a Field Management System? + * + * @return True if the robot is competing on a field being controlled by a + * Field Management System + */ + bool IsFMSAttached() const; + + private: + HAL_ControlWord m_controlWord; +}; + +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java new file mode 100644 index 0000000000..016abc7ca3 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java @@ -0,0 +1,127 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.hal.ControlWord; +import edu.wpi.first.hal.HAL; + +/** A wrapper around Driver Station control word. */ +public class DSControlWord { + private final ControlWord m_controlWord = new ControlWord(); + + /** + * DSControlWord constructor. + * + *

Upon construction, the current Driver Station control word is read and stored internally. + */ + public DSControlWord() { + update(); + } + + /** Update internal Driver Station control word. */ + public void update() { + HAL.getControlWord(m_controlWord); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be enabled. + * + * @return True if the robot is enabled, false otherwise. + */ + public boolean isEnabled() { + return m_controlWord.getEnabled() && m_controlWord.getDSAttached(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be disabled. + * + * @return True if the robot should be disabled, false otherwise. + */ + public boolean isDisabled() { + return !isEnabled(); + } + + /** + * Gets a value indicating whether the Robot is e-stopped. + * + * @return True if the robot is e-stopped, false otherwise. + */ + public boolean isEStopped() { + return m_controlWord.getEStop(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * autonomous mode. + * + * @return True if autonomous mode should be enabled, false otherwise. + */ + public boolean isAutonomous() { + return m_controlWord.getAutonomous(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * autonomous mode and enabled. + * + * @return True if autonomous should be set and the robot should be enabled. + */ + public boolean isAutonomousEnabled() { + return m_controlWord.getAutonomous() + && m_controlWord.getEnabled() + && m_controlWord.getDSAttached(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controlled mode. + * + * @return True if operator-controlled mode should be enabled, false otherwise. + */ + public boolean isTeleop() { + return !(isAutonomous() || isTest()); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controller mode and enabled. + * + * @return True if operator-controlled mode should be set and the robot should be enabled. + */ + public boolean isTeleopEnabled() { + return !m_controlWord.getAutonomous() + && !m_controlWord.getTest() + && m_controlWord.getEnabled() + && m_controlWord.getDSAttached(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in test + * mode. + * + * @return True if test mode should be enabled, false otherwise. + */ + public boolean isTest() { + return m_controlWord.getTest(); + } + + /** + * Gets a value indicating whether the Driver Station is attached. + * + * @return True if Driver Station is attached, false otherwise. + */ + public boolean isDSAttached() { + return m_controlWord.getDSAttached(); + } + + /** + * Gets if the driver station attached to a Field Management System. + * + * @return true if the robot is competing on a field being controlled by a Field Management System + */ + public boolean isFMSAttached() { + return m_controlWord.getFMSAttached(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java index c5a5957d03..a8c92a6b7c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -61,6 +61,7 @@ public abstract class IterativeRobotBase extends RobotBase { kTest } + private final DSControlWord m_word = new DSControlWord(); private Mode m_lastMode = Mode.kNone; private final double m_period; private final Watchdog m_watchdog; @@ -258,14 +259,15 @@ public abstract class IterativeRobotBase extends RobotBase { m_watchdog.reset(); // Get current mode + m_word.update(); Mode mode = Mode.kNone; - if (isDisabled()) { + if (m_word.isDisabled()) { mode = Mode.kDisabled; - } else if (isAutonomous()) { + } else if (m_word.isAutonomous()) { mode = Mode.kAutonomous; - } else if (isOperatorControl()) { + } else if (m_word.isTeleop()) { mode = Mode.kTeleop; - } else if (isTest()) { + } else if (m_word.isTest()) { mode = Mode.kTest; }