From dacded37e546679e0e3ad08b6c2d039b326d2b69 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Fri, 12 Dec 2025 21:25:57 -0700 Subject: [PATCH] [hal, wpilib] Add OpMode support (#7744) User code: - OpModeRobot used as the robot base class - LinearOpMode and PeriodicOpMode are provided opmode base classes - In Java, annotations can be used to automatically register opmode classes Additional user code functionality: - OpMode (string) is available in addition to the overall auto/teleop/test robot mode - OpMode does not indicate enable (enable/disable is still separate) - The HAL API uses integer UIDs; these are exposed at the user API level as well for faster checks - User code creates opmodes on startup (these have name, category, description, etc). DS: - DS will present opmode selection lists for auto and teleop for match/practice. During a match, the DS will automatically activate the selected opmode in the corresponding match period. - For testing, an overall mode is selected (e.g. teleop/auto/test) and a single opmode is selected Future work: - Command framework support/integration - Python annotation support - Unit tests (needs race-free DS sim updates) - Porting of examples Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../org/wpilib/command2/CommandScheduler.java | 5 +- .../cpp/frc2/command/CommandScheduler.cpp | 6 +- .../org/wpilib/MockHardwareExtension.java | 4 +- .../button/RobotModeTriggersTest.java | 12 +- .../command/button/RobotModeTriggersTest.cpp | 12 +- design-docs/opmodes.md | 534 +++++++++++++ docs/build.gradle | 1 + glass/src/lib/native/cpp/other/FMS.cpp | 15 +- .../native/include/wpi/glass/other/FMS.hpp | 13 +- glass/src/libnt/native/cpp/NTFMS.cpp | 46 +- .../include/wpi/glass/networktables/NTFMS.hpp | 12 +- hal/BUILD.bazel | 18 +- hal/robotpy_pybind_build_info.bzl | 11 +- .../native/cpp/mrc/protobuf/MrcComm.npb.cpp | 727 +++++++++--------- .../native/cpp/mrc/protobuf/MrcComm.npb.h | 30 +- .../org/wpilib/hardware/hal/ControlWord.java | 216 ++++-- .../wpilib/hardware/hal/DriverStationJNI.java | 79 +- .../org/wpilib/hardware/hal/OpModeOption.java | 67 ++ .../org/wpilib/hardware/hal/RobotMode.java | 42 + .../hal/simulation/DriverStationDataJNI.java | 38 +- .../hardware/hal/simulation/SimulatorJNI.java | 9 + .../hal/struct/ControlWordStruct.java | 50 ++ hal/src/main/native/cpp/DashboardOpMode.cpp | 162 ++++ .../main/native/cpp/jni/DriverStationJNI.cpp | 105 +-- hal/src/main/native/cpp/jni/HALUtil.cpp | 67 ++ hal/src/main/native/cpp/jni/HALUtil.h | 7 + .../jni/simulation/DriverStationDataJNI.cpp | 178 +++-- .../simulation/OpModeOptionsCallbackStore.cpp | 121 +++ .../simulation/OpModeOptionsCallbackStore.h | 40 + .../cpp/jni/simulation/SimulatorJNI.cpp | 100 ++- .../native/cpp/jni/simulation/SimulatorJNI.h | 1 + .../native/cpp/proto/ControlDataProto.cpp | 16 +- hal/src/main/native/cpp/proto/OpModeProto.cpp | 30 +- .../include/wpi/hal/DashboardOpMode.hpp | 19 + .../native/include/wpi/hal/DriverStation.h | 67 +- .../include/wpi/hal/DriverStationTypes.h | 364 ++++++++- .../wpi/hal/simulation/DriverStationData.h | 34 +- .../include/wpi/hal/simulation/MockHooks.h | 21 + hal/src/main/native/sim/DriverStation.cpp | 67 +- hal/src/main/native/sim/MockHooks.cpp | 9 + .../native/sim/mockdata/DriverStationData.cpp | 123 ++- .../sim/mockdata/DriverStationDataInternal.h | 36 +- .../native/systemcore/FRCDriverStation.cpp | 147 ++-- .../systemcore/mockdata/DriverStationData.cpp | 21 +- .../native/systemcore/mockdata/MockHooks.cpp | 6 + hal/src/main/python/pyproject.toml | 1 + .../main/python/semiwrap/DashboardOpMode.yml | 6 + .../main/python/semiwrap/DriverStation.yml | 11 +- .../python/semiwrap/DriverStationTypes.yml | 59 +- .../semiwrap/simulation/DriverStationData.yml | 27 +- .../python/semiwrap/simulation/MockHooks.yml | 6 + hal/src/mrc/include/mrc/NetComm.h | 115 ++- hal/src/mrc/include/mrc/NtNetComm.h | 5 +- hal/src/mrc/proto/MrcComm.proto | 6 +- shared/examplecheck.gradle | 2 +- simulation/halsim_ds_socket/build.gradle | 1 + .../src/main/native/cpp/DSCommPacket.cpp | 40 +- .../src/main/native/cpp/main.cpp | 2 + simulation/halsim_gui/build.gradle | 1 + .../src/main/native/cpp/DriverStationGui.cpp | 219 +++++- simulation/halsim_ws_client/build.gradle | 3 + .../native/cpp/WSProvider_DriverStation.cpp | 16 +- .../ws_core/WSProvider_DriverStation.hpp | 3 +- simulation/halsim_ws_server/build.gradle | 3 + .../src/test/native/cpp/main.cpp | 2 +- simulation/halsim_xrp/build.gradle | 1 + styleguide/spotbugs-exclude.xml | 4 + wpilibc/robotpy_pybind_build_info.bzl | 60 +- .../cpp/driverstation/DSControlWord.cpp | 55 -- .../cpp/driverstation/DriverStation.cpp | 242 +++--- .../cpp/framework/IterativeRobotBase.cpp | 51 +- .../main/native/cpp/framework/OpModeRobot.cpp | 248 ++++++ .../main/native/cpp/framework/RobotState.cpp | 33 - .../native/cpp/hardware/motor/MotorSafety.cpp | 6 +- .../cpp/internal/DriverStationModeThread.cpp | 49 +- .../main/native/cpp/opmode/LinearOpMode.cpp | 21 + .../main/native/cpp/opmode/PeriodicOpMode.cpp | 158 ++++ .../native/cpp/simulation/CallbackStore.cpp | 13 + .../cpp/simulation/DriverStationSim.cpp | 63 +- .../main/native/cpp/simulation/SimHooks.cpp | 8 + wpilibc/src/main/native/cppcs/RobotBase.cpp | 26 +- .../wpi/driverstation/DSControlWord.hpp | 102 --- .../wpi/driverstation/DriverStation.hpp | 150 +++- .../wpi/framework/IterativeRobotBase.hpp | 4 +- .../include/wpi/framework/OpModeRobot.hpp | 234 ++++++ .../include/wpi/framework/RobotBase.hpp | 36 +- .../include/wpi/framework/RobotState.hpp | 59 -- .../wpi/internal/DriverStationModeThread.hpp | 46 +- .../include/wpi/opmode/LinearOpMode.hpp | 68 ++ .../main/native/include/wpi/opmode/OpMode.hpp | 45 ++ .../include/wpi/opmode/PeriodicOpMode.hpp | 178 +++++ .../include/wpi/simulation/CallbackStore.hpp | 15 + .../wpi/simulation/DriverStationSim.hpp | 121 ++- .../include/wpi/simulation/SimHooks.hpp | 15 + wpilibc/src/main/python/pyproject.toml | 8 +- .../main/python/semiwrap/DSControlWord.yml | 24 - .../main/python/semiwrap/DriverStation.yml | 30 +- .../semiwrap/DriverStationModeThread.yml | 5 +- .../src/main/python/semiwrap/LinearOpMode.yml | 8 + wpilibc/src/main/python/semiwrap/OpMode.yml | 6 + .../src/main/python/semiwrap/OpModeRobot.yml | 26 + .../main/python/semiwrap/PeriodicOpMode.yml | 17 + .../src/main/python/semiwrap/RobotBase.yml | 18 +- .../src/main/python/semiwrap/RobotState.yml | 10 - .../semiwrap/simulation/CallbackStore.yml | 4 + .../semiwrap/simulation/DriverStationSim.yml | 17 +- .../python/semiwrap/simulation/SimHooks.yml | 2 + wpilibc/src/main/python/wpilib/__init__.py | 16 +- wpilibc/src/main/python/wpilib/opmoderobot.py | 61 ++ .../python/wpilib/src/rpy/ControlWord.cpp | 18 - .../main/python/wpilib/src/rpy/ControlWord.h | 7 - .../src/test/native/cpp/OpModeRobotTest.cpp | 175 +++++ .../src/test/native/cpp/TimedRobotTest.cpp | 25 +- .../cpp/simulation/DriverStationSimTest.cpp | 33 +- wpilibcExamples/example_projects.bzl | 1 + .../src/main/cpp/examples/HAL/c/Robot.c | 60 +- .../main/cpp/templates/opmode/cpp/Robot.cpp | 31 + .../templates/opmode/cpp/opmode/MyAuto.cpp | 33 + .../templates/opmode/cpp/opmode/MyTeleop.cpp | 26 + .../cpp/templates/opmode/include/Robot.hpp | 14 + .../opmode/include/opmode/MyAuto.hpp | 23 + .../opmode/include/opmode/MyTeleop.hpp | 23 + .../templates/robotbaseskeleton/cpp/Robot.cpp | 18 +- .../src/main/cpp/templates/templates.json | 10 + .../ArmSimulation/cpp/ArmSimulationTest.cpp | 4 +- .../cpp/DigitalCommunicationTest.cpp | 4 +- .../cpp/ElevatorSimulationTest.cpp | 4 +- .../cpp/I2CCommunicationTest.cpp | 4 +- .../cpp/PotentiometerPIDTest.cpp | 3 +- .../wpilib/driverstation/DSControlWord.java | 126 --- .../wpilib/driverstation/DriverStation.java | 377 +++++++-- .../wpilib/framework/IterativeRobotBase.java | 71 +- .../org/wpilib/framework/OpModeRobot.java | 701 +++++++++++++++++ .../java/org/wpilib/framework/RobotBase.java | 38 +- .../java/org/wpilib/framework/RobotState.java | 66 -- .../wpilib/hardware/motor/MotorSafety.java | 5 +- .../internal/DriverStationModeThread.java | 88 +-- .../java/org/wpilib/opmode/Autonomous.java | 56 ++ .../java/org/wpilib/opmode/LinearOpMode.java | 84 ++ .../main/java/org/wpilib/opmode/OpMode.java | 39 + .../org/wpilib/opmode/PeriodicOpMode.java | 344 +++++++++ .../main/java/org/wpilib/opmode/Teleop.java | 56 ++ .../java/org/wpilib/opmode/TestOpMode.java | 56 ++ .../wpilib/simulation/DriverStationSim.java | 112 +-- .../java/org/wpilib/simulation/SimHooks.java | 19 + .../org/wpilib/MockHardwareExtension.java | 4 +- .../org/wpilib/framework/TimedRobotTest.java | 25 +- .../simulation/DriverStationSimTest.java | 23 +- wpilibjExamples/example_projects.bzl | 1 + .../educational/EducationalRobot.java | 21 +- .../org/wpilib/templates/opmode/Main.java | 25 + .../org/wpilib/templates/opmode/Robot.java | 33 + .../templates/opmode/opmode/MyAuto.java | 29 + .../templates/opmode/opmode/MyTeleop.java | 44 ++ .../templates/robotbaseskeleton/Robot.java | 21 +- .../romieducational/EducationalRobot.java | 21 +- .../java/org/wpilib/templates/templates.json | 11 + .../xrpeducational/EducationalRobot.java | 21 +- .../armsimulation/ArmSimulationTest.java | 4 +- .../DigitalCommunicationTest.java | 3 +- .../ElevatorSimulationTest.java | 4 +- .../I2CCommunicationTest.java | 3 +- .../PotentiometerPIDTest.java | 3 +- 163 files changed, 7454 insertions(+), 2175 deletions(-) create mode 100644 design-docs/opmodes.md create mode 100644 hal/src/main/java/org/wpilib/hardware/hal/OpModeOption.java create mode 100644 hal/src/main/java/org/wpilib/hardware/hal/RobotMode.java create mode 100644 hal/src/main/java/org/wpilib/hardware/hal/struct/ControlWordStruct.java create mode 100644 hal/src/main/native/cpp/DashboardOpMode.cpp create mode 100644 hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.cpp create mode 100644 hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.h create mode 100644 hal/src/main/native/include/wpi/hal/DashboardOpMode.hpp create mode 100644 hal/src/main/python/semiwrap/DashboardOpMode.yml delete mode 100644 wpilibc/src/main/native/cpp/driverstation/DSControlWord.cpp create mode 100644 wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp delete mode 100644 wpilibc/src/main/native/cpp/framework/RobotState.cpp create mode 100644 wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp create mode 100644 wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp delete mode 100644 wpilibc/src/main/native/include/wpi/driverstation/DSControlWord.hpp create mode 100644 wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp delete mode 100644 wpilibc/src/main/native/include/wpi/framework/RobotState.hpp create mode 100644 wpilibc/src/main/native/include/wpi/opmode/LinearOpMode.hpp create mode 100644 wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp create mode 100644 wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp delete mode 100644 wpilibc/src/main/python/semiwrap/DSControlWord.yml create mode 100644 wpilibc/src/main/python/semiwrap/LinearOpMode.yml create mode 100644 wpilibc/src/main/python/semiwrap/OpMode.yml create mode 100644 wpilibc/src/main/python/semiwrap/OpModeRobot.yml create mode 100644 wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml delete mode 100644 wpilibc/src/main/python/semiwrap/RobotState.yml create mode 100644 wpilibc/src/main/python/wpilib/opmoderobot.py delete mode 100644 wpilibc/src/main/python/wpilib/src/rpy/ControlWord.cpp delete mode 100644 wpilibc/src/main/python/wpilib/src/rpy/ControlWord.h create mode 100644 wpilibc/src/test/native/cpp/OpModeRobotTest.cpp create mode 100644 wpilibcExamples/src/main/cpp/templates/opmode/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/templates/opmode/cpp/opmode/MyAuto.cpp create mode 100644 wpilibcExamples/src/main/cpp/templates/opmode/cpp/opmode/MyTeleop.cpp create mode 100644 wpilibcExamples/src/main/cpp/templates/opmode/include/Robot.hpp create mode 100644 wpilibcExamples/src/main/cpp/templates/opmode/include/opmode/MyAuto.hpp create mode 100644 wpilibcExamples/src/main/cpp/templates/opmode/include/opmode/MyTeleop.hpp delete mode 100644 wpilibj/src/main/java/org/wpilib/driverstation/DSControlWord.java create mode 100644 wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java delete mode 100644 wpilibj/src/main/java/org/wpilib/framework/RobotState.java create mode 100644 wpilibj/src/main/java/org/wpilib/opmode/Autonomous.java create mode 100644 wpilibj/src/main/java/org/wpilib/opmode/LinearOpMode.java create mode 100644 wpilibj/src/main/java/org/wpilib/opmode/OpMode.java create mode 100644 wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java create mode 100644 wpilibj/src/main/java/org/wpilib/opmode/Teleop.java create mode 100644 wpilibj/src/main/java/org/wpilib/opmode/TestOpMode.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/opmode/Main.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/opmode/Robot.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyAuto.java create mode 100644 wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyTeleop.java diff --git a/commandsv2/src/main/java/org/wpilib/command2/CommandScheduler.java b/commandsv2/src/main/java/org/wpilib/command2/CommandScheduler.java index c87db7e080..699811a2d6 100644 --- a/commandsv2/src/main/java/org/wpilib/command2/CommandScheduler.java +++ b/commandsv2/src/main/java/org/wpilib/command2/CommandScheduler.java @@ -26,7 +26,6 @@ import org.wpilib.command2.Command.InterruptionBehavior; import org.wpilib.driverstation.DriverStation; import org.wpilib.event.EventLoop; import org.wpilib.framework.RobotBase; -import org.wpilib.framework.RobotState; import org.wpilib.framework.TimedRobot; import org.wpilib.hardware.hal.HAL; import org.wpilib.system.Watchdog; @@ -190,7 +189,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable { // run when disabled, or the command is already scheduled. if (m_disabled || isScheduled(command) - || RobotState.isDisabled() && !command.runsWhenDisabled()) { + || DriverStation.isDisabled() && !command.runsWhenDisabled()) { return; } @@ -270,7 +269,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable { m_watchdog.addEpoch("buttons.run()"); m_inRunLoop = true; - boolean isDisabled = RobotState.isDisabled(); + boolean isDisabled = DriverStation.isDisabled(); // Run scheduled commands, remove finished commands. for (Iterator iterator = m_scheduledCommands.iterator(); iterator.hasNext(); ) { Command command = iterator.next(); diff --git a/commandsv2/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/commandsv2/src/main/native/cpp/frc2/command/CommandScheduler.cpp index 348a27d1d0..9c9a8bdb25 100644 --- a/commandsv2/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/commandsv2/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -12,8 +12,8 @@ #include "wpi/commands2/CommandPtr.hpp" #include "wpi/commands2/Subsystem.hpp" +#include "wpi/driverstation/DriverStation.hpp" #include "wpi/framework/RobotBase.hpp" -#include "wpi/framework/RobotState.hpp" #include "wpi/framework/TimedRobot.hpp" #include "wpi/hal/HALBase.h" #include "wpi/hal/UsageReporting.h" @@ -102,7 +102,7 @@ void CommandScheduler::Schedule(Command* command) { RequireUngrouped(command); if (m_impl->disabled || m_impl->scheduledCommands.contains(command) || - (wpi::RobotState::IsDisabled() && !command->RunsWhenDisabled())) { + (wpi::DriverStation::IsDisabled() && !command->RunsWhenDisabled())) { return; } @@ -184,7 +184,7 @@ void CommandScheduler::Run() { loopCache->Poll(); m_watchdog.AddEpoch("buttons.Run()"); - bool isDisabled = wpi::RobotState::IsDisabled(); + bool isDisabled = wpi::DriverStation::IsDisabled(); // create a new set to avoid iterator invalidation. for (Command* command : wpi::util::SmallSet(m_impl->scheduledCommands)) { if (!IsScheduled(command)) { diff --git a/commandsv2/src/test/java/org/wpilib/MockHardwareExtension.java b/commandsv2/src/test/java/org/wpilib/MockHardwareExtension.java index 0c277ce33b..0918f18729 100644 --- a/commandsv2/src/test/java/org/wpilib/MockHardwareExtension.java +++ b/commandsv2/src/test/java/org/wpilib/MockHardwareExtension.java @@ -8,6 +8,7 @@ import org.junit.jupiter.api.extension.BeforeAllCallback; import org.junit.jupiter.api.extension.ExtensionContext; import org.junit.jupiter.api.extension.ExtensionContext.Namespace; import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.simulation.DriverStationSim; public final class MockHardwareExtension implements BeforeAllCallback { @@ -31,9 +32,8 @@ public final class MockHardwareExtension implements BeforeAllCallback { private void initializeHardware() { HAL.initialize(500, 0); DriverStationSim.setDsAttached(true); - DriverStationSim.setAutonomous(false); DriverStationSim.setEnabled(true); - DriverStationSim.setTest(true); + DriverStationSim.setRobotMode(RobotMode.TEST); DriverStationSim.notifyNewData(); } } diff --git a/commandsv2/src/test/java/org/wpilib/command2/button/RobotModeTriggersTest.java b/commandsv2/src/test/java/org/wpilib/command2/button/RobotModeTriggersTest.java index f072e18a7d..faa0909548 100644 --- a/commandsv2/src/test/java/org/wpilib/command2/button/RobotModeTriggersTest.java +++ b/commandsv2/src/test/java/org/wpilib/command2/button/RobotModeTriggersTest.java @@ -8,14 +8,14 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; import org.wpilib.command2.CommandTestBase; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.simulation.DriverStationSim; class RobotModeTriggersTest extends CommandTestBase { @Test void autonomousTest() { DriverStationSim.resetData(); - DriverStationSim.setAutonomous(true); - DriverStationSim.setTest(false); + DriverStationSim.setRobotMode(RobotMode.AUTONOMOUS); DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData(); Trigger auto = RobotModeTriggers.autonomous(); @@ -25,8 +25,7 @@ class RobotModeTriggersTest extends CommandTestBase { @Test void teleopTest() { DriverStationSim.resetData(); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(false); + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData(); Trigger teleop = RobotModeTriggers.teleop(); @@ -36,8 +35,7 @@ class RobotModeTriggersTest extends CommandTestBase { @Test void testModeTest() { DriverStationSim.resetData(); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(true); + DriverStationSim.setRobotMode(RobotMode.TEST); DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData(); Trigger test = RobotModeTriggers.test(); @@ -47,8 +45,6 @@ class RobotModeTriggersTest extends CommandTestBase { @Test void disabledTest() { DriverStationSim.resetData(); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(false); DriverStationSim.setEnabled(false); DriverStationSim.notifyNewData(); Trigger disabled = RobotModeTriggers.disabled(); diff --git a/commandsv2/src/test/native/cpp/wpi/command/button/RobotModeTriggersTest.cpp b/commandsv2/src/test/native/cpp/wpi/command/button/RobotModeTriggersTest.cpp index d58d945b52..3035e83257 100644 --- a/commandsv2/src/test/native/cpp/wpi/command/button/RobotModeTriggersTest.cpp +++ b/commandsv2/src/test/native/cpp/wpi/command/button/RobotModeTriggersTest.cpp @@ -7,6 +7,7 @@ #include "../CommandTestBase.hpp" #include "wpi/commands2/button/Trigger.hpp" #include "wpi/driverstation/DriverStation.hpp" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/simulation/DriverStationSim.hpp" using namespace wpi::cmd; @@ -15,8 +16,7 @@ class RobotModeTriggersTest : public CommandTestBase {}; TEST(RobotModeTriggersTest, Autonomous) { DriverStationSim::ResetData(); - DriverStationSim::SetAutonomous(true); - DriverStationSim::SetTest(false); + DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS); DriverStationSim::SetEnabled(true); DriverStationSim::NotifyNewData(); Trigger autonomous = RobotModeTriggers::Autonomous(); @@ -25,8 +25,7 @@ TEST(RobotModeTriggersTest, Autonomous) { TEST(RobotModeTriggersTest, Teleop) { DriverStationSim::ResetData(); - DriverStationSim::SetAutonomous(false); - DriverStationSim::SetTest(false); + DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED); DriverStationSim::SetEnabled(true); DriverStationSim::NotifyNewData(); Trigger teleop = RobotModeTriggers::Teleop(); @@ -35,8 +34,6 @@ TEST(RobotModeTriggersTest, Teleop) { TEST(RobotModeTriggersTest, Disabled) { DriverStationSim::ResetData(); - DriverStationSim::SetAutonomous(false); - DriverStationSim::SetTest(false); DriverStationSim::SetEnabled(false); DriverStationSim::NotifyNewData(); Trigger disabled = RobotModeTriggers::Disabled(); @@ -45,8 +42,7 @@ TEST(RobotModeTriggersTest, Disabled) { TEST(RobotModeTriggersTest, TestMode) { DriverStationSim::ResetData(); - DriverStationSim::SetAutonomous(false); - DriverStationSim::SetTest(true); + DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST); DriverStationSim::SetEnabled(true); DriverStationSim::NotifyNewData(); Trigger test = RobotModeTriggers::Test(); diff --git a/design-docs/opmodes.md b/design-docs/opmodes.md new file mode 100644 index 0000000000..92c6e2ae5a --- /dev/null +++ b/design-docs/opmodes.md @@ -0,0 +1,534 @@ +# Summary + +This document describes a standardized approach for operators to select different code to run for different robot modes of operation and for programmers to easily write code that creates these selection options. + +A note on terminology: the word "opmode" is used in this document to describe this functionality. + +# Motivation + +Operator selection of different code implementing unique top-level robot behavior–without recompilation of the robot program–is a very common need across most FTC and FRC teams, so it’s desirable to have a standardized approach for cleanly structuring robot code to support this, along with integrated support for selection at the Driver Station. + +Primary use cases for operator-selectable code include: + +- Multiple autonomous routines (e.g. following different paths, performing different actions) + +- Different teleoperated behavior (e.g. tank vs arcade drive, different button mappings, operating restrictions for robot demonstrations/guest drivers) + +- Testing (e.g. testing of the whole robot or a single subsystem, sensor, or motor) + +Notably these use cases span both matches and off-field testing. + +Secondarily, some teams may want to use different code structures for different use cases (e.g. linear for autonomous and iterative for test or teleop). + +# Background + +Both FTC and FRC historically have had at least some level of support for operator selection of code, but the approaches used and resulting functionality and behaviors are quite different. To provide context and background driving the design decisions in this document, a summary of these behaviors (as of 2025) is provided in this section. + +## FRC + +### Core concepts + +- The robot has 3 fixed system-level modes of operation: autonomous, teleoperated, and test +- The robot is in either enabled (actuators enabled) or disabled (actuators disabled) state +- In competition matches, the Field Management System (FMS) automatically transitions between autonomous, disabled, and teleoperated modes +- Robot code is structured around a top-level Robot class that instantiates the hardware objects as member variables (often organized into subsystem objects stored as members) and functions that run in each robot mode; the library robot base class handles calling these functions as directed by the DS +- Selection of different user code routines within these system-level modes (e.g. multiple autonomous routines) is usually performed using a separate dashboard application +- In robot code, the selection list is generated by user code instantiating a utility class called `SendableChooser`, adding options to it with function calls during initialization, and calling a function to get the selected option at the appropriate time + +### Driver Station + +When not connected to the Field Management System (FMS), the [FRC driver station application](https://docs.wpilib.org/en/stable/docs/software/driverstation/driver-station.html) provides the operator a fixed selection of 4 different modes of robot operation (teleoperated, autonomous, test, and practice) and buttons to enable and disable the robot. In three of these modes (teleoperated, autonomous, and test), selecting a mode and enabling the robot immediately starts code execution for that mode for an unlimited amount of time (until the user disables the robot). The "practice" mode provides for off-field testing of match behavior by mimicking match behavior (automatically transitioning disabled->auto->disabled->teleop->disabled with pre-configured durations for each step). + +![Driver Station GUI](https://docs.wpilib.org/en/stable/_images/ds-operation-tab.webp) + +All actuators are disabled at all times the robot is disabled. The robot starts in disabled state (prior to receiving driver station packets) and the only way to enable the robot is via the driver station. Any loss of communication results in the robot transitioning back to disabled state. Similarly, the driver station starts in disabled state and automatically transitions to disabled state when communication is lost. + +In autonomous mode, joystick inputs from the Driver Station are not passed to the robot; values are latched at the last state before entering autonomous mode. Joystick inputs are sent to the robot in all other modes, including disabled. + +When a match starts, the FMS commands the driver station to enable the robot in autonomous mode for the autonomous period, followed by a short time disabled (typically 1-3 seconds), followed by enabling the robot in teleoperated mode for the teleoperated period, followed by disable. + +Separate from the driver station application, WPILib-provided as well as custom dashboard applications are commonly utilized by teams to provide a drop-down list for selection of autonomous routines. Operators select the desired autonomous routine via the dashboard prior to the start of a match (or off-field testing of autonomous). Some teams use other methods to select different autonomous routines (e.g. physical jumpers on the robot, using joystick inputs while the robot is disabled), but the dashboard method is the standard approach. + +### Robot Code + +The standard WPILib team code structure derives a single team-written `Robot` class from an "periodic" robot base class. This `Robot` class is constructed and run by the Java `main()` function. After construction, the base class implementation runs a periodic loop (typically running on a 20 ms period, although that can be changed by the user) that reads the enabled state and teleop/auto/test mode provided by the DS and calls overrideable functions for each mode (disabled, teleop, auto, and test), as well as a `robotPeriodic` overrideable function that is always called regardless of mode. Three overrideable functions are provided for each mode: an init function (called when the mode is transitioned into from another mode), a periodic function (called on a fixed period), and an exit function (called when the mode is transitioned out of into another mode). Teams override these functions in their Robot derived class to implement their robot code. Because of this code structure, teams generally create their hardware configuration (can be flat or organized into subsystems) and other objects as member variables within the Robot class (constructed at robot start) and share them across all modes of operation. + +WPILib provides a class called `SendableChooser` for creating the drop-down lists shown on the dashboard. This class is a generic/template class that provides a map of string key (shown on the drop-down) and object value (read by robot code). This is typically displayed by dashboards as a simple list with no categorization. This feature is most often used by teams for autonomous routine selection, but is not limited to that use case. Examples and templates show teams how to instantiate this class, add it to the dashboard, and use it for operator selection of different autonomous routines (by reading the chosen value from the object and executing different code). Examples of how to use `SendableChooser` for operator selection in other modes (e.g. teleop or test) is not provided, and it's generally uncommon for teams to use it in that way–most team code has just a single teleop routine, and the test mode is rarely used (manual testing code is instead usually integrated as part of the teleop routine). + +Historically, WPILib offered a "simple" (later renamed to "sample") robot base class that had single overrideable functions for teleop, auto, and test, with no outer periodic loop (the user was responsible for writing the loop). This was deprecated and removed circa 2016 as it was common to see teams writing autonomous loops or sleeps without proper exit condition checking (e.g. wait for the robot to drive X feet), resulting in the robot code never exiting autonomous and thus not transitioning to teleoperated in matches. After removal of this template, this relatively common issue has entirely disappeared. + +## FTC + +### Core concepts + +- Robot programs are structured around the concept of "opmodes"; there are two *types* of opmodes: teleoperated and autonomous +- In competition matches, operators manually operate the DS to start the autonomous and teleoperated periods (there is no FMS) +- Selection of opmodes is fully integrated into the DS; an opmode must be selected before Init is pressed +- The DS has a three-step manual process for running robot code: select the opmode, press Init (robot enabled, opmode constructed, opmode `init()` called), press Play (opmode `run()` or equivalent called) +- In robot code, opmodes are represented by users creating individual annotated classes; a library opmode manager handles generation of the opmode selection list (by scanning for annotated classes) and instantiating and calling class functions as directed by the DS +- Hardware objects are created by the library based on an XML file; opmodes read this map to get access to the objects + +### Driver Station + +Opmodes are presented in the DS as a selectable list containing both auto and teleop opmodes. Opmode selection on the DS is just UI candy; the robot knows nothing about what the operator has done until Init is pressed, at which point the opmode is communicated to the robot and the robot is enabled. If an auto op mode is selected, an additional button appears that allows the operator to pre-select a teleop op mode. This eases the match transition between auto and teleop, but it's still necessary for the operator to manually press Init and Play buttons to start the teleop mode. + +![FTC DS OpMode Selection Screen](https://github.com/FIRST-Tech-Challenge/FtcRobotController/wiki/images/Automatically-Loading-a-Driver-Controlled-Op-Mode/selectTeleOp.png) + +The robot actuators are enabled as soon as the Init button is pressed. Pressing the Play button starts OpMode execution. Pressing the Stop button (shown after Init is pressed) disables the robot. + +Auto modes by default stop/disable after 30 seconds, but this can be turned off for non-competition use. + +### Robot Code + +FTC robot programs are structured around the concept of "opmodes." Opmodes are user classes that are registered as either teleoperated or autonomous opmodes using Java annotations. Several different opmode base class options are available, including a "linear" opmode that provides a single function (and the user is responsible for the loop) and an "iterative" opmode that calls a single function periodically. + +The library contains an opmode manager that is responsible for registering and switching between opmodes based on operator input. The switching is generally done cooperatively, but if an opmode fails to return within a reasonable amount of time, several increasing steps are taken to try to get it to exit; if ultimately the opmode does not exit, the entire robot executable is terminated (and automatically restarted). + +Opmode classes are constructed when the user taps the Init button in the DS. This works because hardware initialization processes never take more than a few seconds, and there's always a human lag between tapping Init and tapping Start. + +Hardware configuration is stored in an XML file that is consumed on APK startup, or when a different configuration is selected by the operator. Consuming the XML file instantiates all objects (sensors, motors, servos, etc...) and stores them in a dictionary. This dictionary then persists until either the robot is power cycled, or the operator selects a different configuration. User code pulls items out of the dictionary, typically in an initialization portion of an opmode, and then use throughout the opmode. Notably, when the opmode finishes, or is stopped, the dictionary persists. + +As opmodes are separate classes and there is no top-level robot class in the standard template, sharing state between opmodes is often a challenge for teams. + +The Java annotation-based registration approach allows for specifying a string name for the opmode (displayed on the DS); this is automatically generated from the class name if none is provided. The annotation also allows specifying a string group name; this is used to provide display grouping of the opmodes at the DS. Two annotations are used to separately register autonomous and teleop opmodes to filter the displayed lists on the DS for each mode. Additionally, opmodes may be disabled through the use of an additional `@Disabled` annotation; these are hidden from the DS displayed list. + +# Requirements and Desirable Features + +For competition, matches usually consist of an autonomous period followed by a teleoperated period. FTC and FRC currently have different amounts of operator interaction. In FRC, the time period between auto and teleop is very short (can be <1 second some years), and so it's a requirement for FRC that both the auto and teleop opmodes be able to be pre-selected by the operator prior to the start of the match, and for the DS to handle automatically transitioning between these opmodes. In FTC, there is currently no FMS, and so a longer delay between auto and teleop is common to allow teams to pick up controllers, but pre-selecting a teleop opmode enables a more efficient transition to teleop, so having this feature benefits FTC use cases as well. + +It is not a requirement to support different programming languages for different operator-selectable opmodes (e.g. auto in Python, teleop in Java). Supporting this would require completely terminating the robot executable and starting a new robot executable (and re-initializing all the hardware), which can take a substantial and variable amount of time to complete. This is very undesirable for FRC due to the short transition from auto to teleop; although it's less problematic for FTC, it makes sharing state between opmodes essentially impossible, and sharing hardware configurations is also difficult. + +Similarly, it is not a requirement to support running the same robot project on entirely different physical robots. That situation is better handled through entirely separate robot projects that define the unique hardware configuration. + +A clear enable/disable in the Driver Station that disables all robot actuators when disabled is a requirement because it is a safety-critical feature for FRC due to the size and power of FRC robots. + +The enabled "Init" step in the 2025 FTC SDK/DS is not a requirement due to anticipated rule changes. However, performing initialization of opmodes while the robot is still disabled is a requirement for both FTC and FRC, as it's important for user code to be able to do expensive opmode-specific operations (e.g. computing autonomous paths) prior to actually starting the match. + +Providing the top-level teleop, autonomous, and test selection available in the 2025 FRC driver station is desirable and has few downsides for either program. In combination with code specifying the applicable opmodes (also desirable), this allows for filtering down of selectable opmodes. The "practice" mode feature of the 2025 FRC driver station which automatically transitions between modes based on time is a very useful tool for teams to simulate match transitions at home and should be retained, but should be renamed to reduce confusion, particularly as this mode is likely to be used for FTC matches without a FMS, "match" mode could be a good name for this feature. + +# Design + +## Overview / Key Features + +- DS provides a prominent enable/disable state control and a teleop/auto/test/match selector. For teleop/auto/test, enabling the robot immediately starts execution of the selected opmode; in match mode, a match sequence is followed (auto mode followed by a disabled delay, followed by teleop mode). + +- DS provides drop-down selector(s) for the user-defined opmodes; these are filtered based on the top-level mode selector (e.g. if auto is selected, a single drop-down selector of just the auto opmodes is provided). For match mode or when FMS-attached, two drop downs are provided (one for auto opmode selection and one for teleop opmode selection). The drop-down selector provides grouped categories as specified by the robot program. + +- Robot programs are structured to have a top-level Robot class (e.g. motors/sensors/subsystems); this is also where robot-wide initialization is performed (in the class constructor or static initializer block). The Robot class provides overrideable periodic type functions for users to run code when the robot is disabled. + +- OpModes are usually registered via annotation of classes. These annotations may specify a name (or default to the class name), group name (for grouping of routines in the selection list), and description. Annotations are used to specify whether the class is a teleop, auto, or test opmode. + +- OpModes may also be registered via annotation of functions (in the Robot class only) or via explicit function calls. As C++ does not support annotations, function call registration is the only available method in that language. If manually registered, `publishOpModes()` must be called to publish the list of opmodes to the DS. + +- For maximum flexibility, all code in the robot project has access to the enable/disable state, the overall robot teleop/auto/test mode, and the selected opmodes for teleop, auto, and test (even when the robot is disabled). + +- OpModes may be periodic or linear (or custom). The Robot base class handles switching between opmodes and opmode object instance creation. OpMode objects are constructed when the drop-down selection is made in the DS and run when the robot is enabled. + +How opmodes work with the command-based framework is described in [a separate design document](opmodes-commandbased.md). + +## Driver Station + +For reference, the FRC driver station is shown below. This illustrates the enable/disable control (2) as well as the teleop/auto/practice/test selector (1). For this design, "practice" is renamed to "match" for clarity. + +![Driver Station GUI](https://docs.wpilib.org/en/stable/_images/ds-operation-tab.webp) + +One or two drop-down selectors are added to this main screen. One selector is displayed in teleop, auto, and test modes; two selectors (one for auto and one for teleop) is displayed in match mode (either manually selected by clicking "match," or when the DS is FMS-attached). + +The drop-down lists have the opmodes (as defined by user code) organized into groups, similar to the below. The groups and opmodes are sorted. + +![Combo box with groups](https://i.sstatic.net/4Cmfk.gif) + +Only the opmodes appropriate to the selected mode are shown in the corresponding list. For example, if the "Test" option is selected, a single drop-down list with only test opmodes listed will be shown. + +## Robot Code User-Facing API + +Java is used for illustrative purposes. Python and C++ should follow a similar structure, except that C++ doesn't support annotations. + +### OpModeRobot + +The `OpModeRobot` class is the base class for the user's `Robot` class. It also implements the private library machinery for robot startup and robot execution (including creating and transitioning between opmodes in accordance with the opmode lifecycle, as described in the following section). + +```java +public abstract class OpModeRobot { + public void nonePeriodic() { + // this code is called when no opmode is selected + } + + // these functions allow users to add opmodes without annotations + public void addAutonomousOpMode(Supplier factory, String name, String group, String description) {...} + public void addTeleoperatedOpMode(Supplier factory, String name, String group, String description) {...} + public void addTestOpMode(Supplier factory, String name, String group, String description) {...} +} +``` + +### OpMode Classes + +There are library base classes for different coding styles of routines. The `OpMode` interface serves as the base interface for all opmodes. Most users will use either `LinearOpMode` or `PeriodicOpMode` abstract base class implementations of this interface, but this interface enables users to create more customized opmodes while still utilizing the core robot base class opmode-switching implementation. + +The full lifecycle of a opmode is as follows: +- Operator selects opmode on DS -> opmode object is constructed +- If different opmode is selected -> `opModeClose()` is called (which typically just calls `close()`), object is released to GC +- While opmode is selected and robot is disabled, `disabledPeriodic()` is called +- When the robot is enabled in the selected opmode, `opModeRun()` is called; the result of this is different for different opmode base classes: + - `start()` is called once (for `PeriodicOpMode`) + - `run()` is called (for `LinearOpMode`), or `periodic()` is called periodically (for `PeriodicOpMode`) +- When the robot is disabled, `opModeStop()` is called (which results in `end()` being called for `PeriodicOpMode`), followed by `opModeClose()` (which results in `close()` being called for both `PeriodicOpMode` and `LinearOpMode`), object is released to GC + +Following `opModeClose()` being called, a *new* opmode object is constructed based on the DS teleop/auto/test/match selector and selected opmode. In teleop/auto/test, the drop-down selection will be the same as before the previous enable, so the same opmode class is constructed again. In match (or when FMS-connected), only the selected auto opmode object is initially constructed; once auto completes, the selected teleop opmode object is constructed. Thus only zero or one opmode objects will ever be "alive" at any given time. + +For consistency in operation, the library will ensure that `disabledPeriodic()` is always called at least once before `opModeRun()` is called. + +User implementations of opmode classes may have either a no-parameter constructor or a constructor that accepts the user's `Robot` class type. If available, the library will call the latter and pass the user's `Robot` object to it when constructing the class. + +The library will use escalating steps to attempt to terminate a opmode if `opModeRun()` does not return within a reasonable timeframe after `opModeStop()` is called, up to and including termination of the robot executable process (which will result in an automatic restart of it at the system level). User code (particularly in `LinearOpMode` derived classes) should check for `isRunning()` returning false and return as quickly as possible to allow the opmode to terminate gracefully. + +```java +public interface OpMode { + // this function is called periodically while the opmode is selected on the DS (robot is disabled) + // Note: it may be called once when the robot is enabled to ensure it's always called once before + // opModeRun() is called + default void disabledPeriodic() {} + + // this function is called when the opmode starts (robot is enabled) + void opModeRun(long opModeId) throws InterruptedException; + + // this function is called asynchronously when the robot is disabled, + // to request the opmode return from opModeRun() + void opModeStop(); + + // this function is called when the opmode is de-selected on the DS or after + // opModeRun() returns + void opModeClose(); +} +``` + +```java +public abstract class LinearOpMode implements OpMode { + // the class is constructed when the opmode is selected on the DS + + @Override + public void disabledPeriodic() { + // this code is called periodically while the opmode is selected on the DS (robot is disabled) + } + + public void close() { + // this code is called when the opmode is de-selected on the DS + } + + // this function is called once to run the opmode (robot is enabled) + public abstract void run() throws InterruptedException: + + public boolean isRunning() { + // returns true until opModeStop() is called + } + + // implements OpMode interface + @Override + public final void opModeRun(long opModeId) { + run(); + } + + @Override + public final void opModeStop() { + // pseudo-code + isRunning = false; + } + + @Override + public final void opModeClose() { + close(); + } +} +``` + +```java +public abstract class PeriodicOpMode implements OpMode { + // the class is constructed when the opmode is selected on the DS + + // periodic opmodes may specify their period; if unspecified, a default period of 20 ms is used + protected PeriodicOpMode() {...} + protected PeriodicOpMode(double period) {...} + + @Override + public void disabledPeriodic() { + // this code is called periodically while the opmode is selected on the DS (robot is disabled) + } + + public void close() { + // this code is called when the opmode is de-selected on the DS + } + + public void start() { + // this code is called when the opmode starts (robot is enabled) + } + + // this function is called periodically while the opmode is running (robot is enabled) + public abstract void periodic(); + + public void end() { + // this code is called when the opmode ends (robot is disabled) + } + + // additional periodic functions can be added using these functions + public final void addPeriodic(Runnable callback, double period) {...} + public final void addPeriodic(Runnable callback, double period, double offset) {...} + + // returns the start time of the current loop in microseconds + public final long getLoopStartTime() {...} + + // implements OpMode interface + @Override + public final void opModeRun(long opModeId) { + // psuedo-code + start(); + while (isRunning) { + // wait for next periodic time + // set loop start time + periodic(); // or addPeriodic() callback, as appropriate + } + end(); + } + + @Override + public final void opModeStop() { + // pseudo-code + isRunning = false; + } + + @Override + public final void opModeClose() { + close(); + } +} +``` + +### Annotations + +All annotations are class-level. All elements are optional and may be omitted. If name is omitted, the class name is used. If group is emitted, the opmode is listed under a default group in the DS. The description is blank if it is omitted. + +```java +@Autonomous(String name, String group, String description) +@Teleop(String name, String group, String description) +@TestOpMode(String name, String group, String description) +``` + +Example use cases: + +```java +// name will be "MyAuto", default group +@Autonomous +public class MyAuto extends OpModeBaseClass {...} + +// will use default group +@Teleop("my teleop") +public class MyTeleop extends OpModeBaseClass {...} + +@TestOpMode(name="my test", group="mechanisms", description="tests arm") +public class MyTest extends OpModeBaseClass {...} +``` + +### DriverStation class + +Note: the DriverStation class contains many other functions; only the opmode-relevant functions are shown here. + +```java +public final class DriverStation { + // these return the enabled state; + // the robot is always disabled when the DS is not connected + public static boolean isEnabled() {...} + public static boolean isDisabled() {...} + + // these return the overall robot mode; + // they return false when the robot is disabled + public static boolean isAutonomous() {...} + public static boolean isTeleoperated() {...} + public static boolean isTest() {...} + + // returns the currently selected opmode for the currently selected robot mode; + // this works even when the robot is disabled + public static String getOpMode() {...} + // returns the opmode ID instead of the the string name + public static long getOpModeId() {...} + + // returns true if the opmode is the provided ID/name + public static boolean isOpMode(long id) {...} + public static boolean isOpMode(String name) {...} + + // add/remove opmodes + public static long addOpMode(RobotMode mode, String name, String group, String description, Color textColor, Color backgroundColor) {...} + public static long removeOpMode(RobotMode mode, String name) {...} + // publish the list of opmodes to the DS + public static void publishOpModes() {...} + // clear the list of opmodes + public static void clearOpModes() {...} +} +``` + +## Java Robot Code Examples + +Non-command-based robots will typically use classes to define opmodes. + +The following example code for non-command-based Java demonstrates the following: +- Robot class +- A periodic autonomous opmode +- A periodic teleop opmode +- A linear test opmode + +Robot: + +```java +public class Robot extends OpModeRobot { + public final DifferentialDrive drive = new DifferentialDrive(...); + + public Robot() {} +} +``` + +Autonomous opmode: + +```java +@Autonomous(name="Drive straight", group="Drive") +public class AutoDriveStraight extends PeriodicOpMode { + private final Robot robot; + private final Timer timer = new Timer(); + + public AutoDriveStraight(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + timer.start(); + } + + @Override + public void periodic() { + // drive forward for 2 seconds at half speed, then stop + if (!timer.hasElapsed(2.0)) { + robot.drive.arcadeDrive(0.5, 0); + } else { + robot.drive.arcadeDrive(0, 0); + } + } +} +``` + +Teleop opmode: + +```java +@Teleop +public class Teleop extends PeriodicOpMode { + private final Joystick joy = new Joystick(1); + private final Robot robot; + + public Teleop(Robot robot) { + this.robot = robot; + } + + @Override + public void periodic() { + robot.drive.arcadeDrive(joy.getY(), joy.getX()); + } +} +``` + +Test opmode: + +```java +@TestOpMode("Blink dashboard indicator") +public class TestDashboardIndicator extends LinearOpMode { + @Override + public void run() { + Telemetry.log("indicator", true); + Timer.sleep(0.5); + Telemetry.log("indicator", false); + } +} +``` + +## C++ Robot Code + +C++ does not support annotations, so explicit registration via `OpModeRobot` function calls is required. + +## Python Robot Code + +Should be able to be annotation (decorator) based, similar to Java. + +## HAL + +At the HAL level, Control Word bits indicate enabled state and teleop/auto/test selection (identical to current WPILib). The Control Word needs to be lengthened to 64-bit to indicate the selected opmode, and functions to maintain the list of available opmodes need to be added. An "opmode ID" is the combination of the opmode hash and the robot mode (e.g. teleop/auto/test). + +To communicate the selected opmode ID, `HAL_ControlWord` needs to become a 64-bit type; changing it from a bitfield to a type with helper functions also will improve the ease of use: +- `HAL_ControlWord HAL_MakeControlWord(int64_t opModeHash, HAL_RobotMode robotMode, HAL_Bool enabled, HAL_Bool eStop, HAL_Bool fmsAttached, HAL_Bool dsAttached)` - builds a control word from its parts +- `int64_t HAL_ControlWord_GetOpModeHash(HAL_ControlWord word)` - gets the opmode hash from the control word +- `int64_t HAL_ControlWord_GetOpModeId(HAL_ControlWord word)` - gets the opmode ID from the control word (combination of hash and robot mode); 0 is returned if the hash is 0 +- `void HAL_ControlWord_SetOpModeId(HAL_ControlWord* word, int64_t id)` - sets the opmode ID portion of a control word +- `HAL_RobotMode HAL_ControlWord_GetRobotMode(HAL_ControlWord word)` - gets the robot mode from the control word +- `HAL_Bool HAL_ControlWord_IsEnabled(HAL_ControlWord word)` - returns true if the control word indicates the robot is enabled +- `HAL_Bool HAL_ControlWord_IsEStopped(HAL_ControlWord word)` - returns true if the control word indicates the robot is estopped +- `HAL_Bool HAL_ControlWord_IsFMSAttached(HAL_ControlWord word)` - returns true if the control word indicates the robot is attached to the FMS +- `HAL_Bool HAL_ControlWord_IsDSAttached(HAL_ControlWord word)` - returns true if the control word indicates the robot is attached to the DS +- `int64_t HAL_MakeOpModeId(HAL_RobotMode mode, int64_t hash)` - makes an opmode ID from a robot mode and an opmode hash +- `HAL_RobotMode HAL_OpMode_GetRobotMode(int64_t id)` - gets the robot mode portion of an opmode ID +- `int64_t HAL_OpMode_GetHash(int64_t id)` - gets the opmode hash portion of an opmode ID + +Adding the following function enables maintaining the available opmode options list: +- `void HAL_SetOpModeOptions(HAL_OpModeOption* array, int count)` – sets the list of available opmode options + +`HAL_OpModeOption` is a structure describing each option: +- `long id` - unique ID identifying the opmode (robot mode, name) pair. This encodes the robot mode in the upper bits, indicating which robot mode the opmode should be visible for (auto/teleop/test) +- `string name` +- `string group` +- `string description` +- `int textColor` - optional, used to set the text color for the option in the DS GUI +- `int backgroundColor` - optional, used to set the text color for the option in the DS GUI + +The user program observe functions also need to be updated: +- Replace `HAL_ObserveUserProgramDisabled()`, `HAL_ObserveUserProgramAutonomous()`, `HAL_ObserveUserProgramTeleop()`, and `HAL_ObserveUserProgramTest()` with `HAL_ObserveUserProgram(HAL_ControlWord word)` + +## DS Protocol + +The list of opmodes (and details thereof) is communicated from the Robot to the DS via the tagged TCP link. + +As in the current FRC protocol, the enabled state and teleop/auto/test selection are sent as part of the UDP control word from the DS to the Robot. Additionally, the selected opmode is sent as part of every UDP packet. This is done via a 56-bit hash of each opmode's name. Hash collisions are extremely low probability given the low quantity of opmodes, but this can be checked on the robot side when generating the opmode list, and spaces appended as necessary to the provided names to uniquify the hashes. The selected opmode is sent even when the robot is disabled. + +# Migration from 2025 FRC (WPILib) + +Key differences from 2025 FRC: +- The per-opmode overrideable functions in Robot are replaced with separate annotated per-opmode classes (for non-command-based) +- `SendableChooser` is no longer required for selecting autonomous routines; instead this functionality is integrated into opmodes, as users can create/register multiple autonomous opmodes classes, and it's been extended to support multiple teleoperated and multiple test opmodes +- Selection of autonomous opmodes is integrated into the DS instead of being performed by the dashboard +- OpModes are no longer limited to just timed (periodic); a mix of different opmode functionality/approaches across different robot opmodes is possible (e.g. teleop can be periodic and autonomous linear) +- Linear opmodes are now available; these are improved versions of the old SimpleRobot/SampleRobot + +# Migration from 2025 FTC (FTC SDK) + +Key differences from 2025 FTC: +- There is no hardware map. Hardware objects are instead directly instantiated inside a top-level Robot class. This Robot class is provided as a parameter to the opmode constructor. +- Init is integrated into the opmode constructor. There is no equivalent to the the enabled init step; the opmode constructor is run as soon as the opmode is selected on the DS, while the robot is still disabled. Match periods will start as soon as the robot is enabled. +- There is no `@Disabled` annotation for opmodes; the opmode annotation can simply be commented out to achieve the same result. + +# Drawbacks + +# Alternatives + +Use SendableChooser for more opmodes (teleop and test as well as autonomous); downsides of this: +- Doesn't allow flexibility of mixing linear and periodic opmodes +- Harder to use than annotations (generic class) +- No grouping, no filtering by opmode (although these could be added) + +# Trades + +- Overall naming: mode? routine? opmode? + +- Naming of opmode functions? start-periodic-end, vs init-periodic-exit (2025 FRC PeriodicRobot), vs init-execute-end (2025 FRC Command), vs init-start-loop-stop (2025 FTC OpMode; note init behaves like the constructor here) + +- For matches, should we construct teleop at the same time as auto? If we do that, we probably need a disabledStart() or 2025 FTC opmode style init() that's run on teleop after auto completes, and don't run disabledPeriodic for both simultaneously. + +# Unresolved Questions + +- FRC SendableChooser has a "default" option set by robot code. Do we want something similar here or should it be 100% DS driven? It's kind of nice to be able to set a default (e.g. via a `setDefaultAutonomousOpMode(String)` function in `Robot`), but also might be a little fragile since it's name based. If it's done via annotation, what happens if multiple annotations are marked as default? + +- Should it be possible to have multiple top-level Robot classes (e.g. for different robot configurations), and have that be selectable at the DS as well? This is a bit ugly to support even if the Robot object is passed into the opmode constructor, because different Robot class types will only work with certain opmodes. + +- Python--will decorators be able to work similarly to Java annotations for opmode registration? diff --git a/docs/build.gradle b/docs/build.gradle index 00bd4eaa33..95551ea169 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -164,6 +164,7 @@ task generateJavaDocs(type: Javadoc) { "-org.wpilib.hardware.hal," + "-org.wpilib.hardware.hal.simulation," + // TODO: ^ Document these, then remove them from the list + "-org.wpilib.hardware.hal.struct," + "-org.wpilib.math.proto," + "-org.wpilib.math.struct," + "-org.wpilib.math.controller.proto," + diff --git a/glass/src/lib/native/cpp/other/FMS.cpp b/glass/src/lib/native/cpp/other/FMS.cpp index 90583d3363..fb6ca4c1a0 100644 --- a/glass/src/lib/native/cpp/other/FMS.cpp +++ b/glass/src/lib/native/cpp/other/FMS.cpp @@ -16,6 +16,8 @@ using namespace wpi::glass; static const char* stations[] = {"Invalid", "Red 1", "Red 2", "Red 3", "Blue 1", "Blue 2", "Blue 3"}; +static const char* robotModes[] = {"Unknown", "Autonomous", "Teleoperated", + "Test"}; void wpi::glass::DisplayFMS(FMSModel* model, bool editableDsAttached) { if (!model->Exists() || model->IsReadOnly()) { @@ -107,17 +109,12 @@ void wpi::glass::DisplayFMSReadOnly(FMSModel* model) { ImGui::SameLine(); ImGui::TextUnformatted(exists ? (data->GetValue() ? "Yes" : "No") : "?"); } - if (auto data = model->GetTestData()) { - ImGui::Selectable("Test Mode: "); + if (auto data = model->GetRobotModeData()) { + ImGui::Selectable("Robot Mode: "); data->EmitDrag(); ImGui::SameLine(); - ImGui::TextUnformatted(exists ? (data->GetValue() ? "Yes" : "No") : "?"); - } - if (auto data = model->GetAutonomousData()) { - ImGui::Selectable("Autonomous Mode: "); - data->EmitDrag(); - ImGui::SameLine(); - ImGui::TextUnformatted(exists ? (data->GetValue() ? "Yes" : "No") : "?"); + ImGui::TextUnformatted( + exists ? robotModes[static_cast(data->GetValue())] : "?"); } if (auto data = model->GetFmsAttachedData()) { ImGui::Selectable("FMS Attached: "); diff --git a/glass/src/lib/native/include/wpi/glass/other/FMS.hpp b/glass/src/lib/native/include/wpi/glass/other/FMS.hpp index 3689108560..ed392f2d7c 100644 --- a/glass/src/lib/native/include/wpi/glass/other/FMS.hpp +++ b/glass/src/lib/native/include/wpi/glass/other/FMS.hpp @@ -22,14 +22,20 @@ class StringSource; class FMSModel : public Model { public: + enum RobotMode { + kUnknown = 0, + kAutonomous = 1, + kTeleop = 2, + kTest = 3, + }; + virtual BooleanSource* GetFmsAttachedData() = 0; virtual BooleanSource* GetDsAttachedData() = 0; virtual IntegerSource* GetAllianceStationIdData() = 0; virtual DoubleSource* GetMatchTimeData() = 0; virtual BooleanSource* GetEStopData() = 0; virtual BooleanSource* GetEnabledData() = 0; - virtual BooleanSource* GetTestData() = 0; - virtual BooleanSource* GetAutonomousData() = 0; + virtual IntegerSource* GetRobotModeData() = 0; virtual StringSource* GetGameSpecificMessageData() = 0; virtual void SetFmsAttached(bool val) = 0; @@ -38,8 +44,7 @@ class FMSModel : public Model { virtual void SetMatchTime(double val) = 0; virtual void SetEStop(bool val) = 0; virtual void SetEnabled(bool val) = 0; - virtual void SetTest(bool val) = 0; - virtual void SetAutonomous(bool val) = 0; + virtual void SetRobotMode(RobotMode val) = 0; virtual void SetGameSpecificMessage(std::string_view val) = 0; }; diff --git a/glass/src/libnt/native/cpp/NTFMS.cpp b/glass/src/libnt/native/cpp/NTFMS.cpp index 6c0802010e..ddbf0c76b6 100644 --- a/glass/src/libnt/native/cpp/NTFMS.cpp +++ b/glass/src/libnt/native/cpp/NTFMS.cpp @@ -10,8 +10,17 @@ #include -#include "wpi/util/SmallVector.hpp" -#include "wpi/util/timestamp.h" +#include "wpi/util/Endian.hpp" + +// FIXME: use dynamic struct decoding +// Duplicated here from DriverStationTypes.h to avoid HAL dependency +#define HAL_CONTROLWORD_OPMODE_HASH_MASK 0x00FFFFFFFFFFFFFFLL +#define HAL_CONTROLWORD_ROBOT_MODE_MASK 0x0300000000000000LL +#define HAL_CONTROLWORD_ROBOT_MODE_SHIFT 56 +#define HAL_CONTROLWORD_ENABLED_MASK 0x0400000000000000LL +#define HAL_CONTROLWORD_ESTOP_MASK 0x0800000000000000LL +#define HAL_CONTROLWORD_FMS_ATTACHED_MASK 0x1000000000000000LL +#define HAL_CONTROLWORD_DS_ATTACHED_MASK 0x2000000000000000LL using namespace wpi::glass; @@ -28,15 +37,14 @@ NTFMSModel::NTFMSModel(wpi::nt::NetworkTableInstance inst, .Subscribe(false)}, m_station{inst.GetIntegerTopic(fmt::format("{}/StationNumber", path)) .Subscribe(0)}, - m_controlWord{inst.GetIntegerTopic(fmt::format("{}/FMSControlData", path)) - .Subscribe(0)}, + m_controlWord{inst.GetRawTopic(fmt::format("{}/ControlWord", path)) + .Subscribe("struct:ControlWord", {})}, m_fmsAttached{fmt::format("NT_FMS:FMSAttached:{}", path)}, m_dsAttached{fmt::format("NT_FMS:DSAttached:{}", path)}, m_allianceStationId{fmt::format("NT_FMS:AllianceStationID:{}", path)}, m_estop{fmt::format("NT_FMS:EStop:{}", path)}, m_enabled{fmt::format("NT_FMS:RobotEnabled:{}", path)}, - m_test{fmt::format("NT_FMS:TestMode:{}", path)}, - m_autonomous{fmt::format("NT_FMS:AutonomousMode:{}", path)}, + m_robotMode{fmt::format("NT_FMS:RobotMode:{}", path)}, m_gameSpecificMessageData{ fmt::format("NT_FMS:GameSpecificMessage:{}", path)} {} @@ -55,14 +63,24 @@ void NTFMSModel::Update() { m_allianceStationId.SetValue(v.value - 1 + 3 * (isRed ? 0 : 1), v.time); } for (auto&& v : m_controlWord.ReadQueue()) { - uint32_t controlWord = v.value; - // See HAL_ControlWord definition - m_enabled.SetValue(((controlWord & 0x01) != 0) ? 1 : 0, v.time); - m_autonomous.SetValue(((controlWord & 0x02) != 0) ? 1 : 0, v.time); - m_test.SetValue(((controlWord & 0x04) != 0) ? 1 : 0, v.time); - m_estop.SetValue(((controlWord & 0x08) != 0) ? 1 : 0, v.time); - m_fmsAttached.SetValue(((controlWord & 0x10) != 0) ? 1 : 0, v.time); - m_dsAttached.SetValue(((controlWord & 0x20) != 0) ? 1 : 0, v.time); + if (v.value.size() != sizeof(uint64_t)) { + continue; + } + uint64_t controlWord = wpi::util::support::endian::read64le(v.value.data()); + // See wpi::Struct definition + m_enabled.SetValue( + ((controlWord & HAL_CONTROLWORD_ENABLED_MASK) != 0) ? 1 : 0, v.time); + m_robotMode.SetValue((controlWord & HAL_CONTROLWORD_ROBOT_MODE_MASK) >> + HAL_CONTROLWORD_ROBOT_MODE_SHIFT, + v.time); + m_estop.SetValue(((controlWord & HAL_CONTROLWORD_ESTOP_MASK) != 0) ? 1 : 0, + v.time); + m_fmsAttached.SetValue( + ((controlWord & HAL_CONTROLWORD_FMS_ATTACHED_MASK) != 0) ? 1 : 0, + v.time); + m_dsAttached.SetValue( + ((controlWord & HAL_CONTROLWORD_DS_ATTACHED_MASK) != 0) ? 1 : 0, + v.time); } for (auto&& v : m_gameSpecificMessage.ReadQueue()) { m_gameSpecificMessageData.SetValue(std::move(v.value), v.time); diff --git a/glass/src/libnt/native/include/wpi/glass/networktables/NTFMS.hpp b/glass/src/libnt/native/include/wpi/glass/networktables/NTFMS.hpp index e0a9d3778e..5de9fcb396 100644 --- a/glass/src/libnt/native/include/wpi/glass/networktables/NTFMS.hpp +++ b/glass/src/libnt/native/include/wpi/glass/networktables/NTFMS.hpp @@ -11,6 +11,7 @@ #include "wpi/nt/BooleanTopic.hpp" #include "wpi/nt/IntegerTopic.hpp" #include "wpi/nt/NetworkTableInstance.hpp" +#include "wpi/nt/RawTopic.hpp" #include "wpi/nt/StringTopic.hpp" namespace wpi::glass { @@ -32,8 +33,7 @@ class NTFMSModel : public FMSModel { DoubleSource* GetMatchTimeData() override { return nullptr; } BooleanSource* GetEStopData() override { return &m_estop; } BooleanSource* GetEnabledData() override { return &m_enabled; } - BooleanSource* GetTestData() override { return &m_test; } - BooleanSource* GetAutonomousData() override { return &m_autonomous; } + IntegerSource* GetRobotModeData() override { return &m_robotMode; } StringSource* GetGameSpecificMessageData() override { return &m_gameSpecificMessageData; } @@ -45,8 +45,7 @@ class NTFMSModel : public FMSModel { void SetMatchTime(double val) override {} void SetEStop(bool val) override {} void SetEnabled(bool val) override {} - void SetTest(bool val) override {} - void SetAutonomous(bool val) override {} + void SetRobotMode(RobotMode val) override {} void SetGameSpecificMessage(std::string_view val) override {} void Update() override; @@ -58,15 +57,14 @@ class NTFMSModel : public FMSModel { wpi::nt::StringSubscriber m_gameSpecificMessage; wpi::nt::BooleanSubscriber m_alliance; wpi::nt::IntegerSubscriber m_station; - wpi::nt::IntegerSubscriber m_controlWord; + wpi::nt::RawSubscriber m_controlWord; BooleanSource m_fmsAttached; BooleanSource m_dsAttached; IntegerSource m_allianceStationId; BooleanSource m_estop; BooleanSource m_enabled; - BooleanSource m_test; - BooleanSource m_autonomous; + IntegerSource m_robotMode; StringSource m_gameSpecificMessageData; }; diff --git a/hal/BUILD.bazel b/hal/BUILD.bazel index f80892a31d..106237851e 100644 --- a/hal/BUILD.bazel +++ b/hal/BUILD.bazel @@ -114,12 +114,10 @@ wpilib_cc_library( visibility = ["//visibility:public"], deps = [ ":generated_mrc_cc_headers", + "//ntcore", + "//wpinet", "//wpiutil", ] + select({ - "@rules_bzlmodrio_toolchains//constraints/is_systemcore:systemcore": [ - "//ntcore", - "//wpinet", - ], "//conditions:default": [ "//ntcore:ntcore_c_headers", ], @@ -129,12 +127,10 @@ wpilib_cc_library( wpilib_cc_shared_library( name = "shared/wpiHal", dynamic_deps = [ + "//ntcore:shared/ntcore", + "//wpinet:shared/wpinet", "//wpiutil:shared/wpiutil", ] + select({ - "@rules_bzlmodrio_toolchains//constraints/is_systemcore:systemcore": [ - "//ntcore:shared/ntcore", - "//wpinet:shared/wpinet", - ], "//conditions:default": [], }), visibility = ["//visibility:public"], @@ -146,12 +142,10 @@ wpilib_cc_shared_library( wpilib_cc_static_library( name = "static/wpiHal", static_deps = [ + "//ntcore:static/ntcore", + "//wpinet:static/wpinet", "//wpiutil:static/wpiutil", ] + select({ - "@rules_bzlmodrio_toolchains//constraints/is_systemcore:systemcore": [ - "//ntcore:static/ntcore", - "//wpinet:static/wpinet", - ], "//conditions:default": [], }), visibility = ["//visibility:public"], diff --git a/hal/robotpy_pybind_build_info.bzl b/hal/robotpy_pybind_build_info.bzl index 3841cab450..f1fffa6f44 100644 --- a/hal/robotpy_pybind_build_info.bzl +++ b/hal/robotpy_pybind_build_info.bzl @@ -302,6 +302,14 @@ def wpihal_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ tmpl_class_names = [], trampolines = [], ), + struct( + class_name = "DashboardOpMode", + yml_file = "semiwrap/DashboardOpMode.yml", + header_root = "$(execpath :robotpy-native-wpihal.copy_headers)", + header_file = "$(execpath :robotpy-native-wpihal.copy_headers)/wpi/hal/DashboardOpMode.hpp", + tmpl_class_names = [], + trampolines = [], + ), struct( class_name = "DIO", yml_file = "semiwrap/DIO.yml", @@ -325,7 +333,6 @@ def wpihal_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ header_file = "$(execpath :robotpy-native-wpihal.copy_headers)/wpi/hal/DriverStationTypes.h", tmpl_class_names = [], trampolines = [ - ("HAL_ControlWord", "__HAL_ControlWord.hpp"), ("HAL_JoystickAxes", "__HAL_JoystickAxes.hpp"), ("HAL_JoystickPOVs", "__HAL_JoystickPOVs.hpp"), ("HAL_JoystickButtons", "__HAL_JoystickButtons.hpp"), @@ -334,6 +341,8 @@ def wpihal_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ ("HAL_JoystickTouchpadFinger", "__HAL_JoystickTouchpadFinger.hpp"), ("HAL_JoystickTouchpad", "__HAL_JoystickTouchpad.hpp"), ("HAL_JoystickTouchpads", "__HAL_JoystickTouchpads.hpp"), + ("HAL_OpModeOption", "__HAL_OpModeOption.hpp"), + ("wpi::hal::ControlWord", "wpi__hal__ControlWord.hpp"), ], ), struct( diff --git a/hal/src/generated/main/native/cpp/mrc/protobuf/MrcComm.npb.cpp b/hal/src/generated/main/native/cpp/mrc/protobuf/MrcComm.npb.cpp index 532389fed4..8163f53281 100644 --- a/hal/src/generated/main/native/cpp/mrc/protobuf/MrcComm.npb.cpp +++ b/hal/src/generated/main/native/cpp/mrc/protobuf/MrcComm.npb.cpp @@ -60,7 +60,7 @@ static const uint8_t file_descriptor[] { 0x6f,0x62,0x75,0x66,0x43,0x6f,0x6e,0x74,0x72,0x6f, 0x6c,0x44,0x61,0x74,0x61,0x12,0x20,0x0a,0x0b,0x43, 0x6f,0x6e,0x74,0x72,0x6f,0x6c,0x57,0x6f,0x72,0x64, -0x18,0x01,0x20,0x01,0x28,0x0d,0x52,0x0b,0x43,0x6f, +0x18,0x05,0x20,0x01,0x28,0x0d,0x52,0x0b,0x43,0x6f, 0x6e,0x74,0x72,0x6f,0x6c,0x57,0x6f,0x72,0x64,0x12, 0x1c,0x0a,0x09,0x4d,0x61,0x74,0x63,0x68,0x54,0x69, 0x6d,0x65,0x18,0x02,0x20,0x01,0x28,0x05,0x52,0x09, @@ -154,353 +154,388 @@ static const uint8_t file_descriptor[] { 0x1c,0x0a,0x09,0x43,0x61,0x6c,0x6c,0x53,0x74,0x61, 0x63,0x6b,0x18,0x05,0x20,0x01,0x28,0x09,0x52,0x09, 0x43,0x61,0x6c,0x6c,0x53,0x74,0x61,0x63,0x6b,0x22, -0x38,0x0a,0x0e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, -0x66,0x4f,0x70,0x4d,0x6f,0x64,0x65,0x12,0x12,0x0a, -0x04,0x48,0x61,0x73,0x68,0x18,0x01,0x20,0x01,0x28, -0x06,0x52,0x04,0x48,0x61,0x73,0x68,0x12,0x12,0x0a, -0x04,0x4e,0x61,0x6d,0x65,0x18,0x02,0x20,0x01,0x28, -0x09,0x52,0x04,0x4e,0x61,0x6d,0x65,0x22,0x4b,0x0a, -0x18,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x41, -0x76,0x61,0x69,0x6c,0x61,0x62,0x6c,0x65,0x4f,0x70, -0x4d,0x6f,0x64,0x65,0x73,0x12,0x2f,0x0a,0x05,0x4d, -0x6f,0x64,0x65,0x73,0x18,0x01,0x20,0x03,0x28,0x0b, -0x32,0x19,0x2e,0x6d,0x72,0x63,0x2e,0x70,0x72,0x6f, -0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, -0x66,0x4f,0x70,0x4d,0x6f,0x64,0x65,0x52,0x05,0x4d, -0x6f,0x64,0x65,0x73,0x22,0xc4,0x01,0x0a,0x1a,0x50, -0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x45,0x72,0x72, -0x6f,0x72,0x49,0x6e,0x66,0x6f,0x54,0x69,0x6d,0x65, -0x73,0x74,0x61,0x6d,0x70,0x12,0x3a,0x0a,0x09,0x45, -0x72,0x72,0x6f,0x72,0x49,0x6e,0x66,0x6f,0x18,0x01, -0x20,0x01,0x28,0x0b,0x32,0x1c,0x2e,0x6d,0x72,0x63, -0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f, -0x74,0x6f,0x62,0x75,0x66,0x45,0x72,0x72,0x6f,0x72, -0x49,0x6e,0x66,0x6f,0x52,0x09,0x45,0x72,0x72,0x6f, -0x72,0x49,0x6e,0x66,0x6f,0x12,0x1c,0x0a,0x09,0x54, -0x69,0x6d,0x65,0x73,0x74,0x61,0x6d,0x70,0x18,0x02, -0x20,0x01,0x28,0x04,0x52,0x09,0x54,0x69,0x6d,0x65, -0x73,0x74,0x61,0x6d,0x70,0x12,0x26,0x0a,0x0e,0x53, -0x65,0x71,0x75,0x65,0x6e,0x63,0x65,0x4e,0x75,0x6d, -0x62,0x65,0x72,0x18,0x03,0x20,0x01,0x28,0x05,0x52, -0x0e,0x53,0x65,0x71,0x75,0x65,0x6e,0x63,0x65,0x4e, -0x75,0x6d,0x62,0x65,0x72,0x12,0x24,0x0a,0x0d,0x4e, +0xb8,0x01,0x0a,0x0e,0x50,0x72,0x6f,0x74,0x6f,0x62, +0x75,0x66,0x4f,0x70,0x4d,0x6f,0x64,0x65,0x12,0x12, +0x0a,0x04,0x48,0x61,0x73,0x68,0x18,0x01,0x20,0x01, +0x28,0x06,0x52,0x04,0x48,0x61,0x73,0x68,0x12,0x12, +0x0a,0x04,0x4e,0x61,0x6d,0x65,0x18,0x02,0x20,0x01, +0x28,0x09,0x52,0x04,0x4e,0x61,0x6d,0x65,0x12,0x14, +0x0a,0x05,0x47,0x72,0x6f,0x75,0x70,0x18,0x03,0x20, +0x01,0x28,0x09,0x52,0x05,0x47,0x72,0x6f,0x75,0x70, +0x12,0x20,0x0a,0x0b,0x44,0x65,0x73,0x63,0x72,0x69, +0x70,0x74,0x69,0x6f,0x6e,0x18,0x04,0x20,0x01,0x28, +0x09,0x52,0x0b,0x44,0x65,0x73,0x63,0x72,0x69,0x70, +0x74,0x69,0x6f,0x6e,0x12,0x1c,0x0a,0x09,0x54,0x65, +0x78,0x74,0x43,0x6f,0x6c,0x6f,0x72,0x18,0x05,0x20, +0x01,0x28,0x05,0x52,0x09,0x54,0x65,0x78,0x74,0x43, +0x6f,0x6c,0x6f,0x72,0x12,0x28,0x0a,0x0f,0x42,0x61, +0x63,0x6b,0x67,0x72,0x6f,0x75,0x6e,0x64,0x43,0x6f, +0x6c,0x6f,0x72,0x18,0x06,0x20,0x01,0x28,0x05,0x52, +0x0f,0x42,0x61,0x63,0x6b,0x67,0x72,0x6f,0x75,0x6e, +0x64,0x43,0x6f,0x6c,0x6f,0x72,0x22,0x4b,0x0a,0x18, +0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x41,0x76, +0x61,0x69,0x6c,0x61,0x62,0x6c,0x65,0x4f,0x70,0x4d, +0x6f,0x64,0x65,0x73,0x12,0x2f,0x0a,0x05,0x4d,0x6f, +0x64,0x65,0x73,0x18,0x01,0x20,0x03,0x28,0x0b,0x32, +0x19,0x2e,0x6d,0x72,0x63,0x2e,0x70,0x72,0x6f,0x74, +0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,0x66, +0x4f,0x70,0x4d,0x6f,0x64,0x65,0x52,0x05,0x4d,0x6f, +0x64,0x65,0x73,0x22,0xc4,0x01,0x0a,0x1a,0x50,0x72, +0x6f,0x74,0x6f,0x62,0x75,0x66,0x45,0x72,0x72,0x6f, +0x72,0x49,0x6e,0x66,0x6f,0x54,0x69,0x6d,0x65,0x73, +0x74,0x61,0x6d,0x70,0x12,0x3a,0x0a,0x09,0x45,0x72, +0x72,0x6f,0x72,0x49,0x6e,0x66,0x6f,0x18,0x01,0x20, +0x01,0x28,0x0b,0x32,0x1c,0x2e,0x6d,0x72,0x63,0x2e, +0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74, +0x6f,0x62,0x75,0x66,0x45,0x72,0x72,0x6f,0x72,0x49, +0x6e,0x66,0x6f,0x52,0x09,0x45,0x72,0x72,0x6f,0x72, +0x49,0x6e,0x66,0x6f,0x12,0x1c,0x0a,0x09,0x54,0x69, +0x6d,0x65,0x73,0x74,0x61,0x6d,0x70,0x18,0x02,0x20, +0x01,0x28,0x04,0x52,0x09,0x54,0x69,0x6d,0x65,0x73, +0x74,0x61,0x6d,0x70,0x12,0x26,0x0a,0x0e,0x53,0x65, +0x71,0x75,0x65,0x6e,0x63,0x65,0x4e,0x75,0x6d,0x62, +0x65,0x72,0x18,0x03,0x20,0x01,0x28,0x05,0x52,0x0e, +0x53,0x65,0x71,0x75,0x65,0x6e,0x63,0x65,0x4e,0x75, +0x6d,0x62,0x65,0x72,0x12,0x24,0x0a,0x0d,0x4e,0x75, +0x6d,0x4f,0x63,0x63,0x75,0x72,0x61,0x6e,0x63,0x65, +0x73,0x18,0x04,0x20,0x01,0x28,0x05,0x52,0x0d,0x4e, 0x75,0x6d,0x4f,0x63,0x63,0x75,0x72,0x61,0x6e,0x63, -0x65,0x73,0x18,0x04,0x20,0x01,0x28,0x05,0x52,0x0d, -0x4e,0x75,0x6d,0x4f,0x63,0x63,0x75,0x72,0x61,0x6e, -0x63,0x65,0x73,0x22,0x86,0x01,0x0a,0x1c,0x50,0x72, -0x6f,0x74,0x6f,0x62,0x75,0x66,0x43,0x6f,0x6e,0x73, -0x6f,0x6c,0x65,0x4c,0x69,0x6e,0x65,0x54,0x69,0x6d, -0x65,0x73,0x74,0x61,0x6d,0x70,0x12,0x20,0x0a,0x0b, -0x43,0x6f,0x6e,0x73,0x6f,0x6c,0x65,0x4c,0x69,0x6e, -0x65,0x18,0x01,0x20,0x01,0x28,0x09,0x52,0x0b,0x43, +0x65,0x73,0x22,0x86,0x01,0x0a,0x1c,0x50,0x72,0x6f, +0x74,0x6f,0x62,0x75,0x66,0x43,0x6f,0x6e,0x73,0x6f, +0x6c,0x65,0x4c,0x69,0x6e,0x65,0x54,0x69,0x6d,0x65, +0x73,0x74,0x61,0x6d,0x70,0x12,0x20,0x0a,0x0b,0x43, 0x6f,0x6e,0x73,0x6f,0x6c,0x65,0x4c,0x69,0x6e,0x65, -0x12,0x1c,0x0a,0x09,0x54,0x69,0x6d,0x65,0x73,0x74, -0x61,0x6d,0x70,0x18,0x02,0x20,0x01,0x28,0x04,0x52, -0x09,0x54,0x69,0x6d,0x65,0x73,0x74,0x61,0x6d,0x70, -0x12,0x26,0x0a,0x0e,0x53,0x65,0x71,0x75,0x65,0x6e, -0x63,0x65,0x4e,0x75,0x6d,0x62,0x65,0x72,0x18,0x03, -0x20,0x01,0x28,0x05,0x52,0x0e,0x53,0x65,0x71,0x75, -0x65,0x6e,0x63,0x65,0x4e,0x75,0x6d,0x62,0x65,0x72, -0x42,0x0f,0x0a,0x0d,0x63,0x6f,0x6d,0x2e,0x6d,0x72, -0x63,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0x93,0x17, -0x0a,0x06,0x12,0x04,0x00,0x00,0x5b,0x01,0x0a,0x08, -0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,0x08, -0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a,0x08, -0x0a,0x01,0x08,0x12,0x03,0x04,0x00,0x26,0x0a,0x09, -0x0a,0x02,0x08,0x01,0x12,0x03,0x04,0x00,0x26,0x0a, -0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x06,0x00,0x0a, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,0x12,0x03, -0x06,0x08,0x1a,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02, -0x00,0x12,0x03,0x07,0x04,0x11,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x07,0x04,0x0a, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x01,0x12, -0x03,0x07,0x0b,0x0c,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x00,0x03,0x12,0x03,0x07,0x0f,0x10,0x0a,0x0b, -0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03,0x08,0x04, -0x11,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x05, -0x12,0x03,0x08,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x01,0x01,0x12,0x03,0x08,0x0b,0x0c,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03,0x12,0x03, -0x08,0x0f,0x10,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02, -0x02,0x12,0x03,0x09,0x04,0x12,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x09,0x04,0x08, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x01,0x12, -0x03,0x09,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x02,0x03,0x12,0x03,0x09,0x10,0x11,0x0a,0x0a, -0x0a,0x02,0x04,0x01,0x12,0x04,0x0c,0x00,0x0e,0x01, -0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,0x12,0x03,0x0c, -0x08,0x1c,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x00, -0x12,0x03,0x0d,0x04,0x2c,0x0a,0x0c,0x0a,0x05,0x04, -0x01,0x02,0x00,0x04,0x12,0x03,0x0d,0x04,0x0c,0x0a, -0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x06,0x12,0x03, -0x0d,0x0d,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02, -0x00,0x01,0x12,0x03,0x0d,0x20,0x27,0x0a,0x0c,0x0a, -0x05,0x04,0x01,0x02,0x00,0x03,0x12,0x03,0x0d,0x2a, -0x2b,0x0a,0x0a,0x0a,0x02,0x04,0x02,0x12,0x04,0x10, -0x00,0x1a,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x02,0x01, -0x12,0x03,0x10,0x08,0x1c,0x0a,0x0b,0x0a,0x04,0x04, -0x02,0x02,0x00,0x12,0x03,0x11,0x04,0x20,0x0a,0x0c, -0x0a,0x05,0x04,0x02,0x02,0x00,0x05,0x12,0x03,0x11, -0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00, -0x01,0x12,0x03,0x11,0x0b,0x1b,0x0a,0x0c,0x0a,0x05, -0x04,0x02,0x02,0x00,0x03,0x12,0x03,0x11,0x1e,0x1f, -0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x01,0x12,0x03, -0x12,0x04,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, -0x01,0x05,0x12,0x03,0x12,0x04,0x0a,0x0a,0x0c,0x0a, -0x05,0x04,0x02,0x02,0x01,0x01,0x12,0x03,0x12,0x0b, -0x12,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x01,0x03, -0x12,0x03,0x12,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04, -0x02,0x02,0x02,0x12,0x03,0x13,0x04,0x1d,0x0a,0x0c, -0x0a,0x05,0x04,0x02,0x02,0x02,0x05,0x12,0x03,0x13, -0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x02, -0x01,0x12,0x03,0x13,0x0b,0x18,0x0a,0x0c,0x0a,0x05, -0x04,0x02,0x02,0x02,0x03,0x12,0x03,0x13,0x1b,0x1c, -0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x03,0x12,0x03, -0x14,0x04,0x1d,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, -0x03,0x04,0x12,0x03,0x14,0x04,0x0c,0x0a,0x0c,0x0a, -0x05,0x04,0x02,0x02,0x03,0x05,0x12,0x03,0x14,0x0d, -0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x03,0x01, -0x12,0x03,0x14,0x14,0x18,0x0a,0x0c,0x0a,0x05,0x04, -0x02,0x02,0x03,0x03,0x12,0x03,0x14,0x1b,0x1c,0x0a, -0x3e,0x0a,0x04,0x04,0x02,0x02,0x04,0x12,0x03,0x17, -0x04,0x18,0x1a,0x31,0x20,0x45,0x61,0x63,0x68,0x20, -0x50,0x4f,0x56,0x20,0x74,0x61,0x6b,0x65,0x73,0x20, -0x75,0x70,0x20,0x34,0x20,0x62,0x69,0x74,0x73,0x0a, -0x20,0x57,0x65,0x20,0x63,0x61,0x6e,0x20,0x66,0x69, -0x74,0x20,0x38,0x20,0x69,0x6e,0x20,0x68,0x65,0x72, -0x65,0x2e,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, -0x04,0x05,0x12,0x03,0x17,0x04,0x0a,0x0a,0x0c,0x0a, -0x05,0x04,0x02,0x02,0x04,0x01,0x12,0x03,0x17,0x0b, -0x13,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x04,0x03, -0x12,0x03,0x17,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04, -0x02,0x02,0x05,0x12,0x03,0x18,0x04,0x14,0x0a,0x0c, -0x0a,0x05,0x04,0x02,0x02,0x05,0x05,0x12,0x03,0x18, -0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x05, -0x01,0x12,0x03,0x18,0x0b,0x0f,0x0a,0x0c,0x0a,0x05, -0x04,0x02,0x02,0x05,0x03,0x12,0x03,0x18,0x12,0x13, -0x0a,0x0b,0x0a,0x04,0x04,0x02,0x02,0x06,0x12,0x03, -0x19,0x04,0x30,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02, -0x06,0x04,0x12,0x03,0x19,0x04,0x0c,0x0a,0x0c,0x0a, -0x05,0x04,0x02,0x02,0x06,0x06,0x12,0x03,0x19,0x0d, -0x21,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x06,0x01, -0x12,0x03,0x19,0x22,0x2b,0x0a,0x0c,0x0a,0x05,0x04, -0x02,0x02,0x06,0x03,0x12,0x03,0x19,0x2e,0x2f,0x0a, -0x0a,0x0a,0x02,0x04,0x03,0x12,0x04,0x1c,0x00,0x21, -0x01,0x0a,0x0a,0x0a,0x03,0x04,0x03,0x01,0x12,0x03, -0x1c,0x08,0x1b,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02, -0x00,0x12,0x03,0x1d,0x04,0x1b,0x0a,0x0c,0x0a,0x05, -0x04,0x03,0x02,0x00,0x05,0x12,0x03,0x1d,0x04,0x0a, -0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x01,0x12, -0x03,0x1d,0x0b,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x03, -0x02,0x00,0x03,0x12,0x03,0x1d,0x19,0x1a,0x0a,0x0b, -0x0a,0x04,0x04,0x03,0x02,0x01,0x12,0x03,0x1e,0x04, -0x18,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x05, -0x12,0x03,0x1e,0x04,0x09,0x0a,0x0c,0x0a,0x05,0x04, -0x03,0x02,0x01,0x01,0x12,0x03,0x1e,0x0a,0x13,0x0a, -0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x03,0x12,0x03, -0x1e,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02, -0x02,0x12,0x03,0x1f,0x04,0x30,0x0a,0x0c,0x0a,0x05, -0x04,0x03,0x02,0x02,0x04,0x12,0x03,0x1f,0x04,0x0c, -0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x02,0x06,0x12, -0x03,0x1f,0x0d,0x21,0x0a,0x0c,0x0a,0x05,0x04,0x03, -0x02,0x02,0x01,0x12,0x03,0x1f,0x22,0x2b,0x0a,0x0c, -0x0a,0x05,0x04,0x03,0x02,0x02,0x03,0x12,0x03,0x1f, -0x2e,0x2f,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x03, -0x12,0x03,0x20,0x04,0x1e,0x0a,0x0c,0x0a,0x05,0x04, -0x03,0x02,0x03,0x05,0x12,0x03,0x20,0x04,0x0b,0x0a, -0x0c,0x0a,0x05,0x04,0x03,0x02,0x03,0x01,0x12,0x03, -0x20,0x0c,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, -0x03,0x03,0x12,0x03,0x20,0x1c,0x1d,0x0a,0x0a,0x0a, -0x02,0x04,0x04,0x12,0x04,0x23,0x00,0x28,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x04,0x01,0x12,0x03,0x23,0x08, -0x22,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x00,0x12, -0x03,0x24,0x04,0x1c,0x0a,0x0c,0x0a,0x05,0x04,0x04, -0x02,0x00,0x05,0x12,0x03,0x24,0x04,0x0a,0x0a,0x0c, -0x0a,0x05,0x04,0x04,0x02,0x00,0x01,0x12,0x03,0x24, -0x0b,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00, -0x03,0x12,0x03,0x24,0x1a,0x1b,0x0a,0x0b,0x0a,0x04, -0x04,0x04,0x02,0x01,0x12,0x03,0x25,0x04,0x17,0x0a, -0x0c,0x0a,0x05,0x04,0x04,0x02,0x01,0x05,0x12,0x03, -0x25,0x04,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, -0x01,0x01,0x12,0x03,0x25,0x09,0x12,0x0a,0x0c,0x0a, -0x05,0x04,0x04,0x02,0x01,0x03,0x12,0x03,0x25,0x15, -0x16,0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x02,0x12, -0x03,0x26,0x04,0x1b,0x0a,0x0c,0x0a,0x05,0x04,0x04, -0x02,0x02,0x05,0x12,0x03,0x26,0x04,0x0a,0x0a,0x0c, -0x0a,0x05,0x04,0x04,0x02,0x02,0x01,0x12,0x03,0x26, -0x0b,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x02, -0x03,0x12,0x03,0x26,0x19,0x1a,0x0a,0x0b,0x0a,0x04, -0x04,0x04,0x02,0x03,0x12,0x03,0x27,0x04,0x20,0x0a, -0x0c,0x0a,0x05,0x04,0x04,0x02,0x03,0x05,0x12,0x03, -0x27,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, -0x03,0x01,0x12,0x03,0x27,0x0b,0x1b,0x0a,0x0c,0x0a, -0x05,0x04,0x04,0x02,0x03,0x03,0x12,0x03,0x27,0x1e, -0x1f,0x0a,0x0a,0x0a,0x02,0x04,0x05,0x12,0x04,0x2a, -0x00,0x2c,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x05,0x01, -0x12,0x03,0x2a,0x08,0x23,0x0a,0x0b,0x0a,0x04,0x04, -0x05,0x02,0x00,0x12,0x03,0x2b,0x04,0x38,0x0a,0x0c, -0x0a,0x05,0x04,0x05,0x02,0x00,0x04,0x12,0x03,0x2b, -0x04,0x0c,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00, -0x06,0x12,0x03,0x2b,0x0d,0x27,0x0a,0x0c,0x0a,0x05, -0x04,0x05,0x02,0x00,0x01,0x12,0x03,0x2b,0x28,0x33, -0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12, -0x03,0x2b,0x36,0x37,0x0a,0x0a,0x0a,0x02,0x04,0x06, -0x12,0x04,0x2e,0x00,0x32,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x06,0x01,0x12,0x03,0x2e,0x08,0x1e,0x0a,0x0b, -0x0a,0x04,0x04,0x06,0x02,0x00,0x12,0x03,0x2f,0x04, -0x14,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x05, -0x12,0x03,0x2f,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x00,0x01,0x12,0x03,0x2f,0x0b,0x0f,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x03,0x12,0x03, -0x2f,0x12,0x13,0x0a,0x2b,0x0a,0x04,0x04,0x06,0x02, -0x01,0x12,0x03,0x30,0x04,0x16,0x22,0x1e,0x20,0x31, -0x36,0x20,0x62,0x69,0x74,0x73,0x2c,0x20,0x6c,0x65, -0x66,0x74,0x20,0x6d,0x73,0x62,0x2c,0x20,0x72,0x69, -0x67,0x68,0x74,0x20,0x6c,0x73,0x62,0x0a,0x0a,0x0c, -0x0a,0x05,0x04,0x06,0x02,0x01,0x05,0x12,0x03,0x30, -0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01, -0x01,0x12,0x03,0x30,0x0b,0x11,0x0a,0x0c,0x0a,0x05, -0x04,0x06,0x02,0x01,0x03,0x12,0x03,0x30,0x14,0x15, -0x0a,0x2b,0x0a,0x04,0x04,0x06,0x02,0x02,0x12,0x03, -0x31,0x04,0x1d,0x22,0x1e,0x20,0x31,0x36,0x20,0x62, -0x69,0x74,0x73,0x2c,0x20,0x6c,0x65,0x66,0x74,0x20, -0x6d,0x73,0x62,0x2c,0x20,0x72,0x69,0x67,0x68,0x74, -0x20,0x6c,0x73,0x62,0x0a,0x0a,0x0c,0x0a,0x05,0x04, -0x06,0x02,0x02,0x05,0x12,0x03,0x31,0x04,0x0a,0x0a, -0x0c,0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03, -0x31,0x0b,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02, -0x02,0x03,0x12,0x03,0x31,0x1b,0x1c,0x0a,0x0a,0x0a, -0x02,0x04,0x07,0x12,0x04,0x34,0x00,0x36,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x07,0x01,0x12,0x03,0x34,0x08, -0x1f,0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x00,0x12, -0x03,0x35,0x04,0x30,0x0a,0x0c,0x0a,0x05,0x04,0x07, -0x02,0x00,0x04,0x12,0x03,0x35,0x04,0x0c,0x0a,0x0c, -0x0a,0x05,0x04,0x07,0x02,0x00,0x06,0x12,0x03,0x35, -0x0d,0x23,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00, -0x01,0x12,0x03,0x35,0x24,0x2b,0x0a,0x0c,0x0a,0x05, -0x04,0x07,0x02,0x00,0x03,0x12,0x03,0x35,0x2e,0x2f, -0x0a,0x0a,0x0a,0x02,0x04,0x08,0x12,0x04,0x38,0x00, -0x3d,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x08,0x01,0x12, -0x03,0x38,0x08,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x08, -0x02,0x00,0x12,0x03,0x39,0x04,0x19,0x0a,0x0c,0x0a, -0x05,0x04,0x08,0x02,0x00,0x05,0x12,0x03,0x39,0x04, -0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x01, -0x12,0x03,0x39,0x0b,0x14,0x0a,0x0c,0x0a,0x05,0x04, -0x08,0x02,0x00,0x03,0x12,0x03,0x39,0x17,0x18,0x0a, -0x0b,0x0a,0x04,0x04,0x08,0x02,0x01,0x12,0x03,0x3a, -0x04,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01, -0x05,0x12,0x03,0x3a,0x04,0x09,0x0a,0x0c,0x0a,0x05, -0x04,0x08,0x02,0x01,0x01,0x12,0x03,0x3a,0x0a,0x15, -0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x03,0x12, -0x03,0x3a,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x08, -0x02,0x02,0x12,0x03,0x3b,0x04,0x1b,0x0a,0x0c,0x0a, -0x05,0x04,0x08,0x02,0x02,0x05,0x12,0x03,0x3b,0x04, -0x09,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x01, -0x12,0x03,0x3b,0x0a,0x16,0x0a,0x0c,0x0a,0x05,0x04, -0x08,0x02,0x02,0x03,0x12,0x03,0x3b,0x19,0x1a,0x0a, -0x0b,0x0a,0x04,0x04,0x08,0x02,0x03,0x12,0x03,0x3c, -0x04,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03, -0x05,0x12,0x03,0x3c,0x04,0x09,0x0a,0x0c,0x0a,0x05, -0x04,0x08,0x02,0x03,0x01,0x12,0x03,0x3c,0x0a,0x13, -0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x03,0x12, -0x03,0x3c,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x09, -0x12,0x04,0x3f,0x00,0x45,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x09,0x01,0x12,0x03,0x3f,0x08,0x19,0x0a,0x0b, -0x0a,0x04,0x04,0x09,0x02,0x00,0x12,0x03,0x40,0x04, -0x15,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x05, -0x12,0x03,0x40,0x04,0x08,0x0a,0x0c,0x0a,0x05,0x04, -0x09,0x02,0x00,0x01,0x12,0x03,0x40,0x09,0x10,0x0a, -0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x03,0x12,0x03, -0x40,0x13,0x14,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02, -0x01,0x12,0x03,0x41,0x04,0x19,0x0a,0x0c,0x0a,0x05, -0x04,0x09,0x02,0x01,0x05,0x12,0x03,0x41,0x04,0x0a, -0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x01,0x12, -0x03,0x41,0x0b,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x09, -0x02,0x01,0x03,0x12,0x03,0x41,0x17,0x18,0x0a,0x0b, -0x0a,0x04,0x04,0x09,0x02,0x02,0x12,0x03,0x42,0x04, -0x17,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x05, -0x12,0x03,0x42,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04, -0x09,0x02,0x02,0x01,0x12,0x03,0x42,0x0b,0x12,0x0a, -0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x03,0x12,0x03, -0x42,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02, -0x03,0x12,0x03,0x43,0x04,0x18,0x0a,0x0c,0x0a,0x05, -0x04,0x09,0x02,0x03,0x05,0x12,0x03,0x43,0x04,0x0a, -0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x01,0x12, -0x03,0x43,0x0b,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09, -0x02,0x03,0x03,0x12,0x03,0x43,0x16,0x17,0x0a,0x0b, -0x0a,0x04,0x04,0x09,0x02,0x04,0x12,0x03,0x44,0x04, -0x19,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x04,0x05, -0x12,0x03,0x44,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04, -0x09,0x02,0x04,0x01,0x12,0x03,0x44,0x0b,0x14,0x0a, -0x0c,0x0a,0x05,0x04,0x09,0x02,0x04,0x03,0x12,0x03, -0x44,0x17,0x18,0x0a,0x0a,0x0a,0x02,0x04,0x0a,0x12, -0x04,0x47,0x00,0x4a,0x01,0x0a,0x0a,0x0a,0x03,0x04, -0x0a,0x01,0x12,0x03,0x47,0x08,0x16,0x0a,0x0b,0x0a, -0x04,0x04,0x0a,0x02,0x00,0x12,0x03,0x48,0x04,0x15, -0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x05,0x12, -0x03,0x48,0x04,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x0a, -0x02,0x00,0x01,0x12,0x03,0x48,0x0c,0x10,0x0a,0x0c, -0x0a,0x05,0x04,0x0a,0x02,0x00,0x03,0x12,0x03,0x48, -0x13,0x14,0x0a,0x0b,0x0a,0x04,0x04,0x0a,0x02,0x01, -0x12,0x03,0x49,0x04,0x14,0x0a,0x0c,0x0a,0x05,0x04, -0x0a,0x02,0x01,0x05,0x12,0x03,0x49,0x04,0x0a,0x0a, -0x0c,0x0a,0x05,0x04,0x0a,0x02,0x01,0x01,0x12,0x03, -0x49,0x0b,0x0f,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02, -0x01,0x03,0x12,0x03,0x49,0x12,0x13,0x0a,0x0a,0x0a, -0x02,0x04,0x0b,0x12,0x04,0x4c,0x00,0x4e,0x01,0x0a, -0x0a,0x0a,0x03,0x04,0x0b,0x01,0x12,0x03,0x4c,0x08, -0x20,0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02,0x00,0x12, -0x03,0x4d,0x04,0x26,0x0a,0x0c,0x0a,0x05,0x04,0x0b, -0x02,0x00,0x04,0x12,0x03,0x4d,0x04,0x0c,0x0a,0x0c, -0x0a,0x05,0x04,0x0b,0x02,0x00,0x06,0x12,0x03,0x4d, -0x0d,0x1b,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00, -0x01,0x12,0x03,0x4d,0x1c,0x21,0x0a,0x0c,0x0a,0x05, -0x04,0x0b,0x02,0x00,0x03,0x12,0x03,0x4d,0x24,0x25, -0x0a,0x0a,0x0a,0x02,0x04,0x0c,0x12,0x04,0x50,0x00, -0x55,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0c,0x01,0x12, -0x03,0x50,0x08,0x22,0x0a,0x0b,0x0a,0x04,0x04,0x0c, -0x02,0x00,0x12,0x03,0x51,0x04,0x24,0x0a,0x0c,0x0a, -0x05,0x04,0x0c,0x02,0x00,0x06,0x12,0x03,0x51,0x04, -0x15,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x01, -0x12,0x03,0x51,0x16,0x1f,0x0a,0x0c,0x0a,0x05,0x04, -0x0c,0x02,0x00,0x03,0x12,0x03,0x51,0x22,0x23,0x0a, -0x0b,0x0a,0x04,0x04,0x0c,0x02,0x01,0x12,0x03,0x52, -0x04,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01, -0x05,0x12,0x03,0x52,0x04,0x0a,0x0a,0x0c,0x0a,0x05, -0x04,0x0c,0x02,0x01,0x01,0x12,0x03,0x52,0x0b,0x14, -0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x03,0x12, -0x03,0x52,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x0c, -0x02,0x02,0x12,0x03,0x53,0x04,0x1d,0x0a,0x0c,0x0a, -0x05,0x04,0x0c,0x02,0x02,0x05,0x12,0x03,0x53,0x04, -0x09,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x02,0x01, -0x12,0x03,0x53,0x0a,0x18,0x0a,0x0c,0x0a,0x05,0x04, -0x0c,0x02,0x02,0x03,0x12,0x03,0x53,0x1b,0x1c,0x0a, -0x0b,0x0a,0x04,0x04,0x0c,0x02,0x03,0x12,0x03,0x54, -0x04,0x1c,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x03, -0x05,0x12,0x03,0x54,0x04,0x09,0x0a,0x0c,0x0a,0x05, -0x04,0x0c,0x02,0x03,0x01,0x12,0x03,0x54,0x0a,0x17, -0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x03,0x03,0x12, -0x03,0x54,0x1a,0x1b,0x0a,0x0a,0x0a,0x02,0x04,0x0d, -0x12,0x04,0x57,0x00,0x5b,0x01,0x0a,0x0a,0x0a,0x03, -0x04,0x0d,0x01,0x12,0x03,0x57,0x08,0x24,0x0a,0x0b, -0x0a,0x04,0x04,0x0d,0x02,0x00,0x12,0x03,0x58,0x04, -0x1b,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x05, -0x12,0x03,0x58,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04, -0x0d,0x02,0x00,0x01,0x12,0x03,0x58,0x0b,0x16,0x0a, -0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x03,0x12,0x03, -0x58,0x19,0x1a,0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02, -0x01,0x12,0x03,0x59,0x04,0x19,0x0a,0x0c,0x0a,0x05, -0x04,0x0d,0x02,0x01,0x05,0x12,0x03,0x59,0x04,0x0a, -0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x01,0x12, -0x03,0x59,0x0b,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0d, -0x02,0x01,0x03,0x12,0x03,0x59,0x17,0x18,0x0a,0x0b, -0x0a,0x04,0x04,0x0d,0x02,0x02,0x12,0x03,0x5a,0x04, -0x1d,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x02,0x05, -0x12,0x03,0x5a,0x04,0x09,0x0a,0x0c,0x0a,0x05,0x04, -0x0d,0x02,0x02,0x01,0x12,0x03,0x5a,0x0a,0x18,0x0a, -0x0c,0x0a,0x05,0x04,0x0d,0x02,0x02,0x03,0x12,0x03, -0x5a,0x1b,0x1c,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f, -0x33, +0x18,0x01,0x20,0x01,0x28,0x09,0x52,0x0b,0x43,0x6f, +0x6e,0x73,0x6f,0x6c,0x65,0x4c,0x69,0x6e,0x65,0x12, +0x1c,0x0a,0x09,0x54,0x69,0x6d,0x65,0x73,0x74,0x61, +0x6d,0x70,0x18,0x02,0x20,0x01,0x28,0x04,0x52,0x09, +0x54,0x69,0x6d,0x65,0x73,0x74,0x61,0x6d,0x70,0x12, +0x26,0x0a,0x0e,0x53,0x65,0x71,0x75,0x65,0x6e,0x63, +0x65,0x4e,0x75,0x6d,0x62,0x65,0x72,0x18,0x03,0x20, +0x01,0x28,0x05,0x52,0x0e,0x53,0x65,0x71,0x75,0x65, +0x6e,0x63,0x65,0x4e,0x75,0x6d,0x62,0x65,0x72,0x42, +0x0f,0x0a,0x0d,0x63,0x6f,0x6d,0x2e,0x6d,0x72,0x63, +0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0xef,0x18,0x0a, +0x06,0x12,0x04,0x00,0x00,0x5f,0x01,0x0a,0x08,0x0a, +0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,0x08,0x0a, +0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a,0x08,0x0a, +0x01,0x08,0x12,0x03,0x04,0x00,0x26,0x0a,0x09,0x0a, +0x02,0x08,0x01,0x12,0x03,0x04,0x00,0x26,0x0a,0x0a, +0x0a,0x02,0x04,0x00,0x12,0x04,0x06,0x00,0x0a,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,0x12,0x03,0x06, +0x08,0x1a,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x00, +0x12,0x03,0x07,0x04,0x11,0x0a,0x0c,0x0a,0x05,0x04, +0x00,0x02,0x00,0x05,0x12,0x03,0x07,0x04,0x0a,0x0a, +0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x01,0x12,0x03, +0x07,0x0b,0x0c,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, +0x00,0x03,0x12,0x03,0x07,0x0f,0x10,0x0a,0x0b,0x0a, +0x04,0x04,0x00,0x02,0x01,0x12,0x03,0x08,0x04,0x11, +0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x05,0x12, +0x03,0x08,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x00, +0x02,0x01,0x01,0x12,0x03,0x08,0x0b,0x0c,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x01,0x03,0x12,0x03,0x08, +0x0f,0x10,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x02, +0x12,0x03,0x09,0x04,0x12,0x0a,0x0c,0x0a,0x05,0x04, +0x00,0x02,0x02,0x05,0x12,0x03,0x09,0x04,0x08,0x0a, +0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x01,0x12,0x03, +0x09,0x09,0x0d,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, +0x02,0x03,0x12,0x03,0x09,0x10,0x11,0x0a,0x0a,0x0a, +0x02,0x04,0x01,0x12,0x04,0x0c,0x00,0x0e,0x01,0x0a, +0x0a,0x0a,0x03,0x04,0x01,0x01,0x12,0x03,0x0c,0x08, +0x1c,0x0a,0x0b,0x0a,0x04,0x04,0x01,0x02,0x00,0x12, +0x03,0x0d,0x04,0x2c,0x0a,0x0c,0x0a,0x05,0x04,0x01, +0x02,0x00,0x04,0x12,0x03,0x0d,0x04,0x0c,0x0a,0x0c, +0x0a,0x05,0x04,0x01,0x02,0x00,0x06,0x12,0x03,0x0d, +0x0d,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00, +0x01,0x12,0x03,0x0d,0x20,0x27,0x0a,0x0c,0x0a,0x05, +0x04,0x01,0x02,0x00,0x03,0x12,0x03,0x0d,0x2a,0x2b, +0x0a,0x0a,0x0a,0x02,0x04,0x02,0x12,0x04,0x10,0x00, +0x1a,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x02,0x01,0x12, +0x03,0x10,0x08,0x1c,0x0a,0x0b,0x0a,0x04,0x04,0x02, +0x02,0x00,0x12,0x03,0x11,0x04,0x20,0x0a,0x0c,0x0a, +0x05,0x04,0x02,0x02,0x00,0x05,0x12,0x03,0x11,0x04, +0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x00,0x01, +0x12,0x03,0x11,0x0b,0x1b,0x0a,0x0c,0x0a,0x05,0x04, +0x02,0x02,0x00,0x03,0x12,0x03,0x11,0x1e,0x1f,0x0a, +0x0b,0x0a,0x04,0x04,0x02,0x02,0x01,0x12,0x03,0x12, +0x04,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x01, +0x05,0x12,0x03,0x12,0x04,0x0a,0x0a,0x0c,0x0a,0x05, +0x04,0x02,0x02,0x01,0x01,0x12,0x03,0x12,0x0b,0x12, +0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x01,0x03,0x12, +0x03,0x12,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x02, +0x02,0x02,0x12,0x03,0x13,0x04,0x1d,0x0a,0x0c,0x0a, +0x05,0x04,0x02,0x02,0x02,0x05,0x12,0x03,0x13,0x04, +0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x02,0x01, +0x12,0x03,0x13,0x0b,0x18,0x0a,0x0c,0x0a,0x05,0x04, +0x02,0x02,0x02,0x03,0x12,0x03,0x13,0x1b,0x1c,0x0a, +0x0b,0x0a,0x04,0x04,0x02,0x02,0x03,0x12,0x03,0x14, +0x04,0x1d,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x03, +0x04,0x12,0x03,0x14,0x04,0x0c,0x0a,0x0c,0x0a,0x05, +0x04,0x02,0x02,0x03,0x05,0x12,0x03,0x14,0x0d,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x03,0x01,0x12, +0x03,0x14,0x14,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x02, +0x02,0x03,0x03,0x12,0x03,0x14,0x1b,0x1c,0x0a,0x3e, +0x0a,0x04,0x04,0x02,0x02,0x04,0x12,0x03,0x17,0x04, +0x18,0x1a,0x31,0x20,0x45,0x61,0x63,0x68,0x20,0x50, +0x4f,0x56,0x20,0x74,0x61,0x6b,0x65,0x73,0x20,0x75, +0x70,0x20,0x34,0x20,0x62,0x69,0x74,0x73,0x0a,0x20, +0x57,0x65,0x20,0x63,0x61,0x6e,0x20,0x66,0x69,0x74, +0x20,0x38,0x20,0x69,0x6e,0x20,0x68,0x65,0x72,0x65, +0x2e,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x04, +0x05,0x12,0x03,0x17,0x04,0x0a,0x0a,0x0c,0x0a,0x05, +0x04,0x02,0x02,0x04,0x01,0x12,0x03,0x17,0x0b,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x04,0x03,0x12, +0x03,0x17,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x02, +0x02,0x05,0x12,0x03,0x18,0x04,0x14,0x0a,0x0c,0x0a, +0x05,0x04,0x02,0x02,0x05,0x05,0x12,0x03,0x18,0x04, +0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x05,0x01, +0x12,0x03,0x18,0x0b,0x0f,0x0a,0x0c,0x0a,0x05,0x04, +0x02,0x02,0x05,0x03,0x12,0x03,0x18,0x12,0x13,0x0a, +0x0b,0x0a,0x04,0x04,0x02,0x02,0x06,0x12,0x03,0x19, +0x04,0x30,0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x06, +0x04,0x12,0x03,0x19,0x04,0x0c,0x0a,0x0c,0x0a,0x05, +0x04,0x02,0x02,0x06,0x06,0x12,0x03,0x19,0x0d,0x21, +0x0a,0x0c,0x0a,0x05,0x04,0x02,0x02,0x06,0x01,0x12, +0x03,0x19,0x22,0x2b,0x0a,0x0c,0x0a,0x05,0x04,0x02, +0x02,0x06,0x03,0x12,0x03,0x19,0x2e,0x2f,0x0a,0x0a, +0x0a,0x02,0x04,0x03,0x12,0x04,0x1c,0x00,0x21,0x01, +0x0a,0x0a,0x0a,0x03,0x04,0x03,0x01,0x12,0x03,0x1c, +0x08,0x1b,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x00, +0x12,0x03,0x1d,0x04,0x1b,0x0a,0x0c,0x0a,0x05,0x04, +0x03,0x02,0x00,0x05,0x12,0x03,0x1d,0x04,0x0a,0x0a, +0x0c,0x0a,0x05,0x04,0x03,0x02,0x00,0x01,0x12,0x03, +0x1d,0x0b,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, +0x00,0x03,0x12,0x03,0x1d,0x19,0x1a,0x0a,0x0b,0x0a, +0x04,0x04,0x03,0x02,0x01,0x12,0x03,0x1e,0x04,0x18, +0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x01,0x05,0x12, +0x03,0x1e,0x04,0x09,0x0a,0x0c,0x0a,0x05,0x04,0x03, +0x02,0x01,0x01,0x12,0x03,0x1e,0x0a,0x13,0x0a,0x0c, +0x0a,0x05,0x04,0x03,0x02,0x01,0x03,0x12,0x03,0x1e, +0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x02, +0x12,0x03,0x1f,0x04,0x30,0x0a,0x0c,0x0a,0x05,0x04, +0x03,0x02,0x02,0x04,0x12,0x03,0x1f,0x04,0x0c,0x0a, +0x0c,0x0a,0x05,0x04,0x03,0x02,0x02,0x06,0x12,0x03, +0x1f,0x0d,0x21,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02, +0x02,0x01,0x12,0x03,0x1f,0x22,0x2b,0x0a,0x0c,0x0a, +0x05,0x04,0x03,0x02,0x02,0x03,0x12,0x03,0x1f,0x2e, +0x2f,0x0a,0x0b,0x0a,0x04,0x04,0x03,0x02,0x03,0x12, +0x03,0x20,0x04,0x1e,0x0a,0x0c,0x0a,0x05,0x04,0x03, +0x02,0x03,0x05,0x12,0x03,0x20,0x04,0x0b,0x0a,0x0c, +0x0a,0x05,0x04,0x03,0x02,0x03,0x01,0x12,0x03,0x20, +0x0c,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x03,0x02,0x03, +0x03,0x12,0x03,0x20,0x1c,0x1d,0x0a,0x0a,0x0a,0x02, +0x04,0x04,0x12,0x04,0x23,0x00,0x28,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x04,0x01,0x12,0x03,0x23,0x08,0x22, +0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x00,0x12,0x03, +0x24,0x04,0x1c,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, +0x00,0x05,0x12,0x03,0x24,0x04,0x0a,0x0a,0x0c,0x0a, +0x05,0x04,0x04,0x02,0x00,0x01,0x12,0x03,0x24,0x0b, +0x17,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x00,0x03, +0x12,0x03,0x24,0x1a,0x1b,0x0a,0x0b,0x0a,0x04,0x04, +0x04,0x02,0x01,0x12,0x03,0x25,0x04,0x17,0x0a,0x0c, +0x0a,0x05,0x04,0x04,0x02,0x01,0x05,0x12,0x03,0x25, +0x04,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x01, +0x01,0x12,0x03,0x25,0x09,0x12,0x0a,0x0c,0x0a,0x05, +0x04,0x04,0x02,0x01,0x03,0x12,0x03,0x25,0x15,0x16, +0x0a,0x0b,0x0a,0x04,0x04,0x04,0x02,0x02,0x12,0x03, +0x26,0x04,0x1b,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02, +0x02,0x05,0x12,0x03,0x26,0x04,0x0a,0x0a,0x0c,0x0a, +0x05,0x04,0x04,0x02,0x02,0x01,0x12,0x03,0x26,0x0b, +0x16,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x02,0x03, +0x12,0x03,0x26,0x19,0x1a,0x0a,0x0b,0x0a,0x04,0x04, +0x04,0x02,0x03,0x12,0x03,0x27,0x04,0x20,0x0a,0x0c, +0x0a,0x05,0x04,0x04,0x02,0x03,0x05,0x12,0x03,0x27, +0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x04,0x02,0x03, +0x01,0x12,0x03,0x27,0x0b,0x1b,0x0a,0x0c,0x0a,0x05, +0x04,0x04,0x02,0x03,0x03,0x12,0x03,0x27,0x1e,0x1f, +0x0a,0x0a,0x0a,0x02,0x04,0x05,0x12,0x04,0x2a,0x00, +0x2c,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x05,0x01,0x12, +0x03,0x2a,0x08,0x23,0x0a,0x0b,0x0a,0x04,0x04,0x05, +0x02,0x00,0x12,0x03,0x2b,0x04,0x38,0x0a,0x0c,0x0a, +0x05,0x04,0x05,0x02,0x00,0x04,0x12,0x03,0x2b,0x04, +0x0c,0x0a,0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x06, +0x12,0x03,0x2b,0x0d,0x27,0x0a,0x0c,0x0a,0x05,0x04, +0x05,0x02,0x00,0x01,0x12,0x03,0x2b,0x28,0x33,0x0a, +0x0c,0x0a,0x05,0x04,0x05,0x02,0x00,0x03,0x12,0x03, +0x2b,0x36,0x37,0x0a,0x0a,0x0a,0x02,0x04,0x06,0x12, +0x04,0x2e,0x00,0x32,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x06,0x01,0x12,0x03,0x2e,0x08,0x1e,0x0a,0x0b,0x0a, +0x04,0x04,0x06,0x02,0x00,0x12,0x03,0x2f,0x04,0x14, +0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x00,0x05,0x12, +0x03,0x2f,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x00,0x01,0x12,0x03,0x2f,0x0b,0x0f,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x00,0x03,0x12,0x03,0x2f, +0x12,0x13,0x0a,0x2b,0x0a,0x04,0x04,0x06,0x02,0x01, +0x12,0x03,0x30,0x04,0x16,0x22,0x1e,0x20,0x31,0x36, +0x20,0x62,0x69,0x74,0x73,0x2c,0x20,0x6c,0x65,0x66, +0x74,0x20,0x6d,0x73,0x62,0x2c,0x20,0x72,0x69,0x67, +0x68,0x74,0x20,0x6c,0x73,0x62,0x0a,0x0a,0x0c,0x0a, +0x05,0x04,0x06,0x02,0x01,0x05,0x12,0x03,0x30,0x04, +0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x01,0x01, +0x12,0x03,0x30,0x0b,0x11,0x0a,0x0c,0x0a,0x05,0x04, +0x06,0x02,0x01,0x03,0x12,0x03,0x30,0x14,0x15,0x0a, +0x2b,0x0a,0x04,0x04,0x06,0x02,0x02,0x12,0x03,0x31, +0x04,0x1d,0x22,0x1e,0x20,0x31,0x36,0x20,0x62,0x69, +0x74,0x73,0x2c,0x20,0x6c,0x65,0x66,0x74,0x20,0x6d, +0x73,0x62,0x2c,0x20,0x72,0x69,0x67,0x68,0x74,0x20, +0x6c,0x73,0x62,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x06, +0x02,0x02,0x05,0x12,0x03,0x31,0x04,0x0a,0x0a,0x0c, +0x0a,0x05,0x04,0x06,0x02,0x02,0x01,0x12,0x03,0x31, +0x0b,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x06,0x02,0x02, +0x03,0x12,0x03,0x31,0x1b,0x1c,0x0a,0x0a,0x0a,0x02, +0x04,0x07,0x12,0x04,0x34,0x00,0x36,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x07,0x01,0x12,0x03,0x34,0x08,0x1f, +0x0a,0x0b,0x0a,0x04,0x04,0x07,0x02,0x00,0x12,0x03, +0x35,0x04,0x30,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02, +0x00,0x04,0x12,0x03,0x35,0x04,0x0c,0x0a,0x0c,0x0a, +0x05,0x04,0x07,0x02,0x00,0x06,0x12,0x03,0x35,0x0d, +0x23,0x0a,0x0c,0x0a,0x05,0x04,0x07,0x02,0x00,0x01, +0x12,0x03,0x35,0x24,0x2b,0x0a,0x0c,0x0a,0x05,0x04, +0x07,0x02,0x00,0x03,0x12,0x03,0x35,0x2e,0x2f,0x0a, +0x0a,0x0a,0x02,0x04,0x08,0x12,0x04,0x38,0x00,0x3d, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x08,0x01,0x12,0x03, +0x38,0x08,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02, +0x00,0x12,0x03,0x39,0x04,0x19,0x0a,0x0c,0x0a,0x05, +0x04,0x08,0x02,0x00,0x05,0x12,0x03,0x39,0x04,0x0a, +0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x00,0x01,0x12, +0x03,0x39,0x0b,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x08, +0x02,0x00,0x03,0x12,0x03,0x39,0x17,0x18,0x0a,0x0b, +0x0a,0x04,0x04,0x08,0x02,0x01,0x12,0x03,0x3a,0x04, +0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x05, +0x12,0x03,0x3a,0x04,0x09,0x0a,0x0c,0x0a,0x05,0x04, +0x08,0x02,0x01,0x01,0x12,0x03,0x3a,0x0a,0x15,0x0a, +0x0c,0x0a,0x05,0x04,0x08,0x02,0x01,0x03,0x12,0x03, +0x3a,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x08,0x02, +0x02,0x12,0x03,0x3b,0x04,0x1b,0x0a,0x0c,0x0a,0x05, +0x04,0x08,0x02,0x02,0x05,0x12,0x03,0x3b,0x04,0x09, +0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x02,0x01,0x12, +0x03,0x3b,0x0a,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x08, +0x02,0x02,0x03,0x12,0x03,0x3b,0x19,0x1a,0x0a,0x0b, +0x0a,0x04,0x04,0x08,0x02,0x03,0x12,0x03,0x3c,0x04, +0x18,0x0a,0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x05, +0x12,0x03,0x3c,0x04,0x09,0x0a,0x0c,0x0a,0x05,0x04, +0x08,0x02,0x03,0x01,0x12,0x03,0x3c,0x0a,0x13,0x0a, +0x0c,0x0a,0x05,0x04,0x08,0x02,0x03,0x03,0x12,0x03, +0x3c,0x16,0x17,0x0a,0x0a,0x0a,0x02,0x04,0x09,0x12, +0x04,0x3f,0x00,0x45,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x09,0x01,0x12,0x03,0x3f,0x08,0x19,0x0a,0x0b,0x0a, +0x04,0x04,0x09,0x02,0x00,0x12,0x03,0x40,0x04,0x15, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x00,0x05,0x12, +0x03,0x40,0x04,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x00,0x01,0x12,0x03,0x40,0x09,0x10,0x0a,0x0c, +0x0a,0x05,0x04,0x09,0x02,0x00,0x03,0x12,0x03,0x40, +0x13,0x14,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x01, +0x12,0x03,0x41,0x04,0x19,0x0a,0x0c,0x0a,0x05,0x04, +0x09,0x02,0x01,0x05,0x12,0x03,0x41,0x04,0x0a,0x0a, +0x0c,0x0a,0x05,0x04,0x09,0x02,0x01,0x01,0x12,0x03, +0x41,0x0b,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, +0x01,0x03,0x12,0x03,0x41,0x17,0x18,0x0a,0x0b,0x0a, +0x04,0x04,0x09,0x02,0x02,0x12,0x03,0x42,0x04,0x17, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x02,0x05,0x12, +0x03,0x42,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x02,0x01,0x12,0x03,0x42,0x0b,0x12,0x0a,0x0c, +0x0a,0x05,0x04,0x09,0x02,0x02,0x03,0x12,0x03,0x42, +0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x09,0x02,0x03, +0x12,0x03,0x43,0x04,0x18,0x0a,0x0c,0x0a,0x05,0x04, +0x09,0x02,0x03,0x05,0x12,0x03,0x43,0x04,0x0a,0x0a, +0x0c,0x0a,0x05,0x04,0x09,0x02,0x03,0x01,0x12,0x03, +0x43,0x0b,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02, +0x03,0x03,0x12,0x03,0x43,0x16,0x17,0x0a,0x0b,0x0a, +0x04,0x04,0x09,0x02,0x04,0x12,0x03,0x44,0x04,0x19, +0x0a,0x0c,0x0a,0x05,0x04,0x09,0x02,0x04,0x05,0x12, +0x03,0x44,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x09, +0x02,0x04,0x01,0x12,0x03,0x44,0x0b,0x14,0x0a,0x0c, +0x0a,0x05,0x04,0x09,0x02,0x04,0x03,0x12,0x03,0x44, +0x17,0x18,0x0a,0x0a,0x0a,0x02,0x04,0x0a,0x12,0x04, +0x47,0x00,0x4e,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0a, +0x01,0x12,0x03,0x47,0x08,0x16,0x0a,0x0b,0x0a,0x04, +0x04,0x0a,0x02,0x00,0x12,0x03,0x48,0x04,0x15,0x0a, +0x0c,0x0a,0x05,0x04,0x0a,0x02,0x00,0x05,0x12,0x03, +0x48,0x04,0x0b,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02, +0x00,0x01,0x12,0x03,0x48,0x0c,0x10,0x0a,0x0c,0x0a, +0x05,0x04,0x0a,0x02,0x00,0x03,0x12,0x03,0x48,0x13, +0x14,0x0a,0x0b,0x0a,0x04,0x04,0x0a,0x02,0x01,0x12, +0x03,0x49,0x04,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0a, +0x02,0x01,0x05,0x12,0x03,0x49,0x04,0x0a,0x0a,0x0c, +0x0a,0x05,0x04,0x0a,0x02,0x01,0x01,0x12,0x03,0x49, +0x0b,0x0f,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x01, +0x03,0x12,0x03,0x49,0x12,0x13,0x0a,0x0b,0x0a,0x04, +0x04,0x0a,0x02,0x02,0x12,0x03,0x4a,0x04,0x15,0x0a, +0x0c,0x0a,0x05,0x04,0x0a,0x02,0x02,0x05,0x12,0x03, +0x4a,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02, +0x02,0x01,0x12,0x03,0x4a,0x0b,0x10,0x0a,0x0c,0x0a, +0x05,0x04,0x0a,0x02,0x02,0x03,0x12,0x03,0x4a,0x13, +0x14,0x0a,0x0b,0x0a,0x04,0x04,0x0a,0x02,0x03,0x12, +0x03,0x4b,0x04,0x1b,0x0a,0x0c,0x0a,0x05,0x04,0x0a, +0x02,0x03,0x05,0x12,0x03,0x4b,0x04,0x0a,0x0a,0x0c, +0x0a,0x05,0x04,0x0a,0x02,0x03,0x01,0x12,0x03,0x4b, +0x0b,0x16,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x03, +0x03,0x12,0x03,0x4b,0x19,0x1a,0x0a,0x0b,0x0a,0x04, +0x04,0x0a,0x02,0x04,0x12,0x03,0x4c,0x04,0x18,0x0a, +0x0c,0x0a,0x05,0x04,0x0a,0x02,0x04,0x05,0x12,0x03, +0x4c,0x04,0x09,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02, +0x04,0x01,0x12,0x03,0x4c,0x0a,0x13,0x0a,0x0c,0x0a, +0x05,0x04,0x0a,0x02,0x04,0x03,0x12,0x03,0x4c,0x16, +0x17,0x0a,0x0b,0x0a,0x04,0x04,0x0a,0x02,0x05,0x12, +0x03,0x4d,0x04,0x1e,0x0a,0x0c,0x0a,0x05,0x04,0x0a, +0x02,0x05,0x05,0x12,0x03,0x4d,0x04,0x09,0x0a,0x0c, +0x0a,0x05,0x04,0x0a,0x02,0x05,0x01,0x12,0x03,0x4d, +0x0a,0x19,0x0a,0x0c,0x0a,0x05,0x04,0x0a,0x02,0x05, +0x03,0x12,0x03,0x4d,0x1c,0x1d,0x0a,0x0a,0x0a,0x02, +0x04,0x0b,0x12,0x04,0x50,0x00,0x52,0x01,0x0a,0x0a, +0x0a,0x03,0x04,0x0b,0x01,0x12,0x03,0x50,0x08,0x20, +0x0a,0x0b,0x0a,0x04,0x04,0x0b,0x02,0x00,0x12,0x03, +0x51,0x04,0x26,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02, +0x00,0x04,0x12,0x03,0x51,0x04,0x0c,0x0a,0x0c,0x0a, +0x05,0x04,0x0b,0x02,0x00,0x06,0x12,0x03,0x51,0x0d, +0x1b,0x0a,0x0c,0x0a,0x05,0x04,0x0b,0x02,0x00,0x01, +0x12,0x03,0x51,0x1c,0x21,0x0a,0x0c,0x0a,0x05,0x04, +0x0b,0x02,0x00,0x03,0x12,0x03,0x51,0x24,0x25,0x0a, +0x0a,0x0a,0x02,0x04,0x0c,0x12,0x04,0x54,0x00,0x59, +0x01,0x0a,0x0a,0x0a,0x03,0x04,0x0c,0x01,0x12,0x03, +0x54,0x08,0x22,0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02, +0x00,0x12,0x03,0x55,0x04,0x24,0x0a,0x0c,0x0a,0x05, +0x04,0x0c,0x02,0x00,0x06,0x12,0x03,0x55,0x04,0x15, +0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x00,0x01,0x12, +0x03,0x55,0x16,0x1f,0x0a,0x0c,0x0a,0x05,0x04,0x0c, +0x02,0x00,0x03,0x12,0x03,0x55,0x22,0x23,0x0a,0x0b, +0x0a,0x04,0x04,0x0c,0x02,0x01,0x12,0x03,0x56,0x04, +0x19,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x05, +0x12,0x03,0x56,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04, +0x0c,0x02,0x01,0x01,0x12,0x03,0x56,0x0b,0x14,0x0a, +0x0c,0x0a,0x05,0x04,0x0c,0x02,0x01,0x03,0x12,0x03, +0x56,0x17,0x18,0x0a,0x0b,0x0a,0x04,0x04,0x0c,0x02, +0x02,0x12,0x03,0x57,0x04,0x1d,0x0a,0x0c,0x0a,0x05, +0x04,0x0c,0x02,0x02,0x05,0x12,0x03,0x57,0x04,0x09, +0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x02,0x01,0x12, +0x03,0x57,0x0a,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x0c, +0x02,0x02,0x03,0x12,0x03,0x57,0x1b,0x1c,0x0a,0x0b, +0x0a,0x04,0x04,0x0c,0x02,0x03,0x12,0x03,0x58,0x04, +0x1c,0x0a,0x0c,0x0a,0x05,0x04,0x0c,0x02,0x03,0x05, +0x12,0x03,0x58,0x04,0x09,0x0a,0x0c,0x0a,0x05,0x04, +0x0c,0x02,0x03,0x01,0x12,0x03,0x58,0x0a,0x17,0x0a, +0x0c,0x0a,0x05,0x04,0x0c,0x02,0x03,0x03,0x12,0x03, +0x58,0x1a,0x1b,0x0a,0x0a,0x0a,0x02,0x04,0x0d,0x12, +0x04,0x5b,0x00,0x5f,0x01,0x0a,0x0a,0x0a,0x03,0x04, +0x0d,0x01,0x12,0x03,0x5b,0x08,0x24,0x0a,0x0b,0x0a, +0x04,0x04,0x0d,0x02,0x00,0x12,0x03,0x5c,0x04,0x1b, +0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x00,0x05,0x12, +0x03,0x5c,0x04,0x0a,0x0a,0x0c,0x0a,0x05,0x04,0x0d, +0x02,0x00,0x01,0x12,0x03,0x5c,0x0b,0x16,0x0a,0x0c, +0x0a,0x05,0x04,0x0d,0x02,0x00,0x03,0x12,0x03,0x5c, +0x19,0x1a,0x0a,0x0b,0x0a,0x04,0x04,0x0d,0x02,0x01, +0x12,0x03,0x5d,0x04,0x19,0x0a,0x0c,0x0a,0x05,0x04, +0x0d,0x02,0x01,0x05,0x12,0x03,0x5d,0x04,0x0a,0x0a, +0x0c,0x0a,0x05,0x04,0x0d,0x02,0x01,0x01,0x12,0x03, +0x5d,0x0b,0x14,0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02, +0x01,0x03,0x12,0x03,0x5d,0x17,0x18,0x0a,0x0b,0x0a, +0x04,0x04,0x0d,0x02,0x02,0x12,0x03,0x5e,0x04,0x1d, +0x0a,0x0c,0x0a,0x05,0x04,0x0d,0x02,0x02,0x05,0x12, +0x03,0x5e,0x04,0x09,0x0a,0x0c,0x0a,0x05,0x04,0x0d, +0x02,0x02,0x01,0x12,0x03,0x5e,0x0a,0x18,0x0a,0x0c, +0x0a,0x05,0x04,0x0d,0x02,0x02,0x03,0x12,0x03,0x5e, +0x1b,0x1c,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, + }; static const char file_name[] = "MrcComm.proto"; static const char mrc_proto_ProtobufFingerData_name[] = "mrc.proto.ProtobufFingerData"; diff --git a/hal/src/generated/main/native/cpp/mrc/protobuf/MrcComm.npb.h b/hal/src/generated/main/native/cpp/mrc/protobuf/MrcComm.npb.h index c6728be9f3..f3ebcf9bde 100644 --- a/hal/src/generated/main/native/cpp/mrc/protobuf/MrcComm.npb.h +++ b/hal/src/generated/main/native/cpp/mrc/protobuf/MrcComm.npb.h @@ -54,10 +54,10 @@ typedef struct _mrc_proto_ProtobufControlData { static std::string_view msg_name(void) noexcept; static pb_filedesc_t file_descriptor(void) noexcept; - uint32_t ControlWord; int32_t MatchTime; pb_callback_t Joysticks; uint64_t CurrentOpMode; + uint32_t ControlWord; } mrc_proto_ProtobufControlData; typedef struct _mrc_proto_ProtobufJoystickDescriptor { @@ -127,6 +127,10 @@ typedef struct _mrc_proto_ProtobufOpMode { uint64_t Hash; pb_callback_t Name; + pb_callback_t Group; + pb_callback_t Description; + int32_t TextColor; + int32_t BackgroundColor; } mrc_proto_ProtobufOpMode; typedef struct _mrc_proto_ProtobufAvailableOpModes { @@ -163,28 +167,28 @@ typedef struct _mrc_proto_ProtobufConsoleLineTimestamp { #define mrc_proto_ProtobufFingerData_init_default {0, 0, 0} #define mrc_proto_ProtobufTouchpadData_init_default {{{NULL}, NULL}} #define mrc_proto_ProtobufJoystickData_init_default {0, 0, 0, {{NULL}, NULL}, 0, 0, {{NULL}, NULL}} -#define mrc_proto_ProtobufControlData_init_default {0, 0, {{NULL}, NULL}, 0} +#define mrc_proto_ProtobufControlData_init_default {0, {{NULL}, NULL}, 0, 0} #define mrc_proto_ProtobufJoystickDescriptor_init_default {{{NULL}, NULL}, 0, 0, 0} #define mrc_proto_ProtobufJoystickDescriptors_init_default {{{NULL}, NULL}} #define mrc_proto_ProtobufJoystickOutput_init_default {0, 0, 0} #define mrc_proto_ProtobufJoystickOutputs_init_default {{{NULL}, NULL}} #define mrc_proto_ProtobufMatchInfo_init_default {{{NULL}, NULL}, 0, 0, 0} #define mrc_proto_ProtobufErrorInfo_init_default {0, 0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}} -#define mrc_proto_ProtobufOpMode_init_default {0, {{NULL}, NULL}} +#define mrc_proto_ProtobufOpMode_init_default {0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, 0, 0} #define mrc_proto_ProtobufAvailableOpModes_init_default {{{NULL}, NULL}} #define mrc_proto_ProtobufErrorInfoTimestamp_init_default {{{NULL}, NULL}, 0, 0, 0} #define mrc_proto_ProtobufConsoleLineTimestamp_init_default {{{NULL}, NULL}, 0, 0} #define mrc_proto_ProtobufFingerData_init_zero {0, 0, 0} #define mrc_proto_ProtobufTouchpadData_init_zero {{{NULL}, NULL}} #define mrc_proto_ProtobufJoystickData_init_zero {0, 0, 0, {{NULL}, NULL}, 0, 0, {{NULL}, NULL}} -#define mrc_proto_ProtobufControlData_init_zero {0, 0, {{NULL}, NULL}, 0} +#define mrc_proto_ProtobufControlData_init_zero {0, {{NULL}, NULL}, 0, 0} #define mrc_proto_ProtobufJoystickDescriptor_init_zero {{{NULL}, NULL}, 0, 0, 0} #define mrc_proto_ProtobufJoystickDescriptors_init_zero {{{NULL}, NULL}} #define mrc_proto_ProtobufJoystickOutput_init_zero {0, 0, 0} #define mrc_proto_ProtobufJoystickOutputs_init_zero {{{NULL}, NULL}} #define mrc_proto_ProtobufMatchInfo_init_zero {{{NULL}, NULL}, 0, 0, 0} #define mrc_proto_ProtobufErrorInfo_init_zero {0, 0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}} -#define mrc_proto_ProtobufOpMode_init_zero {0, {{NULL}, NULL}} +#define mrc_proto_ProtobufOpMode_init_zero {0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, 0, 0} #define mrc_proto_ProtobufAvailableOpModes_init_zero {{{NULL}, NULL}} #define mrc_proto_ProtobufErrorInfoTimestamp_init_zero {{{NULL}, NULL}, 0, 0, 0} #define mrc_proto_ProtobufConsoleLineTimestamp_init_zero {{{NULL}, NULL}, 0, 0} @@ -201,10 +205,10 @@ typedef struct _mrc_proto_ProtobufConsoleLineTimestamp { #define mrc_proto_ProtobufJoystickData_POVCount_tag 5 #define mrc_proto_ProtobufJoystickData_POVs_tag 6 #define mrc_proto_ProtobufJoystickData_Touchpads_tag 7 -#define mrc_proto_ProtobufControlData_ControlWord_tag 1 #define mrc_proto_ProtobufControlData_MatchTime_tag 2 #define mrc_proto_ProtobufControlData_Joysticks_tag 3 #define mrc_proto_ProtobufControlData_CurrentOpMode_tag 4 +#define mrc_proto_ProtobufControlData_ControlWord_tag 5 #define mrc_proto_ProtobufJoystickDescriptor_JoystickName_tag 1 #define mrc_proto_ProtobufJoystickDescriptor_IsGamepad_tag 2 #define mrc_proto_ProtobufJoystickDescriptor_GamepadType_tag 3 @@ -225,6 +229,10 @@ typedef struct _mrc_proto_ProtobufConsoleLineTimestamp { #define mrc_proto_ProtobufErrorInfo_CallStack_tag 5 #define mrc_proto_ProtobufOpMode_Hash_tag 1 #define mrc_proto_ProtobufOpMode_Name_tag 2 +#define mrc_proto_ProtobufOpMode_Group_tag 3 +#define mrc_proto_ProtobufOpMode_Description_tag 4 +#define mrc_proto_ProtobufOpMode_TextColor_tag 5 +#define mrc_proto_ProtobufOpMode_BackgroundColor_tag 6 #define mrc_proto_ProtobufAvailableOpModes_Modes_tag 1 #define mrc_proto_ProtobufErrorInfoTimestamp_ErrorInfo_tag 1 #define mrc_proto_ProtobufErrorInfoTimestamp_Timestamp_tag 2 @@ -261,10 +269,10 @@ X(a, CALLBACK, REPEATED, MESSAGE, Touchpads, 7) #define mrc_proto_ProtobufJoystickData_Touchpads_MSGTYPE mrc_proto_ProtobufTouchpadData #define mrc_proto_ProtobufControlData_FIELDLIST(X, a) \ -X(a, STATIC, SINGULAR, UINT32, ControlWord, 1) \ X(a, STATIC, SINGULAR, INT32, MatchTime, 2) \ X(a, CALLBACK, REPEATED, MESSAGE, Joysticks, 3) \ -X(a, STATIC, SINGULAR, FIXED64, CurrentOpMode, 4) +X(a, STATIC, SINGULAR, FIXED64, CurrentOpMode, 4) \ +X(a, STATIC, SINGULAR, UINT32, ControlWord, 5) #define mrc_proto_ProtobufControlData_CALLBACK pb_default_field_callback #define mrc_proto_ProtobufControlData_DEFAULT NULL #define mrc_proto_ProtobufControlData_Joysticks_MSGTYPE mrc_proto_ProtobufJoystickData @@ -315,7 +323,11 @@ X(a, CALLBACK, SINGULAR, STRING, CallStack, 5) #define mrc_proto_ProtobufOpMode_FIELDLIST(X, a) \ X(a, STATIC, SINGULAR, FIXED64, Hash, 1) \ -X(a, CALLBACK, SINGULAR, STRING, Name, 2) +X(a, CALLBACK, SINGULAR, STRING, Name, 2) \ +X(a, CALLBACK, SINGULAR, STRING, Group, 3) \ +X(a, CALLBACK, SINGULAR, STRING, Description, 4) \ +X(a, STATIC, SINGULAR, INT32, TextColor, 5) \ +X(a, STATIC, SINGULAR, INT32, BackgroundColor, 6) #define mrc_proto_ProtobufOpMode_CALLBACK pb_default_field_callback #define mrc_proto_ProtobufOpMode_DEFAULT NULL diff --git a/hal/src/main/java/org/wpilib/hardware/hal/ControlWord.java b/hal/src/main/java/org/wpilib/hardware/hal/ControlWord.java index 2855842a2e..4fac7e18c0 100644 --- a/hal/src/main/java/org/wpilib/hardware/hal/ControlWord.java +++ b/hal/src/main/java/org/wpilib/hardware/hal/ControlWord.java @@ -4,31 +4,59 @@ package org.wpilib.hardware.hal; +import org.wpilib.hardware.hal.struct.ControlWordStruct; + /** A wrapper for the HALControlWord bitfield. */ public class ControlWord { - private boolean m_enabled; - private boolean m_autonomous; - private boolean m_test; - private boolean m_emergencyStop; - private boolean m_fmsAttached; - private boolean m_dsAttached; + private static final long OPMODE_HASH_MASK = 0x00FFFFFFFFFFFFFFL; + private static final long ROBOT_MODE_MASK = 0x0300000000000000L; + private static final long ROBOT_MODE_SHIFT = 56; + private static final long ENABLED_MASK = 0x0400000000000000L; + private static final long ESTOP_MASK = 0x0800000000000000L; + private static final long FMS_ATTACHED_MASK = 0x1000000000000000L; + private static final long DS_ATTACHED_MASK = 0x2000000000000000L; + + private long m_word; + private RobotMode m_robotMode = RobotMode.UNKNOWN; /** Default constructor. */ public ControlWord() {} - void update( + /** + * Updates from state values. + * + * @param opModeHash opmode hash + * @param robotMode robot mode + * @param enabled enabled + * @param emergencyStop emergency stopped + * @param fmsAttached FMS attached + * @param dsAttached DS attached + */ + public void update( + long opModeHash, + RobotMode robotMode, boolean enabled, - boolean autonomous, - boolean test, boolean emergencyStop, boolean fmsAttached, boolean dsAttached) { - m_enabled = enabled; - m_autonomous = autonomous; - m_test = test; - m_emergencyStop = emergencyStop; - m_fmsAttached = fmsAttached; - m_dsAttached = dsAttached; + m_word = + (opModeHash & OPMODE_HASH_MASK) + | ((long) robotMode.getValue() << ROBOT_MODE_SHIFT) + | (enabled ? ENABLED_MASK : 0) + | (emergencyStop ? ESTOP_MASK : 0) + | (fmsAttached ? FMS_ATTACHED_MASK : 0) + | (dsAttached ? DS_ATTACHED_MASK : 0); + m_robotMode = robotMode; + } + + /** + * Updates from the native HAL value. + * + * @param word value + */ + public void update(long word) { + m_word = word; + m_robotMode = RobotMode.fromInt((int) ((word & ROBOT_MODE_MASK) >> ROBOT_MODE_SHIFT)); } /** @@ -37,12 +65,43 @@ public class ControlWord { * @param word word to update from */ public void update(ControlWord word) { - m_enabled = word.m_enabled; - m_autonomous = word.m_autonomous; - m_test = word.m_test; - m_emergencyStop = word.m_emergencyStop; - m_fmsAttached = word.m_fmsAttached; - m_dsAttached = word.m_dsAttached; + m_word = word.m_word; + m_robotMode = word.m_robotMode; + } + + /** + * Gets the opmode ID. + * + * @return the opmode ID + */ + public long getOpModeId() { + // if the hash portion is zero, return 0 + if ((m_word & OPMODE_HASH_MASK) == 0) { + return 0; + } + // otherwise return the full ID (which includes the robot mode) + return m_word & (OPMODE_HASH_MASK | ROBOT_MODE_MASK); + } + + /** + * Sets the opmode ID. + * + * @param id opmode ID + */ + public void setOpModeId(long id) { + m_word &= ~(OPMODE_HASH_MASK | ROBOT_MODE_MASK); + m_word |= id & (OPMODE_HASH_MASK | ROBOT_MODE_MASK); + // keep robot mode in sync + m_robotMode = RobotMode.fromInt((int) ((m_word & ROBOT_MODE_MASK) >> ROBOT_MODE_SHIFT)); + } + + /** + * Gets the robot mode. + * + * @return the robot mode + */ + public RobotMode getRobotMode() { + return m_robotMode; } /** @@ -50,26 +109,8 @@ public class ControlWord { * * @return the Enabled flag */ - public boolean getEnabled() { - return m_enabled; - } - - /** - * Gets the Autonomous mode flag. - * - * @return the Autonomous mode flag - */ - public boolean getAutonomous() { - return m_autonomous; - } - - /** - * Gets the Test mode flag. - * - * @return the Test mode flag - */ - public boolean getTest() { - return m_test; + public boolean isEnabled() { + return (m_word & ENABLED_MASK) != 0; } /** @@ -77,8 +118,8 @@ public class ControlWord { * * @return the E-Stop flag */ - public boolean getEStop() { - return m_emergencyStop; + public boolean isEStopped() { + return (m_word & ESTOP_MASK) != 0; } /** @@ -86,8 +127,8 @@ public class ControlWord { * * @return the FMS attached flag */ - public boolean getFMSAttached() { - return m_fmsAttached; + public boolean isFMSAttached() { + return (m_word & FMS_ATTACHED_MASK) != 0; } /** @@ -95,7 +136,88 @@ public class ControlWord { * * @return the DS attached flag */ - public boolean getDSAttached() { - return m_dsAttached; + public boolean isDSAttached() { + return (m_word & DS_ATTACHED_MASK) != 0; } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * autonomous mode. + * + * @return True if autonomous mode should be enabled, false otherwise. + */ + public boolean isAutonomous() { + return getRobotMode() == RobotMode.AUTONOMOUS; + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * autonomous mode and enabled. + * + * @return True if autonomous should be set and the robot should be enabled. + */ + public boolean isAutonomousEnabled() { + return isAutonomous() && isEnabled() && isDSAttached(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controlled mode. + * + * @return True if operator-controlled mode should be enabled, false otherwise. + */ + public boolean isTeleop() { + return getRobotMode() == RobotMode.TELEOPERATED; + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in + * operator-controller mode and enabled. + * + * @return True if operator-controlled mode should be set and the robot should be enabled. + */ + public boolean isTeleopEnabled() { + return isTeleop() && isEnabled() && isDSAttached(); + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in test + * mode. + * + * @return True if test mode should be enabled, false otherwise. + */ + public boolean isTest() { + return getRobotMode() == RobotMode.TEST; + } + + /** + * Gets a value indicating whether the Driver Station requires the robot to be running in test + * mode and enabled. + * + * @return True if test mode should be set and the robot should be enabled. + */ + public boolean isTestEnabled() { + return isTest() && isEnabled() && isDSAttached(); + } + + /** + * Gets the native HAL control word value. + * + * @return control word value + */ + public long getNative() { + return m_word; + } + + @Override + public boolean equals(Object obj) { + return obj instanceof ControlWord word && m_word == word.m_word; + } + + @Override + public int hashCode() { + return Long.hashCode(m_word); + } + + public static final ControlWordStruct struct = new ControlWordStruct(); } diff --git a/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java b/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java index f6a0fc32c8..db76350815 100644 --- a/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java +++ b/hal/src/main/java/org/wpilib/hardware/hal/DriverStationJNI.java @@ -20,44 +20,14 @@ public class DriverStationJNI extends JNIWrapper { public static native void observeUserProgramStarting(); /** - * Sets the disabled flag in the DS. + * Sets the control state returned to the DS. * *

This is used for the DS to ensure the robot is properly responding to its state request. * Ensure this gets called about every 50ms, or the robot will be disabled by the DS. * - * @see "HAL_ObserveUserProgramDisabled" + * @param word control word returned by nativeGetControlWord() */ - public static native void observeUserProgramDisabled(); - - /** - * Sets the autonomous enabled flag in the DS. - * - *

This is used for the DS to ensure the robot is properly responding to its state request. - * Ensure this gets called about every 50ms, or the robot will be disabled by the DS. - * - * @see "HAL_ObserveUserProgramAutonomous" - */ - public static native void observeUserProgramAutonomous(); - - /** - * Sets the teleoperated enabled flag in the DS. - * - *

This is used for the DS to ensure the robot is properly responding to its state request. - * Ensure this gets called about every 50ms, or the robot will be disabled by the DS. - * - * @see "HAL_ObserveUserProgramTeleop" - */ - public static native void observeUserProgramTeleop(); - - /** - * Sets the test mode flag in the DS. - * - *

This is used for the DS to ensure the robot is properly responding to its state request. - * Ensure this gets called about every 50ms, or the robot will be disabled by the DS. - * - * @see "HAL_ObserveUserProgramTest" - */ - public static native void observeUserProgramTest(); + public static native void observeUserProgram(long word); /** * Gets the current control word of the driver station. @@ -68,7 +38,7 @@ public class DriverStationJNI extends JNIWrapper { * @see "HAL_GetControlWord" * @see getControlWord for a version easier to parse */ - public static native int nativeGetControlWord(); + public static native long nativeGetControlWord(); /** * Gets the current control word of the driver station. @@ -79,16 +49,41 @@ public class DriverStationJNI extends JNIWrapper { * @see "HAL_GetControlWord" */ public static void getControlWord(ControlWord controlWord) { - int word = nativeGetControlWord(); - controlWord.update( - (word & 1) != 0, - ((word >> 1) & 1) != 0, - ((word >> 2) & 1) != 0, - ((word >> 3) & 1) != 0, - ((word >> 4) & 1) != 0, - ((word >> 5) & 1) != 0); + controlWord.update(nativeGetControlWord()); } + /** + * Gets the current control word of the driver station. Unlike nativeGetControlWord, this function + * gets the latest value rather than using the value cached by refreshDSData(). + * + *

The control word contains the robot state. + * + * @return the control word + * @see "HAL_GetUncachedControlWord" + * @see getUncachedControlWord for a version easier to parse + */ + public static native long nativeGetUncachedControlWord(); + + /** + * Gets the current control word of the driver station. Unlike getControlWord, this function gets + * the latest value rather than using the value cached by refreshDSData(). + * + *

The control work contains the robot state. + * + * @param controlWord the ControlWord to update + * @see "HAL_GetControlWord" + */ + public static void getUncachedControlWord(ControlWord controlWord) { + controlWord.update(nativeGetUncachedControlWord()); + } + + /** + * Sets operating mode options. + * + * @param options operating mode options + */ + public static native void setOpModeOptions(OpModeOption[] options); + /** * Gets the current alliance station ID. * diff --git a/hal/src/main/java/org/wpilib/hardware/hal/OpModeOption.java b/hal/src/main/java/org/wpilib/hardware/hal/OpModeOption.java new file mode 100644 index 0000000000..0e471823d8 --- /dev/null +++ b/hal/src/main/java/org/wpilib/hardware/hal/OpModeOption.java @@ -0,0 +1,67 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.hal; + +/** An individual opmode option. */ +public class OpModeOption { + /** Unique id. Encodes robot mode in bits 57-56, LSB 56 bits is hash of name. */ + @SuppressWarnings("MemberName") + public final long id; + + @SuppressWarnings("MemberName") + public final String name; + + @SuppressWarnings("MemberName") + public final String group; + + @SuppressWarnings("MemberName") + public final String description; + + @SuppressWarnings("MemberName") + public final int textColor; + + @SuppressWarnings("MemberName") + public final int backgroundColor; + + /** + * Constructor. + * + * @param id id + * @param name name + * @param group group + * @param description description + * @param textColor text color (0x00RRGGBB or -1 for default) + * @param backgroundColor background color (0x00RRGGBB or -1 for default) + */ + public OpModeOption( + long id, String name, String group, String description, int textColor, int backgroundColor) { + this.id = id; + this.name = name; + this.group = group; + this.description = description; + this.textColor = textColor; + this.backgroundColor = backgroundColor; + } + + /** + * Gets the robot mode encoded in the ID. + * + * @return robot mode + */ + public RobotMode getMode() { + return RobotMode.fromInt((int) ((id >> 56) & 0x3)); + } + + /** + * Makes an ID from a robot mode and a hash. + * + * @param mode robot mode + * @param hash hash of name + * @return ID + */ + public static long makeId(RobotMode mode, long hash) { + return ((mode.getValue() & 0x3L) << 56) | (hash & 0x00FFFFFFFFFFFFFFL); + } +} diff --git a/hal/src/main/java/org/wpilib/hardware/hal/RobotMode.java b/hal/src/main/java/org/wpilib/hardware/hal/RobotMode.java new file mode 100644 index 0000000000..02b3e5713d --- /dev/null +++ b/hal/src/main/java/org/wpilib/hardware/hal/RobotMode.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.hal; + +/** Robot mode. Note this does not indicate enabled state. */ +public enum RobotMode { + /** Unknown. */ + UNKNOWN(0), + /** Autonomous. */ + AUTONOMOUS(1), + /** Teleoperated. */ + TELEOPERATED(2), + /** Test. */ + TEST(3); + + private final int value; + + RobotMode(int value) { + this.value = value; + } + + /** + * Gets the integer value for the mode. + * + * @return value + */ + public int getValue() { + return value; + } + + /** Gets a mode from an integer value. */ + public static RobotMode fromInt(int value) { + return switch (value) { + case 1 -> AUTONOMOUS; + case 2 -> TELEOPERATED; + case 3 -> TEST; + default -> UNKNOWN; + }; + } +} diff --git a/hal/src/main/java/org/wpilib/hardware/hal/simulation/DriverStationDataJNI.java b/hal/src/main/java/org/wpilib/hardware/hal/simulation/DriverStationDataJNI.java index d21953c3ca..94e414723c 100644 --- a/hal/src/main/java/org/wpilib/hardware/hal/simulation/DriverStationDataJNI.java +++ b/hal/src/main/java/org/wpilib/hardware/hal/simulation/DriverStationDataJNI.java @@ -4,7 +4,10 @@ package org.wpilib.hardware.hal.simulation; +import java.util.function.BiConsumer; import org.wpilib.hardware.hal.JNIWrapper; +import org.wpilib.hardware.hal.OpModeOption; +import org.wpilib.hardware.hal.RobotMode; /** JNI for Driver Station data. */ public class DriverStationDataJNI extends JNIWrapper { @@ -16,22 +19,22 @@ public class DriverStationDataJNI extends JNIWrapper { public static native void setEnabled(boolean enabled); - public static native int registerAutonomousCallback( + public static native int registerRobotModeCallback( NotifyCallback callback, boolean initialNotify); - public static native void cancelAutonomousCallback(int uid); + public static native void cancelRobotModeCallback(int uid); - public static native boolean getAutonomous(); + private static native int nativeGetRobotMode(); - public static native void setAutonomous(boolean autonomous); + public static RobotMode getRobotMode() { + return RobotMode.fromInt(nativeGetRobotMode()); + } - public static native int registerTestCallback(NotifyCallback callback, boolean initialNotify); + private static native void nativeSetRobotMode(int mode); - public static native void cancelTestCallback(int uid); - - public static native boolean getTest(); - - public static native void setTest(boolean test); + public static void setRobotMode(RobotMode mode) { + nativeSetRobotMode(mode.getValue()); + } public static native int registerEStopCallback(NotifyCallback callback, boolean initialNotify); @@ -77,6 +80,21 @@ public class DriverStationDataJNI extends JNIWrapper { public static native void setMatchTime(double matchTime); + public static native int registerOpModeCallback(NotifyCallback callback, boolean initialNotify); + + public static native void cancelOpModeCallback(int uid); + + public static native long getOpMode(); + + public static native void setOpMode(long opMode); + + public static native int registerOpModeOptionsCallback( + BiConsumer callback, boolean initialNotify); + + public static native void cancelOpModeOptionsCallback(int uid); + + public static native OpModeOption[] getOpModeOptions(); + public static native void setJoystickAxes( byte joystickNum, float[] axesArray, short availableAxes); diff --git a/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java b/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java index dd9e74ce2e..3ca71ce596 100644 --- a/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java +++ b/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java @@ -4,6 +4,7 @@ package org.wpilib.hardware.hal.simulation; +import org.wpilib.hardware.hal.ControlWord; import org.wpilib.hardware.hal.JNIWrapper; /** JNI for simulator. */ @@ -16,6 +17,14 @@ public class SimulatorJNI extends JNIWrapper { public static native boolean getProgramStarted(); + public static native void setProgramState(long word); + + public static native long nativeGetProgramState(); + + public static void getProgramState(ControlWord controlWord) { + controlWord.update(nativeGetProgramState()); + } + public static native void restartTiming(); public static native void pauseTiming(); diff --git a/hal/src/main/java/org/wpilib/hardware/hal/struct/ControlWordStruct.java b/hal/src/main/java/org/wpilib/hardware/hal/struct/ControlWordStruct.java new file mode 100644 index 0000000000..93eb797640 --- /dev/null +++ b/hal/src/main/java/org/wpilib/hardware/hal/struct/ControlWordStruct.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.hal.struct; + +import java.nio.ByteBuffer; +import org.wpilib.hardware.hal.ControlWord; +import org.wpilib.util.struct.Struct; + +public class ControlWordStruct implements Struct { + @Override + public Class getTypeClass() { + return ControlWord.class; + } + + @Override + public String getTypeName() { + return "ControlWord"; + } + + @Override + public int getSize() { + return 8; + } + + @Override + public String getSchema() { + return "uint64 opModeHash:56;" + + "enum{unknown=0,autonomous=1,teleoperated=2,test=3} uint64 robotMode:2;" + + "bool enabled:1;bool eStop:1;bool fmsAttached:1;bool dsAttached:1;"; + } + + @Override + public ControlWord unpack(ByteBuffer bb) { + ControlWord word = new ControlWord(); + unpackInto(word, bb); + return word; + } + + @Override + public void unpackInto(ControlWord out, ByteBuffer bb) { + out.update(bb.getLong()); + } + + @Override + public void pack(ByteBuffer bb, ControlWord value) { + bb.putLong(value.getNative()); + } +} diff --git a/hal/src/main/native/cpp/DashboardOpMode.cpp b/hal/src/main/native/cpp/DashboardOpMode.cpp new file mode 100644 index 0000000000..c793675a88 --- /dev/null +++ b/hal/src/main/native/cpp/DashboardOpMode.cpp @@ -0,0 +1,162 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/hal/DashboardOpMode.hpp" + +#include +#include +#include + +#include + +#include "wpi/hal/DriverStationTypes.h" +#include "wpi/nt/NetworkTableInstance.hpp" +#include "wpi/nt/NetworkTableListener.hpp" +#include "wpi/nt/StringArrayTopic.hpp" +#include "wpi/nt/StringTopic.hpp" +#include "wpi/nt/ntcore_cpp.hpp" +#include "wpi/util/StringMap.hpp" +#include "wpi/util/mutex.hpp" +#include "wpi/util/string.h" + +using namespace wpi; + +namespace { +class DashboardOpModeSender { + public: + void Start(nt::NetworkTableInstance inst, std::string_view tableName) { + m_typeTopic = inst.GetStringTopic(fmt::format("{}/.type", tableName)); + m_optionsTopic = + inst.GetStringArrayTopic(fmt::format("{}/options", tableName)); + m_activeTopic = inst.GetStringTopic(fmt::format("{}/active", tableName)); + m_selectedSub = inst.GetStringTopic(fmt::format("{}/selected", tableName)) + .Subscribe(""); + m_selectedListener = nt::NetworkTableListener::CreateListener( + m_selectedSub, NT_EVENT_VALUE_ALL | NT_EVENT_IMMEDIATE, + [this](const nt::Event& event) { + if (auto data = event.GetValueEventData()) { + if (data->value.IsString()) { + m_activePub.Set(data->value.GetString()); + } + } + }); + } + + void Enable() { + m_typePub = m_typeTopic.Publish(); + m_typePub.Set("String Chooser"); + + m_optionsPub = m_optionsTopic.Publish(); + m_optionsPub.Set(m_options); + + m_activePub = m_activeTopic.Publish(); + m_activePub.Set(""); + } + + void SetOptions(std::span options, + HAL_RobotMode mode) { + m_optionMap.clear(); + m_options.clear(); + for (auto&& option : options) { + if (HAL_OpMode_GetRobotMode(option.id) == mode) { + auto name = util::to_string_view(&option.name); + m_optionMap[name] = option.id; + m_options.emplace_back(name); + } + } + if (m_optionsPub) { + m_optionsPub.Set(m_options); + } + } + + int64_t GetSelected() const { + auto it = m_optionMap.find(m_selectedSub.Get()); + if (it == m_optionMap.end()) { + return 0; + } + return it->second; + } + + private: + nt::StringTopic m_typeTopic; + nt::StringPublisher m_typePub; + + nt::StringArrayTopic m_optionsTopic; + nt::StringArrayPublisher m_optionsPub; + + nt::StringTopic m_activeTopic; + nt::StringPublisher m_activePub; + + nt::StringSubscriber m_selectedSub; + nt::NetworkTableListener m_selectedListener; + + util::StringMap m_optionMap; + std::vector m_options; +}; + +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"); + } + + util::mutex mutex; + DashboardOpModeSender autoOpModes; + DashboardOpModeSender teleopOpModes; + DashboardOpModeSender testOpModes; +}; +} // namespace + +static DashboardOpModeInstance* gInstance; +static std::atomic_flag gStarted{}; +static std::atomic_flag gEnabled{}; + +void hal::InitializeDashboardOpMode() { + static DashboardOpModeInstance inst; + gInstance = &inst; +} + +void hal::SetDashboardOpModeOptions(std::span options) { + std::scoped_lock lock{gInstance->mutex}; + gInstance->autoOpModes.SetOptions(options, HAL_ROBOTMODE_AUTONOMOUS); + gInstance->teleopOpModes.SetOptions(options, HAL_ROBOTMODE_TELEOPERATED); + gInstance->testOpModes.SetOptions(options, HAL_ROBOTMODE_TEST); +} + +void hal::StartDashboardOpMode() { + if (gStarted.test_and_set()) { + return; + } + std::scoped_lock lock{gInstance->mutex}; + gInstance->Start(nt::NetworkTableInstance::GetDefault()); +} + +void hal::EnableDashboardOpMode() { + if (gEnabled.test_and_set()) { + return; + } + StartDashboardOpMode(); + std::scoped_lock lock{gInstance->mutex}; + gInstance->autoOpModes.Enable(); + gInstance->teleopOpModes.Enable(); + gInstance->testOpModes.Enable(); +} + +int64_t hal::GetDashboardSelectedOpMode(HAL_RobotMode robotMode) { + if (!gEnabled.test()) { + return 0; + } + std::scoped_lock lock{gInstance->mutex}; + switch (robotMode) { + case HAL_ROBOTMODE_AUTONOMOUS: + return gInstance->autoOpModes.GetSelected(); + case HAL_ROBOTMODE_TELEOPERATED: + return gInstance->teleopOpModes.GetSelected(); + case HAL_ROBOTMODE_TEST: + return gInstance->testOpModes.GetSelected(); + default: + return 0; + } +} diff --git a/hal/src/main/native/cpp/jni/DriverStationJNI.cpp b/hal/src/main/native/cpp/jni/DriverStationJNI.cpp index 63cd17a3a3..6a0d339d6a 100644 --- a/hal/src/main/native/cpp/jni/DriverStationJNI.cpp +++ b/hal/src/main/native/cpp/jni/DriverStationJNI.cpp @@ -5,6 +5,8 @@ #include #include +#include +#include #include @@ -61,68 +63,77 @@ Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramStarting /* * Class: org_wpilib_hardware_hal_DriverStationJNI - * Method: observeUserProgramDisabled - * Signature: ()V + * Method: observeUserProgram + * Signature: (J)V */ JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramDisabled - (JNIEnv*, jclass) +Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgram + (JNIEnv*, jclass, jlong word) { - HAL_ObserveUserProgramDisabled(); -} - -/* - * Class: org_wpilib_hardware_hal_DriverStationJNI - * Method: observeUserProgramAutonomous - * Signature: ()V - */ -JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramAutonomous - (JNIEnv*, jclass) -{ - HAL_ObserveUserProgramAutonomous(); -} - -/* - * Class: org_wpilib_hardware_hal_DriverStationJNI - * Method: observeUserProgramTeleop - * Signature: ()V - */ -JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramTeleop - (JNIEnv*, jclass) -{ - HAL_ObserveUserProgramTeleop(); -} - -/* - * Class: org_wpilib_hardware_hal_DriverStationJNI - * Method: observeUserProgramTest - * Signature: ()V - */ -JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramTest - (JNIEnv*, jclass) -{ - HAL_ObserveUserProgramTest(); + HAL_ObserveUserProgram({.value = word}); } /* * Class: org_wpilib_hardware_hal_DriverStationJNI * Method: nativeGetControlWord - * Signature: ()I + * Signature: ()J */ -JNIEXPORT jint JNICALL +JNIEXPORT jlong JNICALL Java_org_wpilib_hardware_hal_DriverStationJNI_nativeGetControlWord (JNIEnv*, jclass) { - static_assert(sizeof(HAL_ControlWord) == sizeof(jint), + static_assert(sizeof(HAL_ControlWord) == sizeof(jlong), "Java int must match the size of control word"); HAL_ControlWord controlWord; HAL_GetControlWord(&controlWord); - jint retVal = 0; - std::memcpy(&retVal, &controlWord, sizeof(HAL_ControlWord)); - return retVal; + return controlWord.value; +} + +/* + * Class: org_wpilib_hardware_hal_DriverStationJNI + * Method: nativeGetUncachedControlWord + * Signature: ()J + */ +JNIEXPORT jlong JNICALL +Java_org_wpilib_hardware_hal_DriverStationJNI_nativeGetUncachedControlWord + (JNIEnv*, jclass) +{ + static_assert(sizeof(HAL_ControlWord) == sizeof(jlong), + "Java int must match the size of control word"); + HAL_ControlWord controlWord; + HAL_GetUncachedControlWord(&controlWord); + return controlWord.value; +} + +/* + * Class: org_wpilib_hardware_hal_DriverStationJNI + * Method: setOpModeOptions + * Signature: ([Ljava/lang/Object;)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_DriverStationJNI_setOpModeOptions + (JNIEnv* env, jclass, jobjectArray options) +{ + std::vector coptions; + if (options != nullptr) { + jsize length = env->GetArrayLength(options); + coptions.reserve(length); + for (jsize i = 0; i < length; i++) { + JLocal option{env, env->GetObjectArrayElement(options, i)}; + if (!option) { + ThrowIllegalArgumentException(env, "Null OpModeOption passed in array"); + return; + } + auto coption = CreateOpModeOptionFromJava(env, option); + if (coption.id == 0) { + // exception thrown + return; + } + coptions.emplace_back(std::move(coption)); + } + } + int32_t status = HAL_SetOpModeOptions(coptions.data(), coptions.size()); + CheckStatusForceThrow(env, status); } /* diff --git a/hal/src/main/native/cpp/jni/HALUtil.cpp b/hal/src/main/native/cpp/jni/HALUtil.cpp index 65af623615..efb7c6bb1a 100644 --- a/hal/src/main/native/cpp/jni/HALUtil.cpp +++ b/hal/src/main/native/cpp/jni/HALUtil.cpp @@ -53,6 +53,7 @@ static JClass matchInfoDataCls; static JClass canReceiveMessageCls; static JClass canStreamMessageCls; static JClass halValueCls; +static JClass opModeOptionCls; static JClass revPHVersionCls; static JClass canStreamOverflowExCls; @@ -63,6 +64,7 @@ static const JClassInit classes[] = { {"org/wpilib/hardware/hal/MatchInfoData", &matchInfoDataCls}, {"org/wpilib/hardware/hal/can/CANReceiveMessage", &canReceiveMessageCls}, {"org/wpilib/hardware/hal/can/CANStreamMessage", &canStreamMessageCls}, + {"org/wpilib/hardware/hal/OpModeOption", &opModeOptionCls}, {"org/wpilib/hardware/hal/HALValue", &halValueCls}, {"org/wpilib/hardware/hal/REVPHVersion", &revPHVersionCls}, {"org/wpilib/hardware/hal/can/CANStreamOverflowException", @@ -188,6 +190,71 @@ void ThrowBoundaryException(JNIEnv* env, double value, double lower, env->Throw(static_cast(ex)); } +jobject CreateOpModeOption(JNIEnv* env, const HAL_OpModeOption& option) { + static jmethodID constructor = env->GetMethodID( + opModeOptionCls, "", + "(JLjava/lang/String;L/java/lang/String;Ljava/lang/String;II)V"); + JLocal name{ + env, MakeJString(env, wpi::util::to_string_view(&option.name))}; + JLocal group{ + env, MakeJString(env, wpi::util::to_string_view(&option.group))}; + JLocal desc{ + env, MakeJString(env, wpi::util::to_string_view(&option.description))}; + return env->NewObject(opModeOptionCls, constructor, + static_cast(option.id), name.obj(), group.obj(), + desc.obj(), static_cast(option.textColor), + static_cast(option.backgroundColor)); +} + +jobjectArray CreateOpModeOptionArray( + JNIEnv* env, std::span options) { + jobjectArray arr = + env->NewObjectArray(options.size(), opModeOptionCls, nullptr); + if (!arr) { + return nullptr; + } + size_t i = 0; + for (auto& option : options) { + JLocal elem{env, CreateOpModeOption(env, option)}; + env->SetObjectArrayElement(arr, i++, elem); + } + return arr; +} + +HAL_OpModeOption CreateOpModeOptionFromJava(JNIEnv* env, jobject option) { + static jfieldID idField = env->GetFieldID(opModeOptionCls, "id", "J"); + static jfieldID nameField = + env->GetFieldID(opModeOptionCls, "name", "Ljava/lang/String;"); + static jfieldID groupField = + env->GetFieldID(opModeOptionCls, "group", "Ljava/lang/String;"); + static jfieldID descriptionField = + env->GetFieldID(opModeOptionCls, "description", "Ljava/lang/String;"); + static jfieldID textColorField = + env->GetFieldID(opModeOptionCls, "textColor", "I"); + static jfieldID backgroundColorField = + env->GetFieldID(opModeOptionCls, "backgroundColor", "I"); + if (!idField || !nameField || !groupField || !descriptionField || + !textColorField || !backgroundColorField) { + ThrowIllegalArgumentException(env, "Missing field in OpModeOption"); + return {0, {}, {}, {}, 0, 0}; + } + int64_t id = env->GetLongField(option, idField); + JLocal name{ + env, static_cast(env->GetObjectField(option, nameField))}; + JLocal group{ + env, static_cast(env->GetObjectField(option, groupField))}; + JLocal description{ + env, static_cast(env->GetObjectField(option, descriptionField))}; + int32_t textColor = env->GetIntField(option, textColorField); + int32_t backgroundColor = env->GetIntField(option, backgroundColorField); + return {id, + wpi::util::alloc_wpi_string(JStringRef{env, name}), + wpi::util::alloc_wpi_string(JStringRef{env, group}), + wpi::util::alloc_wpi_string(JStringRef{env, description}), + textColor, + backgroundColor}; +} + jobject CreateREVPHVersion(JNIEnv* env, uint32_t firmwareMajor, uint32_t firmwareMinor, uint32_t firmwareFix, uint32_t hardwareMinor, uint32_t hardwareMajor, diff --git a/hal/src/main/native/cpp/jni/HALUtil.h b/hal/src/main/native/cpp/jni/HALUtil.h index 4b46482785..20d6069588 100644 --- a/hal/src/main/native/cpp/jni/HALUtil.h +++ b/hal/src/main/native/cpp/jni/HALUtil.h @@ -7,9 +7,11 @@ #include #include +#include #include struct HAL_MatchInfo; +struct HAL_OpModeOption; struct HAL_Value; namespace wpi::hal { @@ -49,6 +51,11 @@ void ThrowIndexOutOfBoundsException(JNIEnv* env, std::string_view msg); void ThrowBoundaryException(JNIEnv* env, double value, double lower, double upper); +jobject CreateOpModeOption(JNIEnv* env, const HAL_OpModeOption& option); +jobjectArray CreateOpModeOptionArray(JNIEnv* env, + std::span options); +HAL_OpModeOption CreateOpModeOptionFromJava(JNIEnv* env, jobject option); + jobject CreateREVPHVersion(JNIEnv* env, uint32_t firmwareMajor, uint32_t firmwareMinor, uint32_t firmwareFix, uint32_t hardwareMinor, uint32_t hardwareMajor, diff --git a/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp index fe566fc127..b3cb25ffe4 100644 --- a/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp +++ b/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp @@ -4,7 +4,10 @@ #include +#include "../HALUtil.h" #include "CallbackStore.h" +#include "OpModeOptionsCallbackStore.h" +#include "SimulatorJNI.h" #include "org_wpilib_hardware_hal_simulation_DriverStationDataJNI.h" #include "wpi/hal/simulation/DriverStationData.h" #include "wpi/hal/simulation/MockHooks.h" @@ -69,103 +72,53 @@ Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setEnabled /* * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI - * Method: registerAutonomousCallback + * Method: registerRobotModeCallback * Signature: (Ljava/lang/Object;Z)I */ JNIEXPORT jint JNICALL -Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerAutonomousCallback +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerRobotModeCallback (JNIEnv* env, jclass, jobject callback, jboolean initialNotify) { return sim::AllocateCallbackNoIndex( env, callback, initialNotify, - &HALSIM_RegisterDriverStationAutonomousCallback); + &HALSIM_RegisterDriverStationRobotModeCallback); } /* * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI - * Method: cancelAutonomousCallback + * Method: cancelRobotModeCallback * Signature: (I)V */ JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelAutonomousCallback - (JNIEnv* env, jclass, jint handle) -{ - return sim::FreeCallbackNoIndex( - env, handle, &HALSIM_CancelDriverStationAutonomousCallback); -} - -/* - * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI - * Method: getAutonomous - * Signature: ()Z - */ -JNIEXPORT jboolean JNICALL -Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getAutonomous - (JNIEnv*, jclass) -{ - return HALSIM_GetDriverStationAutonomous(); -} - -/* - * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI - * Method: setAutonomous - * Signature: (Z)V - */ -JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setAutonomous - (JNIEnv*, jclass, jboolean value) -{ - HALSIM_SetDriverStationAutonomous(value); -} - -/* - * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI - * Method: registerTestCallback - * Signature: (Ljava/lang/Object;Z)I - */ -JNIEXPORT jint JNICALL -Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerTestCallback - (JNIEnv* env, jclass, jobject callback, jboolean initialNotify) -{ - return sim::AllocateCallbackNoIndex( - env, callback, initialNotify, &HALSIM_RegisterDriverStationTestCallback); -} - -/* - * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI - * Method: cancelTestCallback - * Signature: (I)V - */ -JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelTestCallback +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelRobotModeCallback (JNIEnv* env, jclass, jint handle) { return sim::FreeCallbackNoIndex(env, handle, - &HALSIM_CancelDriverStationTestCallback); + &HALSIM_CancelDriverStationRobotModeCallback); } /* * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI - * Method: getTest - * Signature: ()Z + * Method: nativeGetRobotMode + * Signature: ()I */ -JNIEXPORT jboolean JNICALL -Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getTest +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_nativeGetRobotMode (JNIEnv*, jclass) { - return HALSIM_GetDriverStationTest(); + return HALSIM_GetDriverStationRobotMode(); } /* * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI - * Method: setTest - * Signature: (Z)V + * Method: nativeSetRobotMode + * Signature: (I)V */ JNIEXPORT void JNICALL -Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setTest - (JNIEnv*, jclass, jboolean value) +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_nativeSetRobotMode + (JNIEnv*, jclass, jint value) { - HALSIM_SetDriverStationTest(value); + HALSIM_SetDriverStationRobotMode(static_cast(value)); } /* @@ -423,6 +376,99 @@ Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setMatchTime HALSIM_SetDriverStationMatchTime(value); } +/* + * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI + * Method: registerOpModeCallback + * Signature: (Ljava/lang/Object;Z)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerOpModeCallback + (JNIEnv* env, jclass, jobject callback, jboolean initialNotify) +{ + return sim::AllocateCallbackNoIndex( + env, callback, initialNotify, + &HALSIM_RegisterDriverStationOpModeCallback); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI + * Method: cancelOpModeCallback + * Signature: (I)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelOpModeCallback + (JNIEnv* env, jclass, jint handle) +{ + return sim::FreeCallbackNoIndex(env, handle, + &HALSIM_CancelDriverStationOpModeCallback); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI + * Method: getOpMode + * Signature: ()J + */ +JNIEXPORT jlong JNICALL +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getOpMode + (JNIEnv* env, jclass) +{ + return HALSIM_GetDriverStationOpMode(); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI + * Method: setOpMode + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setOpMode + (JNIEnv* env, jclass, jlong value) +{ + HALSIM_SetDriverStationOpMode(value); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI + * Method: registerOpModeOptionsCallback + * Signature: (Ljava/lang/Object;Z)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerOpModeOptionsCallback + (JNIEnv* env, jclass, jobject callback, jboolean initialNotify) +{ + return sim::AllocateOpModeOptionsCallback( + env, callback, initialNotify, &HALSIM_RegisterOpModeOptionsCallback); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI + * Method: cancelOpModeOptionsCallback + * Signature: (I)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelOpModeOptionsCallback + (JNIEnv* env, jclass, jint handle) +{ + sim::FreeOpModeOptionsCallback(env, handle, + &HALSIM_CancelOpModeOptionsCallback); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI + * Method: getOpModeOptions + * Signature: ()[Ljava/lang/Object; + */ +JNIEXPORT jobjectArray JNICALL +Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getOpModeOptions + (JNIEnv* env, jclass) +{ + int32_t count; + HAL_OpModeOption* options = HALSIM_GetOpModeOptions(&count); + auto rv = CreateOpModeOptionArray(env, {options, options + count}); + HALSIM_FreeOpModeOptionsArray(options, count); + return rv; +} + /* * Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI * Method: setJoystickAxes diff --git a/hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.cpp b/hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.cpp new file mode 100644 index 0000000000..4bbd8e071d --- /dev/null +++ b/hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.cpp @@ -0,0 +1,121 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "OpModeOptionsCallbackStore.h" + +#include + +#include +#include + +#include "../HALUtil.h" +#include "SimulatorJNI.h" +#include "wpi/hal/Types.h" +#include "wpi/hal/handles/UnlimitedHandleResource.h" +#include "wpi/util/jni_util.hpp" + +using namespace wpi::hal; +using namespace wpi::hal::sim; +using namespace wpi::util::java; + +static UnlimitedHandleResource* callbackHandles; + +namespace wpi::hal::sim { +void InitializeOpModeOptionsStore() { + static UnlimitedHandleResource + cb; + callbackHandles = &cb; +} +} // namespace wpi::hal::sim + +void OpModeOptionsCallbackStore::create(JNIEnv* env, jobject obj) { + m_call = JGlobal(env, obj); +} + +void OpModeOptionsCallbackStore::performCallback( + const char* name, const HAL_OpModeOption* opmodes, int32_t count) { + JNIEnv* env; + JavaVM* vm = sim::GetJVM(); + bool didAttachThread = false; + int tryGetEnv = vm->GetEnv(reinterpret_cast(&env), JNI_VERSION_1_6); + if (tryGetEnv == JNI_EDETACHED) { + // Thread not attached + didAttachThread = true; + if (vm->AttachCurrentThread(reinterpret_cast(&env), nullptr) != 0) { + // Failed to attach, log and return + std::puts("Failed to attach"); + std::fflush(stdout); + return; + } + } else if (tryGetEnv == JNI_EVERSION) { + std::puts("Invalid JVM Version requested"); + std::fflush(stdout); + } + + JLocal toCallbackArr{ + env, CreateOpModeOptionArray(env, {opmodes, opmodes + count})}; + + env->CallVoidMethod(m_call, sim::GetBiConsumerCallback(), + MakeJString(env, name), toCallbackArr.obj()); + + if (env->ExceptionCheck()) { + env->ExceptionDescribe(); + } + + if (didAttachThread) { + vm->DetachCurrentThread(); + } +} + +void OpModeOptionsCallbackStore::free(JNIEnv* env) { + m_call.free(env); +} + +SIM_JniHandle sim::AllocateOpModeOptionsCallback( + JNIEnv* env, jobject callback, jboolean initialNotify, + RegisterOpModeOptionsCallbackFunc createCallback) { + auto callbackStore = std::make_shared(); + + auto handle = callbackHandles->Allocate(callbackStore); + + if (handle == HAL_kInvalidHandle) { + return -1; + } + + uintptr_t handleAsPtr = static_cast(handle); + void* handleAsVoidPtr = reinterpret_cast(handleAsPtr); + + callbackStore->create(env, callback); + + auto callbackFunc = [](const char* name, void* param, + const HAL_OpModeOption* opmodes, int32_t count) { + uintptr_t handleTmp = reinterpret_cast(param); + SIM_JniHandle handle = static_cast(handleTmp); + auto data = callbackHandles->Get(handle); + if (!data) { + return; + } + + data->performCallback(name, opmodes, count); + }; + + auto id = createCallback(callbackFunc, handleAsVoidPtr, initialNotify); + + callbackStore->setCallbackId(id); + + return handle; +} + +void sim::FreeOpModeOptionsCallback( + JNIEnv* env, SIM_JniHandle handle, + FreeOpModeOptionsCallbackFunc freeCallback) { + auto callback = callbackHandles->Free(handle); + if (callback == nullptr) { + return; + } + freeCallback(callback->getCallbackId()); + callback->free(env); +} diff --git a/hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.h b/hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.h new file mode 100644 index 0000000000..36ca9594e6 --- /dev/null +++ b/hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.h @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "SimulatorJNI.h" +#include "wpi/hal/Types.h" +#include "wpi/hal/simulation/DriverStationData.h" +#include "wpi/util/jni_util.hpp" + +namespace wpi::hal::sim { +class OpModeOptionsCallbackStore { + public: + void create(JNIEnv* env, jobject obj); + void performCallback(const char* name, const HAL_OpModeOption* opmodes, + int32_t count); + void free(JNIEnv* env); + void setCallbackId(int32_t id) { callbackId = id; } + int32_t getCallbackId() { return callbackId; } + + private: + wpi::util::java::JGlobal m_call; + int32_t callbackId; +}; + +void InitializeOpModeOptionsStore(); + +using RegisterOpModeOptionsCallbackFunc = int32_t (*)( + HAL_OpModeOptionsCallback callback, void* param, HAL_Bool initialNotify); +using FreeOpModeOptionsCallbackFunc = void (*)(int32_t uid); + +SIM_JniHandle AllocateOpModeOptionsCallback( + JNIEnv* env, jobject callback, jboolean initialNotify, + RegisterOpModeOptionsCallbackFunc createCallback); +void FreeOpModeOptionsCallback(JNIEnv* env, SIM_JniHandle handle, + FreeOpModeOptionsCallbackFunc freeCallback); +} // namespace wpi::hal::sim diff --git a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp index 58a57b2fe1..d6547d3932 100644 --- a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp +++ b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp @@ -7,6 +7,7 @@ #include "BufferCallbackStore.h" #include "CallbackStore.h" #include "ConstBufferCallbackStore.h" +#include "OpModeOptionsCallbackStore.h" #include "SimDeviceDataJNI.h" #include "org_wpilib_hardware_hal_simulation_SimulatorJNI.h" #include "wpi/hal/HAL.h" @@ -20,9 +21,35 @@ static JavaVM* jvm = nullptr; static JClass notifyCallbackCls; static JClass bufferCallbackCls; static JClass constBufferCallbackCls; +static JClass biConsumerCls; static jmethodID notifyCallbackCallback; static jmethodID bufferCallbackCallback; static jmethodID constBufferCallbackCallback; +static jmethodID biConsumerCallback; + +static const JClassInit classes[] = { + {"org/wpilib/hardware/hal/simulation/NotifyCallback", ¬ifyCallbackCls}, + {"org/wpilib/hardware/hal/simulation/BufferCallback", &bufferCallbackCls}, + {"org/wpilib/hardware/hal/simulation/ConstBufferCallback", + &constBufferCallbackCls}, + {"java/util/function/BiConsumer", &biConsumerCls}, +}; + +static const struct JMethodInit { + JClass* cls; + const char* name; + const char* sig; + jmethodID* method; +} methods[] = { + {¬ifyCallbackCls, "callbackNative", "(Ljava/lang/String;IJD)V", + ¬ifyCallbackCallback}, + {&bufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V", + &bufferCallbackCallback}, + {&constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V", + &constBufferCallbackCallback}, + {&biConsumerCls, "accept", "(Ljava/lang/Object;Ljava/lang/Object;)V", + &biConsumerCallback}, +}; namespace wpi::hal::sim { jint SimOnLoad(JavaVM* vm, void* reserved) { @@ -33,45 +60,24 @@ jint SimOnLoad(JavaVM* vm, void* reserved) { return JNI_ERR; } - notifyCallbackCls = - JClass(env, "org/wpilib/hardware/hal/simulation/NotifyCallback"); - if (!notifyCallbackCls) { - return JNI_ERR; + for (auto& c : classes) { + *c.cls = JClass(env, c.name); + if (!*c.cls) { + return JNI_ERR; + } } - notifyCallbackCallback = env->GetMethodID(notifyCallbackCls, "callbackNative", - "(Ljava/lang/String;IJD)V"); - if (!notifyCallbackCallback) { - return JNI_ERR; - } - - bufferCallbackCls = - JClass(env, "org/wpilib/hardware/hal/simulation/BufferCallback"); - if (!bufferCallbackCls) { - return JNI_ERR; - } - - bufferCallbackCallback = env->GetMethodID(bufferCallbackCls, "callback", - "(Ljava/lang/String;[BI)V"); - if (!bufferCallbackCallback) { - return JNI_ERR; - } - - constBufferCallbackCls = - JClass(env, "org/wpilib/hardware/hal/simulation/ConstBufferCallback"); - if (!constBufferCallbackCls) { - return JNI_ERR; - } - - constBufferCallbackCallback = env->GetMethodID( - constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V"); - if (!constBufferCallbackCallback) { - return JNI_ERR; + for (auto& m : methods) { + *m.method = env->GetMethodID(*m.cls, m.name, m.sig); + if (!*m.method) { + return JNI_ERR; + } } InitializeStore(); InitializeBufferStore(); InitializeConstBufferStore(); + InitializeOpModeOptionsStore(); if (!InitializeSimDeviceDataJNI(env)) { return JNI_ERR; } @@ -88,6 +94,7 @@ void SimOnUnload(JavaVM* vm, void* reserved) { notifyCallbackCls.free(env); bufferCallbackCls.free(env); constBufferCallbackCls.free(env); + biConsumerCls.free(env); FreeSimDeviceDataJNI(env); jvm = nullptr; } @@ -108,6 +115,9 @@ jmethodID GetConstBufferCallback() { return constBufferCallbackCallback; } +jmethodID GetBiConsumerCallback() { + return biConsumerCallback; +} } // namespace wpi::hal::sim extern "C" { @@ -159,6 +169,32 @@ Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_getProgramStarted return HALSIM_GetProgramStarted(); } +/* + * Class: org_wpilib_hardware_hal_simulation_SimulatorJNI + * Method: setProgramState + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_setProgramState + (JNIEnv*, jclass, jlong word) +{ + HALSIM_SetProgramState({word}); +} + +/* + * Class: org_wpilib_hardware_hal_simulation_SimulatorJNI + * Method: nativeGetProgramState + * Signature: ()J + */ +JNIEXPORT jlong JNICALL +Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_nativeGetProgramState + (JNIEnv*, jclass) +{ + HAL_ControlWord word; + HALSIM_GetProgramState(&word); + return word.value; +} + /* * Class: org_wpilib_hardware_hal_simulation_SimulatorJNI * Method: restartTiming diff --git a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h index e70a438a69..02718341c9 100644 --- a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h +++ b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h @@ -15,4 +15,5 @@ JavaVM* GetJVM(); jmethodID GetNotifyCallback(); jmethodID GetBufferCallback(); jmethodID GetConstBufferCallback(); +jmethodID GetBiConsumerCallback(); } // namespace wpi::hal::sim diff --git a/hal/src/main/native/cpp/proto/ControlDataProto.cpp b/hal/src/main/native/cpp/proto/ControlDataProto.cpp index da4246dab7..796563906d 100644 --- a/hal/src/main/native/cpp/proto/ControlDataProto.cpp +++ b/hal/src/main/native/cpp/proto/ControlDataProto.cpp @@ -11,8 +11,7 @@ static_assert(sizeof(mrc::ControlFlags) == sizeof(uint32_t)); namespace { constexpr uint32_t EnabledMask = 0x1; -constexpr uint32_t AutoMask = 0x2; -constexpr uint32_t TestMask = 0x4; +constexpr uint32_t RobotModeMask = 0x6; constexpr uint32_t EStopMask = 0x8; constexpr uint32_t FmsConnectedMask = 0x10; constexpr uint32_t DsConnectedMask = 0x20; @@ -20,8 +19,7 @@ constexpr uint32_t WatchdogActiveMask = 0x40; constexpr uint32_t AllianceMask = 0x1F80; constexpr uint32_t EnabledShift = 0; -constexpr uint32_t AutoShift = 1; -constexpr uint32_t TestShift = 2; +constexpr uint32_t RobotModeShift = 1; constexpr uint32_t EStopShift = 3; constexpr uint32_t FmsConnectedShift = 4; constexpr uint32_t DsConnectedShift = 5; @@ -33,8 +31,7 @@ constexpr uint32_t AllianceShift = 7; constexpr uint32_t FromControlWord(mrc::ControlFlags Word) { uint32_t Ret = 0; WORD_TO_INT(Enabled); - WORD_TO_INT(Auto); - WORD_TO_INT(Test); + WORD_TO_INT(RobotMode); WORD_TO_INT(EStop); WORD_TO_INT(FmsConnected); WORD_TO_INT(DsConnected); @@ -50,8 +47,7 @@ constexpr uint32_t FromControlWord(mrc::ControlFlags Word) { constexpr mrc::ControlFlags ToControlWord(uint32_t Word) { mrc::ControlFlags Ret = {}; INT_TO_WORD(Enabled); - INT_TO_WORD(Auto); - INT_TO_WORD(Test); + INT_TO_WORD(RobotMode); INT_TO_WORD(EStop); INT_TO_WORD(FmsConnected); INT_TO_WORD(DsConnected); @@ -67,10 +63,10 @@ std::optional wpi::util::Protobuf::Unpack( wpi::util::UnpackCallback JoystickCb; mrc_proto_ProtobufControlData Msg{ - .ControlWord = 0, .MatchTime = 0, .Joysticks = JoystickCb.Callback(), .CurrentOpMode = 0, + .ControlWord = 0, }; if (!Stream.Decode(Msg)) { @@ -99,10 +95,10 @@ bool wpi::util::Protobuf::Pack( wpi::util::PackCallback Joysticks{Sticks}; mrc_proto_ProtobufControlData Msg{ - .ControlWord = FromControlWord(Value.ControlWord), .MatchTime = Value.MatchTime, .Joysticks = Joysticks.Callback(), .CurrentOpMode = Value.CurrentOpMode.ToValue(), + .ControlWord = FromControlWord(Value.ControlWord), }; return Stream.Encode(Msg); diff --git a/hal/src/main/native/cpp/proto/OpModeProto.cpp b/hal/src/main/native/cpp/proto/OpModeProto.cpp index b76370e1ce..dde44bf5af 100644 --- a/hal/src/main/native/cpp/proto/OpModeProto.cpp +++ b/hal/src/main/native/cpp/proto/OpModeProto.cpp @@ -12,36 +12,52 @@ std::optional wpi::util::Protobuf::Unpack( InputStream& Stream) { wpi::util::UnpackCallback NameCb; + wpi::util::UnpackCallback GroupCb; + wpi::util::UnpackCallback DescriptionCb; mrc_proto_ProtobufOpMode Msg; Msg.Name = NameCb.Callback(); + Msg.Group = GroupCb.Callback(); + Msg.Description = DescriptionCb.Callback(); if (!Stream.Decode(Msg)) { return {}; } auto Name = NameCb.Items(); + auto Group = GroupCb.Items(); + auto Description = DescriptionCb.Items(); - if (Name.empty()) { + if (Name.empty() || Group.empty() || Description.empty()) { return {}; } - mrc::OpMode OutputData; - OutputData.MoveName(std::move(Name[0])); - - OutputData.Hash = mrc::OpModeHash::FromValue(Msg.Hash); - - return OutputData; + return mrc::OpMode{ + mrc::OpModeHash::FromValue(Msg.Hash), + std::move(Name[0]), + std::move(Group[0]), + std::move(Description[0]), + Msg.TextColor, + Msg.BackgroundColor, + }; } bool wpi::util::Protobuf::Pack(OutputStream& Stream, const mrc::OpMode& Value) { std::string_view EventNameStr = Value.GetName(); wpi::util::PackCallback EventName{&EventNameStr}; + std::string_view EventGroupStr = Value.GetGroup(); + wpi::util::PackCallback EventGroup{&EventGroupStr}; + std::string_view EventDescriptionStr = Value.GetDescription(); + wpi::util::PackCallback EventDescription{&EventDescriptionStr}; mrc_proto_ProtobufOpMode Msg{ .Hash = Value.Hash.ToValue(), .Name = EventName.Callback(), + .Group = EventGroup.Callback(), + .Description = EventDescription.Callback(), + .TextColor = Value.GetTextColor(), + .BackgroundColor = Value.GetBackgroundColor(), }; return Stream.Encode(Msg); diff --git a/hal/src/main/native/include/wpi/hal/DashboardOpMode.hpp b/hal/src/main/native/include/wpi/hal/DashboardOpMode.hpp new file mode 100644 index 0000000000..3c8e383c84 --- /dev/null +++ b/hal/src/main/native/include/wpi/hal/DashboardOpMode.hpp @@ -0,0 +1,19 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "wpi/hal/DriverStationTypes.h" + +namespace wpi::hal { + +void InitializeDashboardOpMode(); +void SetDashboardOpModeOptions(std::span options); +void StartDashboardOpMode(); +void EnableDashboardOpMode(); +int64_t GetDashboardSelectedOpMode(HAL_RobotMode robotMode); + +} // namespace wpi::hal diff --git a/hal/src/main/native/include/wpi/hal/DriverStation.h b/hal/src/main/native/include/wpi/hal/DriverStation.h index c196d977a6..145703f35c 100644 --- a/hal/src/main/native/include/wpi/hal/DriverStation.h +++ b/hal/src/main/native/include/wpi/hal/DriverStation.h @@ -64,6 +64,28 @@ int32_t HAL_SendConsoleLine(const char* line); */ int32_t HAL_GetControlWord(HAL_ControlWord* controlWord); +/** + * Gets the current control word of the driver station. Unlike + * HAL_GetControlWord, this function gets the latest value rather than using the + * value cached by HAL_RefreshDSData(). + * + * The control word contains the robot state. + * + * @param controlWord the control word (out) + * @return the error code, or 0 for success + */ +int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord); + +/** + * Sets operating mode options. + * + * @param options array of operating mode options + * @param count number of options in the array + * @return the error code, or 0 for success + */ +int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options, + int32_t count); + /** * Gets the current alliance station ID. * @@ -264,42 +286,31 @@ void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle); void HAL_ObserveUserProgramStarting(void); /** - * Sets the disabled flag in the DS. + * Sets the control word state returned to the DS. * * This is used for the DS to ensure the robot is properly responding to its * state request. Ensure this gets called about every 50ms, or the robot will be * disabled by the DS. - */ -void HAL_ObserveUserProgramDisabled(void); - -/** - * Sets the autonomous enabled flag in the DS. * - * This is used for the DS to ensure the robot is properly responding to its - * state request. Ensure this gets called about every 50ms, or the robot will be - * disabled by the DS. + * @param word control word returned by HAL_GetControlWord */ -void HAL_ObserveUserProgramAutonomous(void); - -/** - * Sets the teleoperated enabled flag in the DS. - * - * This is used for the DS to ensure the robot is properly responding to its - * state request. Ensure this gets called about every 50ms, or the robot will be - * disabled by the DS. - */ -void HAL_ObserveUserProgramTeleop(void); - -/** - * Sets the test mode flag in the DS. - * - * This is used for the DS to ensure the robot is properly responding to its - * state request. Ensure this gets called about every 50ms, or the robot will be - * disabled by the DS. - */ -void HAL_ObserveUserProgramTest(void); +void HAL_ObserveUserProgram(HAL_ControlWord word); #ifdef __cplusplus } // extern "C" + +namespace wpi::hal { +inline ControlWord GetControlWord() { + HAL_ControlWord word; + HAL_GetControlWord(&word); + return ControlWord{word}; +} + +inline ControlWord GetUncachedControlWord() { + HAL_ControlWord word; + HAL_GetUncachedControlWord(&word); + return ControlWord{word}; +} +} // namespace wpi::hal #endif /** @} */ diff --git a/hal/src/main/native/include/wpi/hal/DriverStationTypes.h b/hal/src/main/native/include/wpi/hal/DriverStationTypes.h index e7f3540afb..57c8a0476b 100644 --- a/hal/src/main/native/include/wpi/hal/DriverStationTypes.h +++ b/hal/src/main/native/include/wpi/hal/DriverStationTypes.h @@ -7,6 +7,11 @@ #include #include "wpi/hal/Types.h" +#include "wpi/util/string.h" + +#ifdef __cplusplus +#include "wpi/util/struct/Struct.hpp" +#endif // __cplusplus /** * @defgroup hal_driverstation Driver Station Functions @@ -14,15 +19,16 @@ * @{ */ +#define HAL_CONTROLWORD_OPMODE_HASH_MASK 0x00FFFFFFFFFFFFFFLL +#define HAL_CONTROLWORD_ROBOT_MODE_MASK 0x0300000000000000LL +#define HAL_CONTROLWORD_ROBOT_MODE_SHIFT 56 +#define HAL_CONTROLWORD_ENABLED_MASK 0x0400000000000000LL +#define HAL_CONTROLWORD_ESTOP_MASK 0x0800000000000000LL +#define HAL_CONTROLWORD_FMS_ATTACHED_MASK 0x1000000000000000LL +#define HAL_CONTROLWORD_DS_ATTACHED_MASK 0x2000000000000000LL + struct HAL_ControlWord { - uint32_t enabled : 1; - uint32_t autonomous : 1; - uint32_t test : 1; - uint32_t eStop : 1; - uint32_t fmsAttached : 1; - uint32_t dsAttached : 1; - uint32_t watchdogEnabled : 1; - uint32_t control_reserved : 25; + int64_t value; }; typedef struct HAL_ControlWord HAL_ControlWord; @@ -50,6 +56,13 @@ HAL_ENUM(HAL_MatchType) { HAL_kMatchType_elimination, }; +HAL_ENUM(HAL_RobotMode) { + HAL_ROBOTMODE_UNKNOWN = 0, + HAL_ROBOTMODE_AUTONOMOUS, + HAL_ROBOTMODE_TELEOPERATED, + HAL_ROBOTMODE_TEST, +}; + /** * The maximum number of touchpads that will be stored in a single * HAL_JoystickTouchpads struct. This is used for allocating buffers, not @@ -157,4 +170,339 @@ struct HAL_MatchInfo { uint16_t gameSpecificMessageSize; }; typedef struct HAL_MatchInfo HAL_MatchInfo; + +#define HAL_OPMODE_HASH_MASK HAL_CONTROLWORD_OPMODE_HASH_MASK +#define HAL_OPMODE_ROBOT_MODE_MASK HAL_CONTROLWORD_ROBOT_MODE_MASK +#define HAL_OPMODE_ROBOT_MODE_SHIFT HAL_CONTROLWORD_ROBOT_MODE_SHIFT + +struct HAL_OpModeOption { + int64_t id; // encodes robot mode in bits 57-56, LSB 56 bits is hash of name + struct WPI_String name; + struct WPI_String group; + struct WPI_String description; + int32_t textColor; // 0x00RRGGBB or -1 for default + int32_t backgroundColor; // 0x00RRGGBB or -1 for default +}; +typedef struct HAL_OpModeOption HAL_OpModeOption; + +#ifdef __cplusplus +extern "C" { +#endif // __cplusplus + +inline HAL_ControlWord HAL_MakeControlWord(int64_t opModeHash, + HAL_RobotMode robotMode, + HAL_Bool enabled, HAL_Bool eStop, + HAL_Bool fmsAttached, + HAL_Bool dsAttached) { + HAL_ControlWord word; + word.value = + (opModeHash & HAL_CONTROLWORD_OPMODE_HASH_MASK) | + (((uint64_t)(robotMode) << HAL_CONTROLWORD_ROBOT_MODE_SHIFT) & // NOLINT + HAL_CONTROLWORD_ROBOT_MODE_MASK) | + (enabled ? HAL_CONTROLWORD_ENABLED_MASK : 0) | + (eStop ? HAL_CONTROLWORD_ESTOP_MASK : 0) | + (fmsAttached ? HAL_CONTROLWORD_FMS_ATTACHED_MASK : 0) | + (dsAttached ? HAL_CONTROLWORD_DS_ATTACHED_MASK : 0); + return word; +} + +inline int64_t HAL_ControlWord_GetOpModeHash(HAL_ControlWord word) { + return word.value & HAL_CONTROLWORD_OPMODE_HASH_MASK; +} + +inline int64_t HAL_ControlWord_GetOpModeId(HAL_ControlWord word) { + // if the hash portion is zero, return 0 + if ((word.value & HAL_CONTROLWORD_OPMODE_HASH_MASK) == 0) { + return 0; + } + // otherwise return the full ID (which includes the robot mode) + return word.value & + (HAL_CONTROLWORD_OPMODE_HASH_MASK | HAL_CONTROLWORD_ROBOT_MODE_MASK); +} + +inline void HAL_ControlWord_SetOpModeId(HAL_ControlWord* word, int64_t id) { + // clear out the old hash and robot mode + word->value &= + ~(HAL_CONTROLWORD_OPMODE_HASH_MASK | HAL_CONTROLWORD_ROBOT_MODE_MASK); + // set the new id + word->value |= + id & (HAL_CONTROLWORD_OPMODE_HASH_MASK | HAL_CONTROLWORD_ROBOT_MODE_MASK); +} + +inline HAL_RobotMode HAL_ControlWord_GetRobotMode(HAL_ControlWord word) { + // NOLINTBEGIN + return (HAL_RobotMode)((word.value & HAL_CONTROLWORD_ROBOT_MODE_MASK) >> + HAL_CONTROLWORD_ROBOT_MODE_SHIFT); + // NOLINTEND +} + +inline HAL_Bool HAL_ControlWord_IsEnabled(HAL_ControlWord word) { + return (word.value & HAL_CONTROLWORD_ENABLED_MASK) != 0; +} + +inline HAL_Bool HAL_ControlWord_IsEStopped(HAL_ControlWord word) { + return (word.value & HAL_CONTROLWORD_ESTOP_MASK) != 0; +} + +inline HAL_Bool HAL_ControlWord_IsFMSAttached(HAL_ControlWord word) { + return (word.value & HAL_CONTROLWORD_FMS_ATTACHED_MASK) != 0; +} + +inline HAL_Bool HAL_ControlWord_IsDSAttached(HAL_ControlWord word) { + return (word.value & HAL_CONTROLWORD_DS_ATTACHED_MASK) != 0; +} + +// NOLINTBEGIN +// for use at compile time +#define HAL_MAKE_OPMODEID(mode, hash) \ + ((((int64_t)(mode) << HAL_OPMODE_ROBOT_MODE_SHIFT) & \ + HAL_OPMODE_ROBOT_MODE_MASK) | \ + ((hash) & HAL_OPMODE_HASH_MASK)) +// NOLINTEND + +inline int64_t HAL_MakeOpModeId(HAL_RobotMode mode, int64_t hash) { + return HAL_MAKE_OPMODEID(mode, hash); +} + +inline HAL_RobotMode HAL_OpMode_GetRobotMode(int64_t id) { + return (HAL_RobotMode)((id & HAL_OPMODE_ROBOT_MODE_MASK) >> // NOLINT + HAL_OPMODE_ROBOT_MODE_SHIFT); +} + +inline int64_t HAL_OpMode_GetHash(int64_t id) { + return id & HAL_OPMODE_HASH_MASK; +} +#ifdef __cplusplus +} // extern "C" +#endif // __cplusplus + /** @} */ + +#ifdef __cplusplus +namespace wpi::hal { + +/** + * The overall robot mode (not including enabled state). + */ +enum class RobotMode { + /// Unknown. + UNKNOWN = HAL_ROBOTMODE_UNKNOWN, + /// Autonomous. + AUTONOMOUS = HAL_ROBOTMODE_AUTONOMOUS, + /// Qualification. + TELEOPERATED = HAL_ROBOTMODE_TELEOPERATED, + /// Elimination. + TEST = HAL_ROBOTMODE_TEST +}; + +/** + * A wrapper around Driver Station control word. + */ +class ControlWord { + public: + /** + * Default constructor. + */ + ControlWord() = default; + + /** + * Constructs from state values. + * + * @param opModeHash opmode hash + * @param robotMode robot mode + * @param enabled enabled + * @param eStop emergency stopped + * @param fmsAttached FMS attached + * @param dsAttached DS attached + */ + ControlWord(int64_t opModeHash, RobotMode robotMode, bool enabled, bool eStop, + bool fmsAttached, bool dsAttached) + : m_word{HAL_MakeControlWord(opModeHash, + static_cast(robotMode), + enabled, eStop, fmsAttached, dsAttached)} {} + + /** + * Constructs from the native HAL value. + * + * @param word value + */ + explicit ControlWord(HAL_ControlWord word) : m_word{word} {} + + ControlWord(const ControlWord&) = default; + ControlWord& operator=(const ControlWord&) = default; + + /** + * Updates from state values. + * + * @param opModeHash opmode hash + * @param robotMode robot mode + * @param enabled enabled + * @param eStop emergency stopped + * @param fmsAttached FMS attached + * @param dsAttached DS attached + */ + void Update(int64_t opModeHash, RobotMode robotMode, bool enabled, bool eStop, + bool fmsAttached, bool dsAttached) { + m_word = + HAL_MakeControlWord(opModeHash, static_cast(robotMode), + enabled, eStop, fmsAttached, dsAttached); + } + + /** + * Updates from the native HAL value. + * + * @param word value + */ + void Update(HAL_ControlWord word) { m_word = word; } + + /** + * Check if the DS has enabled the robot. + * + * @return True if the robot is enabled and the DS is connected + */ + bool IsEnabled() const { return HAL_ControlWord_IsEnabled(m_word); } + + /** + * Gets the current robot mode. + * + *

Note that this does not indicate whether the robot is enabled or + * disabled. + * + * @return robot mode + */ + RobotMode GetRobotMode() const { + return static_cast(HAL_ControlWord_GetRobotMode(m_word)); + } + + /** + * Gets the current opmode ID. + * + * @return opmode + */ + int64_t GetOpModeId() const { return HAL_ControlWord_GetOpModeId(m_word); } + + /** + * Sets the opmode ID. + * + * @param id opmode ID + */ + void SetOpModeId(int64_t id) { HAL_ControlWord_SetOpModeId(&m_word, id); } + + /** + * Check if the robot is e-stopped. + * + * @return True if the robot is e-stopped + */ + bool IsEStopped() const { return HAL_ControlWord_IsEStopped(m_word); } + + /** + * Is the driver station attached to a Field Management System? + * + * @return True if the robot is competing on a field being controlled by a + * Field Management System + */ + bool IsFMSAttached() const { return HAL_ControlWord_IsFMSAttached(m_word); } + + /** + * Check if the DS is attached. + * + * @return True if the DS is connected to the robot + */ + bool IsDSAttached() const { return HAL_ControlWord_IsDSAttached(m_word); } + + /** + * Check if the DS is commanding autonomous mode. + * + * @return True if the robot is being commanded to be in autonomous mode + */ + bool IsAutonomous() const { return GetRobotMode() == RobotMode::AUTONOMOUS; } + + /** + * Check if the DS is commanding autonomous mode and if it has enabled the + * robot. + * + * @return True if the robot is being commanded to be in autonomous mode and + * enabled. + */ + bool IsAutonomousEnabled() const { + return IsAutonomous() && IsEnabled() && IsDSAttached(); + } + + /** + * Check if the DS is commanding teleop mode. + * + * @return True if the robot is being commanded to be in teleop mode + */ + bool IsTeleop() const { return GetRobotMode() == RobotMode::TELEOPERATED; } + + /** + * Check if the DS is commanding teleop mode and if it has enabled the robot. + * + * @return True if the robot is being commanded to be in teleop mode and + * enabled. + */ + bool IsTeleopEnabled() const { + return IsTeleop() && IsEnabled() && IsDSAttached(); + } + + /** + * Check if the DS is commanding test mode. + * + * @return True if the robot is being commanded to be in test mode + */ + bool IsTest() const { return GetRobotMode() == RobotMode::TEST; } + + /** + * Check if the DS is commanding test mode and if it has enabled the robot. + * + * @return True if the robot is being commanded to be in test mode and + * enabled. + */ + bool IsTestEnabled() const { + return IsTest() && IsEnabled() && IsDSAttached(); + } + + /** + * Get the HAL raw value. + * + * @return Control word value + */ + HAL_ControlWord GetValue() const { return m_word; } + + private: + HAL_ControlWord m_word{.value = 0}; +}; + +inline bool operator==(const ControlWord& lhs, const ControlWord& rhs) { + return lhs.GetValue().value == rhs.GetValue().value; +} + +inline bool operator!=(const ControlWord& lhs, const ControlWord& rhs) { + return !(lhs == rhs); +} + +} // namespace wpi::hal + +template <> +struct wpi::util::Struct { + static constexpr std::string_view GetTypeName() { return "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}" + "uint64 robotMode:2;" + "bool enabled:1;bool eStop:1;bool fmsAttached:1;bool dsAttached:1;"; + } + + static inline wpi::hal::ControlWord Unpack(std::span data) { + return wpi::hal::ControlWord{ + {.value = wpi::util::UnpackStruct(data)}}; + } + static inline void Pack(std::span data, + wpi::hal::ControlWord value) { + wpi::util::PackStruct(data, value.GetValue().value); + } +}; + +static_assert(wpi::util::StructSerializable); +#endif // __cplusplus diff --git a/hal/src/main/native/include/wpi/hal/simulation/DriverStationData.h b/hal/src/main/native/include/wpi/hal/simulation/DriverStationData.h index 097cc9a1f1..8339a84283 100644 --- a/hal/src/main/native/include/wpi/hal/simulation/DriverStationData.h +++ b/hal/src/main/native/include/wpi/hal/simulation/DriverStationData.h @@ -11,6 +11,9 @@ #include "wpi/hal/simulation/NotifyListener.h" #include "wpi/util/string.h" +typedef void (*HAL_OpModeOptionsCallback)(const char* name, void* param, + const HAL_OpModeOption* opmodes, + int32_t count); typedef void (*HAL_JoystickAxesCallback)(const char* name, void* param, int32_t joystickNum, const HAL_JoystickAxes* axes); @@ -39,6 +42,7 @@ extern "C" { #endif void HALSIM_ResetDriverStationData(void); + int32_t HALSIM_RegisterDriverStationEnabledCallback(HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify); @@ -46,18 +50,11 @@ void HALSIM_CancelDriverStationEnabledCallback(int32_t uid); HAL_Bool HALSIM_GetDriverStationEnabled(void); void HALSIM_SetDriverStationEnabled(HAL_Bool enabled); -int32_t HALSIM_RegisterDriverStationAutonomousCallback( +int32_t HALSIM_RegisterDriverStationRobotModeCallback( HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify); -void HALSIM_CancelDriverStationAutonomousCallback(int32_t uid); -HAL_Bool HALSIM_GetDriverStationAutonomous(void); -void HALSIM_SetDriverStationAutonomous(HAL_Bool autonomous); - -int32_t HALSIM_RegisterDriverStationTestCallback(HAL_NotifyCallback callback, - void* param, - HAL_Bool initialNotify); -void HALSIM_CancelDriverStationTestCallback(int32_t uid); -HAL_Bool HALSIM_GetDriverStationTest(void); -void HALSIM_SetDriverStationTest(HAL_Bool test); +void HALSIM_CancelDriverStationRobotModeCallback(int32_t uid); +HAL_RobotMode HALSIM_GetDriverStationRobotMode(void); +void HALSIM_SetDriverStationRobotMode(HAL_RobotMode mode); int32_t HALSIM_RegisterDriverStationEStopCallback(HAL_NotifyCallback callback, void* param, @@ -91,6 +88,21 @@ void HALSIM_CancelDriverStationMatchTimeCallback(int32_t uid); double HALSIM_GetDriverStationMatchTime(void); void HALSIM_SetDriverStationMatchTime(double matchTime); +int32_t HALSIM_RegisterDriverStationOpModeCallback(HAL_NotifyCallback callback, + void* param, + HAL_Bool initialNotify); +void HALSIM_CancelDriverStationOpModeCallback(int32_t uid); +int64_t HALSIM_GetDriverStationOpMode(void); +void HALSIM_SetDriverStationOpMode(int64_t opmode); + +int32_t HALSIM_RegisterOpModeOptionsCallback(HAL_OpModeOptionsCallback callback, + void* param, + HAL_Bool initialNotify); +void HALSIM_CancelOpModeOptionsCallback(int32_t uid); +struct HAL_OpModeOption* HALSIM_GetOpModeOptions(int32_t* len); + +void HALSIM_FreeOpModeOptionsArray(struct HAL_OpModeOption* arr, size_t length); + int32_t HALSIM_RegisterJoystickAxesCallback(int32_t joystickNum, HAL_JoystickAxesCallback callback, void* param, diff --git a/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h b/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h index 852435572f..2e900730b3 100644 --- a/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h +++ b/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h @@ -4,14 +4,21 @@ #pragma once +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/HALBase.h" #include "wpi/hal/Types.h" +#ifdef __cplusplus extern "C" { +#endif + void HALSIM_SetRuntimeType(HAL_RuntimeType type); void HALSIM_WaitForProgramStart(void); void HALSIM_SetProgramStarted(HAL_Bool started); HAL_Bool HALSIM_GetProgramStarted(void); +void HALSIM_SetProgramState(HAL_ControlWord controlWord); +void HALSIM_GetProgramState(HAL_ControlWord* controlWord); + void HALSIM_RestartTiming(void); void HALSIM_PauseTiming(void); void HALSIM_ResumeTiming(void); @@ -38,4 +45,18 @@ void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid); void HALSIM_CancelAllSimPeriodicCallbacks(void); +#ifdef __cplusplus } // extern "C" + +namespace wpi::hal::sim { +inline void SetProgramState(const ControlWord& controlWord) { + HALSIM_SetProgramState(controlWord.GetValue()); +} + +inline ControlWord GetProgramState() { + HAL_ControlWord word; + HALSIM_GetProgramState(&word); + return ControlWord{word}; +} +} // namespace wpi::hal::sim +#endif // __cplusplus diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index 4aa53a30e2..499532c0c5 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -18,11 +18,13 @@ #include "HALInitializer.h" #include "mockdata/DriverStationDataInternal.h" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/Errors.h" #include "wpi/hal/cpp/fpga_clock.h" #include "wpi/hal/simulation/MockHooks.h" #include "wpi/util/EventVector.hpp" #include "wpi/util/mutex.hpp" +#include "wpi/util/string.h" static wpi::util::mutex msgMutex; static std::atomic sendErrorHandler{nullptr}; @@ -69,15 +71,10 @@ void JoystickDataCache::Update() { allianceStation = SimDriverStationData->allianceStationId; matchTime = SimDriverStationData->matchTime; - HAL_ControlWord tmpControlWord; - std::memset(&tmpControlWord, 0, sizeof(tmpControlWord)); - tmpControlWord.enabled = SimDriverStationData->enabled; - tmpControlWord.autonomous = SimDriverStationData->autonomous; - tmpControlWord.test = SimDriverStationData->test; - tmpControlWord.eStop = SimDriverStationData->eStop; - tmpControlWord.fmsAttached = SimDriverStationData->fmsAttached; - tmpControlWord.dsAttached = SimDriverStationData->dsAttached; - this->controlWord = tmpControlWord; + controlWord = HAL_MakeControlWord( + SimDriverStationData->opMode, SimDriverStationData->robotMode, + SimDriverStationData->enabled, SimDriverStationData->eStop, + SimDriverStationData->fmsAttached, SimDriverStationData->dsAttached); } #define CHECK_JOYSTICK_NUMBER(stickNum) \ @@ -221,6 +218,7 @@ int32_t HAL_SendConsoleLine(const char* line) { int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) { if (gShutdown) { + controlWord->value = 0; return INCOMPATIBLE_STATE; } std::scoped_lock lock{driverStation->cacheMutex}; @@ -228,6 +226,35 @@ int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) { return 0; } +int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) { + if (gShutdown) { + controlWord->value = 0; + return INCOMPATIBLE_STATE; + } + bool dsAttached = SimDriverStationData->dsAttached; + if (dsAttached) { + *controlWord = HAL_MakeControlWord( + SimDriverStationData->opMode, SimDriverStationData->robotMode, + SimDriverStationData->enabled, SimDriverStationData->eStop, + SimDriverStationData->fmsAttached, SimDriverStationData->dsAttached); + } else { + controlWord->value = 0; + } + return 0; +} + +int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options, + int32_t count) { + if (gShutdown) { + return 0; + } + if (count < 0 || count > 1000 || (count != 0 && !options)) { + return PARAMETER_OUT_OF_RANGE; + } + SimDriverStationData->SetOpModeOptions({options, options + count}); + return 0; +} + HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) { if (gShutdown) { return HAL_AllianceStationID_kUnknown; @@ -370,20 +397,8 @@ void HAL_ObserveUserProgramStarting(void) { HALSIM_SetProgramStarted(true); } -void HAL_ObserveUserProgramDisabled(void) { - // TODO -} - -void HAL_ObserveUserProgramAutonomous(void) { - // TODO -} - -void HAL_ObserveUserProgramTeleop(void) { - // TODO -} - -void HAL_ObserveUserProgramTest(void) { - // TODO +void HAL_ObserveUserProgram(HAL_ControlWord word) { + HALSIM_SetProgramState(word); } HAL_Bool HAL_RefreshDSData(void) { @@ -415,8 +430,7 @@ HAL_Bool HAL_RefreshDSData(void) { // Also, when the DS has never been connected the rest of the fields // in control word are garbage, so we also need to zero out in that // case too - std::memset(¤tRead->controlWord, 0, - sizeof(currentRead->controlWord)); + currentRead->controlWord.value = 0; } newestControlWord = currentRead->controlWord; } @@ -450,7 +464,8 @@ HAL_Bool HAL_GetOutputsEnabled(void) { return false; } std::scoped_lock lock{driverStation->cacheMutex}; - return newestControlWord.enabled && newestControlWord.dsAttached; + return HAL_ControlWord_IsEnabled(newestControlWord) && + HAL_ControlWord_IsDSAttached(newestControlWord); } } // extern "C" diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp index 7699d67ed8..ebe5fddb50 100644 --- a/hal/src/main/native/sim/MockHooks.cpp +++ b/hal/src/main/native/sim/MockHooks.cpp @@ -15,6 +15,7 @@ #include "wpi/util/timestamp.h" static std::atomic programStarted{false}; +static std::atomic programState{0}; static std::atomic programStartTime{0}; static std::atomic programPauseTime{0}; @@ -98,6 +99,14 @@ HAL_Bool HALSIM_GetProgramStarted(void) { return GetProgramStarted(); } +void HALSIM_SetProgramState(HAL_ControlWord controlWord) { + programState = controlWord.value; +} + +void HALSIM_GetProgramState(HAL_ControlWord* controlWord) { + controlWord->value = programState; +} + void HALSIM_RestartTiming(void) { RestartTiming(); } diff --git a/hal/src/main/native/sim/mockdata/DriverStationData.cpp b/hal/src/main/native/sim/mockdata/DriverStationData.cpp index b64d102810..b4c2d04128 100644 --- a/hal/src/main/native/sim/mockdata/DriverStationData.cpp +++ b/hal/src/main/native/sim/mockdata/DriverStationData.cpp @@ -2,14 +2,30 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include "wpi/hal/simulation/DriverStationData.h" + +#include #include +#include +#include + +#include #include "DriverStationDataInternal.h" +#include "wpi/hal/DashboardOpMode.hpp" +#include "wpi/hal/DriverStationTypes.h" using namespace wpi::hal; +static void FreeOpModeOption(HAL_OpModeOption& option) { + WPI_FreeString(&option.name); + WPI_FreeString(&option.group); + WPI_FreeString(&option.description); +} + namespace wpi::hal::init { void InitializeDriverStationData() { + wpi::hal::InitializeDashboardOpMode(); static DriverStationData dsd; ::wpi::hal::SimDriverStationData = &dsd; } @@ -21,15 +37,21 @@ DriverStationData::DriverStationData() { ResetData(); } +DriverStationData::~DriverStationData() { + for (auto&& option : m_opModeOptions) { + FreeOpModeOption(option); + } +} + void DriverStationData::ResetData() { enabled.Reset(false); - autonomous.Reset(false); - test.Reset(false); + robotMode.Reset(HAL_ROBOTMODE_UNKNOWN); eStop.Reset(false); fmsAttached.Reset(false); dsAttached.Reset(false); allianceStationId.Reset(static_cast(0)); matchTime.Reset(-1.0); + opMode.Reset(0); { std::scoped_lock lock(m_joystickDataMutex); @@ -61,7 +83,72 @@ void DriverStationData::ResetData() { m_matchInfoCallbacks.Reset(); m_matchInfo = HAL_MatchInfo{}; } + { + std::scoped_lock lock{m_opModeMutex}; + m_opModeOptionsCallbacks.Reset(); + // XXX: do not clear options vector as it comes from robot code? + } m_newDataCallbacks.Reset(); + wpi::hal::SetDashboardOpModeOptions({}); +} + +void DriverStationData::SetOpModeOptions( + std::span options) { + std::scoped_lock lock{m_opModeMutex}; + + for (auto&& option : m_opModeOptions) { + FreeOpModeOption(option); + } + m_opModeOptions.clear(); + m_opModeOptions.reserve(options.size()); + for (const auto& option : options) { + if (option.id == 0) { + continue; + } + m_opModeOptions.emplace_back( + HAL_OpModeOption{static_cast(option.id), + wpi::util::copy_wpi_string(option.name), + wpi::util::copy_wpi_string(option.group), + wpi::util::copy_wpi_string(option.description), + option.textColor, option.backgroundColor}); + } + m_opModeOptionsCallbacks.Invoke(m_opModeOptions.data(), + m_opModeOptions.size()); + wpi::hal::SetDashboardOpModeOptions(options); +} + +int32_t DriverStationData::RegisterOpModeOptionsCallback( + HAL_OpModeOptionsCallback callback, void* param, HAL_Bool initialNotify) { + std::scoped_lock lock(m_opModeMutex); + int32_t uid = m_opModeOptionsCallbacks.Register(callback, param); + if (initialNotify) { + callback(GetOpModeOptionsName(), param, m_opModeOptions.data(), + m_opModeOptions.size()); + } + return uid; +} + +void DriverStationData::CancelOpModeOptionsCallback(int32_t uid) { + m_opModeOptionsCallbacks.Cancel(uid); +} + +HAL_OpModeOption* DriverStationData::GetOpModeOptions(int32_t* len) { + std::scoped_lock lock(m_opModeMutex); + *len = 0; + if (m_opModeOptions.empty()) { + return nullptr; + } + + auto options = static_cast( + std::malloc(sizeof(HAL_OpModeOption) * m_opModeOptions.size())); + std::copy(m_opModeOptions.begin(), m_opModeOptions.end(), options); + *len = m_opModeOptions.size(); + for (auto&& option : std::span{options, m_opModeOptions.size()}) { + option.name = wpi::util::copy_wpi_string(option.name); + option.group = wpi::util::copy_wpi_string(option.group); + option.description = wpi::util::copy_wpi_string(option.description); + } + return options; } #define DEFINE_CPPAPI_CALLBACKS(name, data, data2) \ @@ -495,13 +582,38 @@ void HALSIM_ResetDriverStationData(void) { SimDriverStationData, LOWERNAME) DEFINE_CAPI(HAL_Bool, Enabled, enabled) -DEFINE_CAPI(HAL_Bool, Autonomous, autonomous) -DEFINE_CAPI(HAL_Bool, Test, test) +DEFINE_CAPI(HAL_RobotMode, RobotMode, robotMode) DEFINE_CAPI(HAL_Bool, EStop, eStop) DEFINE_CAPI(HAL_Bool, FmsAttached, fmsAttached) DEFINE_CAPI(HAL_Bool, DsAttached, dsAttached) DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId, allianceStationId) DEFINE_CAPI(double, MatchTime, matchTime) +DEFINE_CAPI(int64_t, OpMode, opMode) + +int32_t HALSIM_RegisterOpModeOptionsCallback(HAL_OpModeOptionsCallback callback, + void* param, + HAL_Bool initialNotify) { + return SimDriverStationData->RegisterOpModeOptionsCallback(callback, param, + initialNotify); +} + +void HALSIM_CancelOpModeOptionsCallback(int32_t uid) { + return SimDriverStationData->CancelOpModeOptionsCallback(uid); +} + +struct HAL_OpModeOption* HALSIM_GetOpModeOptions(int32_t* len) { + return SimDriverStationData->GetOpModeOptions(len); +} + +void HALSIM_FreeOpModeOptionsArray(struct HAL_OpModeOption* arr, + size_t length) { + for (size_t i = 0; i < length; ++i) { + WPI_FreeString(&arr[i].name); + WPI_FreeString(&arr[i].group); + WPI_FreeString(&arr[i].description); + } + std::free(arr); +} #undef DEFINE_CAPI #define DEFINE_CAPI(name, data) \ @@ -704,8 +816,7 @@ void HALSIM_RegisterDriverStationAllCallbacks(HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) { REGISTER(enabled); - REGISTER(autonomous); - REGISTER(test); + REGISTER(robotMode); REGISTER(eStop); REGISTER(fmsAttached); REGISTER(dsAttached); diff --git a/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h b/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h index 8947380ec4..1e2eb85567 100644 --- a/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h +++ b/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h @@ -4,8 +4,13 @@ #pragma once -#include +#include +#include +#include +#include + +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/simulation/DriverStationData.h" #include "wpi/hal/simulation/SimCallbackRegistry.h" #include "wpi/hal/simulation/SimDataValue.h" @@ -15,13 +20,14 @@ namespace wpi::hal { class DriverStationData { HAL_SIMDATAVALUE_DEFINE_NAME(Enabled) - HAL_SIMDATAVALUE_DEFINE_NAME(Autonomous) - HAL_SIMDATAVALUE_DEFINE_NAME(Test) + HAL_SIMDATAVALUE_DEFINE_NAME(RobotMode) HAL_SIMDATAVALUE_DEFINE_NAME(EStop) HAL_SIMDATAVALUE_DEFINE_NAME(FmsAttached) HAL_SIMDATAVALUE_DEFINE_NAME(DsAttached) HAL_SIMDATAVALUE_DEFINE_NAME(AllianceStationId) HAL_SIMDATAVALUE_DEFINE_NAME(MatchTime) + HAL_SIMDATAVALUE_DEFINE_NAME(OpMode) + HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(OpModeOptions) HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickAxes) HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickPOVs) HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(JoystickButtons) @@ -36,11 +42,25 @@ class DriverStationData { MakeAllianceStationIdValue(HAL_AllianceStationID value) { return HAL_MakeEnum(value); } + static LLVM_ATTRIBUTE_ALWAYS_INLINE HAL_Value + MakeRobotModeValue(HAL_RobotMode value) { + return HAL_MakeEnum(value); + } public: DriverStationData(); + ~DriverStationData(); + DriverStationData(const DriverStationData&) = delete; + DriverStationData& operator=(const DriverStationData&) = delete; void ResetData(); + void SetOpModeOptions(std::span options); + + int32_t RegisterOpModeOptionsCallback(HAL_OpModeOptionsCallback callback, + void* param, HAL_Bool initialNotify); + void CancelOpModeOptionsCallback(int32_t uid); + HAL_OpModeOption* GetOpModeOptions(int32_t* len); + int32_t RegisterJoystickAxesCallback(int32_t joystickNum, HAL_JoystickAxesCallback callback, void* param, HAL_Bool initialNotify); @@ -141,8 +161,8 @@ class DriverStationData { void SetReplayNumber(int32_t replayNumber); SimDataValue enabled{false}; - SimDataValue autonomous{false}; - SimDataValue test{false}; + SimDataValue robotMode{ + HAL_ROBOTMODE_UNKNOWN}; SimDataValue eStop{false}; SimDataValue fmsAttached{ false}; @@ -151,8 +171,11 @@ class DriverStationData { GetAllianceStationIdName> allianceStationId{static_cast(0)}; SimDataValue matchTime{-1.0}; + SimDataValue opMode{0}; private: + SimCallbackRegistry + m_opModeOptionsCallbacks; SimCallbackRegistry m_joystickAxesCallbacks; SimCallbackRegistry @@ -194,6 +217,9 @@ class DriverStationData { wpi::util::spinlock m_matchInfoMutex; HAL_MatchInfo m_matchInfo; + + wpi::util::spinlock m_opModeMutex; + std::vector m_opModeOptions; }; extern DriverStationData* SimDriverStationData; } // namespace wpi::hal diff --git a/hal/src/main/native/systemcore/FRCDriverStation.cpp b/hal/src/main/native/systemcore/FRCDriverStation.cpp index 95bc4def86..f5f67d60cf 100644 --- a/hal/src/main/native/systemcore/FRCDriverStation.cpp +++ b/hal/src/main/native/systemcore/FRCDriverStation.cpp @@ -18,7 +18,9 @@ #include "HALInitializer.h" #include "SystemServerInternal.h" #include "mrc/NtNetComm.h" +#include "wpi/hal/DashboardOpMode.hpp" #include "wpi/hal/DriverStation.h" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/Errors.h" #include "wpi/hal/proto/ControlData.h" #include "wpi/hal/proto/ErrorInfo.h" @@ -37,6 +39,7 @@ #include "wpi/util/SmallVector.hpp" #include "wpi/util/condition_variable.hpp" #include "wpi/util/mutex.hpp" +#include "wpi/util/string.h" #include "wpi/util/timestamp.h" static_assert(sizeof(int32_t) >= sizeof(int), @@ -86,9 +89,7 @@ struct SystemServerDriverStation { MRC_MAX_NUM_JOYSTICKS> joystickOutputTopics; - wpi::nt::ProtobufPublisher> teleopOpModes; - wpi::nt::ProtobufPublisher> autoOpModes; - wpi::nt::ProtobufPublisher> testOpModes; + wpi::nt::ProtobufPublisher> opModeOptionsPublisher; wpi::nt::IntegerPublisher traceOpModePublisher; NT_Listener controlDataListener; @@ -151,33 +152,11 @@ struct SystemServerDriverStation { ROBOT_JOYSTICK_DESCRIPTORS_PATH) .Subscribe({}); - teleopOpModes = ntInst - .GetProtobufTopic>( - ROBOT_TELEOP_OP_MODES_PATH) - .Publish(); - autoOpModes = ntInst - .GetProtobufTopic>( - ROBOT_AUTO_OP_MODES_PATH) - .Publish(); - testOpModes = ntInst - .GetProtobufTopic>( - ROBOT_TEST_OP_MODES_PATH) - .Publish(); - - std::vector staticTeleopOpModes; - staticTeleopOpModes.emplace_back( - mrc::OpMode{"TeleOp", mrc::OpModeHash::MakeTele(2)}); - teleopOpModes.Set(staticTeleopOpModes); - - std::vector staticAutoOpModes; - staticAutoOpModes.emplace_back( - mrc::OpMode{"Auto", mrc::OpModeHash::MakeAuto(1)}); - autoOpModes.Set(staticAutoOpModes); - - std::vector staticTestOpModes; - staticTestOpModes.emplace_back( - mrc::OpMode{"Test", mrc::OpModeHash::MakeTest(3)}); - testOpModes.Set(staticTestOpModes); + opModeOptionsPublisher = ntInst + .GetProtobufTopic>( + ROBOT_OP_MODE_OPTIONS_PATH) + .Publish(); + opModeOptionsPublisher.Set({}); controlDataListener = ntInst.AddListener( controlDataSubscriber, NT_EVENT_VALUE_REMOTE | NT_EVENT_UNPUBLISH, @@ -243,13 +222,20 @@ void JoystickDataCache::Update(const mrc::ControlData& data) { allianceInt += 1; allianceStation = static_cast(allianceInt); - std::memset(&controlWord, 0, sizeof(controlWord)); - controlWord.enabled = data.ControlWord.Enabled; - controlWord.fmsAttached = data.ControlWord.FmsConnected; - controlWord.dsAttached = data.ControlWord.DsConnected; - controlWord.eStop = data.ControlWord.EStop; - controlWord.test = data.ControlWord.Test; - controlWord.autonomous = data.ControlWord.Auto; + if (data.ControlWord.SupportsOpModes) { + controlWord = HAL_MakeControlWord( + data.CurrentOpMode.ToValue(), + static_cast(data.ControlWord.RobotMode), + data.ControlWord.Enabled, data.ControlWord.EStop, + data.ControlWord.FmsConnected, data.ControlWord.DsConnected); + } else { + wpi::hal::EnableDashboardOpMode(); + auto robotMode = static_cast(data.ControlWord.RobotMode); + controlWord = HAL_MakeControlWord( + wpi::hal::GetDashboardSelectedOpMode(robotMode), robotMode, + data.ControlWord.Enabled, data.ControlWord.EStop, + data.ControlWord.FmsConnected, data.ControlWord.DsConnected); + } auto sticks = data.Joysticks(); @@ -373,7 +359,8 @@ void TcpCache::Update() { namespace wpi::hal::init { void InitializeFRCDriverStation() { - std::memset(&newestControlWord, 0, sizeof(newestControlWord)); + InitializeDashboardOpMode(); + newestControlWord.value = 0; static FRCDriverStation ds; driverStation = &ds; } @@ -478,6 +465,63 @@ int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) { return 0; } +int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) { + mrc::ControlData data; + int64_t dataTime{0}; + bool dataValid = systemServerDs->GetLastControlData(&data, &dataTime); + if (dataValid && data.ControlWord.DsConnected) { + if (data.ControlWord.SupportsOpModes) { + *controlWord = HAL_MakeControlWord( + data.CurrentOpMode.ToValue(), + static_cast(data.ControlWord.RobotMode), + data.ControlWord.Enabled, data.ControlWord.EStop, + data.ControlWord.FmsConnected, data.ControlWord.DsConnected); + } else { + wpi::hal::EnableDashboardOpMode(); + auto robotMode = static_cast(data.ControlWord.RobotMode); + *controlWord = HAL_MakeControlWord( + wpi::hal::GetDashboardSelectedOpMode(robotMode), robotMode, + data.ControlWord.Enabled, data.ControlWord.EStop, + data.ControlWord.FmsConnected, data.ControlWord.DsConnected); + } + } else { + // DS disconnected. Clear the control word + controlWord->value = 0; + } + return 0; +} + +int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options, + int32_t count) { + if (count < 0 || count > 1000 || (count != 0 && !options)) { + return PARAMETER_OUT_OF_RANGE; + } + + std::vector newOptions; + newOptions.reserve(count); + if (count != 0) { + for (auto&& option : std::span{options, options + count}) { + if (option.id == 0) { + continue; + } + newOptions.emplace_back(mrc::OpModeHash::FromValue(option.id), + wpi::util::to_string_view(&option.name), + wpi::util::to_string_view(&option.group), + wpi::util::to_string_view(&option.description), + option.textColor, option.backgroundColor); + } + } + + { + std::scoped_lock lock{tcpCacheMutex}; + systemServerDs->opModeOptionsPublisher.Set(newOptions); + } + + wpi::hal::SetDashboardOpModeOptions({options, options + count}); + + return 0; +} + int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) { CHECK_JOYSTICK_NUMBER(joystickNum); std::scoped_lock lock{cacheMutex}; @@ -619,24 +663,11 @@ void HAL_ObserveUserProgramStarting(void) { systemServerDs->hasUserCodeReadyPublisher.Set(true); } -void HAL_ObserveUserProgramDisabled(void) { - systemServerDs->traceOpModePublisher.Set( - mrc::OpModeHash::MakeTele(1, false).ToValue()); -} - -void HAL_ObserveUserProgramAutonomous(void) { - auto tVal = mrc::OpModeHash::MakeAuto(2, true).ToValue(); - systemServerDs->traceOpModePublisher.Set(tVal); -} - -void HAL_ObserveUserProgramTeleop(void) { - auto tVal = mrc::OpModeHash::MakeTele(1, true).ToValue(); - systemServerDs->traceOpModePublisher.Set(tVal); -} - -void HAL_ObserveUserProgramTest(void) { - systemServerDs->traceOpModePublisher.Set( - mrc::OpModeHash::MakeTest(3, true).ToValue()); +void HAL_ObserveUserProgram(HAL_ControlWord word) { + systemServerDs->traceOpModePublisher.Set(word.value & + (HAL_CONTROLWORD_OPMODE_HASH_MASK | + HAL_CONTROLWORD_ROBOT_MODE_MASK | + HAL_CONTROLWORD_ENABLED_MASK)); } HAL_Bool HAL_RefreshDSData(void) { @@ -657,8 +688,7 @@ HAL_Bool HAL_RefreshDSData(void) { updatedData = true; } else { // DS disconnected. Clear the control word - std::memset(&cacheToUpdate->controlWord, 0, - sizeof(cacheToUpdate->controlWord)); + cacheToUpdate->controlWord.value = 0; } { @@ -694,6 +724,7 @@ HAL_Bool HAL_GetSystemTimeValid(int32_t* status) { namespace wpi::hal { void InitializeDriverStation() { + StartDashboardOpMode(); systemServerDs = new ::SystemServerDriverStation{wpi::hal::GetSystemServer()}; } diff --git a/hal/src/main/native/systemcore/mockdata/DriverStationData.cpp b/hal/src/main/native/systemcore/mockdata/DriverStationData.cpp index 0878d06003..b22dd6d98d 100644 --- a/hal/src/main/native/systemcore/mockdata/DriverStationData.cpp +++ b/hal/src/main/native/systemcore/mockdata/DriverStationData.cpp @@ -4,6 +4,7 @@ #include "wpi/hal/simulation/DriverStationData.h" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/simulation/SimDataValue.h" extern "C" { @@ -14,14 +15,30 @@ void HALSIM_ResetDriverStationData(void) {} RETURN) DEFINE_CAPI(HAL_Bool, Enabled, false) -DEFINE_CAPI(HAL_Bool, Autonomous, false) -DEFINE_CAPI(HAL_Bool, Test, false) +DEFINE_CAPI(HAL_RobotMode, RobotMode, HAL_ROBOTMODE_UNKNOWN) DEFINE_CAPI(HAL_Bool, EStop, false) DEFINE_CAPI(HAL_Bool, FmsAttached, false) DEFINE_CAPI(HAL_Bool, DsAttached, false) DEFINE_CAPI(HAL_AllianceStationID, AllianceStationId, HAL_AllianceStationID_kRed1) DEFINE_CAPI(double, MatchTime, 0) +DEFINE_CAPI(int64_t, OpMode, 0) + +int32_t HALSIM_RegisterOpModeOptionsCallback(HAL_OpModeOptionsCallback callback, + void* param, + HAL_Bool initialNotify) { + return 0; +} + +void HALSIM_CancelOpModeOptionsCallback(int32_t uid) {} + +struct HAL_OpModeOption* HALSIM_GetOpModeOptions(int32_t* len) { + *len = 0; + return nullptr; +} + +void HALSIM_FreeOpModeOptionsArray(struct HAL_OpModeOption* arr, + size_t length) {} #undef DEFINE_CAPI #define DEFINE_CAPI(name, data) \ diff --git a/hal/src/main/native/systemcore/mockdata/MockHooks.cpp b/hal/src/main/native/systemcore/mockdata/MockHooks.cpp index c09a67ca40..a619cfb439 100644 --- a/hal/src/main/native/systemcore/mockdata/MockHooks.cpp +++ b/hal/src/main/native/systemcore/mockdata/MockHooks.cpp @@ -16,6 +16,12 @@ HAL_Bool HALSIM_GetProgramStarted(void) { return false; } +void HALSIM_SetProgramState(HAL_ControlWord controlWord) {} + +void HALSIM_GetProgramState(HAL_ControlWord* controlWord) { + controlWord->value = 0; +} + void HALSIM_RestartTiming(void) {} void HALSIM_PauseTiming(void) {} diff --git a/hal/src/main/python/pyproject.toml b/hal/src/main/python/pyproject.toml index 2c14ae0a58..a8c3d1cbd8 100644 --- a/hal/src/main/python/pyproject.toml +++ b/hal/src/main/python/pyproject.toml @@ -90,6 +90,7 @@ CANAPITypes = "wpi/hal/CANAPITypes.h" CTREPCM = "wpi/hal/CTREPCM.h" Constants = "wpi/hal/Constants.h" Counter = "wpi/hal/Counter.h" +DashboardOpMode = "wpi/hal/DashboardOpMode.hpp" DIO = "wpi/hal/DIO.h" # DMA = "hal/DMA.h" DriverStation = "wpi/hal/DriverStation.h" diff --git a/hal/src/main/python/semiwrap/DashboardOpMode.yml b/hal/src/main/python/semiwrap/DashboardOpMode.yml new file mode 100644 index 0000000000..34f684159c --- /dev/null +++ b/hal/src/main/python/semiwrap/DashboardOpMode.yml @@ -0,0 +1,6 @@ +functions: + InitializeDashboardOpMode: + SetDashboardOpModeOptions: + StartDashboardOpMode: + EnableDashboardOpMode: + GetDashboardSelectedOpMode: diff --git a/hal/src/main/python/semiwrap/DriverStation.yml b/hal/src/main/python/semiwrap/DriverStation.yml index 6b25923786..b4338414e0 100644 --- a/hal/src/main/python/semiwrap/DriverStation.yml +++ b/hal/src/main/python/semiwrap/DriverStation.yml @@ -36,9 +36,12 @@ functions: HAL_ProvideNewDataEventHandle: HAL_RemoveNewDataEventHandle: HAL_ObserveUserProgramStarting: - HAL_ObserveUserProgramDisabled: - HAL_ObserveUserProgramAutonomous: - HAL_ObserveUserProgramTeleop: - HAL_ObserveUserProgramTest: HAL_GetJoystickIsGamepad: HAL_GetJoystickTouchpads: + HAL_GetUncachedControlWord: + HAL_SetOpModeOptions: + HAL_ObserveUserProgram: + GetControlWord: + ignore: true + GetUncachedControlWord: + ignore: true diff --git a/hal/src/main/python/semiwrap/DriverStationTypes.yml b/hal/src/main/python/semiwrap/DriverStationTypes.yml index fc08278082..72ef21b68e 100644 --- a/hal/src/main/python/semiwrap/DriverStationTypes.yml +++ b/hal/src/main/python/semiwrap/DriverStationTypes.yml @@ -11,17 +11,12 @@ enums: value_prefix: HAL_JoystickPOV HAL_MatchType: value_prefix: HAL_kMatchType + HAL_RobotMode: + ignore: true + RobotMode: classes: HAL_ControlWord: - attributes: - enabled: - autonomous: - test: - eStop: - fmsAttached: - dsAttached: - control_reserved: - watchdogEnabled: + ignore: true HAL_JoystickAxes: attributes: axes: @@ -62,3 +57,49 @@ classes: attributes: count: touchpads: + HAL_OpModeOption: + attributes: + id: + name: + group: + description: + textColor: + backgroundColor: + wpi::hal::ControlWord: + methods: + ControlWord: + overloads: + "": + int64_t, RobotMode, bool, bool, bool, bool: + HAL_ControlWord: + Update: + overloads: + int64_t, RobotMode, bool, bool, bool, bool: + HAL_ControlWord: + IsEnabled: + GetRobotMode: + GetOpModeId: + SetOpModeId: + IsEStopped: + IsFMSAttached: + IsDSAttached: + IsAutonomous: + IsAutonomousEnabled: + IsTeleop: + IsTeleopEnabled: + IsTest: + IsTestEnabled: + GetValue: +functions: + HAL_MakeControlWord: + HAL_ControlWord_GetOpModeHash: + HAL_ControlWord_GetOpModeId: + HAL_ControlWord_SetOpModeId: + HAL_ControlWord_GetRobotMode: + HAL_ControlWord_IsEnabled: + HAL_ControlWord_IsEStopped: + HAL_ControlWord_IsFMSAttached: + HAL_ControlWord_IsDSAttached: + HAL_MakeOpModeId: + HAL_OpMode_GetRobotMode: + HAL_OpMode_GetHash: diff --git a/hal/src/main/python/semiwrap/simulation/DriverStationData.yml b/hal/src/main/python/semiwrap/simulation/DriverStationData.yml index 68dae4266b..7b411287df 100644 --- a/hal/src/main/python/semiwrap/simulation/DriverStationData.yml +++ b/hal/src/main/python/semiwrap/simulation/DriverStationData.yml @@ -8,16 +8,6 @@ functions: HALSIM_CancelDriverStationEnabledCallback: HALSIM_GetDriverStationEnabled: HALSIM_SetDriverStationEnabled: - HALSIM_RegisterDriverStationAutonomousCallback: - ignore: true - HALSIM_CancelDriverStationAutonomousCallback: - HALSIM_GetDriverStationAutonomous: - HALSIM_SetDriverStationAutonomous: - HALSIM_RegisterDriverStationTestCallback: - ignore: true - HALSIM_CancelDriverStationTestCallback: - HALSIM_GetDriverStationTest: - HALSIM_SetDriverStationTest: HALSIM_RegisterDriverStationEStopCallback: ignore: true HALSIM_CancelDriverStationEStopCallback: @@ -123,3 +113,20 @@ functions: HALSIM_SetJoystickTouchpads: HALSIM_SetJoystickTouchpadCounts: HALSIM_SetJoystickTouchpadFinger: + HALSIM_RegisterDriverStationRobotModeCallback: + ignore: true + HALSIM_CancelDriverStationRobotModeCallback: + HALSIM_GetDriverStationRobotMode: + HALSIM_SetDriverStationRobotMode: + HALSIM_RegisterDriverStationOpModeCallback: + ignore: true + HALSIM_CancelDriverStationOpModeCallback: + HALSIM_GetDriverStationOpMode: + HALSIM_SetDriverStationOpMode: + HALSIM_RegisterOpModeOptionsCallback: + ignore: true + HALSIM_CancelOpModeOptionsCallback: + HALSIM_GetOpModeOptions: + ignore: true + HALSIM_FreeOpModeOptionsArray: + ignore: true diff --git a/hal/src/main/python/semiwrap/simulation/MockHooks.yml b/hal/src/main/python/semiwrap/simulation/MockHooks.yml index 367131c8b2..4fe91009fb 100644 --- a/hal/src/main/python/semiwrap/simulation/MockHooks.yml +++ b/hal/src/main/python/semiwrap/simulation/MockHooks.yml @@ -10,6 +10,12 @@ functions: HALSIM_WaitForProgramStart: HALSIM_SetProgramStarted: HALSIM_GetProgramStarted: + HALSIM_SetProgramState: + ignore: true + HALSIM_GetProgramState: + ignore: true + SetProgramState: + GetProgramState: HALSIM_RestartTiming: HALSIM_PauseTiming: HALSIM_ResumeTiming: diff --git a/hal/src/mrc/include/mrc/NetComm.h b/hal/src/mrc/include/mrc/NetComm.h index b09cd228d3..7e3c40bfeb 100644 --- a/hal/src/mrc/include/mrc/NetComm.h +++ b/hal/src/mrc/include/mrc/NetComm.h @@ -18,22 +18,28 @@ namespace mrc { +enum class RobotMode : uint8_t { + Unknown = 0, + Autonomous = 1, + Teleoperated = 2, + Test = 3 +}; + struct OpModeHash { uint64_t Hash : 56 = 0; - uint64_t IsAuto : 1 = 0; - uint64_t IsTest : 1 = 0; + uint64_t RobotMode : 2 = 0; uint64_t IsEnabled : 1 = 0; uint64_t Reserved : 5 = 0; - static constexpr uint64_t AutoMask = 0x0100000000000000; - static constexpr uint64_t TestMask = 0x0200000000000000; + static constexpr uint64_t RobotModeMask = 0x0300000000000000; static constexpr uint64_t EnabledMask = 0x0400000000000000; static constexpr uint64_t HashMask = 0x00FFFFFFFFFFFFFF; + static constexpr int RobotModeShift = 56; constexpr static OpModeHash MakeTest(uint64_t Hash, bool Enabled = false) { OpModeHash FullHash; FullHash.Hash = Hash & HashMask; - FullHash.IsTest = 1; + FullHash.RobotMode = static_cast(RobotMode::Test); FullHash.IsEnabled = Enabled ? 1 : 0; return FullHash; } @@ -41,6 +47,7 @@ struct OpModeHash { constexpr static OpModeHash MakeTele(uint64_t Hash, bool Enabled = false) { OpModeHash FullHash; FullHash.Hash = Hash & HashMask; + FullHash.RobotMode = static_cast(RobotMode::Teleoperated); FullHash.IsEnabled = Enabled ? 1 : 0; return FullHash; } @@ -48,7 +55,7 @@ struct OpModeHash { constexpr static OpModeHash MakeAuto(uint64_t Hash, bool Enabled = false) { OpModeHash FullHash; FullHash.Hash = Hash & HashMask; - FullHash.IsAuto = 1; + FullHash.RobotMode = static_cast(RobotMode::Autonomous); FullHash.IsEnabled = Enabled ? 1 : 0; return FullHash; } @@ -56,16 +63,14 @@ struct OpModeHash { constexpr static OpModeHash FromValue(uint64_t Value) { OpModeHash RetVal; RetVal.Hash = Value & HashMask; - RetVal.IsAuto = (Value & AutoMask) != 0; - RetVal.IsTest = (Value & TestMask) != 0; + RetVal.RobotMode = (Value & RobotModeMask) >> RobotModeShift; RetVal.IsEnabled = (Value & EnabledMask) != 0; return RetVal; } constexpr uint64_t ToValue() const { uint64_t RetVal = Hash & HashMask; - RetVal |= IsAuto ? AutoMask : 0; - RetVal |= IsTest ? TestMask : 0; + RetVal |= static_cast(RobotMode) << RobotModeShift; RetVal |= IsEnabled ? EnabledMask : 0; return RetVal; } @@ -73,21 +78,22 @@ struct OpModeHash { struct ControlFlags { uint32_t Enabled : 1 = 0; - uint32_t Auto : 1 = 0; - uint32_t Test : 1 = 0; + uint32_t RobotMode : 2 = 0; uint32_t EStop : 1 = 0; uint32_t FmsConnected : 1 = 0; uint32_t DsConnected : 1 = 0; uint32_t WatchdogActive : 1 = 0; + uint32_t SupportsOpModes : 1 = 0; uint32_t Alliance : 6 = 0; - uint32_t Reserved : 19 = 0; + uint32_t Reserved : 18 = 0; constexpr bool operator==(const ControlFlags& Other) const { - return Enabled == Other.Enabled && Auto == Other.Auto && - Test == Other.Test && EStop == Other.EStop && - FmsConnected == Other.FmsConnected && + return Enabled == Other.Enabled && RobotMode == Other.RobotMode && + EStop == Other.EStop && FmsConnected == Other.FmsConnected && DsConnected == Other.DsConnected && - WatchdogActive == Other.WatchdogActive && Alliance == Other.Alliance; + WatchdogActive == Other.WatchdogActive && + SupportsOpModes == Other.SupportsOpModes && + Alliance == Other.Alliance; } constexpr bool operator!=(const ControlFlags& Other) const { @@ -96,12 +102,12 @@ struct ControlFlags { constexpr void Reset() { Enabled = 0; - Auto = 0; - Test = 0; + RobotMode = 0; EStop = 0; FmsConnected = 0; DsConnected = 0; WatchdogActive = 0; + SupportsOpModes = 0; Alliance = 0; Reserved = 0; } @@ -494,8 +500,13 @@ struct ErrorInfo { }; struct OpMode { - OpMode(std::string_view _Name, OpModeHash _Hash) : Hash(_Hash) { + OpMode(OpModeHash _Hash, std::string_view _Name, std::string_view _Group, + std::string_view _Description, int32_t _TextColor = -1, + int32_t _BackgroundColor = -1) + : Hash(_Hash), TextColor{_TextColor}, BackgroundColor{_BackgroundColor} { SetName(_Name); + SetGroup(_Group); + SetDescription(_Description); } OpMode() = default; @@ -527,8 +538,72 @@ struct OpMode { Name.size()}; } + void SetGroup(std::string_view NewGroup) { + if (NewGroup.size() > MRC_MAX_OPMODE_LEN) { + NewGroup = NewGroup.substr(0, MRC_MAX_OPMODE_LEN); + } + Group = NewGroup; + } + + void MoveGroup(std::string&& NewGroup) { + Group = std::move(NewGroup); + if (Group.size() > MRC_MAX_OPMODE_LEN) { + Group.resize(MRC_MAX_OPMODE_LEN); + } + } + + std::string_view GetGroup() const { return Group; } + + std::span WritableGroupBuffer(size_t Len) { + if (Len > MRC_MAX_OPMODE_LEN) { + Len = MRC_MAX_OPMODE_LEN; + } + Group.resize(Len); + return std::span{reinterpret_cast(Group.data()), + Group.size()}; + } + + void SetDescription(std::string_view NewDescription) { + if (NewDescription.size() > MRC_MAX_OPMODE_LEN) { + NewDescription = NewDescription.substr(0, MRC_MAX_OPMODE_LEN); + } + Description = NewDescription; + } + + void MoveDescription(std::string&& NewDescription) { + Description = std::move(NewDescription); + if (Description.size() > MRC_MAX_OPMODE_LEN) { + Description.resize(MRC_MAX_OPMODE_LEN); + } + } + + std::string_view GetDescription() const { return Description; } + + std::span WritableDescriptionBuffer(size_t Len) { + if (Len > MRC_MAX_OPMODE_LEN) { + Len = MRC_MAX_OPMODE_LEN; + } + Description.resize(Len); + return std::span{reinterpret_cast(Description.data()), + Description.size()}; + } + + void SetTextColor(int32_t NewTextColor) { TextColor = NewTextColor; } + + int32_t GetTextColor() const { return TextColor; } + + void SetBackgroundColor(int32_t NewBackgroundColor) { + BackgroundColor = NewBackgroundColor; + } + + int32_t GetBackgroundColor() const { return BackgroundColor; } + private: std::string Name; + std::string Group; + std::string Description; + int32_t TextColor{-1}; + int32_t BackgroundColor{-1}; }; } // namespace mrc diff --git a/hal/src/mrc/include/mrc/NtNetComm.h b/hal/src/mrc/include/mrc/NtNetComm.h index 735c5732ae..003242a943 100644 --- a/hal/src/mrc/include/mrc/NtNetComm.h +++ b/hal/src/mrc/include/mrc/NtNetComm.h @@ -41,10 +41,7 @@ #define ROBOT_USER_VERSION_STR_PATH \ (ROBOT_REPORTING_DATA_PREFIX "UserVersionStr") -#define ROBOT_MODES_PREFIX "/Netcomm/Modes/" -#define ROBOT_TELEOP_OP_MODES_PATH (ROBOT_MODES_PREFIX "TeleopOpModes") -#define ROBOT_AUTO_OP_MODES_PATH (ROBOT_MODES_PREFIX "AutoOpModes") -#define ROBOT_TEST_OP_MODES_PATH (ROBOT_MODES_PREFIX "TestOpModes") +#define ROBOT_OP_MODE_OPTIONS_PATH "/Netcomm/OpModeOptions" #define ROBOT_SYSTEM_SERVER_PREFIX "/sys/" #define ROBOT_BATTERY_VOLTAGE_PATH (ROBOT_SYSTEM_SERVER_PREFIX "battery") diff --git a/hal/src/mrc/proto/MrcComm.proto b/hal/src/mrc/proto/MrcComm.proto index 229b7d5e8e..f3feac7e7e 100644 --- a/hal/src/mrc/proto/MrcComm.proto +++ b/hal/src/mrc/proto/MrcComm.proto @@ -27,7 +27,7 @@ message ProtobufJoystickData { } message ProtobufControlData { - uint32 ControlWord = 1; + uint32 ControlWord = 5; int32 MatchTime = 2; repeated ProtobufJoystickData Joysticks = 3; fixed64 CurrentOpMode = 4; @@ -72,6 +72,10 @@ message ProtobufErrorInfo { message ProtobufOpMode { fixed64 Hash = 1; string Name = 2; + string Group = 3; + string Description = 4; + int32 TextColor = 5; + int32 BackgroundColor = 6; } message ProtobufAvailableOpModes { diff --git a/shared/examplecheck.gradle b/shared/examplecheck.gradle index b9dee932e7..934deaed8d 100644 --- a/shared/examplecheck.gradle +++ b/shared/examplecheck.gradle @@ -62,7 +62,7 @@ def tagList = [ "XboxController", "PS4Controller", "PS5Controller", "Joystick", /* --- RobotBase --- */ - "Timed", "Timeslice", "RobotBase", "Educational", + "Timed", "Timeslice", "RobotBase", "Educational", "OpMode", /* --- Misc --- */ /* (try to keep this section minimal) */ diff --git a/simulation/halsim_ds_socket/build.gradle b/simulation/halsim_ds_socket/build.gradle index a8bbf86f32..a1805c6d08 100644 --- a/simulation/halsim_ds_socket/build.gradle +++ b/simulation/halsim_ds_socket/build.gradle @@ -42,6 +42,7 @@ model { } binaries { all { + project(':hal').addHalDependency(it, 'shared') project(':ntcore').addNtcoreDependency(it, 'shared') lib project: ':wpinet', library: 'wpinet', linkage: 'shared' lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp index bcdd88297c..09a5c16e48 100644 --- a/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp +++ b/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp @@ -5,12 +5,11 @@ #include "wpi/halsim/ds_socket/DSCommPacket.hpp" #include -#include #include #include -#include -#include +#include "wpi/hal/DashboardOpMode.hpp" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/simulation/DriverStationData.h" #include "wpi/hal/simulation/MockHooks.h" @@ -54,13 +53,18 @@ DSCommPacket::DSCommPacket() { **--------------------------------------------------------------------------*/ void DSCommPacket::SetControl(uint8_t control, uint8_t request) { - std::memset(&m_control_word, 0, sizeof(m_control_word)); - m_control_word.enabled = (control & kEnabled) != 0; - m_control_word.autonomous = (control & kAutonomous) != 0; - m_control_word.test = (control & kTest) != 0; - m_control_word.eStop = (control & kEmergencyStop) != 0; - m_control_word.fmsAttached = (control & kFMS_Attached) != 0; - m_control_word.dsAttached = (request & kRequestNormalMask) != 0; + HAL_RobotMode robotMode; + if ((control & kAutonomous) != 0) { + robotMode = HAL_ROBOTMODE_AUTONOMOUS; + } else if ((control & kTest) != 0) { + robotMode = HAL_ROBOTMODE_TEST; + } else { + robotMode = HAL_ROBOTMODE_TELEOPERATED; + } + m_control_word = HAL_MakeControlWord( + wpi::hal::GetDashboardSelectedOpMode(robotMode), robotMode, + (control & kEnabled) != 0, (control & kEmergencyStop) != 0, + (control & kFMS_Attached) != 0, (request & kRequestNormalMask) != 0); m_control_sent = control; } @@ -305,17 +309,19 @@ void DSCommPacket::SetupSendHeader(wpi::net::raw_uv_ostream& buf) { void DSCommPacket::SendUDPToHALSim(void) { SendJoysticks(); - if (!m_control_word.enabled) { + if (!HAL_ControlWord_IsEnabled(m_control_word)) { m_match_time = -1; } HALSIM_SetDriverStationMatchTime(m_match_time); - HALSIM_SetDriverStationEnabled(m_control_word.enabled); - HALSIM_SetDriverStationAutonomous(m_control_word.autonomous); - HALSIM_SetDriverStationTest(m_control_word.test); - HALSIM_SetDriverStationEStop(m_control_word.eStop); - HALSIM_SetDriverStationFmsAttached(m_control_word.fmsAttached); - HALSIM_SetDriverStationDsAttached(m_control_word.dsAttached); + HALSIM_SetDriverStationEnabled(HAL_ControlWord_IsEnabled(m_control_word)); + HALSIM_SetDriverStationRobotMode( + HAL_ControlWord_GetRobotMode(m_control_word)); + HALSIM_SetDriverStationEStop(HAL_ControlWord_IsEStopped(m_control_word)); + HALSIM_SetDriverStationFmsAttached( + HAL_ControlWord_IsFMSAttached(m_control_word)); + HALSIM_SetDriverStationDsAttached( + HAL_ControlWord_IsDSAttached(m_control_word)); HALSIM_SetDriverStationAllianceStationId(m_alliance_station); HALSIM_NotifyDriverStationNewData(); diff --git a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp index 353757fc9a..c4f68bdd39 100644 --- a/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp +++ b/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp @@ -20,6 +20,7 @@ #include #include +#include "wpi/hal/DashboardOpMode.hpp" #include "wpi/hal/Extensions.h" #include "wpi/halsim/ds_socket/DSCommPacket.hpp" #include "wpi/net/EventLoopRunner.hpp" @@ -97,6 +98,7 @@ static void SetupTcp(wpi::net::uv::Loop& loop) { tcp->Listen([t = tcp.get()] { auto client = t->Accept(); gDSConnected = true; + wpi::hal::EnableDashboardOpMode(); client->data.connect([t](Buffer& buf, size_t len) { HandleTcpDataStream(buf, len, *t->GetData()); diff --git a/simulation/halsim_gui/build.gradle b/simulation/halsim_gui/build.gradle index 598fd814dc..3334c87312 100644 --- a/simulation/halsim_gui/build.gradle +++ b/simulation/halsim_gui/build.gradle @@ -27,6 +27,7 @@ model { lib project: ':glass', library: 'glass', linkage: 'static' lib project: ':wpigui', library: 'wpigui', linkage: 'static' lib project: ':wpimath', library: 'wpimath', linkage: 'shared' + project(':hal').addHalDependency(it, 'shared') project(':ntcore').addNtcoreDependency(it, 'shared') lib project: ':wpinet', library: 'wpinet', linkage: 'shared' lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' diff --git a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp index d37399bc94..83a8baa3b9 100644 --- a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp +++ b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -23,6 +24,7 @@ #include "wpi/glass/support/ExtraGuiWidgets.hpp" #include "wpi/glass/support/NameSetting.hpp" #include "wpi/gui/wpigui.hpp" +#include "wpi/hal/DashboardOpMode.hpp" #include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/simulation/DriverStationData.h" #include "wpi/hal/simulation/MockHooks.h" @@ -226,9 +228,8 @@ class FMSSimModel : public wpi::glass::FMSModel { wpi::glass::DoubleSource* GetMatchTimeData() override { return &m_matchTime; } wpi::glass::BooleanSource* GetEStopData() override { return &m_estop; } wpi::glass::BooleanSource* GetEnabledData() override { return &m_enabled; } - wpi::glass::BooleanSource* GetTestData() override { return &m_test; } - wpi::glass::BooleanSource* GetAutonomousData() override { - return &m_autonomous; + wpi::glass::IntegerSource* GetRobotModeData() override { + return &m_robotMode; } wpi::glass::StringSource* GetGameSpecificMessageData() override { return &m_gameMessage; @@ -242,8 +243,7 @@ class FMSSimModel : public wpi::glass::FMSModel { void SetMatchTime(double val) override { m_matchTime.SetValue(val); } void SetEStop(bool val) override { m_estop.SetValue(val); } void SetEnabled(bool val) override { m_enabled.SetValue(val); } - void SetTest(bool val) override { m_test.SetValue(val); } - void SetAutonomous(bool val) override { m_autonomous.SetValue(val); } + void SetRobotMode(RobotMode val) override { m_robotMode.SetValue(val); } void SetGameSpecificMessage(std::string_view val) override { m_gameMessage.SetValue(val); } @@ -263,8 +263,7 @@ class FMSSimModel : public wpi::glass::FMSModel { wpi::glass::DoubleSource m_matchTime{"FMS:MatchTime"}; wpi::glass::BooleanSource m_estop{"FMS:EStop"}; wpi::glass::BooleanSource m_enabled{"FMS:RobotEnabled"}; - wpi::glass::BooleanSource m_test{"FMS:TestMode"}; - wpi::glass::BooleanSource m_autonomous{"FMS:AutonomousMode"}; + wpi::glass::IntegerSource m_robotMode{"FMS:RobotMode"}; double m_startMatchTime = -1.0; wpi::glass::StringSource m_gameMessage{"FMS:GameSpecificMessage"}; }; @@ -287,11 +286,72 @@ static std::unique_ptr gFMSModel; std::unique_ptr DriverStationGui::dsManager; static bool* gpDisableDS = nullptr; +static bool* gpDashboardOpModes = nullptr; static bool* gpZeroDisconnectedJoysticks = nullptr; static bool* gpUseEnableDisableHotkeys = nullptr; static bool* gpUseEstopHotkey = nullptr; static std::atomic* gpDSSocketConnected = nullptr; +// OpMode options +namespace { +struct OpModeOption { + int64_t id; + std::string name; + std::string description; + int32_t textColor; + int32_t backgroundColor; +}; + +struct OpModes { + std::map ids; + wpi::util::StringMap> groups; +}; +} // namespace +static wpi::util::mutex gOpModeOptionsMutex; +static OpModes gAutoOpModes; +static OpModes gTeleopOpModes; +static OpModes gTestOpModes; + +static void UpdateOpModes(const char* name, void* param, + const HAL_OpModeOption* opmodes, int32_t count) { + std::scoped_lock lock(gOpModeOptionsMutex); + gAutoOpModes.ids.clear(); + gAutoOpModes.groups.clear(); + gTeleopOpModes.ids.clear(); + gTeleopOpModes.groups.clear(); + gTestOpModes.ids.clear(); + gTestOpModes.groups.clear(); + for (auto&& o : std::span{opmodes, opmodes + count}) { + OpModes* vec; + switch (HAL_OpMode_GetRobotMode(o.id)) { + case HAL_ROBOTMODE_AUTONOMOUS: + vec = &gAutoOpModes; + break; + case HAL_ROBOTMODE_TELEOPERATED: + vec = &gTeleopOpModes; + break; + case HAL_ROBOTMODE_TEST: + vec = &gTestOpModes; + break; + default: + continue; + } + vec->ids[o.id] = wpi::util::to_string_view(&o.name); + vec->groups[wpi::util::to_string_view(&o.group)].emplace_back( + OpModeOption{o.id, std::string{wpi::util::to_string_view(&o.name)}, + std::string{wpi::util::to_string_view(&o.description)}, + o.textColor, o.backgroundColor}); + } + for (auto&& vec : {&gAutoOpModes, &gTeleopOpModes, &gTestOpModes}) { + for (auto&& [group, options] : vec->groups) { + std::sort(options.begin(), options.end(), + [](const OpModeOption& a, const OpModeOption& b) { + return a.name < b.name; + }); + } + } +} + static inline bool IsDSDisabled() { return (gpDisableDS != nullptr && *gpDisableDS) || (gpDSSocketConnected && *gpDSSocketConnected); @@ -1001,19 +1061,24 @@ void RobotJoystick::GetHAL(int i) { HALSIM_GetJoystickPOVs(i, &data.povs); } -static void DriverStationConnect(bool enabled, bool autonomous, bool test) { +static void DriverStationSetRobotMode(HAL_RobotMode mode) { if (!HALSIM_GetDriverStationDsAttached()) { // initialize FMS bits too gFMSModel->SetDsAttached(true); - gFMSModel->SetEnabled(enabled); - gFMSModel->SetAutonomous(autonomous); - gFMSModel->SetTest(test); + gFMSModel->SetEnabled(false); + gFMSModel->SetRobotMode(static_cast(mode)); gFMSModel->UpdateHAL(); - } else { - HALSIM_SetDriverStationEnabled(enabled); - HALSIM_SetDriverStationAutonomous(autonomous); - HALSIM_SetDriverStationTest(test); } + HALSIM_SetDriverStationDsAttached(true); + HALSIM_SetDriverStationEnabled(false); + HALSIM_SetDriverStationOpMode(0); + HALSIM_SetDriverStationRobotMode(mode); +} + +static void DriverStationSetEnabled(bool enabled) { + gFMSModel->SetEnabled(enabled); + gFMSModel->UpdateHAL(); + HALSIM_SetDriverStationEnabled(enabled); } static void DriverStationExecute() { @@ -1061,6 +1126,10 @@ static void DriverStationExecute() { return; } + if (*gpDashboardOpModes) { + wpi::hal::EnableDashboardOpMode(); + } + double curTime = glfwGetTime(); // update system joysticks @@ -1082,8 +1151,10 @@ static void DriverStationExecute() { bool isAttached = HALSIM_GetDriverStationDsAttached(); bool isEnabled = HALSIM_GetDriverStationEnabled(); - bool isAuto = HALSIM_GetDriverStationAutonomous(); - bool isTest = HALSIM_GetDriverStationTest(); + HAL_RobotMode robotMode = HALSIM_GetDriverStationRobotMode(); + int64_t opMode = HALSIM_GetDriverStationOpMode(); + bool started = HALSIM_GetProgramStarted(); + int64_t programOpMode = wpi::hal::sim::GetProgramState().GetOpModeId(); // Robot state { @@ -1119,31 +1190,104 @@ static void DriverStationExecute() { ImGui::Begin(title, nullptr, ImGuiWindowFlags_AlwaysAutoResize); if (ImGui::Selectable("Disconnected", !isAttached)) { HALSIM_SetDriverStationEnabled(false); + HALSIM_SetDriverStationOpMode(0); HALSIM_SetDriverStationDsAttached(false); isAttached = false; gFMSModel->Update(); } - if (ImGui::Selectable("Disabled", isAttached && !isEnabled) || + if (ImGui::Selectable( + "Autonomous", + isAttached && robotMode == HAL_ROBOTMODE_AUTONOMOUS)) { + DriverStationSetRobotMode(HAL_ROBOTMODE_AUTONOMOUS); + } + if (ImGui::Selectable( + "Teleoperated", + isAttached && robotMode == HAL_ROBOTMODE_TELEOPERATED)) { + DriverStationSetRobotMode(HAL_ROBOTMODE_TELEOPERATED); + } + if (ImGui::Selectable("Test", + isAttached && robotMode == HAL_ROBOTMODE_TEST)) { + DriverStationSetRobotMode(HAL_ROBOTMODE_TEST); + } + // OpMode + bool canEnable = isAttached && started; + if (*gpDashboardOpModes) { + HALSIM_SetDriverStationOpMode( + wpi::hal::GetDashboardSelectedOpMode(robotMode)); + ImGui::Separator(); + } else { + OpModes* modes; + switch (robotMode) { + case HAL_ROBOTMODE_AUTONOMOUS: + modes = &gAutoOpModes; + break; + case HAL_ROBOTMODE_TELEOPERATED: + modes = &gTeleopOpModes; + break; + case HAL_ROBOTMODE_TEST: + modes = &gTestOpModes; + break; + default: + modes = nullptr; + break; + } + if (modes) { + std::scoped_lock lock{gOpModeOptionsMutex}; + auto nameIt = modes->ids.find(opMode); + auto name = nameIt != modes->ids.end() ? nameIt->second.c_str() : ""; + if (ImGui::BeginCombo("OpMode", name)) { + for (auto&& [groupName, group] : modes->groups) { + if (!groupName.empty()) { + ImGui::TextDisabled("%s", groupName.c_str()); + ImGui::Separator(); + } + for (auto&& mode : group) { + bool selected = mode.id == opMode; + if (ImGui::Selectable(mode.name.c_str(), selected)) { + HALSIM_SetDriverStationOpMode(mode.id); + } + } + } + ImGui::EndCombo(); + } + ImGui::SameLine(); + if (opMode == programOpMode) { + if (opMode == 0) { + ImGui::TextUnformatted("NONE"); + if (!modes->ids.empty()) { + canEnable = false; + } + } else { + ImGui::TextUnformatted("GOOD"); + } + } else { + ImGui::TextUnformatted("BAD "); + canEnable = false; + } + } else { + if (ImGui::BeginCombo("OpMode", "")) { + ImGui::EndCombo(); + } + ImGui::SameLine(); + ImGui::TextUnformatted(" "); + } + } + // Enable/Disable + if (ImGui::Selectable("Disable", isAttached && !isEnabled, + isAttached ? 0 : ImGuiSelectableFlags_Disabled) || disableHotkey) { - DriverStationConnect(false, false, false); + DriverStationSetEnabled(false); } - if (ImGui::Selectable("Autonomous", - isAttached && isEnabled && isAuto && !isTest)) { - DriverStationConnect(true, true, false); - } - if (ImGui::Selectable("Teleoperated", - isAttached && isEnabled && !isAuto && !isTest) || - enableHotkey) { - DriverStationConnect(true, false, false); - } - if (ImGui::Selectable("Test", isEnabled && isTest)) { - DriverStationConnect(true, false, true); + if (ImGui::Selectable("Enable", isAttached && isEnabled, + canEnable ? 0 : ImGuiSelectableFlags_Disabled) || + (canEnable && enableHotkey)) { + DriverStationSetEnabled(true); } ImGui::End(); } // Update HAL - if (isAttached && !isAuto) { + if (isAttached && robotMode != HAL_ROBOTMODE_AUTONOMOUS) { for (int i = 0, end = gRobotJoysticks.size(); i < end && i < HAL_kMaxJoysticks; ++i) { gRobotJoysticks[i].SetHAL(i); @@ -1171,8 +1315,8 @@ void FMSSimModel::UpdateHAL() { static_cast(m_allianceStationId.GetValue())); HALSIM_SetDriverStationEStop(m_estop.GetValue()); HALSIM_SetDriverStationEnabled(m_enabled.GetValue()); - HALSIM_SetDriverStationTest(m_test.GetValue()); - HALSIM_SetDriverStationAutonomous(m_autonomous.GetValue()); + HALSIM_SetDriverStationRobotMode( + static_cast(m_robotMode.GetValue())); HALSIM_SetDriverStationMatchTime(m_matchTime.GetValue()); auto str = wpi::util::make_string(m_gameMessage.GetValue()); HALSIM_SetGameSpecificMessage(&str); @@ -1186,8 +1330,7 @@ void FMSSimModel::Update() { m_allianceStationId.SetValue(HALSIM_GetDriverStationAllianceStationId()); m_estop.SetValue(HALSIM_GetDriverStationEStop()); m_enabled.SetValue(enabled); - m_test.SetValue(HALSIM_GetDriverStationTest()); - m_autonomous.SetValue(HALSIM_GetDriverStationAutonomous()); + m_robotMode.SetValue(HALSIM_GetDriverStationRobotMode()); double matchTime = HALSIM_GetDriverStationMatchTime(); if (!IsDSDisabled() && enabled) { @@ -1405,6 +1548,9 @@ void DSManager::DisplayMenu() { if (gpDisableDS != nullptr) { ImGui::MenuItem("Turn off DS", nullptr, gpDisableDS); } + if (gpDashboardOpModes != nullptr) { + ImGui::MenuItem("Use Dashboard OpModes", nullptr, gpDashboardOpModes); + } if (gpZeroDisconnectedJoysticks != nullptr) { ImGui::MenuItem("Zero disconnected joysticks", nullptr, gpZeroDisconnectedJoysticks); @@ -1437,10 +1583,13 @@ void DriverStationGui::GlobalInit() { gFMSModel = std::make_unique(); + HALSIM_RegisterOpModeOptionsCallback(UpdateOpModes, nullptr, true); + wpi::gui::AddEarlyExecute(DriverStationExecute); storageRoot.SetCustomApply([&storageRoot] { gpDisableDS = &storageRoot.GetBool("disable", false); + gpDashboardOpModes = &storageRoot.GetBool("dashboardOpModes", false); gpZeroDisconnectedJoysticks = &storageRoot.GetBool("zeroDisconnectedJoysticks", true); gpUseEnableDisableHotkeys = diff --git a/simulation/halsim_ws_client/build.gradle b/simulation/halsim_ws_client/build.gradle index 71ed2dc2f7..e8bd3313a9 100644 --- a/simulation/halsim_ws_client/build.gradle +++ b/simulation/halsim_ws_client/build.gradle @@ -28,7 +28,10 @@ model { return } + project(':hal').addHalDependency(it, 'shared') + project(':ntcore').addNtcoreDependency(it, 'shared') lib project: ':wpinet', library: 'wpinet', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static" } } diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp index 0d32e430e4..0f4569c3c3 100644 --- a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp @@ -48,8 +48,7 @@ HALSimWSProviderDriverStation::~HALSimWSProviderDriverStation() { void HALSimWSProviderDriverStation::RegisterCallbacks() { m_enabledCbKey = REGISTER(Enabled, ">enabled", bool, boolean); - m_autonomousCbKey = REGISTER(Autonomous, ">autonomous", bool, boolean); - m_testCbKey = REGISTER(Test, ">test", bool, boolean); + m_robotModeCbKey = REGISTER(RobotMode, ">robotMode", int, enum); m_estopCbKey = REGISTER(EStop, ">estop", bool, boolean); m_fmsCbKey = REGISTER(FmsAttached, ">fms", bool, boolean); m_dsCbKey = REGISTER(DsAttached, ">ds", bool, boolean); @@ -102,8 +101,7 @@ void HALSimWSProviderDriverStation::CancelCallbacks() { void HALSimWSProviderDriverStation::DoCancelCallbacks() { HALSIM_CancelDriverStationEnabledCallback(m_enabledCbKey); - HALSIM_CancelDriverStationAutonomousCallback(m_autonomousCbKey); - HALSIM_CancelDriverStationTestCallback(m_testCbKey); + HALSIM_CancelDriverStationRobotModeCallback(m_robotModeCbKey); HALSIM_CancelDriverStationEStopCallback(m_estopCbKey); HALSIM_CancelDriverStationFmsAttachedCallback(m_fmsCbKey); HALSIM_CancelDriverStationDsAttachedCallback(m_dsCbKey); @@ -112,8 +110,7 @@ void HALSimWSProviderDriverStation::DoCancelCallbacks() { HALSIM_CancelDriverStationMatchTimeCallback(m_matchTimeCbKey); m_enabledCbKey = 0; - m_autonomousCbKey = 0; - m_testCbKey = 0; + m_robotModeCbKey = 0; m_estopCbKey = 0; m_fmsCbKey = 0; m_dsCbKey = 0; @@ -133,11 +130,8 @@ void HALSimWSProviderDriverStation::OnNetValueChanged( if ((it = json.find(">enabled")) != json.end()) { HALSIM_SetDriverStationEnabled(it.value()); } - if ((it = json.find(">autonomous")) != json.end()) { - HALSIM_SetDriverStationAutonomous(it.value()); - } - if ((it = json.find(">test")) != json.end()) { - HALSIM_SetDriverStationTest(it.value()); + if ((it = json.find(">robotMode")) != json.end()) { + HALSIM_SetDriverStationRobotMode(it.value()); } if ((it = json.find(">estop")) != json.end()) { HALSIM_SetDriverStationEStop(it.value()); diff --git a/simulation/halsim_ws_core/src/main/native/include/wpi/halsim/ws_core/WSProvider_DriverStation.hpp b/simulation/halsim_ws_core/src/main/native/include/wpi/halsim/ws_core/WSProvider_DriverStation.hpp index e4f3da8952..f32ac0b56d 100644 --- a/simulation/halsim_ws_core/src/main/native/include/wpi/halsim/ws_core/WSProvider_DriverStation.hpp +++ b/simulation/halsim_ws_core/src/main/native/include/wpi/halsim/ws_core/WSProvider_DriverStation.hpp @@ -26,8 +26,7 @@ class HALSimWSProviderDriverStation : public HALSimWSHalProvider { private: int32_t m_enabledCbKey = 0; - int32_t m_autonomousCbKey = 0; - int32_t m_testCbKey = 0; + int32_t m_robotModeCbKey = 0; int32_t m_estopCbKey = 0; int32_t m_fmsCbKey = 0; int32_t m_dsCbKey = 0; diff --git a/simulation/halsim_ws_server/build.gradle b/simulation/halsim_ws_server/build.gradle index 49a314d2e2..2973e0b799 100644 --- a/simulation/halsim_ws_server/build.gradle +++ b/simulation/halsim_ws_server/build.gradle @@ -49,14 +49,17 @@ model { return } + project(':hal').addHalDependency(it, 'shared') project(':ntcore').addNtcoreDependency(it, 'shared') lib project: ':wpinet', library: 'wpinet', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static" } withType(GoogleTestTestSuiteBinarySpec) { project(':hal').addHalDependency(it, 'shared') project(':ntcore').addNtcoreDependency(it, 'shared') + lib project: ':wpinet', library: 'wpinet', linkage: 'shared' lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' lib library: pluginName, linkage: 'shared' } diff --git a/simulation/halsim_ws_server/src/test/native/cpp/main.cpp b/simulation/halsim_ws_server/src/test/native/cpp/main.cpp index 6135dac186..aeff393a35 100644 --- a/simulation/halsim_ws_server/src/test/native/cpp/main.cpp +++ b/simulation/halsim_ws_server/src/test/native/cpp/main.cpp @@ -165,7 +165,7 @@ TEST_F(WebServerIntegrationTest, DriverStation) { // Compare results HAL_ControlWord cw; HAL_GetControlWord(&cw); - bool test_value = cw.enabled; + bool test_value = HAL_ControlWord_IsEnabled(cw); EXPECT_EQ(EXPECTED_VALUE, test_value); } diff --git a/simulation/halsim_xrp/build.gradle b/simulation/halsim_xrp/build.gradle index ef64c56e4d..6185c799dd 100644 --- a/simulation/halsim_xrp/build.gradle +++ b/simulation/halsim_xrp/build.gradle @@ -30,6 +30,7 @@ model { project(':ntcore').addNtcoreDependency(it, 'shared') lib project: ':wpinet', library: 'wpinet', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static" } } diff --git a/styleguide/spotbugs-exclude.xml b/styleguide/spotbugs-exclude.xml index f9f803e67b..3cad3f3d8a 100644 --- a/styleguide/spotbugs-exclude.xml +++ b/styleguide/spotbugs-exclude.xml @@ -27,6 +27,10 @@ + + + + diff --git a/wpilibc/robotpy_pybind_build_info.bzl b/wpilibc/robotpy_pybind_build_info.bzl index 1e950fa6a4..ea3f672d70 100644 --- a/wpilibc/robotpy_pybind_build_info.bzl +++ b/wpilibc/robotpy_pybind_build_info.bzl @@ -150,16 +150,6 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ ("wpi::PyNotifier", "wpi__PyNotifier.hpp"), ], ), - struct( - class_name = "DSControlWord", - yml_file = "semiwrap/DSControlWord.yml", - header_root = "$(execpath :robotpy-native-wpilib.copy_headers)", - header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/driverstation/DSControlWord.hpp", - tmpl_class_names = [], - trampolines = [ - ("wpi::DSControlWord", "wpi__DSControlWord.hpp"), - ], - ), struct( class_name = "DriverStation", yml_file = "semiwrap/DriverStation.yml", @@ -261,6 +251,16 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ ("wpi::IterativeRobotBase", "wpi__IterativeRobotBase.hpp"), ], ), + struct( + class_name = "OpModeRobot", + yml_file = "semiwrap/OpModeRobot.yml", + header_root = "$(execpath :robotpy-native-wpilib.copy_headers)", + header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/framework/OpModeRobot.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::OpModeRobotBase", "wpi__OpModeRobotBase.hpp"), + ], + ), struct( class_name = "RobotBase", yml_file = "semiwrap/RobotBase.yml", @@ -271,16 +271,6 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ ("wpi::RobotBase", "wpi__RobotBase.hpp"), ], ), - struct( - class_name = "RobotState", - yml_file = "semiwrap/RobotState.yml", - header_root = "$(execpath :robotpy-native-wpilib.copy_headers)", - header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/framework/RobotState.hpp", - tmpl_class_names = [], - trampolines = [ - ("wpi::RobotState", "wpi__RobotState.hpp"), - ], - ), struct( class_name = "TimedRobot", yml_file = "semiwrap/TimedRobot.yml", @@ -776,6 +766,36 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ ("wpi::internal::DriverStationModeThread", "wpi__internal__DriverStationModeThread.hpp"), ], ), + struct( + class_name = "LinearOpMode", + yml_file = "semiwrap/LinearOpMode.yml", + header_root = "$(execpath :robotpy-native-wpilib.copy_headers)", + header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/opmode/LinearOpMode.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::LinearOpMode", "wpi__LinearOpMode.hpp"), + ], + ), + struct( + class_name = "OpMode", + yml_file = "semiwrap/OpMode.yml", + header_root = "$(execpath :robotpy-native-wpilib.copy_headers)", + header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/opmode/OpMode.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::OpMode", "wpi__OpMode.hpp"), + ], + ), + struct( + class_name = "PeriodicOpMode", + yml_file = "semiwrap/PeriodicOpMode.yml", + header_root = "$(execpath :robotpy-native-wpilib.copy_headers)", + header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/opmode/PeriodicOpMode.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::PeriodicOpMode", "wpi__PeriodicOpMode.hpp"), + ], + ), struct( class_name = "Field2d", yml_file = "semiwrap/Field2d.yml", diff --git a/wpilibc/src/main/native/cpp/driverstation/DSControlWord.cpp b/wpilibc/src/main/native/cpp/driverstation/DSControlWord.cpp deleted file mode 100644 index 35a2de8be4..0000000000 --- a/wpilibc/src/main/native/cpp/driverstation/DSControlWord.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "wpi/driverstation/DSControlWord.hpp" - -#include "wpi/hal/DriverStation.h" - -using namespace wpi; - -DSControlWord::DSControlWord() { - HAL_GetControlWord(&m_controlWord); -} - -bool DSControlWord::IsEnabled() const { - return m_controlWord.enabled && m_controlWord.dsAttached; -} - -bool DSControlWord::IsDisabled() const { - return !(m_controlWord.enabled && m_controlWord.dsAttached); -} - -bool DSControlWord::IsEStopped() const { - return m_controlWord.eStop; -} - -bool DSControlWord::IsAutonomous() const { - return m_controlWord.autonomous; -} - -bool DSControlWord::IsAutonomousEnabled() const { - return m_controlWord.autonomous && m_controlWord.enabled && - m_controlWord.dsAttached; -} - -bool DSControlWord::IsTeleop() const { - return !(m_controlWord.autonomous || m_controlWord.test); -} - -bool DSControlWord::IsTeleopEnabled() const { - return !m_controlWord.autonomous && !m_controlWord.test && - m_controlWord.enabled && m_controlWord.dsAttached; -} - -bool DSControlWord::IsTest() const { - return m_controlWord.test; -} - -bool DSControlWord::IsDSAttached() const { - return m_controlWord.dsAttached; -} - -bool DSControlWord::IsFMSAttached() const { - return m_controlWord.fmsAttached; -} diff --git a/wpilibc/src/main/native/cpp/driverstation/DriverStation.cpp b/wpilibc/src/main/native/cpp/driverstation/DriverStation.cpp index b6684b7723..8df4f8490c 100644 --- a/wpilibc/src/main/native/cpp/driverstation/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/driverstation/DriverStation.cpp @@ -8,10 +8,12 @@ #include #include +#include #include #include #include #include +#include #include @@ -25,12 +27,18 @@ #include "wpi/nt/NetworkTable.hpp" #include "wpi/nt/NetworkTableInstance.hpp" #include "wpi/nt/StringTopic.hpp" +#include "wpi/nt/StructTopic.hpp" #include "wpi/system/Errors.hpp" #include "wpi/system/Timer.hpp" +#include "wpi/util/Color.hpp" +#include "wpi/util/DenseMap.hpp" #include "wpi/util/EventVector.hpp" +#include "wpi/util/StringExtras.hpp" #include "wpi/util/condition_variable.hpp" #include "wpi/util/json.hpp" #include "wpi/util/mutex.hpp" +#include "wpi/util/string.h" +#include "wpi/util/struct/Struct.hpp" #include "wpi/util/timestamp.h" using namespace wpi; @@ -70,6 +78,14 @@ class MatchDataSenderEntry { static constexpr std::string_view kSmartDashboardType = "FMSInfo"; struct MatchDataSender { + MatchDataSender() + : controlWord{table->GetStructTopic("ControlWord") + .Publish()}, + opMode{table->GetStringTopic("OpMode").Publish()} { + controlWord.Set(prevControlWord); + opMode.Set(""); + } + std::shared_ptr table = wpi::nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo"); MatchDataSenderEntry typeMetaData{ @@ -89,8 +105,9 @@ struct MatchDataSender { true}; MatchDataSenderEntry station{table, "StationNumber", 1}; - MatchDataSenderEntry controlWord{table, - "FMSControlData", 0}; + wpi::nt::StructPublisher controlWord; + wpi::nt::StringPublisher opMode; + wpi::hal::ControlWord prevControlWord; }; class JoystickLogSender { @@ -119,11 +136,9 @@ class DataLogSender { private: std::atomic_bool m_initialized{false}; - HAL_ControlWord m_prevControlWord; - wpi::log::BooleanLogEntry m_logEnabled; - wpi::log::BooleanLogEntry m_logAutonomous; - wpi::log::BooleanLogEntry m_logTest; - wpi::log::BooleanLogEntry m_logEstop; + hal::ControlWord m_prevControlWord; + wpi::log::StructLogEntry m_logControlWord; + wpi::log::StringLogEntry m_logOpMode; bool m_logJoysticks; std::array m_joysticks; @@ -146,13 +161,23 @@ struct Instance { bool silenceJoystickWarning = false; - // Robot state status variables - bool userInDisabled = false; - bool userInAutonomous = false; - bool userInTeleop = false; - bool userInTest = false; + // Op mode lookup + wpi::util::mutex opModeMutex; + wpi::util::DenseMap opModes; wpi::units::second_t nextMessageTime = 0_s; + + std::string OpModeToString(int64_t id) { + std::scoped_lock lock{opModeMutex}; + if (id == 0) { + return ""; + } + auto it = opModes.find(id); + if (it != opModes.end()) { + return std::string{wpi::util::to_string_view(&it->second.name)}; + } + return fmt::format("<{}>", id); + } }; } // namespace @@ -577,70 +602,103 @@ bool DriverStation::IsJoystickConnected(int stick) { GetStickPOVsAvailable(stick) != 0; } -bool DriverStation::IsEnabled() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return controlWord.enabled && controlWord.dsAttached; +static int64_t DoAddOpMode(RobotMode mode, std::string_view name, + std::string_view group, std::string_view description, + int32_t textColor, int32_t backgroundColor) { + if (wpi::util::trim(name).empty()) { + return 0; + } + + WPI_String nameWpi = wpi::util::make_string(name); + WPI_String groupWpi = wpi::util::make_string(group); + WPI_String descriptionWpi = wpi::util::make_string(description); + + auto& inst = ::GetInstance(); + std::scoped_lock lock{inst.opModeMutex}; + std::string nameCopy{name}; + for (;;) { + int64_t id = HAL_MakeOpModeId(static_cast(mode), + std::hash{}(nameCopy)); + auto [it, isNew] = inst.opModes.try_emplace( + id, HAL_OpModeOption{id, nameWpi, groupWpi, descriptionWpi, textColor, + backgroundColor}); + if (isNew) { + return id; + } + if (HAL_OpMode_GetRobotMode(it->second.id) == + static_cast(mode) && + wpi::util::to_string_view(&it->second.name) == name) { + return 0; // can't insert duplicate name + } + // collision, try again with space appended + nameCopy += ' '; + } } -bool DriverStation::IsDisabled() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return !(controlWord.enabled && controlWord.dsAttached); +static int32_t ConvertColorToInt(const wpi::util::Color& color) { + return ((static_cast(color.red * 255) & 0xff) << 16) | + ((static_cast(color.green * 255) & 0xff) << 8) | + (static_cast(color.blue * 255) & 0xff); } -bool DriverStation::IsEStopped() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return controlWord.eStop; +int64_t DriverStation::AddOpMode(RobotMode mode, std::string_view name, + std::string_view group, + std::string_view description, + const wpi::util::Color& textColor, + const wpi::util::Color& backgroundColor) { + return DoAddOpMode(mode, name, group, description, + ConvertColorToInt(textColor), + ConvertColorToInt(backgroundColor)); } -bool DriverStation::IsAutonomous() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return controlWord.autonomous; +int64_t DriverStation::AddOpMode(RobotMode mode, std::string_view name, + std::string_view group, + std::string_view description) { + return DoAddOpMode(mode, name, group, description, -1, -1); } -bool DriverStation::IsAutonomousEnabled() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return controlWord.autonomous && controlWord.enabled; +int64_t DriverStation::RemoveOpMode(RobotMode mode, std::string_view name) { + if (wpi::util::trim(name).empty()) { + return 0; + } + + auto& inst = ::GetInstance(); + std::scoped_lock lock{inst.opModeMutex}; + // we have to loop over all entries to find the one with the correct name + // because the of the unique ID generation scheme + for (auto it = inst.opModes.begin(), end = inst.opModes.end(); it != end; + ++it) { + if (HAL_OpMode_GetRobotMode(it->second.id) == + static_cast(mode) && + wpi::util::to_string_view(&it->second.name) == name) { + int64_t id = it->second.id; + inst.opModes.erase(it); + return id; + } + } + return 0; } -bool DriverStation::IsTeleop() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return !(controlWord.autonomous || controlWord.test); +void DriverStation::PublishOpModes() { + auto& inst = ::GetInstance(); + std::scoped_lock lock{inst.opModeMutex}; + std::vector options; + options.reserve(inst.opModes.size()); + for (auto&& [id, option] : inst.opModes) { + options.emplace_back(option); + } + HAL_SetOpModeOptions(options.data(), options.size()); } -bool DriverStation::IsTeleopEnabled() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return !controlWord.autonomous && !controlWord.test && controlWord.enabled; +void DriverStation::ClearOpModes() { + auto& inst = ::GetInstance(); + std::scoped_lock lock{inst.opModeMutex}; + inst.opModes.clear(); + HAL_SetOpModeOptions(nullptr, 0); } -bool DriverStation::IsTest() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return controlWord.test; -} - -bool DriverStation::IsTestEnabled() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return controlWord.test && controlWord.enabled; -} - -bool DriverStation::IsDSAttached() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return controlWord.dsAttached; -} - -bool DriverStation::IsFMSAttached() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - return controlWord.fmsAttached; +std::string DriverStation::GetOpMode() { + return GetInstance().OpModeToString(GetOpModeId()); } std::string DriverStation::GetGameSpecificMessage() { @@ -853,11 +911,16 @@ void SendMatchData() { inst.matchDataSender.replayNumber.Set(tmpDataStore.replayNumber); inst.matchDataSender.matchType.Set(static_cast(tmpDataStore.matchType)); - HAL_ControlWord ctlWord; - HAL_GetControlWord(&ctlWord); - int32_t wordInt = 0; - std::memcpy(&wordInt, &ctlWord, sizeof(wordInt)); - inst.matchDataSender.controlWord.Set(wordInt); + hal::ControlWord ctlWord = hal::GetControlWord(); + if (ctlWord != inst.matchDataSender.prevControlWord) { + int64_t opModeId = ctlWord.GetOpModeId(); + if (opModeId != inst.matchDataSender.prevControlWord.GetOpModeId()) { + inst.matchDataSender.opMode.Set(inst.OpModeToString(opModeId)); + } + + inst.matchDataSender.prevControlWord = ctlWord; + inst.matchDataSender.controlWord.Set(ctlWord); + } } void JoystickLogSender::Init(wpi::log::DataLog& log, unsigned int stick, @@ -939,17 +1002,17 @@ void JoystickLogSender::AppendPOVs(const HAL_JoystickPOVs& povs, void DataLogSender::Init(wpi::log::DataLog& log, bool logJoysticks, int64_t timestamp) { - m_logEnabled = wpi::log::BooleanLogEntry{log, "DS:enabled", timestamp}; - m_logAutonomous = wpi::log::BooleanLogEntry{log, "DS:autonomous", timestamp}; - m_logTest = wpi::log::BooleanLogEntry{log, "DS:test", timestamp}; - m_logEstop = wpi::log::BooleanLogEntry{log, "DS:estop", timestamp}; + m_logControlWord = wpi::log::StructLogEntry{ + log, "DS:controlWord", timestamp}; + m_logOpMode = wpi::log::StringLogEntry{log, "DS:opMode", timestamp}; - // append initial control word values - HAL_GetControlWord(&m_prevControlWord); - m_logEnabled.Append(m_prevControlWord.enabled, timestamp); - m_logAutonomous.Append(m_prevControlWord.autonomous, timestamp); - m_logTest.Append(m_prevControlWord.test, timestamp); - m_logEstop.Append(m_prevControlWord.eStop, timestamp); + // append initial control word value + m_prevControlWord = hal::GetControlWord(); + m_logControlWord.Append(m_prevControlWord); + + // append initial opmode value + auto& inst = GetInstance(); + m_logOpMode.Append(inst.OpModeToString(m_prevControlWord.GetOpModeId())); m_logJoysticks = logJoysticks; if (logJoysticks) { @@ -968,21 +1031,18 @@ void DataLogSender::Send(uint64_t timestamp) { } // append control word value changes - HAL_ControlWord ctlWord; - HAL_GetControlWord(&ctlWord); - if (ctlWord.enabled != m_prevControlWord.enabled) { - m_logEnabled.Append(ctlWord.enabled, timestamp); + hal::ControlWord ctlWord = hal::GetControlWord(); + if (ctlWord != m_prevControlWord) { + // append opmode value changes + int64_t opModeId = ctlWord.GetOpModeId(); + if (opModeId != m_prevControlWord.GetOpModeId()) { + auto& inst = GetInstance(); + m_logOpMode.Append(inst.OpModeToString(opModeId)); + } + + m_prevControlWord = ctlWord; + m_logControlWord.Append(ctlWord); } - if (ctlWord.autonomous != m_prevControlWord.autonomous) { - m_logAutonomous.Append(ctlWord.autonomous, timestamp); - } - if (ctlWord.test != m_prevControlWord.test) { - m_logTest.Append(ctlWord.test, timestamp); - } - if (ctlWord.eStop != m_prevControlWord.eStop) { - m_logEstop.Append(ctlWord.eStop, timestamp); - } - m_prevControlWord = ctlWord; if (m_logJoysticks) { // append joystick value changes diff --git a/wpilibc/src/main/native/cpp/framework/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/framework/IterativeRobotBase.cpp index 483f2ae2cd..36a9ace393 100644 --- a/wpilibc/src/main/native/cpp/framework/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/framework/IterativeRobotBase.cpp @@ -4,9 +4,9 @@ #include "wpi/framework/IterativeRobotBase.hpp" -#include "wpi/driverstation/DSControlWord.hpp" #include "wpi/driverstation/DriverStation.hpp" #include "wpi/hal/DriverStation.h" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/nt/NetworkTableInstance.hpp" #include "wpi/smartdashboard/SmartDashboard.hpp" #include "wpi/system/Errors.hpp" @@ -98,18 +98,10 @@ void IterativeRobotBase::LoopFunc() { DriverStation::RefreshData(); m_watchdog.Reset(); - // Get current mode - DSControlWord word; - Mode mode = Mode::kNone; - if (word.IsDisabled()) { - mode = Mode::kDisabled; - } else if (word.IsAutonomous()) { - mode = Mode::kAutonomous; - } else if (word.IsTeleop()) { - mode = Mode::kTeleop; - } else if (word.IsTest()) { - mode = Mode::kTest; - } + // Get current mode; treat disabled as unknown + wpi::hal::ControlWord word = wpi::hal::GetControlWord(); + bool enabled = word.IsEnabled(); + RobotMode mode = enabled ? word.GetRobotMode() : RobotMode::UNKNOWN; if (!m_calledDsConnected && word.IsDSAttached()) { m_calledDsConnected = true; @@ -117,51 +109,48 @@ void IterativeRobotBase::LoopFunc() { } // If mode changed, call mode exit and entry functions - if (m_lastMode != mode) { + if (m_lastMode != static_cast(mode)) { // Call last mode's exit function - if (m_lastMode == Mode::kDisabled) { + if (m_lastMode == static_cast(RobotMode::UNKNOWN)) { DisabledExit(); - } else if (m_lastMode == Mode::kAutonomous) { + } else if (m_lastMode == static_cast(RobotMode::AUTONOMOUS)) { AutonomousExit(); - } else if (m_lastMode == Mode::kTeleop) { + } else if (m_lastMode == static_cast(RobotMode::TELEOPERATED)) { TeleopExit(); - } else if (m_lastMode == Mode::kTest) { + } else if (m_lastMode == static_cast(RobotMode::TEST)) { TestExit(); } // Call current mode's entry function - if (mode == Mode::kDisabled) { + if (mode == RobotMode::UNKNOWN) { DisabledInit(); m_watchdog.AddEpoch("DisabledInit()"); - } else if (mode == Mode::kAutonomous) { + } else if (mode == RobotMode::AUTONOMOUS) { AutonomousInit(); m_watchdog.AddEpoch("AutonomousInit()"); - } else if (mode == Mode::kTeleop) { + } else if (mode == RobotMode::TELEOPERATED) { TeleopInit(); m_watchdog.AddEpoch("TeleopInit()"); - } else if (mode == Mode::kTest) { + } else if (mode == RobotMode::TEST) { TestInit(); m_watchdog.AddEpoch("TestInit()"); } - m_lastMode = mode; + m_lastMode = static_cast(mode); } // Call the appropriate function depending upon the current robot mode - if (mode == Mode::kDisabled) { - HAL_ObserveUserProgramDisabled(); + HAL_ObserveUserProgram(word.GetValue()); + if (mode == RobotMode::UNKNOWN) { DisabledPeriodic(); m_watchdog.AddEpoch("DisabledPeriodic()"); - } else if (mode == Mode::kAutonomous) { - HAL_ObserveUserProgramAutonomous(); + } else if (mode == RobotMode::AUTONOMOUS) { AutonomousPeriodic(); m_watchdog.AddEpoch("AutonomousPeriodic()"); - } else if (mode == Mode::kTeleop) { - HAL_ObserveUserProgramTeleop(); + } else if (mode == RobotMode::TELEOPERATED) { TeleopPeriodic(); m_watchdog.AddEpoch("TeleopPeriodic()"); - } else if (mode == Mode::kTest) { - HAL_ObserveUserProgramTest(); + } else if (mode == RobotMode::TEST) { TestPeriodic(); m_watchdog.AddEpoch("TestPeriodic()"); } diff --git a/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp new file mode 100644 index 0000000000..791e7a0ca4 --- /dev/null +++ b/wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp @@ -0,0 +1,248 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/framework/OpModeRobot.hpp" + +#include +#include +#include +#include +#include + +#include + +#include "wpi/driverstation/DriverStation.hpp" +#include "wpi/hal/DriverStation.h" +#include "wpi/hal/DriverStationTypes.h" +#include "wpi/hal/HALBase.h" +#include "wpi/hal/Notifier.h" +#include "wpi/opmode/OpMode.hpp" +#include "wpi/util/SafeThread.hpp" +#include "wpi/util/Synchronization.h" + +using namespace wpi; + +namespace { +class MonitorThread : public wpi::util::SafeThreadEvent { + public: + MonitorThread(int64_t modeId, wpi::util::Event& dsEvent, + HAL_NotifierHandle notifier, std::weak_ptr activeOpMode) + : m_modeId{modeId}, + m_dsEvent{dsEvent.GetHandle()}, + m_notifier{notifier}, + m_activeOpMode{std::move(activeOpMode)} {} + + private: + void Main() override { + // Wait for DS to disable or change modes + WPI_EventHandle events[] = {m_dsEvent, m_stopEvent.GetHandle()}; + WPI_Handle signaledBuf[2]; + for (;;) { + auto signaled = wpi::util::WaitForObjects(events, signaledBuf); + if (signaled.empty()) { + return; // handles destroyed + } + for (auto signal : signaled) { + if ((signal & 0x80000000) != 0 || signal == m_stopEvent.GetHandle()) { + return; // handle destroyed or transitioned + } + } + + // did the opmode or enable state change? + HAL_ControlWord word; + HAL_GetUncachedControlWord(&word); + if (!HAL_ControlWord_IsEnabled(word) || + HAL_ControlWord_GetOpModeId(word) != m_modeId) { + break; + } + } + + // call opmode stop + auto opMode = m_activeOpMode.lock(); + if (opMode) { + opMode->OpModeStop(); + } + + events[0] = m_notifier; + int32_t status = 0; + HAL_SetNotifierAlarm(m_notifier, 1000000, 0, false, true, &status); // 1s + auto signaled = wpi::util::WaitForObjects(events, signaledBuf); + if (signaled.empty()) { + return; // handles destroyed + } + for (auto signal : signaled) { + if ((signal & 0x80000000) != 0 || signal == m_stopEvent.GetHandle()) { + return; // handle destroyed or transitioned + } + } + + // if it hasn't transitioned after 1 second, terminate the program + WPILIB_ReportError(err::Error, "OpMode did not exit, terminating program"); + HAL_Shutdown(); + std::exit(0); + } + + int64_t m_modeId; + WPI_EventHandle m_dsEvent; + HAL_NotifierHandle m_notifier; + std::weak_ptr m_activeOpMode; +}; +} // namespace + +void OpModeRobotBase::StartCompetition() { + fmt::print("********** Robot program startup complete **********\n"); + HAL_ObserveUserProgramStarting(); + + wpi::util::Event event; + struct DSListener { + wpi::util::Event& event; + explicit DSListener(wpi::util::Event& event) : event{event} { + HAL_ProvideNewDataEventHandle(event.GetHandle()); + } + ~DSListener() { HAL_RemoveNewDataEventHandle(event.GetHandle()); } + } listener{event}; + + int32_t status = 0; + m_notifier = HAL_CreateNotifier(&status); + HAL_SetNotifierName(m_notifier, "OpModeRobot", &status); + + int64_t lastModeId = -1; + bool calledDriverStationConnected = false; + std::shared_ptr opMode; + WPI_EventHandle events[] = {event.GetHandle(), + static_cast(m_notifier)}; + WPI_Handle signaledBuf[2]; + for (;;) { + // Wait for new data from the driver station, with 50 ms timeout + HAL_SetNotifierAlarm(m_notifier, 50000, 0, false, true, &status); + auto signaled = wpi::util::WaitForObjects(events, signaledBuf); + if (signaled.empty()) { + return; // handles destroyed + } + for (auto signal : signaled) { + if ((signal & 0x80000000) != 0) { + return; // handle destroyed + } + } + + // Get the latest control word and opmode + DriverStation::RefreshData(); + hal::ControlWord ctlWord = DriverStation::GetControlWord(); + + if (!calledDriverStationConnected && ctlWord.IsDSAttached()) { + calledDriverStationConnected = true; + DriverStationConnected(); + } + + int64_t modeId; + if (!ctlWord.IsDSAttached()) { + modeId = 0; + } else { + modeId = ctlWord.GetOpModeId(); + } + + if (!opMode || modeId != lastModeId) { + if (opMode) { + // no or different opmode selected + opMode.reset(); + } + + if (modeId == 0) { + // no opmode selected + NonePeriodic(); + HAL_ObserveUserProgram(ctlWord.GetValue()); + continue; + } + + auto data = m_opModes.lookup(modeId); + if (!data.factory) { + WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId); + ctlWord.SetOpModeId(0); + HAL_ObserveUserProgram(ctlWord.GetValue()); + continue; + } + + // Instantiate the opmode + fmt::print("********** Starting OpMode {} **********\n", data.name); + opMode = data.factory(); + if (!opMode) { + // could not construct + ctlWord.SetOpModeId(0); + HAL_ObserveUserProgram(ctlWord.GetValue()); + continue; + } + { + std::scoped_lock lock(m_opModeMutex); + m_activeOpMode = opMode; + } + lastModeId = modeId; + // Ensure disabledPeriodic is called at least once + opMode->DisabledPeriodic(); + } + + HAL_ObserveUserProgram(ctlWord.GetValue()); + + if (ctlWord.IsEnabled()) { + // When enabled, call the opmode run function, then close and clear + wpi::util::SafeThreadOwner monitor; + monitor.Start(modeId, event, static_cast(m_notifier), + opMode); + opMode->OpModeRun(modeId); + opMode.reset(); + } else { + // When disabled, call the DisabledPeriodic function + opMode->DisabledPeriodic(); + } + } +} + +void OpModeRobotBase::EndCompetition() { + m_notifier = {}; + std::shared_ptr opMode; + { + std::scoped_lock lock(m_opModeMutex); + opMode = m_activeOpMode.lock(); + } + if (opMode) { + opMode->OpModeStop(); + } +} + +void OpModeRobotBase::AddOpModeFactory( + OpModeFactory factory, RobotMode mode, std::string_view name, + std::string_view group, std::string_view description, + const wpi::util::Color& textColor, + const wpi::util::Color& backgroundColor) { + int64_t id = DriverStation::AddOpMode(mode, name, group, description, + textColor, backgroundColor); + if (id != 0) { + m_opModes[id] = OpModeData{std::string{name}, std::move(factory)}; + } +} + +void OpModeRobotBase::AddOpModeFactory(OpModeFactory factory, RobotMode mode, + std::string_view name, + std::string_view group, + std::string_view description) { + int64_t id = DriverStation::AddOpMode(mode, name, group, description); + if (id != 0) { + m_opModes[id] = OpModeData{std::string{name}, std::move(factory)}; + } +} + +void OpModeRobotBase::RemoveOpMode(RobotMode mode, std::string_view name) { + int64_t id = DriverStation::RemoveOpMode(mode, name); + if (id != 0) { + m_opModes.erase(id); + } +} + +void OpModeRobotBase::PublishOpModes() { + DriverStation::PublishOpModes(); +} + +void OpModeRobotBase::ClearOpModes() { + DriverStation::ClearOpModes(); + m_opModes.clear(); +} diff --git a/wpilibc/src/main/native/cpp/framework/RobotState.cpp b/wpilibc/src/main/native/cpp/framework/RobotState.cpp deleted file mode 100644 index 55704a6044..0000000000 --- a/wpilibc/src/main/native/cpp/framework/RobotState.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "wpi/framework/RobotState.hpp" - -#include "wpi/driverstation/DriverStation.hpp" - -using namespace wpi; - -bool RobotState::IsDisabled() { - return DriverStation::IsDisabled(); -} - -bool RobotState::IsEnabled() { - return DriverStation::IsEnabled(); -} - -bool RobotState::IsEStopped() { - return DriverStation::IsEStopped(); -} - -bool RobotState::IsTeleop() { - return DriverStation::IsTeleop(); -} - -bool RobotState::IsAutonomous() { - return DriverStation::IsAutonomous(); -} - -bool RobotState::IsTest() { - return DriverStation::IsTest(); -} diff --git a/wpilibc/src/main/native/cpp/hardware/motor/MotorSafety.cpp b/wpilibc/src/main/native/cpp/hardware/motor/MotorSafety.cpp index 70b5226752..041bdf47ee 100644 --- a/wpilibc/src/main/native/cpp/hardware/motor/MotorSafety.cpp +++ b/wpilibc/src/main/native/cpp/hardware/motor/MotorSafety.cpp @@ -4,11 +4,11 @@ #include "wpi/hardware/motor/MotorSafety.hpp" -#include #include #include "wpi/driverstation/DriverStation.hpp" #include "wpi/hal/DriverStation.h" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/system/Errors.hpp" #include "wpi/util/SafeThread.hpp" #include "wpi/util/SmallPtrSet.hpp" @@ -32,9 +32,9 @@ void Thread::Main() { bool signaled = wpi::util::WaitForObject(event.GetHandle(), 0.1, &timedOut); if (signaled) { HAL_ControlWord controlWord; - std::memset(&controlWord, 0, sizeof(controlWord)); HAL_GetControlWord(&controlWord); - if (!(controlWord.enabled && controlWord.dsAttached)) { + if (!HAL_ControlWord_IsEnabled(controlWord) || + !HAL_ControlWord_IsDSAttached(controlWord)) { safetyCounter = 0; } if (++safetyCounter >= 4) { diff --git a/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp b/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp index 7b776f6a17..b5ce68b45a 100644 --- a/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp +++ b/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp @@ -10,55 +10,30 @@ using namespace wpi::internal; -DriverStationModeThread::DriverStationModeThread() { +DriverStationModeThread::DriverStationModeThread(wpi::hal::ControlWord word) + : m_userControlWord{word.GetValue().value} { + HAL_ProvideNewDataEventHandle(m_event.GetHandle()); m_keepAlive = true; - m_thread = std::thread{[&] { Run(); }}; + m_thread = std::thread{[this] { Run(); }}; } DriverStationModeThread::~DriverStationModeThread() { + HAL_RemoveNewDataEventHandle(m_event.GetHandle()); m_keepAlive = false; + m_event.Set(); if (m_thread.joinable()) { m_thread.join(); } } -void DriverStationModeThread::InDisabled(bool entering) { - m_userInDisabled = entering; -} - -void DriverStationModeThread::InAutonomous(bool entering) { - m_userInAutonomous = entering; -} - -void DriverStationModeThread::InTeleop(bool entering) { - m_userInTeleop = entering; -} - -void DriverStationModeThread::InTest(bool entering) { - m_userInTest = entering; -} - void DriverStationModeThread::Run() { - wpi::util::Event event{false, false}; - HAL_ProvideNewDataEventHandle(event.GetHandle()); - - while (m_keepAlive.load()) { + for (;;) { bool timedOut = false; - wpi::util::WaitForObject(event.GetHandle(), 0.1, &timedOut); + wpi::util::WaitForObject(m_event.GetHandle(), 0.1, &timedOut); + if (!m_keepAlive) { + break; + } wpi::DriverStation::RefreshData(); - if (m_userInDisabled) { - HAL_ObserveUserProgramDisabled(); - } - if (m_userInAutonomous) { - HAL_ObserveUserProgramAutonomous(); - } - if (m_userInTeleop) { - HAL_ObserveUserProgramTeleop(); - } - if (m_userInTest) { - HAL_ObserveUserProgramTest(); - } + HAL_ObserveUserProgram({.value = m_userControlWord}); } - - HAL_RemoveNewDataEventHandle(event.GetHandle()); } diff --git a/wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp b/wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp new file mode 100644 index 0000000000..17f2a083c0 --- /dev/null +++ b/wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp @@ -0,0 +1,21 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/opmode/LinearOpMode.hpp" + +#include "wpi/hal/DriverStation.h" +#include "wpi/internal/DriverStationModeThread.hpp" + +using namespace wpi; + +void LinearOpMode::OpModeRun(int64_t opModeId) { + auto word = wpi::hal::GetControlWord(); + word.SetOpModeId(opModeId); + internal::DriverStationModeThread bgThread{word}; + Run(); +} + +void LinearOpMode::OpModeStop() { + m_running = false; +} diff --git a/wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp b/wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp new file mode 100644 index 0000000000..d77b0f587b --- /dev/null +++ b/wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp @@ -0,0 +1,158 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/opmode/PeriodicOpMode.hpp" + +#include + +#include "wpi/driverstation/DriverStation.hpp" +#include "wpi/hal/DriverStation.h" +#include "wpi/hal/UsageReporting.h" +#include "wpi/nt/NetworkTableInstance.hpp" +#include "wpi/smartdashboard/SmartDashboard.hpp" +#include "wpi/system/Errors.hpp" +#include "wpi/system/RobotController.hpp" + +using namespace wpi; + +PeriodicOpMode::Callback::Callback(std::function func, + std::chrono::microseconds startTime, + std::chrono::microseconds period, + std::chrono::microseconds offset) + : func{std::move(func)}, + period{period}, + expirationTime( + startTime + offset + period + + (std::chrono::microseconds{wpi::RobotController::GetFPGATime()} - + startTime) / + period * period) {} + +PeriodicOpMode::~PeriodicOpMode() { + if (m_notifier != HAL_kInvalidHandle) { + HAL_DestroyNotifier(m_notifier); + } +} + +PeriodicOpMode::PeriodicOpMode(wpi::units::second_t period) + : m_period{period}, + m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) { + m_startTime = std::chrono::microseconds{RobotController::GetFPGATime()}; + AddPeriodic([=, this] { LoopFunc(); }, period); + + int32_t status = 0; + m_notifier = HAL_CreateNotifier(&status); + WPILIB_CheckErrorStatus(status, "CreateNotifier"); + HAL_SetNotifierName(m_notifier, "PeriodicOpMode", &status); + + HAL_ReportUsage("OpMode", "PeriodicOpMode"); +} + +void PeriodicOpMode::AddPeriodic(std::function callback, + wpi::units::second_t period, + wpi::units::second_t offset) { + m_callbacks.emplace( + callback, m_startTime, + std::chrono::microseconds{static_cast(period.value() * 1e6)}, + std::chrono::microseconds{static_cast(offset.value() * 1e6)}); +} + +void PeriodicOpMode::LoopFunc() { + DriverStation::RefreshData(); + HAL_ControlWord word; + HAL_GetControlWord(&word); + HAL_ControlWord_SetOpModeId(&word, m_opModeId); + HAL_ObserveUserProgram(word); + + if (!DriverStation::IsEnabled() || + DriverStation::GetOpModeId() != m_opModeId) { + m_running = false; + return; + } + + m_watchdog.Reset(); + Periodic(); + m_watchdog.AddEpoch("Periodic()"); + + SmartDashboard::UpdateValues(); + m_watchdog.AddEpoch("SmartDashboard::UpdateValues()"); + + // if constexpr (IsSimulation()) { + // HAL_SimPeriodicBefore(); + // SimulationPeriodic(); + // HAL_SimPeriodicAfter(); + // m_watchdog.AddEpoch("SimulationPeriodic()"); + // } + + m_watchdog.Disable(); + + // Flush NetworkTables + nt::NetworkTableInstance::GetDefault().FlushLocal(); + + // Warn on loop time overruns + if (m_watchdog.IsExpired()) { + m_watchdog.PrintEpochs(); + } +} + +void PeriodicOpMode::OpModeRun(int64_t opModeId) { + m_opModeId = opModeId; + + Start(); + + while (m_running) { + // We don't have to check there's an element in the queue first because + // there's always at least one (the constructor adds one). It's reenqueued + // at the end of the loop. + auto callback = m_callbacks.pop(); + + int32_t status = 0; + HAL_SetNotifierAlarm(m_notifier, callback.expirationTime.count(), 0, true, + true, &status); + WPILIB_CheckErrorStatus(status, "SetNotifierAlarm"); + + if (WPI_WaitForObject(m_notifier) == 0) { + break; + } + + m_loopStartTimeUs = RobotController::GetFPGATime(); + std::chrono::microseconds currentTime{m_loopStartTimeUs}; + + callback.func(); + + // Increment the expiration time by the number of full periods it's behind + // plus one to avoid rapid repeat fires from a large loop overrun. We assume + // currentTime ≥ expirationTime rather than checking for it since the + // callback wouldn't be running otherwise. + callback.expirationTime += + callback.period + (currentTime - callback.expirationTime) / + callback.period * callback.period; + m_callbacks.push(std::move(callback)); + + // Process all other callbacks that are ready to run + while (m_callbacks.top().expirationTime <= currentTime) { + callback = m_callbacks.pop(); + + callback.func(); + + callback.expirationTime += + callback.period + (currentTime - callback.expirationTime) / + callback.period * callback.period; + m_callbacks.push(std::move(callback)); + } + } + End(); +} + +void PeriodicOpMode::OpModeStop() { + HAL_DestroyNotifier(m_notifier); + m_notifier = HAL_kInvalidHandle; +} + +void PeriodicOpMode::PrintLoopOverrunMessage() { + WPILIB_ReportWarning("Loop time of {:.6f}s overrun", m_period.value()); +} + +void PeriodicOpMode::PrintWatchdogEpochs() { + m_watchdog.PrintEpochs(); +} diff --git a/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp b/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp index ae99e5e276..7d49790e7c 100644 --- a/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp +++ b/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp @@ -21,6 +21,13 @@ void wpi::sim::ConstBufferCallbackStoreThunk(const char* name, void* param, count); } +void wpi::sim::OpModeOptionsCallbackStoreThunk(const char* name, void* param, + const HAL_OpModeOption* opmodes, + int32_t count) { + reinterpret_cast(param)->opModeOptionsCallback( + name, {opmodes, opmodes + count}); +} + CallbackStore::CallbackStore(int32_t i, NotifyCallback cb, CancelCallbackNoIndexFunc ccf) : index(i), callback(std::move(cb)), cancelType(NoIndex) { @@ -66,6 +73,12 @@ CallbackStore::CallbackStore(int32_t i, int32_t c, int32_t u, this->cccf = ccf; } +CallbackStore::CallbackStore(int32_t u, OpModeOptionsCallback cb, + CancelCallbackNoIndexFunc ccf) + : uid{u}, opModeOptionsCallback{std::move(cb)}, cancelType{NoIndex} { + this->ccnif = ccf; +} + CallbackStore::~CallbackStore() { switch (cancelType) { case Normal: diff --git a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp index 748df13737..639b13677c 100644 --- a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp @@ -31,38 +31,21 @@ void DriverStationSim::SetEnabled(bool enabled) { HALSIM_SetDriverStationEnabled(enabled); } -std::unique_ptr DriverStationSim::RegisterAutonomousCallback( +std::unique_ptr DriverStationSim::RegisterRobotModeCallback( NotifyCallback callback, bool initialNotify) { auto store = std::make_unique( - -1, callback, &HALSIM_CancelDriverStationAutonomousCallback); - store->SetUid(HALSIM_RegisterDriverStationAutonomousCallback( + -1, callback, &HALSIM_CancelDriverStationRobotModeCallback); + store->SetUid(HALSIM_RegisterDriverStationRobotModeCallback( &CallbackStoreThunk, store.get(), initialNotify)); return store; } -bool DriverStationSim::GetAutonomous() { - return HALSIM_GetDriverStationAutonomous(); +HAL_RobotMode DriverStationSim::GetRobotMode() { + return HALSIM_GetDriverStationRobotMode(); } -void DriverStationSim::SetAutonomous(bool autonomous) { - HALSIM_SetDriverStationAutonomous(autonomous); -} - -std::unique_ptr DriverStationSim::RegisterTestCallback( - NotifyCallback callback, bool initialNotify) { - auto store = std::make_unique( - -1, callback, &HALSIM_CancelDriverStationTestCallback); - store->SetUid(HALSIM_RegisterDriverStationTestCallback( - &CallbackStoreThunk, store.get(), initialNotify)); - return store; -} - -bool DriverStationSim::GetTest() { - return HALSIM_GetDriverStationTest(); -} - -void DriverStationSim::SetTest(bool test) { - HALSIM_SetDriverStationTest(test); +void DriverStationSim::SetRobotMode(HAL_RobotMode robotMode) { + HALSIM_SetDriverStationRobotMode(robotMode); } std::unique_ptr DriverStationSim::RegisterEStopCallback( @@ -152,6 +135,38 @@ void DriverStationSim::SetMatchTime(double matchTime) { HALSIM_SetDriverStationMatchTime(matchTime); } +std::unique_ptr DriverStationSim::RegisterOpModeCallback( + NotifyCallback callback, bool initialNotify) { + auto store = std::make_unique( + -1, callback, &HALSIM_CancelDriverStationOpModeCallback); + store->SetUid(HALSIM_RegisterDriverStationOpModeCallback( + &CallbackStoreThunk, store.get(), initialNotify)); + return store; +} + +int64_t DriverStationSim::GetOpMode() { + return HALSIM_GetDriverStationOpMode(); +} + +void DriverStationSim::SetOpMode(int64_t opmode) { + HALSIM_SetDriverStationOpMode(opmode); +} + +std::unique_ptr DriverStationSim::RegisterOpModeOptionsCallback( + OpModeOptionsCallback callback, bool initialNotify) { + auto store = std::make_unique( + -1, callback, &HALSIM_CancelOpModeOptionsCallback); + store->SetUid(HALSIM_RegisterOpModeOptionsCallback( + &OpModeOptionsCallbackStoreThunk, store.get(), initialNotify)); + return store; +} + +OpModeOptions DriverStationSim::GetOpModeOptions() { + int32_t len; + auto options = HALSIM_GetOpModeOptions(&len); + return {options, len}; +} + void DriverStationSim::NotifyNewData() { wpi::util::Event waitEvent{true}; HAL_ProvideNewDataEventHandle(waitEvent.GetHandle()); diff --git a/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp index d4b5b614cf..fa6d46351b 100644 --- a/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp @@ -24,6 +24,14 @@ bool GetProgramStarted() { return HALSIM_GetProgramStarted(); } +void SetProgramState(wpi::hal::ControlWord controlWord) { + wpi::hal::sim::SetProgramState(controlWord); +} + +wpi::hal::ControlWord GetProgramState() { + return wpi::hal::sim::GetProgramState(); +} + void RestartTiming() { HALSIM_RestartTiming(); } diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp index 9ea748caf8..006d63ebd1 100644 --- a/wpilibc/src/main/native/cppcs/RobotBase.cpp +++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp @@ -4,6 +4,8 @@ #include "wpi/framework/RobotBase.hpp" +#include + #ifdef __FRC_SYSTEMCORE__ #include #endif @@ -140,38 +142,46 @@ static void SetupMathShared() { std::make_unique()); } -bool RobotBase::IsEnabled() const { +bool RobotBase::IsEnabled() { return DriverStation::IsEnabled(); } -bool RobotBase::IsDisabled() const { +bool RobotBase::IsDisabled() { return DriverStation::IsDisabled(); } -bool RobotBase::IsAutonomous() const { +bool RobotBase::IsAutonomous() { return DriverStation::IsAutonomous(); } -bool RobotBase::IsAutonomousEnabled() const { +bool RobotBase::IsAutonomousEnabled() { return DriverStation::IsAutonomousEnabled(); } -bool RobotBase::IsTeleop() const { +bool RobotBase::IsTeleop() { return DriverStation::IsTeleop(); } -bool RobotBase::IsTeleopEnabled() const { +bool RobotBase::IsTeleopEnabled() { return DriverStation::IsTeleopEnabled(); } -bool RobotBase::IsTest() const { +bool RobotBase::IsTest() { return DriverStation::IsTest(); } -bool RobotBase::IsTestEnabled() const { +bool RobotBase::IsTestEnabled() { return DriverStation::IsTestEnabled(); } +int64_t RobotBase::GetOpModeId() { + return DriverStation::GetOpModeId(); +} + +std::string RobotBase::GetOpMode() { + return DriverStation::GetOpMode(); +} + std::thread::id RobotBase::GetThreadId() { return m_threadId; } diff --git a/wpilibc/src/main/native/include/wpi/driverstation/DSControlWord.hpp b/wpilibc/src/main/native/include/wpi/driverstation/DSControlWord.hpp deleted file mode 100644 index 4e5b00881f..0000000000 --- a/wpilibc/src/main/native/include/wpi/driverstation/DSControlWord.hpp +++ /dev/null @@ -1,102 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include "wpi/hal/DriverStationTypes.h" - -namespace wpi { - -/** - * A wrapper around Driver Station control word. - */ -class DSControlWord { - public: - /** - * DSControlWord constructor. - * - * Upon construction, the current Driver Station control word is read and - * stored internally. - */ - DSControlWord(); - - /** - * Check if the DS has enabled the robot. - * - * @return True if the robot is enabled and the DS is connected - */ - bool IsEnabled() const; - - /** - * Check if the robot is disabled. - * - * @return True if the robot is explicitly disabled or the DS is not connected - */ - bool IsDisabled() const; - - /** - * Check if the robot is e-stopped. - * - * @return True if the robot is e-stopped - */ - bool IsEStopped() const; - - /** - * Check if the DS is commanding autonomous mode. - * - * @return True if the robot is being commanded to be in autonomous mode - */ - bool IsAutonomous() const; - - /** - * Check if the DS is commanding autonomous mode and if it has enabled the - * robot. - * - * @return True if the robot is being commanded to be in autonomous mode and - * enabled. - */ - bool IsAutonomousEnabled() const; - - /** - * Check if the DS is commanding teleop mode. - * - * @return True if the robot is being commanded to be in teleop mode - */ - bool IsTeleop() const; - - /** - * Check if the DS is commanding teleop mode and if it has enabled the robot. - * - * @return True if the robot is being commanded to be in teleop mode and - * enabled. - */ - bool IsTeleopEnabled() const; - - /** - * Check if the DS is commanding test mode. - * - * @return True if the robot is being commanded to be in test mode - */ - bool IsTest() const; - - /** - * Check if the DS is attached. - * - * @return True if the DS is connected to the robot - */ - bool IsDSAttached() const; - - /** - * Is the driver station attached to a Field Management System? - * - * @return True if the robot is competing on a field being controlled by a - * Field Management System - */ - bool IsFMSAttached() const; - - private: - HAL_ControlWord m_controlWord; -}; - -} // namespace wpi diff --git a/wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp b/wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp index ec9bec08e9..2130b75aeb 100644 --- a/wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp +++ b/wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp @@ -6,7 +6,9 @@ #include #include +#include +#include "wpi/hal/DriverStation.h" #include "wpi/hal/DriverStationTypes.h" #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/units/time.hpp" @@ -16,8 +18,14 @@ namespace wpi::log { class DataLog; } // namespace wpi::log +namespace wpi::util { +class Color; +} // namespace wpi::util + namespace wpi { +using wpi::hal::RobotMode; + /** * Provide access to the network communication data to / from the Driver * Station. @@ -313,28 +321,41 @@ class DriverStation final { * * @return True if the robot is enabled and the DS is connected */ - static bool IsEnabled(); + static bool IsEnabled() { + hal::ControlWord controlWord = GetControlWord(); + return controlWord.IsEnabled() && controlWord.IsDSAttached(); + } /** * Check if the robot is disabled. * * @return True if the robot is explicitly disabled or the DS is not connected */ - static bool IsDisabled(); + static bool IsDisabled() { return !IsEnabled(); } /** * Check if the robot is e-stopped. * * @return True if the robot is e-stopped */ - static bool IsEStopped(); + static bool IsEStopped() { return GetControlWord().IsEStopped(); } + + /** + * Gets the current robot mode. + * + *

Note that this does not indicate whether the robot is enabled or + * disabled. + * + * @return robot mode + */ + static RobotMode GetRobotMode() { return GetControlWord().GetRobotMode(); } /** * Check if the DS is commanding autonomous mode. * * @return True if the robot is being commanded to be in autonomous mode */ - static bool IsAutonomous(); + static bool IsAutonomous() { return GetControlWord().IsAutonomous(); } /** * Check if the DS is commanding autonomous mode and if it has enabled the @@ -343,14 +364,16 @@ class DriverStation final { * @return True if the robot is being commanded to be in autonomous mode and * enabled. */ - static bool IsAutonomousEnabled(); + static bool IsAutonomousEnabled() { + return GetControlWord().IsAutonomousEnabled(); + } /** * Check if the DS is commanding teleop mode. * * @return True if the robot is being commanded to be in teleop mode */ - static bool IsTeleop(); + static bool IsTeleop() { return GetControlWord().IsTeleop(); } /** * Check if the DS is commanding teleop mode and if it has enabled the robot. @@ -358,14 +381,14 @@ class DriverStation final { * @return True if the robot is being commanded to be in teleop mode and * enabled. */ - static bool IsTeleopEnabled(); + static bool IsTeleopEnabled() { return GetControlWord().IsTeleopEnabled(); } /** * Check if the DS is commanding test mode. * * @return True if the robot is being commanded to be in test mode */ - static bool IsTest(); + static bool IsTest() { return GetControlWord().IsTest(); } /** * Check if the DS is commanding Test mode and if it has enabled the robot. @@ -373,14 +396,112 @@ class DriverStation final { * @return True if the robot is being commanded to be in Test mode and * enabled. */ - static bool IsTestEnabled(); + static bool IsTestEnabled() { return GetControlWord().IsTestEnabled(); } + + /** + * Adds an operating mode option. It's necessary to call PublishOpModes() to + * make the added modes visible to the driver station. + * + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + * @param textColor text color + * @param backgroundColor background color + * @return unique ID used to later identify the operating mode; if a blank + * name is passed, 0 is returned; identical names for the same robot + * mode result in a 0 return value + */ + static int64_t AddOpMode(RobotMode mode, std::string_view name, + std::string_view group, std::string_view description, + const wpi::util::Color& textColor, + const wpi::util::Color& backgroundColor); + + /** + * Adds an operating mode option. It's necessary to call PublishOpModes() to + * make the added modes visible to the driver station. + * + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + * @return unique ID used to later identify the operating mode; if a blank + * name is passed, 0 is returned; identical names for the same robot + * mode result in a 0 return value + */ + static int64_t AddOpMode(RobotMode mode, std::string_view name, + std::string_view group = {}, + std::string_view description = {}); + + /** + * Removes an operating mode option. It's necessary to call PublishOpModes() + * to make the removed mode no longer visible to the driver station. + * + * @param mode robot mode + * @param name name of the operating mode + * @return unique ID for the opmode, or 0 if not found + */ + static int64_t RemoveOpMode(RobotMode mode, std::string_view name); + + /** + * Publishes the operating mode options to the driver station. + */ + static void PublishOpModes(); + + /** + * Clears all operating mode options and publishes an empty list to the driver + * station. + */ + static void ClearOpModes(); + + /** + * Gets the operating mode selected on the driver station. Note this does not + * mean the robot is enabled; use IsEnabled() for that. In a match, this will + * indicate the operating mode selected for auto before the match starts + * (i.e., while the robot is disabled in auto mode); after the auto period + * ends, this will change to reflect the operating mode selected for teleop. + * + * @return the unique ID provided by the AddOpMode() function; may return 0 or + * a unique ID not added, so callers should be prepared to handle that case + */ + static int64_t GetOpModeId() { return GetControlWord().GetOpModeId(); } + + /** + * Gets the operating mode selected on the driver station. Note this does not + * mean the robot is enabled; use IsEnabled() for that. In a match, this will + * indicate the operating mode selected for auto before the match starts + * (i.e., while the robot is disabled in auto mode); after the auto period + * ends, this will change to reflect the operating mode selected for teleop. + * + * @return Operating mode string; may return a string not in the list of + * options, so callers should be prepared to handle that case + */ + static std::string GetOpMode(); + + /** + * Check to see if the selected operating mode is a particular value. Note + * this does not mean the robot is enabled; use IsEnabled() for that. + * + * @param id operating mode unique ID + * @return True if that mode is the current mode + */ + static bool IsOpMode(int64_t id) { return GetOpModeId() == id; } + + /** + * Check to see if the selected operating mode is a particular value. Note + * this does not mean the robot is enabled; use IsEnabled() for that. + * + * @param mode operating mode + * @return True if that mode is the current mode + */ + static bool IsOpMode(std::string_view mode) { return GetOpMode() == mode; } /** * Check if the DS is attached. * * @return True if the DS is connected to the robot */ - static bool IsDSAttached(); + static bool IsDSAttached() { return GetControlWord().IsDSAttached(); } /** * Is the driver station attached to a Field Management System? @@ -388,7 +509,7 @@ class DriverStation final { * @return True if the robot is competing on a field being controlled by a * Field Management System */ - static bool IsFMSAttached(); + static bool IsFMSAttached() { return GetControlWord().IsFMSAttached(); } /** * Returns the game specific message provided by the FMS. @@ -482,6 +603,13 @@ class DriverStation final { */ static double GetBatteryVoltage(); + /** + * Get the current control word. + * + * @return control word + */ + static hal::ControlWord GetControlWord() { return hal::GetControlWord(); } + /** * Copy data from the DS task for the user. If no new data exists, it will * just be returned, otherwise the data will be copied from the DS polling diff --git a/wpilibc/src/main/native/include/wpi/framework/IterativeRobotBase.hpp b/wpilibc/src/main/native/include/wpi/framework/IterativeRobotBase.hpp index 726e3b5c54..408885d7ec 100644 --- a/wpilibc/src/main/native/include/wpi/framework/IterativeRobotBase.hpp +++ b/wpilibc/src/main/native/include/wpi/framework/IterativeRobotBase.hpp @@ -226,9 +226,7 @@ class IterativeRobotBase : public RobotBase { void LoopFunc(); private: - enum class Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest }; - - Mode m_lastMode = Mode::kNone; + int m_lastMode = -1; wpi::units::second_t m_period; Watchdog m_watchdog; bool m_ntFlushEnabled = true; diff --git a/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp b/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp new file mode 100644 index 0000000000..a99ddc6021 --- /dev/null +++ b/wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp @@ -0,0 +1,234 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include +#include +#include + +#include "wpi/framework/RobotBase.hpp" +#include "wpi/hal/DriverStationTypes.h" +#include "wpi/hal/Notifier.h" +#include "wpi/opmode/OpMode.hpp" +#include "wpi/util/DenseMap.hpp" +#include "wpi/util/mutex.hpp" + +namespace wpi::util { +class Color; +} // namespace wpi::util + +namespace wpi { + +using RobotMode = wpi::hal::RobotMode; + +namespace detail { +template +concept OpModeDerived = std::derived_from; +template +concept NoArgOpMode = std::constructible_from && OpModeDerived; +template +concept OneArgOpMode = std::constructible_from && OpModeDerived; +} // namespace detail + +/** + * Concept indicating a class is derived from OpMode and has either a + * no-argument constructor or a constructorthat accepts R&. + * + * @tparam T opmode class + * @tparam R robot class + */ +template +concept ConstructibleOpMode = + detail::NoArgOpMode || detail::OneArgOpMode; + +/** + * OpModeRobotBase is the non-templated base class for OpModeRobot. Users should + * generally prefer using OpModeRobot instead of this class. + * + * Opmodes are constructed when selected on the driver station, and destroyed + * when the robot is disabled after being enabled or a different opmode is + * selected. When no opmode is selected, NonePeriodic() is called. The + * DriverStationConnected() function is called the first time the driver station + * connects to the robot. + */ +class OpModeRobotBase : public RobotBase { + public: + using OpModeFactory = std::function()>; + + /** + * Provide an alternate "main loop" via StartCompetition(). + */ + void StartCompetition() override; + + /** + * Ends the main loop in StartCompetition(). + */ + void EndCompetition() override; + + /** + * Constructor. + */ + OpModeRobotBase() = default; + OpModeRobotBase(OpModeRobotBase&&) = delete; + OpModeRobotBase& operator=(OpModeRobotBase&&) = delete; + + /** + * Function called exactly once after the DS is connected. + * + * Code that needs to know the DS state should go here. + * + * Users should override this method for initialization that needs to occur + * after the DS is connected, such as needing the alliance information. + */ + virtual void DriverStationConnected() {} + + /** + * Function called periodically anytime when no opmode is selected, including + * when the Driver Station is disconnected. + */ + virtual void NonePeriodic() {} + + /** + * Adds an operating mode option using a factory function that creates the + * opmode. It's necessary to call PublishOpModes() to make the added modes + * visible to the driver station. + * + * @param factory factory function + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + * @param textColor text color + * @param backgroundColor background color + */ + void AddOpModeFactory(OpModeFactory factory, RobotMode mode, + std::string_view name, std::string_view group, + std::string_view description, + const wpi::util::Color& textColor, + const wpi::util::Color& backgroundColor); + + /** + * Adds an operating mode option using a factory function that creates the + * opmode. It's necessary to call PublishOpModes() to make the added modes + * visible to the driver station. + * + * @param factory factory function + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + */ + void AddOpModeFactory(OpModeFactory factory, RobotMode mode, + std::string_view name, std::string_view group = {}, + std::string_view description = {}); + + /** + * Removes an operating mode option. It's necessary to call PublishOpModes() + * to make the removed mode no longer visible to the driver station. + * + * @param mode robot mode + * @param name name of the operating mode + */ + void RemoveOpMode(RobotMode mode, std::string_view name); + + /** + * Publishes the operating mode options to the driver station. + */ + void PublishOpModes(); + + /** + * Clears all operating mode options and publishes an empty list to the driver + * station. + */ + void ClearOpModes(); + + private: + struct OpModeData { + std::string name; + OpModeFactory factory; + }; + wpi::util::DenseMap m_opModes; + wpi::hal::Handle m_notifier; + wpi::util::mutex m_opModeMutex; + std::weak_ptr m_activeOpMode; +}; + +/** + * OpModeRobot implements the opmode-based robot program framework. + * + * The OpModeRobot class is intended to be subclassed by a user creating a robot + * program. Users must provide their derived class as a template parameter to + * this class. + * + * Opmodes are constructed when selected on the driver station, and destroyed + * when the robot is disabled after being enabled or a different opmode is + * selected. When no opmode is selected, NonePeriodic() is called. The + * DriverStationConnected() function is called the first time the driver station + * connects to the robot. + * + * @tparam Derived derived class + */ +template +class OpModeRobot : public OpModeRobotBase { + public: + /** + * Adds an operating mode option. It's necessary to call PublishOpModes() to + * make the added modes visible to the driver station. + * + * @tparam T opmode class; must be a public, non-abstract subclass of OpMode + * with a public constructor that either takes no arguments or accepts a + * single argument of this class's type (the latter is preferred). + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + * @param textColor text color + * @param backgroundColor background color + */ + template T> + void AddOpMode(RobotMode mode, std::string_view name, std::string_view group, + std::string_view description, + const wpi::util::Color& textColor, + const wpi::util::Color& backgroundColor) { + if constexpr (detail::OneArgOpMode) { + AddOpModeFactory( + [this] { return std::make_unique(*static_cast(this)); }, + mode, name, group, description, textColor, backgroundColor); + } else if constexpr (detail::NoArgOpMode) { + AddOpModeFactory([] { return std::make_unique(); }, mode, name, group, + description, textColor, backgroundColor); + } + } + + /** + * Adds an operating mode option. It's necessary to call PublishOpModes() to + * make the added modes visible to the driver station. + * + * @tparam T opmode class; must be a public, non-abstract subclass of OpMode + * with a public constructor that either takes no arguments or accepts a + * single argument of this class's type (the latter is preferred). + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + */ + template T> + void AddOpMode(RobotMode mode, std::string_view name, + std::string_view group = {}, + std::string_view description = {}) { + if constexpr (detail::OneArgOpMode) { + AddOpModeFactory( + [this] { return std::make_unique(*static_cast(this)); }, + mode, name, group, description); + } else if constexpr (detail::NoArgOpMode) { + AddOpModeFactory([] { return std::make_unique(); }, mode, name, group, + description); + } + } +}; + +} // namespace wpi diff --git a/wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp b/wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp index 6d8b723b2e..f162a2b6b7 100644 --- a/wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp +++ b/wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp @@ -5,6 +5,7 @@ #pragma once #include +#include #include #include "wpi/hal/DriverStation.h" @@ -148,14 +149,14 @@ class RobotBase { * * @return True if the Robot is currently enabled by the Driver Station. */ - bool IsEnabled() const; + static bool IsEnabled(); /** * Determine if the Robot is currently disabled. * * @return True if the Robot is currently disabled by the Driver Station. */ - bool IsDisabled() const; + static bool IsDisabled(); /** * Determine if the robot is currently in Autonomous mode. @@ -163,7 +164,7 @@ class RobotBase { * @return True if the robot is currently operating Autonomously as determined * by the Driver Station. */ - bool IsAutonomous() const; + static bool IsAutonomous(); /** * Determine if the robot is currently in Autonomous mode and enabled. @@ -171,7 +172,7 @@ class RobotBase { * @return True if the robot us currently operating Autonomously while enabled * as determined by the Driver Station. */ - bool IsAutonomousEnabled() const; + static bool IsAutonomousEnabled(); /** * Determine if the robot is currently in Operator Control mode. @@ -179,7 +180,7 @@ class RobotBase { * @return True if the robot is currently operating in Tele-Op mode as * determined by the Driver Station. */ - bool IsTeleop() const; + static bool IsTeleop(); /** * Determine if the robot is current in Operator Control mode and enabled. @@ -187,7 +188,7 @@ class RobotBase { * @return True if the robot is currently operating in Tele-Op mode while * enabled as determined by the Driver Station. */ - bool IsTeleopEnabled() const; + static bool IsTeleopEnabled(); /** * Determine if the robot is currently in Test mode. @@ -195,7 +196,7 @@ class RobotBase { * @return True if the robot is currently running in Test mode as determined * by the Driver Station. */ - bool IsTest() const; + static bool IsTest(); /** * Determine if the robot is current in Test mode and enabled. @@ -203,7 +204,26 @@ class RobotBase { * @return True if the robot is currently operating in Test mode while * enabled as determined by the Driver Station. */ - bool IsTestEnabled() const; + static bool IsTestEnabled(); + + /** + * Gets the currently selected operating mode of the driver station. Note this + * does not mean the robot is enabled; use IsEnabled() for that. + * + * @return the unique ID provided by the DriverStation::AddOpMode() function; + * may return 0 or a unique ID not added, so callers should be prepared to + * handle that case + */ + static int64_t GetOpModeId(); + + /** + * Gets the currently selected operating mode of the driver station. Note this + * does not mean the robot is enabled; use IsEnabled() for that. + * + * @return Operating mode string; may return a string not in the list of + * options, so callers should be prepared to handle that case + */ + static std::string GetOpMode(); /** * Returns the main thread ID. diff --git a/wpilibc/src/main/native/include/wpi/framework/RobotState.hpp b/wpilibc/src/main/native/include/wpi/framework/RobotState.hpp deleted file mode 100644 index a3d4669222..0000000000 --- a/wpilibc/src/main/native/include/wpi/framework/RobotState.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -namespace wpi { - -/** - * Robot state utility functions. - */ -class RobotState { - public: - RobotState() = delete; - - /** - * Returns true if the robot is disabled. - * - * @return True if the robot is disabled. - */ - static bool IsDisabled(); - - /** - * Returns true if the robot is enabled. - * - * @return True if the robot is enabled. - */ - static bool IsEnabled(); - - /** - * Returns true if the robot is E-stopped. - * - * @return True if the robot is E-stopped. - */ - static bool IsEStopped(); - - /** - * Returns true if the robot is in teleop mode. - * - * @return True if the robot is in teleop mode. - */ - static bool IsTeleop(); - - /** - * Returns true if the robot is in autonomous mode. - * - * @return True if the robot is in autonomous mode. - */ - static bool IsAutonomous(); - - /** - * Returns true if the robot is in test mode. - * - * @return True if the robot is in test mode. - */ - static bool IsTest(); -}; - -} // namespace wpi diff --git a/wpilibc/src/main/native/include/wpi/internal/DriverStationModeThread.hpp b/wpilibc/src/main/native/include/wpi/internal/DriverStationModeThread.hpp index 1df5c707b9..f21b92187e 100644 --- a/wpilibc/src/main/native/include/wpi/internal/DriverStationModeThread.hpp +++ b/wpilibc/src/main/native/include/wpi/internal/DriverStationModeThread.hpp @@ -7,6 +7,9 @@ #include #include +#include "wpi/hal/DriverStationTypes.h" +#include "wpi/util/Synchronization.h" + namespace wpi::internal { /** * For internal use only. @@ -15,8 +18,10 @@ class DriverStationModeThread { public: /** * For internal use only. + * + * @param word initial control word */ - DriverStationModeThread(); + explicit DriverStationModeThread(wpi::hal::ControlWord word); ~DriverStationModeThread(); @@ -30,44 +35,17 @@ class DriverStationModeThread { * Only to be used to tell the Driver Station what code you claim to be * executing for diagnostic purposes only. * - * @param entering If true, starting disabled code; if false, leaving disabled - * code + * @param word control word */ - void InDisabled(bool entering); - - /** - * Only to be used to tell the Driver Station what code you claim to be - * executing for diagnostic purposes only. - * - * @param entering If true, starting autonomous code; if false, leaving - * autonomous code - */ - void InAutonomous(bool entering); - - /** - * Only to be used to tell the Driver Station what code you claim to be - * executing for diagnostic purposes only. - * - * @param entering If true, starting teleop code; if false, leaving teleop - * code - */ - void InTeleop(bool entering); - - /** - * Only to be used to tell the Driver Station what code you claim to be - * executing for diagnostic purposes only. - * - * @param entering If true, starting test code; if false, leaving test code - */ - void InTest(bool entering); + void InControl(wpi::hal::ControlWord word) { + m_userControlWord = word.GetValue().value; + } private: std::atomic_bool m_keepAlive{false}; + wpi::util::Event m_event{false, false}; std::thread m_thread; void Run(); - bool m_userInDisabled{false}; - bool m_userInAutonomous{false}; - bool m_userInTeleop{false}; - bool m_userInTest{false}; + std::atomic m_userControlWord; }; } // namespace wpi::internal diff --git a/wpilibc/src/main/native/include/wpi/opmode/LinearOpMode.hpp b/wpilibc/src/main/native/include/wpi/opmode/LinearOpMode.hpp new file mode 100644 index 0000000000..ccd07d38c0 --- /dev/null +++ b/wpilibc/src/main/native/include/wpi/opmode/LinearOpMode.hpp @@ -0,0 +1,68 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include + +#include "wpi/opmode/OpMode.hpp" + +namespace wpi { + +/** + * An opmode structure for "linear" operation. The user is responsible for + * implementing any looping behavior; after Run() returns it will not be called + * again on the same object. + * + * Lifecycle: + * + * - Constructed when opmode selected on driver station + * + * - DisabledPeriodic() called periodically as long as DS is disabled + * + * - When DS transitions from disabled to enabled, Run() is called exactly once + * + * - When DS transitions from enabled to disabled, or a different opmode is + * selected on the driver station, object is destroyed and not reused + * + * The user is responsible for exiting Run() when the opmode is directed to stop + * executing. This is indicated by IsRunning() returning false. All other + * methods should be written to return as quickly as possible when IsRunning() + * returns false. + */ +class LinearOpMode : public OpMode { + public: + /** + * Called periodically while the opmode is selected on the DS and the robot is + * disabled. + */ + void DisabledPeriodic() override {} + + /** + * Called once when the robot is enabled. When it returns, it will not be + * called again on the same object. + */ + virtual void Run() = 0; + + /** + * Returns true while this opmode is selected (regardless of enable state). + * All other functions should be written to return as quickly as possible when + * this returns false. + * + * @return True if opmode selected, false otherwise + */ + bool IsRunning() const { return m_running; } + + // implements OpMode interface + void OpModeRun(int64_t opModeId) final; + + void OpModeStop() final; + + private: + std::atomic_bool m_running{true}; +}; + +} // namespace wpi diff --git a/wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp b/wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp new file mode 100644 index 0000000000..2bac4ff0f8 --- /dev/null +++ b/wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace wpi { + +/** + * Top-level interface for opmode classes. Users should generally extend one of + * the abstract implementations of this interface (e.g. PeriodicOpMode or + * LinearOpMode) rather than directly implementing this interface. + */ +class OpMode { + public: + /** + * The object is destroyed when the opmode is no longer selected on the DS or + * after OpModeRun() returns. + */ + virtual ~OpMode() = default; + + /** + * This function is called periodically while the opmode is selected on the DS + * (robot is disabled). Code that should only run once when the opmode is + * selected should go in the opmode constructor. + */ + virtual void DisabledPeriodic() {} + + /** + * This function is called when the opmode starts (robot is enabled). + * + * @param opModeId opmode unique ID + */ + virtual void OpModeRun(int64_t opModeId) = 0; + + /** + * This function is called asynchronously when the robot is disabled, to + * request the opmode return from OpModeRun(). + */ + virtual void OpModeStop() = 0; +}; + +} // namespace wpi diff --git a/wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp b/wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp new file mode 100644 index 0000000000..46d0513a0e --- /dev/null +++ b/wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp @@ -0,0 +1,178 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include + +#include "wpi/hal/Notifier.h" +#include "wpi/hal/Types.h" +#include "wpi/opmode/OpMode.hpp" +#include "wpi/system/Watchdog.hpp" +#include "wpi/units/time.hpp" +#include "wpi/util/priority_queue.hpp" + +namespace wpi { + +/** + * An opmode structure for periodic operation. This base class implements a loop + * that runs one or more functions periodically (on a set time interval aka loop + * period). The primary periodic callback function is the Periodic() function; + * the time interval for this callback is 20 ms by default, but may be changed + * via passing a different time interval to the constructor. Additional periodic + * callbacks with different intervals can be added using the AddPeriodic() set + * of functions. + * + * Lifecycle: + * + * - Constructed when opmode selected on driver station + * + * - DisabledPeriodic() called periodically as long as DS is disabled. Note + * this is not called on a set time interval (it does not use the same time + * interval as Periodic()) + * + * - When DS transitions from disabled to enabled, Start() is called once + * + * - While DS is enabled, Periodic() is called periodically on the time interval + * set by the constructor, and additional periodic callbacks added via + * AddPeriodic() are called periodically on their set time intervals + * + * - When DS transitions from enabled to disabled, or a different opmode is + * selected on the driver station when the DS is enabled, End() is called, + * followed by the object being destroyed; the object is not reused + * + * - If a different opmode is selected on the driver station when the DS is + * disabled, the object is destroyed (without End() being called); the object + * is not reused + */ +class PeriodicOpMode : public OpMode { + public: + /** Default loop period. */ + static constexpr auto kDefaultPeriod = 20_ms; + + protected: + /** + * Constructor. Periodic opmodes may specify the period used for the + * Periodic() function. + * + * @param period period for callbacks to the Periodic() function + */ + explicit PeriodicOpMode(wpi::units::second_t period = kDefaultPeriod); + + public: + ~PeriodicOpMode() override; + + /** + * Called periodically while the opmode is selected on the DS (robot is + * disabled). + */ + void DisabledPeriodic() override {} + + /** + * Called a single time when the robot transitions from disabled to enabled. + * This is called prior to Periodic() being called. + */ + virtual void Start() {} + + /** Called periodically while the robot is enabled. */ + virtual void Periodic() = 0; + + /** + * Called a single time when the robot transitions from enabled to disabled, + * or just before the destructor is called if a different opmode is selected + * while the robot is enabled. + */ + virtual void End() {} + + /** + * Return the system clock time in microseconds for the start of the current + * periodic loop. This is in the same time base as Timer.getFPGATimestamp(), + * but is stable through a loop. It is updated at the beginning of every + * periodic callback (including the normal periodic loop). + * + * @return Robot running time in microseconds, as of the start of the current + * periodic function. + */ + int64_t GetLoopStartTime() const { return m_loopStartTimeUs; } + + /** + * Add a callback to run at a specific period with a starting time offset. + * + * This is scheduled on the same Notifier as Periodic(), so Periodic() and the + * callback run synchronously. Interactions between them are thread-safe. + * + * @param callback The callback to run. + * @param period The period at which to run the callback. + * @param offset The offset from the common starting time. This is useful + * for scheduling a callback in a different timeslot relative + * to TimedRobot. + */ + void AddPeriodic(std::function callback, wpi::units::second_t period, + wpi::units::second_t offset = 0_s); + + /** + * Gets time period between calls to Periodic() functions. + */ + wpi::units::second_t GetPeriod() const { return m_period; } + + /** + * Prints list of epochs added so far and their times. + */ + void PrintWatchdogEpochs(); + + protected: + /** Loop function. */ + void LoopFunc(); + + public: + // implements OpMode interface + void OpModeRun(int64_t opModeId) final; + + void OpModeStop() final; + + private: + class Callback { + public: + std::function func; + std::chrono::microseconds period; + std::chrono::microseconds expirationTime; + + /** + * Construct a callback container. + * + * @param func The callback to run. + * @param startTime The common starting point for all callback scheduling. + * @param period The period at which to run the callback. + * @param offset The offset from the common starting time. + */ + Callback(std::function func, std::chrono::microseconds startTime, + std::chrono::microseconds period, + std::chrono::microseconds offset); + + bool operator>(const Callback& rhs) const { + return expirationTime > rhs.expirationTime; + } + }; + + int64_t m_opModeId; + bool m_running = true; + + wpi::hal::Handle m_notifier; + std::chrono::microseconds m_startTime; + int64_t m_loopStartTimeUs = 0; + wpi::units::second_t m_period; + Watchdog m_watchdog; + + wpi::util::priority_queue, + std::greater> + m_callbacks; + + void PrintLoopOverrunMessage(); +}; + +} // namespace wpi diff --git a/wpilibc/src/main/native/include/wpi/simulation/CallbackStore.hpp b/wpilibc/src/main/native/include/wpi/simulation/CallbackStore.hpp index 1639c36a17..b701212624 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/CallbackStore.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/CallbackStore.hpp @@ -5,8 +5,10 @@ #pragma once #include +#include #include +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/Value.h" namespace wpi::sim { @@ -14,6 +16,8 @@ namespace wpi::sim { using NotifyCallback = std::function; using ConstBufferCallback = std::function; +using OpModeOptionsCallback = + std::function)>; using CancelCallbackFunc = void (*)(int32_t index, int32_t uid); using CancelCallbackNoIndexFunc = void (*)(int32_t uid); using CancelCallbackChannelFunc = void (*)(int32_t index, int32_t channel, @@ -23,6 +27,9 @@ void CallbackStoreThunk(const char* name, void* param, const HAL_Value* value); void ConstBufferCallbackStoreThunk(const char* name, void* param, const unsigned char* buffer, unsigned int count); +void OpModeOptionsCallbackStoreThunk(const char* name, void* param, + const HAL_OpModeOption* opmodes, + int32_t count); /** * Manages simulation callbacks; each object is associated with a callback. @@ -46,6 +53,9 @@ class CallbackStore { CallbackStore(int32_t i, int32_t c, int32_t u, ConstBufferCallback cb, CancelCallbackChannelFunc ccf); + CallbackStore(int32_t u, OpModeOptionsCallback cb, + CancelCallbackNoIndexFunc ccf); + CallbackStore(const CallbackStore&) = delete; CallbackStore& operator=(const CallbackStore&) = delete; @@ -60,6 +70,10 @@ class CallbackStore { const unsigned char* buffer, unsigned int count); + friend void OpModeOptionsCallbackStoreThunk(const char* name, void* param, + const HAL_OpModeOption* opmodes, + int32_t count); + private: int32_t index; int32_t channel; @@ -67,6 +81,7 @@ class CallbackStore { NotifyCallback callback; ConstBufferCallback constBufferCallback; + OpModeOptionsCallback opModeOptionsCallback; union { CancelCallbackFunc ccf; CancelCallbackChannelFunc cccf; diff --git a/wpilibc/src/main/native/include/wpi/simulation/DriverStationSim.hpp b/wpilibc/src/main/native/include/wpi/simulation/DriverStationSim.hpp index e2b2997146..255c43501f 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/DriverStationSim.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/DriverStationSim.hpp @@ -4,14 +4,46 @@ #pragma once +#include + #include #include "wpi/driverstation/DriverStation.hpp" #include "wpi/hal/DriverStationTypes.h" +#include "wpi/hal/simulation/DriverStationData.h" #include "wpi/simulation/CallbackStore.hpp" namespace wpi::sim { +class OpModeOptions : public std::span { + public: + OpModeOptions() = default; + OpModeOptions(HAL_OpModeOption* options, int32_t len) + : span{options, options + len} {} + OpModeOptions(const OpModeOptions&) = delete; + + OpModeOptions(OpModeOptions&& oth) : span{oth} { + static_cast(oth) = {}; + } + + OpModeOptions& operator=(const OpModeOptions&) = delete; + + OpModeOptions& operator=(OpModeOptions&& oth) { + if (data()) { + HALSIM_FreeOpModeOptionsArray(data(), size()); + } + static_cast(*this) = oth; + static_cast(oth) = {}; + return *this; + } + + ~OpModeOptions() { + if (data()) { + HALSIM_FreeOpModeOptionsArray(data(), size()); + } + } +}; + /** * Class to control a simulated driver station. */ @@ -44,56 +76,29 @@ class DriverStationSim { static void SetEnabled(bool enabled); /** - * Register a callback on whether the DS is in autonomous mode. + * Register a callback on DS robot mode changes. * - * @param callback the callback that will be called on autonomous mode - * entrance/exit + * @param callback the callback that will be called when robot mode changes * @param initialNotify if true, the callback will be run on the initial value * @return the CallbackStore object associated with this callback */ [[nodiscard]] - static std::unique_ptr RegisterAutonomousCallback( + static std::unique_ptr RegisterRobotModeCallback( NotifyCallback callback, bool initialNotify); /** - * Check if the DS is in autonomous. + * Get the robot mode set by the DS. * - * @return true if autonomous + * @return robot mode */ - static bool GetAutonomous(); + static HAL_RobotMode GetRobotMode(); /** - * Change whether the DS is in autonomous. + * Change the robot mode set by the DS. * - * @param autonomous the new value + * @param robotMode the new value */ - static void SetAutonomous(bool autonomous); - - /** - * Register a callback on whether the DS is in test mode. - * - * @param callback the callback that will be called whenever the test mode - * is entered or left - * @param initialNotify if true, the callback will be run on the initial value - * @return the CallbackStore object associated with this callback - */ - [[nodiscard]] - static std::unique_ptr RegisterTestCallback( - NotifyCallback callback, bool initialNotify); - - /** - * Check if the DS is in test. - * - * @return true if test - */ - static bool GetTest(); - - /** - * Change whether the DS is in test. - * - * @param test the new value - */ - static void SetTest(bool test); + static void SetRobotMode(HAL_RobotMode robotMode); /** * Register a callback on the eStop state. @@ -225,6 +230,50 @@ class DriverStationSim { */ static void SetMatchTime(double matchTime); + /** + * Register a callback on DS opmode changes. + * + * @param callback the callback that will be called when opmode changes + * @param initialNotify if true, the callback will be run on the initial value + * @return the CallbackStore object associated with this callback + */ + [[nodiscard]] + static std::unique_ptr RegisterOpModeCallback( + NotifyCallback callback, bool initialNotify); + + /** + * Get the opmode set by the DS. + * + * @return opmode + */ + static int64_t GetOpMode(); + + /** + * Change the opmode set by the DS. + * + * @param opmode the new value + */ + static void SetOpMode(int64_t opmode); + + /** + * Register a callback on opmode options changes. + * + * @param callback the callback that will be called when the list of opmodes + * changes + * @param initialNotify if true, the callback will be run on the initial value + * @return the CallbackStore object associated with this callback. + */ + [[nodiscard]] + static std::unique_ptr RegisterOpModeOptionsCallback( + OpModeOptionsCallback callback, bool initialNotify); + + /** + * Gets the list of opmode options. + * + * @return opmodes list + */ + static OpModeOptions GetOpModeOptions(); + /** * Updates DriverStation data so that new values are visible to the user * program. diff --git a/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp b/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp index 680bd1fbbc..c36e66ff2e 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp @@ -6,6 +6,7 @@ #include +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/HALBase.h" #include "wpi/units/time.hpp" @@ -37,6 +38,20 @@ void SetProgramStarted(bool started); */ bool GetProgramStarted(); +/** + * Sets the user program state (control word). + * + * @param controlWord control word + */ +void SetProgramState(wpi::hal::ControlWord controlWord); + +/** + * Gets the user program state (control word). + * + * @return Control word + */ +wpi::hal::ControlWord GetProgramState(); + /** * Restart the simulator time. */ diff --git a/wpilibc/src/main/python/pyproject.toml b/wpilibc/src/main/python/pyproject.toml index 1e46cb86c6..4d2121048b 100644 --- a/wpilibc/src/main/python/pyproject.toml +++ b/wpilibc/src/main/python/pyproject.toml @@ -94,7 +94,6 @@ MotorControllerGroup = "rpy/MotorControllerGroup.h" Notifier = "rpy/Notifier.h" # wpi/driverstation -DSControlWord = "wpi/driverstation/DSControlWord.hpp" DriverStation = "wpi/driverstation/DriverStation.hpp" Gamepad = "wpi/driverstation/Gamepad.hpp" GenericHID = "wpi/driverstation/GenericHID.hpp" @@ -106,8 +105,8 @@ XboxController = "wpi/driverstation/XboxController.hpp" # wpi/framework IterativeRobotBase = "wpi/framework/IterativeRobotBase.hpp" +OpModeRobot = "wpi/framework/OpModeRobot.hpp" RobotBase = "wpi/framework/RobotBase.hpp" -RobotState = "wpi/framework/RobotState.hpp" TimedRobot = "wpi/framework/TimedRobot.hpp" TimesliceRobot = "wpi/framework/TimesliceRobot.hpp" @@ -182,6 +181,11 @@ Encoder = "wpi/hardware/rotation/Encoder.hpp" # wpi/internal DriverStationModeThread = "wpi/internal/DriverStationModeThread.hpp" +# wpi/opmode +LinearOpMode = "wpi/opmode/LinearOpMode.hpp" +OpMode = "wpi/opmode/OpMode.hpp" +PeriodicOpMode = "wpi/opmode/PeriodicOpMode.hpp" + # wpi/smartdashboard Field2d = "wpi/smartdashboard/Field2d.hpp" FieldObject2d = "wpi/smartdashboard/FieldObject2d.hpp" diff --git a/wpilibc/src/main/python/semiwrap/DSControlWord.yml b/wpilibc/src/main/python/semiwrap/DSControlWord.yml deleted file mode 100644 index 39b5343d53..0000000000 --- a/wpilibc/src/main/python/semiwrap/DSControlWord.yml +++ /dev/null @@ -1,24 +0,0 @@ -classes: - wpi::DSControlWord: - methods: - DSControlWord: - IsEnabled: - no_release_gil: true - IsDisabled: - no_release_gil: true - IsEStopped: - no_release_gil: true - IsAutonomous: - no_release_gil: true - IsAutonomousEnabled: - no_release_gil: true - IsTeleop: - no_release_gil: true - IsTeleopEnabled: - no_release_gil: true - IsTest: - no_release_gil: true - IsDSAttached: - no_release_gil: true - IsFMSAttached: - no_release_gil: true diff --git a/wpilibc/src/main/python/semiwrap/DriverStation.yml b/wpilibc/src/main/python/semiwrap/DriverStation.yml index 74f8f2a13b..0512810df4 100644 --- a/wpilibc/src/main/python/semiwrap/DriverStation.yml +++ b/wpilibc/src/main/python/semiwrap/DriverStation.yml @@ -1,6 +1,6 @@ extra_includes: -- rpy/ControlWord.h - wpi/datalog/DataLog.hpp +- wpi/util/Color.hpp classes: wpi::DriverStation: @@ -57,21 +57,23 @@ classes: GetStickPOVsAvailable: GetStickButtonsMaximumIndex: GetStickButtonsAvailable: + GetRobotMode: + AddOpMode: + overloads: + RobotMode, std::string_view, std::string_view, std::string_view: + RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&: + RemoveOpMode: + PublishOpModes: + ClearOpModes: + GetOpModeId: + GetOpMode: + IsOpMode: + overloads: + int64_t: + std::string_view: + GetControlWord: GetStickTouchpadFinger: GetStickTouchpadFingerAvailable: - inline_code: | - .def("getControlState", - [](DriverStation *self) -> std::tuple { - py::gil_scoped_release release; - return rpy::GetControlState(); - }, - py::doc("More efficient way to determine what state the robot is in.\n" - "\n" - ":returns: booleans representing enabled, isautonomous, istest\n" - "\n" - ".. versionadded:: 2019.2.1\n" - "\n" - ".. note:: This function only exists in RobotPy\n")) wpi::DriverStation::TouchpadFinger: attributes: down: diff --git a/wpilibc/src/main/python/semiwrap/DriverStationModeThread.yml b/wpilibc/src/main/python/semiwrap/DriverStationModeThread.yml index 7ec1c67047..5ce03e7357 100644 --- a/wpilibc/src/main/python/semiwrap/DriverStationModeThread.yml +++ b/wpilibc/src/main/python/semiwrap/DriverStationModeThread.yml @@ -3,7 +3,4 @@ classes: rename: _DriverStationModeThread methods: DriverStationModeThread: - InAutonomous: - InDisabled: - InTeleop: - InTest: + InControl: diff --git a/wpilibc/src/main/python/semiwrap/LinearOpMode.yml b/wpilibc/src/main/python/semiwrap/LinearOpMode.yml new file mode 100644 index 0000000000..657b8caca8 --- /dev/null +++ b/wpilibc/src/main/python/semiwrap/LinearOpMode.yml @@ -0,0 +1,8 @@ +classes: + wpi::LinearOpMode: + methods: + DisabledPeriodic: + Run: + IsRunning: + OpModeRun: + OpModeStop: diff --git a/wpilibc/src/main/python/semiwrap/OpMode.yml b/wpilibc/src/main/python/semiwrap/OpMode.yml new file mode 100644 index 0000000000..6a06592005 --- /dev/null +++ b/wpilibc/src/main/python/semiwrap/OpMode.yml @@ -0,0 +1,6 @@ +classes: + wpi::OpMode: + methods: + DisabledPeriodic: + OpModeRun: + OpModeStop: diff --git a/wpilibc/src/main/python/semiwrap/OpModeRobot.yml b/wpilibc/src/main/python/semiwrap/OpModeRobot.yml new file mode 100644 index 0000000000..3ff5c48da2 --- /dev/null +++ b/wpilibc/src/main/python/semiwrap/OpModeRobot.yml @@ -0,0 +1,26 @@ +extra_includes: +- wpi/util/Color.hpp + +classes: + wpi::OpModeRobotBase: + methods: + StartCompetition: + EndCompetition: + OpModeRobotBase: + DriverStationConnected: + NonePeriodic: + AddOpModeFactory: + overloads: + OpModeFactory, RobotMode, std::string_view, std::string_view, std::string_view: + ? OpModeFactory, RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color& + : + RemoveOpMode: + PublishOpModes: + ClearOpModes: + wpi::OpModeRobot: + ignore: true + methods: + AddOpMode: + overloads: + RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&: + RobotMode, std::string_view, std::string_view, std::string_view: diff --git a/wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml b/wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml new file mode 100644 index 0000000000..e7316f3866 --- /dev/null +++ b/wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml @@ -0,0 +1,17 @@ +classes: + wpi::PeriodicOpMode: + attributes: + kDefaultPeriod: + methods: + DisabledPeriodic: + Start: + Periodic: + End: + GetLoopStartTime: + AddPeriodic: + GetPeriod: + PrintWatchdogEpochs: + OpModeRun: + OpModeStop: + PeriodicOpMode: + LoopFunc: diff --git a/wpilibc/src/main/python/semiwrap/RobotBase.yml b/wpilibc/src/main/python/semiwrap/RobotBase.yml index 7fa1f770ab..a64747063a 100644 --- a/wpilibc/src/main/python/semiwrap/RobotBase.yml +++ b/wpilibc/src/main/python/semiwrap/RobotBase.yml @@ -1,6 +1,5 @@ extra_includes: - wpi/driverstation/DriverStation.hpp -- rpy/ControlWord.h functions: # TODO @@ -36,6 +35,8 @@ classes: IsReal: IsSimulation: RobotBase: + GetOpModeId: + GetOpMode: inline_code: | .def_static("main", [](py::object robot_cls) -> py::object { @@ -43,20 +44,7 @@ classes: auto starter = start.attr("RobotStarter")(); return starter.attr("run")(robot_cls); }, - py::arg("robot_cls"), py::doc("Starting point for the application")) - .def( - "getControlState", - [](RobotBase *self) -> std::tuple { - py::gil_scoped_release release; - return rpy::GetControlState(); - }, - py::doc("More efficient way to determine what state the robot is in.\n" - "\n" - ":returns: booleans representing enabled, isautonomous, istest\n" - "\n" - ".. versionadded:: 2019.2.1\n" - "\n" - ".. note:: This function only exists in RobotPy\n")); + py::arg("robot_cls"), py::doc("Starting point for the application")); auto logger = py::module::import("logging").attr("getLogger")("robot"); cls_RobotBase.attr("logger") = logger; diff --git a/wpilibc/src/main/python/semiwrap/RobotState.yml b/wpilibc/src/main/python/semiwrap/RobotState.yml deleted file mode 100644 index 2ccb2d0e84..0000000000 --- a/wpilibc/src/main/python/semiwrap/RobotState.yml +++ /dev/null @@ -1,10 +0,0 @@ -classes: - wpi::RobotState: - nodelete: true - methods: - IsDisabled: - IsEnabled: - IsEStopped: - IsTeleop: - IsAutonomous: - IsTest: diff --git a/wpilibc/src/main/python/semiwrap/simulation/CallbackStore.yml b/wpilibc/src/main/python/semiwrap/simulation/CallbackStore.yml index eb974c231d..c8f6c1d65c 100644 --- a/wpilibc/src/main/python/semiwrap/simulation/CallbackStore.yml +++ b/wpilibc/src/main/python/semiwrap/simulation/CallbackStore.yml @@ -3,6 +3,8 @@ functions: ignore: true ConstBufferCallbackStoreThunk: ignore: true + OpModeOptionsCallbackStoreThunk: + ignore: true classes: wpi::sim::CallbackStore: force_type_casters: @@ -23,4 +25,6 @@ classes: ignore: true int32_t, int32_t, int32_t, ConstBufferCallback, CancelCallbackChannelFunc: ignore: true + int32_t, OpModeOptionsCallback, CancelCallbackNoIndexFunc: + ignore: true SetUid: diff --git a/wpilibc/src/main/python/semiwrap/simulation/DriverStationSim.yml b/wpilibc/src/main/python/semiwrap/simulation/DriverStationSim.yml index 3dd60b2f4e..27778c29ef 100644 --- a/wpilibc/src/main/python/semiwrap/simulation/DriverStationSim.yml +++ b/wpilibc/src/main/python/semiwrap/simulation/DriverStationSim.yml @@ -1,4 +1,6 @@ classes: + wpi::sim::OpModeOptions: + ignore: true wpi::sim::DriverStationSim: force_type_casters: - std::function @@ -6,12 +8,6 @@ classes: RegisterEnabledCallback: GetEnabled: SetEnabled: - RegisterAutonomousCallback: - GetAutonomous: - SetAutonomous: - RegisterTestCallback: - GetTest: - SetTest: RegisterEStopCallback: GetEStop: SetEStop: @@ -51,3 +47,12 @@ classes: SetJoystickPOVsAvailable: SetJoystickButtonsMaximumIndex: SetJoystickButtonsAvailable: + RegisterRobotModeCallback: + GetRobotMode: + SetRobotMode: + RegisterOpModeCallback: + GetOpMode: + SetOpMode: + RegisterOpModeOptionsCallback: + GetOpModeOptions: + ignore: true diff --git a/wpilibc/src/main/python/semiwrap/simulation/SimHooks.yml b/wpilibc/src/main/python/semiwrap/simulation/SimHooks.yml index 856d4c8ac7..95a4ff2f5d 100644 --- a/wpilibc/src/main/python/semiwrap/simulation/SimHooks.yml +++ b/wpilibc/src/main/python/semiwrap/simulation/SimHooks.yml @@ -3,6 +3,8 @@ functions: WaitForProgramStart: SetProgramStarted: GetProgramStarted: + SetProgramState: + GetProgramState: RestartTiming: PauseTiming: ResumeTiming: diff --git a/wpilibc/src/main/python/wpilib/__init__.py b/wpilibc/src/main/python/wpilib/__init__.py index 35fd9041c2..e5d2d8e525 100644 --- a/wpilibc/src/main/python/wpilib/__init__.py +++ b/wpilibc/src/main/python/wpilib/__init__.py @@ -16,7 +16,6 @@ from ._wpilib import ( CANStatus, Compressor, CompressorConfigType, - DSControlWord, DataLogManager, DigitalInput, DigitalOutput, @@ -37,6 +36,7 @@ from ._wpilib import ( Joystick, Koors40, LEDPattern, + LinearOpMode, Mechanism2d, MechanismLigament2d, MechanismObject2d, @@ -45,6 +45,9 @@ from ._wpilib import ( MotorSafety, Notifier, OnboardIMU, + OpMode, + OpModeRobotBase, + PeriodicOpMode, PS4Controller, PS5Controller, PWM, @@ -63,7 +66,6 @@ from ._wpilib import ( Preferences, RobotBase, RobotController, - RobotState, RuntimeType, SendableBuilderImpl, SendableChooser, @@ -106,7 +108,6 @@ __all__ = [ "CANStatus", "Compressor", "CompressorConfigType", - "DSControlWord", "DataLogManager", "DigitalInput", "DigitalOutput", @@ -127,6 +128,7 @@ __all__ = [ "Joystick", "Koors40", "LEDPattern", + "LinearOpMode", "Mechanism2d", "MechanismLigament2d", "MechanismObject2d", @@ -135,6 +137,9 @@ __all__ = [ "MotorSafety", "Notifier", "OnboardIMU", + "OpMode", + "OpModeRobotBase", + "PeriodicOpMode", "PS4Controller", "PS5Controller", "PWM", @@ -153,7 +158,6 @@ __all__ = [ "Preferences", "RobotBase", "RobotController", - "RobotState", "RuntimeType", "SendableBuilderImpl", "SendableChooser", @@ -191,6 +195,10 @@ __all__ += ["reportError", "reportWarning"] del _init__wpilib +from .opmoderobot import OpModeRobot + +__all__ += ["OpModeRobot"] + from .cameraserver import CameraServer from .deployinfo import getDeployData diff --git a/wpilibc/src/main/python/wpilib/opmoderobot.py b/wpilibc/src/main/python/wpilib/opmoderobot.py new file mode 100644 index 0000000000..9a26f71b24 --- /dev/null +++ b/wpilibc/src/main/python/wpilib/opmoderobot.py @@ -0,0 +1,61 @@ +from hal import RobotMode +from typing import Optional +from wpiutil import Color + +__all__ = ["OpModeRobot"] + +from ._wpilib import OpModeRobotBase, OpMode + +class OpModeRobot(OpModeRobotBase): + """ + OpModeRobot implements the opmode-based robot program framework. + + The OpModeRobot class is intended to be subclassed by a user creating a robot + program. + + Opmodes are constructed when selected on the driver station, and destroyed + when the robot is disabled after being enabled or a different opmode is + selected. When no opmode is selected, nonePeriodic() is called. The + driverStationConnected() function is called the first time the driver station + connects to the robot. + """ + + def __init__(self): + super().__init__() + + def addOpMode(self, + opmodeCls: type, + mode: RobotMode, + name: str, + group: Optional[str] = None, + description: Optional[str] = None, + textColor: Optional[Color] = None, + backgroundColor: Optional[Color] = None) -> None: + """ + Adds an operating mode option. It's necessary to call PublishOpModes() to + make the added modes visible to the driver station. + + The textColor and backgroundColor parameters are optional, but setting + only one has no effect (if only one is provided, it will be ignored). + + :param opmodeCls: opmode class; must be a public, non-abstract subclass of OpMode + with a constructor that either takes no arguments or accepts a + single argument of this class's type (the latter is preferred). + :param mode: robot mode + :param name: name of the operating mode + :param group: group of the operating mode + :param description: description of the operating mode + :param textColor: text color + :param backgroundColor: background color + """ + def makeOpModeInstance() -> OpMode: + # Try to instantiate with robot argument first + try: + return opmodeCls(self) # type: ignore + except TypeError: + # Fallback to no-argument constructor + return opmodeCls() # type: ignore + if textColor is None or backgroundColor is None: + self.addOpModeFactory(makeOpModeInstance, mode, name, group or "", description or "") + else: + self.addOpModeFactory(makeOpModeInstance, mode, name, group or "", description or "", textColor, backgroundColor) diff --git a/wpilibc/src/main/python/wpilib/src/rpy/ControlWord.cpp b/wpilibc/src/main/python/wpilib/src/rpy/ControlWord.cpp deleted file mode 100644 index 08e1fc0809..0000000000 --- a/wpilibc/src/main/python/wpilib/src/rpy/ControlWord.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "rpy/ControlWord.h" - -#include "wpi/hal/DriverStation.h" - -namespace rpy { - -std::tuple GetControlState() { - HAL_ControlWord controlWord; - HAL_GetControlWord(&controlWord); - - bool enable = controlWord.enabled != 0 && controlWord.dsAttached != 0; - bool auton = controlWord.autonomous != 0; - bool test = controlWord.test != 0; - - return std::make_tuple(enable, auton, test); -} - -} // namespace rpy diff --git a/wpilibc/src/main/python/wpilib/src/rpy/ControlWord.h b/wpilibc/src/main/python/wpilib/src/rpy/ControlWord.h deleted file mode 100644 index c708af7cd3..0000000000 --- a/wpilibc/src/main/python/wpilib/src/rpy/ControlWord.h +++ /dev/null @@ -1,7 +0,0 @@ -#include - -namespace rpy { - -std::tuple GetControlState(); - -} // namespace rpy diff --git a/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp b/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp new file mode 100644 index 0000000000..a5c798b0b1 --- /dev/null +++ b/wpilibc/src/test/native/cpp/OpModeRobotTest.cpp @@ -0,0 +1,175 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/framework/OpModeRobot.hpp" + +#include + +#include + +#include "wpi/simulation/DriverStationSim.hpp" +#include "wpi/simulation/SimHooks.hpp" +#include "wpi/util/Color.hpp" + +namespace { +class OpModeRobotTest : public ::testing::Test { + protected: + void SetUp() override { + wpi::sim::PauseTiming(); + wpi::sim::SetProgramStarted(false); + } + + void TearDown() override { wpi::sim::ResumeTiming(); } +}; + +class MockRobot; + +class MockOpMode : public wpi::OpMode { + public: + std::atomic m_disabledPeriodicCount{0}; + std::atomic m_opModeRunCount{0}; + std::atomic m_opModeStopCount{0}; + + MockOpMode() = default; + void DisabledPeriodic() override { m_disabledPeriodicCount++; } + void OpModeRun(int64_t opModeId) override { m_opModeRunCount++; } + void OpModeStop() override { m_opModeStopCount++; } +}; + +class OneArgOpMode : public wpi::OpMode { + public: + explicit OneArgOpMode(MockRobot& robot) {} + void OpModeRun(int64_t opModeId) override {} + void OpModeStop() override {} +}; + +class MockRobot : public wpi::OpModeRobot { + public: + std::atomic m_driverStationConnectedCount{0}; + std::atomic m_nonePeriodicCount{0}; + + MockRobot() = default; + + void DriverStationConnected() override { m_driverStationConnectedCount++; } + + void NonePeriodic() override { m_nonePeriodicCount++; } +}; +} // namespace + +static_assert(wpi::ConstructibleOpMode); +static_assert(wpi::ConstructibleOpMode); + +TEST_F(OpModeRobotTest, AddOpMode) { + struct MyMockRobot : public MockRobot { + MyMockRobot() { + AddOpMode(wpi::RobotMode::AUTONOMOUS, "NoArgOpMode-Auto", + "Group", "Description", wpi::util::Color::kWhite, + wpi::util::Color::kBlack); + AddOpMode(wpi::RobotMode::TEST, "OneArgOpMode-Test", + "Group", "Description", wpi::util::Color::kWhite, + wpi::util::Color::kBlack); + AddOpMode(wpi::RobotMode::TELEOPERATED, "NoArgOpMode"); + AddOpMode(wpi::RobotMode::TELEOPERATED, "OneArgOpMode"); + PublishOpModes(); + } + }; + MyMockRobot robot; + + auto options = wpi::sim::DriverStationSim::GetOpModeOptions(); + ASSERT_EQ(options.size(), 4u); + int indexes[4] = {-1, -1, -1, -1}; + for (size_t i = 0; i < options.size(); ++i) { + auto name = wpi::util::to_string_view(&options[i].name); + if (name == "NoArgOpMode-Auto") { + indexes[0] = i; + } else if (name == "OneArgOpMode-Test") { + indexes[1] = i; + } else if (name == "NoArgOpMode") { + indexes[2] = i; + } else if (name == "OneArgOpMode") { + indexes[3] = i; + } + } + + int i = indexes[0]; + ASSERT_NE(i, -1); + EXPECT_EQ(wpi::util::to_string_view(&options[i].group), "Group"); + EXPECT_EQ(wpi::util::to_string_view(&options[i].description), "Description"); + EXPECT_EQ(options[i].textColor, 0xffffff); + EXPECT_EQ(options[i].backgroundColor, 0x000000); + + i = indexes[1]; + ASSERT_NE(i, -1); + EXPECT_EQ(wpi::util::to_string_view(&options[i].group), "Group"); + EXPECT_EQ(wpi::util::to_string_view(&options[i].description), "Description"); + EXPECT_EQ(options[i].textColor, 0xffffff); + EXPECT_EQ(options[i].backgroundColor, 0x000000); + + i = indexes[2]; + ASSERT_NE(i, -1); + EXPECT_EQ(wpi::util::to_string_view(&options[i].group), ""); + EXPECT_EQ(wpi::util::to_string_view(&options[i].description), ""); + EXPECT_EQ(options[i].textColor, -1); + EXPECT_EQ(options[i].backgroundColor, -1); + + i = indexes[3]; + ASSERT_NE(i, -1); + EXPECT_EQ(wpi::util::to_string_view(&options[i].group), ""); + EXPECT_EQ(wpi::util::to_string_view(&options[i].description), ""); + EXPECT_EQ(options[i].textColor, -1); + EXPECT_EQ(options[i].backgroundColor, -1); +} + +TEST_F(OpModeRobotTest, ClearOpModes) { + struct MyMockRobot : public MockRobot { + MyMockRobot() { + AddOpMode(wpi::RobotMode::TELEOPERATED, "NoArgOpMode"); + AddOpMode(wpi::RobotMode::TELEOPERATED, "OneArgOpMode"); + PublishOpModes(); + } + }; + MyMockRobot robot; + + robot.ClearOpModes(); + auto options = wpi::sim::DriverStationSim::GetOpModeOptions(); + EXPECT_TRUE(options.empty()); +} + +TEST_F(OpModeRobotTest, RemoveOpMode) { + struct MyMockRobot : public MockRobot { + MyMockRobot() { + AddOpMode(wpi::RobotMode::TELEOPERATED, "NoArgOpMode"); + AddOpMode(wpi::RobotMode::TELEOPERATED, "OneArgOpMode"); + PublishOpModes(); + } + }; + MyMockRobot robot; + + robot.RemoveOpMode(wpi::RobotMode::TELEOPERATED, "NoArgOpMode"); + robot.PublishOpModes(); + auto options = wpi::sim::DriverStationSim::GetOpModeOptions(); + ASSERT_EQ(options.size(), 1u); + EXPECT_EQ(wpi::util::to_string_view(&options[0].name), "OneArgOpMode"); +} + +TEST_F(OpModeRobotTest, NonePeriodic) { + struct MyMockRobot : public MockRobot { + MyMockRobot() { + AddOpMode(wpi::RobotMode::TELEOPERATED, "NoArgOpMode"); + AddOpMode(wpi::RobotMode::TELEOPERATED, "OneArgOpMode"); + PublishOpModes(); + } + }; + MyMockRobot robot; + + std::thread robotThread{[&] { robot.StartCompetition(); }}; + wpi::sim::WaitForProgramStart(); + + // Time step to get periodic calls on 50 ms timeout + wpi::sim::StepTiming(110_ms); + EXPECT_EQ(robot.m_nonePeriodicCount.load(), 2u); + + robot.EndCompetition(); + robotThread.join(); +} diff --git a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp index b06c250f1f..94d34c5249 100644 --- a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp +++ b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp @@ -11,6 +11,7 @@ #include +#include "wpi/hal/DriverStationTypes.h" #include "wpi/simulation/DriverStationSim.hpp" #include "wpi/simulation/SimHooks.hpp" @@ -161,8 +162,7 @@ TEST_F(TimedRobotTest, AutonomousMode) { wpi::sim::WaitForProgramStart(); wpi::sim::DriverStationSim::SetEnabled(true); - wpi::sim::DriverStationSim::SetAutonomous(true); - wpi::sim::DriverStationSim::SetTest(false); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS); wpi::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(1u, robot.m_simulationInitCount); @@ -234,8 +234,7 @@ TEST_F(TimedRobotTest, TeleopMode) { wpi::sim::WaitForProgramStart(); wpi::sim::DriverStationSim::SetEnabled(true); - wpi::sim::DriverStationSim::SetAutonomous(false); - wpi::sim::DriverStationSim::SetTest(false); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED); wpi::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(1u, robot.m_simulationInitCount); @@ -306,8 +305,7 @@ TEST_F(TimedRobotTest, TestMode) { wpi::sim::WaitForProgramStart(); wpi::sim::DriverStationSim::SetEnabled(true); - wpi::sim::DriverStationSim::SetAutonomous(false); - wpi::sim::DriverStationSim::SetTest(true); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST); wpi::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(1u, robot.m_simulationInitCount); @@ -369,8 +367,6 @@ TEST_F(TimedRobotTest, TestMode) { EXPECT_EQ(0u, robot.m_testExitCount); wpi::sim::DriverStationSim::SetEnabled(false); - wpi::sim::DriverStationSim::SetAutonomous(false); - wpi::sim::DriverStationSim::SetTest(false); wpi::sim::DriverStationSim::NotifyNewData(); wpi::sim::StepTiming(20_ms); // Wait for Notifiers @@ -404,8 +400,6 @@ TEST_F(TimedRobotTest, ModeChange) { // Start in disabled wpi::sim::DriverStationSim::SetEnabled(false); - wpi::sim::DriverStationSim::SetAutonomous(false); - wpi::sim::DriverStationSim::SetTest(false); wpi::sim::DriverStationSim::NotifyNewData(); EXPECT_EQ(0u, robot.m_disabledInitCount); @@ -432,8 +426,7 @@ TEST_F(TimedRobotTest, ModeChange) { // Transition to autonomous wpi::sim::DriverStationSim::SetEnabled(true); - wpi::sim::DriverStationSim::SetAutonomous(true); - wpi::sim::DriverStationSim::SetTest(false); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS); wpi::sim::DriverStationSim::NotifyNewData(); wpi::sim::StepTiming(kPeriod); @@ -450,8 +443,7 @@ TEST_F(TimedRobotTest, ModeChange) { // Transition to teleop wpi::sim::DriverStationSim::SetEnabled(true); - wpi::sim::DriverStationSim::SetAutonomous(false); - wpi::sim::DriverStationSim::SetTest(false); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED); wpi::sim::DriverStationSim::NotifyNewData(); wpi::sim::StepTiming(kPeriod); @@ -468,8 +460,7 @@ TEST_F(TimedRobotTest, ModeChange) { // Transition to test wpi::sim::DriverStationSim::SetEnabled(true); - wpi::sim::DriverStationSim::SetAutonomous(false); - wpi::sim::DriverStationSim::SetTest(true); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST); wpi::sim::DriverStationSim::NotifyNewData(); wpi::sim::StepTiming(kPeriod); @@ -486,8 +477,6 @@ TEST_F(TimedRobotTest, ModeChange) { // Transition to disabled wpi::sim::DriverStationSim::SetEnabled(false); - wpi::sim::DriverStationSim::SetAutonomous(false); - wpi::sim::DriverStationSim::SetTest(false); wpi::sim::DriverStationSim::NotifyNewData(); wpi::sim::StepTiming(kPeriod); diff --git a/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp index 9351f8d8e3..1544aee294 100644 --- a/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp @@ -12,7 +12,7 @@ #include "callback_helpers/TestCallbackHelpers.hpp" #include "wpi/driverstation/DriverStation.hpp" #include "wpi/driverstation/Joystick.hpp" -#include "wpi/framework/RobotState.hpp" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/simulation/SimHooks.hpp" using namespace wpi; @@ -26,11 +26,11 @@ TEST(DriverStationTest, Enabled) { BooleanCallback callback; auto cb = DriverStationSim::RegisterEnabledCallback(callback.GetCallback(), false); + DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED); DriverStationSim::SetEnabled(true); DriverStationSim::NotifyNewData(); EXPECT_TRUE(DriverStationSim::GetEnabled()); EXPECT_TRUE(DriverStation::IsEnabled()); - EXPECT_TRUE(RobotState::IsEnabled()); EXPECT_TRUE(callback.WasTriggered()); EXPECT_TRUE(callback.GetLastValue()); } @@ -40,16 +40,16 @@ TEST(DriverStationTest, AutonomousMode) { DriverStationSim::ResetData(); EXPECT_FALSE(DriverStation::IsAutonomous()); - BooleanCallback callback; - auto cb = DriverStationSim::RegisterAutonomousCallback(callback.GetCallback(), - false); - DriverStationSim::SetAutonomous(true); + EnumCallback callback; + auto cb = DriverStationSim::RegisterRobotModeCallback(callback.GetCallback(), + false); + DriverStationSim::SetRobotMode(HAL_ROBOTMODE_AUTONOMOUS); DriverStationSim::NotifyNewData(); - EXPECT_TRUE(DriverStationSim::GetAutonomous()); + EXPECT_EQ(DriverStationSim::GetRobotMode(), HAL_ROBOTMODE_AUTONOMOUS); EXPECT_TRUE(DriverStation::IsAutonomous()); - EXPECT_TRUE(RobotState::IsAutonomous()); + EXPECT_EQ(DriverStation::GetRobotMode(), RobotMode::AUTONOMOUS); EXPECT_TRUE(callback.WasTriggered()); - EXPECT_TRUE(callback.GetLastValue()); + EXPECT_EQ(callback.GetLastValue(), HAL_ROBOTMODE_AUTONOMOUS); } TEST(DriverStationTest, Mode) { @@ -57,16 +57,16 @@ TEST(DriverStationTest, Mode) { DriverStationSim::ResetData(); EXPECT_FALSE(DriverStation::IsTest()); - BooleanCallback callback; - auto cb = - DriverStationSim::RegisterTestCallback(callback.GetCallback(), false); - DriverStationSim::SetTest(true); + EnumCallback callback; + auto cb = DriverStationSim::RegisterRobotModeCallback(callback.GetCallback(), + false); + DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TEST); DriverStationSim::NotifyNewData(); - EXPECT_TRUE(DriverStationSim::GetTest()); + EXPECT_EQ(DriverStationSim::GetRobotMode(), HAL_ROBOTMODE_TEST); EXPECT_TRUE(DriverStation::IsTest()); - EXPECT_TRUE(RobotState::IsTest()); + EXPECT_EQ(DriverStation::GetRobotMode(), RobotMode::TEST); EXPECT_TRUE(callback.WasTriggered()); - EXPECT_TRUE(callback.GetLastValue()); + EXPECT_EQ(callback.GetLastValue(), HAL_ROBOTMODE_TEST); } TEST(DriverStationTest, Estop) { @@ -81,7 +81,6 @@ TEST(DriverStationTest, Estop) { DriverStationSim::NotifyNewData(); EXPECT_TRUE(DriverStationSim::GetEStop()); EXPECT_TRUE(DriverStation::IsEStopped()); - EXPECT_TRUE(RobotState::IsEStopped()); EXPECT_TRUE(callback.WasTriggered()); EXPECT_TRUE(callback.GetLastValue()); } diff --git a/wpilibcExamples/example_projects.bzl b/wpilibcExamples/example_projects.bzl index 7b6e9099f3..96e4db6b33 100644 --- a/wpilibcExamples/example_projects.bzl +++ b/wpilibcExamples/example_projects.bzl @@ -86,6 +86,7 @@ SNIPPETS_FOLDERS = [ TEMPLATES_FOLDERS = [ "commandv2", "commandv2skeleton", + "opmode", "robotbaseskeleton", "timed", "timedskeleton", diff --git a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c index 64f732b522..72a4034dbb 100644 --- a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c +++ b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c @@ -15,36 +15,18 @@ that want even more control over what code runs on their robot. #include +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/HAL.h" -enum DriverStationMode { - DisabledMode, - TeleopMode, - TestMode, - AutoMode, -}; - -enum DriverStationMode getDSMode(void) { +HAL_RobotMode getDSMode(void) { // Get Robot State HAL_ControlWord word; HAL_GetControlWord(&word); // We send the observes, otherwise the DS disables - if (!word.enabled) { - HAL_ObserveUserProgramDisabled(); - return DisabledMode; - } else { - if (word.autonomous) { - HAL_ObserveUserProgramAutonomous(); - return AutoMode; - } else if (word.test) { - HAL_ObserveUserProgramTest(); - return TestMode; - } else { - HAL_ObserveUserProgramTeleop(); - return TeleopMode; - } - } + HAL_ObserveUserProgram(word); + return HAL_ControlWord_IsEnabled(word) ? HAL_ControlWord_GetRobotMode(word) + : HAL_ROBOTMODE_UNKNOWN; } int main(void) { @@ -57,6 +39,28 @@ int main(void) { int32_t status = 0; + // Create an opmode per robot mode + static struct HAL_OpModeOption opmodes[] = { + {HAL_MAKE_OPMODEID(HAL_ROBOTMODE_AUTONOMOUS, 0), + {"Auto", 4}, + {"", 0}, + {"", 0}, + -1, + -1}, + {HAL_MAKE_OPMODEID(HAL_ROBOTMODE_TELEOPERATED, 0), + {"Teleop", 6}, + {"", 0}, + {"", 0}, + -1, + -1}, + {HAL_MAKE_OPMODEID(HAL_ROBOTMODE_TEST, 0), + {"Test", 4}, + {"", 0}, + {"", 0}, + -1, + -1}}; + HAL_SetOpModeOptions(opmodes, sizeof(opmodes) / sizeof(opmodes[0])); + // For DS to see valid robot code HAL_ObserveUserProgramStarting(); @@ -95,11 +99,11 @@ int main(void) { HAL_RefreshDSData(); - enum DriverStationMode dsMode = getDSMode(); + HAL_RobotMode dsMode = getDSMode(); switch (dsMode) { - case DisabledMode: + case HAL_ROBOTMODE_UNKNOWN: break; - case TeleopMode: + case HAL_ROBOTMODE_TELEOPERATED: status = 0; if (HAL_GetDIO(dio, &status)) { HAL_SetPWMPulseTimeMicroseconds(pwmPort, 2000, &status); @@ -107,9 +111,9 @@ int main(void) { HAL_SetPWMPulseTimeMicroseconds(pwmPort, 1500, &status); } break; - case AutoMode: + case HAL_ROBOTMODE_AUTONOMOUS: break; - case TestMode: + case HAL_ROBOTMODE_TEST: break; default: break; diff --git a/wpilibcExamples/src/main/cpp/templates/opmode/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/opmode/cpp/Robot.cpp new file mode 100644 index 0000000000..ebe37518a1 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/opmode/cpp/Robot.cpp @@ -0,0 +1,31 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "Robot.hpp" + +#include "opmode/MyAuto.hpp" +#include "opmode/MyTeleop.hpp" + +Robot::Robot() { + // Add opmodes to the robot here. + AddOpMode(wpi::RobotMode::TELEOPERATED, "My Teleop", "", + "An example teleop opmode"); + AddOpMode(wpi::RobotMode::AUTONOMOUS, "My Auto", ""); + PublishOpModes(); +} + +/** This function is called exactly once when the DS first connects. */ +void Robot::DriverStationConnected() {} + +/** + * This function is called periodically anytime when no opmode is selected, + * including when the Driver Station is disconnected. + */ +void Robot::NonePeriodic() {} + +#ifndef RUNNING_WPILIB_TESTS +int main() { + return wpi::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/templates/opmode/cpp/opmode/MyAuto.cpp b/wpilibcExamples/src/main/cpp/templates/opmode/cpp/opmode/MyAuto.cpp new file mode 100644 index 0000000000..746c29ab3d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/opmode/cpp/opmode/MyAuto.cpp @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "opmode/MyAuto.hpp" + +#include "Robot.hpp" + +/** The Robot instance is passed into the opmode via the constructor. */ +MyAuto::MyAuto(Robot& robot) : m_robot{robot} { + /* + * Can call the base class constructor with the period to set a different + * periodic time interval. + * + * Additional periodic methods may be configured with AddPeriodic(). + */ +} + +MyAuto::~MyAuto() { + /* Called when the opmode is de-selected. */ +} + +void MyAuto::Start() { + /* Called once when the robot is first enabled. */ +} + +void MyAuto::Periodic() { + /* Called periodically (set time interval) while the robot is enabled. */ +} + +void MyAuto::End() { + /* Called when the robot is disabled (after previously being enabled). */ +} diff --git a/wpilibcExamples/src/main/cpp/templates/opmode/cpp/opmode/MyTeleop.cpp b/wpilibcExamples/src/main/cpp/templates/opmode/cpp/opmode/MyTeleop.cpp new file mode 100644 index 0000000000..c3fac32fa4 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/opmode/cpp/opmode/MyTeleop.cpp @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "opmode/MyTeleop.hpp" + +#include "Robot.hpp" + +/** The Robot instance is passed into the opmode via the constructor. */ +MyTeleop::MyTeleop(Robot& robot) : m_robot{robot} {} + +MyTeleop::~MyTeleop() { + /* Called when the opmode is de-selected. */ +} + +void MyTeleop::Start() { + /* Called once when the robot is first enabled. */ +} + +void MyTeleop::Periodic() { + /* Called periodically (set time interval) while the robot is enabled. */ +} + +void MyTeleop::End() { + /* Called when the robot is disabled (after previously being enabled). */ +} diff --git a/wpilibcExamples/src/main/cpp/templates/opmode/include/Robot.hpp b/wpilibcExamples/src/main/cpp/templates/opmode/include/Robot.hpp new file mode 100644 index 0000000000..5ebd2fdede --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/opmode/include/Robot.hpp @@ -0,0 +1,14 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/framework/OpModeRobot.hpp" + +class Robot : public wpi::OpModeRobot { + public: + Robot(); + void DriverStationConnected() override; + void NonePeriodic() override; +}; diff --git a/wpilibcExamples/src/main/cpp/templates/opmode/include/opmode/MyAuto.hpp b/wpilibcExamples/src/main/cpp/templates/opmode/include/opmode/MyAuto.hpp new file mode 100644 index 0000000000..30e93ddcf7 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/opmode/include/opmode/MyAuto.hpp @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/opmode/PeriodicOpMode.hpp" + +class Robot; + +class MyAuto : public wpi::PeriodicOpMode { + public: + /** The Robot instance is passed into the opmode via the constructor. */ + explicit MyAuto(Robot& robot); + ~MyAuto() override; + void Start() override; + void Periodic() override; + void End() override; + + private: + [[maybe_unused]] + Robot& m_robot; +}; diff --git a/wpilibcExamples/src/main/cpp/templates/opmode/include/opmode/MyTeleop.hpp b/wpilibcExamples/src/main/cpp/templates/opmode/include/opmode/MyTeleop.hpp new file mode 100644 index 0000000000..be99d1d4e2 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/templates/opmode/include/opmode/MyTeleop.hpp @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/opmode/PeriodicOpMode.hpp" + +class Robot; + +class MyTeleop : public wpi::PeriodicOpMode { + public: + /** The Robot instance is passed into the opmode via the constructor. */ + explicit MyTeleop(Robot& robot); + ~MyTeleop() override; + void Start() override; + void Periodic() override; + void End() override; + + private: + [[maybe_unused]] + Robot& m_robot; +}; diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp index 595a39b59a..10666b9ec8 100644 --- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp @@ -7,7 +7,6 @@ #include "wpi/driverstation/DriverStation.hpp" #include "wpi/hal/DriverStation.h" #include "wpi/internal/DriverStationModeThread.hpp" -#include "wpi/nt/NetworkTable.hpp" Robot::Robot() {} @@ -20,7 +19,13 @@ void Robot::Teleop() {} void Robot::Test() {} void Robot::StartCompetition() { - wpi::internal::DriverStationModeThread modeThread; + wpi::internal::DriverStationModeThread modeThread{wpi::hal::GetControlWord()}; + + // Create an opmode per robot mode + wpi::DriverStation::AddOpMode(wpi::RobotMode::AUTONOMOUS, "Auto"); + wpi::DriverStation::AddOpMode(wpi::RobotMode::TELEOPERATED, "Teleop"); + wpi::DriverStation::AddOpMode(wpi::RobotMode::TEST, "Test"); + wpi::DriverStation::PublishOpModes(); wpi::util::Event event{false, false}; wpi::DriverStation::ProvideRefreshedDataEventHandle(event.GetHandle()); @@ -29,31 +34,24 @@ void Robot::StartCompetition() { HAL_ObserveUserProgramStarting(); while (!m_exit) { + modeThread.InControl(wpi::DriverStation::GetControlWord()); if (IsDisabled()) { - modeThread.InDisabled(true); Disabled(); - modeThread.InDisabled(false); while (IsDisabled()) { wpi::util::WaitForObject(event.GetHandle()); } } else if (IsAutonomous()) { - modeThread.InAutonomous(true); Autonomous(); - modeThread.InAutonomous(false); while (IsAutonomousEnabled()) { wpi::util::WaitForObject(event.GetHandle()); } } else if (IsTest()) { - modeThread.InTest(true); Test(); - modeThread.InTest(false); while (IsTest() && IsEnabled()) { wpi::util::WaitForObject(event.GetHandle()); } } else { - modeThread.InTeleop(true); Teleop(); - modeThread.InTeleop(false); while (IsTeleopEnabled()) { wpi::util::WaitForObject(event.GetHandle()); } diff --git a/wpilibcExamples/src/main/cpp/templates/templates.json b/wpilibcExamples/src/main/cpp/templates/templates.json index 559dd70c52..a0da28236b 100644 --- a/wpilibcExamples/src/main/cpp/templates/templates.json +++ b/wpilibcExamples/src/main/cpp/templates/templates.json @@ -20,6 +20,16 @@ "gradlebase": "cpp", "commandversion": 2 }, + { + "name": "OpMode Robot", + "description": "OpMode style, with explanatory comments and example code.", + "tags": [ + "OpMode" + ], + "foldername": "opmode", + "gradlebase": "cpp", + "commandversion": 2 + }, { "name": "Timed Robot", "description": "Timed style, with explanatory comments and example code.", diff --git a/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp b/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp index f9d8a69481..8f0645a5f0 100644 --- a/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp +++ b/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp @@ -9,6 +9,7 @@ #include "Constants.hpp" #include "Robot.hpp" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/simulation/MockHooks.h" #include "wpi/simulation/DriverStationSim.hpp" #include "wpi/simulation/JoystickSim.hpp" @@ -57,7 +58,7 @@ TEST_P(ArmSimulationTest, Teleop) { // teleop init { - wpi::sim::DriverStationSim::SetAutonomous(false); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED); wpi::sim::DriverStationSim::SetEnabled(true); wpi::sim::DriverStationSim::NotifyNewData(); @@ -130,7 +131,6 @@ TEST_P(ArmSimulationTest, Teleop) { { // Disable - wpi::sim::DriverStationSim::SetAutonomous(false); wpi::sim::DriverStationSim::SetEnabled(false); wpi::sim::DriverStationSim::NotifyNewData(); diff --git a/wpilibcExamples/src/test/cpp/examples/DigitalCommunication/cpp/DigitalCommunicationTest.cpp b/wpilibcExamples/src/test/cpp/examples/DigitalCommunication/cpp/DigitalCommunicationTest.cpp index 4c70132530..432c943b3a 100644 --- a/wpilibcExamples/src/test/cpp/examples/DigitalCommunication/cpp/DigitalCommunicationTest.cpp +++ b/wpilibcExamples/src/test/cpp/examples/DigitalCommunication/cpp/DigitalCommunicationTest.cpp @@ -8,6 +8,7 @@ #include #include "Robot.hpp" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/simulation/DIOSim.hpp" #include "wpi/simulation/DriverStationSim.hpp" #include "wpi/simulation/SimHooks.hpp" @@ -120,7 +121,8 @@ class AutonomousTest : public DigitalCommunicationTest {}; TEST_P(AutonomousTest, Autonomous) { auto autonomous = GetParam(); - wpi::sim::DriverStationSim::SetAutonomous(autonomous); + wpi::sim::DriverStationSim::SetRobotMode( + autonomous ? HAL_ROBOTMODE_AUTONOMOUS : HAL_ROBOTMODE_TELEOPERATED); wpi::sim::DriverStationSim::NotifyNewData(); EXPECT_TRUE(m_autonomousOutput.GetInitialized()); diff --git a/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp b/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp index ae2b08bd3d..7aeab4e80b 100644 --- a/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp +++ b/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp @@ -9,6 +9,7 @@ #include "Constants.hpp" #include "Robot.hpp" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/simulation/MockHooks.h" #include "wpi/simulation/DriverStationSim.hpp" #include "wpi/simulation/JoystickSim.hpp" @@ -51,7 +52,7 @@ class ElevatorSimulationTest : public testing::Test { TEST_F(ElevatorSimulationTest, Teleop) { // teleop init { - wpi::sim::DriverStationSim::SetAutonomous(false); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED); wpi::sim::DriverStationSim::SetEnabled(true); wpi::sim::DriverStationSim::NotifyNewData(); @@ -111,7 +112,6 @@ TEST_F(ElevatorSimulationTest, Teleop) { { // Disable - wpi::sim::DriverStationSim::SetAutonomous(false); wpi::sim::DriverStationSim::SetEnabled(false); wpi::sim::DriverStationSim::NotifyNewData(); diff --git a/wpilibcExamples/src/test/cpp/examples/I2CCommunication/cpp/I2CCommunicationTest.cpp b/wpilibcExamples/src/test/cpp/examples/I2CCommunication/cpp/I2CCommunicationTest.cpp index 8e9b0df455..b03ae5bf3d 100644 --- a/wpilibcExamples/src/test/cpp/examples/I2CCommunication/cpp/I2CCommunicationTest.cpp +++ b/wpilibcExamples/src/test/cpp/examples/I2CCommunication/cpp/I2CCommunicationTest.cpp @@ -8,6 +8,7 @@ #include #include "Robot.hpp" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/simulation/I2CData.h" #include "wpi/simulation/DriverStationSim.hpp" #include "wpi/simulation/SimHooks.hpp" @@ -130,7 +131,8 @@ class AutonomousTest : public I2CCommunicationTest {}; TEST_P(AutonomousTest, Autonomous) { auto autonomous = GetParam(); - wpi::sim::DriverStationSim::SetAutonomous(autonomous); + wpi::sim::DriverStationSim::SetRobotMode( + autonomous ? HAL_ROBOTMODE_AUTONOMOUS : HAL_ROBOTMODE_TELEOPERATED); wpi::sim::DriverStationSim::NotifyNewData(); EXPECT_TRUE(HALSIM_GetI2CInitialized(m_port)); diff --git a/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp index 3fd85ee750..64e0d7b0b6 100644 --- a/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp +++ b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp @@ -8,6 +8,7 @@ #include #include "Robot.hpp" +#include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/simulation/MockHooks.h" #include "wpi/math/system/plant/DCMotor.hpp" #include "wpi/simulation/AnalogInputSim.hpp" @@ -92,7 +93,7 @@ class PotentiometerPIDTest : public testing::Test { TEST_F(PotentiometerPIDTest, Teleop) { // teleop init { - wpi::sim::DriverStationSim::SetAutonomous(false); + wpi::sim::DriverStationSim::SetRobotMode(HAL_ROBOTMODE_TELEOPERATED); wpi::sim::DriverStationSim::SetEnabled(true); wpi::sim::DriverStationSim::NotifyNewData(); diff --git a/wpilibj/src/main/java/org/wpilib/driverstation/DSControlWord.java b/wpilibj/src/main/java/org/wpilib/driverstation/DSControlWord.java deleted file mode 100644 index 0b68001c0f..0000000000 --- a/wpilibj/src/main/java/org/wpilib/driverstation/DSControlWord.java +++ /dev/null @@ -1,126 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.wpilib.driverstation; - -import org.wpilib.hardware.hal.ControlWord; - -/** A wrapper around Driver Station control word. */ -public class DSControlWord { - private final ControlWord m_controlWord = new ControlWord(); - - /** - * DSControlWord constructor. - * - *

Upon construction, the current Driver Station control word is read and stored internally. - */ - public DSControlWord() { - refresh(); - } - - /** Update internal Driver Station control word. */ - public final void refresh() { - DriverStation.refreshControlWordFromCache(m_controlWord); - } - - /** - * Gets a value indicating whether the Driver Station requires the robot to be enabled. - * - * @return True if the robot is enabled, false otherwise. - */ - public boolean isEnabled() { - return m_controlWord.getEnabled() && m_controlWord.getDSAttached(); - } - - /** - * Gets a value indicating whether the Driver Station requires the robot to be disabled. - * - * @return True if the robot should be disabled, false otherwise. - */ - public boolean isDisabled() { - return !isEnabled(); - } - - /** - * Gets a value indicating whether the Robot is e-stopped. - * - * @return True if the robot is e-stopped, false otherwise. - */ - public boolean isEStopped() { - return m_controlWord.getEStop(); - } - - /** - * Gets a value indicating whether the Driver Station requires the robot to be running in - * autonomous mode. - * - * @return True if autonomous mode should be enabled, false otherwise. - */ - public boolean isAutonomous() { - return m_controlWord.getAutonomous(); - } - - /** - * Gets a value indicating whether the Driver Station requires the robot to be running in - * autonomous mode and enabled. - * - * @return True if autonomous should be set and the robot should be enabled. - */ - public boolean isAutonomousEnabled() { - return m_controlWord.getAutonomous() - && m_controlWord.getEnabled() - && m_controlWord.getDSAttached(); - } - - /** - * Gets a value indicating whether the Driver Station requires the robot to be running in - * operator-controlled mode. - * - * @return True if operator-controlled mode should be enabled, false otherwise. - */ - public boolean isTeleop() { - return !(isAutonomous() || isTest()); - } - - /** - * Gets a value indicating whether the Driver Station requires the robot to be running in - * operator-controller mode and enabled. - * - * @return True if operator-controlled mode should be set and the robot should be enabled. - */ - public boolean isTeleopEnabled() { - return !m_controlWord.getAutonomous() - && !m_controlWord.getTest() - && m_controlWord.getEnabled() - && m_controlWord.getDSAttached(); - } - - /** - * Gets a value indicating whether the Driver Station requires the robot to be running in test - * mode. - * - * @return True if test mode should be enabled, false otherwise. - */ - public boolean isTest() { - return m_controlWord.getTest(); - } - - /** - * Gets a value indicating whether the Driver Station is attached. - * - * @return True if Driver Station is attached, false otherwise. - */ - public boolean isDSAttached() { - return m_controlWord.getDSAttached(); - } - - /** - * Gets if the driver station attached to a Field Management System. - * - * @return true if the robot is competing on a field being controlled by a Field Management System - */ - public boolean isFMSAttached() { - return m_controlWord.getFMSAttached(); - } -} diff --git a/wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java b/wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java index 7946210f2b..d48bb7339e 100644 --- a/wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java +++ b/wpilibj/src/main/java/org/wpilib/driverstation/DriverStation.java @@ -4,28 +4,34 @@ package org.wpilib.driverstation; +import java.util.HashMap; import java.util.Map; import java.util.Optional; import java.util.OptionalDouble; import java.util.OptionalInt; import java.util.concurrent.locks.ReentrantLock; import org.wpilib.datalog.BooleanArrayLogEntry; -import org.wpilib.datalog.BooleanLogEntry; import org.wpilib.datalog.DataLog; import org.wpilib.datalog.FloatArrayLogEntry; import org.wpilib.datalog.IntegerArrayLogEntry; +import org.wpilib.datalog.StringLogEntry; +import org.wpilib.datalog.StructLogEntry; import org.wpilib.hardware.hal.AllianceStationID; import org.wpilib.hardware.hal.ControlWord; import org.wpilib.hardware.hal.DriverStationJNI; import org.wpilib.hardware.hal.HAL; import org.wpilib.hardware.hal.MatchInfoData; +import org.wpilib.hardware.hal.OpModeOption; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.math.geometry.Rotation2d; import org.wpilib.networktables.BooleanPublisher; import org.wpilib.networktables.IntegerPublisher; import org.wpilib.networktables.NetworkTableInstance; import org.wpilib.networktables.StringPublisher; import org.wpilib.networktables.StringTopic; +import org.wpilib.networktables.StructPublisher; import org.wpilib.system.Timer; +import org.wpilib.util.Color; import org.wpilib.util.WPIUtilJNI; import org.wpilib.util.concurrent.EventVector; @@ -242,6 +248,22 @@ public final class DriverStation { private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0; private static double m_nextMessageTime; + private static String opModeToString(long id) { + if (id == 0) { + return ""; + } + m_opModesMutex.lock(); + try { + OpModeOption option = m_opModes.get(id); + if (option != null) { + return option.name; + } + } finally { + m_opModesMutex.unlock(); + } + return "<" + id + ">"; + } + @SuppressWarnings("MemberName") private static class MatchDataSender { private static final String kSmartDashboardType = "FMSInfo"; @@ -253,7 +275,8 @@ public final class DriverStation { final IntegerPublisher matchType; final BooleanPublisher alliance; final IntegerPublisher station; - final IntegerPublisher controlWord; + final StructPublisher controlWord; + final StringPublisher opMode; boolean oldIsRedAlliance = true; int oldStationNumber = 1; String oldEventName = ""; @@ -261,7 +284,8 @@ public final class DriverStation { int oldMatchNumber; int oldReplayNumber; int oldMatchType; - int oldControlWord; + final ControlWord oldControlWord = new ControlWord(); + final ControlWord currentControlWord = new ControlWord(); MatchDataSender() { var table = NetworkTableInstance.getDefault().getTable("FMSInfo"); @@ -284,10 +308,13 @@ public final class DriverStation { alliance.set(true); station = table.getIntegerTopic("StationNumber").publish(); station.set(1); - controlWord = table.getIntegerTopic("FMSControlData").publish(); - controlWord.set(0); + controlWord = table.getStructTopic("ControlWord", ControlWord.struct).publish(); + controlWord.set(oldControlWord); + opMode = table.getStringTopic("OpMode").publish(); + opMode.set(""); } + @SuppressWarnings("VariableDeclarationUsageDistance") private void sendMatchData() { AllianceStationID allianceID = DriverStationJNI.getAllianceStation(); final int stationNumber = @@ -307,7 +334,6 @@ public final class DriverStation { int currentMatchNumber; int currentReplayNumber; int currentMatchType; - int currentControlWord; m_cacheDataMutex.lock(); try { currentEventName = DriverStation.m_matchInfo.eventName; @@ -318,7 +344,7 @@ public final class DriverStation { } finally { m_cacheDataMutex.unlock(); } - currentControlWord = DriverStationJNI.nativeGetControlWord(); + DriverStationJNI.getControlWord(currentControlWord); if (oldIsRedAlliance != isRedAlliance) { alliance.set(isRedAlliance); @@ -348,9 +374,13 @@ public final class DriverStation { matchType.set(currentMatchType); oldMatchType = currentMatchType; } - if (currentControlWord != oldControlWord) { + if (!currentControlWord.equals(oldControlWord)) { + long currentOpModeId = currentControlWord.getOpModeId(); + if (currentOpModeId != oldControlWord.getOpModeId()) { + opMode.set(opModeToString(currentOpModeId)); + } controlWord.set(currentControlWord); - oldControlWord = currentControlWord; + oldControlWord.update(currentControlWord); } } } @@ -462,21 +492,16 @@ public final class DriverStation { private static class DataLogSender { DataLogSender(DataLog log, boolean logJoysticks, long timestamp) { - m_logEnabled = new BooleanLogEntry(log, "DS:enabled", timestamp); - m_logAutonomous = new BooleanLogEntry(log, "DS:autonomous", timestamp); - m_logTest = new BooleanLogEntry(log, "DS:test", timestamp); - m_logEstop = new BooleanLogEntry(log, "DS:estop", timestamp); + m_logControlWord = + StructLogEntry.create(log, "DS:controlWord", ControlWord.struct, timestamp); - // append initial control word values - m_wasEnabled = m_controlWordCache.getEnabled(); - m_wasAutonomous = m_controlWordCache.getAutonomous(); - m_wasTest = m_controlWordCache.getTest(); - m_wasEstop = m_controlWordCache.getEStop(); + // append initial control word value + m_logControlWord.append(m_controlWordCache, timestamp); + m_oldControlWord.update(m_controlWordCache); - m_logEnabled.append(m_wasEnabled, timestamp); - m_logAutonomous.append(m_wasAutonomous, timestamp); - m_logTest.append(m_wasTest, timestamp); - m_logEstop.append(m_wasEstop, timestamp); + // append initial opmode value + m_logOpMode = new StringLogEntry(log, "DS:opMode", timestamp); + m_logOpMode.append(m_opModeCache, timestamp); if (logJoysticks) { m_joysticks = new JoystickLogSender[kJoystickPorts]; @@ -490,29 +515,16 @@ public final class DriverStation { public void send(long timestamp) { // append control word value changes - boolean enabled = m_controlWordCache.getEnabled(); - if (enabled != m_wasEnabled) { - m_logEnabled.append(enabled, timestamp); - } - m_wasEnabled = enabled; + if (!m_controlWordCache.equals(m_oldControlWord)) { + // append opmode value changes + long opModeId = m_controlWordCache.getOpModeId(); + if (opModeId != m_oldControlWord.getOpModeId()) { + m_logOpMode.append(m_opModeCache, timestamp); + } - boolean autonomous = m_controlWordCache.getAutonomous(); - if (autonomous != m_wasAutonomous) { - m_logAutonomous.append(autonomous, timestamp); + m_logControlWord.append(m_controlWordCache, timestamp); + m_oldControlWord.update(m_controlWordCache); } - m_wasAutonomous = autonomous; - - boolean test = m_controlWordCache.getTest(); - if (test != m_wasTest) { - m_logTest.append(test, timestamp); - } - m_wasTest = test; - - boolean estop = m_controlWordCache.getEStop(); - if (estop != m_wasEstop) { - m_logEstop.append(estop, timestamp); - } - m_wasEstop = estop; // append joystick value changes for (JoystickLogSender joystick : m_joysticks) { @@ -520,14 +532,9 @@ public final class DriverStation { } } - boolean m_wasEnabled; - boolean m_wasAutonomous; - boolean m_wasTest; - boolean m_wasEstop; - final BooleanLogEntry m_logEnabled; - final BooleanLogEntry m_logAutonomous; - final BooleanLogEntry m_logTest; - final BooleanLogEntry m_logEstop; + final ControlWord m_oldControlWord = new ControlWord(); + final StructLogEntry m_logControlWord; + final StringLogEntry m_logOpMode; final JoystickLogSender[] m_joysticks; } @@ -541,6 +548,7 @@ public final class DriverStation { new HALJoystickTouchpads[kJoystickPorts]; private static MatchInfoData m_matchInfo = new MatchInfoData(); private static ControlWord m_controlWord = new ControlWord(); + private static String m_opMode = ""; private static EventVector m_refreshEvents = new EventVector(); // Joystick Cached Data @@ -555,6 +563,8 @@ public final class DriverStation { private static MatchInfoData m_matchInfoCache = new MatchInfoData(); private static ControlWord m_controlWordCache = new ControlWord(); + private static String m_opModeCache = ""; + // Joystick button rising/falling edge flags private static long[] m_joystickButtonsPressed = new long[kJoystickPorts]; private static long[] m_joystickButtonsReleased = new long[kJoystickPorts]; @@ -566,6 +576,9 @@ public final class DriverStation { private static boolean m_silenceJoystickWarning; + private static final Map m_opModes = new HashMap<>(); + private static final ReentrantLock m_opModesMutex = new ReentrantLock(); + /** * DriverStation constructor. * @@ -1185,7 +1198,7 @@ public final class DriverStation { public static boolean isEnabled() { m_cacheDataMutex.lock(); try { - return m_controlWord.getEnabled() && m_controlWord.getDSAttached(); + return m_controlWord.isEnabled() && m_controlWord.isDSAttached(); } finally { m_cacheDataMutex.unlock(); } @@ -1208,7 +1221,23 @@ public final class DriverStation { public static boolean isEStopped() { m_cacheDataMutex.lock(); try { - return m_controlWord.getEStop(); + return m_controlWord.isEStopped(); + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Gets the current robot mode. + * + *

Note that this does not indicate whether the robot is enabled or disabled. + * + * @return robot mode + */ + public static RobotMode getRobotMode() { + m_cacheDataMutex.lock(); + try { + return m_controlWord.getRobotMode(); } finally { m_cacheDataMutex.unlock(); } @@ -1223,7 +1252,7 @@ public final class DriverStation { public static boolean isAutonomous() { m_cacheDataMutex.lock(); try { - return m_controlWord.getAutonomous(); + return m_controlWord.isAutonomous(); } finally { m_cacheDataMutex.unlock(); } @@ -1238,7 +1267,7 @@ public final class DriverStation { public static boolean isAutonomousEnabled() { m_cacheDataMutex.lock(); try { - return m_controlWord.getAutonomous() && m_controlWord.getEnabled(); + return m_controlWord.isAutonomousEnabled(); } finally { m_cacheDataMutex.unlock(); } @@ -1251,7 +1280,7 @@ public final class DriverStation { * @return True if operator-controlled mode should be enabled, false otherwise. */ public static boolean isTeleop() { - return !(isAutonomous() || isTest()); + return m_controlWord.isTeleop(); } /** @@ -1263,9 +1292,7 @@ public final class DriverStation { public static boolean isTeleopEnabled() { m_cacheDataMutex.lock(); try { - return !m_controlWord.getAutonomous() - && !m_controlWord.getTest() - && m_controlWord.getEnabled(); + return m_controlWord.isTeleopEnabled(); } finally { m_cacheDataMutex.unlock(); } @@ -1280,7 +1307,7 @@ public final class DriverStation { public static boolean isTest() { m_cacheDataMutex.lock(); try { - return m_controlWord.getTest(); + return m_controlWord.isTest(); } finally { m_cacheDataMutex.unlock(); } @@ -1295,12 +1322,230 @@ public final class DriverStation { public static boolean isTestEnabled() { m_cacheDataMutex.lock(); try { - return m_controlWord.getTest() && m_controlWord.getEnabled(); + return m_controlWord.isTestEnabled(); } finally { m_cacheDataMutex.unlock(); } } + private static int convertColorToInt(Color color) { + if (color == null) { + return -1; + } + return (((int) (color.red * 255) & 0xff) << 16) + | (((int) (color.green * 255) & 0xff) << 8) + | ((int) (color.blue * 255) & 0xff); + } + + /** + * Adds an operating mode option. It's necessary to call publishOpModes() to make the added modes + * visible to the driver station. + * + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + * @param textColor text color, or null for default + * @param backgroundColor background color, or null for default + * @return unique ID used to later identify the operating mode + * @throws IllegalArgumentException if name is empty or an operating mode with the same robot mode + * and name already exists + */ + @SuppressWarnings("PMD.UseStringBufferForStringAppends") + public static long addOpMode( + RobotMode mode, + String name, + String group, + String description, + Color textColor, + Color backgroundColor) { + if (name.isBlank()) { + throw new IllegalArgumentException("OpMode name must be non-blank"); + } + // find unique ID + m_opModesMutex.lock(); + try { + String nameCopy = name; + for (; ; ) { + long id = OpModeOption.makeId(mode, nameCopy.hashCode()); + OpModeOption existing = m_opModes.get(id); + if (existing == null) { + m_opModes.put( + id, + new OpModeOption( + id, + name, + group, + description, + convertColorToInt(textColor), + convertColorToInt(backgroundColor))); + return id; + } + if (existing.getMode() == mode && existing.name.equals(name)) { + // already exists + throw new IllegalArgumentException("OpMode " + name + " already exists for mode " + mode); + } + // collision, try again with space appended + nameCopy += ' '; + } + } finally { + m_opModesMutex.unlock(); + } + } + + /** + * Adds an operating mode option. It's necessary to call publishOpModes() to make the added modes + * visible to the driver station. + * + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + * @return unique ID used to later identify the operating mode + * @throws IllegalArgumentException if name is empty or an operating mode with the same name + * already exists + */ + public static long addOpMode(RobotMode mode, String name, String group, String description) { + return addOpMode(mode, name, group, description, null, null); + } + + /** + * Adds an operating mode option. It's necessary to call publishOpModes() to make the added modes + * visible to the driver station. + * + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @return unique ID used to later identify the operating mode + * @throws IllegalArgumentException if name is empty or an operating mode with the same name + * already exists + */ + public static long addOpMode(RobotMode mode, String name, String group) { + return addOpMode(mode, name, group, ""); + } + + /** + * Adds an operating mode option. It's necessary to call publishOpModes() to make the added modes + * visible to the driver station. + * + * @param mode robot mode + * @param name name of the operating mode + * @return unique ID used to later identify the operating mode + * @throws IllegalArgumentException if name is empty or an operating mode with the same name + * already exists + */ + public static long addOpMode(RobotMode mode, String name) { + return addOpMode(mode, name, ""); + } + + /** + * Removes an operating mode option. It's necessary to call publishOpModes() to make the removed + * mode no longer visible to the driver station. + * + * @param mode robot mode + * @param name name of the operating mode + * @return unique ID for the opmode, or 0 if not found + */ + public static long removeOpMode(RobotMode mode, String name) { + if (name.isBlank()) { + return 0; + } + m_opModesMutex.lock(); + try { + // we have to loop over all entries to find the one with the correct name + // because the of the unique ID generation scheme + for (OpModeOption opMode : m_opModes.values()) { + if (opMode.getMode() == mode && opMode.name.equals(name)) { + m_opModes.remove(opMode.id); + return opMode.id; + } + } + } finally { + m_opModesMutex.unlock(); + } + return 0; + } + + /** Publishes the operating mode options to the driver station. */ + public static void publishOpModes() { + m_opModesMutex.lock(); + try { + OpModeOption[] options = new OpModeOption[m_opModes.size()]; + DriverStationJNI.setOpModeOptions(m_opModes.values().toArray(options)); + } finally { + m_opModesMutex.unlock(); + } + } + + /** Clears all operating mode options and publishes an empty list to the driver station. */ + public static void clearOpModes() { + m_opModesMutex.lock(); + try { + m_opModes.clear(); + DriverStationJNI.setOpModeOptions(new OpModeOption[0]); + } finally { + m_opModesMutex.unlock(); + } + } + + /** + * Gets the operating mode selected on the driver station. Note this does not mean the robot is + * enabled; use isEnabled() for that. In a match, this will indicate the operating mode selected + * for auto before the match starts (i.e., while the robot is disabled in auto mode); after the + * auto period ends, this will change to reflect the operating mode selected for teleop. + * + * @return the unique ID provided by the addOpMode() function; may return 0 or a unique ID not + * added, so callers should be prepared to handle that case + */ + public static long getOpModeId() { + m_cacheDataMutex.lock(); + try { + return m_controlWord.getOpModeId(); + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Gets the operating mode selected on the driver station. Note this does not mean the robot is + * enabled; use isEnabled() for that. In a match, this will indicate the operating mode selected + * for auto before the match starts (i.e., while the robot is disabled in auto mode); after the + * auto period ends, this will change to reflect the operating mode selected for teleop. + * + * @return Operating mode string; may return a string not in the list of options, so callers + * should be prepared to handle that case + */ + public static String getOpMode() { + m_cacheDataMutex.lock(); + try { + return m_opMode; + } finally { + m_cacheDataMutex.unlock(); + } + } + + /** + * Check to see if the selected operating mode is a particular value. Note this does not mean the + * robot is enabled; use isEnabled() for that. + * + * @param id operating mode unique ID + * @return True if that mode is the current mode + */ + public static boolean isOpMode(long id) { + return getOpModeId() == id; + } + + /** + * Check to see if the selected operating mode is a particular value. Note this does not mean the + * robot is enabled; use isEnabled() for that. + * + * @param mode operating mode + * @return True if that mode is the current mode + */ + public static boolean isOpMode(String mode) { + return getOpMode().equals(mode); + } + /** * Gets a value indicating whether the Driver Station is attached. * @@ -1309,7 +1554,7 @@ public final class DriverStation { public static boolean isDSAttached() { m_cacheDataMutex.lock(); try { - return m_controlWord.getDSAttached(); + return m_controlWord.isDSAttached(); } finally { m_cacheDataMutex.unlock(); } @@ -1323,7 +1568,7 @@ public final class DriverStation { public static boolean isFMSAttached() { m_cacheDataMutex.lock(); try { - return m_controlWord.getFMSAttached(); + return m_controlWord.isFMSAttached(); } finally { m_cacheDataMutex.unlock(); } @@ -1569,6 +1814,8 @@ public final class DriverStation { DriverStationJNI.getControlWord(m_controlWordCache); + m_opModeCache = opModeToString(m_controlWordCache.getOpModeId()); + DataLogSender dataLogSender; // lock joystick mutex to swap cache data m_cacheDataMutex.lock(); @@ -1612,6 +1859,10 @@ public final class DriverStation { m_controlWord = m_controlWordCache; m_controlWordCache = currentWord; + String currentOpMode = m_opMode; + m_opMode = m_opModeCache; + m_opModeCache = currentOpMode; + dataLogSender = m_dataLogSender; } finally { m_cacheDataMutex.unlock(); diff --git a/wpilibj/src/main/java/org/wpilib/framework/IterativeRobotBase.java b/wpilibj/src/main/java/org/wpilib/framework/IterativeRobotBase.java index 009164591b..89439b7280 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/IterativeRobotBase.java +++ b/wpilibj/src/main/java/org/wpilib/framework/IterativeRobotBase.java @@ -4,10 +4,11 @@ package org.wpilib.framework; -import org.wpilib.driverstation.DSControlWord; import org.wpilib.driverstation.DriverStation; +import org.wpilib.hardware.hal.ControlWord; import org.wpilib.hardware.hal.DriverStationJNI; import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.networktables.NetworkTableInstance; import org.wpilib.smartdashboard.SmartDashboard; import org.wpilib.system.Watchdog; @@ -55,16 +56,8 @@ import org.wpilib.system.Watchdog; * */ public abstract class IterativeRobotBase extends RobotBase { - private enum Mode { - kNone, - kDisabled, - kAutonomous, - kTeleop, - kTest - } - - private final DSControlWord m_word = new DSControlWord(); - private Mode m_lastMode = Mode.kNone; + private final ControlWord m_word = new ControlWord(); + private RobotMode m_lastMode; private final double m_period; private final Watchdog m_watchdog; private boolean m_ntFlushEnabled = true; @@ -256,21 +249,12 @@ public abstract class IterativeRobotBase extends RobotBase { /** Loop function. */ protected void loopFunc() { DriverStation.refreshData(); + DriverStation.refreshControlWordFromCache(m_word); m_watchdog.reset(); - m_word.refresh(); - - // Get current mode - Mode mode = Mode.kNone; - if (m_word.isDisabled()) { - mode = Mode.kDisabled; - } else if (m_word.isAutonomous()) { - mode = Mode.kAutonomous; - } else if (m_word.isTeleop()) { - mode = Mode.kTeleop; - } else if (m_word.isTest()) { - mode = Mode.kTest; - } + // Get current mode; treat disabled as unknown + boolean enabled = m_word.isEnabled(); + RobotMode mode = enabled ? m_word.getRobotMode() : RobotMode.UNKNOWN; if (!m_calledDsConnected && m_word.isDSAttached()) { m_calledDsConnected = true; @@ -279,32 +263,34 @@ public abstract class IterativeRobotBase extends RobotBase { // If mode changed, call mode exit and entry functions if (m_lastMode != mode) { - // Call last mode's exit function - switch (m_lastMode) { - case kDisabled -> disabledExit(); - case kAutonomous -> autonomousExit(); - case kTeleop -> teleopExit(); - case kTest -> testExit(); - default -> { - // NOP + if (m_lastMode != null) { + // Call last mode's exit function + switch (m_lastMode) { + case UNKNOWN -> disabledExit(); + case AUTONOMOUS -> autonomousExit(); + case TELEOPERATED -> teleopExit(); + case TEST -> testExit(); + default -> { + // NOP + } } } // Call current mode's entry function switch (mode) { - case kDisabled -> { + case UNKNOWN -> { disabledInit(); m_watchdog.addEpoch("disabledInit()"); } - case kAutonomous -> { + case AUTONOMOUS -> { autonomousInit(); m_watchdog.addEpoch("autonomousInit()"); } - case kTeleop -> { + case TELEOPERATED -> { teleopInit(); m_watchdog.addEpoch("teleopInit()"); } - case kTest -> { + case TEST -> { testInit(); m_watchdog.addEpoch("testInit()"); } @@ -317,24 +303,21 @@ public abstract class IterativeRobotBase extends RobotBase { } // Call the appropriate function depending upon the current robot mode + DriverStationJNI.observeUserProgram(m_word.getNative()); switch (mode) { - case kDisabled -> { - DriverStationJNI.observeUserProgramDisabled(); + case UNKNOWN -> { disabledPeriodic(); m_watchdog.addEpoch("disabledPeriodic()"); } - case kAutonomous -> { - DriverStationJNI.observeUserProgramAutonomous(); + case AUTONOMOUS -> { autonomousPeriodic(); m_watchdog.addEpoch("autonomousPeriodic()"); } - case kTeleop -> { - DriverStationJNI.observeUserProgramTeleop(); + case TELEOPERATED -> { teleopPeriodic(); m_watchdog.addEpoch("teleopPeriodic()"); } - case kTest -> { - DriverStationJNI.observeUserProgramTest(); + case TEST -> { testPeriodic(); m_watchdog.addEpoch("testPeriodic()"); } diff --git a/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java new file mode 100644 index 0000000000..4bd56590bc --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java @@ -0,0 +1,701 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.framework; + +import java.io.File; +import java.io.IOException; +import java.lang.reflect.Constructor; +import java.lang.reflect.Modifier; +import java.net.URL; +import java.util.Enumeration; +import java.util.HashMap; +import java.util.Map; +import java.util.concurrent.atomic.AtomicReference; +import java.util.function.Supplier; +import java.util.jar.JarEntry; +import java.util.jar.JarFile; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.hardware.hal.ControlWord; +import org.wpilib.hardware.hal.DriverStationJNI; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.NotifierJNI; +import org.wpilib.hardware.hal.RobotMode; +import org.wpilib.opmode.Autonomous; +import org.wpilib.opmode.OpMode; +import org.wpilib.opmode.Teleop; +import org.wpilib.opmode.TestOpMode; +import org.wpilib.util.Color; +import org.wpilib.util.WPIUtilJNI; + +/** + * OpModeRobot implements the opmode-based robot program framework. + * + *

The OpModeRobot class is intended to be subclassed by a user creating a robot program. + * + *

Classes annotated with {@link Autonomous}, {@link Teleop}, and {@link TestOpMode} in the same + * package or subpackages as the user's subclass will be automatically registered as autonomous, + * teleop, and test opmodes respectively. + * + *

Opmodes are constructed when selected on the driver station, and closed/no longer used when + * the robot is disabled after being enabled or a different opmode is selected. When no opmode is + * selected, nonePeriodic() is called. The driverStationConnected() function is called the first + * time the driver station connects to the robot. + */ +public abstract class OpModeRobot extends RobotBase { + private final ControlWord m_word = new ControlWord(); + + private record OpModeFactory(String name, Supplier supplier) {} + + private final Map m_opModes = new HashMap<>(); + private final AtomicReference m_activeOpMode = new AtomicReference<>(null); + private volatile int m_notifier; + + private static void reportAddOpModeError(Class cls, String message) { + DriverStation.reportError("Error adding OpMode " + cls.getSimpleName() + ": " + message, false); + } + + /** + * Find a public constructor to instantiate the opmode. Prefer a single-arg public constructor + * whose parameter type is assignable from this.getClass() (if multiple, pick the most specific + * parameter type). Otherwise return the public no-arg constructor. Return null if neither exists. + */ + private Constructor findOpModeConstructor(Class cls) { + Constructor bestCtor = null; + Class bestParam = null; + for (Constructor ctor : cls.getConstructors()) { + Class[] params = ctor.getParameterTypes(); + if (params.length != 1) { + continue; + } + Class param = params[0]; + if (!param.isAssignableFrom(getClass())) { + continue; + } + if (bestCtor == null || bestParam.isAssignableFrom(param)) { + bestCtor = ctor; + bestParam = param; + } + } + if (bestCtor != null) { + return bestCtor; + } + try { + return cls.getConstructor(); + } catch (NoSuchMethodException e) { + return null; + } + } + + private OpMode constructOpModeClass(Class cls) { + Constructor constructor = findOpModeConstructor(cls); + if (constructor == null) { + DriverStation.reportError( + "No suitable constructor to instantiate OpMode " + cls.getSimpleName(), true); + return null; + } + try { + if (constructor.getParameterCount() == 1) { + return (OpMode) constructor.newInstance(this); + } else { + return (OpMode) constructor.newInstance(); + } + } catch (ReflectiveOperationException e) { + DriverStation.reportError( + "Could not instantiate OpMode " + cls.getSimpleName(), e.getStackTrace()); + return null; + } + } + + private void checkOpModeClass(Class cls) { + // the class must be a subclass of OpMode + if (!OpMode.class.isAssignableFrom(cls)) { + throw new IllegalArgumentException("not a subclass of OpMode"); + } + int modifiers = cls.getModifiers(); + // it cannot be abstract + if (Modifier.isAbstract(modifiers)) { + throw new IllegalArgumentException("is abstract"); + } + // it must be public + if (!Modifier.isPublic(modifiers)) { + throw new IllegalArgumentException("not public"); + } + // it must not be a non-static inner class + if (cls.getEnclosingClass() != null && !Modifier.isStatic(modifiers)) { + throw new IllegalArgumentException("is a non-static inner class"); + } + // it must have a public no-arg constructor or a public constructor that accepts this class + // (or a superclass/interface) as an argument + if (findOpModeConstructor(cls) == null) { + throw new IllegalArgumentException( + "missing public no-arg constructor or constructor accepting " + + getClass().getSimpleName()); + } + } + + /** + * Adds an opmode using a factory function that creates the opmode. It's necessary to call + * publishOpModes() to make the added mode visible to the driver station. + * + * @param factory factory function to create the opmode + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + * @param textColor text color, or null for default + * @param backgroundColor background color, or null for default + * @throws IllegalArgumentException if class does not meet criteria + */ + public void addOpModeFactory( + Supplier factory, + RobotMode mode, + String name, + String group, + String description, + Color textColor, + Color backgroundColor) { + long id = DriverStation.addOpMode(mode, name, group, description, textColor, backgroundColor); + m_opModes.put(id, new OpModeFactory(name, factory)); + } + + /** + * Adds an opmode using a factory function that creates the opmode. It's necessary to call + * publishOpModes() to make the added mode visible to the driver station. + * + * @param factory factory function to create the opmode + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + * @throws IllegalArgumentException if class does not meet criteria + */ + public void addOpModeFactory( + Supplier factory, RobotMode mode, String name, String group, String description) { + addOpModeFactory(factory, mode, name, group, description, null, null); + } + + /** + * Adds an opmode using a factory function that creates the opmode. It's necessary to call + * publishOpModes() to make the added mode visible to the driver station. + * + * @param factory factory function to create the opmode + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @throws IllegalArgumentException if class does not meet criteria + */ + public void addOpModeFactory( + Supplier factory, RobotMode mode, String name, String group) { + addOpModeFactory(factory, mode, name, group, ""); + } + + /** + * Adds an opmode using a factory function that creates the opmode. It's necessary to call + * publishOpModes() to make the added mode visible to the driver station. + * + * @param factory factory function to create the opmode + * @param mode robot mode + * @param name name of the operating mode + * @throws IllegalArgumentException if class does not meet criteria + */ + public void addOpModeFactory(Supplier factory, RobotMode mode, String name) { + addOpModeFactory(factory, mode, name, ""); + } + + /** + * Adds an opmode for an opmode class. 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 of this + * class's type (the latter is preferred). It's necessary to call publishOpModes() to make the + * added mode visible to the driver station. + * + * @param cls class to add + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + * @param textColor text color, or null for default + * @param backgroundColor background color, or null for default + * @throws IllegalArgumentException if class does not meet criteria + */ + public void addOpMode( + Class cls, + RobotMode mode, + String name, + String group, + String description, + Color textColor, + Color backgroundColor) { + checkOpModeClass(cls); + addOpModeFactory( + () -> constructOpModeClass(cls), + mode, + name, + group, + description, + textColor, + backgroundColor); + } + + /** + * Adds an opmode for an opmode class. 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 of this + * class's type (the latter is preferred). It's necessary to call publishOpModes() to make the + * added mode visible to the driver station. + * + * @param cls class to add + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @param description description of the operating mode + * @throws IllegalArgumentException if class does not meet criteria + */ + public void addOpMode( + Class cls, RobotMode mode, String name, String group, String description) { + addOpMode(cls, mode, name, group, description, null, null); + } + + /** + * Adds an opmode for an opmode class. 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 of this + * class's type (the latter is preferred). It's necessary to call publishOpModes() to make the + * added mode visible to the driver station. + * + * @param cls class to add + * @param mode robot mode + * @param name name of the operating mode + * @param group group of the operating mode + * @throws IllegalArgumentException if class does not meet criteria + */ + public void addOpMode(Class cls, RobotMode mode, String name, String group) { + addOpMode(cls, mode, name, group, ""); + } + + /** + * Adds an opmode for an opmode class. 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 of this + * class's type (the latter is preferred). It's necessary to call publishOpModes() to make the + * added mode visible to the driver station. + * + * @param cls class to add + * @param mode robot mode + * @param name name of the operating mode + * @throws IllegalArgumentException if class does not meet criteria + */ + public void addOpMode(Class cls, RobotMode mode, String name) { + addOpMode(cls, mode, name, ""); + } + + private void addOpModeClassImpl( + Class cls, + RobotMode mode, + String name, + String group, + String description, + String textColor, + String backgroundColor) { + if (name == null || name.isBlank()) { + name = cls.getSimpleName(); + } + Color tColor = textColor.isBlank() ? null : Color.fromString(textColor); + Color bColor = backgroundColor.isBlank() ? null : Color.fromString(backgroundColor); + long id = DriverStation.addOpMode(mode, name, group, description, tColor, bColor); + m_opModes.put(id, new OpModeFactory(name, () -> constructOpModeClass(cls))); + } + + private void addAnnotatedOpModeImpl( + Class cls, Autonomous auto, Teleop teleop, TestOpMode test) { + checkOpModeClass(cls); + + // add an opmode for each annotation + if (auto != null) { + addOpModeClassImpl( + cls, + RobotMode.AUTONOMOUS, + auto.name(), + auto.group(), + auto.description(), + auto.textColor(), + auto.backgroundColor()); + } + if (teleop != null) { + addOpModeClassImpl( + cls, + RobotMode.TELEOPERATED, + teleop.name(), + teleop.group(), + teleop.description(), + teleop.textColor(), + teleop.backgroundColor()); + } + if (test != null) { + addOpModeClassImpl( + cls, + RobotMode.TEST, + test.name(), + test.group(), + test.description(), + test.textColor(), + test.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 of this class's type. + * 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 + */ + public void addAnnotatedOpMode(Class 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"); + } + addAnnotatedOpModeImpl(cls, auto, teleop, test); + } + + private void addAnnotatedOpModeClass(String name) { + // trim ".class" from end + String className = name.replace('/', '.').substring(0, name.length() - 6); + Class cls; + try { + cls = Class.forName(className); + } catch (ClassNotFoundException e) { + return; + } + 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) { + return; + } + try { + addAnnotatedOpModeImpl(cls, auto, teleop, test); + } catch (IllegalArgumentException e) { + reportAddOpModeError(cls, e.getMessage()); + } + } + + private void addAnnotatedOpModeClassesDir(File root, File dir, String packagePath) { + File[] files = dir.listFiles(); + if (files == null) { + return; + } + for (File file : files) { + if (file.isDirectory()) { + addAnnotatedOpModeClassesDir(root, file, packagePath); + } else if (file.getName().endsWith(".class")) { + String relPath = root.toPath().relativize(file.toPath()).toString().replace('\\', '/'); + addAnnotatedOpModeClass(packagePath + "." + relPath); + } + } + } + + /** + * 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 + * call publishOpModes() to make the added modes visible to the driver station. + * + * @param pkg package to scan + */ + public void addAnnotatedOpModeClasses(Package pkg) { + String packageName = pkg.getName(); + String packagePath = packageName.replace('.', '/'); + ClassLoader classLoader = Thread.currentThread().getContextClassLoader(); + + try { + Enumeration resources = classLoader.getResources(packagePath); + while (resources.hasMoreElements()) { + URL resource = resources.nextElement(); + if ("jar".equals(resource.getProtocol())) { + // Get path of JAR file from URL path (format "file:!/path_to_entry") + String jarPath = resource.getPath().substring(5, resource.getPath().indexOf('!')); + try (JarFile jar = new JarFile(jarPath)) { + Enumeration entries = jar.entries(); + while (entries.hasMoreElements()) { + String name = entries.nextElement().getName(); + if (!name.startsWith(packagePath) || !name.endsWith(".class")) { + continue; + } + addAnnotatedOpModeClass(name); + } + } + } else if ("file".equals(resource.getProtocol())) { + // Handle .class files in directories + File dir = new File(resource.getPath()); + if (dir.exists() && dir.isDirectory()) { + addAnnotatedOpModeClassesDir(dir, dir, packagePath); + } + } + } + } catch (IOException e) { + e.printStackTrace(); + } + } + + /** + * Removes an operating mode option. It's necessary to call publishOpModes() to make the removed + * mode no longer visible to the driver station. + * + * @param mode robot mode + * @param name name of the operating mode + */ + public void removeOpMode(RobotMode mode, String name) { + long id = DriverStation.removeOpMode(mode, name); + if (id != 0) { + m_opModes.remove(id); + } + } + + /** Publishes the operating mode options to the driver station. */ + public void publishOpModes() { + DriverStation.publishOpModes(); + } + + /** Clears all operating mode options and publishes an empty list to the driver station. */ + public void clearOpModes() { + DriverStation.clearOpModes(); + m_opModes.clear(); + } + + /** Constructor. */ + @SuppressWarnings("this-escape") + public OpModeRobot() { + // Scan for annotated opmode classes within the derived class's package and subpackages + addAnnotatedOpModeClasses(getClass().getPackage()); + DriverStation.publishOpModes(); + } + + /** + * Function called exactly once after the DS is connected. + * + *

Code that needs to know the DS state should go here. + * + *

Users should override this method for initialization that needs to occur after the DS is + * connected, such as needing the alliance information. + */ + public void driverStationConnected() {} + + /** + * Function called periodically anytime when no opmode is selected, including when the Driver + * Station is disconnected. + */ + public void nonePeriodic() {} + + /** + * Background monitor thread. On mode/opmode change, this checks to see if the change is actually + * reflected in this class within a reasonable amount of time. If not, that means that the user + * code is stuck and we need to take action to try to get it to exit (up to and including program + * termination). + */ + private void monitorThreadMain(Thread thr, long opmode, int event, int endEvent) { + ControlWord word = new ControlWord(); + int[] events = {event, endEvent}; + while (true) { + try { + int[] signaled = WPIUtilJNI.waitForObjects(events); + for (int val : signaled) { + if (val < 0) { + return; // handle destroyed + } + } + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + return; + } + + // did the opmode or enable state change? + DriverStationJNI.getUncachedControlWord(word); + if (!word.isEnabled() || word.getOpModeId() != opmode) { + break; + } + } + + // call opmode stop + OpMode opMode = m_activeOpMode.get(); + if (opMode != null) { + opMode.opModeStop(); + } + + events[0] = m_notifier; + NotifierJNI.setNotifierAlarm(m_notifier, 200000, 0, false, true); // 200 ms + try { + int[] signaled = WPIUtilJNI.waitForObjects(events); + for (int val : signaled) { + if (val < 0 || val == endEvent) { + return; // transitioned, or handle destroyed + } + } + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + return; + } + + // if it hasn't transitioned after 200 ms, call thread.interrupt() + DriverStation.reportError("OpMode did not exit, interrupting thread", false); + thr.interrupt(); + + NotifierJNI.setNotifierAlarm(m_notifier, 800000, 0, false, true); // 800 ms + try { + int[] signaled = WPIUtilJNI.waitForObjects(events); + for (int val : signaled) { + if (val < 0 || val == endEvent) { + return; // transitioned, or handle destroyed + } + } + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + return; + } + + // if it hasn't transitioned after 1 second, terminate the program + DriverStation.reportError("OpMode did not exit, terminating program", false); + HAL.terminate(); + HAL.shutdown(); + System.exit(0); + } + + /** Provide an alternate "main loop" via startCompetition(). */ + @Override + public final void startCompetition() { + System.out.println("********** Robot program startup complete **********"); + DriverStationJNI.observeUserProgramStarting(); + + int event = WPIUtilJNI.createEvent(false, false); + DriverStationJNI.provideNewDataEventHandle(event); + + m_notifier = NotifierJNI.createNotifier(); + NotifierJNI.setNotifierName(m_notifier, "OpModeRobot"); + + try { + // Implement the opmode lifecycle + long lastModeId = -1; + boolean calledDriverStationConnected = false; + int[] events = {event, m_notifier}; + while (true) { + // Wait for new data from the driver station, with 50 ms timeout + NotifierJNI.setNotifierAlarm(m_notifier, 50000, 0, false, true); + try { + int[] signaled = WPIUtilJNI.waitForObjects(events); + for (int val : signaled) { + if (val < 0) { + return; // handle destroyed + } + } + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + break; + } + + // Get the latest control word and opmode + DriverStation.refreshData(); + DriverStation.refreshControlWordFromCache(m_word); + + if (!calledDriverStationConnected && m_word.isDSAttached()) { + calledDriverStationConnected = true; + driverStationConnected(); + } + + long modeId; + if (!m_word.isDSAttached()) { + modeId = 0; + } else { + modeId = m_word.getOpModeId(); + } + + OpMode opMode = m_activeOpMode.get(); + if (opMode == null || modeId != lastModeId) { + if (opMode != null) { + // no or different opmode selected + m_activeOpMode.set(null); + opMode.opModeClose(); + } + + if (modeId == 0) { + // no opmode selected + nonePeriodic(); + DriverStationJNI.observeUserProgram(m_word.getNative()); + continue; + } + + OpModeFactory factory = m_opModes.get(modeId); + if (factory == null) { + DriverStation.reportError("No OpMode found for mode " + modeId, false); + m_word.setOpModeId(0); + DriverStationJNI.observeUserProgram(m_word.getNative()); + continue; + } + + // Instantiate the opmode + System.out.println("********** Starting OpMode " + factory.name() + " **********"); + opMode = factory.supplier().get(); + if (opMode == null) { + // could not construct + m_word.setOpModeId(0); + DriverStationJNI.observeUserProgram(m_word.getNative()); + continue; + } + m_activeOpMode.set(opMode); + lastModeId = modeId; + // Ensure disabledPeriodic is always called at least once + opMode.disabledPeriodic(); + } + + DriverStationJNI.observeUserProgram(m_word.getNative()); + + if (m_word.isEnabled()) { + // When enabled, call the opmode run function, then close and clear + int endMonitor = WPIUtilJNI.createEvent(true, false); + Thread curThread = Thread.currentThread(); + Thread monitor = + new Thread( + () -> { + monitorThreadMain(curThread, modeId, event, endMonitor); + }); + monitor.start(); + try { + opMode.opModeRun(modeId); + } catch (InterruptedException e) { + // ignored + } finally { + Thread.interrupted(); + WPIUtilJNI.destroyEvent(endMonitor); + try { + monitor.join(); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + } + } + opMode = m_activeOpMode.getAndSet(null); + if (opMode != null) { + opMode.opModeClose(); + } + } else { + // When disabled, call the disabledPeriodic function + opMode.disabledPeriodic(); + } + } + } finally { + DriverStationJNI.removeNewDataEventHandle(event); + WPIUtilJNI.destroyEvent(event); + NotifierJNI.destroyNotifier(m_notifier); + } + } + + /** Ends the main loop in startCompetition(). */ + @Override + public final void endCompetition() { + NotifierJNI.destroyNotifier(m_notifier); + OpMode opMode = m_activeOpMode.get(); + if (opMode != null) { + opMode.opModeStop(); + } + } +} diff --git a/wpilibj/src/main/java/org/wpilib/framework/RobotBase.java b/wpilibj/src/main/java/org/wpilib/framework/RobotBase.java index 2762048234..1982a910cd 100644 --- a/wpilibj/src/main/java/org/wpilib/framework/RobotBase.java +++ b/wpilibj/src/main/java/org/wpilib/framework/RobotBase.java @@ -180,7 +180,7 @@ public abstract class RobotBase implements AutoCloseable { * * @return True if the Robot is currently disabled by the Driver Station. */ - public boolean isDisabled() { + public static boolean isDisabled() { return DriverStation.isDisabled(); } @@ -189,7 +189,7 @@ public abstract class RobotBase implements AutoCloseable { * * @return True if the Robot is currently enabled by the Driver Station. */ - public boolean isEnabled() { + public static boolean isEnabled() { return DriverStation.isEnabled(); } @@ -198,7 +198,7 @@ public abstract class RobotBase implements AutoCloseable { * * @return True if the robot is currently operating Autonomously. */ - public boolean isAutonomous() { + public static boolean isAutonomous() { return DriverStation.isAutonomous(); } @@ -208,7 +208,7 @@ public abstract class RobotBase implements AutoCloseable { * * @return True if the robot is currently operating autonomously while enabled. */ - public boolean isAutonomousEnabled() { + public static boolean isAutonomousEnabled() { return DriverStation.isAutonomousEnabled(); } @@ -217,7 +217,7 @@ public abstract class RobotBase implements AutoCloseable { * * @return True if the robot is currently operating in Test mode. */ - public boolean isTest() { + public static boolean isTest() { return DriverStation.isTest(); } @@ -226,7 +226,7 @@ public abstract class RobotBase implements AutoCloseable { * * @return True if the robot is currently operating in Test mode while enabled. */ - public boolean isTestEnabled() { + public static boolean isTestEnabled() { return DriverStation.isTestEnabled(); } @@ -236,7 +236,7 @@ public abstract class RobotBase implements AutoCloseable { * * @return True if the robot is currently operating in Tele-Op mode. */ - public boolean isTeleop() { + public static boolean isTeleop() { return DriverStation.isTeleop(); } @@ -246,10 +246,32 @@ public abstract class RobotBase implements AutoCloseable { * * @return True if the robot is currently operating in Tele-Op mode while enabled. */ - public boolean isTeleopEnabled() { + public static boolean isTeleopEnabled() { return DriverStation.isTeleopEnabled(); } + /** + * Gets the currently selected operating mode of the driver station. Note this does not mean the + * robot is enabled; use isEnabled() for that. + * + * @return the unique ID provided by the DriverStation.addOpMode() function; may return 0 or a + * unique ID not added, so callers should be prepared to handle that case + */ + public static long getOpModeId() { + return DriverStation.getOpModeId(); + } + + /** + * Gets the currently selected operating mode of the driver station. Note this does not mean the + * robot is enabled; use isEnabled() for that. + * + * @return Operating mode string; may return a string not in the list of options, so callers + * should be prepared to handle that case + */ + public static String getOpMode() { + return DriverStation.getOpMode(); + } + /** * Start the main robot code. This function will be called once and should not exit until * signalled by {@link #endCompetition()} diff --git a/wpilibj/src/main/java/org/wpilib/framework/RobotState.java b/wpilibj/src/main/java/org/wpilib/framework/RobotState.java deleted file mode 100644 index e5b389cdf5..0000000000 --- a/wpilibj/src/main/java/org/wpilib/framework/RobotState.java +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.wpilib.framework; - -import org.wpilib.driverstation.DriverStation; - -/** Robot state utility functions. */ -public final class RobotState { - /** - * Returns true if the robot is disabled. - * - * @return True if the robot is disabled. - */ - public static boolean isDisabled() { - return DriverStation.isDisabled(); - } - - /** - * Returns true if the robot is enabled. - * - * @return True if the robot is enabled. - */ - public static boolean isEnabled() { - return DriverStation.isEnabled(); - } - - /** - * Returns true if the robot is E-stopped. - * - * @return True if the robot is E-stopped. - */ - public static boolean isEStopped() { - return DriverStation.isEStopped(); - } - - /** - * Returns true if the robot is in teleop mode. - * - * @return True if the robot is in teleop mode. - */ - public static boolean isTeleop() { - return DriverStation.isTeleop(); - } - - /** - * Returns true if the robot is in autonomous mode. - * - * @return True if the robot is in autonomous mode. - */ - public static boolean isAutonomous() { - return DriverStation.isAutonomous(); - } - - /** - * Returns true if the robot is in test mode. - * - * @return True if the robot is in test mode. - */ - public static boolean isTest() { - return DriverStation.isTest(); - } - - private RobotState() {} -} diff --git a/wpilibj/src/main/java/org/wpilib/hardware/motor/MotorSafety.java b/wpilibj/src/main/java/org/wpilib/hardware/motor/MotorSafety.java index 6dc32b3f91..16b466c5f8 100644 --- a/wpilibj/src/main/java/org/wpilib/hardware/motor/MotorSafety.java +++ b/wpilibj/src/main/java/org/wpilib/hardware/motor/MotorSafety.java @@ -7,7 +7,6 @@ package org.wpilib.hardware.motor; import java.util.LinkedHashSet; import java.util.Set; import org.wpilib.driverstation.DriverStation; -import org.wpilib.framework.RobotState; import org.wpilib.hardware.hal.ControlWord; import org.wpilib.hardware.hal.DriverStationJNI; import org.wpilib.system.Timer; @@ -51,7 +50,7 @@ public abstract class MotorSafety { } if (!timedOut) { DriverStationJNI.getControlWord(controlWord); - if (!(controlWord.getEnabled() && controlWord.getDSAttached())) { + if (!(controlWord.isEnabled() && controlWord.isDSAttached())) { safetyCounter = 0; } if (++safetyCounter >= 4) { @@ -135,7 +134,7 @@ public abstract class MotorSafety { stopTime = m_stopTime; } - if (!enabled || RobotState.isDisabled() || RobotState.isTest()) { + if (!enabled || DriverStation.isDisabled() || DriverStation.isTest()) { return; } diff --git a/wpilibj/src/main/java/org/wpilib/internal/DriverStationModeThread.java b/wpilibj/src/main/java/org/wpilib/internal/DriverStationModeThread.java index 945e7c1e18..3dc19b8f13 100644 --- a/wpilibj/src/main/java/org/wpilib/internal/DriverStationModeThread.java +++ b/wpilibj/src/main/java/org/wpilib/internal/DriverStationModeThread.java @@ -5,101 +5,61 @@ package org.wpilib.internal; import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicLong; import org.wpilib.driverstation.DriverStation; +import org.wpilib.hardware.hal.ControlWord; import org.wpilib.hardware.hal.DriverStationJNI; import org.wpilib.util.WPIUtilJNI; /** For internal use only. */ public class DriverStationModeThread implements AutoCloseable { - private final AtomicBoolean m_keepAlive = new AtomicBoolean(); + private final AtomicBoolean m_keepAlive = new AtomicBoolean(true); + private final AtomicLong m_userControlWord; + private final int m_handle = WPIUtilJNI.createEvent(false, false); private final Thread m_thread; - private boolean m_userInDisabled; - private boolean m_userInAutonomous; - private boolean m_userInTeleop; - private boolean m_userInTest; - - /** Internal use only. */ - public DriverStationModeThread() { - m_keepAlive.set(true); + /** + * Internal use only. + * + * @param word control word + */ + public DriverStationModeThread(ControlWord word) { + m_userControlWord = new AtomicLong(word.getNative()); + DriverStationJNI.provideNewDataEventHandle(m_handle); m_thread = new Thread(this::run, "DriverStationMode"); m_thread.start(); } private void run() { - int handle = WPIUtilJNI.createEvent(false, false); - DriverStationJNI.provideNewDataEventHandle(handle); - - while (m_keepAlive.get()) { + while (true) { try { - WPIUtilJNI.waitForObjectTimeout(handle, 0.1); + WPIUtilJNI.waitForObjectTimeout(m_handle, 0.1); } catch (InterruptedException e) { - DriverStationJNI.removeNewDataEventHandle(handle); - WPIUtilJNI.destroyEvent(handle); Thread.currentThread().interrupt(); return; } + if (!m_keepAlive.get()) { + return; + } DriverStation.refreshData(); - if (m_userInDisabled) { - DriverStationJNI.observeUserProgramDisabled(); - } - if (m_userInAutonomous) { - DriverStationJNI.observeUserProgramAutonomous(); - } - if (m_userInTeleop) { - DriverStationJNI.observeUserProgramTeleop(); - } - if (m_userInTest) { - DriverStationJNI.observeUserProgramTest(); - } + DriverStationJNI.observeUserProgram(m_userControlWord.get()); } - - DriverStationJNI.removeNewDataEventHandle(handle); - WPIUtilJNI.destroyEvent(handle); } /** * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic * purposes only. * - * @param entering If true, starting disabled code; if false, leaving disabled code + * @param word control word */ - public void inDisabled(boolean entering) { - m_userInDisabled = entering; - } - - /** - * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic - * purposes only. - * - * @param entering If true, starting autonomous code; if false, leaving autonomous code - */ - public void inAutonomous(boolean entering) { - m_userInAutonomous = entering; - } - - /** - * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic - * purposes only. - * - * @param entering If true, starting teleop code; if false, leaving teleop code - */ - public void inTeleop(boolean entering) { - m_userInTeleop = entering; - } - - /** - * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic - * purposes only. - * - * @param entering If true, starting test code; if false, leaving test code - */ - public void inTest(boolean entering) { - m_userInTest = entering; + public void inControl(ControlWord word) { + m_userControlWord.set(word.getNative()); } @Override public void close() { + DriverStationJNI.removeNewDataEventHandle(m_handle); + WPIUtilJNI.destroyEvent(m_handle); m_keepAlive.set(false); try { m_thread.join(); diff --git a/wpilibj/src/main/java/org/wpilib/opmode/Autonomous.java b/wpilibj/src/main/java/org/wpilib/opmode/Autonomous.java new file mode 100644 index 0000000000..17fd434cfc --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/opmode/Autonomous.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.opmode; + +import java.lang.annotation.Documented; +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** Annotation for automatic registration of autonomous opmode classes. */ +@Retention(RetentionPolicy.RUNTIME) +@Target(ElementType.TYPE) +@Documented +public @interface Autonomous { + /** + * Name. This is shown as the selection name in the Driver Station, and must be unique across all + * autonomous opmodes in the project. If not specified, defaults to the name of the class. + * + * @return Name + */ + String name() default ""; + + /** + * Group. All opmodes with the same group are grouped together for selection. If not specified, + * defaults to ungrouped. + * + * @return Group + */ + String group() default ""; + + /** + * Extended description. Optional. + * + * @return Description + */ + String description() default ""; + + /** + * Text color. Optional. Supports all formats supported by {@link + * org.wpilib.util.Color#fromString(String)}. + * + * @return Text color + */ + String textColor() default ""; + + /** + * Text background color. Optional. Supports all formats supported by {@link + * org.wpilib.util.Color#fromString(String)}. + * + * @return Text background color + */ + String backgroundColor() default ""; +} diff --git a/wpilibj/src/main/java/org/wpilib/opmode/LinearOpMode.java b/wpilibj/src/main/java/org/wpilib/opmode/LinearOpMode.java new file mode 100644 index 0000000000..d72cef4956 --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/opmode/LinearOpMode.java @@ -0,0 +1,84 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.opmode; + +import java.util.concurrent.atomic.AtomicBoolean; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.hardware.hal.ControlWord; +import org.wpilib.internal.DriverStationModeThread; + +/** + * An opmode structure for "linear" operation. The user is responsible for implementing any looping + * behavior; after run() returns it will not be called again on the same object. + * + *

Lifecycle: + * + *

    + *
  • constructed when opmode selected on driver station + *
  • disabledPeriodic() called periodically as long as DS is disabled + *
  • when DS transitions from disabled to enabled, run() will be called exactly once + *
  • when DS transitions from enabled to disabled, or a different opmode is selected on the + * driver station, close() is called; the object is not reused + *
+ * + *

The user is responsible for exiting run() when the opmode is directed to stop executing. This + * is indicated by isRunning() returning false. All other methods should be written to return as + * quickly as possible when isRunning() returns false. + */ +public abstract class LinearOpMode implements OpMode { + /** Constructor. */ + public LinearOpMode() {} + + /** Called periodically while the opmode is selected on the DS and the robot is disabled. */ + @Override + public void disabledPeriodic() {} + + /** + * Called when the opmode is de-selected on the DS. The object is not reused even if the same + * opmode is selected again (a new object will be created). + */ + public void close() {} + + /** + * Called once when the robot is enabled. It will not be called a second time on the same object. + * + * @throws InterruptedException when interrupted + */ + public abstract void run() throws InterruptedException; + + /** + * Returns true while this opmode is selected (regardless of enable state). All other functions + * should be written to return as quickly as possible when this returns false. + * + * @return True if opmode selected, false otherwise + */ + public boolean isRunning() { + return m_running.get(); + } + + // implements OpMode interface + @Override + public final void opModeRun(long opModeId) throws InterruptedException { + ControlWord word = new ControlWord(); + DriverStation.refreshControlWordFromCache(word); + word.setOpModeId(opModeId); + + try (DriverStationModeThread bgThread = new DriverStationModeThread(word)) { + run(); + } + } + + @Override + public final void opModeStop() { + m_running.set(false); + } + + @Override + public final void opModeClose() { + close(); + } + + private final AtomicBoolean m_running = new AtomicBoolean(true); +} diff --git a/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java b/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java new file mode 100644 index 0000000000..9faed882e2 --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/opmode/OpMode.java @@ -0,0 +1,39 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.opmode; + +/** + * Top-level interface for opmode classes. Users should generally extend one of the abstract + * implementations of this interface (e.g. {@link PeriodicOpMode} or {@link LinearOpMode}) rather + * than directly implementing this interface. + */ +public interface OpMode { + /** + * This function is called periodically while the opmode is selected on the DS (robot is + * disabled). Code that should only run once when the opmode is selected should go in the opmode + * constructor. + */ + default void disabledPeriodic() {} + + /** + * This function is called when the opmode starts (robot is enabled). + * + * @param opModeId opmode unique ID + * @throws InterruptedException when interrupted + */ + void opModeRun(long opModeId) throws InterruptedException; + + /** + * This function is called asynchronously when the robot is disabled, to request the opmode return + * from opModeRun(). + */ + void opModeStop(); + + /** + * This function is called when the opmode is no longer selected on the DS or after opModeRun() + * returns. The object will not be reused after this is called. + */ + void opModeClose(); +} diff --git a/wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java b/wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java new file mode 100644 index 0000000000..064cea93e9 --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java @@ -0,0 +1,344 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.opmode; + +import static org.wpilib.units.Units.Seconds; + +import java.util.PriorityQueue; +import org.wpilib.driverstation.DriverStation; +import org.wpilib.hardware.hal.ControlWord; +import org.wpilib.hardware.hal.DriverStationJNI; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.NotifierJNI; +import org.wpilib.networktables.NetworkTableInstance; +import org.wpilib.smartdashboard.SmartDashboard; +import org.wpilib.system.RobotController; +import org.wpilib.system.Watchdog; +import org.wpilib.units.measure.Time; +import org.wpilib.util.WPIUtilJNI; + +/** + * An opmode structure for periodic operation. This base class implements a loop that runs one or + * more functions periodically (on a set time interval aka loop period). The primary periodic + * callback function is the abstract periodic() function; the time interval for this callback is 20 + * ms by default, but may be changed via passing a different time interval to the constructor. + * Additional periodic callbacks with different intervals can be added using the addPeriodic() set + * of functions. + * + *

Lifecycle: + * + *

    + *
  • constructed when opmode selected on driver station + *
  • disabledPeriodic() called periodically as long as DS is disabled. Note this is not called + * on a set time interval (it does not use the same time interval as periodic()) + *
  • when DS transitions from disabled to enabled, start() is called once + *
  • while DS is enabled, periodic() is called periodically on the time interval set by the + * constructor, and additional periodic callbacks added via addPeriodic() are called + * periodically on their set time intervals + *
  • when DS transitions from enabled to disabled, or a different opmode is selected on the + * driver station when the DS is enabled, end() is called, followed by close(); the object is + * not reused + *
  • if a different opmode is selected on the driver station when the DS is disabled, only + * close() is called; the object is not reused + *
+ */ +public abstract class PeriodicOpMode implements OpMode { + @SuppressWarnings("MemberName") + static class Callback implements Comparable { + public Runnable func; + public long period; + public long expirationTime; + + /** + * Construct a callback container. + * + * @param func The callback to run. + * @param startTime The common starting point for all callback scheduling in microseconds. + * @param period The period at which to run the callback in microseconds. + * @param offset The offset from the common starting time in microseconds. + */ + Callback(Runnable func, long startTime, long period, long offset) { + this.func = func; + this.period = period; + this.expirationTime = + startTime + + offset + + this.period + + (RobotController.getFPGATime() - startTime) / this.period * this.period; + } + + @Override + public boolean equals(Object rhs) { + return rhs instanceof Callback callback && expirationTime == callback.expirationTime; + } + + @Override + public int hashCode() { + return Long.hashCode(expirationTime); + } + + @Override + public int compareTo(Callback rhs) { + // Elements with sooner expiration times are sorted as lesser. The head of + // Java's PriorityQueue is the least element. + return Long.compare(expirationTime, rhs.expirationTime); + } + } + + /** Default loop period. */ + public static final double kDefaultPeriod = 0.02; + + // The C pointer to the notifier object. We don't use it directly, it is + // just passed to the JNI bindings. + private int m_notifier = NotifierJNI.createNotifier(); + + private long m_startTimeUs; + private long m_loopStartTimeUs; + + private final ControlWord m_word = new ControlWord(); + private final double m_period; + private final Watchdog m_watchdog; + + private long m_opModeId; + private boolean m_running = true; + + private final PriorityQueue m_callbacks = new PriorityQueue<>(); + + /** + * Constructor. Periodic opmodes may specify the period used for the periodic() function; the + * no-argument constructor uses a default period of 20 ms. + */ + protected PeriodicOpMode() { + this(kDefaultPeriod); + } + + /** + * Constructor. Periodic opmodes may specify the period used for the periodic() function. + * + * @param period period (in seconds) for callbacks to the periodic() function + */ + protected PeriodicOpMode(double period) { + m_startTimeUs = RobotController.getFPGATime(); + m_period = period; + m_watchdog = new Watchdog(period, this::printLoopOverrunMessage); + + addPeriodic(this::loopFunc, period); + NotifierJNI.setNotifierName(m_notifier, "PeriodicOpMode"); + + HAL.reportUsage("OpMode", "PeriodicOpMode"); + } + + /** Called periodically while the opmode is selected on the DS (robot is disabled). */ + @Override + public void disabledPeriodic() {} + + /** + * Called when the opmode is de-selected on the DS. The object is not reused even if the same + * opmode is selected again (a new object will be created). + */ + public void close() {} + + /** + * Called a single time when the robot transitions from disabled to enabled. This is called prior + * to periodic() being called. + */ + public void start() {} + + /** Called periodically while the robot is enabled. */ + public abstract void periodic(); + + /** + * Called a single time when the robot transitions from enabled to disabled, or just before + * close() is called if a different opmode is selected while the robot is enabled. + */ + public void end() {} + + /** + * Return the system clock time in micrseconds for the start of the current periodic loop. This is + * in the same time base as Timer.getFPGATimestamp(), but is stable through a loop. It is updated + * at the beginning of every periodic callback (including the normal periodic loop). + * + * @return Robot running time in microseconds, as of the start of the current periodic function. + */ + public long getLoopStartTime() { + return m_loopStartTimeUs; + } + + /** + * Add a callback to run at a specific period. + * + *

This is scheduled on the same Notifier as periodic(), so periodic() and the callback run + * synchronously. Interactions between them are thread-safe. + * + * @param callback The callback to run. + * @param period The period at which to run the callback in seconds. + */ + public final void addPeriodic(Runnable callback, double period) { + m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0)); + } + + /** + * Add a callback to run at a specific period with a starting time offset. + * + *

This is scheduled on the same Notifier as periodic(), so periodic() and the callback run + * synchronously. Interactions between them are thread-safe. + * + * @param callback The callback to run. + * @param period The period at which to run the callback in seconds. + * @param offset The offset from the common starting time in seconds. This is useful for + * scheduling a callback in a different timeslot relative to PeriodicOpMode. + */ + public final void addPeriodic(Runnable callback, double period, double offset) { + m_callbacks.add( + new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6))); + } + + /** + * Add a callback to run at a specific period. + * + *

This is scheduled on the same Notifier as periodic(), so periodic() and the callback run + * synchronously. Interactions between them are thread-safe. + * + * @param callback The callback to run. + * @param period The period at which to run the callback. + */ + public final void addPeriodic(Runnable callback, Time period) { + addPeriodic(callback, period.in(Seconds)); + } + + /** + * Add a callback to run at a specific period with a starting time offset. + * + *

This is scheduled on the same Notifier as periodic(), so periodic() and the callback run + * synchronously. Interactions between them are thread-safe. + * + * @param callback The callback to run. + * @param period The period at which to run the callback. + * @param offset The offset from the common starting time. This is useful for scheduling a + * callback in a different timeslot relative to PeriodicOpMode. + */ + public final void addPeriodic(Runnable callback, Time period, Time offset) { + addPeriodic(callback, period.in(Seconds), offset.in(Seconds)); + } + + /** + * Gets time period between calls to Periodic() functions. + * + * @return The time period between calls to Periodic() functions. + */ + public double getPeriod() { + return m_period; + } + + /** Loop function. */ + protected void loopFunc() { + DriverStation.refreshData(); + DriverStation.refreshControlWordFromCache(m_word); + m_word.setOpModeId(m_opModeId); + DriverStationJNI.observeUserProgram(m_word.getNative()); + + if (!DriverStation.isEnabled() || DriverStation.getOpModeId() != m_opModeId) { + m_running = false; + return; + } + + m_watchdog.reset(); + periodic(); + m_watchdog.addEpoch("periodic()"); + + SmartDashboard.updateValues(); + m_watchdog.addEpoch("SmartDashboard.updateValues()"); + + // if (isSimulation()) { + // HAL.simPeriodicBefore(); + // simulationPeriodic(); + // HAL.simPeriodicAfter(); + // m_watchdog.addEpoch("simulationPeriodic()"); + // } + + m_watchdog.disable(); + + // Flush NetworkTables + NetworkTableInstance.getDefault().flushLocal(); + + // Warn on loop time overruns + if (m_watchdog.isExpired()) { + m_watchdog.printEpochs(); + } + } + + // implements OpMode interface + @Override + public final void opModeRun(long opModeId) { + m_opModeId = opModeId; + + start(); + + while (m_running) { + // We don't have to check there's an element in the queue first because + // there's always at least one (the constructor adds one). It's reenqueued + // at the end of the loop. + var callback = m_callbacks.poll(); + NotifierJNI.setNotifierAlarm(m_notifier, callback.expirationTime, 0, true, true); + + try { + WPIUtilJNI.waitForObject(m_notifier); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + break; + } + + long currentTime = RobotController.getFPGATime(); + m_loopStartTimeUs = RobotController.getFPGATime(); + + callback.func.run(); + + // Increment the expiration time by the number of full periods it's behind + // plus one to avoid rapid repeat fires from a large loop overrun. We + // assume currentTime ≥ expirationTime rather than checking for it since + // the callback wouldn't be running otherwise. + callback.expirationTime += + callback.period + + (currentTime - callback.expirationTime) / callback.period * callback.period; + m_callbacks.add(callback); + + // Process all other callbacks that are ready to run + while (m_callbacks.peek().expirationTime <= currentTime) { + callback = m_callbacks.poll(); + + callback.func.run(); + + callback.expirationTime += + callback.period + + (currentTime - callback.expirationTime) / callback.period * callback.period; + m_callbacks.add(callback); + } + } + end(); + } + + @Override + public final void opModeStop() { + NotifierJNI.destroyNotifier(m_notifier); + m_notifier = 0; + } + + @Override + public final void opModeClose() { + if (m_notifier != 0) { + NotifierJNI.destroyNotifier(m_notifier); + } + close(); + } + + /** Prints list of epochs added so far and their times. */ + public void printWatchdogEpochs() { + m_watchdog.printEpochs(); + } + + private void printLoopOverrunMessage() { + DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false); + } +} diff --git a/wpilibj/src/main/java/org/wpilib/opmode/Teleop.java b/wpilibj/src/main/java/org/wpilib/opmode/Teleop.java new file mode 100644 index 0000000000..3186384b32 --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/opmode/Teleop.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.opmode; + +import java.lang.annotation.Documented; +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** Annotation for automatic registration of teleoperated opmode classes. */ +@Retention(RetentionPolicy.RUNTIME) +@Target(ElementType.TYPE) +@Documented +public @interface Teleop { + /** + * Name. This is shown as the selection name in the Driver Station, and must be unique across all + * teleoperated opmodes in the project. If not specified, defaults to the name of the class. + * + * @return Name + */ + String name() default ""; + + /** + * Group. All opmodes with the same group are grouped together for selection. If not specified, + * defaults to ungrouped. + * + * @return Group + */ + String group() default ""; + + /** + * Extended description. Optional. + * + * @return Description + */ + String description() default ""; + + /** + * Text color. Optional. Supports all formats supported by {@link + * org.wpilib.util.Color#fromString(String)}. + * + * @return Text color + */ + String textColor() default ""; + + /** + * Text background color. Optional. Supports all formats supported by {@link + * org.wpilib.util.Color#fromString(String)}. + * + * @return Text background color + */ + String backgroundColor() default ""; +} diff --git a/wpilibj/src/main/java/org/wpilib/opmode/TestOpMode.java b/wpilibj/src/main/java/org/wpilib/opmode/TestOpMode.java new file mode 100644 index 0000000000..a8597b1e57 --- /dev/null +++ b/wpilibj/src/main/java/org/wpilib/opmode/TestOpMode.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.opmode; + +import java.lang.annotation.Documented; +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** Annotation for automatic registration of test opmode classes. */ +@Retention(RetentionPolicy.RUNTIME) +@Target(ElementType.TYPE) +@Documented +public @interface TestOpMode { + /** + * 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. + * + * @return Name + */ + String name() default ""; + + /** + * Group. All opmodes with the same group are grouped together for selection. If not specified, + * defaults to ungrouped. + * + * @return Group + */ + String group() default ""; + + /** + * Extended description. Optional. + * + * @return Description + */ + String description() default ""; + + /** + * Text color. Optional. Supports all formats supported by {@link + * org.wpilib.util.Color#fromString(String)}. + * + * @return Text color + */ + String textColor() default ""; + + /** + * Text background color. Optional. Supports all formats supported by {@link + * org.wpilib.util.Color#fromString(String)}. + * + * @return Text background color + */ + String backgroundColor() default ""; +} diff --git a/wpilibj/src/main/java/org/wpilib/simulation/DriverStationSim.java b/wpilibj/src/main/java/org/wpilib/simulation/DriverStationSim.java index 09a6f8cd0a..4a7eb780bf 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/DriverStationSim.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/DriverStationSim.java @@ -4,9 +4,12 @@ package org.wpilib.simulation; +import java.util.function.BiConsumer; import org.wpilib.driverstation.DriverStation; import org.wpilib.hardware.hal.AllianceStationID; import org.wpilib.hardware.hal.DriverStationJNI; +import org.wpilib.hardware.hal.OpModeOption; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.hardware.hal.simulation.DriverStationDataJNI; import org.wpilib.hardware.hal.simulation.NotifyCallback; import org.wpilib.util.WPIUtilJNI; @@ -49,64 +52,34 @@ public final class DriverStationSim { } /** - * Register a callback on whether the DS is in autonomous mode. + * Register a callback on DS robot mode changes. * - * @param callback the callback that will be called on autonomous mode entrance/exit + * @param callback the callback that will be called when the robot mode changes * @param initialNotify if true, the callback will be run on the initial value * @return the {@link CallbackStore} object associated with this callback. */ - public static CallbackStore registerAutonomousCallback( + public static CallbackStore registerRobotModeCallback( NotifyCallback callback, boolean initialNotify) { - int uid = DriverStationDataJNI.registerAutonomousCallback(callback, initialNotify); - return new CallbackStore(uid, DriverStationDataJNI::cancelAutonomousCallback); + int uid = DriverStationDataJNI.registerRobotModeCallback(callback, initialNotify); + return new CallbackStore(uid, DriverStationDataJNI::cancelRobotModeCallback); } /** - * Check if the DS is in autonomous. + * Gets the robot mode set by the DS. * - * @return true if autonomous + * @return robot mode */ - public static boolean getAutonomous() { - return DriverStationDataJNI.getAutonomous(); + public static RobotMode getRobotMode() { + return DriverStationDataJNI.getRobotMode(); } /** - * Change whether the DS is in autonomous. + * Change the robot mode set by the DS. * - * @param autonomous the new value + * @param mode the new value */ - public static void setAutonomous(boolean autonomous) { - DriverStationDataJNI.setAutonomous(autonomous); - } - - /** - * Register a callback on whether the DS is in test mode. - * - * @param callback the callback that will be called whenever the test mode is entered or left - * @param initialNotify if true, the callback will be run on the initial value - * @return the {@link CallbackStore} object associated with this callback. - */ - public static CallbackStore registerTestCallback(NotifyCallback callback, boolean initialNotify) { - int uid = DriverStationDataJNI.registerTestCallback(callback, initialNotify); - return new CallbackStore(uid, DriverStationDataJNI::cancelTestCallback); - } - - /** - * Check if the DS is in test. - * - * @return true if test - */ - public static boolean getTest() { - return DriverStationDataJNI.getTest(); - } - - /** - * Change whether the DS is in test. - * - * @param test the new value - */ - public static void setTest(boolean test) { - DriverStationDataJNI.setTest(test); + public static void setRobotMode(RobotMode mode) { + DriverStationDataJNI.setRobotMode(mode); } /** @@ -283,6 +256,59 @@ public final class DriverStationSim { DriverStationDataJNI.setMatchTime(matchTime); } + /** + * Register a callback on opmode changes. + * + * @param callback the callback that will be called when the opmode changes + * @param initialNotify if true, the callback will be run on the initial value + * @return the {@link CallbackStore} object associated with this callback. + */ + public static CallbackStore registerOpModeCallback( + NotifyCallback callback, boolean initialNotify) { + int uid = DriverStationDataJNI.registerOpModeCallback(callback, initialNotify); + return new CallbackStore(uid, DriverStationDataJNI::cancelOpModeCallback); + } + + /** + * Gets the opmode. + * + * @return opmode + */ + public static long getOpMode() { + return DriverStationDataJNI.getOpMode(); + } + + /** + * Change the opmode. + * + * @param opmode the new value + */ + public static void setOpMode(long opmode) { + DriverStationDataJNI.setOpMode(opmode); + } + + /** + * Register a callback on opmode options changes. + * + * @param callback the callback that will be called when the list of opmodes changes + * @param initialNotify if true, the callback will be run on the initial value + * @return the {@link CallbackStore} object associated with this callback. + */ + public static CallbackStore registerOpModeOptionsCallback( + BiConsumer callback, boolean initialNotify) { + int uid = DriverStationDataJNI.registerOpModeOptionsCallback(callback, initialNotify); + return new CallbackStore(uid, DriverStationDataJNI::cancelOpModeOptionsCallback); + } + + /** + * Gets the list of opmode options. + * + * @return opmodes list + */ + public static OpModeOption[] getOpModeOptions() { + return DriverStationDataJNI.getOpModeOptions(); + } + /** Updates DriverStation data so that new values are visible to the user program. */ public static void notifyNewData() { int handle = WPIUtilJNI.createEvent(false, false); diff --git a/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java b/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java index 5ebcccbe1f..ba3c51d556 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java @@ -4,6 +4,7 @@ package org.wpilib.simulation; +import org.wpilib.hardware.hal.ControlWord; import org.wpilib.hardware.hal.simulation.SimulatorJNI; /** Simulation hooks. */ @@ -42,6 +43,24 @@ public final class SimHooks { return SimulatorJNI.getProgramStarted(); } + /** + * Sets the user program state (control word). + * + * @param controlWord control word + */ + public static void setProgramState(ControlWord controlWord) { + SimulatorJNI.setProgramState(controlWord.getNative()); + } + + /** + * Gets the user program state (control word). + * + * @param controlWord control word (output) + */ + public static void getProgramState(ControlWord controlWord) { + SimulatorJNI.getProgramState(controlWord); + } + /** Restart the simulator time. */ public static void restartTiming() { SimulatorJNI.restartTiming(); diff --git a/wpilibj/src/test/java/org/wpilib/MockHardwareExtension.java b/wpilibj/src/test/java/org/wpilib/MockHardwareExtension.java index e23ac069e8..2bea975b53 100644 --- a/wpilibj/src/test/java/org/wpilib/MockHardwareExtension.java +++ b/wpilibj/src/test/java/org/wpilib/MockHardwareExtension.java @@ -8,6 +8,7 @@ import org.junit.jupiter.api.extension.BeforeAllCallback; import org.junit.jupiter.api.extension.ExtensionContext; import org.junit.jupiter.api.extension.ExtensionContext.Namespace; import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.simulation.DriverStationSim; public final class MockHardwareExtension implements BeforeAllCallback { @@ -31,8 +32,7 @@ public final class MockHardwareExtension implements BeforeAllCallback { private void initializeHardware() { HAL.initialize(500, 0); DriverStationSim.setDsAttached(true); - DriverStationSim.setAutonomous(false); DriverStationSim.setEnabled(true); - DriverStationSim.setTest(true); + DriverStationSim.setRobotMode(RobotMode.TEST); } } diff --git a/wpilibj/src/test/java/org/wpilib/framework/TimedRobotTest.java b/wpilibj/src/test/java/org/wpilib/framework/TimedRobotTest.java index e5daa21d62..414fabdce4 100644 --- a/wpilibj/src/test/java/org/wpilib/framework/TimedRobotTest.java +++ b/wpilibj/src/test/java/org/wpilib/framework/TimedRobotTest.java @@ -11,6 +11,7 @@ import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.junit.jupiter.api.parallel.ResourceLock; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.simulation.DriverStationSim; import org.wpilib.simulation.SimHooks; @@ -218,8 +219,7 @@ class TimedRobotTest { SimHooks.waitForProgramStart(); DriverStationSim.setEnabled(true); - DriverStationSim.setAutonomous(true); - DriverStationSim.setTest(false); + DriverStationSim.setRobotMode(RobotMode.AUTONOMOUS); DriverStationSim.notifyNewData(); assertEquals(1, robot.m_simulationInitCount.get()); @@ -300,8 +300,7 @@ class TimedRobotTest { SimHooks.waitForProgramStart(); DriverStationSim.setEnabled(true); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(false); + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); DriverStationSim.notifyNewData(); assertEquals(1, robot.m_simulationInitCount.get()); @@ -382,8 +381,7 @@ class TimedRobotTest { SimHooks.waitForProgramStart(); DriverStationSim.setEnabled(true); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(true); + DriverStationSim.setRobotMode(RobotMode.TEST); DriverStationSim.notifyNewData(); assertEquals(1, robot.m_simulationInitCount.get()); @@ -445,8 +443,6 @@ class TimedRobotTest { assertEquals(0, robot.m_testExitCount.get()); DriverStationSim.setEnabled(false); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(false); DriverStationSim.notifyNewData(); SimHooks.stepTiming(0.02); @@ -490,8 +486,6 @@ class TimedRobotTest { // Start in disabled DriverStationSim.setEnabled(false); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(false); DriverStationSim.notifyNewData(); assertEquals(0, robot.m_disabledInitCount.get()); @@ -518,8 +512,7 @@ class TimedRobotTest { // Transition to autonomous DriverStationSim.setEnabled(true); - DriverStationSim.setAutonomous(true); - DriverStationSim.setTest(false); + DriverStationSim.setRobotMode(RobotMode.AUTONOMOUS); DriverStationSim.notifyNewData(); SimHooks.stepTiming(kPeriod); @@ -536,8 +529,7 @@ class TimedRobotTest { // Transition to teleop DriverStationSim.setEnabled(true); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(false); + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); DriverStationSim.notifyNewData(); SimHooks.stepTiming(kPeriod); @@ -554,8 +546,7 @@ class TimedRobotTest { // Transition to test DriverStationSim.setEnabled(true); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(true); + DriverStationSim.setRobotMode(RobotMode.TEST); DriverStationSim.notifyNewData(); SimHooks.stepTiming(kPeriod); @@ -572,8 +563,6 @@ class TimedRobotTest { // Transition to disabled DriverStationSim.setEnabled(false); - DriverStationSim.setAutonomous(false); - DriverStationSim.setTest(false); DriverStationSim.notifyNewData(); SimHooks.stepTiming(kPeriod); diff --git a/wpilibj/src/test/java/org/wpilib/simulation/DriverStationSimTest.java b/wpilibj/src/test/java/org/wpilib/simulation/DriverStationSimTest.java index eb9647fc3c..0b62322d12 100644 --- a/wpilibj/src/test/java/org/wpilib/simulation/DriverStationSimTest.java +++ b/wpilibj/src/test/java/org/wpilib/simulation/DriverStationSimTest.java @@ -14,6 +14,7 @@ import org.junit.jupiter.params.provider.EnumSource; import org.wpilib.driverstation.DriverStation; import org.wpilib.hardware.hal.AllianceStationID; import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.simulation.testutils.BooleanCallback; import org.wpilib.simulation.testutils.DoubleCallback; import org.wpilib.simulation.testutils.EnumCallback; @@ -42,14 +43,15 @@ class DriverStationSimTest { DriverStationSim.resetData(); assertFalse(DriverStation.isAutonomous()); - BooleanCallback callback = new BooleanCallback(); - try (CallbackStore cb = DriverStationSim.registerAutonomousCallback(callback, false)) { - DriverStationSim.setAutonomous(true); + EnumCallback callback = new EnumCallback(); + try (CallbackStore cb = DriverStationSim.registerRobotModeCallback(callback, false)) { + DriverStationSim.setRobotMode(RobotMode.AUTONOMOUS); DriverStationSim.notifyNewData(); - assertTrue(DriverStationSim.getAutonomous()); + assertEquals(RobotMode.AUTONOMOUS, DriverStationSim.getRobotMode()); assertTrue(DriverStation.isAutonomous()); + assertEquals(RobotMode.AUTONOMOUS, DriverStation.getRobotMode()); assertTrue(callback.wasTriggered()); - assertTrue(callback.getSetValue()); + assertEquals(RobotMode.AUTONOMOUS.getValue(), callback.getSetValue()); } } @@ -59,14 +61,15 @@ class DriverStationSimTest { DriverStationSim.resetData(); assertFalse(DriverStation.isTest()); - BooleanCallback callback = new BooleanCallback(); - try (CallbackStore cb = DriverStationSim.registerTestCallback(callback, false)) { - DriverStationSim.setTest(true); + EnumCallback callback = new EnumCallback(); + try (CallbackStore cb = DriverStationSim.registerRobotModeCallback(callback, false)) { + DriverStationSim.setRobotMode(RobotMode.TEST); DriverStationSim.notifyNewData(); - assertTrue(DriverStationSim.getTest()); + assertEquals(RobotMode.TEST, DriverStationSim.getRobotMode()); assertTrue(DriverStation.isTest()); + assertEquals(RobotMode.TEST, DriverStation.getRobotMode()); assertTrue(callback.wasTriggered()); - assertTrue(callback.getSetValue()); + assertEquals(RobotMode.TEST.getValue(), callback.getSetValue()); } } diff --git a/wpilibjExamples/example_projects.bzl b/wpilibjExamples/example_projects.bzl index ba3971e388..e9bf6b2436 100644 --- a/wpilibjExamples/example_projects.bzl +++ b/wpilibjExamples/example_projects.bzl @@ -86,6 +86,7 @@ TEMPLATES_FOLDERS = [ "commandv2", "commandv2skeleton", "educational", + "opmode", "robotbaseskeleton", "romicommandv2", "romieducational", diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/educational/EducationalRobot.java b/wpilibjExamples/src/main/java/org/wpilib/templates/educational/EducationalRobot.java index f23f0c7b77..71f094f82c 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/templates/educational/EducationalRobot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/educational/EducationalRobot.java @@ -6,7 +6,9 @@ package org.wpilib.templates.educational; import org.wpilib.driverstation.DriverStation; import org.wpilib.framework.RobotBase; +import org.wpilib.hardware.hal.ControlWord; import org.wpilib.hardware.hal.DriverStationJNI; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.internal.DriverStationModeThread; import org.wpilib.util.WPIUtilJNI; @@ -34,7 +36,14 @@ public class EducationalRobot extends RobotBase { @Override public void startCompetition() { - DriverStationModeThread modeThread = new DriverStationModeThread(); + // Create an opmode per robot mode + DriverStation.addOpMode(RobotMode.AUTONOMOUS, "Auto"); + DriverStation.addOpMode(RobotMode.TELEOPERATED, "Teleop"); + DriverStation.addOpMode(RobotMode.TEST, "Test"); + DriverStation.publishOpModes(); + + final ControlWord word = new ControlWord(); + DriverStationModeThread modeThread = new DriverStationModeThread(word); int event = WPIUtilJNI.createEvent(false, false); @@ -44,10 +53,10 @@ public class EducationalRobot extends RobotBase { DriverStationJNI.observeUserProgramStarting(); while (!Thread.currentThread().isInterrupted() && !m_exit) { + DriverStation.refreshControlWordFromCache(word); + modeThread.inControl(word); if (isDisabled()) { - modeThread.inDisabled(true); disabled(); - modeThread.inDisabled(false); while (isDisabled()) { try { WPIUtilJNI.waitForObject(event); @@ -56,9 +65,7 @@ public class EducationalRobot extends RobotBase { } } } else if (isAutonomous()) { - modeThread.inAutonomous(true); autonomous(); - modeThread.inAutonomous(false); while (isAutonomousEnabled()) { try { WPIUtilJNI.waitForObject(event); @@ -67,9 +74,7 @@ public class EducationalRobot extends RobotBase { } } } else if (isTest()) { - modeThread.inTest(true); test(); - modeThread.inTest(false); while (isTest() && isEnabled()) { try { WPIUtilJNI.waitForObject(event); @@ -78,9 +83,7 @@ public class EducationalRobot extends RobotBase { } } } else { - modeThread.inTeleop(true); teleop(); - modeThread.inTeleop(false); while (isTeleopEnabled()) { try { WPIUtilJNI.waitForObject(event); diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/Main.java b/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/Main.java new file mode 100644 index 0000000000..04de054082 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.templates.opmode; + +import org.wpilib.framework.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/Robot.java new file mode 100644 index 0000000000..589246dc4e --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/Robot.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.templates.opmode; + +import org.wpilib.framework.OpModeRobot; + +/** + * The methods in this class are called automatically as described in the OpModeRobot documentation. + * OpMode classes anywhere in the package (or sub-packages) where this class is located are + * automatically registered to display in the Driver Station. If you change the name of this class + * or the package after creating this project, you must also update the Main.java file in the + * project. + */ +public class Robot extends OpModeRobot { + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + public Robot() {} + + /** This function is called exactly once when the DS first connects. */ + @Override + public void driverStationConnected() {} + + /** + * This function is called periodically anytime when no opmode is selected, including when the + * Driver Station is disconnected. + */ + @Override + public void nonePeriodic() {} +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyAuto.java b/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyAuto.java new file mode 100644 index 0000000000..15c2d76687 --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyAuto.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.templates.opmode.opmode; + +import org.wpilib.opmode.Autonomous; +import org.wpilib.opmode.PeriodicOpMode; +import org.wpilib.templates.opmode.Robot; + +@Autonomous(name = "My Auto", group = "Group 1") +public class MyAuto extends PeriodicOpMode { + private final Robot m_robot; + + /** The Robot instance is passed into the opmode via the constructor. */ + public MyAuto(Robot robot) { + m_robot = robot; + /* + * Can call super(period) to set a different periodic time interval. + * + * Additional periodic methods may be configured with addPeriodic(). + */ + } + + @Override + public void periodic() { + // Put custom auto code here + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyTeleop.java b/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyTeleop.java new file mode 100644 index 0000000000..1a91977adc --- /dev/null +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyTeleop.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.templates.opmode.opmode; + +import org.wpilib.opmode.PeriodicOpMode; +import org.wpilib.opmode.Teleop; +import org.wpilib.templates.opmode.Robot; + +@Teleop +public class MyTeleop extends PeriodicOpMode { + private final Robot m_robot; + + /** The Robot instance is passed into the opmode via the constructor. */ + public MyTeleop(Robot robot) { + m_robot = robot; + } + + @Override + public void disabledPeriodic() { + /* Called periodically (on every DS packet) while the robot is disabled. */ + } + + @Override + public void start() { + /* Called once when the robot is enabled. */ + } + + @Override + public void periodic() { + /* Called periodically (set time interval) while the robot is enabled. */ + } + + @Override + public void end() { + /* Called when the robot is disabled (after previously being enabled). */ + } + + @Override + public void close() { + /* Called when the opmode is de-selected / no additional methods will be called. */ + } +} diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/robotbaseskeleton/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/templates/robotbaseskeleton/Robot.java index 817f3b9189..a6618a914d 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/templates/robotbaseskeleton/Robot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/robotbaseskeleton/Robot.java @@ -6,7 +6,9 @@ package org.wpilib.templates.robotbaseskeleton; import org.wpilib.driverstation.DriverStation; import org.wpilib.framework.RobotBase; +import org.wpilib.hardware.hal.ControlWord; import org.wpilib.hardware.hal.DriverStationJNI; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.internal.DriverStationModeThread; import org.wpilib.util.WPIUtilJNI; @@ -29,7 +31,14 @@ public class Robot extends RobotBase { @Override public void startCompetition() { - DriverStationModeThread modeThread = new DriverStationModeThread(); + // Create an opmode per robot mode + DriverStation.addOpMode(RobotMode.AUTONOMOUS, "Auto"); + DriverStation.addOpMode(RobotMode.TELEOPERATED, "Teleop"); + DriverStation.addOpMode(RobotMode.TEST, "Test"); + DriverStation.publishOpModes(); + + final ControlWord word = new ControlWord(); + DriverStationModeThread modeThread = new DriverStationModeThread(word); int event = WPIUtilJNI.createEvent(false, false); @@ -39,10 +48,10 @@ public class Robot extends RobotBase { DriverStationJNI.observeUserProgramStarting(); while (!Thread.currentThread().isInterrupted() && !m_exit) { + DriverStation.refreshControlWordFromCache(word); + modeThread.inControl(word); if (isDisabled()) { - modeThread.inDisabled(true); disabled(); - modeThread.inDisabled(false); while (isDisabled()) { try { WPIUtilJNI.waitForObject(event); @@ -51,9 +60,7 @@ public class Robot extends RobotBase { } } } else if (isAutonomous()) { - modeThread.inAutonomous(true); autonomous(); - modeThread.inAutonomous(false); while (isAutonomousEnabled()) { try { WPIUtilJNI.waitForObject(event); @@ -62,9 +69,7 @@ public class Robot extends RobotBase { } } } else if (isTest()) { - modeThread.inTest(true); test(); - modeThread.inTest(false); while (isTest() && isEnabled()) { try { WPIUtilJNI.waitForObject(event); @@ -73,9 +78,7 @@ public class Robot extends RobotBase { } } } else { - modeThread.inTeleop(true); teleop(); - modeThread.inTeleop(false); while (isTeleopEnabled()) { try { WPIUtilJNI.waitForObject(event); diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/romieducational/EducationalRobot.java b/wpilibjExamples/src/main/java/org/wpilib/templates/romieducational/EducationalRobot.java index e311c2ceb2..b3d59d41e3 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/templates/romieducational/EducationalRobot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/romieducational/EducationalRobot.java @@ -6,7 +6,9 @@ package org.wpilib.templates.romieducational; import org.wpilib.driverstation.DriverStation; import org.wpilib.framework.RobotBase; +import org.wpilib.hardware.hal.ControlWord; import org.wpilib.hardware.hal.DriverStationJNI; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.internal.DriverStationModeThread; import org.wpilib.util.WPIUtilJNI; @@ -34,7 +36,14 @@ public class EducationalRobot extends RobotBase { @Override public void startCompetition() { - DriverStationModeThread modeThread = new DriverStationModeThread(); + // Create an opmode per robot mode + DriverStation.addOpMode(RobotMode.AUTONOMOUS, "Auto"); + DriverStation.addOpMode(RobotMode.TELEOPERATED, "Teleop"); + DriverStation.addOpMode(RobotMode.TEST, "Test"); + DriverStation.publishOpModes(); + + final ControlWord word = new ControlWord(); + DriverStationModeThread modeThread = new DriverStationModeThread(word); int event = WPIUtilJNI.createEvent(false, false); @@ -44,10 +53,10 @@ public class EducationalRobot extends RobotBase { DriverStationJNI.observeUserProgramStarting(); while (!Thread.currentThread().isInterrupted() && !m_exit) { + DriverStation.refreshControlWordFromCache(word); + modeThread.inControl(word); if (isDisabled()) { - modeThread.inDisabled(true); disabled(); - modeThread.inDisabled(false); while (isDisabled()) { try { WPIUtilJNI.waitForObject(event); @@ -56,9 +65,7 @@ public class EducationalRobot extends RobotBase { } } } else if (isAutonomous()) { - modeThread.inAutonomous(true); autonomous(); - modeThread.inAutonomous(false); while (isAutonomousEnabled()) { try { WPIUtilJNI.waitForObject(event); @@ -67,9 +74,7 @@ public class EducationalRobot extends RobotBase { } } } else if (isTest()) { - modeThread.inTest(true); test(); - modeThread.inTest(false); while (isTest() && isEnabled()) { try { WPIUtilJNI.waitForObject(event); @@ -78,9 +83,7 @@ public class EducationalRobot extends RobotBase { } } } else { - modeThread.inTeleop(true); teleop(); - modeThread.inTeleop(false); while (isTeleopEnabled()) { try { WPIUtilJNI.waitForObject(event); diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/templates.json b/wpilibjExamples/src/main/java/org/wpilib/templates/templates.json index d8aa483399..6d6b4c606d 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/templates/templates.json +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/templates.json @@ -22,6 +22,17 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "OpMode Robot", + "description": "OpMode style, with explanatory comments and example code.", + "tags": [ + "OpMode" + ], + "foldername": "opmode", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "Timed Robot", "description": "Timed style, with explanatory comments and example code.", diff --git a/wpilibjExamples/src/main/java/org/wpilib/templates/xrpeducational/EducationalRobot.java b/wpilibjExamples/src/main/java/org/wpilib/templates/xrpeducational/EducationalRobot.java index 74e168d159..d8135d6bdd 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/templates/xrpeducational/EducationalRobot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/templates/xrpeducational/EducationalRobot.java @@ -6,7 +6,9 @@ package org.wpilib.templates.xrpeducational; import org.wpilib.driverstation.DriverStation; import org.wpilib.framework.RobotBase; +import org.wpilib.hardware.hal.ControlWord; import org.wpilib.hardware.hal.DriverStationJNI; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.internal.DriverStationModeThread; import org.wpilib.util.WPIUtilJNI; @@ -34,7 +36,14 @@ public class EducationalRobot extends RobotBase { @Override public void startCompetition() { - DriverStationModeThread modeThread = new DriverStationModeThread(); + // Create an opmode per robot mode + DriverStation.addOpMode(RobotMode.AUTONOMOUS, "Auto"); + DriverStation.addOpMode(RobotMode.TELEOPERATED, "Teleop"); + DriverStation.addOpMode(RobotMode.TEST, "Test"); + DriverStation.publishOpModes(); + + final ControlWord word = new ControlWord(); + DriverStationModeThread modeThread = new DriverStationModeThread(word); int event = WPIUtilJNI.createEvent(false, false); @@ -44,10 +53,10 @@ public class EducationalRobot extends RobotBase { DriverStationJNI.observeUserProgramStarting(); while (!Thread.currentThread().isInterrupted() && !m_exit) { + DriverStation.refreshControlWordFromCache(word); + modeThread.inControl(word); if (isDisabled()) { - modeThread.inDisabled(true); disabled(); - modeThread.inDisabled(false); while (isDisabled()) { try { WPIUtilJNI.waitForObject(event); @@ -56,9 +65,7 @@ public class EducationalRobot extends RobotBase { } } } else if (isAutonomous()) { - modeThread.inAutonomous(true); autonomous(); - modeThread.inAutonomous(false); while (isAutonomousEnabled()) { try { WPIUtilJNI.waitForObject(event); @@ -67,9 +74,7 @@ public class EducationalRobot extends RobotBase { } } } else if (isTest()) { - modeThread.inTest(true); test(); - modeThread.inTest(false); while (isTest() && isEnabled()) { try { WPIUtilJNI.waitForObject(event); @@ -78,9 +83,7 @@ public class EducationalRobot extends RobotBase { } } } else { - modeThread.inTeleop(true); teleop(); - modeThread.inTeleop(false); while (isTeleopEnabled()) { try { WPIUtilJNI.waitForObject(event); diff --git a/wpilibjExamples/src/test/java/org/wpilib/examples/armsimulation/ArmSimulationTest.java b/wpilibjExamples/src/test/java/org/wpilib/examples/armsimulation/ArmSimulationTest.java index d21204b316..4343cdc2ba 100644 --- a/wpilibjExamples/src/test/java/org/wpilib/examples/armsimulation/ArmSimulationTest.java +++ b/wpilibjExamples/src/test/java/org/wpilib/examples/armsimulation/ArmSimulationTest.java @@ -13,6 +13,7 @@ import org.junit.jupiter.api.parallel.ResourceLock; import org.junit.jupiter.params.ParameterizedTest; import org.junit.jupiter.params.provider.ValueSource; import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.math.util.Units; import org.wpilib.simulation.DriverStationSim; import org.wpilib.simulation.EncoderSim; @@ -75,7 +76,7 @@ class ArmSimulationTest { assertEquals(setpoint, Preferences.getDouble(Constants.kArmPositionKey, Double.NaN)); // teleop init { - DriverStationSim.setAutonomous(false); + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData(); @@ -135,7 +136,6 @@ class ArmSimulationTest { { // Disable - DriverStationSim.setAutonomous(false); DriverStationSim.setEnabled(false); DriverStationSim.notifyNewData(); diff --git a/wpilibjExamples/src/test/java/org/wpilib/examples/digitalcommunication/DigitalCommunicationTest.java b/wpilibjExamples/src/test/java/org/wpilib/examples/digitalcommunication/DigitalCommunicationTest.java index ec49d2877b..8598f4a4b9 100644 --- a/wpilibjExamples/src/test/java/org/wpilib/examples/digitalcommunication/DigitalCommunicationTest.java +++ b/wpilibjExamples/src/test/java/org/wpilib/examples/digitalcommunication/DigitalCommunicationTest.java @@ -16,6 +16,7 @@ import org.junit.jupiter.params.provider.EnumSource; import org.junit.jupiter.params.provider.ValueSource; import org.wpilib.hardware.hal.AllianceStationID; import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.simulation.DIOSim; import org.wpilib.simulation.DriverStationSim; import org.wpilib.simulation.SimHooks; @@ -88,7 +89,7 @@ class DigitalCommunicationTest { @ValueSource(booleans = {true, false}) @ParameterizedTest(name = "autonomous[{index}]: {0}") void autonomousTest(boolean autonomous) { - DriverStationSim.setAutonomous(autonomous); + DriverStationSim.setRobotMode(autonomous ? RobotMode.AUTONOMOUS : RobotMode.TELEOPERATED); DriverStationSim.notifyNewData(); assertTrue(m_autonomousOutput.getInitialized()); diff --git a/wpilibjExamples/src/test/java/org/wpilib/examples/elevatorsimulation/ElevatorSimulationTest.java b/wpilibjExamples/src/test/java/org/wpilib/examples/elevatorsimulation/ElevatorSimulationTest.java index 1eb97a86f2..6c5237c139 100644 --- a/wpilibjExamples/src/test/java/org/wpilib/examples/elevatorsimulation/ElevatorSimulationTest.java +++ b/wpilibjExamples/src/test/java/org/wpilib/examples/elevatorsimulation/ElevatorSimulationTest.java @@ -12,6 +12,7 @@ import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.junit.jupiter.api.parallel.ResourceLock; import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.simulation.DriverStationSim; import org.wpilib.simulation.EncoderSim; import org.wpilib.simulation.JoystickSim; @@ -64,7 +65,7 @@ class ElevatorSimulationTest { void teleopTest() { // teleop init { - DriverStationSim.setAutonomous(false); + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData(); @@ -124,7 +125,6 @@ class ElevatorSimulationTest { { // Disable - DriverStationSim.setAutonomous(false); DriverStationSim.setEnabled(false); DriverStationSim.notifyNewData(); diff --git a/wpilibjExamples/src/test/java/org/wpilib/examples/i2ccommunication/I2CCommunicationTest.java b/wpilibjExamples/src/test/java/org/wpilib/examples/i2ccommunication/I2CCommunicationTest.java index 2581cc1b82..f7730bb9f2 100644 --- a/wpilibjExamples/src/test/java/org/wpilib/examples/i2ccommunication/I2CCommunicationTest.java +++ b/wpilibjExamples/src/test/java/org/wpilib/examples/i2ccommunication/I2CCommunicationTest.java @@ -19,6 +19,7 @@ import org.junit.jupiter.params.provider.ValueSource; import org.wpilib.driverstation.DriverStation; import org.wpilib.hardware.hal.AllianceStationID; import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.simulation.CallbackStore; import org.wpilib.simulation.DriverStationSim; import org.wpilib.simulation.I2CSim; @@ -100,7 +101,7 @@ class I2CCommunicationTest { @ValueSource(booleans = {true, false}) @ParameterizedTest(name = "autonomous[{index}]: {0}") void autonomousTest(boolean autonomous) { - DriverStationSim.setAutonomous(autonomous); + DriverStationSim.setRobotMode(autonomous ? RobotMode.AUTONOMOUS : RobotMode.TELEOPERATED); DriverStationSim.notifyNewData(); assertTrue(m_i2c.getInitialized()); diff --git a/wpilibjExamples/src/test/java/org/wpilib/examples/potentiometerpid/PotentiometerPIDTest.java b/wpilibjExamples/src/test/java/org/wpilib/examples/potentiometerpid/PotentiometerPIDTest.java index 1d362fc1f1..c8ecab7b60 100644 --- a/wpilibjExamples/src/test/java/org/wpilib/examples/potentiometerpid/PotentiometerPIDTest.java +++ b/wpilibjExamples/src/test/java/org/wpilib/examples/potentiometerpid/PotentiometerPIDTest.java @@ -13,6 +13,7 @@ import org.junit.jupiter.api.Test; import org.junit.jupiter.api.parallel.ResourceLock; import org.wpilib.hardware.hal.HAL; import org.wpilib.hardware.hal.HAL.SimPeriodicBeforeCallback; +import org.wpilib.hardware.hal.RobotMode; import org.wpilib.math.system.plant.DCMotor; import org.wpilib.math.util.Units; import org.wpilib.simulation.AnalogInputSim; @@ -100,7 +101,7 @@ class PotentiometerPIDTest { void teleopTest() { // teleop init { - DriverStationSim.setAutonomous(false); + DriverStationSim.setRobotMode(RobotMode.TELEOPERATED); DriverStationSim.setEnabled(true); DriverStationSim.notifyNewData();