mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[hal,wpilib] Rename "Test" robot mode to "Utility" (#8782)
The "Utility" name better matches its intended generic use case and avoids overloaded terminology with unit testing (e.g. the need to name the opmode annotation `@TestOpMode`). The driver station will also be updated to reflect this change.
This commit is contained in:
@@ -42,11 +42,11 @@ public final class RobotModeTriggers {
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a trigger that is true when the robot is enabled in test mode.
|
||||
* Returns a trigger that is true when the robot is enabled in utility mode.
|
||||
*
|
||||
* @return A trigger that is true when the robot is enabled in test mode.
|
||||
* @return A trigger that is true when the robot is enabled in utility mode.
|
||||
*/
|
||||
public static Trigger test() {
|
||||
return new Trigger(RobotState::isTestEnabled);
|
||||
public static Trigger utility() {
|
||||
return new Trigger(RobotState::isUtilityEnabled);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -20,6 +20,6 @@ Trigger RobotModeTriggers::Disabled() {
|
||||
return Trigger{&wpi::RobotState::IsDisabled};
|
||||
}
|
||||
|
||||
Trigger RobotModeTriggers::Test() {
|
||||
return Trigger{&wpi::RobotState::IsTestEnabled};
|
||||
Trigger RobotModeTriggers::Utility() {
|
||||
return Trigger{&wpi::RobotState::IsUtilityEnabled};
|
||||
}
|
||||
|
||||
@@ -40,11 +40,11 @@ class RobotModeTriggers {
|
||||
static Trigger Disabled();
|
||||
|
||||
/**
|
||||
* Returns a trigger that is true when the robot is enabled in test mode.
|
||||
* Returns a trigger that is true when the robot is enabled in utility mode.
|
||||
*
|
||||
* @return A trigger that is true when the robot is enabled in test mode.
|
||||
* @return A trigger that is true when the robot is enabled in utility mode.
|
||||
*/
|
||||
static Trigger Test();
|
||||
static Trigger Utility();
|
||||
};
|
||||
|
||||
} // namespace wpi::cmd
|
||||
|
||||
@@ -33,7 +33,7 @@ public final class MockHardwareExtension implements BeforeAllCallback {
|
||||
HAL.initialize(500, 0);
|
||||
DriverStationSim.setDsAttached(true);
|
||||
DriverStationSim.setEnabled(true);
|
||||
DriverStationSim.setRobotMode(RobotMode.TEST);
|
||||
DriverStationSim.setRobotMode(RobotMode.UTILITY);
|
||||
DriverStationSim.notifyNewData();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -33,12 +33,12 @@ class RobotModeTriggersTest extends CommandTestBase {
|
||||
}
|
||||
|
||||
@Test
|
||||
void testModeTest() {
|
||||
void utilityTest() {
|
||||
DriverStationSim.resetData();
|
||||
DriverStationSim.setRobotMode(RobotMode.TEST);
|
||||
DriverStationSim.setRobotMode(RobotMode.UTILITY);
|
||||
DriverStationSim.setEnabled(true);
|
||||
DriverStationSim.notifyNewData();
|
||||
Trigger test = RobotModeTriggers.test();
|
||||
Trigger test = RobotModeTriggers.utility();
|
||||
assertTrue(test.getAsBoolean());
|
||||
}
|
||||
|
||||
|
||||
@@ -40,11 +40,11 @@ TEST(RobotModeTriggersTest, Disabled) {
|
||||
EXPECT_TRUE(disabled.Get());
|
||||
}
|
||||
|
||||
TEST(RobotModeTriggersTest, TestMode) {
|
||||
TEST(RobotModeTriggersTest, UtilityMode) {
|
||||
DriverStationSim::ResetData();
|
||||
DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_TEST);
|
||||
DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_UTILITY);
|
||||
DriverStationSim::SetEnabled(true);
|
||||
DriverStationSim::NotifyNewData();
|
||||
Trigger test = RobotModeTriggers::Test();
|
||||
Trigger test = RobotModeTriggers::Utility();
|
||||
EXPECT_TRUE(test.Get());
|
||||
}
|
||||
|
||||
@@ -43,11 +43,11 @@ public final class RobotModeTriggers {
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a trigger that is true when the robot is enabled in test mode.
|
||||
* Returns a trigger that is true when the robot is enabled in utility mode.
|
||||
*
|
||||
* @return A trigger that is true when the robot is enabled in test mode.
|
||||
* @return A trigger that is true when the robot is enabled in utility mode.
|
||||
*/
|
||||
public static Trigger test() {
|
||||
return new Trigger(RobotState::isTestEnabled);
|
||||
public static Trigger utility() {
|
||||
return new Trigger(RobotState::isUtilityEnabled);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -29,9 +29,9 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
|
||||
/** This function is called periodically during all modes. */
|
||||
@Override
|
||||
|
||||
@@ -35,7 +35,7 @@ class Robot : public wpi::TimedRobot {
|
||||
/**
|
||||
* This function is called periodically during test mode
|
||||
*/
|
||||
void TestPeriodic() override {}
|
||||
void UtilityPeriodic() override {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during all modes
|
||||
|
||||
@@ -181,23 +181,23 @@ public class ControlWord {
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in test
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in utility
|
||||
* mode.
|
||||
*
|
||||
* @return True if test mode should be enabled, false otherwise.
|
||||
* @return True if utility mode should be enabled, false otherwise.
|
||||
*/
|
||||
public boolean isTest() {
|
||||
return getRobotMode() == RobotMode.TEST;
|
||||
public boolean isUtility() {
|
||||
return getRobotMode() == RobotMode.UTILITY;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in test
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in utility
|
||||
* mode and enabled.
|
||||
*
|
||||
* @return True if test mode should be set and the robot should be enabled.
|
||||
* @return True if utility mode should be set and the robot should be enabled.
|
||||
*/
|
||||
public boolean isTestEnabled() {
|
||||
return isTest() && isEnabled() && isDSAttached();
|
||||
public boolean isUtilityEnabled() {
|
||||
return isUtility() && isEnabled() && isDSAttached();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -12,8 +12,8 @@ public enum RobotMode {
|
||||
AUTONOMOUS(1),
|
||||
/** Teleoperated. */
|
||||
TELEOPERATED(2),
|
||||
/** Test. */
|
||||
TEST(3);
|
||||
/** Utility. */
|
||||
UTILITY(3);
|
||||
|
||||
private final int value;
|
||||
|
||||
@@ -35,7 +35,7 @@ public enum RobotMode {
|
||||
return switch (value) {
|
||||
case 1 -> AUTONOMOUS;
|
||||
case 2 -> TELEOPERATED;
|
||||
case 3 -> TEST;
|
||||
case 3 -> UTILITY;
|
||||
default -> UNKNOWN;
|
||||
};
|
||||
}
|
||||
|
||||
@@ -27,7 +27,7 @@ public class ControlWordStruct implements Struct<ControlWord> {
|
||||
@Override
|
||||
public String getSchema() {
|
||||
return "uint64 opModeHash:56;"
|
||||
+ "enum{unknown=0,autonomous=1,teleoperated=2,test=3} uint64 robotMode:2;"
|
||||
+ "enum{unknown=0,autonomous=1,teleoperated=2,utility=3} uint64 robotMode:2;"
|
||||
+ "bool enabled:1;bool eStop:1;bool fmsAttached:1;bool dsAttached:1;";
|
||||
}
|
||||
|
||||
|
||||
@@ -99,13 +99,13 @@ struct DashboardOpModeInstance {
|
||||
void Start(nt::NetworkTableInstance inst) {
|
||||
autoOpModes.Start(inst, "/SmartDashboard/Auto OpMode");
|
||||
teleopOpModes.Start(inst, "/SmartDashboard/Teleop OpMode");
|
||||
testOpModes.Start(inst, "/SmartDashboard/Test OpMode");
|
||||
utilityOpModes.Start(inst, "/SmartDashboard/Utility OpMode");
|
||||
}
|
||||
|
||||
util::mutex mutex;
|
||||
DashboardOpModeSender autoOpModes;
|
||||
DashboardOpModeSender teleopOpModes;
|
||||
DashboardOpModeSender testOpModes;
|
||||
DashboardOpModeSender utilityOpModes;
|
||||
};
|
||||
} // namespace
|
||||
|
||||
@@ -122,7 +122,7 @@ void hal::SetDashboardOpModeOptions(std::span<const HAL_OpModeOption> options) {
|
||||
std::scoped_lock lock{gInstance->mutex};
|
||||
gInstance->autoOpModes.SetOptions(options, HAL_ROBOT_MODE_AUTONOMOUS);
|
||||
gInstance->teleopOpModes.SetOptions(options, HAL_ROBOT_MODE_TELEOPERATED);
|
||||
gInstance->testOpModes.SetOptions(options, HAL_ROBOT_MODE_TEST);
|
||||
gInstance->utilityOpModes.SetOptions(options, HAL_ROBOT_MODE_UTILITY);
|
||||
}
|
||||
|
||||
void hal::StartDashboardOpMode() {
|
||||
@@ -141,7 +141,7 @@ void hal::EnableDashboardOpMode() {
|
||||
std::scoped_lock lock{gInstance->mutex};
|
||||
gInstance->autoOpModes.Enable();
|
||||
gInstance->teleopOpModes.Enable();
|
||||
gInstance->testOpModes.Enable();
|
||||
gInstance->utilityOpModes.Enable();
|
||||
}
|
||||
|
||||
int64_t hal::GetDashboardSelectedOpMode(HAL_RobotMode robotMode) {
|
||||
@@ -154,8 +154,8 @@ int64_t hal::GetDashboardSelectedOpMode(HAL_RobotMode robotMode) {
|
||||
return gInstance->autoOpModes.GetSelected();
|
||||
case HAL_ROBOT_MODE_TELEOPERATED:
|
||||
return gInstance->teleopOpModes.GetSelected();
|
||||
case HAL_ROBOT_MODE_TEST:
|
||||
return gInstance->testOpModes.GetSelected();
|
||||
case HAL_ROBOT_MODE_UTILITY:
|
||||
return gInstance->utilityOpModes.GetSelected();
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -56,7 +56,7 @@ HAL_ENUM(HAL_RobotMode) {
|
||||
HAL_ROBOT_MODE_UNKNOWN = 0,
|
||||
HAL_ROBOT_MODE_AUTONOMOUS,
|
||||
HAL_ROBOT_MODE_TELEOPERATED,
|
||||
HAL_ROBOT_MODE_TEST,
|
||||
HAL_ROBOT_MODE_UTILITY,
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@@ -19,8 +19,8 @@ enum class RobotMode {
|
||||
AUTONOMOUS = HAL_ROBOT_MODE_AUTONOMOUS,
|
||||
/// Teleoperated.
|
||||
TELEOPERATED = HAL_ROBOT_MODE_TELEOPERATED,
|
||||
/// Test.
|
||||
TEST = HAL_ROBOT_MODE_TEST
|
||||
/// Utility.
|
||||
UTILITY = HAL_ROBOT_MODE_UTILITY
|
||||
};
|
||||
|
||||
/**
|
||||
@@ -174,20 +174,20 @@ class ControlWord {
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the DS is commanding test mode.
|
||||
* Check if the DS is commanding utility mode.
|
||||
*
|
||||
* @return True if the robot is being commanded to be in test mode
|
||||
* @return True if the robot is being commanded to be in utility mode
|
||||
*/
|
||||
bool IsTest() const { return GetRobotMode() == RobotMode::TEST; }
|
||||
bool IsUtility() const { return GetRobotMode() == RobotMode::UTILITY; }
|
||||
|
||||
/**
|
||||
* Check if the DS is commanding test mode and if it has enabled the robot.
|
||||
* Check if the DS is commanding utility mode and if it has enabled the robot.
|
||||
*
|
||||
* @return True if the robot is being commanded to be in test mode and
|
||||
* @return True if the robot is being commanded to be in utility mode and
|
||||
* enabled.
|
||||
*/
|
||||
bool IsTestEnabled() const {
|
||||
return IsTest() && IsEnabled() && IsDSAttached();
|
||||
bool IsUtilityEnabled() const {
|
||||
return IsUtility() && IsEnabled() && IsDSAttached();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -217,7 +217,7 @@ struct wpi::util::Struct<wpi::hal::ControlWord> {
|
||||
static constexpr size_t GetSize() { return 8; }
|
||||
static constexpr std::string_view GetSchema() {
|
||||
return "uint64 opModeHash:56;"
|
||||
"enum{unknown=0,autonomous=1,teleoperated=2,test=3}"
|
||||
"enum{unknown=0,autonomous=1,teleoperated=2,utility=3}"
|
||||
"uint64 robotMode:2;"
|
||||
"bool enabled:1;bool eStop:1;bool fmsAttached:1;bool dsAttached:1;";
|
||||
}
|
||||
|
||||
@@ -29,6 +29,6 @@ classes:
|
||||
IsAutonomousEnabled:
|
||||
IsTeleop:
|
||||
IsTeleopEnabled:
|
||||
IsTest:
|
||||
IsTestEnabled:
|
||||
IsUtility:
|
||||
IsUtilityEnabled:
|
||||
GetValue:
|
||||
|
||||
@@ -69,6 +69,6 @@ class MyRobot(commands2.TimedCommandRobot):
|
||||
def teleopPeriodic(self) -> None:
|
||||
"""This function is called periodically during operator control"""
|
||||
|
||||
def testInit(self) -> None:
|
||||
# Cancels all running commands at the start of test mode
|
||||
def utilityInit(self) -> None:
|
||||
# Cancels all running commands at the start of utility mode
|
||||
commands2.CommandScheduler.getInstance().cancelAll()
|
||||
|
||||
@@ -49,8 +49,8 @@ class MyRobot(wpilib.TimedRobot):
|
||||
-self.controller.getLeftY(), -self.controller.getRightX()
|
||||
)
|
||||
|
||||
def testInit(self):
|
||||
"""This function is called once each time the robot enters test mode."""
|
||||
def utilityInit(self):
|
||||
"""This function is called once each time the robot enters utility mode."""
|
||||
|
||||
def testPeriodic(self):
|
||||
"""This function is called periodically during test mode."""
|
||||
def utilityPeriodic(self):
|
||||
"""This function is called periodically during utility mode."""
|
||||
|
||||
@@ -66,6 +66,6 @@ class MyRobot(commands2.TimedCommandRobot):
|
||||
def teleopPeriodic(self) -> None:
|
||||
"""This function is called periodically during operator control"""
|
||||
|
||||
def testInit(self) -> None:
|
||||
# Cancels all running commands at the start of test mode
|
||||
def utilityInit(self) -> None:
|
||||
# Cancels all running commands at the start of utility mode
|
||||
commands2.CommandScheduler.getInstance().cancelAll()
|
||||
|
||||
@@ -66,6 +66,6 @@ class MyRobot(commands2.TimedCommandRobot):
|
||||
def teleopPeriodic(self) -> None:
|
||||
"""This function is called periodically during operator control"""
|
||||
|
||||
def testInit(self) -> None:
|
||||
# Cancels all running commands at the start of test mode
|
||||
def utilityInit(self) -> None:
|
||||
# Cancels all running commands at the start of utility mode
|
||||
commands2.CommandScheduler.getInstance().cancelAll()
|
||||
|
||||
@@ -61,10 +61,10 @@ class MyRobot(commands2.TimedCommandRobot):
|
||||
"""This function is called periodically during operator control."""
|
||||
pass
|
||||
|
||||
def testInit(self) -> None:
|
||||
# Cancels all running commands at the start of test mode.
|
||||
def utilityInit(self) -> None:
|
||||
# Cancels all running commands at the start of utility mode.
|
||||
commands2.CommandScheduler.getInstance().cancelAll()
|
||||
|
||||
def testPeriodic(self) -> None:
|
||||
"""This function is called periodically during test mode."""
|
||||
def utilityPeriodic(self) -> None:
|
||||
"""This function is called periodically during utility mode."""
|
||||
pass
|
||||
|
||||
@@ -92,6 +92,6 @@ class MyRobot(commands2.TimedCommandRobot):
|
||||
def teleopPeriodic(self) -> None:
|
||||
"""This function is called periodically during operator control"""
|
||||
|
||||
def testInit(self) -> None:
|
||||
# Cancels all running commands at the start of test mode
|
||||
def utilityInit(self) -> None:
|
||||
# Cancels all running commands at the start of utility mode
|
||||
commands2.CommandScheduler.getInstance().cancelAll()
|
||||
|
||||
@@ -62,6 +62,6 @@ class MyRobot(commands2.TimedCommandRobot):
|
||||
"""This function is called periodically during operator control"""
|
||||
pass
|
||||
|
||||
def testInit(self) -> None:
|
||||
# Cancels all running commands at the start of test mode
|
||||
def utilityInit(self) -> None:
|
||||
# Cancels all running commands at the start of utility mode
|
||||
commands2.CommandScheduler.getInstance().cancelAll()
|
||||
|
||||
@@ -57,10 +57,10 @@ class MyRobot(TimedCommandRobot):
|
||||
"""This function is called periodically during operator control."""
|
||||
pass
|
||||
|
||||
def testInit(self) -> None:
|
||||
# Cancels all running commands at the start of test mode.
|
||||
def utilityInit(self) -> None:
|
||||
# Cancels all running commands at the start of utility mode.
|
||||
CommandScheduler.getInstance().cancelAll()
|
||||
|
||||
def testPeriodic(self) -> None:
|
||||
"""This function is called periodically during test mode."""
|
||||
def utilityPeriodic(self) -> None:
|
||||
"""This function is called periodically during utility mode."""
|
||||
pass
|
||||
|
||||
@@ -92,6 +92,6 @@ class MyRobot(commands2.TimedCommandRobot):
|
||||
def teleopPeriodic(self) -> None:
|
||||
"""This function is called periodically during operator control"""
|
||||
|
||||
def testInit(self) -> None:
|
||||
# Cancels all running commands at the start of test mode
|
||||
def utilityInit(self) -> None:
|
||||
# Cancels all running commands at the start of utility mode
|
||||
commands2.CommandScheduler.getInstance().cancelAll()
|
||||
|
||||
@@ -56,7 +56,7 @@ void DSCommPacket::SetControl(uint8_t control, uint8_t request) {
|
||||
if ((control & kAutonomous) != 0) {
|
||||
robotMode = HAL_ROBOT_MODE_AUTONOMOUS;
|
||||
} else if ((control & kTest) != 0) {
|
||||
robotMode = HAL_ROBOT_MODE_TEST;
|
||||
robotMode = HAL_ROBOT_MODE_UTILITY;
|
||||
} else {
|
||||
robotMode = HAL_ROBOT_MODE_TELEOPERATED;
|
||||
}
|
||||
|
||||
@@ -309,7 +309,7 @@ struct OpModes {
|
||||
static wpi::util::mutex gOpModeOptionsMutex;
|
||||
static OpModes gAutoOpModes;
|
||||
static OpModes gTeleopOpModes;
|
||||
static OpModes gTestOpModes;
|
||||
static OpModes gUtilityOpModes;
|
||||
|
||||
static void UpdateOpModes(const char* name, void* param,
|
||||
const HAL_OpModeOption* opmodes, int32_t count) {
|
||||
@@ -318,8 +318,8 @@ static void UpdateOpModes(const char* name, void* param,
|
||||
gAutoOpModes.groups.clear();
|
||||
gTeleopOpModes.ids.clear();
|
||||
gTeleopOpModes.groups.clear();
|
||||
gTestOpModes.ids.clear();
|
||||
gTestOpModes.groups.clear();
|
||||
gUtilityOpModes.ids.clear();
|
||||
gUtilityOpModes.groups.clear();
|
||||
for (auto&& o : std::span{opmodes, opmodes + count}) {
|
||||
OpModes* vec;
|
||||
switch (HAL_OpMode_GetRobotMode(o.id)) {
|
||||
@@ -329,8 +329,8 @@ static void UpdateOpModes(const char* name, void* param,
|
||||
case HAL_ROBOT_MODE_TELEOPERATED:
|
||||
vec = &gTeleopOpModes;
|
||||
break;
|
||||
case HAL_ROBOT_MODE_TEST:
|
||||
vec = &gTestOpModes;
|
||||
case HAL_ROBOT_MODE_UTILITY:
|
||||
vec = &gUtilityOpModes;
|
||||
break;
|
||||
default:
|
||||
continue;
|
||||
@@ -341,7 +341,7 @@ static void UpdateOpModes(const char* name, void* param,
|
||||
std::string{wpi::util::to_string_view(&o.description)},
|
||||
o.textColor, o.backgroundColor});
|
||||
}
|
||||
for (auto&& vec : {&gAutoOpModes, &gTeleopOpModes, &gTestOpModes}) {
|
||||
for (auto&& vec : {&gAutoOpModes, &gTeleopOpModes, &gUtilityOpModes}) {
|
||||
for (auto&& [group, options] : vec->groups) {
|
||||
std::sort(options.begin(), options.end(),
|
||||
[](const OpModeOption& a, const OpModeOption& b) {
|
||||
@@ -1181,9 +1181,9 @@ static void DriverStationExecute() {
|
||||
isAttached && robotMode == HAL_ROBOT_MODE_TELEOPERATED)) {
|
||||
DriverStationSetRobotMode(HAL_ROBOT_MODE_TELEOPERATED);
|
||||
}
|
||||
if (ImGui::Selectable("Test",
|
||||
isAttached && robotMode == HAL_ROBOT_MODE_TEST)) {
|
||||
DriverStationSetRobotMode(HAL_ROBOT_MODE_TEST);
|
||||
if (ImGui::Selectable("Utility",
|
||||
isAttached && robotMode == HAL_ROBOT_MODE_UTILITY)) {
|
||||
DriverStationSetRobotMode(HAL_ROBOT_MODE_UTILITY);
|
||||
}
|
||||
// OpMode
|
||||
bool canEnable = isAttached && started;
|
||||
@@ -1200,8 +1200,8 @@ static void DriverStationExecute() {
|
||||
case HAL_ROBOT_MODE_TELEOPERATED:
|
||||
modes = &gTeleopOpModes;
|
||||
break;
|
||||
case HAL_ROBOT_MODE_TEST:
|
||||
modes = &gTestOpModes;
|
||||
case HAL_ROBOT_MODE_UTILITY:
|
||||
modes = &gUtilityOpModes;
|
||||
break;
|
||||
default:
|
||||
modes = nullptr;
|
||||
|
||||
@@ -29,7 +29,7 @@ void IterativeRobotBase::AutonomousInit() {}
|
||||
|
||||
void IterativeRobotBase::TeleopInit() {}
|
||||
|
||||
void IterativeRobotBase::TestInit() {}
|
||||
void IterativeRobotBase::UtilityInit() {}
|
||||
|
||||
void IterativeRobotBase::RobotPeriodic() {
|
||||
static bool firstRun = true;
|
||||
@@ -71,7 +71,7 @@ void IterativeRobotBase::TeleopPeriodic() {
|
||||
}
|
||||
}
|
||||
|
||||
void IterativeRobotBase::TestPeriodic() {
|
||||
void IterativeRobotBase::UtilityPeriodic() {
|
||||
static bool firstRun = true;
|
||||
if (firstRun) {
|
||||
wpi::util::print("Default {}() method... Override me!\n", __FUNCTION__);
|
||||
@@ -85,7 +85,7 @@ void IterativeRobotBase::AutonomousExit() {}
|
||||
|
||||
void IterativeRobotBase::TeleopExit() {}
|
||||
|
||||
void IterativeRobotBase::TestExit() {}
|
||||
void IterativeRobotBase::UtilityExit() {}
|
||||
|
||||
wpi::units::second_t IterativeRobotBase::GetPeriod() const {
|
||||
return m_period;
|
||||
@@ -114,8 +114,8 @@ void IterativeRobotBase::LoopFunc() {
|
||||
AutonomousExit();
|
||||
} else if (m_lastMode == static_cast<int>(RobotMode::TELEOPERATED)) {
|
||||
TeleopExit();
|
||||
} else if (m_lastMode == static_cast<int>(RobotMode::TEST)) {
|
||||
TestExit();
|
||||
} else if (m_lastMode == static_cast<int>(RobotMode::UTILITY)) {
|
||||
UtilityExit();
|
||||
}
|
||||
|
||||
// Call current mode's entry function
|
||||
@@ -128,9 +128,9 @@ void IterativeRobotBase::LoopFunc() {
|
||||
} else if (mode == RobotMode::TELEOPERATED) {
|
||||
TeleopInit();
|
||||
m_watchdog.AddEpoch("TeleopInit()");
|
||||
} else if (mode == RobotMode::TEST) {
|
||||
TestInit();
|
||||
m_watchdog.AddEpoch("TestInit()");
|
||||
} else if (mode == RobotMode::UTILITY) {
|
||||
UtilityInit();
|
||||
m_watchdog.AddEpoch("UtilityInit()");
|
||||
}
|
||||
|
||||
m_lastMode = static_cast<int>(mode);
|
||||
@@ -147,9 +147,9 @@ void IterativeRobotBase::LoopFunc() {
|
||||
} else if (mode == RobotMode::TELEOPERATED) {
|
||||
TeleopPeriodic();
|
||||
m_watchdog.AddEpoch("TeleopPeriodic()");
|
||||
} else if (mode == RobotMode::TEST) {
|
||||
TestPeriodic();
|
||||
m_watchdog.AddEpoch("TestPeriodic()");
|
||||
} else if (mode == RobotMode::UTILITY) {
|
||||
UtilityPeriodic();
|
||||
m_watchdog.AddEpoch("UtilityPeriodic()");
|
||||
}
|
||||
|
||||
RobotPeriodic();
|
||||
|
||||
@@ -153,7 +153,7 @@ void MotorSafety::Check() {
|
||||
stopTime = m_stopTime;
|
||||
}
|
||||
|
||||
if (!enabled || RobotState::IsDisabled() || RobotState::IsTest()) {
|
||||
if (!enabled || RobotState::IsDisabled() || RobotState::IsUtility()) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@@ -158,12 +158,12 @@ bool RobotBase::IsTeleopEnabled() {
|
||||
return RobotState::IsTeleopEnabled();
|
||||
}
|
||||
|
||||
bool RobotBase::IsTest() {
|
||||
return RobotState::IsTest();
|
||||
bool RobotBase::IsUtility() {
|
||||
return RobotState::IsUtility();
|
||||
}
|
||||
|
||||
bool RobotBase::IsTestEnabled() {
|
||||
return RobotState::IsTestEnabled();
|
||||
bool RobotBase::IsUtilityEnabled() {
|
||||
return RobotState::IsUtilityEnabled();
|
||||
}
|
||||
|
||||
int64_t RobotBase::GetOpModeId() {
|
||||
|
||||
@@ -99,20 +99,22 @@ class RobotState final {
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the DS is commanding test mode.
|
||||
* Check if the DS is commanding utility mode.
|
||||
*
|
||||
* @return True if the robot is being commanded to be in test mode
|
||||
* @return True if the robot is being commanded to be in utility mode
|
||||
*/
|
||||
static bool IsTest() { return wpi::internal::DriverStationBackend::IsTest(); }
|
||||
static bool IsUtility() {
|
||||
return wpi::internal::DriverStationBackend::IsUtility();
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the DS is commanding Test mode and if it has enabled the robot.
|
||||
* Check if the DS is commanding Utility mode and if it has enabled the robot.
|
||||
*
|
||||
* @return True if the robot is being commanded to be in Test mode and
|
||||
* @return True if the robot is being commanded to be in Utility mode and
|
||||
* enabled.
|
||||
*/
|
||||
static bool IsTestEnabled() {
|
||||
return wpi::internal::DriverStationBackend::IsTestEnabled();
|
||||
static bool IsUtilityEnabled() {
|
||||
return wpi::internal::DriverStationBackend::IsUtilityEnabled();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -303,19 +303,19 @@ class DriverStationBackend final {
|
||||
static bool IsTeleopEnabled() { return GetControlWord().IsTeleopEnabled(); }
|
||||
|
||||
/**
|
||||
* Check if the DS is commanding test mode.
|
||||
* Check if the DS is commanding utility mode.
|
||||
*
|
||||
* @return True if the robot is being commanded to be in test mode
|
||||
* @return True if the robot is being commanded to be in utility mode
|
||||
*/
|
||||
static bool IsTest() { return GetControlWord().IsTest(); }
|
||||
static bool IsUtility() { return GetControlWord().IsUtility(); }
|
||||
|
||||
/**
|
||||
* Check if the DS is commanding Test mode and if it has enabled the robot.
|
||||
* Check if the DS is commanding Utility mode and if it has enabled the robot.
|
||||
*
|
||||
* @return True if the robot is being commanded to be in Test mode and
|
||||
* @return True if the robot is being commanded to be in Utility mode and
|
||||
* enabled.
|
||||
*/
|
||||
static bool IsTestEnabled() { return GetControlWord().IsTestEnabled(); }
|
||||
static bool IsUtilityEnabled() { return GetControlWord().IsUtilityEnabled(); }
|
||||
|
||||
/**
|
||||
* Adds an operating mode option. It's necessary to call PublishOpModes() to
|
||||
|
||||
@@ -32,8 +32,8 @@ namespace wpi {
|
||||
* another mode
|
||||
* \li TeleopInit() -- called each and every time teleop is entered from another
|
||||
* mode
|
||||
* \li TestInit() -- called each and every time test is entered from another
|
||||
* mode
|
||||
* \li UtilityInit() -- called each and every time utility is entered from
|
||||
* another mode
|
||||
*
|
||||
* Periodic() functions -- each of these functions is called on an interval:
|
||||
*
|
||||
@@ -41,7 +41,7 @@ namespace wpi {
|
||||
* \li DisabledPeriodic()
|
||||
* \li AutonomousPeriodic()
|
||||
* \li TeleopPeriodic()
|
||||
* \li TestPeriodic()
|
||||
* \li UtilityPeriodic()
|
||||
*
|
||||
* Exit() functions -- each of the following functions is called once when the
|
||||
* appropriate mode is exited:
|
||||
@@ -49,7 +49,7 @@ namespace wpi {
|
||||
* \li DisabledExit() -- called each and every time disabled is exited
|
||||
* \li AutonomousExit() -- called each and every time autonomous is exited
|
||||
* \li TeleopExit() -- called each and every time teleop is exited
|
||||
* \li TestExit() -- called each and every time test is exited
|
||||
* \li UtilityExit() -- called each and every time utility is exited
|
||||
*/
|
||||
class IterativeRobotBase : public RobotBase {
|
||||
public:
|
||||
@@ -97,12 +97,12 @@ class IterativeRobotBase : public RobotBase {
|
||||
virtual void TeleopInit();
|
||||
|
||||
/**
|
||||
* Initialization code for test mode should go here.
|
||||
* Initialization code for utility mode should go here.
|
||||
*
|
||||
* Users should override this method for initialization code which will be
|
||||
* called each time the robot enters test mode.
|
||||
* called each time the robot enters utility mode.
|
||||
*/
|
||||
virtual void TestInit();
|
||||
virtual void UtilityInit();
|
||||
|
||||
/**
|
||||
* Periodic code for all modes should go here.
|
||||
@@ -147,13 +147,13 @@ class IterativeRobotBase : public RobotBase {
|
||||
virtual void TeleopPeriodic();
|
||||
|
||||
/**
|
||||
* Periodic code for test mode should go here.
|
||||
* Periodic code for utility mode should go here.
|
||||
*
|
||||
* Users should override this method for code which will be called each time a
|
||||
* new packet is received from the driver station and the robot is in test
|
||||
* new packet is received from the driver station and the robot is in utility
|
||||
* mode.
|
||||
*/
|
||||
virtual void TestPeriodic();
|
||||
virtual void UtilityPeriodic();
|
||||
|
||||
/**
|
||||
* Exit code for disabled mode should go here.
|
||||
@@ -180,12 +180,12 @@ class IterativeRobotBase : public RobotBase {
|
||||
virtual void TeleopExit();
|
||||
|
||||
/**
|
||||
* Exit code for test mode should go here.
|
||||
* Exit code for utility mode should go here.
|
||||
*
|
||||
* Users should override this method for code which will be called each time
|
||||
* the robot exits test mode.
|
||||
* the robot exits utility mode.
|
||||
*/
|
||||
virtual void TestExit();
|
||||
virtual void UtilityExit();
|
||||
|
||||
/**
|
||||
* Gets time period between calls to Periodic() functions.
|
||||
|
||||
@@ -13,14 +13,11 @@
|
||||
#include "wpi/driverstation/Alert.hpp"
|
||||
#include "wpi/framework/RobotBase.hpp"
|
||||
#include "wpi/hal/DriverStationTypes.hpp"
|
||||
#include "wpi/hal/Notifier.h"
|
||||
#include "wpi/internal/PeriodicPriorityQueue.hpp"
|
||||
#include "wpi/opmode/OpMode.hpp"
|
||||
#include "wpi/system/Watchdog.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
#include "wpi/util/DenseMap.hpp"
|
||||
#include "wpi/util/SafeThread.hpp"
|
||||
#include "wpi/util/Synchronization.hpp"
|
||||
#include "wpi/util/mutex.hpp"
|
||||
|
||||
namespace wpi::util {
|
||||
|
||||
@@ -191,20 +191,20 @@ class RobotBase {
|
||||
static bool IsTeleopEnabled();
|
||||
|
||||
/**
|
||||
* Determine if the robot is currently in Test mode.
|
||||
* Determine if the robot is currently in Utility mode.
|
||||
*
|
||||
* @return True if the robot is currently running in Test mode as determined
|
||||
* by the Driver Station.
|
||||
* @return True if the robot is currently running in Utility mode as
|
||||
* determined by the Driver Station.
|
||||
*/
|
||||
static bool IsTest();
|
||||
static bool IsUtility();
|
||||
|
||||
/**
|
||||
* Determine if the robot is current in Test mode and enabled.
|
||||
* Determine if the robot is currently in Utility mode and enabled.
|
||||
*
|
||||
* @return True if the robot is currently operating in Test mode while
|
||||
* @return True if the robot is currently operating in Utility mode while
|
||||
* enabled as determined by the Driver Station.
|
||||
*/
|
||||
static bool IsTestEnabled();
|
||||
static bool IsUtilityEnabled();
|
||||
|
||||
/**
|
||||
* Gets the currently selected operating mode of the driver station. Note this
|
||||
|
||||
@@ -36,8 +36,8 @@ classes:
|
||||
IsAutonomousEnabled:
|
||||
IsTeleop:
|
||||
IsTeleopEnabled:
|
||||
IsTest:
|
||||
IsTestEnabled:
|
||||
IsUtility:
|
||||
IsUtilityEnabled:
|
||||
AddOpMode:
|
||||
overloads:
|
||||
RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&:
|
||||
|
||||
@@ -5,12 +5,12 @@ classes:
|
||||
DisabledInit:
|
||||
AutonomousInit:
|
||||
TeleopInit:
|
||||
TestInit:
|
||||
UtilityInit:
|
||||
RobotPeriodic:
|
||||
DisabledPeriodic:
|
||||
AutonomousPeriodic:
|
||||
TeleopPeriodic:
|
||||
TestPeriodic:
|
||||
UtilityPeriodic:
|
||||
IterativeRobotBase:
|
||||
overloads:
|
||||
double:
|
||||
@@ -22,7 +22,7 @@ classes:
|
||||
DisabledExit:
|
||||
AutonomousExit:
|
||||
TeleopExit:
|
||||
TestExit:
|
||||
UtilityExit:
|
||||
GetPeriod:
|
||||
PrintWatchdogEpochs:
|
||||
doc: |
|
||||
@@ -44,7 +44,7 @@ classes:
|
||||
- DisabledInit() -- called each and every time disabled is entered from another mode
|
||||
- AutonomousInit() -- called each and every time autonomous is entered from another mode
|
||||
- TeleopInit() -- called each and every time teleop is entered from another mode
|
||||
- TestInit() -- called each and every time test is entered from another mode
|
||||
- UtilityInit() -- called each and every time utility is entered from another mode
|
||||
|
||||
Periodic() functions -- each of these functions is called on an interval:
|
||||
|
||||
@@ -52,7 +52,7 @@ classes:
|
||||
- DisabledPeriodic()
|
||||
- AutonomousPeriodic()
|
||||
- TeleopPeriodic()
|
||||
- TestPeriodic()
|
||||
- UtilityPeriodic()
|
||||
|
||||
Exit() functions -- each of the following functions is called once when the
|
||||
appropriate mode is exited:
|
||||
@@ -60,4 +60,4 @@ classes:
|
||||
- DisabledExit() -- called each and every time disabled is exited
|
||||
- AutonomousExit() -- called each and every time autonomous is exited
|
||||
- TeleopExit() -- called each and every time teleop is exited
|
||||
- TestExit() -- called each and every time test is exited
|
||||
- UtilityExit() -- called each and every time utility is exited
|
||||
|
||||
@@ -25,8 +25,8 @@ classes:
|
||||
IsAutonomousEnabled:
|
||||
IsTeleop:
|
||||
IsTeleopEnabled:
|
||||
IsTest:
|
||||
IsTestEnabled:
|
||||
IsUtility:
|
||||
IsUtilityEnabled:
|
||||
GetThreadId:
|
||||
ignore: true
|
||||
StartCompetition:
|
||||
|
||||
@@ -12,8 +12,8 @@ classes:
|
||||
IsAutonomousEnabled:
|
||||
IsTeleop:
|
||||
IsTeleopEnabled:
|
||||
IsTest:
|
||||
IsTestEnabled:
|
||||
IsUtility:
|
||||
IsUtilityEnabled:
|
||||
AddOpMode:
|
||||
overloads:
|
||||
RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&:
|
||||
|
||||
@@ -78,7 +78,7 @@ TEST_F(OpModeRobotTest, AddOpMode) {
|
||||
AddOpMode<MockOpMode>(wpi::RobotMode::AUTONOMOUS, "NoArgOpMode-Auto",
|
||||
"Group", "Description", wpi::util::Color::WHITE,
|
||||
wpi::util::Color::BLACK);
|
||||
AddOpMode<OneArgOpMode>(wpi::RobotMode::TEST, "OneArgOpMode-Test",
|
||||
AddOpMode<OneArgOpMode>(wpi::RobotMode::UTILITY, "OneArgOpMode-Test",
|
||||
"Group", "Description", wpi::util::Color::WHITE,
|
||||
wpi::util::Color::BLACK);
|
||||
AddOpMode<MockOpMode>(wpi::RobotMode::TELEOPERATED, "NoArgOpMode");
|
||||
|
||||
@@ -60,7 +60,7 @@ class MockRobot : public TimedRobot {
|
||||
|
||||
void TeleopInit() override { m_teleopInitCount++; }
|
||||
|
||||
void TestInit() override { m_testInitCount++; }
|
||||
void UtilityInit() override { m_testInitCount++; }
|
||||
|
||||
void RobotPeriodic() override { m_robotPeriodicCount++; }
|
||||
|
||||
@@ -72,7 +72,7 @@ class MockRobot : public TimedRobot {
|
||||
|
||||
void TeleopPeriodic() override { m_teleopPeriodicCount++; }
|
||||
|
||||
void TestPeriodic() override { m_testPeriodicCount++; }
|
||||
void UtilityPeriodic() override { m_testPeriodicCount++; }
|
||||
|
||||
void DisabledExit() override { m_disabledExitCount++; }
|
||||
|
||||
@@ -80,7 +80,7 @@ class MockRobot : public TimedRobot {
|
||||
|
||||
void TeleopExit() override { m_teleopExitCount++; }
|
||||
|
||||
void TestExit() override { m_testExitCount++; }
|
||||
void UtilityExit() override { m_testExitCount++; }
|
||||
};
|
||||
} // namespace
|
||||
|
||||
@@ -305,7 +305,7 @@ TEST_F(TimedRobotTest, TestMode) {
|
||||
wpi::sim::WaitForProgramStart();
|
||||
|
||||
wpi::sim::DriverStationSim::SetEnabled(true);
|
||||
wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_TEST);
|
||||
wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_UTILITY);
|
||||
wpi::sim::DriverStationSim::NotifyNewData();
|
||||
|
||||
EXPECT_EQ(1u, robot.m_simulationInitCount);
|
||||
@@ -460,7 +460,7 @@ TEST_F(TimedRobotTest, ModeChange) {
|
||||
|
||||
// Transition to test
|
||||
wpi::sim::DriverStationSim::SetEnabled(true);
|
||||
wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_TEST);
|
||||
wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_UTILITY);
|
||||
wpi::sim::DriverStationSim::NotifyNewData();
|
||||
|
||||
wpi::sim::StepTiming(kPeriod);
|
||||
|
||||
@@ -54,17 +54,17 @@ TEST(DriverStationTest, Mode) {
|
||||
HAL_Initialize(500, 0);
|
||||
DriverStationSim::ResetData();
|
||||
|
||||
EXPECT_FALSE(RobotState::IsTest());
|
||||
EXPECT_FALSE(RobotState::IsUtility());
|
||||
EnumCallback callback;
|
||||
auto cb = DriverStationSim::RegisterRobotModeCallback(callback.GetCallback(),
|
||||
false);
|
||||
DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_TEST);
|
||||
DriverStationSim::SetRobotMode(HAL_ROBOT_MODE_UTILITY);
|
||||
DriverStationSim::NotifyNewData();
|
||||
EXPECT_EQ(DriverStationSim::GetRobotMode(), HAL_ROBOT_MODE_TEST);
|
||||
EXPECT_TRUE(RobotState::IsTest());
|
||||
EXPECT_EQ(RobotState::GetRobotMode(), RobotMode::TEST);
|
||||
EXPECT_EQ(DriverStationSim::GetRobotMode(), HAL_ROBOT_MODE_UTILITY);
|
||||
EXPECT_TRUE(RobotState::IsUtility());
|
||||
EXPECT_EQ(RobotState::GetRobotMode(), RobotMode::UTILITY);
|
||||
EXPECT_TRUE(callback.WasTriggered());
|
||||
EXPECT_EQ(callback.GetLastValue(), HAL_ROBOT_MODE_TEST);
|
||||
EXPECT_EQ(callback.GetLastValue(), HAL_ROBOT_MODE_UTILITY);
|
||||
}
|
||||
|
||||
TEST(DriverStationTest, Estop) {
|
||||
|
||||
@@ -73,8 +73,8 @@ def test_add_op_mode():
|
||||
)
|
||||
self.addOpMode(
|
||||
OneArgOpMode,
|
||||
RobotMode.TEST,
|
||||
"OneArgOpMode-Test",
|
||||
RobotMode.UTILITY,
|
||||
"OneArgOpMode-Utility",
|
||||
"Group",
|
||||
"Description",
|
||||
Color.WHITE,
|
||||
|
||||
@@ -12,7 +12,7 @@ Robot::Robot() {}
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
* autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
@@ -62,9 +62,9 @@ void Robot::TeleopInit() {
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
* This function is called periodically during utility mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -20,7 +20,7 @@ class Robot : public wpi::TimedRobot {
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
|
||||
@@ -43,9 +43,9 @@ class Robot : public wpi::TimedRobot {
|
||||
m_controller.GetRightX());
|
||||
}
|
||||
|
||||
void TestInit() override {}
|
||||
void UtilityInit() override {}
|
||||
|
||||
void TestPeriodic() override {}
|
||||
void UtilityPeriodic() override {}
|
||||
|
||||
private:
|
||||
// Robot drive system
|
||||
|
||||
@@ -56,8 +56,8 @@ int main(void) {
|
||||
{"", 0},
|
||||
-1,
|
||||
-1},
|
||||
{HAL_MAKE_OPMODEID(HAL_ROBOT_MODE_TEST, 0),
|
||||
{"Test", 4},
|
||||
{HAL_MAKE_OPMODEID(HAL_ROBOT_MODE_UTILITY, 0),
|
||||
{"Utility", 4},
|
||||
{"", 0},
|
||||
{"", 0},
|
||||
-1,
|
||||
@@ -116,7 +116,7 @@ int main(void) {
|
||||
break;
|
||||
case HAL_ROBOT_MODE_AUTONOMOUS:
|
||||
break;
|
||||
case HAL_ROBOT_MODE_TEST:
|
||||
case HAL_ROBOT_MODE_UTILITY:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
||||
@@ -21,7 +21,7 @@ Robot::Robot() {
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
* autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
@@ -70,9 +70,9 @@ void Robot::TeleopInit() {
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
* This function is called periodically during utility mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -18,7 +18,7 @@ class Robot : public wpi::TimedRobot {
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
|
||||
@@ -21,7 +21,7 @@ Robot::Robot() {
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
* autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
@@ -70,9 +70,9 @@ void Robot::TeleopInit() {
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
* This function is called periodically during utility mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -18,7 +18,7 @@ class Robot : public wpi::TimedRobot {
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
|
||||
@@ -45,12 +45,12 @@ void Robot::TeleopInit() {
|
||||
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
void Robot::TestInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
void Robot::UtilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
wpi::cmd::CommandScheduler::GetInstance().CancelAll();
|
||||
}
|
||||
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -20,8 +20,8 @@ class Robot : public wpi::TimedRobot {
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestInit() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityInit() override;
|
||||
void UtilityPeriodic() override;
|
||||
|
||||
private:
|
||||
RapidReactCommandBot m_robot;
|
||||
|
||||
@@ -11,7 +11,7 @@ Robot::Robot() {}
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
* autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
@@ -60,9 +60,9 @@ void Robot::TeleopInit() {
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
* This function is called periodically during utility mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -18,7 +18,7 @@ class Robot : public wpi::TimedRobot {
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
|
||||
@@ -41,13 +41,13 @@ void Robot::TeleopPeriodic() {}
|
||||
|
||||
void Robot::TeleopExit() {}
|
||||
|
||||
void Robot::TestInit() {
|
||||
void Robot::UtilityInit() {
|
||||
wpi::cmd::CommandScheduler::GetInstance().CancelAll();
|
||||
}
|
||||
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
void Robot::TestExit() {}
|
||||
void Robot::UtilityExit() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -23,9 +23,9 @@ class Robot : public wpi::TimedRobot {
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TeleopExit() override;
|
||||
void TestInit() override;
|
||||
void TestPeriodic() override;
|
||||
void TestExit() override;
|
||||
void UtilityInit() override;
|
||||
void UtilityPeriodic() override;
|
||||
void UtilityExit() override;
|
||||
|
||||
private:
|
||||
std::optional<wpi::cmd::CommandPtr> m_autonomousCommand;
|
||||
|
||||
@@ -11,7 +11,7 @@ Robot::Robot() {}
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
* autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
@@ -60,9 +60,9 @@ void Robot::TeleopInit() {
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
* This function is called periodically during utility mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -18,7 +18,7 @@ class Robot : public wpi::TimedRobot {
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
|
||||
@@ -17,7 +17,7 @@ Robot::Robot() {
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want ran during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
* autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
|
||||
@@ -61,9 +61,9 @@ void Robot::TeleopInit() {
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
* This function is called periodically during utility mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -18,7 +18,7 @@ class Robot : public wpi::TimedRobot {
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityPeriodic() override;
|
||||
|
||||
private:
|
||||
// Have it null by default so that if testing teleop it
|
||||
|
||||
@@ -11,7 +11,7 @@ Robot::Robot() {}
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want to run during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
* autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
@@ -60,9 +60,9 @@ void Robot::TeleopInit() {
|
||||
void Robot::TeleopPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called periodically during test mode.
|
||||
* This function is called periodically during utility mode.
|
||||
*/
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
/**
|
||||
* This function is called once when the robot is first started up.
|
||||
|
||||
@@ -20,7 +20,7 @@ class Robot : public wpi::TimedRobot {
|
||||
void AutonomousPeriodic() override;
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityPeriodic() override;
|
||||
void SimulationInit() override;
|
||||
void SimulationPeriodic() override;
|
||||
|
||||
|
||||
@@ -41,13 +41,13 @@ void Robot::TeleopPeriodic() {}
|
||||
|
||||
void Robot::TeleopExit() {}
|
||||
|
||||
void Robot::TestInit() {
|
||||
void Robot::UtilityInit() {
|
||||
wpi::cmd::CommandScheduler::GetInstance().CancelAll();
|
||||
}
|
||||
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
void Robot::TestExit() {}
|
||||
void Robot::UtilityExit() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -23,9 +23,9 @@ class Robot : public wpi::TimedRobot {
|
||||
void TeleopInit() override;
|
||||
void TeleopPeriodic() override;
|
||||
void TeleopExit() override;
|
||||
void TestInit() override;
|
||||
void TestPeriodic() override;
|
||||
void TestExit() override;
|
||||
void UtilityInit() override;
|
||||
void UtilityPeriodic() override;
|
||||
void UtilityExit() override;
|
||||
|
||||
private:
|
||||
std::optional<wpi::cmd::CommandPtr> m_autonomousCommand;
|
||||
|
||||
@@ -17,7 +17,7 @@ void Robot::Autonomous() {}
|
||||
|
||||
void Robot::Teleop() {}
|
||||
|
||||
void Robot::Test() {}
|
||||
void Robot::Utility() {}
|
||||
|
||||
void Robot::StartCompetition() {
|
||||
wpi::internal::DriverStationModeThread modeThread{wpi::hal::GetControlWord()};
|
||||
@@ -25,7 +25,7 @@ void Robot::StartCompetition() {
|
||||
// Create an opmode per robot mode
|
||||
wpi::RobotState::AddOpMode(wpi::RobotMode::AUTONOMOUS, "Auto");
|
||||
wpi::RobotState::AddOpMode(wpi::RobotMode::TELEOPERATED, "Teleop");
|
||||
wpi::RobotState::AddOpMode(wpi::RobotMode::TEST, "Test");
|
||||
wpi::RobotState::AddOpMode(wpi::RobotMode::UTILITY, "Utility");
|
||||
wpi::RobotState::PublishOpModes();
|
||||
|
||||
wpi::util::Event event{false, false};
|
||||
@@ -47,9 +47,9 @@ void Robot::StartCompetition() {
|
||||
while (IsAutonomousEnabled()) {
|
||||
wpi::util::WaitForObject(event.GetHandle());
|
||||
}
|
||||
} else if (IsTest()) {
|
||||
Test();
|
||||
while (IsTest() && IsEnabled()) {
|
||||
} else if (IsUtility()) {
|
||||
Utility();
|
||||
while (IsUtility() && IsEnabled()) {
|
||||
wpi::util::WaitForObject(event.GetHandle());
|
||||
}
|
||||
} else {
|
||||
|
||||
@@ -14,7 +14,7 @@ class Robot : public wpi::RobotBase {
|
||||
void Disabled();
|
||||
void Autonomous();
|
||||
void Teleop();
|
||||
void Test();
|
||||
void Utility();
|
||||
|
||||
void StartCompetition() override;
|
||||
void EndCompetition() override;
|
||||
|
||||
@@ -16,7 +16,7 @@ Robot::Robot() {
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use
|
||||
* this for items like diagnostics that you want ran during disabled,
|
||||
* autonomous, teleoperated and test.
|
||||
* autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p> This runs after the mode specific periodic functions, but before
|
||||
* LiveWindow and SmartDashboard integrated updating.
|
||||
@@ -63,9 +63,9 @@ void Robot::DisabledInit() {}
|
||||
|
||||
void Robot::DisabledPeriodic() {}
|
||||
|
||||
void Robot::TestInit() {}
|
||||
void Robot::UtilityInit() {}
|
||||
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
void Robot::SimulationInit() {}
|
||||
|
||||
|
||||
@@ -19,8 +19,8 @@ class Robot : public wpi::TimedRobot {
|
||||
void TeleopPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void TestInit() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityInit() override;
|
||||
void UtilityPeriodic() override;
|
||||
void SimulationInit() override;
|
||||
void SimulationPeriodic() override;
|
||||
|
||||
|
||||
@@ -16,8 +16,8 @@ void Robot::TeleopPeriodic() {}
|
||||
void Robot::DisabledInit() {}
|
||||
void Robot::DisabledPeriodic() {}
|
||||
|
||||
void Robot::TestInit() {}
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityInit() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
void Robot::SimulationInit() {}
|
||||
void Robot::SimulationPeriodic() {}
|
||||
|
||||
@@ -20,8 +20,8 @@ class Robot : public wpi::TimedRobot {
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
|
||||
void TestInit() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityInit() override;
|
||||
void UtilityPeriodic() override;
|
||||
|
||||
void SimulationInit() override;
|
||||
void SimulationPeriodic() override;
|
||||
|
||||
@@ -74,9 +74,9 @@ void Robot::DisabledInit() {}
|
||||
|
||||
void Robot::DisabledPeriodic() {}
|
||||
|
||||
void Robot::TestInit() {}
|
||||
void Robot::UtilityInit() {}
|
||||
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -19,8 +19,8 @@ class Robot : public wpi::TimesliceRobot {
|
||||
void TeleopPeriodic() override;
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
void TestInit() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityInit() override;
|
||||
void UtilityPeriodic() override;
|
||||
|
||||
private:
|
||||
wpi::SendableChooser<std::string> m_chooser;
|
||||
|
||||
@@ -28,8 +28,8 @@ void Robot::TeleopPeriodic() {}
|
||||
void Robot::DisabledInit() {}
|
||||
void Robot::DisabledPeriodic() {}
|
||||
|
||||
void Robot::TestInit() {}
|
||||
void Robot::TestPeriodic() {}
|
||||
void Robot::UtilityInit() {}
|
||||
void Robot::UtilityPeriodic() {}
|
||||
|
||||
#ifndef RUNNING_WPILIB_TESTS
|
||||
int main() {
|
||||
|
||||
@@ -21,6 +21,6 @@ class Robot : public wpi::TimesliceRobot {
|
||||
void DisabledInit() override;
|
||||
void DisabledPeriodic() override;
|
||||
|
||||
void TestInit() override;
|
||||
void TestPeriodic() override;
|
||||
void UtilityInit() override;
|
||||
void UtilityPeriodic() override;
|
||||
};
|
||||
|
||||
@@ -91,23 +91,23 @@ public final class RobotState {
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in Test
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in Utility
|
||||
* mode.
|
||||
*
|
||||
* @return True if test mode should be enabled, false otherwise.
|
||||
* @return True if utility mode should be enabled, false otherwise.
|
||||
*/
|
||||
public static boolean isTest() {
|
||||
return DriverStationBackend.isTest();
|
||||
public static boolean isUtility() {
|
||||
return DriverStationBackend.isUtility();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in Test
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in Utility
|
||||
* mode and enabled.
|
||||
*
|
||||
* @return True if test mode should be set and the robot should be enabled.
|
||||
* @return True if utility mode should be set and the robot should be enabled.
|
||||
*/
|
||||
public static boolean isTestEnabled() {
|
||||
return DriverStationBackend.isTestEnabled();
|
||||
public static boolean isUtilityEnabled() {
|
||||
return DriverStationBackend.isUtilityEnabled();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -1167,30 +1167,30 @@ public final class DriverStationBackend {
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in Test
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in Utility
|
||||
* mode.
|
||||
*
|
||||
* @return True if test mode should be enabled, false otherwise.
|
||||
* @return True if utility mode should be enabled, false otherwise.
|
||||
*/
|
||||
public static boolean isTest() {
|
||||
public static boolean isUtility() {
|
||||
m_cacheDataMutex.lock();
|
||||
try {
|
||||
return m_controlWord.isTest();
|
||||
return m_controlWord.isUtility();
|
||||
} finally {
|
||||
m_cacheDataMutex.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in Test
|
||||
* Gets a value indicating whether the Driver Station requires the robot to be running in Utility
|
||||
* mode and enabled.
|
||||
*
|
||||
* @return True if test mode should be set and the robot should be enabled.
|
||||
* @return True if utility mode should be set and the robot should be enabled.
|
||||
*/
|
||||
public static boolean isTestEnabled() {
|
||||
public static boolean isUtilityEnabled() {
|
||||
m_cacheDataMutex.lock();
|
||||
try {
|
||||
return m_controlWord.isTestEnabled();
|
||||
return m_controlWord.isUtilityEnabled();
|
||||
} finally {
|
||||
m_cacheDataMutex.unlock();
|
||||
}
|
||||
|
||||
@@ -33,7 +33,7 @@ import org.wpilib.system.Watchdog;
|
||||
* <li>disabledInit() -- called each and every time disabled is entered from another mode
|
||||
* <li>autonomousInit() -- called each and every time autonomous is entered from another mode
|
||||
* <li>teleopInit() -- called each and every time teleop is entered from another mode
|
||||
* <li>testInit() -- called each and every time test is entered from another mode
|
||||
* <li>utilityInit() -- called each and every time utility is entered from another mode
|
||||
* </ul>
|
||||
*
|
||||
* <p>periodic() functions -- each of these functions is called on an interval:
|
||||
@@ -43,7 +43,7 @@ import org.wpilib.system.Watchdog;
|
||||
* <li>disabledPeriodic()
|
||||
* <li>autonomousPeriodic()
|
||||
* <li>teleopPeriodic()
|
||||
* <li>testPeriodic()
|
||||
* <li>utilityPeriodic()
|
||||
* </ul>
|
||||
*
|
||||
* <p>exit() functions -- each of the following functions is called once when the appropriate mode
|
||||
@@ -53,7 +53,7 @@ import org.wpilib.system.Watchdog;
|
||||
* <li>disabledExit() -- called each and every time disabled is exited
|
||||
* <li>autonomousExit() -- called each and every time autonomous is exited
|
||||
* <li>teleopExit() -- called each and every time teleop is exited
|
||||
* <li>testExit() -- called each and every time test is exited
|
||||
* <li>utilityExit() -- called each and every time utility is exited
|
||||
* </ul>
|
||||
*/
|
||||
public abstract class IterativeRobotBase extends RobotBase {
|
||||
@@ -121,12 +121,12 @@ public abstract class IterativeRobotBase extends RobotBase {
|
||||
public void teleopInit() {}
|
||||
|
||||
/**
|
||||
* Initialization code for test mode should go here.
|
||||
* Initialization code for utility mode should go here.
|
||||
*
|
||||
* <p>Users should override this method for initialization code which will be called each time the
|
||||
* robot enters test mode.
|
||||
* robot enters utility mode.
|
||||
*/
|
||||
public void testInit() {}
|
||||
public void utilityInit() {}
|
||||
|
||||
/* ----------- Overridable periodic code ----------------- */
|
||||
|
||||
@@ -186,10 +186,10 @@ public abstract class IterativeRobotBase extends RobotBase {
|
||||
|
||||
private boolean m_tmpFirstRun = true;
|
||||
|
||||
/** Periodic code for test mode should go here. */
|
||||
public void testPeriodic() {
|
||||
/** Periodic code for utility mode should go here. */
|
||||
public void utilityPeriodic() {
|
||||
if (m_tmpFirstRun) {
|
||||
System.out.println("Default testPeriodic() method... Override me!");
|
||||
System.out.println("Default utilityPeriodic() method... Override me!");
|
||||
m_tmpFirstRun = false;
|
||||
}
|
||||
}
|
||||
@@ -219,12 +219,12 @@ public abstract class IterativeRobotBase extends RobotBase {
|
||||
public void teleopExit() {}
|
||||
|
||||
/**
|
||||
* Exit code for test mode should go here.
|
||||
* Exit code for utility mode should go here.
|
||||
*
|
||||
* <p>Users should override this method for code which will be called each time the robot exits
|
||||
* test mode.
|
||||
* utility mode.
|
||||
*/
|
||||
public void testExit() {}
|
||||
public void utilityExit() {}
|
||||
|
||||
/**
|
||||
* Gets time period between calls to Periodic() functions.
|
||||
@@ -258,7 +258,7 @@ public abstract class IterativeRobotBase extends RobotBase {
|
||||
case UNKNOWN -> disabledExit();
|
||||
case AUTONOMOUS -> autonomousExit();
|
||||
case TELEOPERATED -> teleopExit();
|
||||
case TEST -> testExit();
|
||||
case UTILITY -> utilityExit();
|
||||
default -> {
|
||||
// NOP
|
||||
}
|
||||
@@ -279,9 +279,9 @@ public abstract class IterativeRobotBase extends RobotBase {
|
||||
teleopInit();
|
||||
m_watchdog.addEpoch("teleopInit()");
|
||||
}
|
||||
case TEST -> {
|
||||
testInit();
|
||||
m_watchdog.addEpoch("testInit()");
|
||||
case UTILITY -> {
|
||||
utilityInit();
|
||||
m_watchdog.addEpoch("utilityInit()");
|
||||
}
|
||||
default -> {
|
||||
// NOP
|
||||
@@ -306,9 +306,9 @@ public abstract class IterativeRobotBase extends RobotBase {
|
||||
teleopPeriodic();
|
||||
m_watchdog.addEpoch("teleopPeriodic()");
|
||||
}
|
||||
case TEST -> {
|
||||
testPeriodic();
|
||||
m_watchdog.addEpoch("testPeriodic()");
|
||||
case UTILITY -> {
|
||||
utilityPeriodic();
|
||||
m_watchdog.addEpoch("utilityPeriodic()");
|
||||
}
|
||||
default -> {
|
||||
// NOP
|
||||
|
||||
@@ -37,7 +37,7 @@ import org.wpilib.opmode.Autonomous;
|
||||
import org.wpilib.opmode.OpMode;
|
||||
import org.wpilib.opmode.PeriodicOpMode;
|
||||
import org.wpilib.opmode.Teleop;
|
||||
import org.wpilib.opmode.TestOpMode;
|
||||
import org.wpilib.opmode.Utility;
|
||||
import org.wpilib.smartdashboard.SmartDashboard;
|
||||
import org.wpilib.system.RobotController;
|
||||
import org.wpilib.system.Watchdog;
|
||||
@@ -49,9 +49,9 @@ import org.wpilib.util.ConstructorMatch;
|
||||
*
|
||||
* <p>The OpModeRobot class is intended to be subclassed by a user creating a robot program.
|
||||
*
|
||||
* <p>Classes annotated with {@link Autonomous}, {@link Teleop}, and {@link TestOpMode} in the same
|
||||
* <p>Classes annotated with {@link Autonomous}, {@link Teleop}, and {@link Utility} in the same
|
||||
* package or subpackages as the user's subclass are automatically registered as autonomous, teleop,
|
||||
* and test opmodes respectively.
|
||||
* and utility opmodes respectively.
|
||||
*
|
||||
* <p>Opmodes are constructed when selected on the driver station. While selected and disabled,
|
||||
* {@link PeriodicOpMode#disabledPeriodic()} is called. When enabled, {@link PeriodicOpMode#start()}
|
||||
@@ -363,7 +363,7 @@ public abstract class OpModeRobot extends RobotBase {
|
||||
}
|
||||
|
||||
private void addAnnotatedOpModeImpl(
|
||||
Class<? extends OpMode> cls, Autonomous auto, Teleop teleop, TestOpMode test) {
|
||||
Class<? extends OpMode> cls, Autonomous auto, Teleop teleop, Utility utility) {
|
||||
checkOpModeClass(cls);
|
||||
|
||||
// add an opmode for each annotation
|
||||
@@ -387,24 +387,24 @@ public abstract class OpModeRobot extends RobotBase {
|
||||
teleop.textColor(),
|
||||
teleop.backgroundColor());
|
||||
}
|
||||
if (test != null) {
|
||||
if (utility != null) {
|
||||
addOpModeClassImpl(
|
||||
cls,
|
||||
RobotMode.TEST,
|
||||
test.name(),
|
||||
test.group(),
|
||||
test.description(),
|
||||
test.textColor(),
|
||||
test.backgroundColor());
|
||||
RobotMode.UTILITY,
|
||||
utility.name(),
|
||||
utility.group(),
|
||||
utility.description(),
|
||||
utility.textColor(),
|
||||
utility.backgroundColor());
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds an opmode for an opmode class annotated with {@link Autonomous}, {@link Teleop}, or {@link
|
||||
* TestOpMode}. The class must be a public, non-abstract subclass of OpMode with a public
|
||||
* constructor that either takes no arguments or accepts a single argument assignable from this
|
||||
* robot class type (if multiple match, the most specific parameter type is used). It's necessary
|
||||
* to call publishOpModes() to make the added mode visible to the driver station.
|
||||
* Utility}. The class must be a public, non-abstract subclass of OpMode with a public constructor
|
||||
* that either takes no arguments or accepts a single argument assignable from this robot class
|
||||
* type (if multiple match, the most specific parameter type is used). It's necessary to call
|
||||
* publishOpModes() to make the added mode visible to the driver station.
|
||||
*
|
||||
* @param cls class to add
|
||||
* @throws IllegalArgumentException if class does not meet criteria
|
||||
@@ -412,12 +412,11 @@ public abstract class OpModeRobot extends RobotBase {
|
||||
public void addAnnotatedOpMode(Class<? extends OpMode> cls) {
|
||||
Autonomous auto = cls.getAnnotation(Autonomous.class);
|
||||
Teleop teleop = cls.getAnnotation(Teleop.class);
|
||||
TestOpMode test = cls.getAnnotation(TestOpMode.class);
|
||||
if (auto == null && teleop == null && test == null) {
|
||||
throw new IllegalArgumentException(
|
||||
"must be annotated with Autonomous, Teleop, or TestOpMode");
|
||||
Utility utility = cls.getAnnotation(Utility.class);
|
||||
if (auto == null && teleop == null && utility == null) {
|
||||
throw new IllegalArgumentException("must be annotated with Autonomous, Teleop, or Utility");
|
||||
}
|
||||
addAnnotatedOpModeImpl(cls, auto, teleop, test);
|
||||
addAnnotatedOpModeImpl(cls, auto, teleop, utility);
|
||||
}
|
||||
|
||||
private void addAnnotatedOpModeClass(String name) {
|
||||
@@ -431,12 +430,12 @@ public abstract class OpModeRobot extends RobotBase {
|
||||
}
|
||||
Autonomous auto = cls.getAnnotation(Autonomous.class);
|
||||
Teleop teleop = cls.getAnnotation(Teleop.class);
|
||||
TestOpMode test = cls.getAnnotation(TestOpMode.class);
|
||||
if (auto == null && teleop == null && test == null) {
|
||||
Utility utility = cls.getAnnotation(Utility.class);
|
||||
if (auto == null && teleop == null && utility == null) {
|
||||
return;
|
||||
}
|
||||
try {
|
||||
addAnnotatedOpModeImpl(cls, auto, teleop, test);
|
||||
addAnnotatedOpModeImpl(cls, auto, teleop, utility);
|
||||
} catch (IllegalArgumentException e) {
|
||||
reportAddOpModeError(cls, e.getMessage());
|
||||
}
|
||||
@@ -459,7 +458,7 @@ public abstract class OpModeRobot extends RobotBase {
|
||||
|
||||
/**
|
||||
* Scans for classes in the specified package and all nested packages that are annotated with
|
||||
* {@link Autonomous}, {@link Teleop}, or {@link TestOpMode} and registers them. It's necessary to
|
||||
* {@link Autonomous}, {@link Teleop}, or {@link Utility} and registers them. It's necessary to
|
||||
* call publishOpModes() to make the added modes visible to the driver station.
|
||||
*
|
||||
* @param pkg package to scan
|
||||
|
||||
@@ -217,21 +217,22 @@ public abstract class RobotBase implements AutoCloseable {
|
||||
}
|
||||
|
||||
/**
|
||||
* Determine if the robot is currently in Test mode as determined by the Driver Station.
|
||||
* Determine if the robot is currently in Utility mode as determined by the Driver Station.
|
||||
*
|
||||
* @return True if the robot is currently operating in Test mode.
|
||||
* @return True if the robot is currently operating in Utility mode.
|
||||
*/
|
||||
public static boolean isTest() {
|
||||
return RobotState.isTest();
|
||||
public static boolean isUtility() {
|
||||
return RobotState.isUtility();
|
||||
}
|
||||
|
||||
/**
|
||||
* Determine if the robot is current in Test mode and enabled as determined by the Driver Station.
|
||||
* Determine if the robot is current in Utility mode and enabled as determined by the Driver
|
||||
* Station.
|
||||
*
|
||||
* @return True if the robot is currently operating in Test mode while enabled.
|
||||
* @return True if the robot is currently operating in Utility mode while enabled.
|
||||
*/
|
||||
public static boolean isTestEnabled() {
|
||||
return RobotState.isTestEnabled();
|
||||
public static boolean isUtilityEnabled() {
|
||||
return RobotState.isUtilityEnabled();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -134,7 +134,7 @@ public abstract class MotorSafety {
|
||||
stopTime = m_stopTime;
|
||||
}
|
||||
|
||||
if (!enabled || RobotState.isDisabled() || RobotState.isTest()) {
|
||||
if (!enabled || RobotState.isDisabled() || RobotState.isUtility()) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@@ -10,14 +10,14 @@ import java.lang.annotation.Retention;
|
||||
import java.lang.annotation.RetentionPolicy;
|
||||
import java.lang.annotation.Target;
|
||||
|
||||
/** Annotation for automatic registration of test opmode classes. */
|
||||
/** Annotation for automatic registration of utility opmode classes. */
|
||||
@Retention(RetentionPolicy.RUNTIME)
|
||||
@Target(ElementType.TYPE)
|
||||
@Documented
|
||||
public @interface TestOpMode {
|
||||
public @interface Utility {
|
||||
/**
|
||||
* Name. This is shown as the selection name in the Driver Station, and must be unique across all
|
||||
* test opmodes in the project. If not specified, defaults to the name of the class.
|
||||
* utility opmodes in the project. If not specified, defaults to the name of the class.
|
||||
*
|
||||
* @return Name
|
||||
*/
|
||||
@@ -33,6 +33,6 @@ public final class MockHardwareExtension implements BeforeAllCallback {
|
||||
HAL.initialize(500, 0);
|
||||
DriverStationSim.setDsAttached(true);
|
||||
DriverStationSim.setEnabled(true);
|
||||
DriverStationSim.setRobotMode(RobotMode.TEST);
|
||||
DriverStationSim.setRobotMode(RobotMode.UTILITY);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -125,7 +125,7 @@ class OpModeRobotTest {
|
||||
Color.BLACK);
|
||||
addOpModeFactory(
|
||||
() -> new OneArgOpMode(this),
|
||||
RobotMode.TEST,
|
||||
RobotMode.UTILITY,
|
||||
"OneArgOpMode-Test",
|
||||
"Group",
|
||||
"Description",
|
||||
|
||||
@@ -23,19 +23,19 @@ class TimedRobotTest {
|
||||
public final AtomicInteger m_disabledInitCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_autonomousInitCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_teleopInitCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_testInitCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_utilityInitCount = new AtomicInteger(0);
|
||||
|
||||
public final AtomicInteger m_robotPeriodicCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_simulationPeriodicCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_disabledPeriodicCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_autonomousPeriodicCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_teleopPeriodicCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_testPeriodicCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_utilityPeriodicCount = new AtomicInteger(0);
|
||||
|
||||
public final AtomicInteger m_disabledExitCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_autonomousExitCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_teleopExitCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_testExitCount = new AtomicInteger(0);
|
||||
public final AtomicInteger m_utilityExitCount = new AtomicInteger(0);
|
||||
|
||||
MockRobot() {
|
||||
super(kPeriod);
|
||||
@@ -62,8 +62,8 @@ class TimedRobotTest {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
m_testInitCount.addAndGet(1);
|
||||
public void utilityInit() {
|
||||
m_utilityInitCount.addAndGet(1);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -92,8 +92,8 @@ class TimedRobotTest {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testPeriodic() {
|
||||
m_testPeriodicCount.addAndGet(1);
|
||||
public void utilityPeriodic() {
|
||||
m_utilityPeriodicCount.addAndGet(1);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -112,8 +112,8 @@ class TimedRobotTest {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testExit() {
|
||||
m_testExitCount.addAndGet(1);
|
||||
public void utilityExit() {
|
||||
m_utilityExitCount.addAndGet(1);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -145,19 +145,19 @@ class TimedRobotTest {
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(0, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(0, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(0, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(0, robot.m_testPeriodicCount.get());
|
||||
assertEquals(0, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
SimHooks.stepTiming(kPeriod);
|
||||
|
||||
@@ -165,19 +165,19 @@ class TimedRobotTest {
|
||||
assertEquals(1, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(1, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(1, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(1, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(0, robot.m_testPeriodicCount.get());
|
||||
assertEquals(0, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
SimHooks.stepTiming(kPeriod);
|
||||
|
||||
@@ -185,19 +185,19 @@ class TimedRobotTest {
|
||||
assertEquals(1, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(2, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(2, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(2, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(0, robot.m_testPeriodicCount.get());
|
||||
assertEquals(0, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
robot.endCompetition();
|
||||
try {
|
||||
@@ -226,19 +226,19 @@ class TimedRobotTest {
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(0, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(0, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(0, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(0, robot.m_testPeriodicCount.get());
|
||||
assertEquals(0, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
SimHooks.stepTiming(kPeriod);
|
||||
|
||||
@@ -246,19 +246,19 @@ class TimedRobotTest {
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(1, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(1, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(1, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(0, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(1, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(0, robot.m_testPeriodicCount.get());
|
||||
assertEquals(0, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
SimHooks.stepTiming(kPeriod);
|
||||
|
||||
@@ -266,19 +266,19 @@ class TimedRobotTest {
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(1, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(2, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(2, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(0, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(2, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(0, robot.m_testPeriodicCount.get());
|
||||
assertEquals(0, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
robot.endCompetition();
|
||||
try {
|
||||
@@ -307,19 +307,19 @@ class TimedRobotTest {
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(0, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(0, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(0, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(0, robot.m_testPeriodicCount.get());
|
||||
assertEquals(0, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
SimHooks.stepTiming(kPeriod);
|
||||
|
||||
@@ -327,19 +327,19 @@ class TimedRobotTest {
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(1, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(1, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(1, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(0, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(1, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(0, robot.m_testPeriodicCount.get());
|
||||
assertEquals(0, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
SimHooks.stepTiming(kPeriod);
|
||||
|
||||
@@ -347,19 +347,19 @@ class TimedRobotTest {
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(1, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(2, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(2, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(0, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(2, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(0, robot.m_testPeriodicCount.get());
|
||||
assertEquals(0, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
robot.endCompetition();
|
||||
try {
|
||||
@@ -373,7 +373,7 @@ class TimedRobotTest {
|
||||
|
||||
@Test
|
||||
@ResourceLock("timing")
|
||||
void testModeTest() {
|
||||
void utilityModeTest() {
|
||||
MockRobot robot = new MockRobot();
|
||||
|
||||
Thread robotThread = new Thread(robot::startCompetition);
|
||||
@@ -381,26 +381,26 @@ class TimedRobotTest {
|
||||
SimHooks.waitForProgramStart();
|
||||
|
||||
DriverStationSim.setEnabled(true);
|
||||
DriverStationSim.setRobotMode(RobotMode.TEST);
|
||||
DriverStationSim.setRobotMode(RobotMode.UTILITY);
|
||||
DriverStationSim.notifyNewData();
|
||||
|
||||
assertEquals(1, robot.m_simulationInitCount.get());
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(0, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(0, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(0, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(0, robot.m_testPeriodicCount.get());
|
||||
assertEquals(0, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
SimHooks.stepTiming(kPeriod);
|
||||
|
||||
@@ -408,19 +408,19 @@ class TimedRobotTest {
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(1, robot.m_testInitCount.get());
|
||||
assertEquals(1, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(1, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(1, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(0, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(1, robot.m_testPeriodicCount.get());
|
||||
assertEquals(1, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
SimHooks.stepTiming(kPeriod);
|
||||
|
||||
@@ -428,19 +428,19 @@ class TimedRobotTest {
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(1, robot.m_testInitCount.get());
|
||||
assertEquals(1, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(2, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(2, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(0, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(2, robot.m_testPeriodicCount.get());
|
||||
assertEquals(2, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
DriverStationSim.setEnabled(false);
|
||||
DriverStationSim.notifyNewData();
|
||||
@@ -451,19 +451,19 @@ class TimedRobotTest {
|
||||
assertEquals(1, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(1, robot.m_testInitCount.get());
|
||||
assertEquals(1, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(3, robot.m_robotPeriodicCount.get());
|
||||
assertEquals(3, robot.m_simulationPeriodicCount.get());
|
||||
assertEquals(1, robot.m_disabledPeriodicCount.get());
|
||||
assertEquals(0, robot.m_autonomousPeriodicCount.get());
|
||||
assertEquals(0, robot.m_teleopPeriodicCount.get());
|
||||
assertEquals(2, robot.m_testPeriodicCount.get());
|
||||
assertEquals(2, robot.m_utilityPeriodicCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(1, robot.m_testExitCount.get());
|
||||
assertEquals(1, robot.m_utilityExitCount.get());
|
||||
|
||||
robot.endCompetition();
|
||||
try {
|
||||
@@ -491,24 +491,24 @@ class TimedRobotTest {
|
||||
assertEquals(0, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
SimHooks.stepTiming(kPeriod);
|
||||
|
||||
assertEquals(1, robot.m_disabledInitCount.get());
|
||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(0, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
// Transition to autonomous
|
||||
DriverStationSim.setEnabled(true);
|
||||
@@ -520,12 +520,12 @@ class TimedRobotTest {
|
||||
assertEquals(1, robot.m_disabledInitCount.get());
|
||||
assertEquals(1, robot.m_autonomousInitCount.get());
|
||||
assertEquals(0, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(1, robot.m_disabledExitCount.get());
|
||||
assertEquals(0, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
// Transition to teleop
|
||||
DriverStationSim.setEnabled(true);
|
||||
@@ -537,16 +537,16 @@ class TimedRobotTest {
|
||||
assertEquals(1, robot.m_disabledInitCount.get());
|
||||
assertEquals(1, robot.m_autonomousInitCount.get());
|
||||
assertEquals(1, robot.m_teleopInitCount.get());
|
||||
assertEquals(0, robot.m_testInitCount.get());
|
||||
assertEquals(0, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(1, robot.m_disabledExitCount.get());
|
||||
assertEquals(1, robot.m_autonomousExitCount.get());
|
||||
assertEquals(0, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
// Transition to test
|
||||
// Transition to utility
|
||||
DriverStationSim.setEnabled(true);
|
||||
DriverStationSim.setRobotMode(RobotMode.TEST);
|
||||
DriverStationSim.setRobotMode(RobotMode.UTILITY);
|
||||
DriverStationSim.notifyNewData();
|
||||
|
||||
SimHooks.stepTiming(kPeriod);
|
||||
@@ -554,12 +554,12 @@ class TimedRobotTest {
|
||||
assertEquals(1, robot.m_disabledInitCount.get());
|
||||
assertEquals(1, robot.m_autonomousInitCount.get());
|
||||
assertEquals(1, robot.m_teleopInitCount.get());
|
||||
assertEquals(1, robot.m_testInitCount.get());
|
||||
assertEquals(1, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(1, robot.m_disabledExitCount.get());
|
||||
assertEquals(1, robot.m_autonomousExitCount.get());
|
||||
assertEquals(1, robot.m_teleopExitCount.get());
|
||||
assertEquals(0, robot.m_testExitCount.get());
|
||||
assertEquals(0, robot.m_utilityExitCount.get());
|
||||
|
||||
// Transition to disabled
|
||||
DriverStationSim.setEnabled(false);
|
||||
@@ -570,12 +570,12 @@ class TimedRobotTest {
|
||||
assertEquals(2, robot.m_disabledInitCount.get());
|
||||
assertEquals(1, robot.m_autonomousInitCount.get());
|
||||
assertEquals(1, robot.m_teleopInitCount.get());
|
||||
assertEquals(1, robot.m_testInitCount.get());
|
||||
assertEquals(1, robot.m_utilityInitCount.get());
|
||||
|
||||
assertEquals(1, robot.m_disabledExitCount.get());
|
||||
assertEquals(1, robot.m_autonomousExitCount.get());
|
||||
assertEquals(1, robot.m_teleopExitCount.get());
|
||||
assertEquals(1, robot.m_testExitCount.get());
|
||||
assertEquals(1, robot.m_utilityExitCount.get());
|
||||
|
||||
robot.endCompetition();
|
||||
try {
|
||||
|
||||
@@ -64,16 +64,16 @@ class DriverStationSimTest {
|
||||
HAL.initialize(500, 0);
|
||||
DriverStationSim.resetData();
|
||||
|
||||
assertFalse(RobotState.isTest());
|
||||
assertFalse(RobotState.isUtility());
|
||||
EnumCallback callback = new EnumCallback();
|
||||
try (CallbackStore cb = DriverStationSim.registerRobotModeCallback(callback, false)) {
|
||||
DriverStationSim.setRobotMode(RobotMode.TEST);
|
||||
DriverStationSim.setRobotMode(RobotMode.UTILITY);
|
||||
DriverStationSim.notifyNewData();
|
||||
assertEquals(RobotMode.TEST, DriverStationSim.getRobotMode());
|
||||
assertTrue(RobotState.isTest());
|
||||
assertEquals(RobotMode.TEST, RobotState.getRobotMode());
|
||||
assertEquals(RobotMode.UTILITY, DriverStationSim.getRobotMode());
|
||||
assertTrue(RobotState.isUtility());
|
||||
assertEquals(RobotMode.UTILITY, RobotState.getRobotMode());
|
||||
assertTrue(callback.wasTriggered());
|
||||
assertEquals(RobotMode.TEST.getValue(), callback.getSetValue());
|
||||
assertEquals(RobotMode.UTILITY.getValue(), callback.getSetValue());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -89,12 +89,12 @@ public class Robot extends TimedRobot {
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
public void utilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
}
|
||||
|
||||
@@ -63,11 +63,11 @@ public class Robot extends TimedRobot {
|
||||
m_robotDrive.arcadeDrive(-m_controller.getLeftY(), -m_controller.getRightX());
|
||||
}
|
||||
|
||||
/** This function is called once each time the robot enters test mode. */
|
||||
/** This function is called once each time the robot enters utility mode. */
|
||||
@Override
|
||||
public void testInit() {}
|
||||
public void utilityInit() {}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
}
|
||||
|
||||
@@ -91,12 +91,12 @@ public class Robot extends TimedRobot {
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
public void utilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
}
|
||||
|
||||
@@ -39,7 +39,7 @@ public class Robot extends TimedRobot {
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
|
||||
* that you want ran during disabled, autonomous, teleoperated and test.
|
||||
* that you want ran during disabled, autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
|
||||
* SmartDashboard integrated updating.
|
||||
@@ -98,12 +98,12 @@ public class Robot extends TimedRobot {
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
public void utilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
}
|
||||
|
||||
@@ -87,12 +87,12 @@ public class Robot extends TimedRobot {
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
public void utilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
}
|
||||
|
||||
@@ -82,12 +82,12 @@ public class Robot extends TimedRobot {
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
public void utilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
}
|
||||
|
||||
@@ -79,12 +79,12 @@ public class Robot extends TimedRobot {
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
public void utilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
}
|
||||
|
||||
@@ -82,14 +82,14 @@ public class Robot extends TimedRobot {
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
public void utilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
|
||||
/** This function is called once when the robot is first started up. */
|
||||
@Override
|
||||
|
||||
@@ -64,11 +64,11 @@ public class Robot extends TimedRobot {
|
||||
m_robotDrive.arcadeDrive(-m_controller.getRawAxis(2), -m_controller.getRawAxis(1));
|
||||
}
|
||||
|
||||
/** This function is called once each time the robot enters test mode. */
|
||||
/** This function is called once each time the robot enters utility mode. */
|
||||
@Override
|
||||
public void testInit() {}
|
||||
public void utilityInit() {}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
}
|
||||
|
||||
@@ -30,7 +30,7 @@ public class Robot extends TimedRobot {
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
|
||||
* that you want ran during disabled, autonomous, teleoperated and test.
|
||||
* that you want ran during disabled, autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
|
||||
* SmartDashboard integrated updating.
|
||||
@@ -89,12 +89,12 @@ public class Robot extends TimedRobot {
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
public void utilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
}
|
||||
|
||||
@@ -30,7 +30,7 @@ public class Robot extends TimedRobot {
|
||||
|
||||
/**
|
||||
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
|
||||
* that you want ran during disabled, autonomous, teleoperated and test.
|
||||
* that you want ran during disabled, autonomous, teleoperated and utility.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
|
||||
* SmartDashboard integrated updating.
|
||||
@@ -82,14 +82,14 @@ public class Robot extends TimedRobot {
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
// Cancels all running commands at the start of test mode.
|
||||
public void utilityInit() {
|
||||
// Cancels all running commands at the start of utility mode.
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
/** This function is called periodically during test mode. */
|
||||
/** This function is called periodically during utility mode. */
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
|
||||
/** This function is called once when the robot is first started up. */
|
||||
@Override
|
||||
|
||||
@@ -60,13 +60,13 @@ public class Robot extends TimedRobot {
|
||||
public void teleopExit() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
public void utilityInit() {
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
public void utilityPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testExit() {}
|
||||
public void utilityExit() {}
|
||||
}
|
||||
|
||||
@@ -28,7 +28,7 @@ public class EducationalRobot extends RobotBase {
|
||||
run();
|
||||
}
|
||||
|
||||
public void test() {
|
||||
public void utility() {
|
||||
run();
|
||||
}
|
||||
|
||||
@@ -39,7 +39,7 @@ public class EducationalRobot extends RobotBase {
|
||||
// Create an opmode per robot mode
|
||||
RobotState.addOpMode(RobotMode.AUTONOMOUS, "Auto");
|
||||
RobotState.addOpMode(RobotMode.TELEOPERATED, "Teleop");
|
||||
RobotState.addOpMode(RobotMode.TEST, "Test");
|
||||
RobotState.addOpMode(RobotMode.UTILITY, "Utility");
|
||||
RobotState.publishOpModes();
|
||||
|
||||
final ControlWord word = new ControlWord();
|
||||
@@ -73,9 +73,9 @@ public class EducationalRobot extends RobotBase {
|
||||
Thread.currentThread().interrupt();
|
||||
}
|
||||
}
|
||||
} else if (isTest()) {
|
||||
test();
|
||||
while (isTest() && isEnabled()) {
|
||||
} else if (isUtility()) {
|
||||
utility();
|
||||
while (isUtility() && isEnabled()) {
|
||||
try {
|
||||
WPIUtilJNI.waitForObject(event);
|
||||
} catch (InterruptedException e) {
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user