diff --git a/.github/actions/pregen/action.yml b/.github/actions/pregen/action.yml index 1346a79696..e05e19e1ba 100644 --- a/.github/actions/pregen/action.yml +++ b/.github/actions/pregen/action.yml @@ -53,6 +53,11 @@ runs: ./wpimath/generate_quickbuf.py --quickbuf_plugin protoc-gen-quickbuf-1.3.3-linux-x86_64.exe shell: bash + - name: Regenerate Commands v3 + run: | + ./commandsv3/generate_files.py --quickbuf_plugin protoc-gen-quickbuf-1.3.3-linux-x86_64.exe + shell: bash + - name: Regenerate wpiunits run: ./wpiunits/generate_units.py shell: bash diff --git a/BUILD.bazel b/BUILD.bazel index dbd1720ce7..157474099b 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -51,6 +51,7 @@ write_source_files( "//wpilibj:write_wpilibj", "//wpilibjExamples:write_example_project_list", "//wpilibNewCommands:write_wpilib_new_commands", + "//commandsv3:write_commandsv3", "//wpimath:write_wpimath", "//wpiunits:write_wpiunits", "//wpiutil:write_wpiutil", @@ -100,6 +101,7 @@ publish_all( "//wpigui:wpigui-cpp_publish.publish", "//wpilibNewCommands:wpilibNewCommands-cpp_publish.publish", "//wpilibNewCommands:wpilibNewCommands-java_publish.publish", + "//commandsv3:commandsv3-java_publish.publish", "//wpilibc:wpilibc-cpp_publish.publish", "//wpilibcExamples:commands_publish.publish", "//wpilibcExamples:examples_publish.publish", diff --git a/CMakeLists.txt b/CMakeLists.txt index 71fce23497..6d6d4c80c8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -346,12 +346,14 @@ endif() if(WITH_WPILIB) set(APRILTAG_DEP_REPLACE "find_dependency(apriltag)") + set(COMMANDSV3_DEP_REPLACE "find_dependency(commandsv3)") set(WPILIBC_DEP_REPLACE "find_dependency(wpilibc)") set(WPILIBJ_DEP_REPLACE "find_dependency(wpilibj)") set(WPILIBNEWCOMMANDS_DEP_REPLACE "find_dependency(wpilibNewCommands)") add_subdirectory(apriltag) add_subdirectory(wpilibj) add_subdirectory(wpilibc) + add_subdirectory(commandsv3) # must be after wpilibj add_subdirectory(wpilibNewCommands) add_subdirectory(romiVendordep) add_subdirectory(xrpVendordep) diff --git a/README-CMake.md b/README-CMake.md index eaa954d2d1..a91885f19a 100644 --- a/README-CMake.md +++ b/README-CMake.md @@ -5,6 +5,7 @@ WPILib is normally built with Gradle, however for some systems, such as Linux ba ## Libraries that get built * apriltag * cameraserver +* commandsv3 * cscore * fieldImages * hal (simulation HAL only) diff --git a/commandsv3/.styleguide b/commandsv3/.styleguide new file mode 100644 index 0000000000..0f2122eeb7 --- /dev/null +++ b/commandsv3/.styleguide @@ -0,0 +1,12 @@ +modifiableFileExclude { + \.patch$ +} + +generatedFileExclude { + src/generated/main/java/org/wpilib/commands3/button/ + src/generated/main/java/org/wpilib/commands3/proto/ +} + +repoRootNameOverride { + commandsv3 +} diff --git a/commandsv3/BUILD.bazel b/commandsv3/BUILD.bazel new file mode 100644 index 0000000000..2916630565 --- /dev/null +++ b/commandsv3/BUILD.bazel @@ -0,0 +1,92 @@ +load("@allwpilib_pip_deps//:requirements.bzl", "requirement") +load("@aspect_bazel_lib//lib:write_source_files.bzl", "write_source_files") +load("@rules_java//java:defs.bzl", "java_binary") +load("@rules_python//python:defs.bzl", "py_binary") +load("//commandsv3:generate.bzl", "generate_commandsv3") +load("//shared/bazel/rules:java_rules.bzl", "wpilib_java_junit5_test", "wpilib_java_library") + +py_binary( + name = "generate_files", + srcs = ["generate_files.py"], + target_compatible_with = select({ + "@rules_bzlmodrio_toolchains//constraints/is_systemcore:systemcore": ["@platforms//:incompatible"], + "//conditions:default": [], + }), + deps = [requirement("jinja2")], +) + +filegroup( + name = "templates", + srcs = glob(["src/generate/main/**"]) + [ + "//wpilibj:hid_schema", + ], +) + +generate_commandsv3( + name = "generate_commandsv3", +) + +write_source_files( + name = "write_commandsv3", + files = { + "src/generated": ":generate_commandsv3", + }, + suggested_update_target = "//:write_all", + tags = ["pregeneration"], + visibility = ["//visibility:public"], +) + +filegroup( + name = "generated_java", + srcs = glob(["src/generated/main/java/**/*.java"]), +) + +wpilib_java_library( + name = "commandsv3-java", + srcs = glob(["src/main/java/**/*.java"]) + [":generated_java"], + exported_plugins = ["//javacPlugin:plugin"], + maven_artifact_name = "commands3-java", + maven_group_id = "org.wpilib.commands3", + plugins = ["//javacPlugin:plugin"], + visibility = ["//visibility:public"], + deps = [ + "//cscore:cscore-java", + "//hal:hal-java", + "//ntcore:ntcore-java", + "//wpiannotations", + "//wpilibj:wpilibj-java", + "//wpimath:wpimath-java", + "//wpinet:wpinet-java", + "//wpiunits:wpiunits-java", + "//wpiutil:wpiutil-java", + "@maven//:us_hebi_quickbuf_quickbuf_runtime", + ], +) + +wpilib_java_junit5_test( + name = "commandsv3-java-test", + srcs = glob(["**/*.java"]), + deps = [ + ":commandsv3-java", + "//hal:hal-java", + "//ntcore:ntcore-java", + "//wpiannotations", + "//wpilibj:wpilibj-java", + "//wpimath:wpimath-java", + "//wpiunits:wpiunits-java", + "//wpiutil:wpiutil-java", + "@maven//:us_hebi_quickbuf_quickbuf_runtime", + ], +) + +java_binary( + name = "DevMain-Java", + srcs = ["src/dev/java/org/wpilib/commands3/DevMain.java"], + main_class = "org.wpilib.commands3.DevMain", + deps = [ + "//hal:hal-java", + "//ntcore:ntcore-java", + "//wpimath:wpimath-java", + "//wpiutil:wpiutil-java", + ], +) diff --git a/commandsv3/CMakeLists.txt b/commandsv3/CMakeLists.txt new file mode 100644 index 0000000000..d50c6a4971 --- /dev/null +++ b/commandsv3/CMakeLists.txt @@ -0,0 +1,53 @@ +project(commandsv3) + +include(SubDirList) +include(CompileWarnings) +include(AddTest) + +if(WITH_JAVA) + include(UseJava) + + file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java src/generated/main/java/*.java) + file(GLOB QUICKBUF_JAR ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/quickbuf/*.jar) + + add_jar( + commandsv3_jar + ${JAVA_SOURCES} + INCLUDE_JARS + datalog_jar + hal_jar + ntcore_jar + cscore_jar + cameraserver_jar + wpiannotations_jar + wpimath_jar + wpiunits_jar + wpiutil_jar + wpilibj_jar + ${QUICKBUF_JAR} + OUTPUT_NAME commandsv3 + OUTPUT_DIR ${WPILIB_BINARY_DIR}/${java_lib_dest} + ) + + install_jar(commandsv3_jar DESTINATION ${java_lib_dest}) + install_jar_exports( + TARGETS commandsv3_jar + FILE commandsv3_jar.cmake + DESTINATION share/commandsv3 + ) +endif() + +if(WITH_JAVA_SOURCE) + include(UseJava) + include(CreateSourceJar) + add_source_jar( + commandsv3_src_jar + BASE_DIRECTORIES + ${CMAKE_CURRENT_SOURCE_DIR}/src/main/java + ${CMAKE_CURRENT_SOURCE_DIR}/src/generated/main/java + OUTPUT_NAME commandsv3-sources + OUTPUT_DIR ${WPILIB_BINARY_DIR}/${java_lib_dest} + ) + set_property(TARGET commandsv3_src_jar PROPERTY FOLDER "java") + install_jar(commandsv3_src_jar DESTINATION ${java_lib_dest}) +endif() diff --git a/commandsv3/CommandsV3.json b/commandsv3/CommandsV3.json new file mode 100644 index 0000000000..72c540ef84 --- /dev/null +++ b/commandsv3/CommandsV3.json @@ -0,0 +1,18 @@ +{ + "fileName": "CommandsV3.json", + "name": "Commands v3", + "version": "1.0.0", + "uuid": "4decdc05-a056-46cf-9561-39449bbb0130", + "frcYear": "2027_alpha1", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "org.wpilib.commands3", + "artifactId": "commands3-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} diff --git a/commandsv3/build.gradle b/commandsv3/build.gradle new file mode 100644 index 0000000000..b097060d1d --- /dev/null +++ b/commandsv3/build.gradle @@ -0,0 +1,49 @@ +ext { + useJava = true + useCpp = false + baseId = 'commands3' + groupId = 'org.wpilib' + + nativeName = 'commands3' + devMain = 'org.wpilib.commands3.DevMain' +} + +apply from: "${rootDir}/shared/java/javacommon.gradle" + +evaluationDependsOn(':wpiutil') +evaluationDependsOn(':ntcore') +evaluationDependsOn(':hal') +evaluationDependsOn(':wpimath') +evaluationDependsOn(':wpilibj') + +dependencies { + annotationProcessor project(':javacPlugin') + implementation project(':wpiannotations') + implementation project(':wpiutil') + implementation project(':wpinet') + implementation project(':ntcore') + implementation project(':hal') + implementation project(':wpimath') + implementation project(':wpilibj') + api("us.hebi.quickbuf:quickbuf-runtime:1.4") + testAnnotationProcessor project(':javacPlugin') + testImplementation 'org.mockito:mockito-core:4.1.0' +} + +sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java" +sourceSets.main.resources.srcDir "${projectDir}/src/main/proto" + +test { + testLogging { + outputs.upToDateWhen {false} + showStandardStreams = true + } + + // For reflective access to the continuation classes + jvmArgs += [ + '--add-opens', + 'java.base/jdk.internal.vm=ALL-UNNAMED', + '--add-opens', + 'java.base/java.lang=ALL-UNNAMED', + ] +} diff --git a/commandsv3/commandsv3-config.cmake.in b/commandsv3/commandsv3-config.cmake.in new file mode 100644 index 0000000000..30194c4250 --- /dev/null +++ b/commandsv3/commandsv3-config.cmake.in @@ -0,0 +1,15 @@ +include(CMakeFindDependencyMacro) +@WPIUTIL_DEP_REPLACE@ +@DATALOG_DEP_REPLACE@ +@NTCORE_DEP_REPLACE@ +@CSCORE_DEP_REPLACE@ +@CAMERASERVER_DEP_REPLACE@ +@HAL_DEP_REPLACE@ +@WPILIBC_DEP_REPLACE@ +@WPIMATH_DEP_REPLACE@ + +@FILENAME_DEP_REPLACE@ +include(${SELF_DIR}/commandsv3.cmake) +if(@WITH_JAVA@) + include(${SELF_DIR}/commandsv3_jar.cmake) +endif() diff --git a/commandsv3/generate.bzl b/commandsv3/generate.bzl new file mode 100644 index 0000000000..4c462cc341 --- /dev/null +++ b/commandsv3/generate.bzl @@ -0,0 +1,45 @@ +def __generate_commandsv3_impl(ctx): + """ + Custom rule used to create the commandsv3 pre-generated files. See `./README-Bazel.md` for the reasoning. + """ + output_dir = ctx.actions.declare_directory("_gendir") + + args = ctx.actions.args() + args.add("--output_directory", output_dir.path) + args.add("--template_root", "commandsv3/src/generate") + args.add("--protoc", ctx.executable._protoc) + args.add("--quickbuf_plugin", ctx.executable._quickbuf) + + ctx.actions.run( + inputs = ctx.attr._templates.files, + outputs = [output_dir], + executable = ctx.executable._tool, + arguments = [args], + tools = [ctx.executable._protoc, ctx.executable._quickbuf], + ) + + return [DefaultInfo(files = depset([output_dir]))] + +generate_commandsv3 = rule( + implementation = __generate_commandsv3_impl, + attrs = { + "_protoc": attr.label( + default = Label("@com_google_protobuf//:protoc"), + cfg = "exec", + executable = True, + ), + "_quickbuf": attr.label( + default = Label("//:quickbuf_protoc"), + cfg = "exec", + executable = True, + ), + "_templates": attr.label( + default = Label("//commandsv3:templates"), + ), + "_tool": attr.label( + default = Label("//commandsv3:generate_files"), + cfg = "exec", + executable = True, + ), + }, +) diff --git a/commandsv3/generate_files.py b/commandsv3/generate_files.py new file mode 100755 index 0000000000..f6c1cb20c0 --- /dev/null +++ b/commandsv3/generate_files.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python3 + +# 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. + +import argparse +import json +import subprocess +from pathlib import Path + +from jinja2 import Environment, FileSystemLoader + + +def write_controller_file(output_dir: Path, controller_name: str, contents: str): + output_dir.mkdir(parents=True, exist_ok=True) + output_file = output_dir / controller_name + output_file.write_text(contents, encoding="utf-8", newline="\n") + print("Writing to ", output_file) + + +def generate_hids(output_directory: Path, template_directory: Path, schema_file: Path): + with schema_file.open(encoding="utf-8") as f: + controllers = json.load(f) + + # Java files + java_subdirectory = "main/java/org/wpilib/commands3/button" + env = Environment( + loader=FileSystemLoader(template_directory / "main/java"), + autoescape=False, + keep_trailing_newline=True, + ) + root_path = output_directory / java_subdirectory + template = env.get_template("commandhid.java.jinja") + for controller in controllers: + controllerName = f"Command{controller['ConsoleName']}Controller.java" + output = template.render(controller) + write_controller_file(root_path, controllerName, output) + + +def generate_quickbuf( + protoc, quickbuf_plugin: Path, output_directory: Path, proto_dir: Path +): + proto_files = proto_dir.glob("*.proto") + for path in proto_files: + absolute_filename = path.absolute() + args = [protoc] + if quickbuf_plugin: + # Optional on macOS if using protoc-quickbuf + args += [f"--plugin=protoc-gen-quickbuf={quickbuf_plugin}"] + args += [ + f"--quickbuf_out=gen_descriptors=true:{output_directory.absolute()}", + f"-I{absolute_filename.parent}", + absolute_filename, + ] + subprocess.check_call(args) + java_files = (output_directory / "org/wpilib/commands3/proto").glob("*.java") + for java_file in java_files: + with (java_file).open(encoding="utf-8") as f: + content = f.read() + + java_file.write_text( + "// Copyright (c) FIRST and other WPILib contributors.\n// Open Source Software; you can modify and/or share it under the terms of\n// the WPILib BSD license file in the root directory of this project.\n" + + content, + encoding="utf-8", + newline="\n", + ) + + +def main(): + script_path = Path(__file__).resolve() + dirname = script_path.parent + + parser = argparse.ArgumentParser() + parser.add_argument( + "--output_directory", + help="Optional. If set, will output the generated files to this directory, otherwise it will use a path relative to the script", + default=dirname / "src/generated", + type=Path, + ) + parser.add_argument( + "--template_root", + help="Optional. If set, will use this directory as the root for the jinja templates", + default=dirname / "src/generate", + type=Path, + ) + parser.add_argument( + "--schema_file", + help="Optional. If set, will use this file for the joystick schema", + default="wpilibj/src/generate/hids.json", + type=Path, + ) + parser.add_argument( + "--protoc", + help="Protoc executable command", + default="protoc", + ) + parser.add_argument( + "--quickbuf_plugin", + help="Path to the quickbuf protoc plugin", + ) + parser.add_argument( + "--proto_directory", + help="Optional. If set, will use this directory to glob for protobuf files", + default=dirname / "src/main/proto", + type=Path, + ) + args = parser.parse_args() + + generate_hids(args.output_directory, args.template_root, args.schema_file) + generate_quickbuf( + args.protoc, + args.quickbuf_plugin, + args.output_directory / "main/java", + args.proto_directory, + ) + + +if __name__ == "__main__": + main() diff --git a/commandsv3/src/dev/java/org/wpilib/commands3/DevMain.java b/commandsv3/src/dev/java/org/wpilib/commands3/DevMain.java new file mode 100644 index 0000000000..8a92b7b09d --- /dev/null +++ b/commandsv3/src/dev/java/org/wpilib/commands3/DevMain.java @@ -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. + +package org.wpilib.commands3; + +import edu.wpi.first.hal.HALUtil; +import edu.wpi.first.networktables.NetworkTablesJNI; +import edu.wpi.first.util.CombinedRuntimeLoader; + +/** Dev main. */ +public final class DevMain { + /** Main entry point. */ + public static void main(String[] args) { + System.out.println("Commands V3 DevMain"); + + System.out.println(CombinedRuntimeLoader.getPlatformPath()); + System.out.println(NetworkTablesJNI.now()); + System.out.println(HALUtil.getHALRuntimeType()); + } + + private DevMain() {} +} diff --git a/commandsv3/src/generate/main/java/commandhid.java.jinja b/commandsv3/src/generate/main/java/commandhid.java.jinja new file mode 100644 index 0000000000..17f538a762 --- /dev/null +++ b/commandsv3/src/generate/main/java/commandhid.java.jinja @@ -0,0 +1,148 @@ +// 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. + +// THIS FILE WAS AUTO-GENERATED BY ./commandsv3/generate_files.py. DO NOT MODIFY +{% macro capitalize_first(string) -%} +{{ string[0]|capitalize + string[1:] }} +{%- endmacro %} +package org.wpilib.commands3.button; + +import edu.wpi.first.wpilibj.{{ ConsoleName }}Controller; +import edu.wpi.first.wpilibj.event.EventLoop; +import org.wpilib.commands3.Scheduler; +import org.wpilib.commands3.Trigger; + +/** + * A version of {@link {{ ConsoleName }}Controller} with {@link Trigger} factories for command-based. + * + * @see {{ ConsoleName }}Controller + */ +@SuppressWarnings("MethodName") +public class Command{{ ConsoleName }}Controller extends CommandGenericHID { + private final {{ ConsoleName }}Controller m_hid; + + /** + * Construct an instance of a controller. Commands bound to buttons on the controller will be + * scheduled on the {@link Scheduler#getDefault() default scheduler} using its default event loop. + * + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public Command{{ ConsoleName }}Controller(int port) { + super(port); + m_hid = new {{ ConsoleName }}Controller(port); + } + + /** + * Construct an instance of a controller. Commands bound to buttons on the controller will be + * scheduled on the given scheduler using its default event loop. + * + * @param scheduler The scheduler that should execute the triggered commands. + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public Command{{ ConsoleName }}Controller(Scheduler scheduler, int port) { + super(scheduler, port); + m_hid = new {{ ConsoleName }}Controller(port); + } + + /** + * Get the underlying GenericHID object. + * + * @return the wrapped GenericHID object + */ + @Override + public {{ ConsoleName }}Controller getHID() { + return m_hid; + } +{% for button in buttons %} + /** + * Constructs a Trigger instance around the {{ button.DocName|default(button.name) }} button's digital signal. + * + * @return a Trigger instance representing the {{ button.DocName|default(button.name) }} button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #{{ button.name }}(EventLoop) + */ + public Trigger {{ button.name }}() { + return {{ button.name }}(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the {{ button.DocName|default(button.name) }} button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the {{ button.DocName|default(button.name) }} button's digital signal attached + * to the given loop. + */ + public Trigger {{ button.name }}(EventLoop loop) { + return button({{ ConsoleName }}Controller.Button.k{{ capitalize_first(button.name) }}.value, loop); + } +{% endfor -%} +{% for trigger in triggers -%} +{% if trigger.UseThresholdMethods %} + /** + * Constructs a Trigger instance around the axis value of the {{ trigger.DocName }}. The returned + * trigger will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value + * should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @param loop the event loop instance to attach the Trigger to. + * @return a Trigger instance that is true when the {{ trigger.DocName }}'s axis exceeds the provided + * threshold, attached to the given event loop + */ + public Trigger {{ trigger.name }}(double threshold, EventLoop loop) { + return axisGreaterThan({{ ConsoleName }}Controller.Axis.k{{ capitalize_first(trigger.name) }}.value, threshold, loop); + } + + /** + * Constructs a Trigger instance around the axis value of the {{ trigger.DocName }}. The returned + * trigger will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value + * should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @return a Trigger instance that is true when the {{ trigger.DocName }}'s axis exceeds the provided + * threshold, attached to the {@link Scheduler#getDefaultEventLoop() default scheduler event + * loop} on the scheduler passed to the controller's constructor, or the {@link + * Scheduler#getDefault default scheduler} if a scheduler was not explicitly provided. + */ + public Trigger {{ trigger.name }}(double threshold) { + return {{ trigger.name }}(threshold, getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the axis value of the {{ trigger.DocName }}. The returned trigger + * will be true when the axis value is greater than 0.5. + * + * @return a Trigger instance that is true when the {{ trigger.DocName }}'s axis exceeds 0.5, attached to + * the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + */ + public Trigger {{ trigger.name }}() { + return {{ trigger.name }}(0.5); + } +{% endif -%} +{% endfor -%} +{% for stick in sticks %} + /** + * Get the {{ stick.NameParts[1] }} axis value of {{ stick.NameParts[0] }} side of the controller. {{ stick.PositiveDirection }} is positive. + * + * @return The axis value. + */ + public double get{{ stick.NameParts|map("capitalize")|join }}() { + return m_hid.get{{ stick.NameParts|map("capitalize")|join }}(); + } +{% endfor -%} +{% for trigger in triggers %} + /** + * Get the {{ trigger.DocName }} axis value of the controller. Note that this axis is bound to the + * range of [0, 1] as opposed to the usual [-1, 1]. + * + * @return The axis value. + */ + public double get{{ capitalize_first(trigger.name) }}Axis() { + return m_hid.get{{ capitalize_first(trigger.name) }}Axis(); + } +{% endfor -%} +} diff --git a/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandPS4Controller.java b/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandPS4Controller.java new file mode 100644 index 0000000000..e48b2358e0 --- /dev/null +++ b/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandPS4Controller.java @@ -0,0 +1,447 @@ +// 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. + +// THIS FILE WAS AUTO-GENERATED BY ./commandsv3/generate_files.py. DO NOT MODIFY + +package org.wpilib.commands3.button; + +import edu.wpi.first.wpilibj.PS4Controller; +import edu.wpi.first.wpilibj.event.EventLoop; +import org.wpilib.commands3.Scheduler; +import org.wpilib.commands3.Trigger; + +/** + * A version of {@link PS4Controller} with {@link Trigger} factories for command-based. + * + * @see PS4Controller + */ +@SuppressWarnings("MethodName") +public class CommandPS4Controller extends CommandGenericHID { + private final PS4Controller m_hid; + + /** + * Construct an instance of a controller. Commands bound to buttons on the controller will be + * scheduled on the {@link Scheduler#getDefault() default scheduler} using its default event loop. + * + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public CommandPS4Controller(int port) { + super(port); + m_hid = new PS4Controller(port); + } + + /** + * Construct an instance of a controller. Commands bound to buttons on the controller will be + * scheduled on the given scheduler using its default event loop. + * + * @param scheduler The scheduler that should execute the triggered commands. + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public CommandPS4Controller(Scheduler scheduler, int port) { + super(scheduler, port); + m_hid = new PS4Controller(port); + } + + /** + * Get the underlying GenericHID object. + * + * @return the wrapped GenericHID object + */ + @Override + public PS4Controller getHID() { + return m_hid; + } + + /** + * Constructs a Trigger instance around the square button's digital signal. + * + * @return a Trigger instance representing the square button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #square(EventLoop) + */ + public Trigger square() { + return square(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the square button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the square button's digital signal attached + * to the given loop. + */ + public Trigger square(EventLoop loop) { + return button(PS4Controller.Button.kSquare.value, loop); + } + + /** + * Constructs a Trigger instance around the cross button's digital signal. + * + * @return a Trigger instance representing the cross button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #cross(EventLoop) + */ + public Trigger cross() { + return cross(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the cross button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the cross button's digital signal attached + * to the given loop. + */ + public Trigger cross(EventLoop loop) { + return button(PS4Controller.Button.kCross.value, loop); + } + + /** + * Constructs a Trigger instance around the circle button's digital signal. + * + * @return a Trigger instance representing the circle button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #circle(EventLoop) + */ + public Trigger circle() { + return circle(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the circle button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the circle button's digital signal attached + * to the given loop. + */ + public Trigger circle(EventLoop loop) { + return button(PS4Controller.Button.kCircle.value, loop); + } + + /** + * Constructs a Trigger instance around the triangle button's digital signal. + * + * @return a Trigger instance representing the triangle button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #triangle(EventLoop) + */ + public Trigger triangle() { + return triangle(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the triangle button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the triangle button's digital signal attached + * to the given loop. + */ + public Trigger triangle(EventLoop loop) { + return button(PS4Controller.Button.kTriangle.value, loop); + } + + /** + * Constructs a Trigger instance around the left trigger 1 button's digital signal. + * + * @return a Trigger instance representing the left trigger 1 button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #L1(EventLoop) + */ + public Trigger L1() { + return L1(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the left trigger 1 button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the left trigger 1 button's digital signal attached + * to the given loop. + */ + public Trigger L1(EventLoop loop) { + return button(PS4Controller.Button.kL1.value, loop); + } + + /** + * Constructs a Trigger instance around the right trigger 1 button's digital signal. + * + * @return a Trigger instance representing the right trigger 1 button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #R1(EventLoop) + */ + public Trigger R1() { + return R1(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the right trigger 1 button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the right trigger 1 button's digital signal attached + * to the given loop. + */ + public Trigger R1(EventLoop loop) { + return button(PS4Controller.Button.kR1.value, loop); + } + + /** + * Constructs a Trigger instance around the left trigger 2 button's digital signal. + * + * @return a Trigger instance representing the left trigger 2 button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #L2(EventLoop) + */ + public Trigger L2() { + return L2(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the left trigger 2 button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the left trigger 2 button's digital signal attached + * to the given loop. + */ + public Trigger L2(EventLoop loop) { + return button(PS4Controller.Button.kL2.value, loop); + } + + /** + * Constructs a Trigger instance around the right trigger 2 button's digital signal. + * + * @return a Trigger instance representing the right trigger 2 button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #R2(EventLoop) + */ + public Trigger R2() { + return R2(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the right trigger 2 button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the right trigger 2 button's digital signal attached + * to the given loop. + */ + public Trigger R2(EventLoop loop) { + return button(PS4Controller.Button.kR2.value, loop); + } + + /** + * Constructs a Trigger instance around the share button's digital signal. + * + * @return a Trigger instance representing the share button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #share(EventLoop) + */ + public Trigger share() { + return share(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the share button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the share button's digital signal attached + * to the given loop. + */ + public Trigger share(EventLoop loop) { + return button(PS4Controller.Button.kShare.value, loop); + } + + /** + * Constructs a Trigger instance around the options button's digital signal. + * + * @return a Trigger instance representing the options button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #options(EventLoop) + */ + public Trigger options() { + return options(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the options button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the options button's digital signal attached + * to the given loop. + */ + public Trigger options(EventLoop loop) { + return button(PS4Controller.Button.kOptions.value, loop); + } + + /** + * Constructs a Trigger instance around the L3 (left stick) button's digital signal. + * + * @return a Trigger instance representing the L3 (left stick) button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #L3(EventLoop) + */ + public Trigger L3() { + return L3(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the L3 (left stick) button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the L3 (left stick) button's digital signal attached + * to the given loop. + */ + public Trigger L3(EventLoop loop) { + return button(PS4Controller.Button.kL3.value, loop); + } + + /** + * Constructs a Trigger instance around the R3 (right stick) button's digital signal. + * + * @return a Trigger instance representing the R3 (right stick) button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #R3(EventLoop) + */ + public Trigger R3() { + return R3(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the R3 (right stick) button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the R3 (right stick) button's digital signal attached + * to the given loop. + */ + public Trigger R3(EventLoop loop) { + return button(PS4Controller.Button.kR3.value, loop); + } + + /** + * Constructs a Trigger instance around the PlayStation button's digital signal. + * + * @return a Trigger instance representing the PlayStation button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #PS(EventLoop) + */ + public Trigger PS() { + return PS(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the PlayStation button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the PlayStation button's digital signal attached + * to the given loop. + */ + public Trigger PS(EventLoop loop) { + return button(PS4Controller.Button.kPS.value, loop); + } + + /** + * Constructs a Trigger instance around the touchpad button's digital signal. + * + * @return a Trigger instance representing the touchpad button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #touchpad(EventLoop) + */ + public Trigger touchpad() { + return touchpad(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the touchpad button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the touchpad button's digital signal attached + * to the given loop. + */ + public Trigger touchpad(EventLoop loop) { + return button(PS4Controller.Button.kTouchpad.value, loop); + } + + /** + * Get the X axis value of left side of the controller. Right is positive. + * + * @return The axis value. + */ + public double getLeftX() { + return m_hid.getLeftX(); + } + + /** + * Get the Y axis value of left side of the controller. Back is positive. + * + * @return The axis value. + */ + public double getLeftY() { + return m_hid.getLeftY(); + } + + /** + * Get the X axis value of right side of the controller. Right is positive. + * + * @return The axis value. + */ + public double getRightX() { + return m_hid.getRightX(); + } + + /** + * Get the Y axis value of right side of the controller. Back is positive. + * + * @return The axis value. + */ + public double getRightY() { + return m_hid.getRightY(); + } + + /** + * Get the left trigger 2 axis value of the controller. Note that this axis is bound to the + * range of [0, 1] as opposed to the usual [-1, 1]. + * + * @return The axis value. + */ + public double getL2Axis() { + return m_hid.getL2Axis(); + } + + /** + * Get the right trigger 2 axis value of the controller. Note that this axis is bound to the + * range of [0, 1] as opposed to the usual [-1, 1]. + * + * @return The axis value. + */ + public double getR2Axis() { + return m_hid.getR2Axis(); + } +} diff --git a/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandPS5Controller.java b/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandPS5Controller.java new file mode 100644 index 0000000000..5ee032fdb4 --- /dev/null +++ b/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandPS5Controller.java @@ -0,0 +1,447 @@ +// 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. + +// THIS FILE WAS AUTO-GENERATED BY ./commandsv3/generate_files.py. DO NOT MODIFY + +package org.wpilib.commands3.button; + +import edu.wpi.first.wpilibj.PS5Controller; +import edu.wpi.first.wpilibj.event.EventLoop; +import org.wpilib.commands3.Scheduler; +import org.wpilib.commands3.Trigger; + +/** + * A version of {@link PS5Controller} with {@link Trigger} factories for command-based. + * + * @see PS5Controller + */ +@SuppressWarnings("MethodName") +public class CommandPS5Controller extends CommandGenericHID { + private final PS5Controller m_hid; + + /** + * Construct an instance of a controller. Commands bound to buttons on the controller will be + * scheduled on the {@link Scheduler#getDefault() default scheduler} using its default event loop. + * + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public CommandPS5Controller(int port) { + super(port); + m_hid = new PS5Controller(port); + } + + /** + * Construct an instance of a controller. Commands bound to buttons on the controller will be + * scheduled on the given scheduler using its default event loop. + * + * @param scheduler The scheduler that should execute the triggered commands. + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public CommandPS5Controller(Scheduler scheduler, int port) { + super(scheduler, port); + m_hid = new PS5Controller(port); + } + + /** + * Get the underlying GenericHID object. + * + * @return the wrapped GenericHID object + */ + @Override + public PS5Controller getHID() { + return m_hid; + } + + /** + * Constructs a Trigger instance around the square button's digital signal. + * + * @return a Trigger instance representing the square button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #square(EventLoop) + */ + public Trigger square() { + return square(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the square button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the square button's digital signal attached + * to the given loop. + */ + public Trigger square(EventLoop loop) { + return button(PS5Controller.Button.kSquare.value, loop); + } + + /** + * Constructs a Trigger instance around the cross button's digital signal. + * + * @return a Trigger instance representing the cross button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #cross(EventLoop) + */ + public Trigger cross() { + return cross(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the cross button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the cross button's digital signal attached + * to the given loop. + */ + public Trigger cross(EventLoop loop) { + return button(PS5Controller.Button.kCross.value, loop); + } + + /** + * Constructs a Trigger instance around the circle button's digital signal. + * + * @return a Trigger instance representing the circle button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #circle(EventLoop) + */ + public Trigger circle() { + return circle(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the circle button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the circle button's digital signal attached + * to the given loop. + */ + public Trigger circle(EventLoop loop) { + return button(PS5Controller.Button.kCircle.value, loop); + } + + /** + * Constructs a Trigger instance around the triangle button's digital signal. + * + * @return a Trigger instance representing the triangle button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #triangle(EventLoop) + */ + public Trigger triangle() { + return triangle(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the triangle button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the triangle button's digital signal attached + * to the given loop. + */ + public Trigger triangle(EventLoop loop) { + return button(PS5Controller.Button.kTriangle.value, loop); + } + + /** + * Constructs a Trigger instance around the left trigger 1 button's digital signal. + * + * @return a Trigger instance representing the left trigger 1 button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #L1(EventLoop) + */ + public Trigger L1() { + return L1(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the left trigger 1 button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the left trigger 1 button's digital signal attached + * to the given loop. + */ + public Trigger L1(EventLoop loop) { + return button(PS5Controller.Button.kL1.value, loop); + } + + /** + * Constructs a Trigger instance around the right trigger 1 button's digital signal. + * + * @return a Trigger instance representing the right trigger 1 button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #R1(EventLoop) + */ + public Trigger R1() { + return R1(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the right trigger 1 button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the right trigger 1 button's digital signal attached + * to the given loop. + */ + public Trigger R1(EventLoop loop) { + return button(PS5Controller.Button.kR1.value, loop); + } + + /** + * Constructs a Trigger instance around the left trigger 2 button's digital signal. + * + * @return a Trigger instance representing the left trigger 2 button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #L2(EventLoop) + */ + public Trigger L2() { + return L2(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the left trigger 2 button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the left trigger 2 button's digital signal attached + * to the given loop. + */ + public Trigger L2(EventLoop loop) { + return button(PS5Controller.Button.kL2.value, loop); + } + + /** + * Constructs a Trigger instance around the right trigger 2 button's digital signal. + * + * @return a Trigger instance representing the right trigger 2 button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #R2(EventLoop) + */ + public Trigger R2() { + return R2(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the right trigger 2 button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the right trigger 2 button's digital signal attached + * to the given loop. + */ + public Trigger R2(EventLoop loop) { + return button(PS5Controller.Button.kR2.value, loop); + } + + /** + * Constructs a Trigger instance around the create button's digital signal. + * + * @return a Trigger instance representing the create button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #create(EventLoop) + */ + public Trigger create() { + return create(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the create button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the create button's digital signal attached + * to the given loop. + */ + public Trigger create(EventLoop loop) { + return button(PS5Controller.Button.kCreate.value, loop); + } + + /** + * Constructs a Trigger instance around the options button's digital signal. + * + * @return a Trigger instance representing the options button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #options(EventLoop) + */ + public Trigger options() { + return options(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the options button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the options button's digital signal attached + * to the given loop. + */ + public Trigger options(EventLoop loop) { + return button(PS5Controller.Button.kOptions.value, loop); + } + + /** + * Constructs a Trigger instance around the L3 (left stick) button's digital signal. + * + * @return a Trigger instance representing the L3 (left stick) button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #L3(EventLoop) + */ + public Trigger L3() { + return L3(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the L3 (left stick) button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the L3 (left stick) button's digital signal attached + * to the given loop. + */ + public Trigger L3(EventLoop loop) { + return button(PS5Controller.Button.kL3.value, loop); + } + + /** + * Constructs a Trigger instance around the R3 (right stick) button's digital signal. + * + * @return a Trigger instance representing the R3 (right stick) button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #R3(EventLoop) + */ + public Trigger R3() { + return R3(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the R3 (right stick) button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the R3 (right stick) button's digital signal attached + * to the given loop. + */ + public Trigger R3(EventLoop loop) { + return button(PS5Controller.Button.kR3.value, loop); + } + + /** + * Constructs a Trigger instance around the PlayStation button's digital signal. + * + * @return a Trigger instance representing the PlayStation button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #PS(EventLoop) + */ + public Trigger PS() { + return PS(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the PlayStation button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the PlayStation button's digital signal attached + * to the given loop. + */ + public Trigger PS(EventLoop loop) { + return button(PS5Controller.Button.kPS.value, loop); + } + + /** + * Constructs a Trigger instance around the touchpad button's digital signal. + * + * @return a Trigger instance representing the touchpad button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #touchpad(EventLoop) + */ + public Trigger touchpad() { + return touchpad(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the touchpad button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the touchpad button's digital signal attached + * to the given loop. + */ + public Trigger touchpad(EventLoop loop) { + return button(PS5Controller.Button.kTouchpad.value, loop); + } + + /** + * Get the X axis value of left side of the controller. Right is positive. + * + * @return The axis value. + */ + public double getLeftX() { + return m_hid.getLeftX(); + } + + /** + * Get the Y axis value of left side of the controller. Back is positive. + * + * @return The axis value. + */ + public double getLeftY() { + return m_hid.getLeftY(); + } + + /** + * Get the X axis value of right side of the controller. Right is positive. + * + * @return The axis value. + */ + public double getRightX() { + return m_hid.getRightX(); + } + + /** + * Get the Y axis value of right side of the controller. Back is positive. + * + * @return The axis value. + */ + public double getRightY() { + return m_hid.getRightY(); + } + + /** + * Get the left trigger 2 axis value of the controller. Note that this axis is bound to the + * range of [0, 1] as opposed to the usual [-1, 1]. + * + * @return The axis value. + */ + public double getL2Axis() { + return m_hid.getL2Axis(); + } + + /** + * Get the right trigger 2 axis value of the controller. Note that this axis is bound to the + * range of [0, 1] as opposed to the usual [-1, 1]. + * + * @return The axis value. + */ + public double getR2Axis() { + return m_hid.getR2Axis(); + } +} diff --git a/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandStadiaController.java b/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandStadiaController.java new file mode 100644 index 0000000000..3b9cdac1c5 --- /dev/null +++ b/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandStadiaController.java @@ -0,0 +1,451 @@ +// 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. + +// THIS FILE WAS AUTO-GENERATED BY ./commandsv3/generate_files.py. DO NOT MODIFY + +package org.wpilib.commands3.button; + +import edu.wpi.first.wpilibj.StadiaController; +import edu.wpi.first.wpilibj.event.EventLoop; +import org.wpilib.commands3.Scheduler; +import org.wpilib.commands3.Trigger; + +/** + * A version of {@link StadiaController} with {@link Trigger} factories for command-based. + * + * @see StadiaController + */ +@SuppressWarnings("MethodName") +public class CommandStadiaController extends CommandGenericHID { + private final StadiaController m_hid; + + /** + * Construct an instance of a controller. Commands bound to buttons on the controller will be + * scheduled on the {@link Scheduler#getDefault() default scheduler} using its default event loop. + * + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public CommandStadiaController(int port) { + super(port); + m_hid = new StadiaController(port); + } + + /** + * Construct an instance of a controller. Commands bound to buttons on the controller will be + * scheduled on the given scheduler using its default event loop. + * + * @param scheduler The scheduler that should execute the triggered commands. + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public CommandStadiaController(Scheduler scheduler, int port) { + super(scheduler, port); + m_hid = new StadiaController(port); + } + + /** + * Get the underlying GenericHID object. + * + * @return the wrapped GenericHID object + */ + @Override + public StadiaController getHID() { + return m_hid; + } + + /** + * Constructs a Trigger instance around the A button's digital signal. + * + * @return a Trigger instance representing the A button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #a(EventLoop) + */ + public Trigger a() { + return a(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the A button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the A button's digital signal attached + * to the given loop. + */ + public Trigger a(EventLoop loop) { + return button(StadiaController.Button.kA.value, loop); + } + + /** + * Constructs a Trigger instance around the B button's digital signal. + * + * @return a Trigger instance representing the B button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #b(EventLoop) + */ + public Trigger b() { + return b(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the B button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the B button's digital signal attached + * to the given loop. + */ + public Trigger b(EventLoop loop) { + return button(StadiaController.Button.kB.value, loop); + } + + /** + * Constructs a Trigger instance around the X button's digital signal. + * + * @return a Trigger instance representing the X button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #x(EventLoop) + */ + public Trigger x() { + return x(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the X button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the X button's digital signal attached + * to the given loop. + */ + public Trigger x(EventLoop loop) { + return button(StadiaController.Button.kX.value, loop); + } + + /** + * Constructs a Trigger instance around the Y button's digital signal. + * + * @return a Trigger instance representing the Y button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #y(EventLoop) + */ + public Trigger y() { + return y(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the Y button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the Y button's digital signal attached + * to the given loop. + */ + public Trigger y(EventLoop loop) { + return button(StadiaController.Button.kY.value, loop); + } + + /** + * Constructs a Trigger instance around the left bumper button's digital signal. + * + * @return a Trigger instance representing the left bumper button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #leftBumper(EventLoop) + */ + public Trigger leftBumper() { + return leftBumper(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the left bumper button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the left bumper button's digital signal attached + * to the given loop. + */ + public Trigger leftBumper(EventLoop loop) { + return button(StadiaController.Button.kLeftBumper.value, loop); + } + + /** + * Constructs a Trigger instance around the right bumper button's digital signal. + * + * @return a Trigger instance representing the right bumper button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #rightBumper(EventLoop) + */ + public Trigger rightBumper() { + return rightBumper(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the right bumper button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the right bumper button's digital signal attached + * to the given loop. + */ + public Trigger rightBumper(EventLoop loop) { + return button(StadiaController.Button.kRightBumper.value, loop); + } + + /** + * Constructs a Trigger instance around the left stick button's digital signal. + * + * @return a Trigger instance representing the left stick button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #leftStick(EventLoop) + */ + public Trigger leftStick() { + return leftStick(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the left stick button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the left stick button's digital signal attached + * to the given loop. + */ + public Trigger leftStick(EventLoop loop) { + return button(StadiaController.Button.kLeftStick.value, loop); + } + + /** + * Constructs a Trigger instance around the right stick button's digital signal. + * + * @return a Trigger instance representing the right stick button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #rightStick(EventLoop) + */ + public Trigger rightStick() { + return rightStick(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the right stick button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the right stick button's digital signal attached + * to the given loop. + */ + public Trigger rightStick(EventLoop loop) { + return button(StadiaController.Button.kRightStick.value, loop); + } + + /** + * Constructs a Trigger instance around the ellipses button's digital signal. + * + * @return a Trigger instance representing the ellipses button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #ellipses(EventLoop) + */ + public Trigger ellipses() { + return ellipses(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the ellipses button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the ellipses button's digital signal attached + * to the given loop. + */ + public Trigger ellipses(EventLoop loop) { + return button(StadiaController.Button.kEllipses.value, loop); + } + + /** + * Constructs a Trigger instance around the hamburger button's digital signal. + * + * @return a Trigger instance representing the hamburger button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #hamburger(EventLoop) + */ + public Trigger hamburger() { + return hamburger(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the hamburger button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the hamburger button's digital signal attached + * to the given loop. + */ + public Trigger hamburger(EventLoop loop) { + return button(StadiaController.Button.kHamburger.value, loop); + } + + /** + * Constructs a Trigger instance around the stadia button's digital signal. + * + * @return a Trigger instance representing the stadia button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #stadia(EventLoop) + */ + public Trigger stadia() { + return stadia(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the stadia button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the stadia button's digital signal attached + * to the given loop. + */ + public Trigger stadia(EventLoop loop) { + return button(StadiaController.Button.kStadia.value, loop); + } + + /** + * Constructs a Trigger instance around the right trigger button's digital signal. + * + * @return a Trigger instance representing the right trigger button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #rightTrigger(EventLoop) + */ + public Trigger rightTrigger() { + return rightTrigger(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the right trigger button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the right trigger button's digital signal attached + * to the given loop. + */ + public Trigger rightTrigger(EventLoop loop) { + return button(StadiaController.Button.kRightTrigger.value, loop); + } + + /** + * Constructs a Trigger instance around the left trigger button's digital signal. + * + * @return a Trigger instance representing the left trigger button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #leftTrigger(EventLoop) + */ + public Trigger leftTrigger() { + return leftTrigger(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the left trigger button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the left trigger button's digital signal attached + * to the given loop. + */ + public Trigger leftTrigger(EventLoop loop) { + return button(StadiaController.Button.kLeftTrigger.value, loop); + } + + /** + * Constructs a Trigger instance around the google button's digital signal. + * + * @return a Trigger instance representing the google button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #google(EventLoop) + */ + public Trigger google() { + return google(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the google button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the google button's digital signal attached + * to the given loop. + */ + public Trigger google(EventLoop loop) { + return button(StadiaController.Button.kGoogle.value, loop); + } + + /** + * Constructs a Trigger instance around the frame button's digital signal. + * + * @return a Trigger instance representing the frame button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #frame(EventLoop) + */ + public Trigger frame() { + return frame(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the frame button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the frame button's digital signal attached + * to the given loop. + */ + public Trigger frame(EventLoop loop) { + return button(StadiaController.Button.kFrame.value, loop); + } + + /** + * Get the X axis value of left side of the controller. Right is positive. + * + * @return The axis value. + */ + public double getLeftX() { + return m_hid.getLeftX(); + } + + /** + * Get the X axis value of right side of the controller. Right is positive. + * + * @return The axis value. + */ + public double getRightX() { + return m_hid.getRightX(); + } + + /** + * Get the Y axis value of left side of the controller. Back is positive. + * + * @return The axis value. + */ + public double getLeftY() { + return m_hid.getLeftY(); + } + + /** + * Get the Y axis value of right side of the controller. Back is positive. + * + * @return The axis value. + */ + public double getRightY() { + return m_hid.getRightY(); + } +} diff --git a/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandXboxController.java b/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandXboxController.java new file mode 100644 index 0000000000..ef1008d396 --- /dev/null +++ b/commandsv3/src/generated/main/java/org/wpilib/commands3/button/CommandXboxController.java @@ -0,0 +1,435 @@ +// 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. + +// THIS FILE WAS AUTO-GENERATED BY ./commandsv3/generate_files.py. DO NOT MODIFY + +package org.wpilib.commands3.button; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.event.EventLoop; +import org.wpilib.commands3.Scheduler; +import org.wpilib.commands3.Trigger; + +/** + * A version of {@link XboxController} with {@link Trigger} factories for command-based. + * + * @see XboxController + */ +@SuppressWarnings("MethodName") +public class CommandXboxController extends CommandGenericHID { + private final XboxController m_hid; + + /** + * Construct an instance of a controller. Commands bound to buttons on the controller will be + * scheduled on the {@link Scheduler#getDefault() default scheduler} using its default event loop. + * + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public CommandXboxController(int port) { + super(port); + m_hid = new XboxController(port); + } + + /** + * Construct an instance of a controller. Commands bound to buttons on the controller will be + * scheduled on the given scheduler using its default event loop. + * + * @param scheduler The scheduler that should execute the triggered commands. + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public CommandXboxController(Scheduler scheduler, int port) { + super(scheduler, port); + m_hid = new XboxController(port); + } + + /** + * Get the underlying GenericHID object. + * + * @return the wrapped GenericHID object + */ + @Override + public XboxController getHID() { + return m_hid; + } + + /** + * Constructs a Trigger instance around the A button's digital signal. + * + * @return a Trigger instance representing the A button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #a(EventLoop) + */ + public Trigger a() { + return a(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the A button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the A button's digital signal attached + * to the given loop. + */ + public Trigger a(EventLoop loop) { + return button(XboxController.Button.kA.value, loop); + } + + /** + * Constructs a Trigger instance around the B button's digital signal. + * + * @return a Trigger instance representing the B button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #b(EventLoop) + */ + public Trigger b() { + return b(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the B button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the B button's digital signal attached + * to the given loop. + */ + public Trigger b(EventLoop loop) { + return button(XboxController.Button.kB.value, loop); + } + + /** + * Constructs a Trigger instance around the X button's digital signal. + * + * @return a Trigger instance representing the X button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #x(EventLoop) + */ + public Trigger x() { + return x(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the X button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the X button's digital signal attached + * to the given loop. + */ + public Trigger x(EventLoop loop) { + return button(XboxController.Button.kX.value, loop); + } + + /** + * Constructs a Trigger instance around the Y button's digital signal. + * + * @return a Trigger instance representing the Y button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #y(EventLoop) + */ + public Trigger y() { + return y(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the Y button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the Y button's digital signal attached + * to the given loop. + */ + public Trigger y(EventLoop loop) { + return button(XboxController.Button.kY.value, loop); + } + + /** + * Constructs a Trigger instance around the left bumper button's digital signal. + * + * @return a Trigger instance representing the left bumper button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #leftBumper(EventLoop) + */ + public Trigger leftBumper() { + return leftBumper(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the left bumper button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the left bumper button's digital signal attached + * to the given loop. + */ + public Trigger leftBumper(EventLoop loop) { + return button(XboxController.Button.kLeftBumper.value, loop); + } + + /** + * Constructs a Trigger instance around the right bumper button's digital signal. + * + * @return a Trigger instance representing the right bumper button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #rightBumper(EventLoop) + */ + public Trigger rightBumper() { + return rightBumper(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the right bumper button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the right bumper button's digital signal attached + * to the given loop. + */ + public Trigger rightBumper(EventLoop loop) { + return button(XboxController.Button.kRightBumper.value, loop); + } + + /** + * Constructs a Trigger instance around the back button's digital signal. + * + * @return a Trigger instance representing the back button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #back(EventLoop) + */ + public Trigger back() { + return back(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the back button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the back button's digital signal attached + * to the given loop. + */ + public Trigger back(EventLoop loop) { + return button(XboxController.Button.kBack.value, loop); + } + + /** + * Constructs a Trigger instance around the start button's digital signal. + * + * @return a Trigger instance representing the start button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #start(EventLoop) + */ + public Trigger start() { + return start(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the start button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the start button's digital signal attached + * to the given loop. + */ + public Trigger start(EventLoop loop) { + return button(XboxController.Button.kStart.value, loop); + } + + /** + * Constructs a Trigger instance around the left stick button's digital signal. + * + * @return a Trigger instance representing the left stick button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #leftStick(EventLoop) + */ + public Trigger leftStick() { + return leftStick(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the left stick button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the left stick button's digital signal attached + * to the given loop. + */ + public Trigger leftStick(EventLoop loop) { + return button(XboxController.Button.kLeftStick.value, loop); + } + + /** + * Constructs a Trigger instance around the right stick button's digital signal. + * + * @return a Trigger instance representing the right stick button's digital signal attached + * to the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + * @see #rightStick(EventLoop) + */ + public Trigger rightStick() { + return rightStick(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the right stick button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return a Trigger instance representing the right stick button's digital signal attached + * to the given loop. + */ + public Trigger rightStick(EventLoop loop) { + return button(XboxController.Button.kRightStick.value, loop); + } + + /** + * Constructs a Trigger instance around the axis value of the left trigger. The returned + * trigger will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value + * should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @param loop the event loop instance to attach the Trigger to. + * @return a Trigger instance that is true when the left trigger's axis exceeds the provided + * threshold, attached to the given event loop + */ + public Trigger leftTrigger(double threshold, EventLoop loop) { + return axisGreaterThan(XboxController.Axis.kLeftTrigger.value, threshold, loop); + } + + /** + * Constructs a Trigger instance around the axis value of the left trigger. The returned + * trigger will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value + * should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @return a Trigger instance that is true when the left trigger's axis exceeds the provided + * threshold, attached to the {@link Scheduler#getDefaultEventLoop() default scheduler event + * loop} on the scheduler passed to the controller's constructor, or the {@link + * Scheduler#getDefault default scheduler} if a scheduler was not explicitly provided. + */ + public Trigger leftTrigger(double threshold) { + return leftTrigger(threshold, getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the axis value of the left trigger. The returned trigger + * will be true when the axis value is greater than 0.5. + * + * @return a Trigger instance that is true when the left trigger's axis exceeds 0.5, attached to + * the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + */ + public Trigger leftTrigger() { + return leftTrigger(0.5); + } + + /** + * Constructs a Trigger instance around the axis value of the right trigger. The returned + * trigger will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value + * should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @param loop the event loop instance to attach the Trigger to. + * @return a Trigger instance that is true when the right trigger's axis exceeds the provided + * threshold, attached to the given event loop + */ + public Trigger rightTrigger(double threshold, EventLoop loop) { + return axisGreaterThan(XboxController.Axis.kRightTrigger.value, threshold, loop); + } + + /** + * Constructs a Trigger instance around the axis value of the right trigger. The returned + * trigger will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value + * should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @return a Trigger instance that is true when the right trigger's axis exceeds the provided + * threshold, attached to the {@link Scheduler#getDefaultEventLoop() default scheduler event + * loop} on the scheduler passed to the controller's constructor, or the {@link + * Scheduler#getDefault default scheduler} if a scheduler was not explicitly provided. + */ + public Trigger rightTrigger(double threshold) { + return rightTrigger(threshold, getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance around the axis value of the right trigger. The returned trigger + * will be true when the axis value is greater than 0.5. + * + * @return a Trigger instance that is true when the right trigger's axis exceeds 0.5, attached to + * the {@link Scheduler#getDefaultEventLoop() default scheduler event loop} on the + * scheduler passed to the controller's constructor, or the {@link Scheduler#getDefault + * default scheduler} if a scheduler was not explicitly provided. + */ + public Trigger rightTrigger() { + return rightTrigger(0.5); + } + + /** + * Get the X axis value of left side of the controller. Right is positive. + * + * @return The axis value. + */ + public double getLeftX() { + return m_hid.getLeftX(); + } + + /** + * Get the X axis value of right side of the controller. Right is positive. + * + * @return The axis value. + */ + public double getRightX() { + return m_hid.getRightX(); + } + + /** + * Get the Y axis value of left side of the controller. Back is positive. + * + * @return The axis value. + */ + public double getLeftY() { + return m_hid.getLeftY(); + } + + /** + * Get the Y axis value of right side of the controller. Back is positive. + * + * @return The axis value. + */ + public double getRightY() { + return m_hid.getRightY(); + } + + /** + * Get the left trigger axis value of the controller. Note that this axis is bound to the + * range of [0, 1] as opposed to the usual [-1, 1]. + * + * @return The axis value. + */ + public double getLeftTriggerAxis() { + return m_hid.getLeftTriggerAxis(); + } + + /** + * Get the right trigger axis value of the controller. Note that this axis is bound to the + * range of [0, 1] as opposed to the usual [-1, 1]. + * + * @return The axis value. + */ + public double getRightTriggerAxis() { + return m_hid.getRightTriggerAxis(); + } +} diff --git a/commandsv3/src/generated/main/java/org/wpilib/commands3/proto/ProtobufCommands.java b/commandsv3/src/generated/main/java/org/wpilib/commands3/proto/ProtobufCommands.java new file mode 100644 index 0000000000..d0b91ff476 --- /dev/null +++ b/commandsv3/src/generated/main/java/org/wpilib/commands3/proto/ProtobufCommands.java @@ -0,0 +1,1883 @@ +// 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. +// Code generated by protocol buffer compiler. Do not edit! +package org.wpilib.commands3.proto; + +import java.io.IOException; +import us.hebi.quickbuf.Descriptors; +import us.hebi.quickbuf.FieldName; +import us.hebi.quickbuf.InvalidProtocolBufferException; +import us.hebi.quickbuf.JsonSink; +import us.hebi.quickbuf.JsonSource; +import us.hebi.quickbuf.MessageFactory; +import us.hebi.quickbuf.ProtoMessage; +import us.hebi.quickbuf.ProtoSink; +import us.hebi.quickbuf.ProtoSource; +import us.hebi.quickbuf.ProtoUtil; +import us.hebi.quickbuf.RepeatedByte; +import us.hebi.quickbuf.RepeatedMessage; +import us.hebi.quickbuf.Utf8String; + +public final class ProtobufCommands { + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(2543, + "Chdwcm90b2J1Zl9jb21tYW5kcy5wcm90bxIJd3BpLnByb3RvIicKEVByb3RvYnVmTWVjaGFuaXNtEhIK" + + "BG5hbWUYASABKAlSBG5hbWUitgIKD1Byb3RvYnVmQ29tbWFuZBIOCgJpZBgBIAEoDVICaWQSIAoJcGFy" + + "ZW50X2lkGAIgASgNSABSCHBhcmVudElkiAEBEhIKBG5hbWUYAyABKAlSBG5hbWUSGgoIcHJpb3JpdHkY" + + "BCABKAVSCHByaW9yaXR5EkAKDHJlcXVpcmVtZW50cxgFIAMoCzIcLndwaS5wcm90by5Qcm90b2J1Zk1l" + + "Y2hhbmlzbVIMcmVxdWlyZW1lbnRzEiUKDGxhc3RfdGltZV9tcxgGIAEoAUgBUgpsYXN0VGltZU1ziAEB" + + "EicKDXRvdGFsX3RpbWVfbXMYByABKAFIAlILdG90YWxUaW1lTXOIAQFCDAoKX3BhcmVudF9pZEIPCg1f" + + "bGFzdF90aW1lX21zQhAKDl90b3RhbF90aW1lX21zIsEBChFQcm90b2J1ZlNjaGVkdWxlchJDCg9xdWV1" + + "ZWRfY29tbWFuZHMYASADKAsyGi53cGkucHJvdG8uUHJvdG9idWZDb21tYW5kUg5xdWV1ZWRDb21tYW5k" + + "cxJFChBydW5uaW5nX2NvbW1hbmRzGAIgAygLMhoud3BpLnByb3RvLlByb3RvYnVmQ29tbWFuZFIPcnVu" + + "bmluZ0NvbW1hbmRzEiAKDGxhc3RfdGltZV9tcxgDIAEoAVIKbGFzdFRpbWVNc0IcChpvcmcud3BpbGli" + + "LmNvbW1hbmRzMy5wcm90b0r8DgoGEgQAADgBCggKAQwSAwAAEgoICgECEgMCABIKCAoBCBIDBAAzCgkK" + + "AggBEgMEADMK8QEKAgQAEgQQABIBMuQBCk9yIHVzZSB0aGUgZ2VuZXJhdGVfZmlsZXMucHkgc2NyaXB0" + + "OgoKIyBtYWNPUwphbGx3cGlsaWIgJCAuL2NvbW1hbmRzdjMvZ2VuZXJhdGVfZmlsZXMucHkgLS1wcm90" + + "b2M9cHJvdG9jLXF1aWNrYnVmCgojIExpbnV4CmFsbHdwaWxpYiAkIC4vY29tbWFuZHN2My9nZW5lcmF0" + + "ZV9maWxlcy5weSAtLXF1aWNrYnVmX3BsdWdpbiBwcm90b2MtZ2VuLXF1aWNrYnVmLTEuMy4zLWxpbnV4" + + "LXg4Nl82NC5leGUKCgoKAwQAARIDEAgZCgsKBAQAAgASAxECEgoMCgUEAAIABRIDEQIICgwKBQQAAgAB" + + "EgMRCQ0KDAoFBAACAAMSAxEQEQoKCgIEARIEFAAtAQoKCgMEAQESAxQIFwpxCgQEAQIAEgMXAhAaZCBB" + + "IHVuaXF1ZSBJRCBmb3IgdGhlIGNvbW1hbmQuCiBEaWZmZXJlbnQgaW52b2NhdGlvbnMgb2YgdGhlIHNh" + + "bWUgY29tbWFuZCBvYmplY3QgaGF2ZSBkaWZmZXJlbnQgSURzLgoKDAoFBAECAAUSAxcCCAoMCgUEAQIA" + + "ARIDFwkLCgwKBQQBAgADEgMXDg8KYQoEBAECARIDGwIgGlQgVGhlIElEIG9mIHRoZSBwYXJlbnQgY29t", + "bWFuZC4KIE5vdCBpbmNsdWRlZCBpbiB0aGUgbWVzc2FnZSBmb3IgdG9wLWxldmVsIGNvbW1hbmRzLgoK" + + "DAoFBAECAQQSAxsCCgoMCgUEAQIBBRIDGwsRCgwKBQQBAgEBEgMbEhsKDAoFBAECAQMSAxseHwonCgQE" + + "AQICEgMeAhIaGiBUaGUgbmFtZSBvZiB0aGUgY29tbWFuZC4KCgwKBQQBAgIFEgMeAggKDAoFBAECAgES" + + "Ax4JDQoMCgUEAQICAxIDHhARCjEKBAQBAgMSAyECFRokIFRoZSBwcmlvcml0eSBsZXZlbCBvZiB0aGUg" + + "Y29tbWFuZC4KCgwKBQQBAgMFEgMhAgcKDAoFBAECAwESAyEIEAoMCgUEAQIDAxIDIRMUCjYKBAQBAgQS" + + "AyQCLhopIFRoZSBtZWNoYW5pc21zIHJlcXVpcmVkIGJ5IHRoZSBjb21tYW5kLgoKDAoFBAECBAQSAyQC" + + "CgoMCgUEAQIEBhIDJAscCgwKBQQBAgQBEgMkHSkKDAoFBAECBAMSAyQsLQqOAQoEBAECBRIDKAIjGoAB" + + "IEhvdyBtdWNoIHRpbWUgdGhlIGNvbW1hbmQgdG9vayB0byBleGVjdXRlIGluIGl0cyBtb3N0IHJlY2Vu" + + "dCBydW4uCiBPbmx5IGluY2x1ZGVkIGluIGEgbWVzc2FnZSBmb3IgYW4gYWN0aXZlbHkgcnVubmluZyBj" + + "b21tYW5kLgoKDAoFBAECBQQSAygCCgoMCgUEAQIFBRIDKAsRCgwKBQQBAgUBEgMoEh4KDAoFBAECBQMS" + + "AyghIgqAAQoEBAECBhIDLAIkGnMgSG93IGxvbmcgdGhlIGNvbW1hbmQgaGFzIHRha2VuIHRvIHJ1biwg" + + "aW4gYWdncmVnYXRlLgogT25seSBpbmNsdWRlZCBpbiBhIG1lc3NhZ2UgZm9yIGFuIGFjdGl2ZWx5IHJ1" + + "bm5pbmcgY29tbWFuZC4KCgwKBQQBAgYEEgMsAgoKDAoFBAECBgUSAywLEQoMCgUEAQIGARIDLBIfCgwK" + + "BQQBAgYDEgMsIiMKCgoCBAISBC8AOAEKCgoDBAIBEgMvCBkKjQIKBAQCAgASAzMCLxr/ASBOb3RlOiBj" + + "b21tYW5kcyBhcmUgZ2VuZXJhbGx5IHF1ZXVlZCBieSB0cmlnZ2Vycywgd2hpY2ggb2NjdXJzIGltbWVk" + + "aWF0ZWx5IGJlZm9yZSB0aGV5IGFyZQogcHJvbW90ZWQgYW5kIHN0YXJ0IHJ1bm5pbmcuIEVudHJpZXMg" + + "d2lsbCBvbmx5IGFwcGVhciBoZXJlIHdoZW4gc2VyaWFsaXppbmcgYSBzY2hlZHVsZXIKIF9hZnRlcl8g" + + "bWFudWFsbHkgc2NoZWR1bGluZyBhIGNvbW1hbmQgYnV0IF9iZWZvcmVfIGNhbGxpbmcgc2NoZWR1bGVy" + + "LnJ1bigpCgoMCgUEAgIABBIDMwIKCgwKBQQCAgAGEgMzCxoKDAoFBAICAAESAzMbKgoMCgUEAgIAAxID" + + "My0uCgsKBAQCAgESAzQCMAoMCgUEAgIBBBIDNAIKCgwKBQQCAgEGEgM0CxoKDAoFBAICAQESAzQbKwoM", + "CgUEAgIBAxIDNC4vCk8KBAQCAgISAzcCGhpCIEhvdyBtdWNoIHRpbWUgdGhlIHNjaGVkdWxlciB0b29r" + + "IGluIGl0cyBsYXN0IGBydW4oKWAgaW52b2NhdGlvbi4KCgwKBQQCAgIFEgM3AggKDAoFBAICAgESAzcJ" + + "FQoMCgUEAgICAxIDNxgZYgZwcm90bzM="); + + static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("protobuf_commands.proto", "wpi.proto", descriptorData); + + static final Descriptors.Descriptor wpi_proto_ProtobufMechanism_descriptor = descriptor.internalContainedType(38, 39, "ProtobufMechanism", "wpi.proto.ProtobufMechanism"); + + static final Descriptors.Descriptor wpi_proto_ProtobufCommand_descriptor = descriptor.internalContainedType(80, 310, "ProtobufCommand", "wpi.proto.ProtobufCommand"); + + static final Descriptors.Descriptor wpi_proto_ProtobufScheduler_descriptor = descriptor.internalContainedType(393, 193, "ProtobufScheduler", "wpi.proto.ProtobufScheduler"); + + /** + * @return this proto file's descriptor. + */ + public static Descriptors.FileDescriptor getDescriptor() { + return descriptor; + } + + /** + * Protobuf type {@code ProtobufMechanism} + */ + public static final class ProtobufMechanism extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional string name = 1; + */ + private final Utf8String name = Utf8String.newEmptyInstance(); + + private ProtobufMechanism() { + } + + /** + * @return a new empty instance of {@code ProtobufMechanism} + */ + public static ProtobufMechanism newInstance() { + return new ProtobufMechanism(); + } + + /** + * optional string name = 1; + * @return whether the name field is set + */ + public boolean hasName() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional string name = 1; + * @return this + */ + public ProtobufMechanism clearName() { + bitField0_ &= ~0x00000001; + name.clear(); + return this; + } + + /** + * optional string name = 1; + * @return the name + */ + public String getName() { + return name.getString(); + } + + /** + * optional string name = 1; + * @return internal {@code Utf8String} representation of name for reading + */ + public Utf8String getNameBytes() { + return this.name; + } + + /** + * optional string name = 1; + * @return internal {@code Utf8String} representation of name for modifications + */ + public Utf8String getMutableNameBytes() { + bitField0_ |= 0x00000001; + return this.name; + } + + /** + * optional string name = 1; + * @param value the name to set + * @return this + */ + public ProtobufMechanism setName(final CharSequence value) { + bitField0_ |= 0x00000001; + name.copyFrom(value); + return this; + } + + /** + * optional string name = 1; + * @param value the name to set + * @return this + */ + public ProtobufMechanism setName(final Utf8String value) { + bitField0_ |= 0x00000001; + name.copyFrom(value); + return this; + } + + @Override + public ProtobufMechanism copyFrom(final ProtobufMechanism other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + name.copyFrom(other.name); + } + return this; + } + + @Override + public ProtobufMechanism mergeFrom(final ProtobufMechanism other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasName()) { + getMutableNameBytes().copyFrom(other.name); + } + return this; + } + + @Override + public ProtobufMechanism clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + name.clear(); + return this; + } + + @Override + public ProtobufMechanism clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + name.clear(); + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufMechanism)) { + return false; + } + ProtobufMechanism other = (ProtobufMechanism) o; + return bitField0_ == other.bitField0_ + && (!hasName() || name.equals(other.name)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 10); + output.writeStringNoTag(name); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 1 + ProtoSink.computeStringSizeNoTag(name); + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufMechanism mergeFrom(final ProtoSource input) throws IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 10: { + // name + input.readString(name); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeString(FieldNames.name, name); + } + output.endObject(); + } + + @Override + public ProtobufMechanism mergeFrom(final JsonSource input) throws IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case 3373707: { + if (input.isAtField(FieldNames.name)) { + if (!input.trySkipNullValue()) { + input.readString(name); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufMechanism clone() { + return new ProtobufMechanism().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufMechanism parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufMechanism(), data).checkInitialized(); + } + + public static ProtobufMechanism parseFrom(final ProtoSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufMechanism(), input).checkInitialized(); + } + + public static ProtobufMechanism parseFrom(final JsonSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufMechanism(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufMechanism messages + */ + public static MessageFactory getFactory() { + return ProtobufMechanismFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return ProtobufCommands.wpi_proto_ProtobufMechanism_descriptor; + } + + private enum ProtobufMechanismFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufMechanism create() { + return ProtobufMechanism.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName name = FieldName.forField("name"); + } + } + + /** + * Protobuf type {@code ProtobufCommand} + */ + public static final class ProtobufCommand extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * 
+     *  How much time the command took to execute in its most recent run.
+     *  Only included in a message for an actively running command.
+     *
+ * + * optional double last_time_ms = 6; + */ + private double lastTimeMs; + + /** + * 
+     *  How long the command has taken to run, in aggregate.
+     *  Only included in a message for an actively running command.
+     *
+ * + * optional double total_time_ms = 7; + */ + private double totalTimeMs; + + /** + * 
+     *  The priority level of the command.
+     *
+ * + * optional int32 priority = 4; + */ + private int priority; + + /** + * 
+     *  A unique ID for the command.
+     *  Different invocations of the same command object have different IDs.
+     *
+ * + * optional uint32 id = 1; + */ + private int id; + + /** + * 
+     *  The ID of the parent command.
+     *  Not included in the message for top-level commands.
+     *
+ * + * optional uint32 parent_id = 2; + */ + private int parentId; + + /** + * 
+     *  The name of the command.
+     *
+ * + * optional string name = 3; + */ + private final Utf8String name = Utf8String.newEmptyInstance(); + + /** + * 
+     *  The mechanisms required by the command.
+     *
+ * + * repeated .wpi.proto.ProtobufMechanism requirements = 5; + */ + private final RepeatedMessage requirements = RepeatedMessage.newEmptyInstance(ProtobufMechanism.getFactory()); + + private ProtobufCommand() { + } + + /** + * @return a new empty instance of {@code ProtobufCommand} + */ + public static ProtobufCommand newInstance() { + return new ProtobufCommand(); + } + + /** + * 
+     *  How much time the command took to execute in its most recent run.
+     *  Only included in a message for an actively running command.
+     *
+ * + * optional double last_time_ms = 6; + * @return whether the lastTimeMs field is set + */ + public boolean hasLastTimeMs() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * 
+     *  How much time the command took to execute in its most recent run.
+     *  Only included in a message for an actively running command.
+     *
+ * + * optional double last_time_ms = 6; + * @return this + */ + public ProtobufCommand clearLastTimeMs() { + bitField0_ &= ~0x00000002; + lastTimeMs = 0D; + return this; + } + + /** + * 
+     *  How much time the command took to execute in its most recent run.
+     *  Only included in a message for an actively running command.
+     *
+ * + * optional double last_time_ms = 6; + * @return the lastTimeMs + */ + public double getLastTimeMs() { + return lastTimeMs; + } + + /** + * 
+     *  How much time the command took to execute in its most recent run.
+     *  Only included in a message for an actively running command.
+     *
+ * + * optional double last_time_ms = 6; + * @param value the lastTimeMs to set + * @return this + */ + public ProtobufCommand setLastTimeMs(final double value) { + bitField0_ |= 0x00000002; + lastTimeMs = value; + return this; + } + + /** + * 
+     *  How long the command has taken to run, in aggregate.
+     *  Only included in a message for an actively running command.
+     *
+ * + * optional double total_time_ms = 7; + * @return whether the totalTimeMs field is set + */ + public boolean hasTotalTimeMs() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * 
+     *  How long the command has taken to run, in aggregate.
+     *  Only included in a message for an actively running command.
+     *
+ * + * optional double total_time_ms = 7; + * @return this + */ + public ProtobufCommand clearTotalTimeMs() { + bitField0_ &= ~0x00000001; + totalTimeMs = 0D; + return this; + } + + /** + * 
+     *  How long the command has taken to run, in aggregate.
+     *  Only included in a message for an actively running command.
+     *
+ * + * optional double total_time_ms = 7; + * @return the totalTimeMs + */ + public double getTotalTimeMs() { + return totalTimeMs; + } + + /** + * 
+     *  How long the command has taken to run, in aggregate.
+     *  Only included in a message for an actively running command.
+     *
+ * + * optional double total_time_ms = 7; + * @param value the totalTimeMs to set + * @return this + */ + public ProtobufCommand setTotalTimeMs(final double value) { + bitField0_ |= 0x00000001; + totalTimeMs = value; + return this; + } + + /** + * 
+     *  The priority level of the command.
+     *
+ * + * optional int32 priority = 4; + * @return whether the priority field is set + */ + public boolean hasPriority() { + return (bitField0_ & 0x00000008) != 0; + } + + /** + * 
+     *  The priority level of the command.
+     *
+ * + * optional int32 priority = 4; + * @return this + */ + public ProtobufCommand clearPriority() { + bitField0_ &= ~0x00000008; + priority = 0; + return this; + } + + /** + * 
+     *  The priority level of the command.
+     *
+ * + * optional int32 priority = 4; + * @return the priority + */ + public int getPriority() { + return priority; + } + + /** + * 
+     *  The priority level of the command.
+     *
+ * + * optional int32 priority = 4; + * @param value the priority to set + * @return this + */ + public ProtobufCommand setPriority(final int value) { + bitField0_ |= 0x00000008; + priority = value; + return this; + } + + /** + * 
+     *  A unique ID for the command.
+     *  Different invocations of the same command object have different IDs.
+     *
+ * + * optional uint32 id = 1; + * @return whether the id field is set + */ + public boolean hasId() { + return (bitField0_ & 0x00000010) != 0; + } + + /** + * 
+     *  A unique ID for the command.
+     *  Different invocations of the same command object have different IDs.
+     *
+ * + * optional uint32 id = 1; + * @return this + */ + public ProtobufCommand clearId() { + bitField0_ &= ~0x00000010; + id = 0; + return this; + } + + /** + * 
+     *  A unique ID for the command.
+     *  Different invocations of the same command object have different IDs.
+     *
+ * + * optional uint32 id = 1; + * @return the id + */ + public int getId() { + return id; + } + + /** + * 
+     *  A unique ID for the command.
+     *  Different invocations of the same command object have different IDs.
+     *
+ * + * optional uint32 id = 1; + * @param value the id to set + * @return this + */ + public ProtobufCommand setId(final int value) { + bitField0_ |= 0x00000010; + id = value; + return this; + } + + /** + * 
+     *  The ID of the parent command.
+     *  Not included in the message for top-level commands.
+     *
+ * + * optional uint32 parent_id = 2; + * @return whether the parentId field is set + */ + public boolean hasParentId() { + return (bitField0_ & 0x00000004) != 0; + } + + /** + * 
+     *  The ID of the parent command.
+     *  Not included in the message for top-level commands.
+     *
+ * + * optional uint32 parent_id = 2; + * @return this + */ + public ProtobufCommand clearParentId() { + bitField0_ &= ~0x00000004; + parentId = 0; + return this; + } + + /** + * 
+     *  The ID of the parent command.
+     *  Not included in the message for top-level commands.
+     *
+ * + * optional uint32 parent_id = 2; + * @return the parentId + */ + public int getParentId() { + return parentId; + } + + /** + * 
+     *  The ID of the parent command.
+     *  Not included in the message for top-level commands.
+     *
+ * + * optional uint32 parent_id = 2; + * @param value the parentId to set + * @return this + */ + public ProtobufCommand setParentId(final int value) { + bitField0_ |= 0x00000004; + parentId = value; + return this; + } + + /** + * 
+     *  The name of the command.
+     *
+ * + * optional string name = 3; + * @return whether the name field is set + */ + public boolean hasName() { + return (bitField0_ & 0x00000020) != 0; + } + + /** + * 
+     *  The name of the command.
+     *
+ * + * optional string name = 3; + * @return this + */ + public ProtobufCommand clearName() { + bitField0_ &= ~0x00000020; + name.clear(); + return this; + } + + /** + * 
+     *  The name of the command.
+     *
+ * + * optional string name = 3; + * @return the name + */ + public String getName() { + return name.getString(); + } + + /** + * 
+     *  The name of the command.
+     *
+ * + * optional string name = 3; + * @return internal {@code Utf8String} representation of name for reading + */ + public Utf8String getNameBytes() { + return this.name; + } + + /** + * 
+     *  The name of the command.
+     *
+ * + * optional string name = 3; + * @return internal {@code Utf8String} representation of name for modifications + */ + public Utf8String getMutableNameBytes() { + bitField0_ |= 0x00000020; + return this.name; + } + + /** + * 
+     *  The name of the command.
+     *
+ * + * optional string name = 3; + * @param value the name to set + * @return this + */ + public ProtobufCommand setName(final CharSequence value) { + bitField0_ |= 0x00000020; + name.copyFrom(value); + return this; + } + + /** + * 
+     *  The name of the command.
+     *
+ * + * optional string name = 3; + * @param value the name to set + * @return this + */ + public ProtobufCommand setName(final Utf8String value) { + bitField0_ |= 0x00000020; + name.copyFrom(value); + return this; + } + + /** + * 
+     *  The mechanisms required by the command.
+     *
+ * + * repeated .wpi.proto.ProtobufMechanism requirements = 5; + * @return whether the requirements field is set + */ + public boolean hasRequirements() { + return (bitField0_ & 0x00000040) != 0; + } + + /** + * 
+     *  The mechanisms required by the command.
+     *
+ * + * repeated .wpi.proto.ProtobufMechanism requirements = 5; + * @return this + */ + public ProtobufCommand clearRequirements() { + bitField0_ &= ~0x00000040; + requirements.clear(); + return this; + } + + /** + * 
+     *  The mechanisms required by the command.
+     *
+ * + * repeated .wpi.proto.ProtobufMechanism requirements = 5; + * + * This method returns the internal storage object without modifying any has state. + * The returned object should not be modified and be treated as read-only. + * + * Use {@link #getMutableRequirements()} if you want to modify it. + * + * @return internal storage object for reading + */ + public RepeatedMessage getRequirements() { + return requirements; + } + + /** + * 
+     *  The mechanisms required by the command.
+     *
+ * + * repeated .wpi.proto.ProtobufMechanism requirements = 5; + * + * This method returns the internal storage object and sets the corresponding + * has state. The returned object will become part of this message and its + * contents may be modified as long as the has state is not cleared. + * + * @return internal storage object for modifications + */ + public RepeatedMessage getMutableRequirements() { + bitField0_ |= 0x00000040; + return requirements; + } + + /** + * 
+     *  The mechanisms required by the command.
+     *
+ * + * repeated .wpi.proto.ProtobufMechanism requirements = 5; + * @param value the requirements to add + * @return this + */ + public ProtobufCommand addRequirements(final ProtobufMechanism value) { + bitField0_ |= 0x00000040; + requirements.add(value); + return this; + } + + /** + * 
+     *  The mechanisms required by the command.
+     *
+ * + * repeated .wpi.proto.ProtobufMechanism requirements = 5; + * @param values the requirements to add + * @return this + */ + public ProtobufCommand addAllRequirements(final ProtobufMechanism... values) { + bitField0_ |= 0x00000040; + requirements.addAll(values); + return this; + } + + @Override + public ProtobufCommand copyFrom(final ProtobufCommand other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + lastTimeMs = other.lastTimeMs; + totalTimeMs = other.totalTimeMs; + priority = other.priority; + id = other.id; + parentId = other.parentId; + name.copyFrom(other.name); + requirements.copyFrom(other.requirements); + } + return this; + } + + @Override + public ProtobufCommand mergeFrom(final ProtobufCommand other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasLastTimeMs()) { + setLastTimeMs(other.lastTimeMs); + } + if (other.hasTotalTimeMs()) { + setTotalTimeMs(other.totalTimeMs); + } + if (other.hasPriority()) { + setPriority(other.priority); + } + if (other.hasId()) { + setId(other.id); + } + if (other.hasParentId()) { + setParentId(other.parentId); + } + if (other.hasName()) { + getMutableNameBytes().copyFrom(other.name); + } + if (other.hasRequirements()) { + getMutableRequirements().addAll(other.requirements); + } + return this; + } + + @Override + public ProtobufCommand clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + lastTimeMs = 0D; + totalTimeMs = 0D; + priority = 0; + id = 0; + parentId = 0; + name.clear(); + requirements.clear(); + return this; + } + + @Override + public ProtobufCommand clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + name.clear(); + requirements.clearQuick(); + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufCommand)) { + return false; + } + ProtobufCommand other = (ProtobufCommand) o; + return bitField0_ == other.bitField0_ + && (!hasLastTimeMs() || ProtoUtil.isEqual(lastTimeMs, other.lastTimeMs)) + && (!hasTotalTimeMs() || ProtoUtil.isEqual(totalTimeMs, other.totalTimeMs)) + && (!hasPriority() || priority == other.priority) + && (!hasId() || id == other.id) + && (!hasParentId() || parentId == other.parentId) + && (!hasName() || name.equals(other.name)) + && (!hasRequirements() || requirements.equals(other.requirements)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 49); + output.writeDoubleNoTag(lastTimeMs); + } + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 57); + output.writeDoubleNoTag(totalTimeMs); + } + if ((bitField0_ & 0x00000008) != 0) { + output.writeRawByte((byte) 32); + output.writeInt32NoTag(priority); + } + if ((bitField0_ & 0x00000010) != 0) { + output.writeRawByte((byte) 8); + output.writeUInt32NoTag(id); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeRawByte((byte) 16); + output.writeUInt32NoTag(parentId); + } + if ((bitField0_ & 0x00000020) != 0) { + output.writeRawByte((byte) 26); + output.writeStringNoTag(name); + } + if ((bitField0_ & 0x00000040) != 0) { + for (int i = 0; i < requirements.length(); i++) { + output.writeRawByte((byte) 42); + output.writeMessageNoTag(requirements.get(i)); + } + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000002) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000008) != 0) { + size += 1 + ProtoSink.computeInt32SizeNoTag(priority); + } + if ((bitField0_ & 0x00000010) != 0) { + size += 1 + ProtoSink.computeUInt32SizeNoTag(id); + } + if ((bitField0_ & 0x00000004) != 0) { + size += 1 + ProtoSink.computeUInt32SizeNoTag(parentId); + } + if ((bitField0_ & 0x00000020) != 0) { + size += 1 + ProtoSink.computeStringSizeNoTag(name); + } + if ((bitField0_ & 0x00000040) != 0) { + size += (1 * requirements.length()) + ProtoSink.computeRepeatedMessageSizeNoTag(requirements); + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufCommand mergeFrom(final ProtoSource input) throws IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 49: { + // lastTimeMs + lastTimeMs = input.readDouble(); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 57) { + break; + } + } + case 57: { + // totalTimeMs + totalTimeMs = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 32) { + break; + } + } + case 32: { + // priority + priority = input.readInt32(); + bitField0_ |= 0x00000008; + tag = input.readTag(); + if (tag != 8) { + break; + } + } + case 8: { + // id + id = input.readUInt32(); + bitField0_ |= 0x00000010; + tag = input.readTag(); + if (tag != 16) { + break; + } + } + case 16: { + // parentId + parentId = input.readUInt32(); + bitField0_ |= 0x00000004; + tag = input.readTag(); + if (tag != 26) { + break; + } + } + case 26: { + // name + input.readString(name); + bitField0_ |= 0x00000020; + tag = input.readTag(); + if (tag != 42) { + break; + } + } + case 42: { + // requirements + tag = input.readRepeatedMessage(requirements, tag); + bitField0_ |= 0x00000040; + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.lastTimeMs, lastTimeMs); + } + if ((bitField0_ & 0x00000001) != 0) { + output.writeDouble(FieldNames.totalTimeMs, totalTimeMs); + } + if ((bitField0_ & 0x00000008) != 0) { + output.writeInt32(FieldNames.priority, priority); + } + if ((bitField0_ & 0x00000010) != 0) { + output.writeUInt32(FieldNames.id, id); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeUInt32(FieldNames.parentId, parentId); + } + if ((bitField0_ & 0x00000020) != 0) { + output.writeString(FieldNames.name, name); + } + if ((bitField0_ & 0x00000040) != 0) { + output.writeRepeatedMessage(FieldNames.requirements, requirements); + } + output.endObject(); + } + + @Override + public ProtobufCommand mergeFrom(final JsonSource input) throws IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case 1958056841: + case -740797521: { + if (input.isAtField(FieldNames.lastTimeMs)) { + if (!input.trySkipNullValue()) { + lastTimeMs = input.readDouble(); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + case -717217353: + case 1006112349: { + if (input.isAtField(FieldNames.totalTimeMs)) { + if (!input.trySkipNullValue()) { + totalTimeMs = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case -1165461084: { + if (input.isAtField(FieldNames.priority)) { + if (!input.trySkipNullValue()) { + priority = input.readInt32(); + bitField0_ |= 0x00000008; + } + } else { + input.skipUnknownField(); + } + break; + } + case 3355: { + if (input.isAtField(FieldNames.id)) { + if (!input.trySkipNullValue()) { + id = input.readUInt32(); + bitField0_ |= 0x00000010; + } + } else { + input.skipUnknownField(); + } + break; + } + case 1175162725: + case 2070327504: { + if (input.isAtField(FieldNames.parentId)) { + if (!input.trySkipNullValue()) { + parentId = input.readUInt32(); + bitField0_ |= 0x00000004; + } + } else { + input.skipUnknownField(); + } + break; + } + case 3373707: { + if (input.isAtField(FieldNames.name)) { + if (!input.trySkipNullValue()) { + input.readString(name); + bitField0_ |= 0x00000020; + } + } else { + input.skipUnknownField(); + } + break; + } + case -1619874672: { + if (input.isAtField(FieldNames.requirements)) { + if (!input.trySkipNullValue()) { + input.readRepeatedMessage(requirements); + bitField0_ |= 0x00000040; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufCommand clone() { + return new ProtobufCommand().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufCommand parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufCommand(), data).checkInitialized(); + } + + public static ProtobufCommand parseFrom(final ProtoSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufCommand(), input).checkInitialized(); + } + + public static ProtobufCommand parseFrom(final JsonSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufCommand(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufCommand messages + */ + public static MessageFactory getFactory() { + return ProtobufCommandFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return ProtobufCommands.wpi_proto_ProtobufCommand_descriptor; + } + + private enum ProtobufCommandFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufCommand create() { + return ProtobufCommand.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName lastTimeMs = FieldName.forField("lastTimeMs", "last_time_ms"); + + static final FieldName totalTimeMs = FieldName.forField("totalTimeMs", "total_time_ms"); + + static final FieldName priority = FieldName.forField("priority"); + + static final FieldName id = FieldName.forField("id"); + + static final FieldName parentId = FieldName.forField("parentId", "parent_id"); + + static final FieldName name = FieldName.forField("name"); + + static final FieldName requirements = FieldName.forField("requirements"); + } + } + + /** + * Protobuf type {@code ProtobufScheduler} + */ + public static final class ProtobufScheduler extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * 
+     *  How much time the scheduler took in its last `run()` invocation.
+     *
+ * + * optional double last_time_ms = 3; + */ + private double lastTimeMs; + + /** + * 
+     *  Note: commands are generally queued by triggers, which occurs immediately before they are
+     *  promoted and start running. Entries will only appear here when serializing a scheduler
+     *  _after_ manually scheduling a command but _before_ calling scheduler.run()
+     *
+ * + * repeated .wpi.proto.ProtobufCommand queued_commands = 1; + */ + private final RepeatedMessage queuedCommands = RepeatedMessage.newEmptyInstance(ProtobufCommand.getFactory()); + + /** + * repeated .wpi.proto.ProtobufCommand running_commands = 2; + */ + private final RepeatedMessage runningCommands = RepeatedMessage.newEmptyInstance(ProtobufCommand.getFactory()); + + private ProtobufScheduler() { + } + + /** + * @return a new empty instance of {@code ProtobufScheduler} + */ + public static ProtobufScheduler newInstance() { + return new ProtobufScheduler(); + } + + /** + * 
+     *  How much time the scheduler took in its last `run()` invocation.
+     *
+ * + * optional double last_time_ms = 3; + * @return whether the lastTimeMs field is set + */ + public boolean hasLastTimeMs() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * 
+     *  How much time the scheduler took in its last `run()` invocation.
+     *
+ * + * optional double last_time_ms = 3; + * @return this + */ + public ProtobufScheduler clearLastTimeMs() { + bitField0_ &= ~0x00000001; + lastTimeMs = 0D; + return this; + } + + /** + * 
+     *  How much time the scheduler took in its last `run()` invocation.
+     *
+ * + * optional double last_time_ms = 3; + * @return the lastTimeMs + */ + public double getLastTimeMs() { + return lastTimeMs; + } + + /** + * 
+     *  How much time the scheduler took in its last `run()` invocation.
+     *
+ * + * optional double last_time_ms = 3; + * @param value the lastTimeMs to set + * @return this + */ + public ProtobufScheduler setLastTimeMs(final double value) { + bitField0_ |= 0x00000001; + lastTimeMs = value; + return this; + } + + /** + * 
+     *  Note: commands are generally queued by triggers, which occurs immediately before they are
+     *  promoted and start running. Entries will only appear here when serializing a scheduler
+     *  _after_ manually scheduling a command but _before_ calling scheduler.run()
+     *
+ * + * repeated .wpi.proto.ProtobufCommand queued_commands = 1; + * @return whether the queuedCommands field is set + */ + public boolean hasQueuedCommands() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * 
+     *  Note: commands are generally queued by triggers, which occurs immediately before they are
+     *  promoted and start running. Entries will only appear here when serializing a scheduler
+     *  _after_ manually scheduling a command but _before_ calling scheduler.run()
+     *
+ * + * repeated .wpi.proto.ProtobufCommand queued_commands = 1; + * @return this + */ + public ProtobufScheduler clearQueuedCommands() { + bitField0_ &= ~0x00000002; + queuedCommands.clear(); + return this; + } + + /** + * 
+     *  Note: commands are generally queued by triggers, which occurs immediately before they are
+     *  promoted and start running. Entries will only appear here when serializing a scheduler
+     *  _after_ manually scheduling a command but _before_ calling scheduler.run()
+     *
+ * + * repeated .wpi.proto.ProtobufCommand queued_commands = 1; + * + * This method returns the internal storage object without modifying any has state. + * The returned object should not be modified and be treated as read-only. + * + * Use {@link #getMutableQueuedCommands()} if you want to modify it. + * + * @return internal storage object for reading + */ + public RepeatedMessage getQueuedCommands() { + return queuedCommands; + } + + /** + * 
+     *  Note: commands are generally queued by triggers, which occurs immediately before they are
+     *  promoted and start running. Entries will only appear here when serializing a scheduler
+     *  _after_ manually scheduling a command but _before_ calling scheduler.run()
+     *
+ * + * repeated .wpi.proto.ProtobufCommand queued_commands = 1; + * + * This method returns the internal storage object and sets the corresponding + * has state. The returned object will become part of this message and its + * contents may be modified as long as the has state is not cleared. + * + * @return internal storage object for modifications + */ + public RepeatedMessage getMutableQueuedCommands() { + bitField0_ |= 0x00000002; + return queuedCommands; + } + + /** + * 
+     *  Note: commands are generally queued by triggers, which occurs immediately before they are
+     *  promoted and start running. Entries will only appear here when serializing a scheduler
+     *  _after_ manually scheduling a command but _before_ calling scheduler.run()
+     *
+ * + * repeated .wpi.proto.ProtobufCommand queued_commands = 1; + * @param value the queuedCommands to add + * @return this + */ + public ProtobufScheduler addQueuedCommands(final ProtobufCommand value) { + bitField0_ |= 0x00000002; + queuedCommands.add(value); + return this; + } + + /** + * 
+     *  Note: commands are generally queued by triggers, which occurs immediately before they are
+     *  promoted and start running. Entries will only appear here when serializing a scheduler
+     *  _after_ manually scheduling a command but _before_ calling scheduler.run()
+     *
+ * + * repeated .wpi.proto.ProtobufCommand queued_commands = 1; + * @param values the queuedCommands to add + * @return this + */ + public ProtobufScheduler addAllQueuedCommands(final ProtobufCommand... values) { + bitField0_ |= 0x00000002; + queuedCommands.addAll(values); + return this; + } + + /** + * repeated .wpi.proto.ProtobufCommand running_commands = 2; + * @return whether the runningCommands field is set + */ + public boolean hasRunningCommands() { + return (bitField0_ & 0x00000004) != 0; + } + + /** + * repeated .wpi.proto.ProtobufCommand running_commands = 2; + * @return this + */ + public ProtobufScheduler clearRunningCommands() { + bitField0_ &= ~0x00000004; + runningCommands.clear(); + return this; + } + + /** + * repeated .wpi.proto.ProtobufCommand running_commands = 2; + * + * This method returns the internal storage object without modifying any has state. + * The returned object should not be modified and be treated as read-only. + * + * Use {@link #getMutableRunningCommands()} if you want to modify it. + * + * @return internal storage object for reading + */ + public RepeatedMessage getRunningCommands() { + return runningCommands; + } + + /** + * repeated .wpi.proto.ProtobufCommand running_commands = 2; + * + * This method returns the internal storage object and sets the corresponding + * has state. The returned object will become part of this message and its + * contents may be modified as long as the has state is not cleared. + * + * @return internal storage object for modifications + */ + public RepeatedMessage getMutableRunningCommands() { + bitField0_ |= 0x00000004; + return runningCommands; + } + + /** + * repeated .wpi.proto.ProtobufCommand running_commands = 2; + * @param value the runningCommands to add + * @return this + */ + public ProtobufScheduler addRunningCommands(final ProtobufCommand value) { + bitField0_ |= 0x00000004; + runningCommands.add(value); + return this; + } + + /** + * repeated .wpi.proto.ProtobufCommand running_commands = 2; + * @param values the runningCommands to add + * @return this + */ + public ProtobufScheduler addAllRunningCommands(final ProtobufCommand... values) { + bitField0_ |= 0x00000004; + runningCommands.addAll(values); + return this; + } + + @Override + public ProtobufScheduler copyFrom(final ProtobufScheduler other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + lastTimeMs = other.lastTimeMs; + queuedCommands.copyFrom(other.queuedCommands); + runningCommands.copyFrom(other.runningCommands); + } + return this; + } + + @Override + public ProtobufScheduler mergeFrom(final ProtobufScheduler other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasLastTimeMs()) { + setLastTimeMs(other.lastTimeMs); + } + if (other.hasQueuedCommands()) { + getMutableQueuedCommands().addAll(other.queuedCommands); + } + if (other.hasRunningCommands()) { + getMutableRunningCommands().addAll(other.runningCommands); + } + return this; + } + + @Override + public ProtobufScheduler clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + lastTimeMs = 0D; + queuedCommands.clear(); + runningCommands.clear(); + return this; + } + + @Override + public ProtobufScheduler clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + queuedCommands.clearQuick(); + runningCommands.clearQuick(); + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufScheduler)) { + return false; + } + ProtobufScheduler other = (ProtobufScheduler) o; + return bitField0_ == other.bitField0_ + && (!hasLastTimeMs() || ProtoUtil.isEqual(lastTimeMs, other.lastTimeMs)) + && (!hasQueuedCommands() || queuedCommands.equals(other.queuedCommands)) + && (!hasRunningCommands() || runningCommands.equals(other.runningCommands)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 25); + output.writeDoubleNoTag(lastTimeMs); + } + if ((bitField0_ & 0x00000002) != 0) { + for (int i = 0; i < queuedCommands.length(); i++) { + output.writeRawByte((byte) 10); + output.writeMessageNoTag(queuedCommands.get(i)); + } + } + if ((bitField0_ & 0x00000004) != 0) { + for (int i = 0; i < runningCommands.length(); i++) { + output.writeRawByte((byte) 18); + output.writeMessageNoTag(runningCommands.get(i)); + } + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000002) != 0) { + size += (1 * queuedCommands.length()) + ProtoSink.computeRepeatedMessageSizeNoTag(queuedCommands); + } + if ((bitField0_ & 0x00000004) != 0) { + size += (1 * runningCommands.length()) + ProtoSink.computeRepeatedMessageSizeNoTag(runningCommands); + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufScheduler mergeFrom(final ProtoSource input) throws IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 25: { + // lastTimeMs + lastTimeMs = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 10) { + break; + } + } + case 10: { + // queuedCommands + tag = input.readRepeatedMessage(queuedCommands, tag); + bitField0_ |= 0x00000002; + if (tag != 18) { + break; + } + } + case 18: { + // runningCommands + tag = input.readRepeatedMessage(runningCommands, tag); + bitField0_ |= 0x00000004; + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeDouble(FieldNames.lastTimeMs, lastTimeMs); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRepeatedMessage(FieldNames.queuedCommands, queuedCommands); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeRepeatedMessage(FieldNames.runningCommands, runningCommands); + } + output.endObject(); + } + + @Override + public ProtobufScheduler mergeFrom(final JsonSource input) throws IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case 1958056841: + case -740797521: { + if (input.isAtField(FieldNames.lastTimeMs)) { + if (!input.trySkipNullValue()) { + lastTimeMs = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case -167160549: + case -1904270380: { + if (input.isAtField(FieldNames.queuedCommands)) { + if (!input.trySkipNullValue()) { + input.readRepeatedMessage(queuedCommands); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + case -1719052953: + case 1526672648: { + if (input.isAtField(FieldNames.runningCommands)) { + if (!input.trySkipNullValue()) { + input.readRepeatedMessage(runningCommands); + bitField0_ |= 0x00000004; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufScheduler clone() { + return new ProtobufScheduler().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufScheduler parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufScheduler(), data).checkInitialized(); + } + + public static ProtobufScheduler parseFrom(final ProtoSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufScheduler(), input).checkInitialized(); + } + + public static ProtobufScheduler parseFrom(final JsonSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufScheduler(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufScheduler messages + */ + public static MessageFactory getFactory() { + return ProtobufSchedulerFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return ProtobufCommands.wpi_proto_ProtobufScheduler_descriptor; + } + + private enum ProtobufSchedulerFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufScheduler create() { + return ProtobufScheduler.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName lastTimeMs = FieldName.forField("lastTimeMs", "last_time_ms"); + + static final FieldName queuedCommands = FieldName.forField("queuedCommands", "queued_commands"); + + static final FieldName runningCommands = FieldName.forField("runningCommands", "running_commands"); + } + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/Binding.java b/commandsv3/src/main/java/org/wpilib/commands3/Binding.java new file mode 100644 index 0000000000..69943c22c5 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/Binding.java @@ -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. + +package org.wpilib.commands3; + +import edu.wpi.first.util.ErrorMessages; + +/** + * A single trigger binding. + * + * @param scope The scope in which the binding is active. + * @param type The type of binding; or, when the bound command should run + * @param command The bound command. Cannot be null. + * @param frames The stack frames when the binding was created. Used for telemetry and error + * reporting so if a command throws an exception, we can tell users where that command was bound + * instead of giving a fairly useless backtrace of the command framework. + */ +record Binding(BindingScope scope, BindingType type, Command command, StackTraceElement[] frames) { + public Binding { + ErrorMessages.requireNonNullParam(scope, "scope", "Binding"); + ErrorMessages.requireNonNullParam(type, "type", "Binding"); + ErrorMessages.requireNonNullParam(command, "command", "Binding"); + ErrorMessages.requireNonNullParam(frames, "frames", "Binding"); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/BindingScope.java b/commandsv3/src/main/java/org/wpilib/commands3/BindingScope.java new file mode 100644 index 0000000000..3925aca807 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/BindingScope.java @@ -0,0 +1,52 @@ +// 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.commands3; + +/** + * A scope for when a binding is live. Bindings tied to a scope must be deleted when the scope + * becomes inactive. + */ +@SuppressWarnings("PMD.ImplicitFunctionalInterface") +interface BindingScope { + /** + * Checks if the scope is active. Bindings for inactive scopes are removed from the scheduler. + * + * @return True if the scope is still active, false if not. + */ + boolean active(); + + static BindingScope global() { + return Global.INSTANCE; + } + + static BindingScope forCommand(Scheduler scheduler, Command command) { + return new ForCommand(scheduler, command); + } + + /** A global binding scope. Bindings in this scope are always active. */ + final class Global implements BindingScope { + // No reason not to be a singleton. + public static final Global INSTANCE = new Global(); + + @Override + public boolean active() { + return true; + } + } + + /** + * A binding scoped to the lifetime of a specific command. This should be used when a binding is + * created within a command, tying the lifetime of the binding to the declaring command. + * + * @param scheduler The scheduler managing the command. + * @param command The command being scoped to. + */ + record ForCommand(Scheduler scheduler, Command command) implements BindingScope { + @Override + public boolean active() { + return scheduler.isRunning(command); + } + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/BindingType.java b/commandsv3/src/main/java/org/wpilib/commands3/BindingType.java new file mode 100644 index 0000000000..188c39acb8 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/BindingType.java @@ -0,0 +1,46 @@ +// 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.commands3; + +/** Describes when a command bound to a trigger should run. */ +enum BindingType { + /** + * An immediate or manual binding created when calling {@link Scheduler#schedule(Command)} + * directly, without using a trigger to bind it to a signal. + */ + IMMEDIATE, + /** + * Schedules (forks) a command on a rising edge signal. The command will run until it completes or + * is interrupted by another command requiring the same mechanisms. + */ + SCHEDULE_ON_RISING_EDGE, + /** + * Schedules (forks) a command on a falling edge signal. The command will run until it completes + * or is interrupted by another command requiring the same mechanisms. + */ + SCHEDULE_ON_FALLING_EDGE, + /** + * Schedules (forks) a command on a rising edge signal. If the command is still running on the + * next rising edge, it will be canceled then; otherwise, it will be scheduled again. + */ + TOGGLE_ON_RISING_EDGE, + /** + * Schedules (forks) a command on a falling edge signal. If the command is still running on the + * next falling edge, it will be canceled then; otherwise, it will be scheduled again. + */ + TOGGLE_ON_FALLING_EDGE, + /** + * Schedules a command on a rising edge signal. If the command is still running on the next + * falling edge, it will be canceled then - unlike {@link #SCHEDULE_ON_RISING_EDGE}, which would + * allow it to continue to run. + */ + RUN_WHILE_HIGH, + /** + * Schedules a command on a falling edge signal. If the command is still running on the next + * rising edge, it will be canceled then - unlike {@link #SCHEDULE_ON_FALLING_EDGE}, which would + * allow it to continue to run. + */ + RUN_WHILE_LOW +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/Command.java b/commandsv3/src/main/java/org/wpilib/commands3/Command.java new file mode 100644 index 0000000000..4e75a6918b --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/Command.java @@ -0,0 +1,416 @@ +// 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.commands3; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.units.measure.Time; +import java.util.Collection; +import java.util.Collections; +import java.util.Set; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import org.wpilib.annotation.NoDiscard; + +/** + * Performs some task using one or more {@link Mechanism mechanisms}. Commands are fundamentally + * backed by a {@link Coroutine} that can be used to write imperative code that runs asynchronously. + * + *

Programmers familiar with the earlier versions of the command framework can think of a v3 + * command similar to a v1 or v2 command that executes the lifecycle methods in a single method, as + * demonstrated in the example below. (Note, however, that more sophisticated code than this is + * possible! + * + * 

{@code
+ * coroutine -> {
+ *   initialize();
+ *   while (!isFinished()) {
+ *     execute();
+ *     coroutine.yield(); // be sure to yield at the end of the loop
+ *   }
+ *   end();
+ * }
+ * }
+ * + *

Note: Because coroutines are opt-in collaborate constructs, every + * command implementation must call {@link Coroutine#yield()} within any periodic + * loop. Failure to do so may result in an unrecoverable infinite loop. + * + *

Requirements

+ * + *

Commands require zero or more mechanisms. To prevent conflicting control requests from running + * simultaneously (for example, commanding an elevator to both raise and lower at the same time), a + * running command has exclusive ownership of all of its required mechanisms. If another + * command with an equal or greater {@link #priority()} is scheduled that requires one or more of + * those same mechanisms, it will interrupt and cancel the running command. + * + *

The recommended way to create a command is using {@link Mechanism#run(Consumer)} or a related + * factory method to create commands that require a single mechanism (for example, a command that + * drives an elevator up and down or rotates an arm). Commands may be composed into {@link + * ParallelGroup parallel groups} and {@link SequentialGroup sequences} to build more complex + * behavior out of fundamental building blocks. These built-in compositions will require every + * mechanism used by every command in them, even if those commands aren't always running, and thus + * can leave certain required mechanisms in an uncommanded state: owned, but not used, this + * can lead to mechanisms sagging under gravity or running the previous motor control request they + * were given. + * + *

Advanced Usage

+ * + *

For example, a hypothetical drive-and-score sequence could be coded in two ways: one with a + * sequence chain, or one that uses the lower-level coroutine API. Coroutines can be used in an + * async/await style that you may be familiar with from languages like JavaScript, Python, or C# + * (note that there is no {@code async} keyword; commands are inherently asynchronous). Nested + * commands may be forked and awaited, but will not outlive the command that forked them; there is + * no analog for something like a {@code ScheduleCommand} from the v2 commands framework. + * + * 

{@code
+ * class Robot extends TimedRobot {
+ *   private final Drivetrain drivetrain = new Drivetrain();
+ *   private final Elevator elevator = new Elevator();
+ *   private final Gripper gripper = new Gripper();
+ *
+ *  // Basic sequence builder - owns all 3 mechanisms for the full duration,
+ *  // even when they're not being used
+ *  private Command basicScoringSequence() {
+ *     return drivetrain.driveToScoringLocation()
+ *       .andThen(elevator.moveToScoringHeight())
+ *       .andThen(gripper.release())
+ *       .named("Scoring Sequence (Basic)");
+ *   }
+ *
+ *   // Advanced sequence with async/await - only owns mechanisms while they're
+ *   // being used, allowing default commands or other user-triggered commands
+ *   // to run when not in use. Interrupting one of the inner commands while it's
+ *   // running will cancel the entire sequence.
+ *   private Command advancedScoringSequence() {
+ *     return Command.noRequirements().executing(coroutine -> {
+ *       coroutine.await(drivetrain.driveToScoringLocation());
+ *       coroutine.await(elevator.moveToScoringHeight());
+ *       coroutine.await(gripper.release());
+ *     }).named("Scoring Sequence (Advanced)");
+ *   }
+ * }
+ * }
+ */ +@NoDiscard("Commands must be used! Did you mean to fork it or bind it to a trigger?") +public interface Command { + /** The default command priority. */ + int DEFAULT_PRIORITY = 0; + + /** + * The lowest possible command priority. Commands with the lowest priority can be interrupted by + * any other command, including other minimum-priority commands. + */ + int LOWEST_PRIORITY = Integer.MIN_VALUE; + + /** + * The highest possible command priority. Commands with the highest priority can only be + * interrupted by other maximum-priority commands. + */ + int HIGHEST_PRIORITY = Integer.MAX_VALUE; + + /** + * Runs the command. Commands that need to periodically run until a goal state is reached should + * simply run a while loop like {@code while (!atGoal()) { ... } } and call {@link + * Coroutine#yield()} at the end of the loop. + * + *

Warning: any loops in a command must call {@code coroutine.yield()}. + * Failure to do so will prevent anything else in your robot code from running. Commands are + * opt-in collaborative constructs; don't be greedy! + * + * @param coroutine the coroutine backing the command's execution + */ + void run(Coroutine coroutine); + + /** + * An optional lifecycle hook that can be implemented to execute some code whenever this command + * is canceled before it naturally completes. Commands should be careful to do a single-shot + * cleanup (for example, setting a motor to zero volts) and not do any complex looping logic here. + */ + default void onCancel() { + // NOP by default + } + + /** + * The name of the command. + * + * @return the name of the command + */ + @NoDiscard + String name(); + + /** + * The mechanisms required by the command. This is used by the scheduler to determine if two + * commands conflict with each other. No mechanism may be required by more than one running + * command at a time. + * + * @return the set of mechanisms required by the command + */ + @NoDiscard + Set requirements(); + + /** + * The priority of the command. If a command is scheduled that conflicts with another running or + * pending command, their priority values are compared to determine which should run. If the + * scheduled command is lower priority than the running command, then it will not be scheduled and + * the running command will continue to run. If it is the same or higher priority, then the + * running command will be canceled and the scheduled command will start to run. + * + * @return the priority of the command + */ + @NoDiscard + default int priority() { + return DEFAULT_PRIORITY; + } + + /** + * Checks if this command has a lower {@link #priority() priority} than another command. + * + * @param other the command to compare with + * @return true if this command has a lower priority than the other one, false otherwise + */ + @NoDiscard + default boolean isLowerPriorityThan(Command other) { + requireNonNullParam(other, "other", "Command.isLowerPriorityThan"); + + return priority() < other.priority(); + } + + /** + * Checks if this command requires a particular mechanism. + * + * @param mechanism the mechanism to check + * @return true if the mechanism is required, false if not + */ + @NoDiscard + default boolean requires(Mechanism mechanism) { + return requirements().contains(mechanism); + } + + /** + * Checks if this command conflicts with another command. + * + * @param other the commands to check against + * @return true if both commands require at least one of the same mechanism, false if both + * commands have completely different requirements + */ + @NoDiscard + default boolean conflictsWith(Command other) { + requireNonNullParam(other, "other", "Command.conflictsWith"); + + return !Collections.disjoint(requirements(), other.requirements()); + } + + /** + * Creates a new command that runs this one for a maximum duration, after which it is forcibly + * canceled. This is particularly useful for autonomous routines where you want to prevent your + * entire autonomous period spent stuck on a single action because a mechanism doesn't quite reach + * its setpoint (for example, spinning up a flywheel or driving to a particular location on the + * field). The resulting command will have the same name as this one, with the timeout period + * appended. + * + * @param timeout the maximum duration that the command is permitted to run. Negative or zero + * values will result in the command running only once before being canceled. + * @return the timed out command. + */ + default Command withTimeout(Time timeout) { + requireNonNullParam(timeout, "timeout", "Command.withTimeout"); + + return race(this, waitFor(timeout).named("Timeout: " + timeout.toLongString())) + .named(name() + " [" + timeout.toLongString() + " timeout]"); + } + + /** + * Creates a command that does not require any hardware; that is, it does not affect the state of + * any physical objects. This is useful for commands that do some cleanup or state management, + * such as resetting odometry or sensors, that you don't want to interrupt a command that's + * controlling the mechanisms it affects. + * + *

More configuration options are needed after calling this function before the command can be + * created. See {@link StagedCommandBuilder} for details. + * + * @return a builder that can be used to configure the resulting command + */ + static NeedsExecutionBuilderStage noRequirements() { + return new StagedCommandBuilder().noRequirements(); + } + + /** + * Creates a command that requires one or more mechanisms. + * + *

More configuration options are needed after calling this function before the command can be + * created. See {@link StagedCommandBuilder} for details. + * + * @param requirement The first required mechanism + * @param rest Any other required mechanisms + * @return A command builder + */ + static NeedsExecutionBuilderStage requiring(Mechanism requirement, Mechanism... rest) { + // parameters will be null checked by the builder + return new StagedCommandBuilder().requiring(requirement, rest); + } + + /** + * Creates command that requires some number of mechanisms. + * + *

More configuration options are needed after calling this function before the command can be + * created. See {@link StagedCommandBuilder} for details. + * + * @param requirements The required mechanisms. May be empty, but cannot contain null values. + * @return A command builder + */ + static NeedsExecutionBuilderStage requiring(Collection requirements) { + // parameters will be null checked by the builder + return new StagedCommandBuilder().requiring(requirements); + } + + /** + * Starts creating a command that runs a group of multiple commands in parallel. The command will + * complete when every command in the group has completed naturally. + * + *

More configuration options are needed after calling this function before the command can be + * created. See {@link ParallelGroupBuilder} for details. + * + * @param commands The commands to run in parallel + * @return A command builder + */ + static ParallelGroupBuilder parallel(Command... commands) { + // parameters will be null checked by the builder + return new ParallelGroupBuilder().requiring(commands); + } + + /** + * Starts creating a command that runs a group of multiple commands in parallel. The command will + * complete when any command in the group has completed naturally; all other commands in the group + * will be canceled. + * + *

More configuration options are needed after calling this function before the command can be + * created. See {@link ParallelGroupBuilder} for details. + * + * @param commands The commands to run in parallel + * @return A command builder + */ + static ParallelGroupBuilder race(Command... commands) { + // parameters will be null checked by the builder + return new ParallelGroupBuilder().optional(commands); + } + + /** + * Starts creating a command that runs a group of multiple commands in sequence. The command will + * complete when the last command in the group has completed naturally. Commands in the group will + * run in the order they're passed to this method. + * + *

More configuration options are needed after calling this function before the command can be + * created. See {@link SequentialGroupBuilder} for details. + * + * @param commands The commands to run in sequence. + * @return A command builder + */ + static SequentialGroupBuilder sequence(Command... commands) { + // parameters will be null checked by the builder + return new SequentialGroupBuilder().andThen(commands); + } + + /** + * Starts creating a command that simply waits for some condition to be met. The command will not + * require any mechanisms. + * + *

More configuration options are needed after calling this function before the command can be + * created. See {@link StagedCommandBuilder} for details. + * + * @param condition The condition to wait for + * @return A command builder + */ + static NeedsNameBuilderStage waitUntil(BooleanSupplier condition) { + requireNonNullParam(condition, "condition", "Command.waitUntil"); + + return noRequirements().executing(coroutine -> coroutine.waitUntil(condition)); + } + + /** + * Creates a command that simply waits for a given duration. The command will not require any + * mechanisms. + * + * @param duration How long to wait + * @return A command builder + */ + static NeedsNameBuilderStage waitFor(Time duration) { + requireNonNullParam(duration, "duration", "Command.waitFor"); + + return noRequirements().executing(coroutine -> coroutine.wait(duration)); + } + + /** + * Creates a command that runs this one and ends when the end condition is met (if this command + * has not already exited by then). + * + *

More configuration options are needed after calling this function before the command can be + * created. See {@link ParallelGroupBuilder} for details. + * + * @param endCondition The end condition to wait for. + * @return The waiting command + */ + default ParallelGroupBuilder until(BooleanSupplier endCondition) { + requireNonNullParam(endCondition, "endCondition", "Command.until"); + + return new ParallelGroupBuilder() + .optional(this, Command.waitUntil(endCondition).named("Until Condition")); + } + + /** + * Creates a command sequence, starting from this command and then running the next one. More + * commands can be added with the builder before naming and creating the sequence. + * + * 

{@code
+   * Sequence aThenBThenC =
+   *   commandA()
+   *     .andThen(commandB())
+   *     .andThen(commandC())
+   *     .withAutomaticName();
+   * }
+ * + * @param next The command to run after this one in the sequence + * @return A sequence builder + */ + default SequentialGroupBuilder andThen(Command next) { + // parameter will be null checked by the builder + return new SequentialGroupBuilder().andThen(this).andThen(next); + } + + /** + * Creates a parallel command group, running this command alongside one or more other commands. + * The group will exit once every command has finished. + * + *

More configuration options are needed after calling this function before the command can be + * created. See {@link ParallelGroupBuilder} for details. + * + * 

{@code
+   * ParallelGroup abc =
+   *   commandA()
+   *     .alongWith(commandB(), commandC())
+   *     .withAutomaticName();
+   * }
+ * + * @param parallel The commands to run in parallel with this one + * @return A parallel group builder + */ + default ParallelGroupBuilder alongWith(Command... parallel) { + return new ParallelGroupBuilder().requiring(this).requiring(parallel); + } + + /** + * Creates a parallel command group, running this command alongside one or more other commands. + * The group will exit after any command finishes. + * + *

More configuration options are needed after calling this function before the command can be + * created. See {@link ParallelGroupBuilder} for details. + * + * @param parallel The commands to run in parallel with this one + * @return A parallel group builder + */ + default ParallelGroupBuilder raceWith(Command... parallel) { + return new ParallelGroupBuilder().optional(this).optional(parallel); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/CommandState.java b/commandsv3/src/main/java/org/wpilib/commands3/CommandState.java new file mode 100644 index 0000000000..ab10842f13 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/CommandState.java @@ -0,0 +1,100 @@ +// 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.commands3; + +import static java.util.Objects.requireNonNull; + +/** Represents the state of a command as it is executed by the scheduler. */ +final class CommandState { + // Two billion unique IDs should be enough for anybody. + private static int s_lastId = 0; + + private final Command m_command; + private final Command m_parent; + private final Coroutine m_coroutine; + private final Binding m_binding; + private double m_lastRuntimeMs = -1; + private double m_totalRuntimeMs; + private final int m_id; + + /** + * Creates a new command state object. + * + * @param command The command being tracked. + * @param parent The parent command composition that scheduled the tracked command. Null if the + * command was scheduled by a top level construct like trigger bindings or manually scheduled + * by a user. For deeply nested compositions, this tracks the direct parent command + * that invoked the schedule() call; in this manner, an ancestry tree can be built, where each + * {@code CommandState} object references a parent node in the tree. + * @param coroutine The coroutine to which the command is bound. + * @param binding The binding that caused the command to be scheduled. + */ + CommandState(Command command, Command parent, Coroutine coroutine, Binding binding) { + m_command = requireNonNull(command, "WPILib bug: command state given null command"); + m_parent = parent; // allowed to be null + m_coroutine = requireNonNull(coroutine, "WPILib bug: command state given null coroutine"); + m_binding = requireNonNull(binding, "WPILib bug: command state given null binding"); + + // This is incredibly non-thread safe, but nobody should be using the command framework across + // multiple threads anyway. Worst case scenario, we'll get duplicate IDs for commands scheduled + // by different threads and cause bad telemetry data without affecting program correctness. + m_id = ++s_lastId; + } + + public Command command() { + return m_command; + } + + public Command parent() { + return m_parent; + } + + public Coroutine coroutine() { + return m_coroutine; + } + + public Binding binding() { + return m_binding; + } + + /** + * How long the command took to run the last time it executed. Defaults to -1 if the command has + * not run at least once. + * + * @return The runtime, in milliseconds. + */ + public double lastRuntimeMs() { + return m_lastRuntimeMs; + } + + public void setLastRuntimeMs(double lastRuntimeMs) { + m_lastRuntimeMs = lastRuntimeMs; + m_totalRuntimeMs += lastRuntimeMs; + } + + public double totalRuntimeMs() { + return m_totalRuntimeMs; + } + + public int id() { + return m_id; + } + + @Override + public boolean equals(Object obj) { + return obj instanceof CommandState that && m_id == that.m_id; + } + + @Override + public int hashCode() { + return m_id; + } + + @Override + public String toString() { + return "CommandState[command=%s, parent=%s, coroutine=%s, id=%d]" + .formatted(m_command, m_parent, m_coroutine, m_id); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/CommandTraceHelper.java b/commandsv3/src/main/java/org/wpilib/commands3/CommandTraceHelper.java new file mode 100644 index 0000000000..97bcf62476 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/CommandTraceHelper.java @@ -0,0 +1,73 @@ +// 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.commands3; + +import java.util.ArrayList; +import java.util.List; +import java.util.stream.Stream; + +final class CommandTraceHelper { + private CommandTraceHelper() { + // Utility class + } + + /** + * Creates a modified stack trace where the trace of the scheduling code replaces the trace of the + * internal scheduler logic. + * + * @param commandExceptionTrace The trace of the exception raised during command execution. + * @param commandScheduleTrace The trace of when the command was scheduled. + * @return A new array of stack trace elements. + */ + public static StackTraceElement[] modifyTrace( + StackTraceElement[] commandExceptionTrace, StackTraceElement[] commandScheduleTrace) { + List frames = new ArrayList<>(); + + List filteredClasses = + List.of( + Coroutine.class.getName(), + Continuation.class.getName(), + Scheduler.class.getName(), + "org.wpilib.commands3.StagedCommandBuilder$BuilderBackedCommand", + "jdk.internal.vm.Continuation"); + + boolean sawRun = false; + for (var exceptionFrame : commandExceptionTrace) { + if (!filteredClasses.contains(exceptionFrame.getClassName())) { + frames.add(exceptionFrame); + } + + // Inject the schedule trace immediately after the line of user code that called Scheduler.run + if (sawRun) { + // Inject a marker frame just so there's a distinction between the command's code and the + // code that scheduled it, since they occur asynchronously + frames.add(new StackTraceElement("=== Command Binding Trace ===", "", "", -1)); + + // Drop internal trigger frames, since they're not helpful for users. + // The first frame here should be the line of user code that bound the command to a trigger. + Stream.of(commandScheduleTrace) + .dropWhile( + frame -> { + boolean inTriggerInternals = frame.getClassName().equals(Trigger.class.getName()); + boolean isScheduleCall = + frame.getClassName().equals(Scheduler.class.getName()) + && "schedule".equals(frame.getMethodName()); + + return inTriggerInternals || isScheduleCall; + }) + .filter(frame -> !filteredClasses.contains(frame.getClassName())) + .forEach(frames::add); + break; + } + + if (exceptionFrame.getClassName().equals(Scheduler.class.getName()) + && "run".equals(exceptionFrame.getMethodName())) { + sawRun = true; + } + } + + return frames.toArray(StackTraceElement[]::new); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/ConflictDetector.java b/commandsv3/src/main/java/org/wpilib/commands3/ConflictDetector.java new file mode 100644 index 0000000000..5be5953fc7 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/ConflictDetector.java @@ -0,0 +1,122 @@ +// 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.commands3; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashSet; +import java.util.List; +import java.util.Set; +import java.util.stream.Collectors; + +/** Utility class for helping with detecting conflicts between commands. */ +public final class ConflictDetector { + private ConflictDetector() { + // This is a utility class! + } + + /** + * A conflict between two commands. + * + * @param a The first conflicting command. + * @param b The second conflicting command. + * @param sharedRequirements The set of mechanisms required by both commands. This set is + * read-only + */ + public record Conflict(Command a, Command b, Set sharedRequirements) { + /** + * Gets a descriptive message for the conflict. The description includes the names of the + * conflicting commands and the names of all mechanisms required by both commands. + * + * @return A description of the conflict. + */ + public String description() { + var shared = + sharedRequirements.stream() + .map(Mechanism::getName) + .sorted() + .collect(Collectors.joining(", ")); + return "%s and %s both require %s".formatted(a.name(), b.name(), shared); + } + } + + /** + * Validates that a set of commands have no internal requirement conflicts. An error is thrown if + * a conflict is detected. + * + * @param commands The commands to validate + * @throws IllegalArgumentException If at least one pair of commands is found in the input where + * both commands have at least one required mechanism in common + */ + public static void throwIfConflicts(Collection commands) { + requireNonNullParam(commands, "commands", "ConflictDetector.throwIfConflicts"); + + var conflicts = findAllConflicts(commands); + if (conflicts.isEmpty()) { + // No conflicts, all good + return; + } + + StringBuilder message = + new StringBuilder("Commands running in parallel cannot share requirements: "); + for (int i = 0; i < conflicts.size(); i++) { + Conflict conflict = conflicts.get(i); + message.append(conflict.description()); + if (i < conflicts.size() - 1) { + message.append("; "); + } + } + + throw new IllegalArgumentException(message.toString()); + } + + /** + * Finds all conflicting pairs of commands in the input collection. + * + * @param commands The commands to check. + * @return All detected conflicts. The returned list is empty if no conflicts were found. + */ + @SuppressWarnings("PMD.CompareObjectsWithEquals") + public static List findAllConflicts(Collection commands) { + requireNonNullParam(commands, "commands", "ConflictDetector.findAllConflicts"); + + List conflicts = new ArrayList<>(); + + int i = 0; + for (Command command : commands) { + i++; + int j = 0; + for (Command other : commands) { + j++; + if (j <= i) { + // Skip all elements in 0..i so the inner loop only checks elements from i + 1 onward. + // Ordering of the elements in the pair doesn't matter, and this prevents finding every + // pair twice eg (A, B) and (B, A). + continue; + } + + if (command == other) { + // Commands cannot conflict with themselves, so just in case the input collection happens + // to have duplicate elements we just skip any pairs of a command with itself. + continue; + } + + if (command.conflictsWith(other)) { + conflicts.add(findConflict(command, other)); + } + } + } + + return conflicts; + } + + private static Conflict findConflict(Command a, Command b) { + Set sharedRequirements = new HashSet<>(a.requirements()); + sharedRequirements.retainAll(b.requirements()); + return new Conflict(a, b, Set.copyOf(sharedRequirements)); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/Continuation.java b/commandsv3/src/main/java/org/wpilib/commands3/Continuation.java new file mode 100644 index 0000000000..6a0f9aa907 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/Continuation.java @@ -0,0 +1,230 @@ +// 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.commands3; + +import java.lang.invoke.MethodHandle; +import java.lang.invoke.MethodHandles; +import java.lang.invoke.MethodType; +import java.lang.invoke.WrongMethodTypeException; + +/** + * A wrapper around the JDK internal Continuation class. Continuations are one-shot (i.e., not + * reusable after completion) and allow stack frames to be paused and resumed at a later time. They + * are the underpinning for virtual threads, which have their own scheduler and JVM support. Bare + * continuations are designed for internal use by the JVM; we have forcibly opened access to them + * for use by the commands framework due to limitations of virtual threads; notably, their complete + * lack of determinism and timing, which are critically important for real-time systems like robots. + * + *

ONLY USE CONTINUATIONS IN A SINGLE THREADED CONTEXT. The JVM and JIT + * compilers make fundamental assumptions about how continuations are used, and rely on the code + * that uses it (which was intended to be virtual threads) to use it safely. Failure to use + * this API safely can result in JIT compilers to issue invalid code causing buggy behavior and JVM + * crashes at any time, up to and including on a field during an official match. + * + *

Teams don't need to use continuations directly with the commands framework; all mounting and + * unmounting is handled by the command scheduler and a coroutine wrapper. + */ +final class Continuation { + // The underlying jdk.internal.vm.Continuation object + // https://github.com/openjdk/jdk/blob/jdk-21%2B35/src/java.base/share/classes/jdk/internal/vm/Continuation.java + private final Object m_continuation; + + static final Class jdk_internal_vm_Continuation; + private static final MethodHandle CONSTRUCTOR; + + @SuppressWarnings("PMD.AvoidFieldNameMatchingMethodName") + private static final MethodHandle YIELD; + + @SuppressWarnings("PMD.AvoidFieldNameMatchingMethodName") + private static final MethodHandle RUN; + + private static final MethodHandle IS_DONE; + + private static final MethodHandle java_lang_thread_setContinuation; + + private static final ThreadLocal mountedContinuation = new ThreadLocal<>(); + + static { + try { + jdk_internal_vm_Continuation = Class.forName("jdk.internal.vm.Continuation"); + + var lookup = + MethodHandles.privateLookupIn(jdk_internal_vm_Continuation, MethodHandles.lookup()); + + CONSTRUCTOR = + lookup.findConstructor( + jdk_internal_vm_Continuation, + MethodType.methodType( + void.class, ContinuationScope.jdk_internal_vm_ContinuationScope, Runnable.class)); + + YIELD = + lookup.findStatic( + jdk_internal_vm_Continuation, + "yield", + MethodType.methodType( + boolean.class, ContinuationScope.jdk_internal_vm_ContinuationScope)); + + RUN = + lookup.findVirtual( + jdk_internal_vm_Continuation, "run", MethodType.methodType(void.class)); + + IS_DONE = + lookup.findVirtual( + jdk_internal_vm_Continuation, "isDone", MethodType.methodType(boolean.class)); + } catch (Throwable t) { + throw new ExceptionInInitializerError(t); + } + } + + static { + try { + var lookup = MethodHandles.privateLookupIn(Thread.class, MethodHandles.lookup()); + + java_lang_thread_setContinuation = + lookup.findVirtual( + Thread.class, + "setContinuation", + MethodType.methodType(void.class, Continuation.jdk_internal_vm_Continuation)); + } catch (Throwable t) { + throw new ExceptionInInitializerError(t); + } + } + + /** + * Used to wrap any checked exceptions bubbled from when calling the internal continuation methods + * via reflection. Per the Continuation API as of Java 21, none of the methods we're calling will + * throw unchecked exceptions (IllegalState or other runtime exceptions, yes, and we bubble those + * up directly); however, the MethodHandle API's `invoke` method has `throws Throwable` in its + * signature and we have to handle it. + */ + @SuppressWarnings("PMD.DoNotExtendJavaLangError") + static final class InternalContinuationError extends Error { + InternalContinuationError(String message, Throwable cause) { + super(message, cause); + } + } + + private final ContinuationScope m_scope; + + /** + * Constructs a continuation. + * + * @param scope the continuation's scope, used in yield + * @param target the continuation's body + */ + @SuppressWarnings({"PMD.AvoidRethrowingException", "PMD.AvoidCatchingGenericException"}) + Continuation(ContinuationScope scope, Runnable target) { + try { + m_continuation = CONSTRUCTOR.invoke(scope.m_continuationScope, target); + m_scope = scope; + } catch (RuntimeException | Error e) { + throw e; + } catch (Throwable t) { + throw new RuntimeException(t); + } + } + + /** + * Suspends the current continuations up to this continuation's scope. + * + * @return {@code true} for success; {@code false} for failure + * @throws IllegalStateException if not currently in this continuation's scope + */ + @SuppressWarnings({"PMD.AvoidRethrowingException", "PMD.AvoidCatchingGenericException"}) + public boolean yield() { + try { + return (boolean) YIELD.invoke(m_scope.m_continuationScope); + } catch (RuntimeException | Error e) { + throw e; + } catch (Throwable t) { + throw new InternalContinuationError( + "Continuation.yield() encountered an unexpected error", t); + } + } + + /** + * Mounts and runs the continuation body until the body calls {@link #yield()}. If the + * continuation is suspended, it will continue from the most recent yield point. + */ + @SuppressWarnings({"PMD.AvoidRethrowingException", "PMD.AvoidCatchingGenericException"}) + public void run() { + try { + RUN.invoke(m_continuation); + } catch (WrongMethodTypeException | ClassCastException e) { + throw new IllegalStateException("Unable to run the underlying continuation!", e); + } catch (RuntimeException | Error e) { + // The bound task threw an exception; re-throw it + throw e; + } catch (Throwable t) { + throw new InternalContinuationError("Continuation.run() encountered an unexpected error", t); + } + } + + /** + * Tests whether this continuation is completed. + * + * @return whether this continuation is completed + */ + @SuppressWarnings({"PMD.AvoidRethrowingException", "PMD.AvoidCatchingGenericException"}) + public boolean isDone() { + try { + return (boolean) IS_DONE.invoke(m_continuation); + } catch (RuntimeException | Error e) { + throw e; + } catch (Throwable t) { + throw new InternalContinuationError( + "Continuation.isDone() encountered an unexpected error", t); + } + } + + /** + * Mounds a continuation to the current thread. Accepts null for clearing the currently mounted + * continuation. + * + * @param continuation the continuation to mount + */ + @SuppressWarnings({"PMD.AvoidRethrowingException", "PMD.AvoidCatchingGenericException"}) + public static void mountContinuation(Continuation continuation) { + try { + if (continuation == null) { + java_lang_thread_setContinuation.invoke(Thread.currentThread(), null); + mountedContinuation.remove(); + } else { + java_lang_thread_setContinuation.invoke( + Thread.currentThread(), continuation.m_continuation); + mountedContinuation.set(continuation); + } + } catch (RuntimeException | Error e) { + throw e; + } catch (Throwable t) { + // `t` is anything thrown internally by Thread.setContinuation. + // It only assigns to a field, no way to throw + // However, if the invocation fails for some reason, we'll end up with an + // IllegalStateException when attempting to run an unmounted continuation + throw new InternalContinuationError( + "Continuation.mountContinuation() encountered an unexpected error", t); + } + } + + /** + * Gets the currently mounted continuation. This is thread-local value; calling this method on two + * different threads will give two different results. + * + * @return The continuation mounted on the current thread. + */ + public static Continuation getMountedContinuation() { + return mountedContinuation.get(); + } + + @Override + public String toString() { + return m_continuation.toString(); + } + + @SuppressWarnings("PMD.CompareObjectsWithEquals") + boolean isMounted() { + return this == getMountedContinuation(); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/ContinuationScope.java b/commandsv3/src/main/java/org/wpilib/commands3/ContinuationScope.java new file mode 100644 index 0000000000..27484d88fc --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/ContinuationScope.java @@ -0,0 +1,54 @@ +// 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.commands3; + +import java.lang.invoke.MethodHandle; +import java.lang.invoke.MethodHandles; +import java.lang.invoke.MethodType; +import java.util.Objects; + +/** A continuation scope. */ +final class ContinuationScope { + // The underlying jdk.internal.vm.ContinuationScope object + // https://github.com/openjdk/jdk/blob/jdk-21%2B35/src/java.base/share/classes/jdk/internal/vm/ContinuationScope.java + final Object m_continuationScope; + + static final Class jdk_internal_vm_ContinuationScope; + private static final MethodHandle CONSTRUCTOR; + + static { + try { + jdk_internal_vm_ContinuationScope = Class.forName("jdk.internal.vm.ContinuationScope"); + + var lookup = + MethodHandles.privateLookupIn(jdk_internal_vm_ContinuationScope, MethodHandles.lookup()); + + CONSTRUCTOR = + lookup.findConstructor( + jdk_internal_vm_ContinuationScope, MethodType.methodType(void.class, String.class)); + } catch (Throwable t) { + throw new ExceptionInInitializerError(t); + } + } + + /** + * Constructs a new scope. + * + * @param name The scope's name + */ + ContinuationScope(String name) { + Objects.requireNonNull(name); + try { + m_continuationScope = CONSTRUCTOR.invoke(name); + } catch (Throwable e) { + throw new RuntimeException(e); + } + } + + @Override + public String toString() { + return m_continuationScope.toString(); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/Coroutine.java b/commandsv3/src/main/java/org/wpilib/commands3/Coroutine.java new file mode 100644 index 0000000000..71e159e67a --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/Coroutine.java @@ -0,0 +1,377 @@ +// 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.commands3; + +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.Timer; +import java.util.Arrays; +import java.util.Collection; +import java.util.List; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; + +/** + * A coroutine object is injected into command's {@link Command#run(Coroutine)} method to allow + * commands to yield and compositions to run other commands. Commands are considered bound to + * a coroutine while they're scheduled; attempting to use a coroutine outside the command bound to + * it will result in an {@code IllegalStateException} being thrown. + */ +public final class Coroutine { + private final Scheduler m_scheduler; + private final Continuation m_backingContinuation; + + /** + * Creates a new coroutine. Package-private; only the scheduler should be creating these. + * + * @param scheduler The scheduler running the coroutine + * @param scope The continuation scope the coroutine's backing continuation runs in + * @param callback The callback for the continuation to execute when mounted. Often a command + * function's body. + */ + Coroutine(Scheduler scheduler, ContinuationScope scope, Consumer callback) { + m_scheduler = scheduler; + m_backingContinuation = new Continuation(scope, () -> callback.accept(this)); + } + + /** + * Yields control back to the scheduler to allow other commands to execute. This can be thought of + * as "pausing" the currently executing command. + * + * @return true + * @throws IllegalStateException if called anywhere other than the coroutine's running command + */ + public boolean yield() { + requireMounted(); + + try { + return m_backingContinuation.yield(); + } catch (IllegalStateException e) { + if ("Pinned: MONITOR".equals(e.getMessage())) { + // Raised when a continuation yields inside a synchronized block or method: + // https://github.com/openjdk/jdk/blob/jdk-21%2B35/src/java.base/share/classes/jdk/internal/vm/Continuation.java#L396-L402 + // Note: Not a thing in Java 24+ + // Rethrow with an error message that's more helpful for our users + throw new IllegalStateException( + "Coroutine.yield() cannot be called inside a synchronized block or method. " + + "Consider using a Lock instead of synchronized, " + + "or rewrite your code to avoid locks and mutexes altogether.", + e); + } else { + // rethrow + throw e; + } + } + } + + /** + * Parks the current command. No code in a command declared after calling {@code park()} will be + * executed. A parked command will never complete naturally and must be interrupted or canceled. + * + * @throws IllegalStateException if called anywhere other than the coroutine's running command + */ + @SuppressWarnings("InfiniteLoopStatement") + public void park() { + requireMounted(); + + while (true) { + // 'this' is required because 'yield' is a semi-keyword and needs to be qualified + this.yield(); + } + } + + /** + * Schedules a child command and then immediately returns. The child command will run until its + * natural completion, the parent command exits, or the parent command cancels it. + * + *

This is a nonblocking operation. To fork and then wait for the child command to complete, + * use {@link #await(Command)}. + * + *

The parent command will continue executing while the child command runs, and can resync with + * the child command using {@link #await(Command)}. + * + * 

{@code
+   * Command example() {
+   *   return Command.noRequirements().executing(coroutine -> {
+   *     Command child = ...;
+   *     coroutine.fork(child);
+   *     // ... do more things
+   *     // then sync back up with the child command
+   *     coroutine.await(child);
+   *   }).named("Example");
+   * }
+   * }
+ * + *

Note: forking a command that conflicts with a higher-priority command will fail. The forked + * command will not be scheduled, and the existing command will continue to run. + * + * @param commands The commands to fork. + * @throws IllegalStateException if called anywhere other than the coroutine's running command + * @see #await(Command) + */ + public void fork(Command... commands) { + requireMounted(); + + requireNonNullParam(commands, "commands", "Coroutine.fork"); + for (int i = 0; i < commands.length; i++) { + requireNonNullParam(commands[i], "commands[" + i + "]", "Coroutine.fork"); + } + + // Check for user error; there's no reason to fork conflicting commands simultaneously + ConflictDetector.throwIfConflicts(List.of(commands)); + + // Shorthand; this is handy for user-defined compositions + for (var command : commands) { + m_scheduler.schedule(command); + } + } + + /** + * Forks off some commands. Each command will run until its natural completion, the parent command + * exits, or the parent command cancels it. The parent command will continue executing while the + * forked commands run, and can resync with the forked commands using {@link + * #awaitAll(Collection)}. + * + * 

{@code
+   * Command example() {
+   *   return Command.noRequirements().executing(coroutine -> {
+   *     Collection innerCommands = ...;
+   *     coroutine.fork(innerCommands);
+   *     // ... do more things
+   *     // then sync back up with the inner commands
+   *     coroutine.awaitAll(innerCommands);
+   *   }).named("Example");
+   * }
+   * }
+ * + *

Note: forking a command that conflicts with a higher-priority command will fail. The forked + * command will not be scheduled, and the existing command will continue to run. + * + * @param commands The commands to fork. + * @throws IllegalStateException if called anywhere other than the coroutine's running command + */ + public void fork(Collection commands) { + fork(commands.toArray(Command[]::new)); + } + + /** + * Awaits completion of a command. If the command is not currently scheduled or running, it will + * be scheduled automatically. This is a blocking operation and will not return until the command + * completes or has been interrupted by another command scheduled by the same parent. + * + * @param command the command to await + * @throws IllegalStateException if called anywhere other than the coroutine's running command + * @see #fork(Command...) + */ + public void await(Command command) { + requireMounted(); + + requireNonNullParam(command, "command", "Coroutine.await"); + + m_scheduler.schedule(command); + + while (m_scheduler.isScheduledOrRunning(command)) { + // If the command is a one-shot, then the schedule call will completely execute the command. + // There would be nothing to await + this.yield(); + } + } + + /** + * Awaits completion of all given commands. If any command is not current scheduled or running, it + * will be scheduled. + * + * @param commands the commands to await + * @throws IllegalArgumentException if any of the commands conflict with each other + * @throws IllegalStateException if called anywhere other than the coroutine's running command + */ + public void awaitAll(Collection commands) { + requireMounted(); + + requireNonNullParam(commands, "commands", "Coroutine.awaitAll"); + int i = 0; + for (Command command : commands) { + requireNonNullParam(command, "commands[" + i + "]", "Coroutine.awaitAll"); + i++; + } + + ConflictDetector.throwIfConflicts(commands); + + for (var command : commands) { + m_scheduler.schedule(command); + } + + while (commands.stream().anyMatch(m_scheduler::isScheduledOrRunning)) { + this.yield(); + } + } + + /** + * Awaits completion of all given commands. If any command is not current scheduled or running, it + * will be scheduled. + * + * @param commands the commands to await + * @throws IllegalArgumentException if any of the commands conflict with each other + * @throws IllegalStateException if called anywhere other than the coroutine's running command + */ + public void awaitAll(Command... commands) { + awaitAll(Arrays.asList(commands)); + } + + /** + * Awaits completion of any given commands. Any command that's not already scheduled or running + * will be scheduled. After any of the given commands completes, the rest will be canceled. + * + * @param commands the commands to await + * @throws IllegalArgumentException if any of the commands conflict with each other + * @throws IllegalStateException if called anywhere other than the coroutine's running command + */ + public void awaitAny(Collection commands) { + requireMounted(); + + requireNonNullParam(commands, "commands", "Coroutine.awaitAny"); + int i = 0; + for (Command command : commands) { + requireNonNullParam(command, "commands[" + i + "]", "Coroutine.awaitAny"); + i++; + } + + ConflictDetector.throwIfConflicts(commands); + + // Schedule anything that's not already queued or running + for (var command : commands) { + m_scheduler.schedule(command); + } + + while (commands.stream().allMatch(m_scheduler::isScheduledOrRunning)) { + this.yield(); + } + + // At least one command exited; cancel the rest. + commands.forEach(m_scheduler::cancel); + } + + /** + * Awaits completion of any given commands. Any command that's not already scheduled or running + * will be scheduled. After any of the given commands completes, the rest will be canceled. + * + * @param commands the commands to await + * @throws IllegalArgumentException if any of the commands conflict with each other + * @throws IllegalStateException if called anywhere other than the coroutine's running command + */ + public void awaitAny(Command... commands) { + awaitAny(Arrays.asList(commands)); + } + + /** + * Waits for some duration of time to elapse. Returns immediately if the given duration is zero or + * negative. Call this within a command or command composition to introduce a simple delay. + * + *

For example, a basic autonomous routine that drives straight for 5 seconds: + * + * 

{@code
+   * Command timedDrive() {
+   *   return drivebase.run(coroutine -> {
+   *     drivebase.tankDrive(1, 1);
+   *     coroutine.wait(Seconds.of(5));
+   *     drivebase.stop();
+   *   }).named("Timed Drive");
+   * }
+   * }
+ * + *

Note that the resolution of the wait period is equal to the period at which {@link + * Scheduler#run()} is called by the robot program. If using a 20 millisecond update period, the + * wait will be rounded up to the nearest 20 millisecond interval; in this scenario, calling + * {@code wait(Milliseconds.of(1))} and {@code wait(Milliseconds.of(19))} would have identical + * effects. + * + *

Very small loop times near the loop period are sensitive to the order in which commands are + * executed. If a command waits for 10 ms at the end of a scheduler cycle, and then all the + * commands that ran before it complete or exit, and then the next cycle starts immediately, the + * wait will be evaluated at the start of that next cycle, which occurred less than 10 ms + * later. Therefore, the wait will see not enough time has passed and only exit after an + * additional cycle elapses, adding an unexpected extra 20 ms to the wait time. This becomes less + * of a problem with smaller loop periods, as the extra 1-loop delay becomes smaller. + * + * @param duration the duration of time to wait + * @throws IllegalStateException if called anywhere other than the coroutine's running command + */ + public void wait(Time duration) { + requireMounted(); + + requireNonNullParam(duration, "duration", "Coroutine.wait"); + + var timer = new Timer(); + timer.start(); + while (!timer.hasElapsed(duration.in(Seconds))) { + this.yield(); + } + } + + /** + * Yields until a condition is met. + * + * @param condition The condition to wait for + * @throws IllegalStateException if called anywhere other than the coroutine's running command + */ + public void waitUntil(BooleanSupplier condition) { + requireMounted(); + + requireNonNullParam(condition, "condition", "Coroutine.waitUntil"); + + while (!condition.getAsBoolean()) { + this.yield(); + } + } + + /** + * Advanced users only: this permits access to the backing command scheduler to run custom logic + * not provided by the standard coroutine methods. Any commands manually scheduled through this + * will be canceled when the parent command exits - it's not possible to "fork" a command that + * lives longer than the command that scheduled it. + * + * @return the command scheduler backing this coroutine + * @throws IllegalStateException if called anywhere other than the coroutine's running command + */ + public Scheduler scheduler() { + requireMounted(); + + return m_scheduler; + } + + private boolean isMounted() { + return m_backingContinuation.isMounted(); + } + + private void requireMounted() { + // Note: attempting to yield() outside a command will already throw an error due to the + // continuation being unmounted, but other actions like forking and awaiting should also + // throw errors. For consistent messaging, we use this helper in all places, not just the + // ones that interact with the backing continuation. + + if (isMounted()) { + return; + } + + throw new IllegalStateException("Coroutines can only be used by the command bound to them"); + } + + // Package-private for interaction with the scheduler. + // These functions are not intended for team use. + + void runToYieldPoint() { + m_backingContinuation.run(); + } + + void mount() { + Continuation.mountContinuation(m_backingContinuation); + } + + boolean isDone() { + return m_backingContinuation.isDone(); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/Mechanism.java b/commandsv3/src/main/java/org/wpilib/commands3/Mechanism.java new file mode 100644 index 0000000000..4d8e41ac4c --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/Mechanism.java @@ -0,0 +1,165 @@ +// 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.commands3; + +import edu.wpi.first.units.measure.Time; +import java.util.List; +import java.util.function.Consumer; +import org.wpilib.annotation.NoDiscard; + +/** + * Generic base class to represent mechanisms on a robot. Commands can require sole ownership of a + * mechanism; when a command that requires a mechanism is running, no other commands may use it at + * the same time. + * + *

Even though this class is named "Mechanism", it may be used to represent other physical + * hardware on a robot that should be controlled with commands - for example, an LED strip or a + * vision processor that can switch between different pipelines could be represented as mechanisms. + */ +public class Mechanism { + private final String m_name; + + private final Scheduler m_registeredScheduler; + + /** + * Creates a new mechanism registered with the default scheduler instance and named using the name + * of the class. Intended to be used by subclasses to get sane defaults without needing to + * manually declare a constructor. + */ + @SuppressWarnings("this-escape") + protected Mechanism() { + m_name = getClass().getSimpleName(); + m_registeredScheduler = Scheduler.getDefault(); + setDefaultCommand(idle()); + } + + /** + * Creates a new mechanism, registered with the default scheduler instance. + * + * @param name The name of the mechanism. Cannot be null. + */ + public Mechanism(String name) { + this(name, Scheduler.getDefault()); + } + + /** + * Creates a new mechanism, registered with the given scheduler instance. + * + * @param name The name of the mechanism. Cannot be null. + * @param scheduler The registered scheduler. Cannot be null. + */ + @SuppressWarnings("this-escape") + public Mechanism(String name, Scheduler scheduler) { + m_name = name; + m_registeredScheduler = scheduler; + setDefaultCommand(idle()); + } + + /** + * Gets the name of this mechanism. + * + * @return The name of the mechanism. + */ + @NoDiscard + public String getName() { + return m_name; + } + + /** + * Sets the default command to run on the mechanism when no other command is scheduled. The + * default command's priority is effectively the minimum allowable priority for any command + * requiring a mechanism. For this reason, it's recommended that a default command have a priority + * less than {@link Command#DEFAULT_PRIORITY} so it doesn't prevent low-priority commands from + * running. + * + *

The default command is initially an idle command that only owns the mechanism without doing + * anything. This command has the lowest possible priority to allow any other command to run. + * + * @param defaultCommand the new default command + */ + public void setDefaultCommand(Command defaultCommand) { + m_registeredScheduler.setDefaultCommand(this, defaultCommand); + } + + /** + * Gets the default command that was set by the latest call to {@link + * #setDefaultCommand(Command)}. + * + * @return The currently configured default command + */ + public Command getDefaultCommand() { + return m_registeredScheduler.getDefaultCommandFor(this); + } + + /** + * Starts building a command that requires this mechanism. + * + * @param commandBody The main function body of the command. + * @return The command builder, for further configuration. + */ + public NeedsNameBuilderStage run(Consumer commandBody) { + return new StagedCommandBuilder().requiring(this).executing(commandBody); + } + + /** + * Starts building a command that requires this mechanism. The given function will be called + * repeatedly in an infinite loop. Useful for building commands that don't need state or multiple + * stages of logic. + * + * @param loopBody The body of the infinite loop. + * @return The command builder, for further configuration. + */ + public NeedsNameBuilderStage runRepeatedly(Runnable loopBody) { + return run( + coroutine -> { + while (true) { + loopBody.run(); + coroutine.yield(); + } + }); + } + + /** + * Returns a command that idles this mechanism until another command claims it. The idle command + * has {@link Command#LOWEST_PRIORITY the lowest priority} and can be interrupted by any other + * command. + * + *

The {@link #getDefaultCommand() default command} for every mechanism is an idle command + * unless a different default command has been configured. + * + * @return A new idle command. + */ + public Command idle() { + return run(Coroutine::park).withPriority(Command.LOWEST_PRIORITY).named(getName() + "[IDLE]"); + } + + /** + * Returns a command that idles this mechanism for the given duration of time. + * + * @param duration How long the mechanism should idle for. + * @return A new idle command. + */ + public Command idleFor(Time duration) { + return idle().withTimeout(duration); + } + + /** + * Gets all running commands that require this mechanism. Commands are returned in the order in + * which they were scheduled. The returned list is read-only. Every command in the list will have + * been scheduled by the previous entry in the list or by intermediate commands that do not + * require the mechanism. + * + * @return The currently running commands that require the mechanism. + */ + @NoDiscard + public List getRunningCommands() { + return m_registeredScheduler.getRunningCommandsFor(this); + } + + @Override + public String toString() { + return m_name; + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/NeedsExecutionBuilderStage.java b/commandsv3/src/main/java/org/wpilib/commands3/NeedsExecutionBuilderStage.java new file mode 100644 index 0000000000..23b5b2f641 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/NeedsExecutionBuilderStage.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.commands3; + +import java.util.Collection; +import java.util.function.Consumer; +import org.wpilib.annotation.NoDiscard; + +/** + * A stage in a command builder where requirements have already been specified and execution details + * are required. The next stage is a {@link NeedsNameBuilderStage} from {@link + * #executing(Consumer)}. Additional requirements may still be added before moving on to the next + * stage. + */ +@NoDiscard +public interface NeedsExecutionBuilderStage { + /** + * Adds a required mechanism for the command. + * + * @param requirement A required mechanism. Cannot be null. + * @return This builder object, for chaining + * @throws NullPointerException If {@code requirement} is null + */ + NeedsExecutionBuilderStage requiring(Mechanism requirement); + + /** + * Adds one or more required mechanisms for the command. + * + * @param requirement A required mechanism. Cannot be null. + * @param extra Any extra required mechanisms. May be empty, but cannot contain null values. + * @return This builder object, for chaining + * @throws NullPointerException If {@code requirement} is null or {@code extra} contains a null + * value + */ + NeedsExecutionBuilderStage requiring(Mechanism requirement, Mechanism... extra); + + /** + * Adds required mechanisms for the command. + * + * @param requirements Any required mechanisms. May be empty, but cannot contain null values. + * @return This builder object, for chaining + * @throws NullPointerException If {@code requirements} is null or contains a null value. + */ + NeedsExecutionBuilderStage requiring(Collection requirements); + + /** + * Sets the function body of the executing command. + * + * @param impl The command's body. Cannot be null. + * @return A builder for the next stage of command construction. + * @throws NullPointerException If {@code impl} is null. + */ + NeedsNameBuilderStage executing(Consumer impl); +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/NeedsNameBuilderStage.java b/commandsv3/src/main/java/org/wpilib/commands3/NeedsNameBuilderStage.java new file mode 100644 index 0000000000..d89691a6ba --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/NeedsNameBuilderStage.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.commands3; + +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import org.wpilib.annotation.NoDiscard; + +/** + * A stage in a command builder where requirements and main command execution logic have been set. + * No more changes to requirements or command implementation may happen after this point. This is + * the final step in command creation + */ +@NoDiscard +public interface NeedsNameBuilderStage { + /** + * Optionally sets a callback to execute when the command is canceled. The callback will not run + * if the command was canceled after being scheduled but before starting to run, and will not run + * if the command completes naturally or from encountering an unhandled exception. + * + * @param onCancel The function to execute when the command is canceled while running. May be + * null. + * @return This builder object, for chaining + */ + NeedsNameBuilderStage whenCanceled(Runnable onCancel); + + /** + * Sets the priority level of the command. + * + * @param priority The desired priority level. + * @return This builder object, for chaining. + */ + NeedsNameBuilderStage withPriority(int priority); + + /** + * Optionally sets an end condition for the command. If the end condition returns {@code true} + * before the command body set in {@link NeedsExecutionBuilderStage#executing(Consumer)} would + * exit, the command will be canceled by the scheduler. + * + * @param endCondition An optional end condition for the command. May be null. + * @return This builder object, for chaining. + */ + NeedsNameBuilderStage until(BooleanSupplier endCondition); + + /** + * Creates the command based on the options set during previous builder stages. The builders will + * no longer be usable after calling this method. + * + * @param name The name of the command + * @return The built command. + * @throws NullPointerException If {@code name} is null. + */ + Command named(String name); +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/ParallelGroup.java b/commandsv3/src/main/java/org/wpilib/commands3/ParallelGroup.java new file mode 100644 index 0000000000..75025c2b28 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/ParallelGroup.java @@ -0,0 +1,107 @@ +// 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.commands3; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import java.util.Collection; +import java.util.HashSet; +import java.util.LinkedHashSet; +import java.util.Set; + +/** + * A type of command that runs multiple other commands in parallel. The group will end after all + * required commands have completed; if no command is explicitly required for completion, then the + * group will end after any command in the group finishes. Any commands still running when the group + * has reached its completion condition will be canceled. + */ +public final class ParallelGroup implements Command { + private final Collection m_requiredCommands = new HashSet<>(); + private final Collection m_optionalCommands = new LinkedHashSet<>(); + private final Set m_requirements = new HashSet<>(); + private final String m_name; + private final int m_priority; + + /** + * Creates a new parallel group. + * + * @param name The name of the group. + * @param requiredCommands The commands that are required to complete for the group to finish. If + * this is empty, then the group will finish when any optional command completes. + * @param optionalCommands The commands that do not need to complete for the group to finish. If + * this is empty, then the group will finish when all required commands complete. + */ + ParallelGroup( + String name, Collection requiredCommands, Collection optionalCommands) { + requireNonNullParam(name, "name", "ParallelGroup"); + requireNonNullParam(requiredCommands, "requiredCommands", "ParallelGroup"); + requireNonNullParam(optionalCommands, "optionalCommands", "ParallelGroup"); + + int i = 0; + for (Command requiredCommand : requiredCommands) { + requireNonNullParam(requiredCommand, "requiredCommands[" + i + "]", "ParallelGroup"); + i++; + } + + i = 0; + for (Command c : optionalCommands) { + requireNonNullParam(c, "optionalCommands[" + i + "]", "ParallelGroup"); + i++; + } + + var allCommands = new LinkedHashSet(); + allCommands.addAll(requiredCommands); + allCommands.addAll(optionalCommands); + + ConflictDetector.throwIfConflicts(allCommands); + + m_name = name; + m_optionalCommands.addAll(optionalCommands); + m_requiredCommands.addAll(requiredCommands); + + for (var command : allCommands) { + m_requirements.addAll(command.requirements()); + } + + m_priority = + allCommands.stream().mapToInt(Command::priority).max().orElse(Command.DEFAULT_PRIORITY); + } + + @Override + public void run(Coroutine coroutine) { + coroutine.fork(m_optionalCommands); + + if (m_requiredCommands.isEmpty()) { + // No required commands - just wait for the first optional command to finish + coroutine.awaitAny(m_optionalCommands); + } else { + // Wait for every required command to finish + coroutine.awaitAll(m_requiredCommands); + } + + // The scheduler will cancel any optional child commands that are still running when + // the `run` method returns + } + + @Override + public String name() { + return m_name; + } + + @Override + public Set requirements() { + return m_requirements; + } + + @Override + public int priority() { + return m_priority; + } + + @Override + public String toString() { + return "ParallelGroup[name=" + m_name + "]"; + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/ParallelGroupBuilder.java b/commandsv3/src/main/java/org/wpilib/commands3/ParallelGroupBuilder.java new file mode 100644 index 0000000000..e5660ebd23 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/ParallelGroupBuilder.java @@ -0,0 +1,147 @@ +// 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.commands3; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import java.util.Arrays; +import java.util.LinkedHashSet; +import java.util.Set; +import java.util.function.BooleanSupplier; +import java.util.stream.Collectors; +import org.wpilib.annotation.NoDiscard; + +/** + * A builder class to configure and then create a {@link ParallelGroup}. Like {@link + * StagedCommandBuilder}, the final command is created by calling the terminal {@link + * #named(String)} method, or with an automatically generated name using {@link + * #withAutomaticName()}. + */ +@NoDiscard +public class ParallelGroupBuilder { + private final Set m_commands = new LinkedHashSet<>(); + private final Set m_requiredCommands = new LinkedHashSet<>(); + private BooleanSupplier m_endCondition; + + /** + * Creates a new parallel group builder. The builder will have no commands and have no preapplied + * configuration options. + */ + public ParallelGroupBuilder() {} + + /** + * Adds one or more optional commands to the group. They will not be required to complete for the + * parallel group to exit, and will be canceled once all required commands have finished. + * + * @param commands The optional commands to add to the group + * @return The builder object, for chaining + */ + public ParallelGroupBuilder optional(Command... commands) { + requireNonNullParam(commands, "commands", "ParallelGroupBuilder.optional"); + for (int i = 0; i < commands.length; i++) { + requireNonNullParam(commands[i], "commands[" + i + "]", "ParallelGroupBuilder.optional"); + } + + m_commands.addAll(Arrays.asList(commands)); + return this; + } + + /** + * Adds one or more required commands to the group. All required commands will need to complete + * for the group to exit. + * + * @param commands The required commands to add to the group + * @return The builder object, for chaining + */ + public ParallelGroupBuilder requiring(Command... commands) { + requireNonNullParam(commands, "commands", "ParallelGroupBuilder.requiring"); + for (int i = 0; i < commands.length; i++) { + requireNonNullParam(commands[i], "commands[" + i + "]", "ParallelGroupBuilder.requiring"); + } + + m_requiredCommands.addAll(Arrays.asList(commands)); + m_commands.addAll(m_requiredCommands); + return this; + } + + /** + * Adds a command to the group. The command must complete for the group to exit. + * + * @param command The command to add to the group + * @return The builder object, for chaining + */ + // Note: this primarily exists so users can cleanly chain .alongWith calls to build a + // parallel group, eg `fooCommand().alongWith(barCommand()).alongWith(bazCommand())` + public ParallelGroupBuilder alongWith(Command command) { + return requiring(command); + } + + /** + * Adds an end condition to the command group. If this condition is met before all required + * commands have completed, the group will exit early. If multiple end conditions are added (e.g. + * {@code .until(() -> conditionA()).until(() -> conditionB())}), then the last end condition + * added will be used and any previously configured condition will be overridden. + * + * @param condition The end condition for the group. May be null. + * @return The builder object, for chaining + */ + public ParallelGroupBuilder until(BooleanSupplier condition) { + m_endCondition = condition; + return this; + } + + /** + * Creates the group, using the provided named. The group will require everything that the + * commands in the group require, and will have a priority level equal to the highest priority + * among those commands. + * + * @param name The name of the parallel group + * @return The built group + */ + public ParallelGroup named(String name) { + requireNonNullParam(name, "name", "ParallelGroupBuilder.named"); + + var group = new ParallelGroup(name, m_commands, m_requiredCommands); + if (m_endCondition == null) { + // No custom end condition, return the group as is + return group; + } + + // We have a custom end condition, so we need to wrap the group in a race + return new ParallelGroupBuilder() + .optional(group, Command.waitUntil(m_endCondition).named("Until Condition")) + .named(name); + } + + /** + * Creates the group, giving it a name based on the commands within the group. + * + * @return The built group + */ + public ParallelGroup withAutomaticName() { + // eg "(CommandA & CommandB & CommandC)" + String required = + m_requiredCommands.stream().map(Command::name).collect(Collectors.joining(" & ", "(", ")")); + + var optionalCommands = new LinkedHashSet<>(m_commands); + optionalCommands.removeAll(m_requiredCommands); + // eg "(CommandA | CommandB | CommandC)" + String optional = + optionalCommands.stream().map(Command::name).collect(Collectors.joining(" | ", "(", ")")); + + if (m_requiredCommands.isEmpty()) { + // No required commands, pure race + return named(optional); + } else if (optionalCommands.isEmpty()) { + // Everything required + return named(required); + } else { + // Some form of deadline + // eg "[(CommandA & CommandB) * (CommandX | CommandY | ...)]" + String name = "[%s * %s]".formatted(required, optional); + return named(name); + } + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/Scheduler.java b/commandsv3/src/main/java/org/wpilib/commands3/Scheduler.java new file mode 100644 index 0000000000..752b779253 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/Scheduler.java @@ -0,0 +1,970 @@ +// 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.commands3; + +import static edu.wpi.first.units.Units.Microseconds; +import static edu.wpi.first.units.Units.Milliseconds; + +import edu.wpi.first.util.ErrorMessages; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.event.EventLoop; +import java.util.ArrayList; +import java.util.Collection; +import java.util.Collections; +import java.util.HashSet; +import java.util.LinkedHashMap; +import java.util.LinkedHashSet; +import java.util.List; +import java.util.Map; +import java.util.SequencedMap; +import java.util.SequencedSet; +import java.util.Set; +import java.util.Stack; +import java.util.function.Consumer; +import java.util.stream.Collectors; +import org.wpilib.annotation.NoDiscard; +import org.wpilib.commands3.button.CommandGenericHID; +import org.wpilib.commands3.proto.SchedulerProto; + +/** + * Manages the lifecycles of {@link Coroutine}-based {@link Command Commands}. Commands may be + * scheduled directly using {@link #schedule(Command)}, or be bound to {@link Trigger Triggers} to + * automatically handle scheduling and cancellation based on internal or external events. User code + * is responsible for calling {@link #run()} periodically to update trigger conditions and execute + * scheduled commands. Most often, this is done by overriding {@link TimedRobot#robotPeriodic()} to + * include a call to {@code Scheduler.getDefault().run()}: + * + * 

{@code
+ * public class Robot extends TimedRobot {
+ *   @Override
+ *   public void robotPeriodic() {
+ *     // Update the scheduler on every loop
+ *     Scheduler.getDefault().run();
+ *   }
+ * }
+ * }
+ * + *

Danger

+ * + *

The scheduler must be used in a single-threaded program. Commands must be scheduled and + * canceled by the same thread that runs the scheduler, and cannot be run in a virtual thread. + * + *

Using the commands framework in a multithreaded environment can cause crashes in the + * Java virtual machine at any time, including on an official field during a match. The + * Java JIT compilers make assumptions that rely on coroutines being used on a single thread. + * Breaking those assumptions can cause incorrect JIT code to be generated with undefined behavior, + * potentially causing control issues or crashes deep in JIT-generated native code. + * + *

Normal concurrency constructs like locks, atomic references, and synchronized blocks + * or methods cannot save you. + * + *

Lifecycle

+ * + *

The {@link #run()} method runs five steps: + * + *

    + *
  1. Call {@link #sideload(Consumer) periodic sideload functions} + *
  2. Poll all registered triggers to queue and cancel commands + *
  3. Queue default commands for any mechanisms without a running command. The queued commands + * can be superseded by any manual scheduling or commands scheduled by triggers in the next + * run. + *
  4. Start all queued commands. This happens after all triggers are checked in case multiple + * commands with conflicting requirements are queued in the same update; the last command to + * be queued takes precedence over the rest. + *
  5. Loop over all running commands, mounting and calling each in turn until they either exit or + * call {@link Coroutine#yield()}. Commands run in the order in which they were scheduled. + *
+ * + *

Telemetry

+ * + *

There are two mechanisms for telemetry for a scheduler. A protobuf serializer can be used to + * take a snapshot of a scheduler instance, and report what commands are queued (scheduled but have + * not yet started to run), commands that are running (along with timing data for each command), and + * total time spent in the most recent {@link #run()} call. However, it cannot detect one-shot + * commands that are scheduled, run, and complete all in a single {@code run()} invocation - + * effectively, commands that never call {@link Coroutine#yield()} are invisible. + * + *

A second telemetry mechanism is provided by {@link #addEventListener(Consumer)}. The scheduler + * will issue events to all registered listeners when certain events occur (see {@link + * SchedulerEvent} for all event types). These events are emitted immediately and can be used to + * detect lifecycle events for all commands, including one-shots that would be invisible to the + * protobuf serializer. However, it is up to the user to log those events themselves. + */ +public final class Scheduler implements ProtobufSerializable { + private final Map m_defaultCommands = new LinkedHashMap<>(); + + /** The set of commands scheduled since the start of the previous run. */ + private final SequencedSet m_queuedToRun = new LinkedHashSet<>(); + + /** + * The states of all running commands (does not include on deck commands). We preserve insertion + * order to guarantee that child commands run after their parents. + */ + private final SequencedMap m_runningCommands = new LinkedHashMap<>(); + + /** + * The stack of currently executing commands. Child commands are pushed onto the stack and popped + * when they complete. Use {@link #currentState()} and {@link #currentCommand()} to get the + * currently executing command or its state. + */ + private final Stack m_currentCommandAncestry = new Stack<>(); + + /** The periodic callbacks to run, outside of the command structure. */ + private final List m_periodicCallbacks = new ArrayList<>(); + + /** Event loop for trigger bindings. */ + private final EventLoop m_eventLoop = new EventLoop(); + + /** The scope for continuations to yield to. */ + private final ContinuationScope m_scope = new ContinuationScope("coroutine commands"); + + // Telemetry + /** Protobuf serializer for a scheduler. */ + public static final SchedulerProto proto = new SchedulerProto(); + + private double m_lastRunTimeMs = -1; + + private final Set> m_eventListeners = new LinkedHashSet<>(); + + /** The default scheduler instance. */ + private static final Scheduler s_defaultScheduler = new Scheduler(); + + /** + * Gets the default scheduler instance for use in a robot program. Unless otherwise specified, + * triggers and mechanisms will be registered with the default scheduler and require the default + * scheduler to run. However, triggers and mechanisms can be constructed to be registered with a + * specific scheduler instance, which may be useful for isolation for unit tests. + * + * @return the default scheduler instance. + */ + @NoDiscard + public static Scheduler getDefault() { + return s_defaultScheduler; + } + + /** + * Creates a new scheduler object. Note that most built-in constructs like {@link Trigger} and + * {@link CommandGenericHID} will use the {@link #getDefault() default scheduler instance} unless + * they were explicitly constructed with a different scheduler instance. Teams should use the + * default instance for convenience; however, new scheduler instances can be useful for unit + * tests. + * + * @return a new scheduler instance that is independent of the default scheduler instance. + */ + @NoDiscard + public static Scheduler createIndependentScheduler() { + return new Scheduler(); + } + + /** Private constructor. Use static factory methods or the default scheduler instance. */ + private Scheduler() {} + + /** + * Sets the default command for a mechanism. The command must require that mechanism, and cannot + * require any other mechanisms. + * + * @param mechanism the mechanism for which to set the default command + * @param defaultCommand the default command to execute on the mechanism + * @throws IllegalArgumentException if the command does not meet the requirements for being a + * default command + */ + public void setDefaultCommand(Mechanism mechanism, Command defaultCommand) { + if (!defaultCommand.requires(mechanism)) { + throw new IllegalArgumentException( + "A mechanism's default command must require that mechanism"); + } + + if (defaultCommand.requirements().size() > 1) { + throw new IllegalArgumentException( + "A mechanism's default command cannot require other mechanisms"); + } + + m_defaultCommands.put(mechanism, defaultCommand); + } + + /** + * Gets the default command set for a mechanism. + * + * @param mechanism The mechanism + * @return The default command, or null if no default command was ever set + */ + public Command getDefaultCommandFor(Mechanism mechanism) { + return m_defaultCommands.get(mechanism); + } + + /** + * Adds a callback to run as part of the scheduler. The callback should not manipulate or control + * any mechanisms, but can be used to log information, update data (such as simulations or LED + * data buffers), or perform some other helpful task. The callback is responsible for managing its + * own control flow and end conditions. If you want to run a single task periodically for the + * entire lifespan of the scheduler, use {@link #addPeriodic(Runnable)}. + * + *

Note: Like commands, any loops in the callback must appropriately yield + * control back to the scheduler with {@link Coroutine#yield} or risk stalling your program in an + * unrecoverable infinite loop! + * + * @param callback the callback to sideload + */ + public void sideload(Consumer callback) { + var coroutine = new Coroutine(this, m_scope, callback); + m_periodicCallbacks.add(coroutine); + } + + /** + * Adds a task to run repeatedly for as long as the scheduler runs. This internally handles the + * looping and control yielding necessary for proper function. The callback will run at the same + * periodic frequency as the scheduler. + * + *

For example: + * + * 

{@code
+   * scheduler.addPeriodic(() -> leds.setData(ledDataBuffer));
+   * scheduler.addPeriodic(() -> {
+   *   SmartDashboard.putNumber("X", getX());
+   *   SmartDashboard.putNumber("Y", getY());
+   * });
+   * }
+ * + * @param callback the periodic function to run + */ + public void addPeriodic(Runnable callback) { + sideload( + coroutine -> { + while (true) { + callback.run(); + coroutine.yield(); + } + }); + } + + /** Represents possible results of a command scheduling attempt. */ + public enum ScheduleResult { + /** The command was successfully scheduled and added to the queue. */ + SUCCESS, + /** The command is already scheduled or running. */ + ALREADY_RUNNING, + /** The command is a lower priority and conflicts with a command that's already running. */ + LOWER_PRIORITY_THAN_RUNNING_COMMAND, + } + + /** + * Schedules a command to run. If one command schedules another (a "parent" and "child"), the + * child command will be canceled when the parent command completes. It is not possible to fork a + * child command and have it live longer than its parent. + * + *

Does nothing if the command is already scheduled or running, or requires at least one + * mechanism already used by a higher priority command. + * + * @param command the command to schedule + * @return the result of the scheduling attempt. See {@link ScheduleResult} for details. + * @throws IllegalArgumentException if scheduled by a command composition that has already + * scheduled another command that shares at least one required mechanism + */ + // Implementation detail: a child command will immediately start running when scheduled by a + // parent command, skipping the queue entirely. This avoids dead loop cycles where a parent + // schedules a child, appending it to the queue, then waits for the next cycle to pick it up and + // start it. With deeply nested commands, dead loops could quickly to add up and cause the + // innermost commands that actually _do_ something to start running hundreds of milliseconds after + // their root ancestor was scheduled. + public ScheduleResult schedule(Command command) { + // Note: we use a throwable here instead of Thread.currentThread().getStackTrace() for easier + // stack frame filtering and modification. + var binding = + new Binding( + BindingScope.global(), BindingType.IMMEDIATE, command, new Throwable().getStackTrace()); + + return schedule(binding); + } + + // Package-private for use by Trigger + ScheduleResult schedule(Binding binding) { + var command = binding.command(); + + if (isScheduledOrRunning(command)) { + return ScheduleResult.ALREADY_RUNNING; + } + + if (lowerPriorityThanConflictingCommands(command)) { + return ScheduleResult.LOWER_PRIORITY_THAN_RUNNING_COMMAND; + } + + for (var scheduledState : m_queuedToRun) { + if (!command.conflictsWith(scheduledState.command())) { + // No shared requirements, skip + continue; + } + if (command.isLowerPriorityThan(scheduledState.command())) { + // Lower priority than an already-scheduled (but not yet running) command that requires at + // one of the same mechanism. Ignore it. + return ScheduleResult.LOWER_PRIORITY_THAN_RUNNING_COMMAND; + } + } + + // Evict conflicting on-deck commands + // We check above if the input command is lower priority than any of these, + // so at this point we're guaranteed to be >= priority than anything already on deck + evictConflictingOnDeckCommands(command); + + // If the binding is scoped to a particular command, that command is the parent. If we're in the + // middle of a run cycle and running commands, the parent is whatever command is currently + // running. Otherwise, there is no parent command. + var parentCommand = + binding.scope() instanceof BindingScope.ForCommand scope + ? scope.command() + : currentCommand(); + var state = new CommandState(command, parentCommand, buildCoroutine(command), binding); + + emitScheduledEvent(command); + + if (currentState() != null) { + // Scheduling a child command while running. Start it immediately instead of waiting a full + // cycle. This prevents issues with deeply nested command groups taking many scheduler cycles + // to start running the commands that actually _do_ things + evictConflictingRunningCommands(state); + m_runningCommands.put(command, state); + runCommand(state); + } else { + // Scheduling outside a command, add it to the pending set. If it's not overridden by another + // conflicting command being scheduled in the same scheduler loop, it'll be promoted and + // start to run when #runCommands() is called + m_queuedToRun.add(state); + } + + return ScheduleResult.SUCCESS; + } + + /** + * Checks if a command conflicts with and is a lower priority than any running command. Used when + * determining if the command can be scheduled. + */ + private boolean lowerPriorityThanConflictingCommands(Command command) { + Set ancestors = new HashSet<>(); + for (var state = currentState(); state != null; state = m_runningCommands.get(state.parent())) { + ancestors.add(state); + } + + // Check for conflicts with the commands that are already running + for (var state : m_runningCommands.values()) { + if (ancestors.contains(state)) { + // Can't conflict with an ancestor command + continue; + } + + var c = state.command(); + if (c.conflictsWith(command) && command.isLowerPriorityThan(c)) { + return true; + } + } + + return false; + } + + private void evictConflictingOnDeckCommands(Command command) { + for (var iterator = m_queuedToRun.iterator(); iterator.hasNext(); ) { + var scheduledState = iterator.next(); + var scheduledCommand = scheduledState.command(); + if (scheduledCommand.conflictsWith(command)) { + // Remove the lower priority conflicting command from the on deck commands. + // We don't need to call removeOrphanedChildren here because it hasn't started yet, + // meaning it hasn't had a chance to schedule any children + iterator.remove(); + emitInterruptedEvent(scheduledCommand, command); + emitCanceledEvent(scheduledCommand); + } + } + } + + /** + * Cancels all running commands with which an incoming state conflicts. Ancestor commands of the + * incoming state will not be canceled. + */ + @SuppressWarnings("PMD.CompareObjectsWithEquals") + private void evictConflictingRunningCommands(CommandState incomingState) { + // The set of root states with which the incoming state conflicts but does not inherit from + Set conflictingRootStates = + m_runningCommands.values().stream() + .filter(state -> incomingState.command().conflictsWith(state.command())) + .filter(state -> !isAncestorOf(state.command(), incomingState)) + .map( + state -> { + // Find the highest level ancestor of the conflicting command from which the + // incoming state does _not_ inherit. If they're totally unrelated, this will + // get the very root ancestor; otherwise, it'll return a direct sibling of the + // incoming command + CommandState root = state; + while (root.parent() != null && root.parent() != incomingState.parent()) { + root = m_runningCommands.get(root.parent()); + } + return root; + }) + .collect(Collectors.toSet()); + + // Cancel the root commands + for (var conflictingState : conflictingRootStates) { + emitInterruptedEvent(conflictingState.command(), incomingState.command()); + cancel(conflictingState.command()); + } + } + + /** + * Checks if a particular command is an ancestor of another. + * + * @param ancestor the potential ancestor for which to search + * @param state the state to check + * @return true if {@code ancestor} is the direct parent or indirect ancestor, false if not + */ + @SuppressWarnings({"PMD.CompareObjectsWithEquals", "PMD.SimplifyBooleanReturns"}) + private boolean isAncestorOf(Command ancestor, CommandState state) { + if (state.parent() == null) { + // No parent, cannot inherit + return false; + } + if (!m_runningCommands.containsKey(ancestor)) { + // The given ancestor isn't running + return false; + } + if (state.parent() == ancestor) { + // Direct child + return true; + } + // Check if the command's parent inherits from the given ancestor + return m_runningCommands.values().stream() + .filter(s -> state.parent() == s.command()) + .anyMatch(s -> isAncestorOf(ancestor, s)); + } + + /** + * Cancels a command and any other command scheduled by it. This occurs immediately and does not + * need to wait for a call to {@link #run()}. Any command that it scheduled will also be canceled + * to ensure commands within compositions do not continue to run. + * + *

This has no effect if the given command is not currently scheduled or running. + * + * @param command the command to cancel + */ + @SuppressWarnings("PMD.CompareObjectsWithEquals") + public void cancel(Command command) { + if (command == currentCommand()) { + throw new IllegalArgumentException( + "Command `" + command.name() + "` is mounted and cannot be canceled"); + } + + boolean running = isRunning(command); + + // Evict the command. The next call to run() will schedule the default command for all its + // required mechanisms, unless another command requiring those mechanisms is scheduled between + // calling cancel() and calling run() + m_runningCommands.remove(command); + m_queuedToRun.removeIf(state -> state.command() == command); + + if (running) { + // Only run the hook if the command was running. If it was on deck or not + // even in the scheduler at the time, then there's nothing to do + command.onCancel(); + emitCanceledEvent(command); + } + + // Clean up any orphaned child commands; their lifespan must not exceed the parent's + removeOrphanedChildren(command); + } + + /** + * Updates the command scheduler. This will run operations in the following order: + * + *

    + *
  1. Run sideloaded functions from {@link #sideload(Consumer)} and {@link + * #addPeriodic(Runnable)} + *
  2. Update trigger bindings to queue and cancel bound commands + *
  3. Queue default commands for mechanisms that do not have a queued or running command + *
  4. Promote queued commands to the running set + *
  5. For every command in the running set, mount and run that command until it calls {@link + * Coroutine#yield()} or exits + *
+ * + *

This method is intended to be called in a periodic loop like {@link + * TimedRobot#robotPeriodic()} + */ + public void run() { + final long startMicros = RobotController.getTime(); + + // Sideloads may change some state that affects triggers. Run them first. + runPeriodicSideloads(); + + // Poll triggers next to schedule and cancel commands + m_eventLoop.poll(); + + // Schedule default commands for any mechanisms that don't have a running command and didn't + // have a new command scheduled by a sideload function or a trigger + scheduleDefaultCommands(); + + // Move all scheduled commands to the running set + promoteScheduledCommands(); + + // Run every command in order until they call Coroutine.yield() or exit + runCommands(); + + final long endMicros = RobotController.getTime(); + m_lastRunTimeMs = Milliseconds.convertFrom(endMicros - startMicros, Microseconds); + } + + private void promoteScheduledCommands() { + // Clear any commands that conflict with the scheduled set + for (var queuedState : m_queuedToRun) { + evictConflictingRunningCommands(queuedState); + } + + // Move any scheduled commands to the running set + for (var queuedState : m_queuedToRun) { + m_runningCommands.put(queuedState.command(), queuedState); + } + + // Clear the set of on-deck commands, + // since we just put them all into the set of running commands + m_queuedToRun.clear(); + } + + private void runPeriodicSideloads() { + // Update periodic callbacks + for (Coroutine coroutine : m_periodicCallbacks) { + coroutine.mount(); + try { + coroutine.runToYieldPoint(); + } finally { + Continuation.mountContinuation(null); + } + } + + // And remove any periodic callbacks that have completed + m_periodicCallbacks.removeIf(Coroutine::isDone); + } + + private void runCommands() { + // Tick every command that hasn't been completed yet + // Run in reverse so parent commands can resume in the same loop cycle an awaited child command + // completes. Otherwise, parents could only resume on the next loop cycle, introducing a delay + // at every layer of nesting. + for (var state : List.copyOf(m_runningCommands.values()).reversed()) { + runCommand(state); + } + } + + /** + * Mounts and runs a command until it yields or exits. + * + * @param state The command state to run + */ + @SuppressWarnings("PMD.AvoidCatchingGenericException") + private void runCommand(CommandState state) { + final var command = state.command(); + final var coroutine = state.coroutine(); + + if (!m_runningCommands.containsKey(command)) { + // Probably canceled by an owning composition, do not run + return; + } + + var previousState = currentState(); + + m_currentCommandAncestry.push(state); + long startMicros = RobotController.getTime(); + emitMountedEvent(command); + coroutine.mount(); + try { + coroutine.runToYieldPoint(); + } catch (RuntimeException e) { + // Command encountered an uncaught exception. + handleCommandException(state, e); + } finally { + long endMicros = RobotController.getTime(); + double elapsedMs = Milliseconds.convertFrom(endMicros - startMicros, Microseconds); + state.setLastRuntimeMs(elapsedMs); + + if (state.equals(currentState())) { + // Remove the command we just ran from the top of the stack + m_currentCommandAncestry.pop(); + } + + if (previousState != null) { + // Remount the parent command, if there is one + previousState.coroutine().mount(); + } else { + Continuation.mountContinuation(null); + } + } + + if (coroutine.isDone()) { + // Immediately check if the command has completed and remove any children commands. + // This prevents child commands from being executed one extra time in the run() loop + emitCompletedEvent(command); + m_runningCommands.remove(command); + removeOrphanedChildren(command); + } else { + // Yielded + emitYieldedEvent(command); + } + } + + /** + * Handles uncaught runtime exceptions from a mounted and running command. The command's ancestor + * and child commands will all be canceled and the exception's backtrace will be modified to + * include the stack frames of the schedule call site. + * + * @param state The state of the command that encountered the exception. + * @param e The exception that was thrown. + * @throws RuntimeException rethrows the exception, with a modified backtrace pointing to the + * schedule site + */ + @SuppressWarnings("PMD.CompareObjectsWithEquals") + private void handleCommandException(CommandState state, RuntimeException e) { + var command = state.command(); + + // Fetch the root command + // (needs to be done before removing the failed command from the running set) + Command root = command; + while (getParentOf(root) != null) { + root = getParentOf(root); + } + + // Remove it from the running set. + m_runningCommands.remove(command); + + // Intercept the exception, inject stack frames from the schedule site, and rethrow it + var binding = state.binding(); + e.setStackTrace(CommandTraceHelper.modifyTrace(e.getStackTrace(), binding.frames())); + emitCompletedWithErrorEvent(command, e); + + // Clean up child commands after emitting the event so child Canceled events are emitted + // after the parent's CompletedWithError + removeOrphanedChildren(command); + + // Bubble up to the root and cancel all commands between the root and this one + // Note: Because we remove the command from the running set above, we still need to + // clean up all the failed command's children + if (root != null && root != command) { + cancel(root); + } + + // Then rethrow the exception + throw e; + } + + /** + * Gets the currently executing command state, or null if no command is currently executing. + * + * @return the currently executing command state + */ + private CommandState currentState() { + if (m_currentCommandAncestry.isEmpty()) { + // Avoid EmptyStackException + return null; + } + + return m_currentCommandAncestry.peek(); + } + + /** + * Gets the currently executing command, or null if no command is currently executing. + * + * @return the currently executing command + */ + public Command currentCommand() { + var state = currentState(); + if (state == null) { + return null; + } + + return state.command(); + } + + private void scheduleDefaultCommands() { + // Schedule the default commands for every mechanism that doesn't currently have a running or + // scheduled command. + m_defaultCommands.forEach( + (mechanism, defaultCommand) -> { + if (m_runningCommands.keySet().stream().noneMatch(c -> c.requires(mechanism)) + && m_queuedToRun.stream().noneMatch(c -> c.command().requires(mechanism)) + && defaultCommand != null) { + // Nothing currently running or scheduled + // Schedule the mechanism's default command, if it has one + schedule(defaultCommand); + } + }); + } + + /** + * Removes all commands descended from a parent command. This is used to ensure that any command + * scheduled within a composition or group cannot live longer than any ancestor. + * + * @param parent the root command whose descendants to remove from the scheduler + */ + @SuppressWarnings("PMD.CompareObjectsWithEquals") + private void removeOrphanedChildren(Command parent) { + m_runningCommands.entrySet().stream() + .filter(e -> e.getValue().parent() == parent) + .toList() // copy to an intermediate list to avoid concurrent modification + .forEach(e -> cancel(e.getKey())); + } + + /** + * Builds a coroutine object that the command will be bound to. The coroutine will be scoped to + * this scheduler object and cannot be used by another scheduler instance. + * + * @param command the command for which to build a coroutine + * @return the binding coroutine + */ + private Coroutine buildCoroutine(Command command) { + return new Coroutine(this, m_scope, command::run); + } + + /** + * Checks if a command is currently running. + * + * @param command the command to check + * @return true if the command is running, false if not + */ + public boolean isRunning(Command command) { + return m_runningCommands.containsKey(command); + } + + /** + * Checks if a command is currently scheduled to run, but is not yet running. + * + * @param command the command to check + * @return true if the command is scheduled to run, false if not + */ + @SuppressWarnings("PMD.CompareObjectsWithEquals") + public boolean isScheduled(Command command) { + return m_queuedToRun.stream().anyMatch(state -> state.command() == command); + } + + /** + * Checks if a command is currently scheduled to run, or is already running. + * + * @param command the command to check + * @return true if the command is scheduled to run or is already running, false if not + */ + public boolean isScheduledOrRunning(Command command) { + return isScheduled(command) || isRunning(command); + } + + /** + * Gets the set of all currently running commands. Commands are returned in the order in which + * they were scheduled. The returned set is read-only. + * + * @return the currently running commands + */ + public Collection getRunningCommands() { + return Collections.unmodifiableSet(m_runningCommands.keySet()); + } + + /** + * Gets all the currently running commands that require a particular mechanism. Commands are + * returned in the order in which they were scheduled. The returned list is read-only. + * + * @param mechanism the mechanism to get the commands for + * @return the currently running commands that require the mechanism. + */ + public List getRunningCommandsFor(Mechanism mechanism) { + return m_runningCommands.keySet().stream() + .filter(command -> command.requires(mechanism)) + .toList(); + } + + /** + * Cancels all currently running and scheduled commands. All default commands will be scheduled on + * the next call to {@link #run()}, unless a higher priority command is scheduled or triggered + * after {@code cancelAll()} is used. + */ + public void cancelAll() { + for (var onDeckIter = m_queuedToRun.iterator(); onDeckIter.hasNext(); ) { + var state = onDeckIter.next(); + onDeckIter.remove(); + emitCanceledEvent(state.command()); + } + + for (var liveIter = m_runningCommands.entrySet().iterator(); liveIter.hasNext(); ) { + var entry = liveIter.next(); + liveIter.remove(); + Command canceledCommand = entry.getKey(); + canceledCommand.onCancel(); + emitCanceledEvent(canceledCommand); + } + } + + /** + * An event loop used by the scheduler to poll triggers that schedule or cancel commands. This + * event loop is always polled on every call to {@link #run()}. Custom event loops need to be + * bound to this one for synchronicity with the scheduler; however, they can always be polled + * manually before or after the call to {@link #run()}. + * + * @return the default event loop. + */ + @NoDiscard + public EventLoop getDefaultEventLoop() { + return m_eventLoop; + } + + /** + * For internal use. + * + * @return The commands that have been scheduled but not yet started. + */ + @NoDiscard + public Collection getQueuedCommands() { + return m_queuedToRun.stream().map(CommandState::command).toList(); + } + + /** + * For internal use. + * + * @param command The command to check + * @return The command that forked the provided command. Null if the command is not a child of + * another command. + */ + public Command getParentOf(Command command) { + var state = m_runningCommands.get(command); + if (state == null) { + return null; + } + return state.parent(); + } + + /** + * Gets how long a command took to run in the previous cycle. If the command is not currently + * running, returns -1. + * + * @param command The command to check + * @return How long, in milliseconds, the command last took to execute. + */ + public double lastCommandRuntimeMs(Command command) { + if (m_runningCommands.containsKey(command)) { + return m_runningCommands.get(command).lastRuntimeMs(); + } else { + return -1; + } + } + + /** + * Gets how long a command has taken to run, in aggregate, since it was most recently scheduled. + * If the command is not currently running, returns -1. + * + * @param command The command to check + * @return How long, in milliseconds, the command has taken to execute in total + */ + public double totalRuntimeMs(Command command) { + if (m_runningCommands.containsKey(command)) { + return m_runningCommands.get(command).totalRuntimeMs(); + } else { + // Not running; no data + return -1; + } + } + + /** + * Gets the unique run id for a scheduled or running command. If the command is not currently + * scheduled or running, this function returns {@code 0}. + * + * @param command The command to get the run ID for + * @return The run of the command + */ + @NoDiscard + @SuppressWarnings("PMD.CompareObjectsWithEquals") + public int runId(Command command) { + if (m_runningCommands.containsKey(command)) { + return m_runningCommands.get(command).id(); + } + + // Check scheduled commands + for (var scheduled : m_queuedToRun) { + if (scheduled.command() == command) { + return scheduled.id(); + } + } + + return 0; + } + + /** + * Gets how long the scheduler took to process its most recent {@link #run()} invocation, in + * milliseconds. Defaults to -1 if the scheduler has not yet run. + * + * @return How long, in milliseconds, the scheduler last took to execute. + */ + @NoDiscard + public double lastRuntimeMs() { + return m_lastRunTimeMs; + } + + // Event-base telemetry and helpers. The static factories are for convenience to automatically + // set the timestamp instead of littering RobotController.getTime() everywhere. + + private void emitScheduledEvent(Command command) { + var event = new SchedulerEvent.Scheduled(command, RobotController.getTime()); + emitEvent(event); + } + + private void emitMountedEvent(Command command) { + var event = new SchedulerEvent.Mounted(command, RobotController.getTime()); + emitEvent(event); + } + + private void emitYieldedEvent(Command command) { + var event = new SchedulerEvent.Yielded(command, RobotController.getTime()); + emitEvent(event); + } + + private void emitCompletedEvent(Command command) { + var event = new SchedulerEvent.Completed(command, RobotController.getTime()); + emitEvent(event); + } + + private void emitCompletedWithErrorEvent(Command command, Throwable error) { + var event = new SchedulerEvent.CompletedWithError(command, error, RobotController.getTime()); + emitEvent(event); + } + + private void emitCanceledEvent(Command command) { + var event = new SchedulerEvent.Canceled(command, RobotController.getTime()); + emitEvent(event); + } + + private void emitInterruptedEvent(Command command, Command interrupter) { + var event = new SchedulerEvent.Interrupted(command, interrupter, RobotController.getTime()); + emitEvent(event); + } + + /** + * Adds a listener to handle events that are emitted by the scheduler. Events are emitted when + * certain actions are taken by user code or by internal processing logic in the scheduler. + * Listeners should take care to be quick, simple, and not schedule or cancel commands, as that + * may cause inconsistent scheduler behavior or even cause a program crash. + * + *

Listeners are primarily expected to be for data logging and telemetry. In particular, a + * one-shot command (one that never calls {@link Coroutine#yield()}) will never appear in the + * standard protobuf telemetry because it is scheduled, runs, and finishes all in a single + * scheduler cycle. However, {@link SchedulerEvent.Scheduled},{@link SchedulerEvent.Mounted}, and + * {@link SchedulerEvent.Completed} events will be emitted corresponding to those actions, and + * user code can listen for and log such events. + * + * @param listener The listener to add. Cannot be null. + * @throws NullPointerException if given a null listener + */ + public void addEventListener(Consumer listener) { + ErrorMessages.requireNonNullParam(listener, "listener", "addEventListener"); + + m_eventListeners.add(listener); + } + + private void emitEvent(SchedulerEvent event) { + // TODO: Prevent listeners from interacting with the scheduler. + // Scheduling or canceling commands while the scheduler is processing will probably cause + // bugs in user code or even a program crash. + for (var listener : m_eventListeners) { + listener.accept(event); + } + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/SchedulerEvent.java b/commandsv3/src/main/java/org/wpilib/commands3/SchedulerEvent.java new file mode 100644 index 0000000000..32641722bb --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/SchedulerEvent.java @@ -0,0 +1,89 @@ +// 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.commands3; + +import edu.wpi.first.wpilibj.RobotController; +import java.util.function.Consumer; + +/** + * An event that occurs during scheduler processing. This can range from {@link Scheduled a command + * being scheduled} by a trigger or manual call to {@link Scheduler#schedule(Command)} to {@link + * Interrupted a command being interrupted by another}. Event listeners can be registered with a + * scheduler via {@link Scheduler#addEventListener(Consumer)}. All events have a timestamp to + * indicate when the event occurred. + */ +public sealed interface SchedulerEvent { + /** + * The timestamp for when the event occurred. Measured in microseconds since some arbitrary start + * time. + * + * @return The event timestamp. + * @see RobotController#getTime() + */ + long timestampMicros(); + + /** + * An event marking when a command is scheduled in {@link Scheduler#schedule(Command)}. + * + * @param command The command that was scheduled + * @param timestampMicros When the command was scheduled + */ + record Scheduled(Command command, long timestampMicros) implements SchedulerEvent {} + + /** + * An event marking when a command starts running. + * + * @param command The command that started + * @param timestampMicros When the command started + */ + record Mounted(Command command, long timestampMicros) implements SchedulerEvent {} + + /** + * An event marking when a command yielded control with {@link Coroutine#yield()}. + * + * @param command The command that yielded + * @param timestampMicros When the command yielded + */ + record Yielded(Command command, long timestampMicros) implements SchedulerEvent {} + + /** + * An event marking when a command completed naturally. + * + * @param command The command that completed + * @param timestampMicros When the command completed + */ + record Completed(Command command, long timestampMicros) implements SchedulerEvent {} + + /** + * An event marking when a command threw or encountered an unhanded exception. + * + * @param command The command that encountered the error + * @param error The unhandled error + * @param timestampMicros When the error occurred + */ + record CompletedWithError(Command command, Throwable error, long timestampMicros) + implements SchedulerEvent {} + + /** + * An event marking when a command was canceled. If the command was canceled because it was + * interrupted by another command, an {@link Interrupted} will be emitted immediately prior to the + * cancellation event. + * + * @param command The command that was canceled + * @param timestampMicros When the command was canceled + */ + record Canceled(Command command, long timestampMicros) implements SchedulerEvent {} + + /** + * An event marking when a command was interrupted by another. Typically followed by an {@link + * Canceled} event. + * + * @param command The command that was interrupted + * @param interrupter The interrupting command + * @param timestampMicros When the command was interrupted + */ + record Interrupted(Command command, Command interrupter, long timestampMicros) + implements SchedulerEvent {} +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/SequentialGroup.java b/commandsv3/src/main/java/org/wpilib/commands3/SequentialGroup.java new file mode 100644 index 0000000000..e6458527a1 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/SequentialGroup.java @@ -0,0 +1,80 @@ +// 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.commands3; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import java.util.ArrayList; +import java.util.HashSet; +import java.util.List; +import java.util.Set; + +/** + * A sequence of commands that run one after another. Each successive command only starts after its + * predecessor completes execution. The priority of a sequence is equal to the highest priority of + * any of its commands. If all commands in the sequence are able to run while the robot is disabled, + * then the sequence itself will be allowed to run while the robot is disabled. + * + *

The sequence will own all mechanisms required by all commands in the sequence for the + * entire duration of the sequence. This means that a mechanism owned by one command in the + * sequence, but not by a later one, will be uncommanded while that later command executes. + */ +public final class SequentialGroup implements Command { + private final String m_name; + private final List m_commands = new ArrayList<>(); + private final Set m_requirements = new HashSet<>(); + private final int m_priority; + + /** + * Creates a new command sequence. + * + * @param name the name of the sequence + * @param commands the commands to execute within the sequence + */ + SequentialGroup(String name, List commands) { + requireNonNullParam(name, "name", "SequentialGroup"); + requireNonNullParam(commands, "commands", "SequentialGroup"); + for (int i = 0; i < commands.size(); i++) { + requireNonNullParam(commands.get(i), "commands[" + i + "]", "SequentialGroup"); + } + + m_name = name; + m_commands.addAll(commands); + + for (var command : commands) { + m_requirements.addAll(command.requirements()); + } + + m_priority = + commands.stream().mapToInt(Command::priority).max().orElse(Command.DEFAULT_PRIORITY); + } + + @Override + public void run(Coroutine coroutine) { + for (var command : m_commands) { + coroutine.await(command); + } + } + + @Override + public String name() { + return m_name; + } + + @Override + public Set requirements() { + return m_requirements; + } + + @Override + public int priority() { + return m_priority; + } + + @Override + public String toString() { + return "SequentialGroup[name=" + m_name + "]"; + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/SequentialGroupBuilder.java b/commandsv3/src/main/java/org/wpilib/commands3/SequentialGroupBuilder.java new file mode 100644 index 0000000000..5c10a61aa4 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/SequentialGroupBuilder.java @@ -0,0 +1,106 @@ +// 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.commands3; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.function.BooleanSupplier; +import java.util.stream.Collectors; +import org.wpilib.annotation.NoDiscard; + +/** + * A builder class to configure and then create a {@link SequentialGroup}. Like {@link + * StagedCommandBuilder}, the final command is created by calling the terminal {@link + * #named(String)} method, or with an automatically generated name using {@link + * #withAutomaticName()}. + */ +@NoDiscard +public class SequentialGroupBuilder { + private final List m_steps = new ArrayList<>(); + private BooleanSupplier m_endCondition; + + /** + * Creates new SequentialGroupBuilder. The builder will have no commands and have no preapplied + * configuration options. Use {@link #andThen(Command)} to add commands to the sequence. + */ + public SequentialGroupBuilder() {} + + /** + * Adds a command to the sequence. + * + * @param next The next command in the sequence + * @return The builder object, for chaining + */ + public SequentialGroupBuilder andThen(Command next) { + requireNonNullParam(next, "next", "SequentialGroupBuilder.andThen"); + + m_steps.add(next); + return this; + } + + /** + * Adds commands to the sequence. Commands will be added to the sequence in the order they are + * passed to this method. + * + * @param nextCommands The next commands in the sequence + * @return The builder object, for chaining + */ + public SequentialGroupBuilder andThen(Command... nextCommands) { + requireNonNullParam(nextCommands, "nextCommands", "SequentialGroupBuilder.andThen"); + for (int i = 0; i < nextCommands.length; i++) { + requireNonNullParam( + nextCommands[i], "nextCommands[" + i + "]", "SequentialGroupBuilder.andThen"); + } + + m_steps.addAll(Arrays.asList(nextCommands)); + return this; + } + + /** + * Adds an end condition to the command group. If this condition is met before all required + * commands have completed, the group will exit early. If multiple end conditions are added (e.g. + * {@code .until(() -> conditionA()).until(() -> conditionB())}), then the last end condition + * added will be used and any previously configured condition will be overridden. + * + * @param endCondition The end condition for the group + * @return The builder object, for chaining + */ + public SequentialGroupBuilder until(BooleanSupplier endCondition) { + m_endCondition = endCondition; + return this; + } + + /** + * Creates the sequence command, giving it the specified name. + * + * @param name The name of the sequence command + * @return The built command + */ + public Command named(String name) { + var seq = new SequentialGroup(name, m_steps); + if (m_endCondition != null) { + // No custom end condition, return the group as is + return seq; + } + + // We have a custom end condition, so we need to wrap the group in a race + return new ParallelGroupBuilder() + .optional(seq, Command.waitUntil(m_endCondition).named("Until Condition")) + .named(name); + } + + /** + * Creates the sequence command, giving it an automatically generated name based on the commands + * in the sequence. + * + * @return The built command + */ + public Command withAutomaticName() { + return named(m_steps.stream().map(Command::name).collect(Collectors.joining(" -> "))); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/StagedCommandBuilder.java b/commandsv3/src/main/java/org/wpilib/commands3/StagedCommandBuilder.java new file mode 100644 index 0000000000..5b68ce5602 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/StagedCommandBuilder.java @@ -0,0 +1,299 @@ +// 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.commands3; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import java.util.Arrays; +import java.util.Collection; +import java.util.HashSet; +import java.util.Objects; +import java.util.Set; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import org.wpilib.annotation.NoDiscard; + +/** + * A builder class for commands. Command configuration is done in stages, where later stages have + * different configuration options than earlier stages. Commands may only be created after going + * through every stage, enforcing the presence of required options. All commands must have a + * set of requirements (which may be empty), a name, and an implementation. The builder stages are + * defined such that the final stage that creates a command can only be reached after going through + * the earlier stages to configure those required options. + * + *

Example usage: + * + * 

{@code
+ * StagedCommandBuilder start = new StagedCommandBuilder();
+ * NeedsExecutionBuilderStage withRequirements = start.requiring(mechanism1, mechanism2);
+ * NeedsNameBuilderStage withExecution = withRequirements.executing(coroutine -> ...);
+ * Command exampleCommand = withExecution.named("Example Command");
+ * }
+ * + *

Because every method on the builders returns a builder object, these calls can be chained to + * cut down on verbosity and make the code easier to read. This is the recommended style: + * + * 

{@code
+ * Command exampleCommand =
+ *   new StagedCommandBuilder()
+ *     .requiring(mechanism1, mechanism2)
+ *     .executing(coroutine -> ...)
+ *     .named("Example Command");
+ * }
+ * + *

And can be cut down even further by using helper methods provided by the library: + * + * 

{@code
+ * Command exampleCommand =
+ *   Command
+ *     .requiring(mechanism1, mechanism2)
+ *     .executing(coroutine -> ...)
+ *     .named("Example Command");
+ * }
+ */ +@NoDiscard +public final class StagedCommandBuilder { + private final Set m_requirements = new HashSet<>(); + private Consumer m_impl; + private Runnable m_onCancel = () -> {}; + private String m_name; + private int m_priority = Command.DEFAULT_PRIORITY; + private BooleanSupplier m_endCondition; + + private Command m_builtCommand; + + // Using internal anonymous classes to implement the staged builder APIs, but all backed by the + // state of the enclosing StagedCommandBuilder object + + private final NeedsExecutionBuilderStage m_needsExecutionView = + new NeedsExecutionBuilderStage() { + @Override + public NeedsExecutionBuilderStage requiring(Mechanism requirement) { + throwIfAlreadyBuilt(); + + requireNonNullParam(requirement, "requirement", "StagedCommandBuilder.requiring"); + m_requirements.add(requirement); + return this; + } + + @Override + public NeedsExecutionBuilderStage requiring(Mechanism requirement, Mechanism... extra) { + throwIfAlreadyBuilt(); + + requireNonNullParam(requirement, "requirement", "StagedCommandBuilder.requiring"); + requireNonNullParam(extra, "extra", "StagedCommandBuilder.requiring"); + + for (int i = 0; i < extra.length; i++) { + requireNonNullParam(extra[i], "extra[" + i + "]", "StagedCommandBuilder.requiring"); + } + + m_requirements.add(requirement); + m_requirements.addAll(Arrays.asList(extra)); + return this; + } + + @Override + public NeedsExecutionBuilderStage requiring(Collection requirements) { + throwIfAlreadyBuilt(); + + requireNonNullParam(requirements, "requirements", "StagedCommandBuilder.requiring"); + int i = 0; + for (Mechanism requirement : requirements) { + requireNonNullParam( + requirement, "requirements[" + i + "]", "StagedCommandBuilder.requiring"); + i++; + } + + m_requirements.addAll(requirements); + return this; + } + + @Override + public NeedsNameBuilderStage executing(Consumer impl) { + throwIfAlreadyBuilt(); + + requireNonNullParam(impl, "impl", "StagedCommandBuilder.executing"); + m_impl = impl; + return m_needsNameView; + } + }; + + private final NeedsNameBuilderStage m_needsNameView = + new NeedsNameBuilderStage() { + @Override + public NeedsNameBuilderStage whenCanceled(Runnable onCancel) { + throwIfAlreadyBuilt(); + + m_onCancel = onCancel; + return this; + } + + @Override + public NeedsNameBuilderStage withPriority(int priority) { + throwIfAlreadyBuilt(); + + m_priority = priority; + return this; + } + + @Override + public NeedsNameBuilderStage until(BooleanSupplier endCondition) { + throwIfAlreadyBuilt(); + + m_endCondition = endCondition; // allowed to be null + return this; + } + + @Override + public Command named(String name) { + throwIfAlreadyBuilt(); + + requireNonNullParam(name, "name", "StagedCommandBuilder.withName"); + m_name = name; + + var command = new BuilderBackedCommand(StagedCommandBuilder.this); + + if (m_endCondition == null) { + // No custom end condition, just return the raw command + m_builtCommand = command; + } else { + // A custom end condition is implemented as a race group, since we cannot modify the + // command body to inject the end condition. + m_builtCommand = + new ParallelGroupBuilder().requiring(command).until(m_endCondition).named(m_name); + } + + return m_builtCommand; + } + }; + + private static final class BuilderBackedCommand implements Command { + private static final Runnable kNoOp = () -> {}; + + private final Set m_requirements; + private final Consumer m_impl; + private final Runnable m_onCancel; + private final String m_name; + private final int m_priority; + + private BuilderBackedCommand(StagedCommandBuilder builder) { + // Copy builder fields into the command so the builder object can be garbage collected + m_requirements = new HashSet<>(builder.m_requirements); + m_impl = builder.m_impl; + m_onCancel = Objects.requireNonNullElse(builder.m_onCancel, kNoOp); + m_name = builder.m_name; + m_priority = builder.m_priority; + } + + @Override + public void run(Coroutine coroutine) { + m_impl.accept(coroutine); + } + + @Override + public void onCancel() { + m_onCancel.run(); + } + + @Override + public String name() { + return m_name; + } + + @Override + public Set requirements() { + return m_requirements; + } + + @Override + public int priority() { + return m_priority; + } + + @Override + public String toString() { + return name(); + } + } + + /** + * Creates a new command builder. All required options must be set on each stage before a command + * is able to be created. Attempting to create a command without setting all required options will + * result in a compilation error. + */ + public StagedCommandBuilder() {} + + /** + * Explicitly marks the command as requiring no mechanisms. Unless overridden later with {@link + * NeedsExecutionBuilderStage#requiring(Mechanism)} or a similar method, the built command will + * not have ownership over any mechanisms when it runs. Use this for commands that don't need to + * own a mechanism, such as a gyro zeroing command, that does some kind of cleanup task without + * needing to control something. + * + * @return A builder object that can be used to further configure the command. + */ + public NeedsExecutionBuilderStage noRequirements() { + throwIfAlreadyBuilt(); + + return m_needsExecutionView; + } + + /** + * Marks the command as requiring one or more mechanisms. If only a single mechanism is required, + * prefer a factory function like {@link Mechanism#run(Consumer)} or similar - it will + * automatically require the mechanism, instead of it needing to be explicitly specified. + * + * @param requirement The first required mechanism. Cannot be null. + * @param extra Any optional extra required mechanisms. May be empty, but cannot be null or + * contain null values. + * @return A builder object that can be used to further configure the command. + */ + public NeedsExecutionBuilderStage requiring(Mechanism requirement, Mechanism... extra) { + throwIfAlreadyBuilt(); + + requireNonNullParam(requirement, "requirement", "StagedCommandBuilder.requiring"); + requireNonNullParam(extra, "extra", "StagedCommandBuilder.requiring"); + + for (int i = 0; i < extra.length; i++) { + requireNonNullParam(extra[i], "extra[" + i + "]", "StagedCommandBuilder.requiring"); + } + + m_requirements.add(requirement); + m_requirements.addAll(Arrays.asList(extra)); + return m_needsExecutionView; + } + + /** + * Marks the command as requiring zero or more mechanisms. If only a single mechanism is required, + * prefer a factory function like {@link Mechanism#run(Consumer)} or similar - it will + * automatically require the mechanism, instead of it needing to be explicitly specified. + * + * @param requirements A collection of required mechanisms. May be empty, but cannot be null or + * contain null values. + * @return A builder object that can be used to further configure the command. + */ + public NeedsExecutionBuilderStage requiring(Collection requirements) { + throwIfAlreadyBuilt(); + + requireNonNullParam(requirements, "requirements", "StagedCommandBuilder.requiring"); + int i = 0; + for (var mechanism : requirements) { + requireNonNullParam(mechanism, "requirements[" + i + "]", "StagedCommandBuilder.requiring"); + i++; + } + + m_requirements.addAll(requirements); + return m_needsExecutionView; + } + + // Prevent builders from being mutated after command creation + // Weird things could happen like changing requirements, priority level, or even the command + // implementation itself if we didn't prohibit it. + private void throwIfAlreadyBuilt() { + if (m_builtCommand != null) { + throw new IllegalStateException("Command builders cannot be reused"); + } + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/Trigger.java b/commandsv3/src/main/java/org/wpilib/commands3/Trigger.java new file mode 100644 index 0000000000..d9e273f029 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/Trigger.java @@ -0,0 +1,368 @@ +// 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.commands3; + +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.event.EventLoop; +import java.util.ArrayList; +import java.util.EnumMap; +import java.util.List; +import java.util.Map; +import java.util.function.BooleanSupplier; + +/** + * Triggers allow users to specify conditions for when commands should run. Triggers can be set up + * to read from joystick and controller buttons (eg {@link + * org.wpilib.commands3.button.CommandXboxController#x()}) or be customized to read sensor values or + * any other arbitrary true/false condition. + * + *

It is very easy to link a button to a command. For instance, you could link the trigger button + * of a joystick to a "score" command. + * + *

Triggers can easily be composed for advanced functionality using the {@link + * #and(BooleanSupplier)}, {@link #or(BooleanSupplier)}, {@link #negate()} operators. + * + *

Trigger bindings created inside a running command will only be active while that command is + * running. This is useful for defining trigger-based behavior only in a certain scope and avoids + * needing to create dozens of global triggers. Any commands scheduled by these triggers will be + * canceled when the enclosing command exits. + * + * 

{@code
+ * Command shootWhileAiming = Command.noRequirements().executing(co -> {
+ *   turret.atTarget.onTrue(shooter.shootOnce());
+ *   co.await(turret.lockOnGoal());
+ * }).named("Shoot While Aiming");
+ * controller.rightBumper().whileTrue(shootWhileAiming);
+ * }
+ */ +public class Trigger implements BooleanSupplier { + private final BooleanSupplier m_condition; + private final EventLoop m_loop; + private final Scheduler m_scheduler; + private Signal m_previousSignal; + private final Map> m_bindings = new EnumMap<>(BindingType.class); + private final Runnable m_eventLoopCallback = this::poll; + private boolean m_isBoundToEventLoop; // used for lazily binding to the event loop + + /** + * Represents the state of a signal: high or low. Used instead of a boolean for nullity on the + * first run, when the previous signal value is undefined and unknown. + */ + private enum Signal { + /** The signal is high. */ + HIGH, + /** The signal is low. */ + LOW + } + + /** + * Creates a new trigger based on the given condition. Polled by the scheduler's {@link + * Scheduler#getDefaultEventLoop() default event loop}. + * + * @param scheduler The scheduler that should execute triggered commands. + * @param condition the condition represented by this trigger + */ + public Trigger(Scheduler scheduler, BooleanSupplier condition) { + m_scheduler = requireNonNullParam(scheduler, "scheduler", "Trigger"); + m_loop = scheduler.getDefaultEventLoop(); + m_condition = requireNonNullParam(condition, "condition", "Trigger"); + } + + /** + * Creates a new trigger based on the given condition. Triggered commands are executed by the + * {@link Scheduler#getDefault() default scheduler}. + * + *

Polled by the default scheduler button loop. + * + * @param condition the condition represented by this trigger + */ + public Trigger(BooleanSupplier condition) { + this(Scheduler.getDefault(), condition); + } + + /** + * Creates a new trigger based on the given condition. + * + * @param scheduler The scheduler that should execute triggered commands. + * @param loop The event loop to poll the trigger. + * @param condition the condition represented by this trigger + */ + public Trigger(Scheduler scheduler, EventLoop loop, BooleanSupplier condition) { + m_scheduler = requireNonNullParam(scheduler, "scheduler", "Trigger"); + m_loop = requireNonNullParam(loop, "loop", "Trigger"); + m_condition = requireNonNullParam(condition, "condition", "Trigger"); + } + + /** + * Starts the given command whenever the condition changes from `false` to `true`. + * + * @param command the command to start + * @return this trigger, so calls can be chained + */ + public Trigger onTrue(Command command) { + requireNonNullParam(command, "command", "onTrue"); + addBinding(BindingType.SCHEDULE_ON_RISING_EDGE, command); + return this; + } + + /** + * Starts the given command whenever the condition changes from `true` to `false`. + * + * @param command the command to start + * @return this trigger, so calls can be chained + */ + public Trigger onFalse(Command command) { + requireNonNullParam(command, "command", "onFalse"); + addBinding(BindingType.SCHEDULE_ON_FALLING_EDGE, command); + return this; + } + + /** + * Starts the given command when the condition changes to `true` and cancels it when the condition + * changes to `false`. + * + *

Doesn't re-start the command if it ends while the condition is still `true`. + * + * @param command the command to start + * @return this trigger, so calls can be chained + */ + public Trigger whileTrue(Command command) { + requireNonNullParam(command, "command", "whileTrue"); + addBinding(BindingType.RUN_WHILE_HIGH, command); + return this; + } + + /** + * Starts the given command when the condition changes to `false` and cancels it when the + * condition changes to `true`. + * + *

Doesn't re-start the command if it ends while the condition is still `false`. + * + * @param command the command to start + * @return this trigger, so calls can be chained + */ + public Trigger whileFalse(Command command) { + requireNonNullParam(command, "command", "whileFalse"); + addBinding(BindingType.RUN_WHILE_LOW, command); + return this; + } + + /** + * Toggles a command when the condition changes from `false` to `true`. + * + * @param command the command to toggle + * @return this trigger, so calls can be chained + */ + public Trigger toggleOnTrue(Command command) { + requireNonNullParam(command, "command", "toggleOnTrue"); + addBinding(BindingType.TOGGLE_ON_RISING_EDGE, command); + return this; + } + + /** + * Toggles a command when the condition changes from `true` to `false`. + * + * @param command the command to toggle + * @return this trigger, so calls can be chained + */ + public Trigger toggleOnFalse(Command command) { + requireNonNullParam(command, "command", "toggleOnFalse"); + addBinding(BindingType.TOGGLE_ON_FALLING_EDGE, command); + return this; + } + + @Override + public boolean getAsBoolean() { + return m_condition.getAsBoolean(); + } + + /** + * Composes two triggers with logical AND. + * + * @param trigger the condition to compose with + * @return A trigger which is active when both component triggers are active. + */ + public Trigger and(BooleanSupplier trigger) { + return new Trigger( + m_scheduler, m_loop, () -> m_condition.getAsBoolean() && trigger.getAsBoolean()); + } + + /** + * Composes two triggers with logical OR. + * + * @param trigger the condition to compose with + * @return A trigger which is active when either component trigger is active. + */ + public Trigger or(BooleanSupplier trigger) { + return new Trigger( + m_scheduler, m_loop, () -> m_condition.getAsBoolean() || trigger.getAsBoolean()); + } + + /** + * Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the + * negation of this trigger. + * + * @return the negated trigger + */ + public Trigger negate() { + return new Trigger(m_scheduler, m_loop, () -> !m_condition.getAsBoolean()); + } + + /** + * Creates a new debounced trigger from this trigger - it will become active when this trigger has + * been active for longer than the specified period. + * + * @param duration The debounce period. + * @return The debounced trigger (rising edges debounced only) + */ + public Trigger debounce(Time duration) { + return debounce(duration, Debouncer.DebounceType.kRising); + } + + /** + * Creates a new debounced trigger from this trigger - it will become active when this trigger has + * been active for longer than the specified period. + * + * @param duration The debounce period. + * @param type The debounce type. + * @return The debounced trigger. + */ + public Trigger debounce(Time duration, Debouncer.DebounceType type) { + var debouncer = new Debouncer(duration.in(Seconds), type); + return new Trigger(m_scheduler, m_loop, () -> debouncer.calculate(m_condition.getAsBoolean())); + } + + private void poll() { + // Clear bindings that no longer need to run + // This should always be checked, regardless of signal change, since bindings may be scoped + // and those scopes may become inactive. + clearStaleBindings(); + + var signal = readSignal(); + + if (signal == m_previousSignal) { + // No change in the signal. Nothing to do + return; + } + + if (signal == Signal.HIGH) { + // Signal is now high when it wasn't before - a rising edge + scheduleBindings(BindingType.SCHEDULE_ON_RISING_EDGE); + scheduleBindings(BindingType.RUN_WHILE_HIGH); + cancelBindings(BindingType.RUN_WHILE_LOW); + toggleBindings(BindingType.TOGGLE_ON_RISING_EDGE); + } + + if (signal == Signal.LOW) { + // Signal is now low when it wasn't before - a falling edge + scheduleBindings(BindingType.SCHEDULE_ON_FALLING_EDGE); + scheduleBindings(BindingType.RUN_WHILE_LOW); + cancelBindings(BindingType.RUN_WHILE_HIGH); + toggleBindings(BindingType.TOGGLE_ON_FALLING_EDGE); + } + + m_previousSignal = signal; + } + + private Signal readSignal() { + if (m_condition.getAsBoolean()) { + return Signal.HIGH; + } else { + return Signal.LOW; + } + } + + /** Removes bindings in inactive scopes. Running commands tied to those bindings are canceled. */ + private void clearStaleBindings() { + m_bindings.forEach( + (_bindingType, bindings) -> { + for (var iterator = bindings.iterator(); iterator.hasNext(); ) { + var binding = iterator.next(); + if (binding.scope().active()) { + // This binding's scope is still active, leave it running + continue; + } + + // The scope is no long active. Remove the binding and immediately cancel its command. + iterator.remove(); + m_scheduler.cancel(binding.command()); + } + }); + } + + /** + * Schedules all commands bound to the given binding type. + * + * @param bindingType the binding type to schedule + */ + private void scheduleBindings(BindingType bindingType) { + m_bindings.getOrDefault(bindingType, List.of()).forEach(m_scheduler::schedule); + } + + /** + * Cancels all commands bound to the given binding type. + * + * @param bindingType the binding type to cancel + */ + private void cancelBindings(BindingType bindingType) { + m_bindings + .getOrDefault(bindingType, List.of()) + .forEach(binding -> m_scheduler.cancel(binding.command())); + } + + /** + * Toggles all commands bound to the given binding type. If a command is currently scheduled or + * running, it will be canceled; otherwise, it will be scheduled. + * + * @param bindingType the binding type to cancel + */ + private void toggleBindings(BindingType bindingType) { + m_bindings + .getOrDefault(bindingType, List.of()) + .forEach( + binding -> { + var command = binding.command(); + if (m_scheduler.isScheduledOrRunning(command)) { + m_scheduler.cancel(command); + } else { + m_scheduler.schedule(binding); + } + }); + } + + // package-private for testing + void addBinding(BindingScope scope, BindingType bindingType, Command command) { + // Note: we use a throwable here instead of Thread.currentThread().getStackTrace() for easier + // stack frame filtering and modification. + m_bindings + .computeIfAbsent(bindingType, _k -> new ArrayList<>()) + .add(new Binding(scope, bindingType, command, new Throwable().getStackTrace())); + + if (!m_isBoundToEventLoop) { + m_loop.bind(m_eventLoopCallback); + m_isBoundToEventLoop = true; + } + } + + private void addBinding(BindingType bindingType, Command command) { + BindingScope scope = + switch (m_scheduler.currentCommand()) { + case Command c -> { + // A command is creating a binding - make it scoped to that specific command + yield BindingScope.forCommand(m_scheduler, c); + } + case null -> { + // Creating a binding outside a command - it's global in scope + yield BindingScope.global(); + } + }; + + addBinding(scope, bindingType, command); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/button/CommandGenericHID.java b/commandsv3/src/main/java/org/wpilib/commands3/button/CommandGenericHID.java new file mode 100644 index 0000000000..b3e7a710fc --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/button/CommandGenericHID.java @@ -0,0 +1,343 @@ +// 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.commands3.button; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.wpilibj.DriverStation.POVDirection; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.event.EventLoop; +import java.util.HashMap; +import java.util.Map; +import org.wpilib.commands3.Scheduler; +import org.wpilib.commands3.Trigger; + +/** + * A version of {@link GenericHID} with {@link Trigger} factories for command-based. + * + * @see GenericHID + */ +public class CommandGenericHID { + private final Scheduler m_scheduler; + private final GenericHID m_hid; + private final Map> m_buttonCache = new HashMap<>(); + private final Map, Trigger>> m_axisLessThanCache = + new HashMap<>(); + private final Map, Trigger>> m_axisGreaterThanCache = + new HashMap<>(); + private final Map, Trigger>> + m_axisMagnitudeGreaterThanCache = new HashMap<>(); + private final Map> m_povCache = new HashMap<>(); + + /** + * Construct an instance of a device. + * + * @param scheduler The scheduler that should execute the triggered commands. + * @param port The port index on the Driver Station that the device is plugged into. + */ + public CommandGenericHID(Scheduler scheduler, int port) { + m_scheduler = scheduler; + m_hid = new GenericHID(port); + } + + /** + * Construct an instance of a device. + * + * @param port The port index on the Driver Station that the device is plugged into. + */ + public CommandGenericHID(int port) { + this(Scheduler.getDefault(), port); + } + + /** + * Get the underlying GenericHID object. + * + * @return the wrapped GenericHID object + */ + public GenericHID getHID() { + return m_hid; + } + + /** + * Constructs an event instance around this button's digital signal. + * + * @param button the button index + * @return an event instance representing the button's digital signal attached to the {@link + * Scheduler#getDefaultEventLoop() default scheduler button loop}. + * @see #button(int, EventLoop) + */ + public Trigger button(int button) { + return button(button, m_scheduler.getDefaultEventLoop()); + } + + /** + * Constructs an event instance around this button's digital signal. + * + * @param button the button index + * @param loop the event loop instance to attach the event to. + * @return an event instance representing the button's digital signal attached to the given loop. + */ + public Trigger button(int button, EventLoop loop) { + var cache = m_buttonCache.computeIfAbsent(loop, k -> new HashMap<>()); + return cache.computeIfAbsent( + button, k -> new Trigger(m_scheduler, loop, () -> m_hid.getRawButton(k))); + } + + /** + * Constructs a Trigger instance based around this angle of the default (index 0) POV on the HID, + * attached to {@link Scheduler#getDefaultEventLoop() the default command scheduler button loop}. + * + * @param angle POV angle + * @return a Trigger instance based around this angle of a POV on the HID. + */ + public Trigger pov(POVDirection angle) { + return pov(0, angle, m_scheduler.getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance based around this angle of a POV on the HID. + * + * @param pov index of the POV to read (starting at 0). Defaults to 0. + * @param angle POV angle + * @param loop the event loop instance to attach the event to. Defaults to {@link + * Scheduler#getDefaultEventLoop() the default command scheduler button loop}. + * @return a Trigger instance based around this angle of a POV on the HID. + */ + public Trigger pov(int pov, POVDirection angle, EventLoop loop) { + var cache = m_povCache.computeIfAbsent(loop, k -> new HashMap<>()); + // angle.value is a 4 bit bitfield + return cache.computeIfAbsent( + pov * 16 + angle.value, + k -> new Trigger(m_scheduler, loop, () -> m_hid.getPOV(pov) == angle)); + } + + /** + * Constructs a Trigger instance based around the 0 degree angle (up) of the default (index 0) POV + * on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command scheduler + * button loop}. + * + * @return a Trigger instance based around the 0 degree angle of a POV on the HID. + */ + public Trigger povUp() { + return pov(POVDirection.Up); + } + + /** + * Constructs a Trigger instance based around the 45 degree angle (right up) of the default (index + * 0) POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command + * scheduler button loop}. + * + * @return a Trigger instance based around the 45 degree angle of a POV on the HID. + */ + public Trigger povUpRight() { + return pov(POVDirection.UpRight); + } + + /** + * Constructs a Trigger instance based around the 90 degree angle (right) of the default (index 0) + * POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command + * scheduler button loop}. + * + * @return a Trigger instance based around the 90 degree angle of a POV on the HID. + */ + public Trigger povRight() { + return pov(POVDirection.Right); + } + + /** + * Constructs a Trigger instance based around the 135 degree angle (right down) of the default + * (index 0) POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default + * command scheduler button loop}. + * + * @return a Trigger instance based around the 135 degree angle of a POV on the HID. + */ + public Trigger povDownRight() { + return pov(POVDirection.DownRight); + } + + /** + * Constructs a Trigger instance based around the 180 degree angle (down) of the default (index 0) + * POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command + * scheduler button loop}. + * + * @return a Trigger instance based around the 180 degree angle of a POV on the HID. + */ + public Trigger povDown() { + return pov(POVDirection.Down); + } + + /** + * Constructs a Trigger instance based around the 225 degree angle (down left) of the default + * (index 0) POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default + * command scheduler button loop}. + * + * @return a Trigger instance based around the 225 degree angle of a POV on the HID. + */ + public Trigger povDownLeft() { + return pov(POVDirection.DownLeft); + } + + /** + * Constructs a Trigger instance based around the 270 degree angle (left) of the default (index 0) + * POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command + * scheduler button loop}. + * + * @return a Trigger instance based around the 270 degree angle of a POV on the HID. + */ + public Trigger povLeft() { + return pov(POVDirection.Left); + } + + /** + * Constructs a Trigger instance based around the 315 degree angle (left up) of the default (index + * 0) POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default command + * scheduler button loop}. + * + * @return a Trigger instance based around the 315 degree angle of a POV on the HID. + */ + public Trigger povUpLeft() { + return pov(POVDirection.UpLeft); + } + + /** + * Constructs a Trigger instance based around the center (not pressed) position of the default + * (index 0) POV on the HID, attached to {@link Scheduler#getDefaultEventLoop() the default + * command scheduler button loop}. + * + * @return a Trigger instance based around the center position of a POV on the HID. + */ + public Trigger povCenter() { + return pov(POVDirection.Center); + } + + /** + * Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, + * attached to {@link Scheduler#getDefaultEventLoop() the default command scheduler button loop}. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value below which this trigger should return true. + * @return a Trigger instance that is true when the axis value is less than the provided + * threshold. + */ + public Trigger axisLessThan(int axis, double threshold) { + return axisLessThan(axis, threshold, m_scheduler.getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, + * attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value below which this trigger should return true. + * @param loop the event loop instance to attach the trigger to + * @return a Trigger instance that is true when the axis value is less than the provided + * threshold. + */ + public Trigger axisLessThan(int axis, double threshold, EventLoop loop) { + var cache = m_axisLessThanCache.computeIfAbsent(loop, k -> new HashMap<>()); + return cache.computeIfAbsent( + Pair.of(axis, threshold), + k -> new Trigger(m_scheduler, loop, () -> getRawAxis(axis) < threshold)); + } + + /** + * Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, + * attached to {@link Scheduler#getDefaultEventLoop() the default command scheduler button loop}. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value above which this trigger should return true. + * @return a Trigger instance that is true when the axis value is greater than the provided + * threshold. + */ + public Trigger axisGreaterThan(int axis, double threshold) { + return axisGreaterThan(axis, threshold, m_scheduler.getDefaultEventLoop()); + } + + /** + * Constructs a Trigger instance that is true when the axis value is greater than {@code + * threshold}, attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value above which this trigger should return true. + * @param loop the event loop instance to attach the trigger to. + * @return a Trigger instance that is true when the axis value is greater than the provided + * threshold. + */ + public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { + var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); + return cache.computeIfAbsent( + Pair.of(axis, threshold), + k -> new Trigger(m_scheduler, loop, () -> getRawAxis(axis) > threshold)); + } + + /** + * Constructs a Trigger instance that is true when the axis magnitude value is greater than {@code + * threshold}, attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value above which this trigger should return true. + * @param loop the event loop instance to attach the trigger to. + * @return a Trigger instance that is true when the axis magnitude value is greater than the + * provided threshold. + */ + public Trigger axisMagnitudeGreaterThan(int axis, double threshold, EventLoop loop) { + var cache = m_axisMagnitudeGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); + return cache.computeIfAbsent( + Pair.of(axis, threshold), + k -> new Trigger(m_scheduler, loop, () -> Math.abs(getRawAxis(axis)) > threshold)); + } + + /** + * Constructs a Trigger instance that is true when the axis magnitude value is greater than {@code + * threshold}, attached to {@link Scheduler#getDefaultEventLoop() the default command scheduler + * button loop}. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value above which this trigger should return true. + * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). + */ + public Trigger axisMagnitudeGreaterThan(int axis, double threshold) { + return axisMagnitudeGreaterThan(axis, threshold, m_scheduler.getDefaultEventLoop()); + } + + /** + * Get the value of the axis. + * + * @param axis The axis to read, starting at 0. + * @return The value of the axis. + */ + public double getRawAxis(int axis) { + return m_hid.getRawAxis(axis); + } + + /** + * Set the rumble output for the HID. The DS currently supports 2 rumble values, left rumble and + * right rumble. + * + * @param type Which rumble value to set + * @param value The normalized value (0 to 1) to set the rumble to + */ + public void setRumble(GenericHID.RumbleType type, double value) { + m_hid.setRumble(type, value); + } + + /** + * Get if the HID is connected. + * + * @return true if the HID is connected + */ + public boolean isConnected() { + return m_hid.isConnected(); + } + + /** + * Gets the scheduler that should execute the triggered commands. This scheduler is set in the + * constructor, defaulting to {@link Scheduler#getDefault()} if one was not provided. + * + * @return the scheduler that should execute the triggered commands + */ + protected final Scheduler getScheduler() { + return m_scheduler; + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/button/CommandJoystick.java b/commandsv3/src/main/java/org/wpilib/commands3/button/CommandJoystick.java new file mode 100644 index 0000000000..cc7bf9a335 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/button/CommandJoystick.java @@ -0,0 +1,275 @@ +// 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.commands3.button; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.event.EventLoop; +import org.wpilib.commands3.Scheduler; +import org.wpilib.commands3.Trigger; + +/** + * A version of {@link Joystick} with {@link Trigger} factories for command-based. + * + * @see Joystick + */ +public class CommandJoystick extends CommandGenericHID { + private final Joystick m_hid; + + /** + * Construct an instance of a controller. + * + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public CommandJoystick(int port) { + super(port); + m_hid = new Joystick(port); + } + + /** + * Construct an instance of a controller. + * + * @param scheduler The scheduler that should execute the triggered commands. + * @param port The port index on the Driver Station that the controller is plugged into. + */ + public CommandJoystick(Scheduler scheduler, int port) { + super(scheduler, port); + m_hid = new Joystick(port); + } + + /** + * Get the underlying GenericHID object. + * + * @return the wrapped GenericHID object + */ + @Override + public Joystick getHID() { + return m_hid; + } + + /** + * Constructs an event instance around the trigger button's digital signal. + * + * @return an event instance representing the trigger button's digital signal attached to the + * {@link Scheduler#getDefaultEventLoop() default scheduler button loop}. + * @see #trigger(EventLoop) + */ + public Trigger trigger() { + return trigger(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs an event instance around the trigger button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return an event instance representing the trigger button's digital signal attached to the + * given loop. + */ + public Trigger trigger(EventLoop loop) { + return button(Joystick.ButtonType.kTrigger.value, loop); + } + + /** + * Constructs an event instance around the top button's digital signal. + * + * @return an event instance representing the top button's digital signal attached to the {@link + * Scheduler#getDefaultEventLoop() default scheduler button loop}. + * @see #top(EventLoop) + */ + public Trigger top() { + return top(getScheduler().getDefaultEventLoop()); + } + + /** + * Constructs an event instance around the top button's digital signal. + * + * @param loop the event loop instance to attach the event to. + * @return an event instance representing the top button's digital signal attached to the given + * loop. + */ + public Trigger top(EventLoop loop) { + return button(Joystick.ButtonType.kTop.value, loop); + } + + /** + * Set the channel associated with the X axis. + * + * @param channel The channel to set the axis to. + */ + public void setXChannel(int channel) { + m_hid.setXChannel(channel); + } + + /** + * Set the channel associated with the Y axis. + * + * @param channel The channel to set the axis to. + */ + public void setYChannel(int channel) { + m_hid.setYChannel(channel); + } + + /** + * Set the channel associated with the Z axis. + * + * @param channel The channel to set the axis to. + */ + public void setZChannel(int channel) { + m_hid.setZChannel(channel); + } + + /** + * Set the channel associated with the throttle axis. + * + * @param channel The channel to set the axis to. + */ + public void setThrottleChannel(int channel) { + m_hid.setThrottleChannel(channel); + } + + /** + * Set the channel associated with the twist axis. + * + * @param channel The channel to set the axis to. + */ + public void setTwistChannel(int channel) { + m_hid.setTwistChannel(channel); + } + + /** + * Get the channel currently associated with the X axis. + * + * @return The channel for the axis. + */ + public int getXChannel() { + return m_hid.getXChannel(); + } + + /** + * Get the channel currently associated with the Y axis. + * + * @return The channel for the axis. + */ + public int getYChannel() { + return m_hid.getYChannel(); + } + + /** + * Get the channel currently associated with the Z axis. + * + * @return The channel for the axis. + */ + public int getZChannel() { + return m_hid.getZChannel(); + } + + /** + * Get the channel currently associated with the twist axis. + * + * @return The channel for the axis. + */ + public int getTwistChannel() { + return m_hid.getTwistChannel(); + } + + /** + * Get the channel currently associated with the throttle axis. + * + * @return The channel for the axis. + */ + public int getThrottleChannel() { + return m_hid.getThrottleChannel(); + } + + /** + * Get the x position of the HID. + * + *

This depends on the mapping of the joystick connected to the current port. On most + * joysticks, positive is to the right. + * + * @return the x position + */ + public double getX() { + return m_hid.getX(); + } + + /** + * Get the y position of the HID. + * + *

This depends on the mapping of the joystick connected to the current port. On most + * joysticks, positive is to the back. + * + * @return the y position + */ + public double getY() { + return m_hid.getY(); + } + + /** + * Get the z position of the HID. + * + * @return the z position + */ + public double getZ() { + return m_hid.getZ(); + } + + /** + * Get the twist value of the current joystick. This depends on the mapping of the joystick + * connected to the current port. + * + * @return The Twist value of the joystick. + */ + public double getTwist() { + return m_hid.getTwist(); + } + + /** + * Get the throttle value of the current joystick. This depends on the mapping of the joystick + * connected to the current port. + * + * @return The Throttle value of the joystick. + */ + public double getThrottle() { + return m_hid.getThrottle(); + } + + /** + * Get the magnitude of the vector formed by the joystick's current position relative to its + * origin. + * + * @return The magnitude of the direction vector + */ + public double getMagnitude() { + return m_hid.getMagnitude(); + } + + /** + * Get the direction of the vector formed by the joystick and its origin in radians. 0 is forward + * and clockwise is positive. (Straight right is π/2.) + * + * @return The direction of the vector in radians + */ + public double getDirectionRadians() { + // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#joystick-and-controller-coordinate-system + // A positive rotation around the X axis moves the joystick right, and a + // positive rotation around the Y axis moves the joystick backward. When + // treating them as translations, 0 radians is measured from the right + // direction, and angle increases clockwise. + // + // It's rotated 90 degrees CCW (y is negated and the arguments are reversed) + // so that 0 radians is forward. + return m_hid.getDirectionRadians(); + } + + /** + * Get the direction of the vector formed by the joystick and its origin in degrees. 0 is forward + * and clockwise is positive. (Straight right is 90.) + * + * @return The direction of the vector in degrees + */ + public double getDirectionDegrees() { + return m_hid.getDirectionDegrees(); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/button/InternalButton.java b/commandsv3/src/main/java/org/wpilib/commands3/button/InternalButton.java new file mode 100644 index 0000000000..6bedec6d7f --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/button/InternalButton.java @@ -0,0 +1,61 @@ +// 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.commands3.button; + +import java.util.concurrent.atomic.AtomicBoolean; +import org.wpilib.commands3.Trigger; + +/** + * This class is intended to be used within a program. The programmer can manually set its value. + * Also includes a setting for whether it should invert its value. + */ +public class InternalButton extends Trigger { + // need to be references, so they can be mutated after being captured in the constructor. + private final AtomicBoolean m_pressed; + private final AtomicBoolean m_inverted; + + /** Creates an InternalButton that is not inverted. */ + public InternalButton() { + this(false); + } + + /** + * Creates an InternalButton which is inverted depending on the input. + * + * @param inverted if false, then this button is pressed when set to true, otherwise it is pressed + * when set to false. + */ + public InternalButton(boolean inverted) { + this(new AtomicBoolean(), new AtomicBoolean(inverted)); + } + + /* + * Mock constructor so the AtomicBoolean objects can be constructed before the super + * constructor invocation. + */ + private InternalButton(AtomicBoolean state, AtomicBoolean inverted) { + super(() -> state.get() != inverted.get()); + m_pressed = state; + m_inverted = inverted; + } + + /** + * Sets whether to invert button state. + * + * @param inverted Whether button state should be inverted. + */ + public void setInverted(boolean inverted) { + m_inverted.set(inverted); + } + + /** + * Sets whether button is pressed. + * + * @param pressed Whether button is pressed. + */ + public void setPressed(boolean pressed) { + m_pressed.set(pressed); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/button/JoystickButton.java b/commandsv3/src/main/java/org/wpilib/commands3/button/JoystickButton.java new file mode 100644 index 0000000000..5c968a939f --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/button/JoystickButton.java @@ -0,0 +1,24 @@ +// 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.commands3.button; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.wpilibj.GenericHID; +import org.wpilib.commands3.Trigger; + +/** A {@link Trigger} that gets its state from a {@link GenericHID}. */ +public class JoystickButton extends Trigger { + /** + * Creates a joystick button for triggering commands. + * + * @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick, etc) + * @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) } + */ + public JoystickButton(GenericHID joystick, int buttonNumber) { + super(() -> joystick.getRawButton(buttonNumber)); + requireNonNullParam(joystick, "joystick", "JoystickButton"); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/button/NetworkButton.java b/commandsv3/src/main/java/org/wpilib/commands3/button/NetworkButton.java new file mode 100644 index 0000000000..330160339e --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/button/NetworkButton.java @@ -0,0 +1,66 @@ +// 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.commands3.button; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.BooleanTopic; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import org.wpilib.commands3.Trigger; + +/** A {@link Trigger} that uses a {@link NetworkTable} boolean field. */ +public class NetworkButton extends Trigger { + /** + * Creates a NetworkButton that commands can be bound to. + * + * @param topic The boolean topic that contains the value. + */ + public NetworkButton(BooleanTopic topic) { + this(topic.subscribe(false)); + } + + /** + * Creates a NetworkButton that commands can be bound to. + * + * @param sub The boolean subscriber that provides the value. + */ + public NetworkButton(BooleanSubscriber sub) { + super(() -> sub.getTopic().getInstance().isConnected() && sub.get()); + requireNonNullParam(sub, "sub", "NetworkButton"); + } + + /** + * Creates a NetworkButton that commands can be bound to. + * + * @param table The table where the networktable value is located. + * @param field The field that is the value. + */ + public NetworkButton(NetworkTable table, String field) { + this(table.getBooleanTopic(field)); + } + + /** + * Creates a NetworkButton that commands can be bound to. + * + * @param table The table where the networktable value is located. + * @param field The field that is the value. + */ + public NetworkButton(String table, String field) { + this(NetworkTableInstance.getDefault(), table, field); + } + + /** + * Creates a NetworkButton that commands can be bound to. + * + * @param inst The NetworkTable instance to use + * @param table The table where the networktable value is located. + * @param field The field that is the value. + */ + public NetworkButton(NetworkTableInstance inst, String table, String field) { + this(inst.getTable(table), field); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/button/POVButton.java b/commandsv3/src/main/java/org/wpilib/commands3/button/POVButton.java new file mode 100644 index 0000000000..154ddbc3f9 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/button/POVButton.java @@ -0,0 +1,36 @@ +// 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.commands3.button; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.wpilibj.DriverStation.POVDirection; +import edu.wpi.first.wpilibj.GenericHID; +import org.wpilib.commands3.Trigger; + +/** A {@link Trigger} that gets its state from a POV on a {@link GenericHID}. */ +public class POVButton extends Trigger { + /** + * Creates a POV button for triggering commands. + * + * @param joystick The GenericHID object that has the POV + * @param angle The desired angle + * @param povNumber The POV number (see {@link GenericHID#getPOV(int)}) + */ + public POVButton(GenericHID joystick, POVDirection angle, int povNumber) { + super(() -> joystick.getPOV(povNumber) == angle); + requireNonNullParam(joystick, "joystick", "POVButton"); + } + + /** + * Creates a POV button for triggering commands. By default, acts on POV 0 + * + * @param joystick The GenericHID object that has the POV + * @param angle The desired angle + */ + public POVButton(GenericHID joystick, POVDirection angle) { + this(joystick, angle, 0); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/button/RobotModeTriggers.java b/commandsv3/src/main/java/org/wpilib/commands3/button/RobotModeTriggers.java new file mode 100644 index 0000000000..6a84df1ff0 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/button/RobotModeTriggers.java @@ -0,0 +1,53 @@ +// 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.commands3.button; + +import edu.wpi.first.wpilibj.DriverStation; +import org.wpilib.commands3.Trigger; + +/** + * A class containing static {@link Trigger} factories for running callbacks when the robot mode + * changes. + */ +public final class RobotModeTriggers { + // Utility class + private RobotModeTriggers() {} + + /** + * Returns a trigger that is true when the robot is enabled in autonomous mode. + * + * @return A trigger that is true when the robot is enabled in autonomous mode. + */ + public static Trigger autonomous() { + return new Trigger(DriverStation::isAutonomousEnabled); + } + + /** + * Returns a trigger that is true when the robot is enabled in teleop mode. + * + * @return A trigger that is true when the robot is enabled in teleop mode. + */ + public static Trigger teleop() { + return new Trigger(DriverStation::isTeleopEnabled); + } + + /** + * Returns a trigger that is true when the robot is disabled. + * + * @return A trigger that is true when the robot is disabled. + */ + public static Trigger disabled() { + return new Trigger(DriverStation::isDisabled); + } + + /** + * Returns a trigger that is true when the robot is enabled in test mode. + * + * @return A trigger that is true when the robot is enabled in test mode. + */ + public static Trigger test() { + return new Trigger(DriverStation::isTestEnabled); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/proto/CommandProto.java b/commandsv3/src/main/java/org/wpilib/commands3/proto/CommandProto.java new file mode 100644 index 0000000000..f8fe21f771 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/proto/CommandProto.java @@ -0,0 +1,65 @@ +// 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.commands3.proto; + +import edu.wpi.first.util.protobuf.Protobuf; +import org.wpilib.commands3.Command; +import org.wpilib.commands3.Mechanism; +import org.wpilib.commands3.Scheduler; +import org.wpilib.commands3.proto.ProtobufCommands.ProtobufCommand; +import us.hebi.quickbuf.Descriptors; + +/** Protobuf serde for running commands. */ +public class CommandProto implements Protobuf { + private final Scheduler m_scheduler; + + public CommandProto(Scheduler scheduler) { + m_scheduler = scheduler; + } + + @Override + public Class getTypeClass() { + return Command.class; + } + + @Override + public Descriptors.Descriptor getDescriptor() { + return ProtobufCommand.getDescriptor(); + } + + @Override + public ProtobufCommand createMessage() { + return ProtobufCommand.newInstance(); + } + + @Override + public Command unpack(ProtobufCommand msg) { + // Not possible. The command behavior is what really matters, and it cannot be serialized + throw new UnsupportedOperationException("Deserialization not supported"); + } + + @Override + public void pack(ProtobufCommand msg, Command command) { + msg.clear(); + + msg.setId(m_scheduler.runId(command)); + Command parent = m_scheduler.getParentOf(command); + if (parent != null) { + msg.setParentId(m_scheduler.runId(parent)); + } + msg.setName(command.name()); + msg.setPriority(command.priority()); + + Protobuf.packArray( + msg.getMutableRequirements(), + command.requirements().toArray(new Mechanism[0]), + new MechanismProto()); + + if (m_scheduler.isRunning(command)) { + msg.setLastTimeMs(m_scheduler.lastCommandRuntimeMs(command)); + msg.setTotalTimeMs(m_scheduler.totalRuntimeMs(command)); + } + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/proto/MechanismProto.java b/commandsv3/src/main/java/org/wpilib/commands3/proto/MechanismProto.java new file mode 100644 index 0000000000..5e469d2d54 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/proto/MechanismProto.java @@ -0,0 +1,38 @@ +// 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.commands3.proto; + +import edu.wpi.first.util.protobuf.Protobuf; +import org.wpilib.commands3.Mechanism; +import org.wpilib.commands3.proto.ProtobufCommands.ProtobufMechanism; +import us.hebi.quickbuf.Descriptors; + +public class MechanismProto implements Protobuf { + @Override + public Class getTypeClass() { + return Mechanism.class; + } + + @Override + public Descriptors.Descriptor getDescriptor() { + return ProtobufMechanism.getDescriptor(); + } + + @Override + public ProtobufMechanism createMessage() { + return ProtobufMechanism.newInstance(); + } + + @Override + public Mechanism unpack(ProtobufMechanism msg) { + throw new UnsupportedOperationException("Deserialization not supported"); + } + + @Override + public void pack(ProtobufMechanism msg, Mechanism value) { + msg.clear(); + msg.setName(value.getName()); + } +} diff --git a/commandsv3/src/main/java/org/wpilib/commands3/proto/SchedulerProto.java b/commandsv3/src/main/java/org/wpilib/commands3/proto/SchedulerProto.java new file mode 100644 index 0000000000..bc5ad124c4 --- /dev/null +++ b/commandsv3/src/main/java/org/wpilib/commands3/proto/SchedulerProto.java @@ -0,0 +1,58 @@ +// 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.commands3.proto; + +import edu.wpi.first.util.protobuf.Protobuf; +import org.wpilib.commands3.Command; +import org.wpilib.commands3.Scheduler; +import org.wpilib.commands3.proto.ProtobufCommands.ProtobufScheduler; +import us.hebi.quickbuf.Descriptors; + +/** + * Serializes a {@link Scheduler} to a protobuf message. Deserialization is not supported. A + * serialized message will include information about commands that are currently running or + * scheduled (but not yet started), as well as how long the most recent call to {@link + * Scheduler#run()} took to execute. + */ +public class SchedulerProto implements Protobuf { + @Override + public Class getTypeClass() { + return Scheduler.class; + } + + @Override + public Descriptors.Descriptor getDescriptor() { + return ProtobufScheduler.getDescriptor(); + } + + @Override + public ProtobufScheduler createMessage() { + return ProtobufScheduler.newInstance(); + } + + @Override + public Scheduler unpack(ProtobufScheduler msg) { + throw new UnsupportedOperationException("Deserialization not supported"); + } + + @Override + public void pack(ProtobufScheduler msg, Scheduler scheduler) { + msg.clear(); + + var commandProto = new CommandProto(scheduler); + + Protobuf.packArray( + msg.getMutableQueuedCommands(), + scheduler.getQueuedCommands().toArray(new Command[0]), + commandProto); + + Protobuf.packArray( + msg.getMutableRunningCommands(), + scheduler.getRunningCommands().toArray(new Command[0]), + commandProto); + + msg.setLastTimeMs(scheduler.lastRuntimeMs()); + } +} diff --git a/commandsv3/src/main/proto/protobuf_commands.proto b/commandsv3/src/main/proto/protobuf_commands.proto new file mode 100644 index 0000000000..beaba6b8c0 --- /dev/null +++ b/commandsv3/src/main/proto/protobuf_commands.proto @@ -0,0 +1,57 @@ +syntax = "proto3"; + +package wpi.proto; + +option java_package = "org.wpilib.commands3.proto"; + +/* +Or use the generate_files.py script: + +# macOS +allwpilib $ ./commandsv3/generate_files.py --protoc=protoc-quickbuf + +# Linux +allwpilib $ ./commandsv3/generate_files.py --quickbuf_plugin protoc-gen-quickbuf-1.3.3-linux-x86_64.exe + */ + +message ProtobufMechanism { + string name = 1; +} + +message ProtobufCommand { + // A unique ID for the command. + // Different invocations of the same command object have different IDs. + uint32 id = 1; + + // The ID of the parent command. + // Not included in the message for top-level commands. + optional uint32 parent_id = 2; + + // The name of the command. + string name = 3; + + // The priority level of the command. + int32 priority = 4; + + // The mechanisms required by the command. + repeated ProtobufMechanism requirements = 5; + + // How much time the command took to execute in its most recent run. + // Only included in a message for an actively running command. + optional double last_time_ms = 6; + + // How long the command has taken to run, in aggregate. + // Only included in a message for an actively running command. + optional double total_time_ms = 7; +} + +message ProtobufScheduler { + // Note: commands are generally queued by triggers, which occurs immediately before they are + // promoted and start running. Entries will only appear here when serializing a scheduler + // _after_ manually scheduling a command but _before_ calling scheduler.run() + repeated ProtobufCommand queued_commands = 1; + repeated ProtobufCommand running_commands = 2; + + // How much time the scheduler took in its last `run()` invocation. + double last_time_ms = 3; +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/CommandTestBase.java b/commandsv3/src/test/java/org/wpilib/commands3/CommandTestBase.java new file mode 100644 index 0000000000..0423924397 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/CommandTestBase.java @@ -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. + +package org.wpilib.commands3; + +import edu.wpi.first.wpilibj.RobotController; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.BeforeEach; + +class CommandTestBase { + protected Scheduler m_scheduler; + protected List m_events; + + @BeforeEach + void initScheduler() { + RobotController.setTimeSource(() -> System.nanoTime() / 1000L); + m_scheduler = Scheduler.createIndependentScheduler(); + m_events = new ArrayList<>(); + m_scheduler.addEventListener(m_events::add); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/ConflictDetectorTest.java b/commandsv3/src/test/java/org/wpilib/commands3/ConflictDetectorTest.java new file mode 100644 index 0000000000..ae1e5ef862 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/ConflictDetectorTest.java @@ -0,0 +1,70 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.wpilib.commands3.ConflictDetector.findAllConflicts; +import static org.wpilib.commands3.ConflictDetector.throwIfConflicts; + +import java.util.List; +import java.util.Set; +import org.junit.jupiter.api.Test; +import org.wpilib.commands3.ConflictDetector.Conflict; + +class ConflictDetectorTest extends CommandTestBase { + @Test + void emptyInputHasNoConflicts() { + var conflicts = findAllConflicts(List.of()); + assertEquals(0, conflicts.size()); + } + + @Test + void singleInputHasNoConflicts() { + var mech = new Mechanism("Mech", m_scheduler); + var command = Command.requiring(mech).executing(Coroutine::park).named("Command"); + var conflicts = findAllConflicts(List.of(command)); + assertEquals(0, conflicts.size()); + } + + @Test + void commandDoesNotConflictWithSelf() { + var mech = new Mechanism("Mech", m_scheduler); + var command = Command.requiring(mech).executing(Coroutine::park).named("Command"); + var conflicts = findAllConflicts(List.of(command, command)); + assertEquals(0, conflicts.size()); + } + + @Test + void detectManyConflicts() { + var mech1 = new Mechanism("Mech 1", m_scheduler); + var mech2 = new Mechanism("Mech 2", m_scheduler); + + var command1 = Command.requiring(mech1, mech2).executing(Coroutine::park).named("Command1"); + var command2 = Command.requiring(mech1).executing(Coroutine::park).named("Command2"); + var command3 = Command.requiring(mech2).executing(Coroutine::park).named("Command3"); + var command4 = Command.requiring(mech2, mech1).executing(Coroutine::park).named("Command4"); + var allCommands = List.of(command1, command2, command3, command4); + + var conflicts = findAllConflicts(allCommands); + assertEquals(5, conflicts.size(), "Five conflicting pairs should have been found"); + assertEquals(new Conflict(command1, command2, Set.of(mech1)), conflicts.get(0)); + assertEquals(new Conflict(command1, command3, Set.of(mech2)), conflicts.get(1)); + assertEquals(new Conflict(command1, command4, Set.of(mech1, mech2)), conflicts.get(2)); + assertEquals(new Conflict(command2, command4, Set.of(mech1)), conflicts.get(3)); + assertEquals(new Conflict(command3, command4, Set.of(mech2)), conflicts.get(4)); + + // error messaging + var error = assertThrows(IllegalArgumentException.class, () -> throwIfConflicts(allCommands)); + assertEquals( + "Commands running in parallel cannot share requirements: " + + "Command1 and Command2 both require Mech 1; " + + "Command1 and Command3 both require Mech 2; " + + "Command1 and Command4 both require Mech 1, Mech 2; " + + "Command2 and Command4 both require Mech 1; " + + "Command3 and Command4 both require Mech 2", + error.getMessage()); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/CoroutineTest.java b/commandsv3/src/test/java/org/wpilib/commands3/CoroutineTest.java new file mode 100644 index 0000000000..b6779f8a9a --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/CoroutineTest.java @@ -0,0 +1,174 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.Set; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.concurrent.atomic.AtomicReference; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import org.junit.jupiter.api.Test; + +class CoroutineTest extends CommandTestBase { + @Test + void forkMany() { + var a = new NullCommand(); + var b = new NullCommand(); + var c = new NullCommand(); + + var all = + Command.noRequirements() + .executing( + co -> { + co.fork(a, b, c); + co.park(); + }) + .named("Fork Many"); + + m_scheduler.schedule(all); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(a)); + assertTrue(m_scheduler.isRunning(b)); + assertTrue(m_scheduler.isRunning(c)); + } + + @Test + void yieldInSynchronizedBlock() { + Object mutex = new Object(); + AtomicInteger i = new AtomicInteger(0); + + var yieldInSynchronized = + Command.noRequirements() + .executing( + co -> { + while (true) { + synchronized (mutex) { + i.incrementAndGet(); + co.yield(); + } + } + }) + .named("Yield In Synchronized Block"); + + m_scheduler.schedule(yieldInSynchronized); + + var error = assertThrows(IllegalStateException.class, m_scheduler::run); + assertEquals( + "Coroutine.yield() cannot be called inside a synchronized block or method. " + + "Consider using a Lock instead of synchronized, " + + "or rewrite your code to avoid locks and mutexes altogether.", + error.getMessage()); + } + + @Test + void yieldInLockBody() { + Lock lock = new ReentrantLock(); + AtomicInteger i = new AtomicInteger(0); + + var yieldInLock = + Command.noRequirements() + .executing( + co -> { + while (true) { + lock.lock(); + try { + i.incrementAndGet(); + co.yield(); + } finally { + lock.unlock(); + } + } + }) + .named("Increment In Lock Block"); + + m_scheduler.schedule(yieldInLock); + m_scheduler.run(); + assertEquals(1, i.get()); + } + + @Test + void coroutineEscapingCommand() { + AtomicReference escapeeCallback = new AtomicReference<>(); + + var badCommand = + Command.noRequirements() + .executing( + co -> { + escapeeCallback.set(co::yield); + }) + .named("Bad Command"); + + m_scheduler.schedule(badCommand); + m_scheduler.run(); + + var error = assertThrows(IllegalStateException.class, escapeeCallback.get()::run); + assertEquals("Coroutines can only be used by the command bound to them", error.getMessage()); + } + + @Test + void usingParentCoroutineInChildThrows() { + var parent = + Command.noRequirements() + .executing( + parentCoroutine -> { + parentCoroutine.await( + Command.noRequirements() + .executing( + childCoroutine -> { + parentCoroutine.yield(); + }) + .named("Child")); + }) + .named("Parent"); + + m_scheduler.schedule(parent); + var error = assertThrows(IllegalStateException.class, m_scheduler::run); + assertEquals("Coroutines can only be used by the command bound to them", error.getMessage()); + } + + @Test + void awaitAnyCleansUp() { + AtomicBoolean firstRan = new AtomicBoolean(false); + AtomicBoolean secondRan = new AtomicBoolean(false); + AtomicBoolean ranAfterAwait = new AtomicBoolean(false); + + var firstInner = Command.noRequirements().executing(c2 -> firstRan.set(true)).named("First"); + var secondInner = + Command.noRequirements() + .executing( + c2 -> { + secondRan.set(true); + c2.park(); + }) + .named("Second"); + + var outer = + Command.noRequirements() + .executing( + co -> { + co.awaitAny(firstInner, secondInner); + + ranAfterAwait.set(true); + co.park(); // prevent exiting + }) + .named("Command"); + + m_scheduler.schedule(outer); + m_scheduler.run(); + + // Everything should have run... + assertTrue(firstRan.get()); + assertTrue(secondRan.get()); + assertTrue(ranAfterAwait.get()); + + // But only the outer command should still be running; secondInner should have been canceled + assertEquals(Set.of(outer), m_scheduler.getRunningCommands()); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/NullCommand.java b/commandsv3/src/test/java/org/wpilib/commands3/NullCommand.java new file mode 100644 index 0000000000..e66060c7d4 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/NullCommand.java @@ -0,0 +1,24 @@ +// 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.commands3; + +import java.util.Set; + +class NullCommand implements Command { + @Override + public void run(Coroutine coroutine) { + coroutine.park(); + } + + @Override + public String name() { + return "Null Command"; + } + + @Override + public Set requirements() { + return Set.of(); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/ParallelGroupTest.java b/commandsv3/src/test/java/org/wpilib/commands3/ParallelGroupTest.java new file mode 100644 index 0000000000..f89bd647f0 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/ParallelGroupTest.java @@ -0,0 +1,233 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.List; +import java.util.Set; +import java.util.concurrent.atomic.AtomicInteger; +import org.junit.jupiter.api.Test; + +class ParallelGroupTest extends CommandTestBase { + @Test + void parallelAll() { + var r1 = new Mechanism("R1", m_scheduler); + var r2 = new Mechanism("R2", m_scheduler); + + var c1Count = new AtomicInteger(0); + var c2Count = new AtomicInteger(0); + + var c1 = + r1.run( + coroutine -> { + for (int i = 0; i < 5; i++) { + coroutine.yield(); + c1Count.incrementAndGet(); + } + }) + .named("C1"); + var c2 = + r2.run( + coroutine -> { + for (int i = 0; i < 10; i++) { + coroutine.yield(); + c2Count.incrementAndGet(); + } + }) + .named("C2"); + + var parallel = new ParallelGroup("Parallel", List.of(c1, c2), List.of()); + m_scheduler.schedule(parallel); + + // First call to run() should schedule and start the commands + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(parallel)); + assertTrue(m_scheduler.isRunning(c1)); + assertTrue(m_scheduler.isRunning(c2)); + + // Next call to run() should start them + for (int i = 1; i < 5; i++) { + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(c1)); + assertTrue(m_scheduler.isRunning(c2)); + assertEquals(i, c1Count.get()); + assertEquals(i, c2Count.get()); + } + // c1 should finish after 5 iterations; c2 should continue for another 5 + for (int i = 5; i < 10; i++) { + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(c1)); + assertTrue(m_scheduler.isRunning(c2)); + assertEquals(5, c1Count.get()); + assertEquals(i, c2Count.get()); + } + + // one final run() should unschedule the c2 command and end the group + assertTrue(m_scheduler.isRunning(parallel)); + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(c1)); + assertFalse(m_scheduler.isRunning(c2)); + assertFalse(m_scheduler.isRunning(parallel)); + + // and final counts should be 5 and 10 + assertEquals(5, c1Count.get()); + assertEquals(10, c2Count.get()); + } + + @Test + void race() { + var r1 = new Mechanism("R1", m_scheduler); + var r2 = new Mechanism("R2", m_scheduler); + + var c1Count = new AtomicInteger(0); + var c2Count = new AtomicInteger(0); + + var c1 = + r1.run( + coroutine -> { + for (int i = 0; i < 5; i++) { + coroutine.yield(); + c1Count.incrementAndGet(); + } + }) + .named("C1"); + var c2 = + r2.run( + coroutine -> { + for (int i = 0; i < 10; i++) { + coroutine.yield(); + c2Count.incrementAndGet(); + } + }) + .named("C2"); + + var race = new ParallelGroup("Race", List.of(), List.of(c1, c2)); + m_scheduler.schedule(race); + + // First call to run() should schedule the commands + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(race)); + assertTrue(m_scheduler.isRunning(c1)); + assertTrue(m_scheduler.isRunning(c2)); + + for (int i = 1; i < 5; i++) { + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(c1)); + assertTrue(m_scheduler.isRunning(c2)); + assertEquals(i, c1Count.get()); + assertEquals(i, c2Count.get()); + } + m_scheduler.run(); // complete c1 + assertFalse(m_scheduler.isRunning(race)); + assertFalse(m_scheduler.isRunning(c1)); + assertFalse(m_scheduler.isRunning(c2)); + + // and final counts should be 5 and 5 + assertEquals(5, c1Count.get()); + assertEquals(5, c2Count.get()); + } + + @Test + void nested() { + var mechanism = new Mechanism("mechanism", m_scheduler); + + var count = new AtomicInteger(0); + + var command = + mechanism + .run( + coroutine -> { + for (int i = 0; i < 5; i++) { + coroutine.yield(); + count.incrementAndGet(); + } + }) + .named("Command"); + + var inner = new ParallelGroup("Inner", Set.of(command), Set.of()); + var outer = new ParallelGroup("Outer", Set.of(), Set.of(inner)); + + // Scheduling: Outer group should be on deck + m_scheduler.schedule(outer); + assertTrue(m_scheduler.isScheduled(outer)); + assertFalse(m_scheduler.isScheduledOrRunning(inner)); + assertFalse(m_scheduler.isScheduledOrRunning(command)); + + // First run: Inner group and command should both be scheduled and running + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(outer), "Outer group should be running"); + assertTrue(m_scheduler.isRunning(inner), "Inner group should be running"); + assertTrue(m_scheduler.isRunning(command), "Command should be running"); + assertEquals(0, count.get()); + + // Runs 2 through 5: Outer and inner should both be running while the command runs + for (int i = 1; i < 5; i++) { + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(outer), "Outer group should be running"); + assertTrue(m_scheduler.isRunning(inner), "Inner group should be running"); + assertTrue(m_scheduler.isRunning(command), "Command should be running (" + i + ")"); + assertEquals(i, count.get()); + } + + // Run 6: Command should have completed naturally + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(outer), "Outer group should be running"); + assertFalse(m_scheduler.isRunning(inner), "Inner group should be running"); + assertFalse(m_scheduler.isRunning(command), "Command should have completed"); + } + + @Test + void automaticNameRace() { + var a = Command.noRequirements().executing(coroutine -> {}).named("A"); + var b = Command.noRequirements().executing(coroutine -> {}).named("B"); + var c = Command.noRequirements().executing(coroutine -> {}).named("C"); + + var group = new ParallelGroupBuilder().optional(a, b, c).withAutomaticName(); + assertEquals("(A | B | C)", group.name()); + } + + @Test + void automaticNameAll() { + var a = Command.noRequirements().executing(coroutine -> {}).named("A"); + var b = Command.noRequirements().executing(coroutine -> {}).named("B"); + var c = Command.noRequirements().executing(coroutine -> {}).named("C"); + + var group = new ParallelGroupBuilder().requiring(a, b, c).withAutomaticName(); + assertEquals("(A & B & C)", group.name()); + } + + @Test + void automaticNameDeadline() { + var a = Command.noRequirements().executing(coroutine -> {}).named("A"); + var b = Command.noRequirements().executing(coroutine -> {}).named("B"); + var c = Command.noRequirements().executing(coroutine -> {}).named("C"); + + var group = new ParallelGroupBuilder().requiring(a).optional(b, c).withAutomaticName(); + assertEquals("[(A) * (B | C)]", group.name()); + } + + @Test + void inheritsRequirements() { + var mech1 = new Mechanism("Mech 1", m_scheduler); + var mech2 = new Mechanism("Mech 2", m_scheduler); + var command1 = mech1.run(Coroutine::park).named("Command 1"); + var command2 = mech2.run(Coroutine::park).named("Command 2"); + var group = new ParallelGroup("Group", Set.of(command1, command2), Set.of()); + assertEquals(Set.of(mech1, mech2), group.requirements(), "Requirements were not inherited"); + } + + @Test + void inheritsPriority() { + var mech1 = new Mechanism("Mech 1", m_scheduler); + var mech2 = new Mechanism("Mech 2", m_scheduler); + var command1 = mech1.run(Coroutine::park).withPriority(100).named("Command 1"); + var command2 = mech2.run(Coroutine::park).withPriority(200).named("Command 2"); + var group = new ParallelGroup("Group", Set.of(command1, command2), Set.of()); + assertEquals(200, group.priority(), "Priority was not inherited"); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/PriorityCommand.java b/commandsv3/src/test/java/org/wpilib/commands3/PriorityCommand.java new file mode 100644 index 0000000000..6599b3f861 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/PriorityCommand.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.commands3; + +import java.util.Set; + +record PriorityCommand(int priority, Mechanism... subsystems) implements Command { + @Override + public void run(Coroutine coroutine) { + coroutine.park(); + } + + @Override + public Set requirements() { + return Set.of(subsystems); + } + + @Override + public String name() { + return toString(); + } + + @Override + public String toString() { + return "PriorityCommand[priority=" + priority + ", subsystems=" + Set.of(subsystems) + "]"; + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/SchedulerCancellationTests.java b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerCancellationTests.java new file mode 100644 index 0000000000..90d2dfffef --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerCancellationTests.java @@ -0,0 +1,369 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.fail; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.concurrent.atomic.AtomicReference; +import org.junit.jupiter.api.Test; + +class SchedulerCancellationTests extends CommandTestBase { + @Test + void cancelOnInterruptDoesNotResume() { + var count = new AtomicInteger(0); + + var mechanism = new Mechanism("mechanism", m_scheduler); + + var interrupter = + Command.requiring(mechanism) + .executing(coroutine -> {}) + .withPriority(2) + .named("Interrupter"); + + var canceledCommand = + Command.requiring(mechanism) + .executing( + coroutine -> { + count.set(1); + coroutine.yield(); + count.set(2); + }) + .withPriority(1) + .named("Cancel By Default"); + + m_scheduler.schedule(canceledCommand); + m_scheduler.run(); + + m_scheduler.schedule(interrupter); + m_scheduler.run(); + assertEquals(1, count.get()); // the second "set" call should not have run + } + + @Test + void defaultCommandResumesAfterInterruption() { + var count = new AtomicInteger(0); + + var mechanism = new Mechanism("mechanism", m_scheduler); + var defaultCmd = + mechanism + .run( + coroutine -> { + while (true) { + count.incrementAndGet(); + coroutine.yield(); + } + }) + .withPriority(-1) + .named("Default Command"); + + final var newerCmd = mechanism.run(coroutine -> {}).named("Newer Command"); + mechanism.setDefaultCommand(defaultCmd); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(defaultCmd), "Default command should be running"); + + m_scheduler.schedule(newerCmd); + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(defaultCmd), "Default command should have been interrupted"); + assertEquals(1, count.get(), "Default command should have run once"); + + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(defaultCmd), "Default command should have resumed"); + assertEquals(2, count.get()); + } + + @Test + void cancelsEvictsOnDeck() { + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + m_scheduler.schedule(command); + m_scheduler.cancel(command); + assertFalse(m_scheduler.isScheduledOrRunning(command)); + } + + @Test + void commandCancelingSelf() { + var ranAfterCancel = new AtomicBoolean(false); + var commandRef = new AtomicReference(null); + var command = + Command.noRequirements() + .executing( + co -> { + co.scheduler().cancel(commandRef.get()); + ranAfterCancel.set(true); + }) + .named("Command"); + commandRef.set(command); + m_scheduler.schedule(command); + + var error = assertThrows(IllegalArgumentException.class, () -> m_scheduler.run()); + assertEquals("Command `Command` is mounted and cannot be canceled", error.getMessage()); + assertFalse(ranAfterCancel.get(), "Command should have stopped after encountering an error"); + assertFalse( + m_scheduler.isScheduledOrRunning(command), + "Command should have been removed from the scheduler"); + } + + @Test + void cancelAllEvictsOnDeck() { + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + m_scheduler.schedule(command); + m_scheduler.cancelAll(); + assertFalse(m_scheduler.isScheduledOrRunning(command)); + } + + @Test + void cancelAllCancelsAll() { + var commands = new ArrayList(10); + for (int i = 1; i <= 10; i++) { + commands.add(Command.noRequirements().executing(Coroutine::yield).named("Command " + i)); + } + commands.forEach(m_scheduler::schedule); + m_scheduler.run(); + m_scheduler.cancelAll(); + for (Command command : commands) { + if (m_scheduler.isRunning(command)) { + fail(command.name() + " was not canceled by cancelAll()"); + } + } + } + + @Test + void cancelAllCallsOnCancelHookForRunningCommands() { + AtomicBoolean ranHook = new AtomicBoolean(false); + var command = + Command.noRequirements() + .executing(Coroutine::park) + .whenCanceled(() -> ranHook.set(true)) + .named("Command"); + m_scheduler.schedule(command); + m_scheduler.run(); + m_scheduler.cancelAll(); + assertTrue(ranHook.get(), "onCancel hook was not called"); + } + + @Test + void cancelAllDoesNotCallOnCancelHookForQueuedCommands() { + AtomicBoolean ranHook = new AtomicBoolean(false); + var command = + Command.noRequirements() + .executing(Coroutine::park) + .whenCanceled(() -> ranHook.set(true)) + .named("Command"); + m_scheduler.schedule(command); + // no call to run before cancelAll() + m_scheduler.cancelAll(); + assertFalse(ranHook.get(), "onCancel hook was not called"); + } + + @Test + void cancelAllStartsDefaults() { + var mechanisms = new ArrayList(10); + for (int i = 1; i <= 10; i++) { + mechanisms.add(new Mechanism("System " + i, m_scheduler)); + } + + var command = Command.requiring(mechanisms).executing(Coroutine::yield).named("Big Command"); + + // Scheduling the command should evict the on-deck default commands + m_scheduler.schedule(command); + + // Then running should get it into the set of running commands + m_scheduler.run(); + + // Canceling should clear out the set of running commands + m_scheduler.cancelAll(); + + // Then ticking the scheduler once to fully remove the command and schedule the defaults + m_scheduler.run(); + + assertFalse(m_scheduler.isRunning(command), "Command was not canceled by cancelAll()"); + + for (var mechanism : mechanisms) { + var runningCommands = m_scheduler.getRunningCommandsFor(mechanism); + assertEquals( + 1, + runningCommands.size(), + "mechanism " + mechanism + " should have exactly one running command"); + assertEquals( + mechanism.getDefaultCommand(), + runningCommands.getFirst(), + "mechanism " + mechanism + " is not running the default command"); + } + } + + @Test + void cancelDeeplyNestedCompositions() { + Command root = + Command.noRequirements() + .executing( + co -> { + co.await( + Command.noRequirements() + .executing( + co2 -> { + co2.await( + Command.noRequirements() + .executing( + co3 -> { + co3.await( + Command.noRequirements() + .executing(Coroutine::park) + .named("Park")); + }) + .named("C3")); + }) + .named("C2")); + }) + .named("Root"); + + m_scheduler.schedule(root); + + m_scheduler.run(); + assertEquals(4, m_scheduler.getRunningCommands().size()); + + m_scheduler.cancel(root); + assertEquals(0, m_scheduler.getRunningCommands().size()); + } + + @Test + void compositionsDoNotSelfCancel() { + var mech = new Mechanism("The mechanism", m_scheduler); + var group = + mech.run( + co -> { + co.await( + mech.run( + co2 -> { + co2.await( + mech.run( + co3 -> { + co3.await(mech.run(Coroutine::park).named("Park")); + }) + .named("C3")); + }) + .named("C2")); + }) + .named("Group"); + + m_scheduler.schedule(group); + m_scheduler.run(); + assertEquals(4, m_scheduler.getRunningCommands().size()); + assertTrue(m_scheduler.isRunning(group)); + } + + @Test + void compositionsDoNotCancelParent() { + var mech = new Mechanism("The mechanism", m_scheduler); + var group = + mech.run( + co -> { + co.fork(mech.run(Coroutine::park).named("First Child")); + co.fork(mech.run(Coroutine::park).named("Second Child")); + co.park(); + }) + .named("Group"); + + m_scheduler.schedule(group); + m_scheduler.run(); + + // second child interrupts first child + assertEquals( + List.of("Group", "Second Child"), + m_scheduler.getRunningCommands().stream().map(Command::name).toList()); + } + + @Test + void doesNotRunOnCancelWhenInterruptingOnDeck() { + var ran = new AtomicBoolean(false); + + var mechanism = new Mechanism("The mechanism", m_scheduler); + var cmd = mechanism.run(Coroutine::yield).whenCanceled(() -> ran.set(true)).named("cmd"); + var interrupter = mechanism.run(Coroutine::yield).named("Interrupter"); + m_scheduler.schedule(cmd); + m_scheduler.schedule(interrupter); + m_scheduler.run(); + + assertFalse(ran.get(), "onCancel ran when it shouldn't have!"); + } + + @Test + void doesNotRunOnCancelWhenCancelingOnDeck() { + var ran = new AtomicBoolean(false); + + var mechanism = new Mechanism("The mechanism", m_scheduler); + var cmd = mechanism.run(Coroutine::yield).whenCanceled(() -> ran.set(true)).named("cmd"); + m_scheduler.schedule(cmd); + // canceling before calling .run() + m_scheduler.cancel(cmd); + m_scheduler.run(); + + assertFalse(ran.get(), "onCancel ran when it shouldn't have!"); + } + + @Test + void runsOnCancelWhenInterruptingCommand() { + var ran = new AtomicBoolean(false); + + var mechanism = new Mechanism("The mechanism", m_scheduler); + var cmd = mechanism.run(Coroutine::park).whenCanceled(() -> ran.set(true)).named("cmd"); + var interrupter = mechanism.run(Coroutine::park).named("Interrupter"); + m_scheduler.schedule(cmd); + m_scheduler.run(); + m_scheduler.schedule(interrupter); + m_scheduler.run(); + + assertTrue(ran.get(), "onCancel should have run!"); + } + + @Test + void doesNotRunOnCancelWhenCompleting() { + var ran = new AtomicBoolean(false); + + var mechanism = new Mechanism("The mechanism", m_scheduler); + var cmd = mechanism.run(Coroutine::yield).whenCanceled(() -> ran.set(true)).named("cmd"); + m_scheduler.schedule(cmd); + m_scheduler.run(); + m_scheduler.run(); + + assertFalse(m_scheduler.isScheduledOrRunning(cmd)); + assertFalse(ran.get(), "onCancel ran when it shouldn't have!"); + } + + @Test + void runsOnCancelWhenCanceling() { + var ran = new AtomicBoolean(false); + + var mechanism = new Mechanism("The mechanism", m_scheduler); + var cmd = mechanism.run(Coroutine::yield).whenCanceled(() -> ran.set(true)).named("cmd"); + m_scheduler.schedule(cmd); + m_scheduler.run(); + m_scheduler.cancel(cmd); + + assertTrue(ran.get(), "onCancel should have run!"); + } + + @Test + void runsOnCancelWhenCancelingParent() { + var ran = new AtomicBoolean(false); + + var mechanism = new Mechanism("The mechanism", m_scheduler); + var cmd = mechanism.run(Coroutine::yield).whenCanceled(() -> ran.set(true)).named("cmd"); + + var group = new SequentialGroup("Seq", Collections.singletonList(cmd)); + m_scheduler.schedule(group); + m_scheduler.run(); + m_scheduler.cancel(group); + + assertTrue(ran.get(), "onCancel should have run!"); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/SchedulerConflictTests.java b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerConflictTests.java new file mode 100644 index 0000000000..a4823fea4d --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerConflictTests.java @@ -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. + +package org.wpilib.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.function.Supplier; +import org.junit.jupiter.api.Test; + +class SchedulerConflictTests extends CommandTestBase { + @Test + void compositionsCannotAwaitConflictingCommands() { + var mech = new Mechanism("The Mechanism", m_scheduler); + + var group = + Command.noRequirements() + .executing( + co -> { + co.awaitAll( + mech.run(Coroutine::park).named("First"), + mech.run(Coroutine::park).named("Second")); + }) + .named("Group"); + + m_scheduler.schedule(group); + + // Running should attempt to schedule multiple conflicting commands + var exception = assertThrows(IllegalArgumentException.class, m_scheduler::run); + assertEquals( + "Commands running in parallel cannot share requirements: " + + "First and Second both require The Mechanism", + exception.getMessage()); + } + + @Test + void innerCommandMayInterruptOtherInnerCommand() { + var mechanism = new Mechanism("The mechanism", m_scheduler); + var firstRan = new AtomicBoolean(false); + var secondRan = new AtomicBoolean(false); + + var first = + mechanism + .run( + c -> { + firstRan.set(true); + c.park(); + }) + .named("First"); + + var second = + mechanism + .run( + c -> { + secondRan.set(true); + c.park(); + }) + .named("Second"); + + var group = + Command.noRequirements() + .executing( + co -> { + co.fork(first); + co.fork(second); + co.park(); + }) + .named("Group"); + + m_scheduler.schedule(group); + m_scheduler.run(); + + assertTrue(firstRan.get(), "First child should have run to a yield point"); + assertTrue(secondRan.get(), "Second child should have run to a yield point"); + assertFalse( + m_scheduler.isScheduledOrRunning(first), "First child should have been interrupted"); + assertTrue(m_scheduler.isRunning(second), "Second child should still be running"); + assertTrue(m_scheduler.isRunning(group), "Group should still be running"); + } + + @Test + void nestedOneShotCompositionsAllRunInOneCycle() { + var runs = new AtomicInteger(0); + Supplier makeOneShot = + () -> Command.noRequirements().executing(_c -> runs.incrementAndGet()).named("One Shot"); + var command = + Command.noRequirements() + .executing( + co -> { + co.fork(makeOneShot.get()); + co.fork(makeOneShot.get()); + co.fork( + Command.noRequirements() + .executing(inner -> inner.fork(makeOneShot.get())) + .named("Inner")); + co.fork( + Command.noRequirements() + .executing( + co2 -> { + co2.fork(makeOneShot.get()); + co2.fork( + Command.noRequirements() + .executing( + co3 -> { + co3.fork(makeOneShot.get()); + }) + .named("3")); + }) + .named("2")); + }) + .named("Command"); + + m_scheduler.schedule(command); + m_scheduler.run(); + assertEquals(5, runs.get(), "All oneshot commands should have run"); + assertFalse(m_scheduler.isRunning(command), "Command should have exited after one cycle"); + } + + @Test + void childConflictsWithHigherPriorityTopLevel() { + var mechanism = new Mechanism("mechanism", m_scheduler); + var top = mechanism.run(Coroutine::park).withPriority(10).named("Top"); + + // Child conflicts with and is lower priority than the Top command + // It should not be scheduled, and the parent command should exit immediately + var child = mechanism.run(Coroutine::park).named("Child"); + var parent = Command.noRequirements().executing(co -> co.await(child)).named("Parent"); + + m_scheduler.schedule(top); + m_scheduler.schedule(parent); + m_scheduler.run(); + + assertTrue(m_scheduler.isRunning(top), "Top command should not have been interrupted"); + assertFalse(m_scheduler.isRunning(child), "Conflicting child should not have run"); + assertFalse(m_scheduler.isRunning(parent), "Parent of conflicting child should have exited"); + } + + @Test + void childConflictsWithLowerPriorityTopLevel() { + var mechanism = new Mechanism("mechanism", m_scheduler); + var top = mechanism.run(Coroutine::park).withPriority(-10).named("Top"); + + // Child conflicts with and is higher priority than the Top command + // It should be scheduled, and the top command should be interrupted + var child = mechanism.run(Coroutine::park).named("Child"); + var parent = Command.noRequirements().executing(co -> co.await(child)).named("Parent"); + + m_scheduler.schedule(top); + m_scheduler.schedule(parent); + m_scheduler.run(); + + assertFalse(m_scheduler.isRunning(top), "Top command should have been interrupted"); + assertTrue(m_scheduler.isRunning(child), "Conflicting child should be running"); + assertTrue(m_scheduler.isRunning(parent), "Parent of conflicting child should be running"); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/SchedulerErrorHandlingTests.java b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerErrorHandlingTests.java new file mode 100644 index 0000000000..758639dc42 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerErrorHandlingTests.java @@ -0,0 +1,208 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.ArrayList; +import org.junit.jupiter.api.Test; + +class SchedulerErrorHandlingTests extends CommandTestBase { + @Test + void errorDetection() { + var mechanism = new Mechanism("X", m_scheduler); + + var command = + mechanism + .run( + coroutine -> { + throw new RuntimeException("The exception"); + }) + .named("Bad Behavior"); + + new Trigger(m_scheduler, () -> true).onTrue(command); + + var e = assertThrows(RuntimeException.class, m_scheduler::run); + assertEquals("The exception", e.getMessage()); + + assertEquals( + "org.wpilib.commands3.SchedulerErrorHandlingTests", e.getStackTrace()[0].getClassName()); + assertEquals("lambda$errorDetection$0", e.getStackTrace()[0].getMethodName()); + + assertEquals("=== Command Binding Trace ===", e.getStackTrace()[2].getClassName()); + + assertEquals(getClass().getName(), e.getStackTrace()[3].getClassName()); + assertEquals("errorDetection", e.getStackTrace()[3].getMethodName()); + } + + @Test + void nestedErrorDetection() { + var command = + Command.noRequirements() + .executing( + co -> { + co.await( + Command.noRequirements() + .executing( + c2 -> { + new Trigger(m_scheduler, () -> true) + .onTrue( + Command.noRequirements() + .executing( + c3 -> { + // Throws IndexOutOfBoundsException + var unused = new ArrayList<>(0).get(-1); + }) + .named("Throws IndexOutOfBounds")); + c2.park(); + }) + .named("Schedules With Trigger")); + }) + .named("Schedules Directly"); + + m_scheduler.schedule(command); + + // The first run sets up the trigger, but does not fire + // The second run will fire the trigger and cause the inner command to run and throw + m_scheduler.run(); + + var e = assertThrows(IndexOutOfBoundsException.class, m_scheduler::run); + StackTraceElement[] stackTrace = e.getStackTrace(); + + assertEquals("Index -1 out of bounds for length 0", e.getMessage()); + int nestedIndex = 0; + for (; nestedIndex < stackTrace.length; nestedIndex++) { + if (stackTrace[nestedIndex].getClassName().equals(getClass().getName())) { + break; + } + } + + // user code trace for the scheduler run invocation (to `scheduler.run()` in the try block) + assertEquals("lambda$nestedErrorDetection$3", stackTrace[nestedIndex].getMethodName()); + assertEquals("assertThrows", stackTrace[nestedIndex + 1].getMethodName()); + + // user code trace for where the command was scheduled (the `.onTrue()` line) + assertEquals("=== Command Binding Trace ===", stackTrace[nestedIndex + 2].getClassName()); + assertEquals("lambda$nestedErrorDetection$4", stackTrace[nestedIndex + 3].getMethodName()); + assertEquals("lambda$nestedErrorDetection$5", stackTrace[nestedIndex + 4].getMethodName()); + assertEquals("nestedErrorDetection", stackTrace[nestedIndex + 5].getMethodName()); + } + + @Test + void commandEncounteringErrorCancelsChildren() { + var child = Command.noRequirements().executing(Coroutine::park).named("Child 1"); + var command = + Command.noRequirements() + .executing( + co -> { + co.fork(child); + throw new RuntimeException("The exception"); + }) + .named("Bad Behavior"); + + m_scheduler.schedule(command); + assertThrows(RuntimeException.class, m_scheduler::run); + assertFalse( + m_scheduler.isScheduledOrRunning(command), + "Command should have been removed from the scheduler"); + assertFalse( + m_scheduler.isScheduledOrRunning(child), + "Child should have been removed from the scheduler"); + } + + @Test + void childCommandEncounteringErrorCancelsParent() { + var child = + Command.noRequirements() + .executing( + co -> { + throw new RuntimeException("The exception"); // note: bubbles up to the parent + }) + .named("Child 1"); + var command = + Command.noRequirements() + .executing( + co -> { + co.await(child); + co.park(); // pretend other things would happen after the child + }) + .named("Parent"); + + m_scheduler.schedule(command); + assertThrows(RuntimeException.class, m_scheduler::run); + assertFalse( + m_scheduler.isRunning(command), + "Parent command should have been removed from the scheduler"); + assertFalse(m_scheduler.isRunning(child), "Child should have been removed from the scheduler"); + } + + @Test + @SuppressWarnings("PMD.CompareObjectsWithEquals") + void childCommandEncounteringErrorAfterRemountCancelsParent() { + var child = + Command.noRequirements() + .executing( + co -> { + co.yield(); + throw new RuntimeException("The exception"); // does not bubble up to the parent + }) + .named("Child 1"); + var command = + Command.noRequirements() + .executing( + co -> { + co.await(child); + co.park(); // pretend other things would happen after the child + }) + .named("Parent"); + + m_scheduler.schedule(command); + + // first run schedules the child and adds it to the running set + m_scheduler.run(); + + // second run encounters the error in the child + final var error = assertThrows(RuntimeException.class, m_scheduler::run); + assertFalse( + m_scheduler.isRunning(command), + "Parent command should have been removed from the scheduler"); + assertFalse(m_scheduler.isRunning(child), "Child should have been removed from the scheduler"); + + // Full event history + assertEquals(9, m_events.size()); + assertTrue( + m_events.get(0) instanceof SchedulerEvent.Scheduled s && s.command() == command, + "First event should be parent scheduled"); + assertTrue( + m_events.get(1) instanceof SchedulerEvent.Mounted m && m.command() == command, + "Second event should be parent mounted"); + assertTrue( + m_events.get(2) instanceof SchedulerEvent.Scheduled s && s.command() == child, + "Third event should be child scheduled"); + assertTrue( + m_events.get(3) instanceof SchedulerEvent.Mounted m && m.command() == child, + "Fourth event should be child mounted"); + assertTrue( + m_events.get(4) instanceof SchedulerEvent.Yielded y && y.command() == child, + "Fifth event should be child yielded"); + assertTrue( + m_events.get(5) instanceof SchedulerEvent.Yielded y && y.command() == command, + "Sixth event should be parent yielded"); + assertTrue( + m_events.get(6) instanceof SchedulerEvent.Mounted m && m.command() == child, + "Seventh event should be child remounted"); + assertTrue( + m_events.get(7) instanceof SchedulerEvent.CompletedWithError c + && c.command() == child + && c.error() == error, + "Eighth event should be child completed with error"); + assertTrue( + m_events.get(8) instanceof SchedulerEvent.Canceled c && c.command() == command, + "Ninth event should be parent canceled"); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/SchedulerPriorityLevelTests.java b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerPriorityLevelTests.java new file mode 100644 index 0000000000..3e65aa8163 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerPriorityLevelTests.java @@ -0,0 +1,64 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; + +class SchedulerPriorityLevelTests extends CommandTestBase { + @Test + void higherPriorityCancels() { + final var subsystem = new Mechanism("Subsystem", m_scheduler); + + final var lower = new PriorityCommand(-1000, subsystem); + final var higher = new PriorityCommand(+1000, subsystem); + + m_scheduler.schedule(lower); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(lower)); + + m_scheduler.schedule(higher); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(higher)); + assertFalse(m_scheduler.isRunning(lower)); + } + + @Test + void lowerPriorityDoesNotCancel() { + final var subsystem = new Mechanism("Subsystem", m_scheduler); + + final var lower = new PriorityCommand(-1000, subsystem); + final var higher = new PriorityCommand(+1000, subsystem); + + m_scheduler.schedule(higher); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(higher)); + + m_scheduler.schedule(lower); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(higher), "Higher priority command should still be running"); + assertFalse( + m_scheduler.isScheduledOrRunning(lower), "Lower priority command should not be running"); + } + + @Test + void samePriorityCancels() { + final var subsystem = new Mechanism("Subsystem", m_scheduler); + + final var first = new PriorityCommand(512, subsystem); + final var second = new PriorityCommand(512, subsystem); + + m_scheduler.schedule(first); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(first)); + + m_scheduler.schedule(second); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(second), "New command should be running"); + assertFalse(m_scheduler.isRunning(first), "Old command should be canceled"); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/SchedulerSideloadFunctionTests.java b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerSideloadFunctionTests.java new file mode 100644 index 0000000000..958005e5e7 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerSideloadFunctionTests.java @@ -0,0 +1,100 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrowsExactly; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; +import org.junit.jupiter.api.Test; + +class SchedulerSideloadFunctionTests extends CommandTestBase { + @Test + void sideloadThrowingException() { + m_scheduler.sideload( + co -> { + throw new RuntimeException("Bang!"); + }); + + // An exception raised in a sideload function should bubble up + assertEquals( + "Bang!", assertThrowsExactly(RuntimeException.class, m_scheduler::run).getMessage()); + } + + @Test + void periodicSideload() { + AtomicInteger count = new AtomicInteger(0); + m_scheduler.addPeriodic(count::incrementAndGet); + assertEquals(0, count.get()); + + m_scheduler.run(); + assertEquals(1, count.get()); + + m_scheduler.run(); + assertEquals(2, count.get()); + } + + @Test + void sideloadSchedulingCommand() { + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + // one-shot sideload forks a command and immediately exits + m_scheduler.sideload(co -> co.fork(command)); + m_scheduler.run(); + assertTrue( + m_scheduler.isRunning(command), "command should have started and outlasted the sideload"); + } + + @Test + void childCommandEscapesViaSideload() { + var child = Command.noRequirements().executing(Coroutine::park).named("Child"); + var parent = + Command.noRequirements() + .executing( + parentCoroutine -> { + m_scheduler.sideload(sidelodCoroutine -> sidelodCoroutine.fork(child)); + }) + .named("Parent"); + + m_scheduler.schedule(parent); + m_scheduler.run(); + assertFalse(m_scheduler.isScheduledOrRunning(parent), "parent should have exited"); + assertFalse( + m_scheduler.isScheduledOrRunning(child), + "the sideload to schedule the child should not have run yet"); + + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(child), "child should have started running"); + } + + @Test + void sideloadCancelingCommand() { + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + m_scheduler.schedule(command); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command), "command should have started"); + + m_scheduler.sideload(co -> m_scheduler.cancel(command)); + assertTrue(m_scheduler.isRunning(command), "sideload should not have run yet"); + + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(command), "sideload should have canceled the command"); + } + + @Test + void sideloadAffectsStateForTriggerInSameCycle() { + AtomicBoolean signal = new AtomicBoolean(false); + var trigger = new Trigger(m_scheduler, signal::get); + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + trigger.onTrue(command); + m_scheduler.sideload(co -> signal.set(true)); + + m_scheduler.run(); + assertTrue(signal.get(), "Sideload should have run and set the signal"); + assertTrue(m_scheduler.isRunning(command), "Command should have started"); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/SchedulerTelemetryTests.java b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerTelemetryTests.java new file mode 100644 index 0000000000..9c7f07bff4 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerTelemetryTests.java @@ -0,0 +1,116 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +class SchedulerTelemetryTests extends CommandTestBase { + @Test + void protobuf() { + var mech = new Mechanism("The mechanism", m_scheduler); + var parkCommand = mech.run(Coroutine::park).named("Park"); + var c3Command = mech.run(co -> co.await(parkCommand)).named("C3"); + var c2Command = mech.run(co -> co.await(c3Command)).named("C2"); + var group = mech.run(co -> co.await(c2Command)).named("Group"); + + m_scheduler.schedule(group); + m_scheduler.run(); + + var scheduledCommand1 = Command.noRequirements().executing(Coroutine::park).named("Command 1"); + var scheduledCommand2 = Command.noRequirements().executing(Coroutine::park).named("Command 2"); + m_scheduler.schedule(scheduledCommand1); + m_scheduler.schedule(scheduledCommand2); + + var message = Scheduler.proto.createMessage(); + Scheduler.proto.pack(message, m_scheduler); + var messageJson = message.toString(); + assertEquals( + """ + { + "lastTimeMs": %s, + "queuedCommands": [{ + "priority": 0, + "id": %s, + "name": "Command 1", + "requirements": [] + }, { + "priority": 0, + "id": %s, + "name": "Command 2", + "requirements": [] + }], + "runningCommands": [{ + "lastTimeMs": %s, + "totalTimeMs": %s, + "priority": 0, + "id": %s, + "name": "Group", + "requirements": [{ + "name": "The mechanism" + }] + }, { + "lastTimeMs": %s, + "totalTimeMs": %s, + "priority": 0, + "id": %s, + "parentId": %s, + "name": "C2", + "requirements": [{ + "name": "The mechanism" + }] + }, { + "lastTimeMs": %s, + "totalTimeMs": %s, + "priority": 0, + "id": %s, + "parentId": %s, + "name": "C3", + "requirements": [{ + "name": "The mechanism" + }] + }, { + "lastTimeMs": %s, + "totalTimeMs": %s, + "priority": 0, + "id": %s, + "parentId": %s, + "name": "Park", + "requirements": [{ + "name": "The mechanism" + }] + }] + }""" + .formatted( + // Scheduler data + m_scheduler.lastRuntimeMs(), + + // On deck commands + m_scheduler.runId(scheduledCommand1), + m_scheduler.runId(scheduledCommand2), + + // Running commands + m_scheduler.lastCommandRuntimeMs(group), + m_scheduler.totalRuntimeMs(group), + m_scheduler.runId(group), // id + // top-level command, no parent ID + + m_scheduler.lastCommandRuntimeMs(c2Command), + m_scheduler.totalRuntimeMs(c2Command), + m_scheduler.runId(c2Command), // id + m_scheduler.runId(group), // parent + m_scheduler.lastCommandRuntimeMs(c3Command), + m_scheduler.totalRuntimeMs(c3Command), + m_scheduler.runId(c3Command), // id + m_scheduler.runId(c2Command), // parent + m_scheduler.lastCommandRuntimeMs(parkCommand), + m_scheduler.totalRuntimeMs(parkCommand), + m_scheduler.runId(parkCommand), // id + m_scheduler.runId(c3Command) // parent + ), + messageJson); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/SchedulerTest.java b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerTest.java new file mode 100644 index 0000000000..4d5fd92a7b --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerTest.java @@ -0,0 +1,161 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.fail; + +import java.util.List; +import java.util.concurrent.atomic.AtomicBoolean; +import org.junit.jupiter.api.Test; + +class SchedulerTest extends CommandTestBase { + @Test + void basic() { + var enabled = new AtomicBoolean(false); + var ran = new AtomicBoolean(false); + var command = + Command.noRequirements() + .executing( + coroutine -> { + do { + coroutine.yield(); + } while (!enabled.get()); + ran.set(true); + }) + .named("Basic Command"); + + m_scheduler.schedule(command); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command), "Command should be running after being scheduled"); + + enabled.set(true); + m_scheduler.run(); + if (m_scheduler.isRunning(command)) { + fail("Command should no longer be running after awaiting its completion"); + } + + assertTrue(ran.get()); + } + + @Test + @SuppressWarnings("PMD.ImmutableField") // PMD bugs + void atomicity() { + var mechanism = + new Mechanism("X", m_scheduler) { + int m_x = 0; + }; + + // Launch 100 commands that each call `x++` 500 times. + // If commands run on different threads, the lack of atomic + // operations or locks will mean the final number will be + // less than the expected 50,000 + int numCommands = 100; + int iterations = 500; + + for (int cmdCount = 0; cmdCount < numCommands; cmdCount++) { + var command = + Command.noRequirements() + .executing( + coroutine -> { + for (int i = 0; i < iterations; i++) { + mechanism.m_x++; + coroutine.yield(); + } + }) + .named("CountCommand[" + cmdCount + "]"); + + m_scheduler.schedule(command); + } + + for (int i = 0; i < iterations; i++) { + m_scheduler.run(); + } + + assertEquals(numCommands * iterations, mechanism.m_x); + } + + @Test + @SuppressWarnings("PMD.ImmutableField") // PMD bugs + void runMechanism() { + var example = + new Mechanism("Counting", m_scheduler) { + int m_x = 0; + }; + + Command countToTen = + example + .run( + coroutine -> { + example.m_x = 0; + for (int i = 0; i < 10; i++) { + coroutine.yield(); + example.m_x++; + } + }) + .named("Count To Ten"); + + m_scheduler.schedule(countToTen); + for (int i = 0; i < 10; i++) { + m_scheduler.run(); + } + m_scheduler.run(); + + assertEquals(10, example.m_x); + } + + @Test + void compositionsDoNotNeedRequirements() { + var m1 = new Mechanism("M1", m_scheduler); + var m2 = new Mechanism("m2", m_scheduler); + + // the group has no requirements, but can schedule child commands that do + var group = + Command.noRequirements() + .executing( + co -> { + co.awaitAll( + m1.run(Coroutine::park).named("M1 Command"), + m2.run(Coroutine::park).named("M2 Command")); + }) + .named("Composition"); + + m_scheduler.schedule(group); + m_scheduler.run(); // start m1 and m2 commands + assertEquals(3, m_scheduler.getRunningCommands().size()); + } + + @Test + void nestedMechanisms() { + var superstructure = + new Mechanism("Superstructure", m_scheduler) { + private final Mechanism m_elevator = new Mechanism("Elevator", m_scheduler); + private final Mechanism m_arm = new Mechanism("Arm", m_scheduler); + + public Command superCommand() { + return run(co -> { + co.await(m_elevator.run(Coroutine::park).named("Elevator Subcommand")); + co.await(m_arm.run(Coroutine::park).named("Arm Subcommand")); + }) + .named("Super Command"); + } + }; + + m_scheduler.schedule(superstructure.superCommand()); + m_scheduler.run(); + assertEquals( + List.of(superstructure.m_arm.getDefaultCommand()), + superstructure.m_arm.getRunningCommands(), + "Arm should only be running its default command"); + + // Scheduling something that requires an in-use inner mechanism cancels the outer command + m_scheduler.schedule(superstructure.m_elevator.run(Coroutine::park).named("Conflict")); + + m_scheduler.run(); // schedules the default superstructure command + m_scheduler.run(); // starts running the default superstructure command + assertEquals(List.of(superstructure.getDefaultCommand()), superstructure.getRunningCommands()); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/SchedulerTimingTests.java b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerTimingTests.java new file mode 100644 index 0000000000..1643c7777f --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/SchedulerTimingTests.java @@ -0,0 +1,244 @@ +// 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.commands3; + +import static edu.wpi.first.units.Units.Microseconds; +import static edu.wpi.first.units.Units.Milliseconds; +import static edu.wpi.first.units.Units.Seconds; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.wpilibj.RobotController; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.concurrent.atomic.AtomicLong; +import java.util.concurrent.atomic.AtomicReference; +import org.junit.jupiter.api.Test; + +class SchedulerTimingTests extends CommandTestBase { + @Test + void commandAwaitingItself() { + // This command deadlocks on itself. It's calling yield() in an infinite loop, which is + // equivalent to calling Coroutine.park(). No deleterious side effects other than stalling + // the command + AtomicReference commandRef = new AtomicReference<>(); + var command = + Command.noRequirements().executing(co -> co.await(commandRef.get())).named("Self Await"); + commandRef.set(command); + + m_scheduler.schedule(command); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command)); + } + + @Test + void commandDeadlock() { + AtomicReference parentRef = new AtomicReference<>(); + AtomicReference childRef = new AtomicReference<>(); + + // Deadlock scenario: + // parent starts, schedules child, then waits for child to exit + // child starts, waits for parent to exit + // + // Each successive run sees parent mount, check for child, then yield. + // Then sees child mount, check for parent, then also yield. + // This is like two threads spinwaiting for the other to exit. + // + // Externally canceling child allows parent to continue + // Externally canceling parent cancels both + var parent = Command.noRequirements().executing(co -> co.await(childRef.get())).named("Parent"); + var child = Command.noRequirements().executing(co -> co.await(parentRef.get())).named("Child"); + parentRef.set(parent); + childRef.set(child); + + m_scheduler.schedule(parent); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(parent)); + assertTrue(m_scheduler.isRunning(child)); + + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(parent)); + assertTrue(m_scheduler.isRunning(child)); + + m_scheduler.cancel(parent); + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(parent)); + assertFalse(m_scheduler.isRunning(child)); + } + + @Test + void delayedCommandDeadlock() { + AtomicReference ref1 = new AtomicReference<>(); + AtomicReference ref2 = new AtomicReference<>(); + + AtomicBoolean command1CompletedNormally = new AtomicBoolean(false); + AtomicBoolean command2CompletedNormally = new AtomicBoolean(false); + + // Deadlock scenario: + // command1 starts, waits for command2 to exit + // command2 starts, waits for command1 to exit + // + // Each successive run sees command1 mount, check for command2, then yield. + // Then sees command2 mount, check for command1, then also yield. + // This is like two threads spinwaiting for the other to exit. + // + // Externally canceling either command allows the other to exit + var command1 = + Command.noRequirements() + .executing( + co -> { + co.yield(); + co.await(ref2.get()); + command1CompletedNormally.set(true); + }) + .named("Command 1"); + var command2 = + Command.noRequirements() + .executing( + co -> { + co.yield(); + co.await(ref1.get()); + command2CompletedNormally.set(true); + }) + .named("Command 2"); + ref1.set(command1); + ref2.set(command2); + + m_scheduler.schedule(command1); + m_scheduler.schedule(command2); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command1)); + assertTrue(m_scheduler.isRunning(command2)); + + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command1)); + assertTrue(m_scheduler.isRunning(command2)); + + m_scheduler.cancel(command2); + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(command1)); + assertFalse(m_scheduler.isRunning(command2)); + assertTrue( + command1CompletedNormally.get(), + "Command 1 should have completed normally after command 2 stopped"); + assertFalse( + command2CompletedNormally.get(), + "Canceling command 2 should have stopped it before completing"); + } + + @Test + void forkedChildRunsOnce() { + AtomicInteger runCount = new AtomicInteger(0); + + var inner = + Command.noRequirements() + .executing( + co -> { + runCount.incrementAndGet(); + co.yield(); + + runCount.incrementAndGet(); + co.yield(); + }) + .named("Inner"); + + var outer = Command.noRequirements().executing(co -> co.await(inner)).named("Outer"); + m_scheduler.schedule(outer); + m_scheduler.run(); + + assertEquals(1, runCount.get()); + } + + @Test + void shortWaitWaitsOneLoop() { + AtomicLong time = new AtomicLong(0); + RobotController.setTimeSource(time::get); + + AtomicBoolean completedWait = new AtomicBoolean(false); + var command = + Command.noRequirements() + .executing( + co -> { + co.wait(Milliseconds.of(1)); + completedWait.set(true); + }) + .named("Short Wait"); + + m_scheduler.schedule(command); + m_scheduler.run(); + + // wait 1 full second (much longer than the wait period) + time.set((long) Seconds.of(1).in(Microseconds)); + m_scheduler.run(); + assertTrue( + completedWait.get(), + "Command with a short wait should have completed if its duration has elapsed between runs"); + } + + @Test + void shortWaitWaitsOneLoopWithFastPeriod() { + AtomicLong time = new AtomicLong(0); + RobotController.setTimeSource(time::get); + AtomicBoolean completedWait = new AtomicBoolean(false); + var command = + Command.noRequirements() + .executing( + co -> { + co.wait(Milliseconds.of(1)); + completedWait.set(true); + }) + .named("Short Wait"); + + m_scheduler.schedule(command); + m_scheduler.run(); + + // move forward by half the wait period + time.set((long) Milliseconds.of(0.5).in(Microseconds)); + m_scheduler.run(); + assertFalse(completedWait.get(), "Command should still be waiting for 1 ms to elapse"); + + // move forward by the rest of the wait period + time.set((long) Milliseconds.of(1).in(Microseconds)); + m_scheduler.run(); + assertTrue( + completedWait.get(), + "Command with a short wait should have completed if its duration has elapsed between runs"); + } + + @Test + void awaitingExitsImmediatelyWithoutAOneLoopDelay() { + AtomicInteger innerRuns = new AtomicInteger(0); + var inner = + Command.noRequirements() + .executing( + co -> { + // executed immediately when forked + innerRuns.incrementAndGet(); + co.yield(); + + // executed again on the next scheduler run, after the forking command runs + innerRuns.incrementAndGet(); + }) + .named("Inner"); + + var outer = Command.noRequirements().executing(co -> co.await(inner)).named("Outer"); + m_scheduler.schedule(outer); + + // First run: runs outer, forks inner, inner runs to its first yield, outer yields + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(inner)); + assertTrue(m_scheduler.isRunning(outer)); + assertEquals(1, innerRuns.get()); + + // Second run: runs inner to completion, runs outer, outer sees inner is complete and exits + // NOTE: If child commands ran AFTER their parents, then outer would not have exited here and + // would take another scheduler run to complete + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(inner)); + assertFalse(m_scheduler.isRunning(outer)); + assertEquals(2, innerRuns.get()); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/SequentialGroupTest.java b/commandsv3/src/test/java/org/wpilib/commands3/SequentialGroupTest.java new file mode 100644 index 0000000000..4481ee8181 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/SequentialGroupTest.java @@ -0,0 +1,82 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.List; +import java.util.Set; +import org.junit.jupiter.api.Test; + +class SequentialGroupTest extends CommandTestBase { + @Test + void single() { + var command = Command.noRequirements().executing(Coroutine::yield).named("The Command"); + + var sequence = new SequentialGroup("The Sequence", List.of(command)); + m_scheduler.schedule(sequence); + + // First run - the composed command starts and yields; sequence yields + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(sequence)); + assertTrue(m_scheduler.isRunning(command)); + + // Second run - the composed command completes; sequence sees its completion and exits + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(sequence)); + assertFalse(m_scheduler.isRunning(command)); + } + + @Test + void twoCommands() { + var c1 = Command.noRequirements().executing(Coroutine::yield).named("C1"); + var c2 = Command.noRequirements().executing(Coroutine::yield).named("C2"); + + var sequence = new SequentialGroup("C1 > C2", List.of(c1, c2)); + m_scheduler.schedule(sequence); + + // First run - c1 is scheduled and starts + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(sequence), "Sequence should be running"); + assertTrue(m_scheduler.isRunning(c1), "Starting the sequence should start the first command"); + assertFalse( + m_scheduler.isScheduledOrRunning(c2), + "The second command should still be pending completion of the first command"); + + // Second run - c1 completes, sequence sees it finish, schedules c2 + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(sequence)); + assertFalse(m_scheduler.isRunning(c1), "First command should have completed"); + assertTrue( + m_scheduler.isScheduledOrRunning(c2), "Second command should not start in the same cycle"); + + // Third run - c2 completes, sequence sees it finish, exits + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(sequence)); + assertFalse(m_scheduler.isRunning(c2), "Second command should have started"); + } + + @Test + void inheritsRequirements() { + var mech1 = new Mechanism("Mech 1", m_scheduler); + var mech2 = new Mechanism("Mech 2", m_scheduler); + var command1 = mech1.run(Coroutine::park).named("Command 1"); + var command2 = mech2.run(Coroutine::park).named("Command 2"); + var sequence = new SequentialGroup("Sequence", List.of(command1, command2)); + assertEquals(Set.of(mech1, mech2), sequence.requirements(), "Requirements were not inherited"); + } + + @Test + void inheritsPriority() { + var mech1 = new Mechanism("Mech 1", m_scheduler); + var mech2 = new Mechanism("Mech 2", m_scheduler); + var command1 = mech1.run(Coroutine::park).withPriority(100).named("Command 1"); + var command2 = mech2.run(Coroutine::park).withPriority(200).named("Command 2"); + var sequence = new SequentialGroup("Sequence", List.of(command1, command2)); + assertEquals(200, sequence.priority(), "Priority was not inherited"); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/StagedCommandBuilderTest.java b/commandsv3/src/test/java/org/wpilib/commands3/StagedCommandBuilderTest.java new file mode 100644 index 0000000000..44fef542a8 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/StagedCommandBuilderTest.java @@ -0,0 +1,254 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; + +import java.util.Arrays; +import java.util.Collection; +import java.util.List; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class StagedCommandBuilderTest { + private static final Runnable no_op = () -> {}; + + private Mechanism m_mech1; + private Mechanism m_mech2; + + @BeforeEach + void setUp() { + Scheduler scheduler = Scheduler.createIndependentScheduler(); + m_mech1 = new Mechanism("Mech 1", scheduler); + m_mech2 = new Mechanism("Mech 2", scheduler); + } + + // The next two tests are to check that various forms of builder usage are able to compile. + + @Test + void streamlined() { + Command command = + new StagedCommandBuilder() + .noRequirements() + .executing(Coroutine::park) + .until(() -> false) + .named("Name"); + + assertEquals("Name", command.name()); + } + + @Test + void allOptions() { + var mech = new Mechanism("Mech", Scheduler.createIndependentScheduler()); + + Command command = + new StagedCommandBuilder() + .noRequirements() + .requiring(mech) + .requiring(mech, mech) + .requiring(List.of(mech)) + .executing(Coroutine::park) + .whenCanceled(no_op) + .until(() -> false) + .withPriority(10) + .named("Name"); + + assertEquals("Name", command.name()); + } + + @Test + void starting_noRequirements_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var ignored = builder.noRequirements().executing(c -> {}).named("cmd"); + + var err = assertThrows(IllegalStateException.class, builder::noRequirements); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void starting_requiringVarargs_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var ignored = builder.noRequirements().executing(c -> {}).named("cmd"); + + var err = assertThrows(IllegalStateException.class, () -> builder.requiring(m_mech1, m_mech2)); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void starting_requiringCollection_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var ignored = builder.noRequirements().executing(c -> {}).named("cmd"); + + var err = + assertThrows( + IllegalStateException.class, () -> builder.requiring(List.of(m_mech1, m_mech2))); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void requirements_requiringSingle_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var reqStage = builder.noRequirements(); + var ignored = reqStage.executing(c -> {}).named("cmd"); + + var err = assertThrows(IllegalStateException.class, () -> reqStage.requiring(m_mech1)); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void requirements_requiringVarargs_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var reqStage = builder.noRequirements(); + var ignored = reqStage.executing(Coroutine::park).named("cmd"); + + var err = assertThrows(IllegalStateException.class, () -> reqStage.requiring(m_mech1, m_mech2)); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void requirements_requiringCollection_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var reqStage = builder.noRequirements(); + var ignored = reqStage.executing(Coroutine::park).named("cmd"); + + var err = assertThrows(IllegalStateException.class, () -> reqStage.requiring(List.of(m_mech1))); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void requirements_executing_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var reqStage = builder.noRequirements(); + var ignored = reqStage.executing(c -> {}).named("cmd"); + + Consumer impl = Coroutine::park; + var err = assertThrows(IllegalStateException.class, () -> reqStage.executing(impl)); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void execution_whenCanceled_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var execStage = builder.noRequirements().executing(c -> {}); + var ignored = execStage.named("cmd"); + + var err = assertThrows(IllegalStateException.class, () -> execStage.whenCanceled(() -> {})); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void execution_withPriority_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var execStage = builder.noRequirements().executing(c -> {}); + var ignored = execStage.named("cmd"); + + var err = assertThrows(IllegalStateException.class, () -> execStage.withPriority(7)); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void execution_until_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var execStage = builder.noRequirements().executing(c -> {}); + var ignored = execStage.named("cmd"); + + BooleanSupplier endCondition = () -> true; + var err = assertThrows(IllegalStateException.class, () -> execStage.until(endCondition)); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void execution_named_throwsAfterBuild() { + var builder = new StagedCommandBuilder(); + var execStage = builder.noRequirements().executing(c -> {}); + var ignored = execStage.named("cmd"); + + var err = assertThrows(IllegalStateException.class, () -> execStage.named("other")); + assertEquals("Command builders cannot be reused", err.getMessage()); + } + + @Test + void starting_requiringVarargs_nullFirstRequirement_throwsNPE() { + var builder = new StagedCommandBuilder(); + assertThrows(NullPointerException.class, () -> builder.requiring(null, m_mech2)); + } + + @Test + void starting_requiringVarargs_nullArray_throwsNPE() { + var builder = new StagedCommandBuilder(); + assertThrows(NullPointerException.class, () -> builder.requiring(m_mech1, (Mechanism[]) null)); + } + + @Test + void starting_requiringVarargs_nullInExtra_throwsNPE() { + var builder = new StagedCommandBuilder(); + assertThrows(NullPointerException.class, () -> builder.requiring(m_mech1, m_mech2, null)); + } + + @Test + void starting_requiringCollection_nullCollection_throwsNPE() { + var builder = new StagedCommandBuilder(); + assertThrows(NullPointerException.class, () -> builder.requiring((Collection) null)); + } + + @Test + void starting_requiringCollection_nullElement_throwsNPE() { + var builder = new StagedCommandBuilder(); + var listWithNull = Arrays.asList(m_mech1, null, m_mech2); // Arrays.asList allows nulls + assertThrows(NullPointerException.class, () -> builder.requiring(listWithNull)); + } + + @Test + void requirements_requiringSingle_null_throwsNPE() { + var req = new StagedCommandBuilder().noRequirements(); + assertThrows(NullPointerException.class, () -> req.requiring((Mechanism) null)); + } + + @Test + void requirements_requiringVarargs_nullFirstRequirement_throwsNPE() { + var req = new StagedCommandBuilder().noRequirements(); + assertThrows(NullPointerException.class, () -> req.requiring(null, m_mech2)); + } + + @Test + void requirements_requiringVarargs_nullArray_throwsNPE() { + var req = new StagedCommandBuilder().noRequirements(); + assertThrows(NullPointerException.class, () -> req.requiring(m_mech1, (Mechanism[]) null)); + } + + @Test + void requirements_requiringVarargs_nullInExtra_throwsNPE() { + var req = new StagedCommandBuilder().noRequirements(); + assertThrows(NullPointerException.class, () -> req.requiring(m_mech1, m_mech2, null)); + } + + @Test + void requirements_requiringCollection_nullCollection_throwsNPE() { + var req = new StagedCommandBuilder().noRequirements(); + assertThrows(NullPointerException.class, () -> req.requiring((Collection) null)); + } + + @Test + void requirements_requiringCollection_nullElement_throwsNPE() { + var req = new StagedCommandBuilder().noRequirements(); + var listWithNull = Arrays.asList(m_mech1, null); // Arrays.asList allows nulls + assertThrows(NullPointerException.class, () -> req.requiring(listWithNull)); + } + + @Test + void requirements_executing_nullImpl_throwsNPE() { + var req = new StagedCommandBuilder().noRequirements(); + assertThrows(NullPointerException.class, () -> req.executing(null)); + } + + @Test + void execution_named_nullName_throwsNPE() { + var exec = new StagedCommandBuilder().noRequirements().executing(c -> {}); + assertThrows(NullPointerException.class, () -> exec.named(null)); + } +} diff --git a/commandsv3/src/test/java/org/wpilib/commands3/TriggerTest.java b/commandsv3/src/test/java/org/wpilib/commands3/TriggerTest.java new file mode 100644 index 0000000000..fdc5b5dce5 --- /dev/null +++ b/commandsv3/src/test/java/org/wpilib/commands3/TriggerTest.java @@ -0,0 +1,247 @@ +// 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.commands3; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.List; +import java.util.concurrent.atomic.AtomicBoolean; +import org.junit.jupiter.api.Test; + +class TriggerTest extends CommandTestBase { + @Test + void onTrue() { + var signal = new AtomicBoolean(false); + var trigger = new Trigger(m_scheduler, signal::get); + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + trigger.onTrue(command); + + signal.set(true); + m_scheduler.run(); + + assertTrue(m_scheduler.isRunning(command), "Command was not scheduled on rising edge"); + + signal.set(false); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command), "Command should still be running on falling edge"); + } + + @Test + void onFalse() { + var signal = new AtomicBoolean(false); + var trigger = new Trigger(m_scheduler, signal::get); + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + trigger.onFalse(command); + + m_scheduler.run(); + assertTrue( + m_scheduler.isRunning(command), "Command should be scheduled when signal starts low"); + + signal.set(true); + m_scheduler.run(); + assertTrue( + m_scheduler.isRunning(command), "Command should still be running rising falling edge"); + } + + @Test + void whileTrue() { + var signal = new AtomicBoolean(false); + var trigger = new Trigger(m_scheduler, signal::get); + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + trigger.whileTrue(command); + + signal.set(true); + m_scheduler.run(); + + assertTrue(m_scheduler.isRunning(command), "Command was not scheduled on rising edge"); + + signal.set(false); + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(command), "Command should be canceled on falling edge"); + } + + @Test + void whileFalse() { + var signal = new AtomicBoolean(false); + var trigger = new Trigger(m_scheduler, signal::get); + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + trigger.whileFalse(command); + + m_scheduler.run(); + assertTrue( + m_scheduler.isRunning(command), "Command should be scheduled when signal starts low"); + + signal.set(true); + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(command), "Command should be canceled on rising edge"); + } + + @Test + void toggleOnTrue() { + var signal = new AtomicBoolean(false); + var trigger = new Trigger(m_scheduler, signal::get); + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + trigger.toggleOnTrue(command); + + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(command)); + + signal.set(true); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command)); + + signal.set(false); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command)); + + signal.set(true); + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(command)); + } + + @Test + void toggleOnFalse() { + var signal = new AtomicBoolean(false); + var trigger = new Trigger(m_scheduler, signal::get); + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + trigger.toggleOnFalse(command); + + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command)); + + signal.set(true); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command)); + + signal.set(false); + m_scheduler.run(); + assertFalse(m_scheduler.isRunning(command)); + } + + @Test + void commandScoping() { + var innerRan = new AtomicBoolean(false); + var innerSignal = new AtomicBoolean(false); + + var inner = + Command.noRequirements() + .executing( + co -> { + while (true) { + innerRan.set(true); + co.park(); + } + }) + .named("Inner"); + + var outer = + Command.noRequirements() + .executing( + co -> { + new Trigger(m_scheduler, innerSignal::get).onTrue(inner); + // If we yield, then the outer command exits and immediately cancels the + // on-deck inner command before it can run + co.park(); + }) + .named("Outer"); + + m_scheduler.schedule(outer); + m_scheduler.run(); + assertFalse(innerRan.get(), "The bound command should not run before the signal is set"); + + innerSignal.set(true); + m_scheduler.run(); + assertTrue(innerRan.get(), "The nested trigger should have run the bound command"); + + innerRan.set(false); + m_scheduler.run(); + assertFalse(innerRan.get(), "Trigger should not have fired again"); + } + + @Test + void scopeGoingInactiveCancelsBoundCommand() { + var activeScope = new AtomicBoolean(true); + BindingScope scope = activeScope::get; + + var triggerSignal = new AtomicBoolean(false); + var trigger = new Trigger(m_scheduler, triggerSignal::get); + + var command = Command.noRequirements().executing(Coroutine::park).named("Command"); + trigger.addBinding(scope, BindingType.RUN_WHILE_HIGH, command); + + triggerSignal.set(true); + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(command), "Command should have started when triggered"); + + activeScope.set(false); + m_scheduler.run(); + assertFalse( + m_scheduler.isRunning(command), + "Command should have been canceled when scope became inactive"); + } + + // The scheduler lifecycle polls triggers at the start of `run()` + // Even though the trigger condition is set, the command exits and the trigger's scope goes + // inactive before the next `run()` call can poll the trigger + @Test + void triggerFromExitingCommandDoesNotFire() { + var condition = new AtomicBoolean(false); + var triggeredCommandRan = new AtomicBoolean(false); + + var inner = + Command.noRequirements() + .executing( + co -> { + triggeredCommandRan.set(true); + co.park(); + }) + .named("Inner"); + + var awaited = + Command.noRequirements() + .executing( + co -> { + co.yield(); + condition.set(true); + }) + .named("Awaited"); + + var outer = + Command.noRequirements() + .executing( + co -> { + new Trigger(m_scheduler, condition::get).onTrue(inner); + co.await(awaited); + }) + .named("Outer"); + + m_scheduler.schedule(outer); + + // First run: schedules `awaited`, yields + m_scheduler.run(); + assertTrue(m_scheduler.isRunning(outer)); + assertTrue(m_scheduler.isRunning(awaited)); + assertEquals( + List.of("Outer", "Awaited"), + m_scheduler.getRunningCommands().stream().map(Command::name).toList()); + + // Second run: `awaited` resumes, sets the condition, exits. `outer` exits its final `yield` + // and will exit on the next run. The trigger condition has been set, but the trigger is checked + // on the next call to `run()` + m_scheduler.run(); + assertEquals(List.of(), m_scheduler.getRunningCommands().stream().map(Command::name).toList()); + assertTrue(condition.get(), "Condition wasn't set"); + assertFalse(triggeredCommandRan.get(), "Command was unexpectedly triggered"); + + // Third run: trigger binding fires (outside a running command) and queues up `inner`. + // However, the inner command's lifetime is bound to `outer`, and is immediately canceled before + // it can run when the outer command exits. + m_scheduler.run(); + assertEquals(List.of(), m_scheduler.getRunningCommands().stream().map(Command::name).toList()); + assertFalse(triggeredCommandRan.get(), "Command was unexpectedly triggered"); + } +} diff --git a/design-docs/commands-v3.md b/design-docs/commands-v3.md new file mode 100644 index 0000000000..10e77699f8 --- /dev/null +++ b/design-docs/commands-v3.md @@ -0,0 +1,420 @@ +# WPILib Commands Version 3 + +[TOC] + +## Problem Statement {#problem-statement} + +The WPILib command framework uses cooperative multitasking to execute many commands concurrently; +each command has its own `execute()` function called periodically by the scheduler’s `run()` loop. +This API share has a steep learning curve, since new students learn looping by writing their own +`for` or `while` loops - not by using a framework that does the looping for them. Programmers +unfamiliar with the command framework commonly run into a problem where they write a while loop +inside their command’s `execute()` function, thus stalling the scheduler (and, by extension, the +entire robot program) by never ceding control until its end condition is met. + +Version 3 of the commands framework proposes to fix this issue by leveraging a new API introduced in +Java 21: continuations. While they are currently an internal JDK API used to underpin virtual thread +runtimes, we can still access and use it. Continuations are used by the JDK to take and save a +snapshot of a mounted function’s call stack and registers, and can be remounted later to continue +from where it left off; a canceled command would simply never be remounted, and its continuation +left for the garbage collector to clean up. + +## Goals + +### Make command logic look like normal functions + +The hardest part of learning the v1 and v2 command frameworks is the splitting out of logic into +separate init/execute/end/isFinished methods. Coroutines allow us to merge those all back into a +single function: + +```java +void commandBody(Coroutine coroutine) { + initialize(); + while (!isFinished()) { + execute(); + coroutine.yield(); + } + end(); +} +``` + +Coroutines allow commands to be normal methods that yield control back to the scheduler mid-method; +the scheduler will issue a continuation to each command, and the JVM can unmount a continuation - +saving the stack and registers - which can be remounted later. **The primary goal of the new +framework is to allow for commands to be written as normal methods by taking advantage of this +mount/unmount feature.** Everything else is focused on quality of life improvements to the +framework. + +### Forced Naming + +The v2 commands framework makes naming opt-in; it is very easy to forget to set a name on a command +in a factory method, making telemetry less useful than it should be; seeing a +“SequentialCommandGroup” in logs isn’t very useful. The v3 framework will make command names +required in order for commands to be constructed. See [Telemetry](#telemetry) for details. + +### Uncommanded Behavior + +Command groups - or, more generally, commands that schedule other commands - need to own all +mechanisms needed by all their inner commands. However, if they don’t *directly* control a mechanism, +then they will still own the mechanism whenever they’re not running an inner command that uses it, +causing the mechanism to be in an *uncommanded state*. This can cause unexpected behavior with +parallel groups where inner commands don’t all end at the same time, and especially with sequential +groups. For example, something as seemingly basic as `elevator.toL4().andThen(coral.score())` would +*own* the elevator and coral mechanisms for the entire sequence, but not control the elevator during +the coral scoring section, nor control the coral scoring mechanism while the elevator is moving. + +The v2 framework worked around this problem with so-called proxy commands, which have no +requirements of their own and simply schedule and await the *real* command. This removed the +requirement for their used subsystems from the parent group, and allowed for the subsystem’s default +command to run after the proxied command completed. It also meant that *every* command that required +that subsystem would also need to be proxied; otherwise, the group would still have ownership of the +subsystem, and the proxied command would interrupt it (and thus itself). + +The v3 framework allows nested commands to use mechanisms not required by the parent command; +however, the built-in parallel and sequential groups will take full ownership of all mechanisms +required by their nested commands for safety and to keep behavior parity with previous command +frameworks. + +```java +public Command outerCommand() { + // The outer command only requires the outer mechanism + return run(coroutine -> { + // But can schedule a command that requires any other mechanism + coroutine.await(innerMechanism.innerCommand()); + // And only requires that inner mechanism while its command runs + coroutine.await(otherMechanism().otherCommand()); + }).named("Outer"); +} +``` + +Scheduling a command that requires an inner mechanism will also cancel its parent, even if the parent +does not require that inner mechanism. Using the above example, directly scheduling a command that +requires `innerMechanism` will cancel a running `outerCommand` - but only if `outerCommand` is +*currently using* the inner mechanism at the time. + +**Effectively, all child commands in v3 are "proxied"**, using the v2 framework's definition, unless +using the built-in ParallelGroup and Sequence compositions or explicitly adding child command +requirements to the parent. However, child commands _cannot_ interrupt their parent, even if they +share requirements, unlike proxy commands in v2. + +### Priority Levels + +Priority levels allow for finer-grained control over when commands should be interruptible than a +binary interrupt/ignore interrupt flag. This can be particularly helpful for LED control, where +teams use lights to indicate robot activity or error states with error indicators taking priority. + +### Suspend/Resume + +Allow commands to be temporarily paused while a higher priority or interrupting command runs, then +be automatically resumed after that higher priority command finishes. Note that resuming commands +must check for a condition to determine if they should still be running, since the time between +pause and resume can be arbitrarily long. + +Suspending a command will suspend all its children, regardless of if those children have declared +themselves suspendable or not. Likewise, cancelling a non-suspendable command will also cancel all +its children, even if some of those children are suspendable. + +Suspending a command that already has suspended children will still suspend everything; however, +upon resume, those suspended children will remain suspended. + +### Inner Triggers + +Teams often want to use triggers only within the scope of certain commands. For example, running an +autonomous mode that follows a particular path, and do certain actions when various points along the +path are reached, but not have those actions be bound to globally visible triggers that may fire at +other times during a match. In v2, such triggers need to be composed either a with a function that +checks if a valid autonomous mode is running or with the same trigger that can cause that routine to +run. This leads to many, many triggers being defined in robot programs and makes it difficult to +understand when all those triggers may be used. + +The v3 framework will track the scopes in which trigger bindings are created, and automatically +delete those bindings (and cancel any running commands bound to them) when their scopes become +inactive. Users who manually schedule a command create a binding in a "global" scope that is always +active. Binding commands to a trigger, however, will use whatever the narrowest available scope is +at the time - creating a binding inside a running command will be scoped to that command's lifetime; +creating a binding outside a command (for example, in a main Robot class constructor) will also use +the "global" scope. + +```java +void bindDriveButtons() { + // Globally bound and will always be active + controller.a().onTrue(...) +} + +Trigger atScoringPosition = new Trigger(() -> getPosition().isNear(kScoringPosition)); + +Command autonomous() { + return Command.noRequirements().executing(coroutine -> { + // This binding only exists while the autonomous command is running + atScoringPosition.onTrue(score()); + + coroutine.await(driveToScoringPosition()); + }).named("Autonomous"); +} +``` + +### Improved Transparency + +The v2 commands framework only runs top-level commands in the scheduler; commands nested within a +composition like a sequential group or a simple `withTimeout` decoration are hidden from the +scheduler and do not appear in its sendable implementation, making telemetry difficult. A +“SequentialCommandGroup” in logs is less useful than “Wait 10 seconds -> Drive Forward”. Command +groups will automatically include the names of all their constituent commands, with options to +override when desired. + +The v3 framework will also provide a map of what command each mechanism is owned by. The v2 framework +only provides a list of the names of the running commands, without mapping those to subsystems. This +also makes telemetry difficult, since the order of commands in the list does not correspond with +subsystems; a command at a particular index may require different subsystems than a different +command at that same index that’s running at a later point in time, making data analysis in +AdvantageScope difficult since we can’t rely on consistent ordering. + +Telemetry will send lists of the on-deck and running commands. Commands in those lists will appear +as an ID number, parent command ID (if any; top level commands have no parent), name, names of +the required mechanisms, priority level, last time to process, and total processing time. Separate +runs of the same command object have different ID numbers and processing time data. The total time +spent in the scheduler loop will also be included, but not the aggregate total of _all_ loops. + +## Non-Goals + +### Preemptive Multitasking + +It is not a goal of the v3 framework to offer preemptive multitasking (that is, having the scheduler +forcibly suspend a long-running command in order to run another queued one). This would require +support from the JDK for allowing custom virtual thread schedulers, which it does not currently +offer, and a custom thread scheduler would be complicated and difficult to maintain. + +### Multithreading Support + +Like the previous iterations of the command-based framework, v3 will be designed for a +single-threaded environment. All commands will be run by a single thread, which is expected to be +the same thread on which commands are scheduled and canceled via triggers. No guarantees are made +for stability or proper functioning if used in a multithreaded environment. + +### Unbounded use of coroutines + +Coroutines are intended to be used within the context of a running command or sideloaded periodic +function, and rely on their backing continuations to be mounted in order to run. Using a coroutine +outside its command makes no sense, and an error will be thrown if attempting to do so: + +```java +Coroutine coroutine; +var badCommand = Command.noRequirements().executing(co -> { + coroutine = co; +}).named("Do not do this"); + +Scheduler.getInstance().schedule(badCommand); +Scheduler.getInstance().run(); + +// Doing anything with a captured coroutine will throw an error +co.fork(...); // IllegalStateException +co.yield(); // IllegalStateException +``` + +## Implementation Details + +### Nomenclature + +**Schedule**: Adding a command to a queue in the scheduler, requesting that that command start +running in the next call to the scheduler’s `run()` method. + +**Mount**: Make a command active by resuming its stack and executing it on the scheduler’s thread. + +**Unmount**: Freeze a command’s stack and state and remove it from the scheduler’s thread. An +unmounted command may be re-mounted in the future. + +**Run**: Adding a command to the scheduler and have it begin executing. Every unfinished command +will be run by the scheduler until they reach a `Coroutine.yield()` call, at which point the command +is unmounted and the next command can begin. + +**Cancel**: Request that a command stop running. A command that is scheduled and hasn’t yet started +running will be removed from the queue of scheduled commands. Cancellation requests are handled at +the end of each `run() `invocation. Cancelled commands are unmounted and will not be re-mounted; +they must be rescheduled and be issued new carrier coroutines. + +**Interrupt**: Scheduling a command that requires one or more mechanisms already in use by a running +command will interrupt and cancel that running command, so long as the running command has an equal +or lower priority level. Higher-priority commands cannot be interrupted by lower-priority ones. + +### Coroutines and Continuations + +`Continuation` and `ContinuationScope` are JDK-internal classes that manage stack saving and +loading (“unmounting” and “mounting”, respectively). Mounting a continuation essentially means +placing the continuation’s saved stack on top of the current stack. Calling +`Continuation.yield(ContinuationScope)` pauses the code that the continuation is evaluating and +cedes control back to the caller, which can then unmount the continuation. The scheduler uses this +by looping over every running command, mounting the continuation for the command, running the +command - which eventually either returns or calls `coroutine.yield()` - then unmounts the +continuation and moves on to the next command in the list. + +Coroutines are essentially wrappers around a continuation primitive that allows additional access to +the scheduler and provides async/await-like functionality. For example, `Coroutine.await(Command)` +will schedule a given command and call `yield()` in a loop until that command has completed +execution (note: this does *not* naively call `command.run(coroutine)`, which would hide the inner +command from the scheduler and potentially sidestep requirement mutexing) + +### Command Function Bodies + +Command logic lives in a single function `run` that accepts a `Coroutine` object argument. All +yielding is done via that coroutine, as is any management of nested commands (scheduling, awaiting, +cancelling, etc). + +The coroutine’s `yield()` method always returns `true`. When a command is cancelled, it is removed +from the set of running commands and its carrier objects left to be garbage collected. An +`onCancel()` method on the cancelled command will be invoked to let the command do any necessary +cleanup; this is needed because the command body will not be re-mounted and run, so a separate +function is used. Command builders allow this to be specified with a `whenCancelled(Runnable)` step. + +### Command Lifetimes + +All commands are managed and executed by the scheduler. No commands should manually run their inner +commands (which is what the old sequential and parallel groups did), since that hides those inner +commands from the scheduler and telemetry. However, this is not prohibited, nor would there be a way +to detect it. + +Inner commands are bound to the lifetime of their parents. Cancelling a higher-level command must +also cancel all the commands it scheduled (and so on down the hierarchy). Suspending a higher-level +command must likewise also suspend all inner commands, and resuming the higher-level command must +resume all suspended inner commands. Inner commands cannot be resumed before their parent command, +but may be suspended while the parent command still runs. + +When a command is running, the scheduler tracks in a wrapper CommandState object that includes the +command, the command that scheduled it (if applicable), and the coroutine object carrying the +command. + +Each command run has its own coroutine to back its execution. When a command finishes, its backing +continuation is done, and the command is removed from the scheduler. Any unfinished inner commands +are cancelled at this point. + +### Factory Methods and Builders + +In contrast with v2’s decorator API that returns a new command object for each modification (which +had some issues with naming and making telemetry difficult with deeply nested wrapper objects), v3 +uses builder objects that allow configuration to be set before creating a single command at the end. +Builders are defined in stages, with a different type representing each stage, in order to leverage +the type system to prevent users from creating invalid commands and only discovering so at runtime. +Failure to apply required configuration options (eg the main command function or a name) will leave +users with an intermediate-stage builder that cannot be used to create a command, resulting in a +compilation error. + +```java +// OK - requirements, body, and name are all provided +// Each builder method returns a different builder object that provides methods +// for progressing to the next stage. +Command command = Command.noRequirements().executing(...).named("Name"); + +// Compilation error! Missing the command body +Command command = Command.noRequirements().named("Name"); + +// Compilation error! Missing the command name +Command command = Command.noRequirements().executing(...); +``` + +#### Forced Naming + +Builder objects will only permit command creation when a name for the command has been provided. + +#### Automatic Naming + +Parallel and sequential groups and builders permit automatically generated names to be used instead +of manually specified ones. + +Automatic sequence names will simply be the list of all the commands in the sequence, separated by +a “->” arrow sign; for example, “Step 1 -> Step 2 -> Step 3” + +Automatic parallel group names will be the list of the names of the commands in the group, separated +by a pipe “|”. Required commands will be in their own group, separated from non-required commands. +If either subgroup is empty, then there will be no indication in the output (ie, no empty group +token like “()” will appear) + +### Cooperative Multitasking + +All commands *must* yield control back to the coroutine in their control loops. Because we cannot +preempt a running command at any arbitrary point, the command must explicitly and deliberately cede +control back to the scheduler in order for the framework to function. Commands may do this by +calling `coroutine.yield()` on the injected coroutine object, which ultimately delegates to +`Continuation.yield()` to pause the carrier continuation object. + +The scheduler has a single `ContinuationScope` object. When a command is mounted, the scheduler will +create a new `Continuation` object bound to that scope, and a `Coroutine` object bound to the +continuation. `ContinuationScope` and `Continuation` are currently JDK-private classes; the build +needs to open the module to the wpilib/unnamed module for reflective access at compile and runtime. +Because access is entirely reflective, wrapper classes at the library level will be used to make +access easier. + +### Scheduling + +Scheduling a command with `Scheduler.schedule()` will add the command to a queue of pending +commands. Any command in the queue that conflicts and has a lower priority will be removed from the +queue without calling the `onCancel` hook. At the start of `Scheduler.run()`, all commands that +conflict with those in the queue are cancelled, then the queued commands are promoted to running. + +Scheduling an inner command will bypass the queue and immediately begin to run. This avoids delays +caused by loop timings for deeply nested commands. + +### Scheduler `run()` Cycle + +1. Run periodic sideload functions +2. Poll the event loop for triggers (which may queue or cancel commands) +3. Schedule default commands for the next iteration to pick up and start running +4. Promote scheduled commands to running + 1. Cancels any running commands that conflict with scheduled ones +5. Iterate over running commands + 1. Mount + 2. Run until yield point is reached or an error is raised + 3. Unmount + 4. Evict if the command finished + 1. Any inner command still running is canceled + +### Interruption + +* Caused by scheduling a command that requires mechanisms already in use by one or more running + commands +* Results in cancellation of the conflicting commands + * Commands are moved to a pending-cancellation state + * Then when `run()` is next called, they are removed from the scheduler + * This occurs prior to the scheduled commands being promoted to the running state + * The conflicting command’s entire tree (all ancestors and children) will be cancelled +* Siblings in a composition can interrupt each other + * eg `fork(command1); fork(command2)`, where `command1` and `command2` share at least one + common requirement, would cause `command1` to be interrupted by `command2` + +### Telemetry + +Scheduler state is serialized using protobuf. The scheduler will send a list of the currently queued +commands and a list of the current running commands. Commands are serialized as (id: uint32, +parent_id: uint32, name: string, priority: int32, requirements: string array, +last_time_ms: double, total_time_ms: double). Consumers can use the `id` and `parent_id` attributes +to reconstruct the tree structure, if desired. `id` and `parent_id` marginally increase the size of +serialized data, but make the schema and deserialization quite simple. + +Command IDs are unique across all commands, including those that are not currently running. Every time +a command is scheduled, the scheduler will assign that run a unique ID. Note that this is stored as a +32-bit signed integer, so the maximum number of unique IDs is 2^31. No guarantee is made that IDs will +be unique across runs of the same program, nor to the order in which ID numbers are assigned; however, +IDs will be issued in a monotonically increasing fashion - a command scheduled before another will +always have a lower ID number. + +Records in the serialized output will be ordered by scheduling order. As a result, child commands +will always appear _after_ their parent. + +For example, if a scheduler is running a command like this: + +```java +Mechansism m1, m2; + +Command theCommand() { + return ParallelGroup.all( + m1.run(/* ... */).withPriority(1).named("Command 1"), + m2.run(/* ... */).withPriority(2).named("Command 2") + ).withAutomaticName(); +} +``` + +Telemetry for commands in the scheduler would look like: + +| `id` | `parent_id` | `name` | `priority` | `requirements` | `last_time_ms` | `total_time_ms` | +|--------|-------------|---------------------------|------------|----------------|----------------|-----------------| +| 347123 | -- | "(Command 1 & Command 2)" | 2 | ["M1", "M2"] | 0.210 | 5.122 | +| 998712 | 347123 | "Command 1" | 1 | ["M1"] | 0.051 | 1.241 | +| 591564 | 347123 | "Command 2" | 2 | ["M2"] | 0.108 | 3.249 | diff --git a/docs/BUILD.bazel b/docs/BUILD.bazel index 3153b0234f..06f5acdcee 100644 --- a/docs/BUILD.bazel +++ b/docs/BUILD.bazel @@ -106,6 +106,7 @@ javadoc( deps = [ "//apriltag:apriltag-java", "//cameraserver:cameraserver-java", + "//commandsv3:commandsv3-java", "//cscore:cscore-java", "//epilogue-runtime:epilogue-java", "//hal:hal-java", diff --git a/docs/build.gradle b/docs/build.gradle index 65b758674f..489424de2b 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -5,6 +5,7 @@ plugins { evaluationDependsOn(':apriltag') evaluationDependsOn(':cameraserver') +evaluationDependsOn(':commandsv3') evaluationDependsOn(':cscore') evaluationDependsOn(':epilogue-runtime') evaluationDependsOn(':hal') @@ -161,6 +162,7 @@ task generateJavaDocs(type: Javadoc) { "-edu.wpi.first.math.system.plant.struct," + "-edu.wpi.first.math.trajectory.proto," + "-edu.wpi.first.math.trajectory.struct," + + "-org.wpilib.commands3.proto," + // The .measure package contains generated source files for which automatic javadoc // generation is very difficult to do meaningfully. "-edu.wpi.first.units.measure", true) @@ -170,6 +172,7 @@ task generateJavaDocs(type: Javadoc) { dependsOn project(':wpilibj').generateJavaVersion source project(':apriltag').sourceSets.main.java source project(':cameraserver').sourceSets.main.java + source project(':commandsv3').sourceSets.main.java source project(':cscore').sourceSets.main.java source project(':epilogue-runtime').sourceSets.main.java source project(':hal').sourceSets.main.java diff --git a/settings.gradle b/settings.gradle index efcb9caf55..f78ae8bd11 100644 --- a/settings.gradle +++ b/settings.gradle @@ -48,6 +48,7 @@ include 'simulation:halsim_xrp' include 'cameraserver' include 'cameraserver:multiCameraServer' include 'wpilibNewCommands' +include 'commandsv3' include 'romiVendordep' include 'xrpVendordep' include 'developerRobot' diff --git a/styleguide/checkstyle-suppressions.xml b/styleguide/checkstyle-suppressions.xml index 0ff41a1b98..8254e2b628 100644 --- a/styleguide/checkstyle-suppressions.xml +++ b/styleguide/checkstyle-suppressions.xml @@ -10,8 +10,9 @@ suppressions PUBLIC "-//Puppy Crawl//DTD Suppressions 1.1//EN" checks="(LocalVariableName|MemberName|MethodName|MethodTypeParameterName|ParameterName)" /> - + + diff --git a/styleguide/pmd-ruleset.xml b/styleguide/pmd-ruleset.xml index 985db64620..c97ab738f4 100644 --- a/styleguide/pmd-ruleset.xml +++ b/styleguide/pmd-ruleset.xml @@ -15,6 +15,7 @@ .*/*IntegrationTests.* .*/*JNI.* .*/math/proto.* + .*/commands3/proto.* diff --git a/styleguide/spotbugs-exclude.xml b/styleguide/spotbugs-exclude.xml index bdebdc55f3..ac00f14803 100644 --- a/styleguide/spotbugs-exclude.xml +++ b/styleguide/spotbugs-exclude.xml @@ -205,6 +205,11 @@ + + + + + diff --git a/wpilib-config.cmake.in b/wpilib-config.cmake.in index 30080f24d9..1c3bb90ac6 100644 --- a/wpilib-config.cmake.in +++ b/wpilib-config.cmake.in @@ -8,6 +8,7 @@ find_dependency(Threads) @APRILTAG_DEP_REPLACE@ @CAMERASERVER_DEP_REPLACE@ @CSCORE_DEP_REPLACE@ +@COMMANDSV3_DEP_REPLACE@ @DATALOG_DEP_REPLACE@ @HAL_DEP_REPLACE@ @NTCORE_DEP_REPLACE@ diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandSchedulerTest.java similarity index 99% rename from wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java rename to wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandSchedulerTest.java index 0a6eb3df5c..2d4a3e4ab9 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandSchedulerTest.java @@ -13,7 +13,7 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import java.util.concurrent.atomic.AtomicInteger; import org.junit.jupiter.api.Test; -class SchedulerTest extends CommandTestBase { +class CommandSchedulerTest extends CommandTestBase { @Test void schedulerLambdaTestNoInterrupt() { try (CommandScheduler scheduler = new CommandScheduler()) {