Squashed 'third_party/allwpilib/' changes from f1a82828fe..ce550705d7
ce550705d7 [ntcore] Fix client "received unknown id -1" (#6186)
3989617bde [ntcore] NetworkTable::GetStruct: Add I template param (#6183)
f1836e1321 [fieldImages] Fix 2024 field json (#6179)
d05f179a9a [build] Fix running apriltagsvision Java example (#6173)
b1b03bed85 [wpilib] Update MotorControllerGroup deprecation message (#6171)
fa63fbf446 LICENSE.md: Bump year to 2024 (#6169)
4809f3d0fc [apriltag] Add 2024 AprilTag locations (#6168)
dd90965362 [wpiutil] Fix RawFrame.setInfo() NPE (#6167)
8659372d08 [fieldImages] Add 2024 field image (#6166)
a2e4d0b15d [docs] Fix docs for SysID routine (#6164)
0a46a3a618 [wpilib] Make ADXL345 default I2C address public (#6163)
7c26bc70ab [sysid] Load DataLog files directly for analysis (#6103)
f94e3d81b9 [docs] Fix SysId routine JavaDoc warnings (#6159)
6bed82a18e [wpilibc] Clean up C++ SysId routine (#6160)
4595f84719 [wpilib] Report LiveWindow-enabled-in-test (#6158)
707cb06105 [wpilib] Add SysIdRoutine logging utility and command factory (#6033)
3e40b9e5da [wpilib] Correct SmartDashboard usage reporting (#6157)
106518c3f8 [docs] Fix wpilibj JavaDoc warnings (#6154)
19cb2a8eb4 [wpilibj] Make class variables private to match C++ (#6153)
13f4460e00 [docs] Add missing docs to enum fields (NFC) (#6150)
4210f5635d [docs] Fix warnings about undocumented default constructors (#6151)
0f060afb55 [ntcore] Disable WebSocket fragmentation (#6149)
f29a7d2e50 [docs] Add missing JavaDocs (#6146)
6e58db398d [commands] Make Java fields private (#6148)
4ac0720385 [build] Clean up CMake files (#6141)
44db3e0ac0 [sysid] Make constexpr variables outside class scope inline (#6145)
73c7d87db7 [glass] NTStringChooser: Properly set retained (#6144)
25636b712f [build] Remove unnecessary native dependencies in wpilibjExamples (#6143)
01fb98baaa [docs] Add Missing JNI docs from C++ (NFC) (#6139)
5c424248c4 [wpilibj] Remove unused AnalogTriggerException (#6142)
c486972c55 [wpimath] Make ExponentialProfile.State mutable (#6138)
783acb9b72 [wpilibj] Store long preferences as integers (#6136)
99ab836894 [wpiutil] Add missing JavaDocs (NFC) (#6132)
ad0859a8c9 [docs] Add missing JavaDocs (#6125)
5579219716 [docs] Exclude quickbuf files and proto/struct packages from doclint (#6128)
98f06911c7 [sysid] Use eigenvector component instead of eigenvalue for fit quality check (#6131)
e1d49b975c [wpimath] Add LinearFilter reset() overload to initialize input and output buffers (#6133)
8a0bf2b7a4 [hal] Add CANAPITypes to java (#6121)
91d8837c11 [wpilib] Make protected fields in accelerometers/gyros private (#6134)
e7c9f27683 [wpilib] Add functional interface equivalents to MotorController (#6053)
8aca706217 [glass] Add type information to SmartDashboard menu (#6117)
7d3e4ddba9 [docs] Add warning about using user button to docs (NFC) (#6129)
ec3cb3dcba [build] Disable clang-tidy warning about test case names (#6127)
495585b25d [examples] Update april tag family to 36h11 (#6126)
f9aabc5ab2 [wpilib] Throw early when EventLoop is modified while running (#6115)
c16946c0ec [hal] Add CANJNI docs (NFC) (#6120)
b7f4eb2811 [doc] Update maven artifacts for units and apriltags (NFC) (#6123)
f419a62b38 [doc] Update maintainers.md (NFC) (#6124)
938bf45fd9 [wpiutil] Remove type param from ProtobufSerializable and StructSerializable (#6122)
c34debe012 [docs] Link to external OpenCV docs (#6119)
07183765de [hal] Fix formatting of HAL_ENUM enums (NFC) (#6114)
af46034b7f [wpilib] Document only first party controllers are guaranteed to have correct mapping (#6112)
636ef58d94 [hal] Properly error check readCANStreamSession (#6108)
cc631d2a69 [build] Fix generated source set location in the HAL (#6113)
09f76b32c2 [wpimath] Compile with UTF-8 encoding (#6111)
47c5fd8620 [sysid] Check data quality before OLS (#6110)
24a76be694 [hal] Add method to detect if the CAN Stream has overflowed (#6105)
9333951736 [hal] Allocate CANStreamMessage in JNI if null (#6107)
6a2d3c30a6 [wpiutil] Struct: Add info template parameter pack (#6086)
e07de37e64 [commands] Mark ParallelDeadlineGroup.setDeadline() final (#6102)
141241d2d6 [wpilib] Fix usage reporting for static classes (#6090)
f2c2bab7dc [sysid] Fix adjusted R² calculation (#6101)
5659038443 [wpiutil,cscore,apriltag] Fix RawFrame (#6098)
8aeee03626 [commands] Improve error message when composing commands twice in same composition (#6091)
55508706ff [wpiutil,cscore] Move VideoMode.PixelFormat to wpiutil (#6097)
ab78b930e9 [wpilib] ADIS16470: Add access to all 3 axes (#6074)
795d4be9fd [wpilib] Fix precision issue in Color round-and-clamp (#6100)
7aa9ad44b8 [commands] Deprecate C++ TransferOwnership() (#6095)
92c81d0791 [ci] Update pregenerate workflow to actions/checkout@v4 (#6094)
1ce617be07 [ci] Update artifact actions to v4 (#6092)
2441b57156 [wpilib] Add PWMSparkFlex MotorController (#6089)
21d1972d7a [wpiutil] DataLog: Ensure file is written on shutdown (#6087)
c29e8c66cf [wpiutil] DataLog: Fix UB in AppendImpl (#6088)
ab309e34ef [glass] Fix order of loading window settings (#6056)
22a322c9f3 [wpimath] Report error on negative PID gains (#6055)
1dba26c937 [wpilib] Add method to get breaker fault at a specific channel in PowerDistribution[Sticky]Faults (#5521)
ef1cb3f41e [commands] Fix compose-while-scheduled issue and test all compositions (#5581)
aeb1a4aa33 [wpiutil] Add serializable marker interfaces (#6060)
c1178d5add [wpilib] Add StadiaController and command wrapper (#6083)
4e4a468d4d [wpimath] Make feedforward classes throw exceptions for negative Kv or Ka (#6084)
d1793f077d [build] cmake: Add NO_WERROR option to disable -Werror (#6071)
43fb6e9f87 [glass] Add Profiled PID controller support & IZone Support (#5959)
bcef6c5398 [apriltag] Fix Java generation functions (#6063)
4059e0cd9f [hal,wpilib] Add function to control "Radio" LED (#6073)
0b2cfb3abc [dlt] Change datalogtool default folder to logs folder (#6079)
df5e439b0c [wpilib] PS4Controller: enable usage reporting (#6081)
0ff7478968 [cscore] Fix RawFrame class not being loaded in JNI (#6077)
6f23d32fe1 [wpilib] AddressableLED: Update warning about single driver (NFC) (#6069)
35a1c52788 [build] Upgrade quickbuf to 1.3.3 (#6072)
e4e2bafdb1 [sysid] Document timestamp units (#6065)
3d201c71f7 [ntcore] Fix overlapping subscriber handling (#6067)
f02984159f [glass] Check for null entries when updating struct/proto (#6059)
a004c9e05f [commands] SubsystemBase: allow setting name in constructor (#6052)
0b4c6a1546 [wpimath] Add more docs to SimulatedAnnealing (NFC) (#6054)
ab15dae887 [wpilib] ArcadeDrive: Fix max output handling (#6051)
9599c1f56f [hal] Add usage reporting ids from 2024v2 image (#6041)
f87c64af8a [wpimath] MecanumDriveWheelSpeeds: Fix desaturate() (#6040)
8798700cec [wpilibcExamples] Add inline specifier to constexpr constants (#6049)
85c9ae6eff [wpilib] Fix PS5 Controller mappings (#6050)
7c8b7a97ad [wpiutil] Zero out roborio system timestamp (#6042)
d9b504bc84 [wpilib] DataLogManager: Change sim location to logs subdir (#6039)
906b810136 [build] cmake: Fix ntcore generated header install (#6038)
56e5b404d1 Update to final 2024 V2 image (#6034)
8723ee5c39 [ntcore] Add cached topic property (#5494)
192a28af47 Fix JDK 21 warnings (#6028)
d40bdd70ba [build] Upgrade to spotbugs Gradle plugin 6.0.2 (#6027)
7bfadf32e5 [wpilibj] Joystick: make remainder of get axis methods final (#6024)
a770110438 [commands] CommandCompositionError: Include stacktrace of original composition (#5984)
54a55b8b53 [wpiutil,hal] Update image; init Rio Now() HMB with a FPGA session (#6016)
7d4e515a6b [wpimath] Simplify calculation of C for DARE precondition (#6022)
5200316c14 [ntcore] Update transmit period on topic add/remove (#6021)
ddf79a25d4 [wpiunits] Overload Measure.per(Time) to return Measure<Velocity> (#6018)
a71adef316 [wpiutil] Clean up circular_buffer iterator syntax (#6020)
39a0bf4b98 [examples] Call resetOdometry() when controller command is executed (#5905)
f5fc101fda [build] cmake: Export jars and clean up jar installs (#6014)
38bf024c96 [build] Update to Gradle 8.5 (#6007)
9d11544c18 [wpimath] Rotate traveling salesman solution so input and solution have same initial pose (#6015)
28deba20f5 [wpimath] Commit generated quickbuf Java files (#5994)
c2971c0bb3 [build] cmake: Export apriltag and wpimath (#6012)
41cfc961e4 gitattributes: Add linguist-generated locations (#6004)
14c3ade155 [wpimath] Struct cleanup (#6011)
90757b9e90 [wpilib] Make Color::HexString() constexpr (#5985)
2676b77873 Fix compilation issues that occur when building with bazel (#6008)
d32c10487c [examples] Update C++ examples to use CommandPtr (#5988)
9bc5fcf886 [build] cmake: Default WITH_JAVA_SOURCE to WITH_JAVA (#6005)
d431abba3b [upstream_utils] Fix GCEM namespace usage and add hypot(x, y, z) (#6002)
2bb1409b82 Clean up Java style (#5990)
66172ab288 Remove submodule (#6003)
e8f8c0ceb0 [upstream_utils] Update to latest Eigen HEAD (#5996)
890992a849 [hal] Commit generated usage reporting files (#5993)
a583ca01e1 [wpiutil] Change Struct to allow non-constexpr implementation (#5992)
ca272de400 [build] Fix Gradle compile_commands.json and clang-tidy warnings (#5977)
76ae090570 [wpiutil] type_traits: Add is_constexpr() (#5997)
5172ab8fd0 [commands] C++ CommandPtr: Prevent null initialization (#5991)
96914143ba [build] Bump native-utils to fix compile_commands.json (#5989)
464e6121ef [ci] Report failed status to Azure on failed tests (#2654)
5dad46cd45 [wpimath] Commit generated files (#5986)
54ab65a63a [ntcore] Commit generated files (#5962)
7ed900ae3a [wpilib] Add hex string constructor to Color and Color8Bit (#5063)
74b85b76a9 [wpimath] Make gcem call std functions if not constant-evaluated (#5983)
30816111db [wpimath] Fix TimeInterpolatableBuffer crash (#5972)
5cc923de33 [wpilib] DataLogManager: Use logs subdirectory on USB drives (#5975)
1144115da0 [commands] Add GetName to Subsystem, use in Scheduler tracer epochs (#5836)
ac7d726ac3 [wpimath] Add simulated annealing (#5961)
e09be72ee0 [wpimath] Remove unused SimpleMatrixUtils class (#5979)
0f9ebe92d9 [wpimath] Add generic circular buffer class to Java (#5969)
9fa28eb07a [ci] Bump actions/checkout to v4 (#5736)
ca684ac207 [hal] Add capability to read power distribution data as a stream (#4983)
51eecef2bd [wpimath] Optimize 2nd derivative of quintic splines (#3292)
4fcf0b25a1 [build] Apply a formatter for CMake files (#5973)
9b8011aa67 [build] Pin wpiformat version (#5982)
e00a0e84c1 [build] cmake: fix protobuf dependency finding for certain distributions (#5981)
23dd591394 [upstream_utils] Remove libuv patch that adjusts whitespace (#5976)
b0719942f0 [wpiutil] Timestamp: Report errors on Rio HMB init failure (#5974)
7bc89c4322 [wpilib] Update getAlliance() docs (NFC) (#5971)
841ea682d1 [upstream_utils] Upgrade to LLVM 17.0.5 (#5970)
a74db52dae [cameraserver] Add getVideo() pixelFormat overload (#5966)
a7eb422662 [build] Update native utils for new compile commands files (#5968)
544b231d4d [sysid] Add missing cassert include (#5967)
31cd015970 [wpimath] Add SysId doc links to LinearSystemId in C++ (NFC) (#5960)
9280054eab Revert "[build] Export wpimath protobuf symbols (#5952)"
2aba97c610 Export pb files from wpimath
c80b2d2017 [build] Export wpimath protobuf symbols (#5952)
3c0652c18a [cscore] Replace CS_PixelFormat with WPI_PixelFormat (#5954)
95716eb0cb [wpiunits] Documentation improvements (#5932)
423fd75fa8 [wpilib] Default LiveWindowEnabledInTest to false (#5950)
dfdea9c992 [wpimath] Make KalmanFilter variant for asymmetric updates (#5951)
ca81ced409 [wpiutil] Move RawFrame to wpiutil; add generation of RawFrame for AprilTags (#5923)
437cc91af5 [cscore] CvSink: Allow specifying output PixelFormat (#5943)
25b7dca46b [build] Remove CMake flat install option (#5944)
bb05e20247 [wpimath] Add protobuf/struct for trivial types (#5935)
35744a036e [wpimath] Move struct/proto classes to separate files (#5918)
80d7ad58ea [build] Declare platform launcher dependency explicitly (#5909)
f8d983b154 [ntcore] Protobuf/Struct: Use atomic_bool instead of atomic_flag (#5946)
4a44210ee3 [ntcore] NetworkTableInstance: Suppress unused lambda capture warning (#5947)
bdc8620d55 [upstream_utils] Fix fmt compilation errors on Windows (#5948)
0ca1e9b5f9 [wpimath] Add basic wpiunits support (#5821)
cc30824409 [ntcore] Increase client meta-topic decoding limit (#5934)
b1fad062f7 [wpilib] Use RKDP in DifferentialDrivetrainSim (#5931)
ead9ae5a69 [build] Add generateProto dependency to test and dev (#5933)
cfbff32185 [wpiutil] timestamp: Fix startup race on Rio (#5930)
7d90d0bcc3 [wpimath] Clean up StateSpaceUtil (#5891)
7755e45aac [build] Add generated protobuf headers to C++ test include path (#5926)
3985c031da [ntcore] ProtobufSubscriber: Fix typos (#5928)
7a87fe4b60 [ntcore] ProtobufSubscriber: Make mutex and msg mutable (#5927)
09f3ed6a5f [commands] Add static Trigger factories for robot mode changes (#5902)
79dd795bc0 [wpimath] Clean up VecBuilder and MatBuilder (#5906)
e117274a67 [wpilib] Change default Rio log dir from /home/lvuser to /home/lvuser/logs (#5899)
a8b80ca256 [upstream_utils] Update to libuv 1.47.0 (#5889)
b3a9c3e96b [build] Bump macOS deployment target to 12 (#5890)
0f8129677b [build] Distribute wpimath protobuf headers (#5925)
d105f9e3e9 [wpiutil] ProtobufBuffer: Fix buffer reallocation (#5924)
c5f2f6a0fb [fieldImages] Fix typo in field images artifact name (#5922)
c1a57e422a [commands] Clean up make_vector.h (#5917)
78ebc6e9ec [wpimath] change G to gearing in LinearSystemId factories (#5834)
9ada181866 [hal] DriverStation.h: Add stddef.h include (#5897)
95fa5ec72f [wpilibc,ntcoreffi] DataLogManager: join on Stop() call (#5910)
b6f2d3cc14 [build] Remove usage of Version.parse (#5911)
cc2cbeb04c [examples] Replace gyro rotation with poseEstimator rotation (#5900)
fa6b171e1c [wpiutil] Suppress protobuf warning false positives on GCC 13 (#5907)
d504639bbe [apriltag] Improve AprilTag docs (#5895)
3a1194be40 Replace static_cast<void>() with [[maybe_unused]] attribute (#5892)
70392cbbcb [build] cmake: Add protobuf dependency to wpiutil-config (#5886)
17c1bd5a83 [ntcore] Use json_fwd (#5881)
e69a9efeba [wpilibcExamples] Match array parameter bounds (#5880)
14dcd0d26f Use char instead of uint8_t for json::parse (#5877)
ec1d261984 [hal] Fix garbage data for match info before DS connection (#5879)
63dbf5c614 [wpiutil] MemoryBuffer: Fix normal read and file type check (#5875)
b2e7be9250 [ntcore] Only datalog meta-topics if specifically requested (#5873)
201a42a3cd [wpimath] Reorder TrapezoidProfile.calculate() arguments (#5874)
04a781b4d7 [apriltag] Add GetTags to C++ version of AprilTagFieldLayout (#5872)
87a8a1ced4 [docs] Exclude eigen and protobuf from doxygen (#5871)
git-subtree-dir: third_party/allwpilib
git-subtree-split: ce550705d7cdab117c0153a202973fc026a81274
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ic8645d0551d62b411b0a816c493f0f33291896a1
diff --git a/wpilibNewCommands/CMakeLists.txt b/wpilibNewCommands/CMakeLists.txt
index 34a6bb1..4b42aa5 100644
--- a/wpilibNewCommands/CMakeLists.txt
+++ b/wpilibNewCommands/CMakeLists.txt
@@ -4,40 +4,55 @@
include(CompileWarnings)
include(AddTest)
-if (WITH_JAVA)
- find_package(Java REQUIRED)
- include(UseJava)
- set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+if(WITH_JAVA)
+ find_package(Java REQUIRED)
+ include(UseJava)
+ set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
- file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
- add_jar(wpilibNewCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibNewCommands)
+ file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+ add_jar(
+ wpilibNewCommands_jar
+ ${JAVA_SOURCES}
+ INCLUDE_JARS
+ hal_jar
+ ntcore_jar
+ cscore_jar
+ cameraserver_jar
+ wpimath_jar
+ wpiunits_jar
+ wpiutil_jar
+ wpilibj_jar
+ OUTPUT_NAME wpilibNewCommands
+ )
- get_property(WPILIBNEWCOMMANDS_JAR_FILE TARGET wpilibNewCommands_jar PROPERTY JAR_FILE)
- install(FILES ${WPILIBNEWCOMMANDS_JAR_FILE} DESTINATION "${java_lib_dest}")
-
- set_property(TARGET wpilibNewCommands_jar PROPERTY FOLDER "java")
-
- if (WITH_FLAT_INSTALL)
- set (wpilibNewCommands_config_dir ${wpilib_dest})
- else()
- set (wpilibNewCommands_config_dir share/wpilibNewCommands)
- endif()
+ install_jar(wpilibNewCommands_jar DESTINATION ${java_lib_dest})
+ install_jar_exports(
+ TARGETS wpilibNewCommands_jar
+ FILE wpilibNewCommands_jar.cmake
+ DESTINATION share/wpilibNewCommands
+ )
endif()
-if (WITH_JAVA_SOURCE)
- find_package(Java REQUIRED)
- include(UseJava)
- file(GLOB WPILIBNEWCOMMANDS_SOURCES src/main/java/edu/wpi/first/wpilibj2/command/*.java)
- file(GLOB WPILIBNEWCOMMANDS_BUTTON_SOURCES src/main/java/edu/wpi/first/wpilibj2/command/button*.java)
- add_jar(wpilibNewCommands_src_jar
- RESOURCES NAMESPACE "edu/wpi/first/wpilibj2/command" ${WPILIBNEWCOMMANDS_SOURCES}
- NAMESPACE "edu/wpi/first/wpilibj2/command/button" ${WPILIBNEWCOMMANDS_BUTTON_SOURCES}
- OUTPUT_NAME wpilibNewCommands-sources)
+if(WITH_JAVA_SOURCE)
+ find_package(Java REQUIRED)
+ include(UseJava)
+ file(GLOB WPILIBNEWCOMMANDS_SOURCES src/main/java/edu/wpi/first/wpilibj2/command/*.java)
+ file(
+ GLOB WPILIBNEWCOMMANDS_BUTTON_SOURCES
+ src/main/java/edu/wpi/first/wpilibj2/command/button*.java
+ )
+ add_jar(
+ wpilibNewCommands_src_jar
+ RESOURCES
+ NAMESPACE "edu/wpi/first/wpilibj2/command" ${WPILIBNEWCOMMANDS_SOURCES}
+ NAMESPACE "edu/wpi/first/wpilibj2/command/button" ${WPILIBNEWCOMMANDS_BUTTON_SOURCES}
+ OUTPUT_NAME wpilibNewCommands-sources
+ )
- get_property(WPILIBNEWCOMMANDS_SRC_JAR_FILE TARGET wpilibNewCommands_src_jar PROPERTY JAR_FILE)
- install(FILES ${WPILIBNEWCOMMANDS_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
+ get_property(WPILIBNEWCOMMANDS_SRC_JAR_FILE TARGET wpilibNewCommands_src_jar PROPERTY JAR_FILE)
+ install(FILES ${WPILIBNEWCOMMANDS_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
- set_property(TARGET wpilibNewCommands_src_jar PROPERTY FOLDER "java")
+ set_property(TARGET wpilibNewCommands_src_jar PROPERTY FOLDER "java")
endif()
file(GLOB_RECURSE wpilibNewCommands_native_src src/main/native/cpp/*.cpp)
@@ -49,25 +64,28 @@
wpilib_target_warnings(wpilibNewCommands)
target_link_libraries(wpilibNewCommands wpilibc)
-target_include_directories(wpilibNewCommands PUBLIC
- $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
- $<INSTALL_INTERFACE:${include_dest}/wpilibNewCommands>)
+target_include_directories(
+ wpilibNewCommands
+ PUBLIC
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+ $<INSTALL_INTERFACE:${include_dest}/wpilibNewCommands>
+)
-install(TARGETS wpilibNewCommands EXPORT wpilibNewCommands)
+install(TARGETS wpilibNewCommands EXPORT wpilibnewcommands)
install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpilibNewCommands")
-if (FLAT_INSTALL_WPILIB)
- set(wpilibNewCommands_config_dir ${wpilib_dest})
- else()
- set(wpilibNewCommands_config_dir share/wpilibNewCommands)
- endif()
+configure_file(
+ wpilibnewcommands-config.cmake.in
+ ${WPILIB_BINARY_DIR}/wpilibnewcommands-config.cmake
+)
+install(
+ FILES ${WPILIB_BINARY_DIR}/wpilibnewcommands-config.cmake
+ DESTINATION share/wpilibNewCommands
+)
+install(EXPORT wpilibnewcommands DESTINATION share/wpilibNewCommands)
- configure_file(wpilibNewCommands-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake)
- install(FILES ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake DESTINATION ${wpilibNewCommands_config_dir})
- install(EXPORT wpilibNewCommands DESTINATION ${wpilibNewCommands_config_dir})
-
- if (WITH_TESTS)
- wpilib_add_test(wpilibNewCommands src/test/native/cpp)
- target_include_directories(wpilibNewCommands_test PRIVATE src/test/native/include)
- target_link_libraries(wpilibNewCommands_test wpilibNewCommands gmock_main)
- endif()
+if(WITH_TESTS)
+ wpilib_add_test(wpilibNewCommands src/test/native/cpp)
+ target_include_directories(wpilibNewCommands_test PRIVATE src/test/native/include)
+ target_link_libraries(wpilibNewCommands_test wpilibNewCommands gmock_main)
+endif()
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
index a913b55..5ea1a36 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
@@ -25,8 +25,11 @@
* <p>This class is provided by the NewCommands VendorDep
*/
public abstract class Command implements Sendable {
+ /** Requirements set. */
protected Set<Subsystem> m_requirements = new HashSet<>();
+ /** Default constructor. */
+ @SuppressWarnings("this-escape")
protected Command() {
String name = getClass().getName();
SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
index 12243cb..d75f08a 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
@@ -16,4 +16,9 @@
*/
@Deprecated(since = "2024", forRemoval = true)
@SuppressWarnings("PMD.AbstractClassWithoutAnyMethod")
-public abstract class CommandBase extends Command {}
+public abstract class CommandBase extends Command {
+ /** Default constructor. */
+ public CommandBase() {
+ super();
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
index c868539..e61b6f8 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
@@ -61,7 +61,7 @@
private static final Optional<Command> kNoInterruptor = Optional.empty();
- private final Set<Command> m_composedCommands = Collections.newSetFromMap(new WeakHashMap<>());
+ private final Map<Command, Exception> m_composedCommands = new WeakHashMap<>();
// A set of the currently-running commands.
private final Set<Command> m_scheduledCommands = new LinkedHashSet<>();
@@ -261,7 +261,7 @@
if (RobotBase.isSimulation()) {
subsystem.simulationPeriodic();
}
- m_watchdog.addEpoch(subsystem.getClass().getSimpleName() + ".periodic()");
+ m_watchdog.addEpoch(subsystem.getName() + ".periodic()");
}
// Cache the active instance to avoid concurrency problems if setActiveLoop() is called from
@@ -581,12 +581,25 @@
* directly or added to a composition.
*
* @param commands the commands to register
- * @throws IllegalArgumentException if the given commands have already been composed.
+ * @throws IllegalArgumentException if the given commands have already been composed, or the array
+ * of commands has duplicates.
*/
public void registerComposedCommands(Command... commands) {
- var commandSet = Set.of(commands);
- requireNotComposed(commandSet);
- m_composedCommands.addAll(commandSet);
+ Set<Command> commandSet;
+ try {
+ commandSet = Set.of(commands);
+ } catch (IllegalArgumentException e) {
+ throw new IllegalArgumentException(
+ "Cannot compose a command twice in the same composition! (Original exception: "
+ + e
+ + ")");
+ }
+ requireNotComposedOrScheduled(commandSet);
+ var exception = new Exception("Originally composed at:");
+ exception.fillInStackTrace();
+ for (var command : commands) {
+ m_composedCommands.put(command, exception);
+ }
}
/**
@@ -613,30 +626,58 @@
}
/**
- * Requires that the specified command hasn't been already added to a composition.
+ * Requires that the specified command hasn't already been added to a composition.
*
- * @param command The command to check
+ * @param commands The commands to check
* @throws IllegalArgumentException if the given commands have already been composed.
*/
- public void requireNotComposed(Command command) {
- if (m_composedCommands.contains(command)) {
- throw new IllegalArgumentException(
- "Commands that have been composed may not be added to another composition or scheduled "
- + "individually!");
+ public void requireNotComposed(Command... commands) {
+ for (var command : commands) {
+ var exception = m_composedCommands.getOrDefault(command, null);
+ if (exception != null) {
+ throw new IllegalArgumentException(
+ "Commands that have been composed may not be added to another composition or scheduled "
+ + "individually!",
+ exception);
+ }
}
}
/**
- * Requires that the specified commands not have been already added to a composition.
+ * Requires that the specified commands have not already been added to a composition.
*
* @param commands The commands to check
* @throws IllegalArgumentException if the given commands have already been composed.
*/
public void requireNotComposed(Collection<Command> commands) {
- if (!Collections.disjoint(commands, getComposedCommands())) {
+ requireNotComposed(commands.toArray(Command[]::new));
+ }
+
+ /**
+ * Requires that the specified command hasn't already been added to a composition, and is not
+ * currently scheduled.
+ *
+ * @param command The command to check
+ * @throws IllegalArgumentException if the given command has already been composed or scheduled.
+ */
+ public void requireNotComposedOrScheduled(Command command) {
+ if (isScheduled(command)) {
throw new IllegalArgumentException(
- "Commands that have been composed may not be added to another composition or scheduled "
- + "individually!");
+ "Commands that have been scheduled individually may not be added to a composition!");
+ }
+ requireNotComposed(command);
+ }
+
+ /**
+ * Requires that the specified commands have not already been added to a composition, and are not
+ * currently scheduled.
+ *
+ * @param commands The commands to check
+ * @throws IllegalArgumentException if the given commands have already been composed or scheduled.
+ */
+ public void requireNotComposedOrScheduled(Collection<Command> commands) {
+ for (var command : commands) {
+ requireNotComposedOrScheduled(command);
}
}
@@ -651,7 +692,7 @@
}
Set<Command> getComposedCommands() {
- return m_composedCommands;
+ return m_composedCommands.keySet();
}
@Override
@@ -683,9 +724,7 @@
null);
builder.addIntegerArrayProperty(
"Cancel",
- () -> {
- return new long[] {};
- },
+ () -> new long[] {},
toCancel -> {
Map<Long, Command> ids = new LinkedHashMap<>();
for (Command command : m_scheduledCommands) {
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
index 7295e3c..9202d19 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Commands.java
@@ -233,12 +233,13 @@
* the others.
*
* @param deadline the deadline command
- * @param commands the commands to include
+ * @param otherCommands the other commands to include
* @return the command group
* @see ParallelDeadlineGroup
+ * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
*/
- public static Command deadline(Command deadline, Command... commands) {
- return new ParallelDeadlineGroup(deadline, commands);
+ public static Command deadline(Command deadline, Command... otherCommands) {
+ return new ParallelDeadlineGroup(deadline, otherCommands);
}
private Commands() {
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java
index 76a5276..099eba3 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/DeferredCommand.java
@@ -37,6 +37,7 @@
* omission of command requirements. Use {@link Set#of()} to easily construct a requirement
* set.
*/
+ @SuppressWarnings("this-escape")
public DeferredCommand(Supplier<Command> supplier, Set<Subsystem> requirements) {
m_supplier = requireNonNullParam(supplier, "supplier", "DeferredCommand");
addRequirements(requirements.toArray(new Subsystem[0]));
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
index dde5ae1..9f1d8a8 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
@@ -18,10 +18,10 @@
* <p>This class is provided by the NewCommands VendorDep
*/
public class FunctionalCommand extends Command {
- protected final Runnable m_onInit;
- protected final Runnable m_onExecute;
- protected final Consumer<Boolean> m_onEnd;
- protected final BooleanSupplier m_isFinished;
+ private final Runnable m_onInit;
+ private final Runnable m_onExecute;
+ private final Consumer<Boolean> m_onEnd;
+ private final BooleanSupplier m_isFinished;
/**
* Creates a new FunctionalCommand.
@@ -32,6 +32,7 @@
* @param isFinished the function that determines whether the command has finished
* @param requirements the subsystems required by this command
*/
+ @SuppressWarnings("this-escape")
public FunctionalCommand(
Runnable onInit,
Runnable onExecute,
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
index 16d9c8a..06663c9 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
@@ -86,6 +86,7 @@
* voltages.
* @param requirements The subsystems to require.
*/
+ @SuppressWarnings("this-escape")
public MecanumControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
@@ -229,6 +230,7 @@
* @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
* @param requirements The subsystems to require.
*/
+ @SuppressWarnings("this-escape")
public MecanumControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
index ad1a12a..4109091 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
@@ -18,8 +18,8 @@
* <p>This class is provided by the NewCommands VendorDep
*/
public class NotifierCommand extends Command {
- protected final Notifier m_notifier;
- protected final double m_period;
+ private final Notifier m_notifier;
+ private final double m_period;
/**
* Creates a new NotifierCommand.
@@ -28,6 +28,7 @@
* @param period the period at which the notifier should run, in seconds
* @param requirements the subsystems required by this command
*/
+ @SuppressWarnings("this-escape")
public NotifierCommand(Runnable toRun, double period, Subsystem... requirements) {
m_notifier = new Notifier(toRun);
m_period = period;
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
index c761f3f..26627fa 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
@@ -19,9 +19,16 @@
* <p>This class is provided by the NewCommands VendorDep
*/
public class PIDCommand extends Command {
+ /** PID controller. */
protected final PIDController m_controller;
+
+ /** Measurement getter. */
protected DoubleSupplier m_measurement;
+
+ /** Setpoint getter. */
protected DoubleSupplier m_setpoint;
+
+ /** PID controller output consumer. */
protected DoubleConsumer m_useOutput;
/**
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
index 84318de..8146ca4 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
@@ -15,7 +15,10 @@
* <p>This class is provided by the NewCommands VendorDep
*/
public abstract class PIDSubsystem extends SubsystemBase {
+ /** PID controller. */
protected final PIDController m_controller;
+
+ /** Whether PID controller output is enabled. */
protected boolean m_enabled;
/**
@@ -24,6 +27,7 @@
* @param controller the PIDController to use
* @param initialPosition the initial setpoint of the subsystem
*/
+ @SuppressWarnings("this-escape")
public PIDSubsystem(PIDController controller, double initialPosition) {
m_controller = requireNonNullParam(controller, "controller", "PIDSubsystem");
setSetpoint(initialPosition);
@@ -46,6 +50,11 @@
}
}
+ /**
+ * Returns the PIDController.
+ *
+ * @return The controller.
+ */
public PIDController getController() {
return m_controller;
}
@@ -55,7 +64,7 @@
*
* @param setpoint the setpoint for the subsystem
*/
- public void setSetpoint(double setpoint) {
+ public final void setSetpoint(double setpoint) {
m_controller.setSetpoint(setpoint);
}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
index 6d263e8..8acfea8 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
@@ -29,20 +29,18 @@
private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
/**
- * Creates a new ParallelDeadlineGroup. The given commands (including the deadline) will be
+ * Creates a new ParallelDeadlineGroup. The given commands, including the deadline, will be
* executed simultaneously. The composition will finish when the deadline finishes, interrupting
* all other still-running commands. If the composition is interrupted, only the commands still
* running will be interrupted.
*
* @param deadline the command that determines when the composition ends
- * @param commands the commands to be executed
+ * @param otherCommands the other commands to be executed
+ * @throws IllegalArgumentException if the deadline command is also in the otherCommands argument
*/
- public ParallelDeadlineGroup(Command deadline, Command... commands) {
- m_deadline = deadline;
- addCommands(commands);
- if (!m_commands.containsKey(deadline)) {
- addCommands(deadline);
- }
+ public ParallelDeadlineGroup(Command deadline, Command... otherCommands) {
+ addCommands(otherCommands);
+ setDeadline(deadline);
}
/**
@@ -50,11 +48,19 @@
* contained.
*
* @param deadline the command that determines when the group ends
+ * @throws IllegalArgumentException if the deadline command is already in the composition
*/
- public void setDeadline(Command deadline) {
- if (!m_commands.containsKey(deadline)) {
- addCommands(deadline);
+ public final void setDeadline(Command deadline) {
+ @SuppressWarnings("PMD.CompareObjectsWithEquals")
+ boolean isAlreadyDeadline = deadline == m_deadline;
+ if (isAlreadyDeadline) {
+ return;
}
+ if (m_commands.containsKey(deadline)) {
+ throw new IllegalArgumentException(
+ "The deadline command cannot also be in the other commands!");
+ }
+ addCommands(deadline);
m_deadline = deadline;
}
@@ -74,7 +80,7 @@
for (Command command : commands) {
if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
throw new IllegalArgumentException(
- "Multiple commands in a parallel group cannot" + "require the same subsystems");
+ "Multiple commands in a parallel group cannot require the same subsystems");
}
m_commands.put(command, false);
m_requirements.addAll(command.getRequirements());
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
index f7175fc..4e82811 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
@@ -21,9 +21,16 @@
* <p>This class is provided by the NewCommands VendorDep
*/
public class ProfiledPIDCommand extends Command {
+ /** Profiled PID controller. */
protected final ProfiledPIDController m_controller;
+
+ /** Measurement getter. */
protected DoubleSupplier m_measurement;
+
+ /** Goal getter. */
protected Supplier<State> m_goal;
+
+ /** Profiled PID controller output consumer. */
protected BiConsumer<Double, State> m_useOutput;
/**
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
index 283b4bf..05318ed 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
@@ -17,7 +17,10 @@
* <p>This class is provided by the NewCommands VendorDep
*/
public abstract class ProfiledPIDSubsystem extends SubsystemBase {
+ /** Profiled PID controller. */
protected final ProfiledPIDController m_controller;
+
+ /** Whether the profiled PID controller output is enabled. */
protected boolean m_enabled;
/**
@@ -47,6 +50,11 @@
}
}
+ /**
+ * Returns the ProfiledPIDController.
+ *
+ * @return The controller.
+ */
public ProfiledPIDController getController() {
return m_controller;
}
@@ -56,7 +64,7 @@
*
* @param goal The goal state for the subsystem's motion profile.
*/
- public void setGoal(TrapezoidProfile.State goal) {
+ public final void setGoal(TrapezoidProfile.State goal) {
m_controller.setGoal(goal);
}
@@ -65,7 +73,7 @@
*
* @param goal The goal position for the subsystem's motion profile.
*/
- public void setGoal(double goal) {
+ public final void setGoal(double goal) {
setGoal(new TrapezoidProfile.State(goal, 0));
}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java
index d6e93e8..f05b286 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyCommand.java
@@ -35,6 +35,7 @@
*
* @param command the command to run by proxy
*/
+ @SuppressWarnings("this-escape")
public ProxyCommand(Command command) {
this(() -> command);
setName("Proxy(" + command.getName() + ")");
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
index 9f19e4a..48b3756 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
@@ -70,6 +70,7 @@
* the robot drive.
* @param requirements The subsystems to require.
*/
+ @SuppressWarnings("this-escape")
public RamseteCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
@@ -109,6 +110,7 @@
* @param outputMetersPerSecond A function that consumes the computed left and right wheel speeds.
* @param requirements The subsystems to require.
*/
+ @SuppressWarnings("this-escape")
public RamseteCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java
index 5b49ae0..70ff63c 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java
@@ -20,7 +20,7 @@
* <p>This class is provided by the NewCommands VendorDep
*/
public class RepeatCommand extends Command {
- protected final Command m_command;
+ private final Command m_command;
private boolean m_ended;
/**
@@ -29,6 +29,7 @@
*
* @param command the command to run repeatedly
*/
+ @SuppressWarnings("this-escape")
public RepeatCommand(Command command) {
m_command = requireNonNullParam(command, "command", "RepeatCommand");
CommandScheduler.getInstance().registerComposedCommands(command);
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
index ac89dbd..ed94543 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
@@ -41,6 +41,15 @@
default void simulationPeriodic() {}
/**
+ * Gets the subsystem name of this Subsystem.
+ *
+ * @return Subsystem name
+ */
+ default String getName() {
+ return this.getClass().getSimpleName();
+ }
+
+ /**
* Sets the default {@link Command} of the subsystem. The default command will be automatically
* scheduled when no other commands are scheduled that require the subsystem. Default commands
* should generally not end on their own, i.e. their {@link Command#isFinished()} method should
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
index 6cf926e..3a024e5 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
@@ -15,7 +15,8 @@
* <p>This class is provided by the NewCommands VendorDep
*/
public abstract class SubsystemBase implements Subsystem, Sendable {
- /** Constructor. */
+ /** Constructor. Telemetry/log name defaults to the classname. */
+ @SuppressWarnings("this-escape")
public SubsystemBase() {
String name = this.getClass().getSimpleName();
name = name.substring(name.lastIndexOf('.') + 1);
@@ -24,10 +25,22 @@
}
/**
+ * Constructor.
+ *
+ * @param name Name of the subsystem for telemetry and logging.
+ */
+ @SuppressWarnings("this-escape")
+ public SubsystemBase(String name) {
+ SendableRegistry.addLW(this, name, name);
+ CommandScheduler.getInstance().registerSubsystem(this);
+ }
+
+ /**
* Gets the name of this Subsystem.
*
* @return Name
*/
+ @Override
public String getName() {
return SendableRegistry.getName(this);
}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
index 3eacac3..424a288 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
@@ -186,6 +186,7 @@
* @param outputModuleStates The raw output module states from the position controllers.
* @param requirements The subsystems to require.
*/
+ @SuppressWarnings("this-escape")
public SwerveControllerCommand(
Trajectory trajectory,
Supplier<Pose2d> pose,
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
index 57185a9..3bfdbae 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
@@ -35,6 +35,7 @@
* @param currentState The current state
* @param requirements The subsystems required by this command.
*/
+ @SuppressWarnings("this-escape")
public TrapezoidProfileCommand(
TrapezoidProfile profile,
Consumer<State> output,
@@ -60,6 +61,7 @@
* This allows you to change goals at runtime.
*/
@Deprecated(since = "2024", forRemoval = true)
+ @SuppressWarnings("this-escape")
public TrapezoidProfileCommand(
TrapezoidProfile profile, Consumer<State> output, Subsystem... requirements) {
m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
@@ -79,7 +81,7 @@
@SuppressWarnings("removal")
public void execute() {
if (m_newAPI) {
- m_output.accept(m_profile.calculate(m_timer.get(), m_goal.get(), m_currentState.get()));
+ m_output.accept(m_profile.calculate(m_timer.get(), m_currentState.get(), m_goal.get()));
} else {
m_output.accept(m_profile.calculate(m_timer.get()));
}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
index 35c02e6..54d4ede 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
@@ -74,7 +74,7 @@
*
* @param goal The goal state for the subsystem's motion profile.
*/
- public void setGoal(TrapezoidProfile.State goal) {
+ public final void setGoal(TrapezoidProfile.State goal) {
m_goal = goal;
}
@@ -83,7 +83,7 @@
*
* @param goal The goal position for the subsystem's motion profile.
*/
- public void setGoal(double goal) {
+ public final void setGoal(double goal) {
setGoal(new TrapezoidProfile.State(goal, 0));
}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
index e7b32be..6d4c52b 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
@@ -14,7 +14,9 @@
* <p>This class is provided by the NewCommands VendorDep
*/
public class WaitCommand extends Command {
+ /** The timer used for waiting. */
protected Timer m_timer = new Timer();
+
private final double m_duration;
/**
@@ -22,6 +24,7 @@
*
* @param seconds the time to wait, in seconds
*/
+ @SuppressWarnings("this-escape")
public WaitCommand(double seconds) {
m_duration = seconds;
SendableRegistry.setName(this, getName() + ": " + seconds + " seconds");
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
index c687f0a..5e6ccae 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WrapperCommand.java
@@ -15,6 +15,7 @@
* subsystems its components require.
*/
public abstract class WrapperCommand extends Command {
+ /** Command being wrapped. */
protected final Command m_command;
/**
@@ -23,6 +24,7 @@
* @param command the command being wrapped. Trying to directly schedule this command or add it to
* a composition will throw an exception.
*/
+ @SuppressWarnings("this-escape")
protected WrapperCommand(Command command) {
CommandScheduler.getInstance().registerComposedCommands(command);
m_command = command;
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandStadiaController.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandStadiaController.java
new file mode 100644
index 0000000..0d919e5
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandStadiaController.java
@@ -0,0 +1,405 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.StadiaController;
+import edu.wpi.first.wpilibj.event.EventLoop;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * 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.
+ *
+ * @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);
+ }
+
+ /**
+ * Get the underlying GenericHID object.
+ *
+ * @return the wrapped GenericHID object
+ */
+ @Override
+ public StadiaController getHID() {
+ return m_hid;
+ }
+
+ /**
+ * Constructs an event instance around the left bumper's digital signal.
+ *
+ * @return an event instance representing the left bumper's digital signal attached to the {@link
+ * CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #leftBumper(EventLoop)
+ */
+ public Trigger leftBumper() {
+ return leftBumper(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the left bumper's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the right bumper's digital signal attached to the given
+ * loop.
+ */
+ public Trigger leftBumper(EventLoop loop) {
+ return m_hid.leftBumper(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the right bumper's digital signal.
+ *
+ * @return an event instance representing the right bumper's digital signal attached to the {@link
+ * CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #rightBumper(EventLoop)
+ */
+ public Trigger rightBumper() {
+ return rightBumper(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the right bumper's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the left bumper's digital signal attached to the given
+ * loop.
+ */
+ public Trigger rightBumper(EventLoop loop) {
+ return m_hid.rightBumper(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the left stick button's digital signal.
+ *
+ * @return an event instance representing the left stick button's digital signal attached to the
+ * {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #leftStick(EventLoop)
+ */
+ public Trigger leftStick() {
+ return leftStick(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the left stick button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the left stick button's digital signal attached to the
+ * given loop.
+ */
+ public Trigger leftStick(EventLoop loop) {
+ return m_hid.leftStick(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the right stick button's digital signal.
+ *
+ * @return an event instance representing the right stick button's digital signal attached to the
+ * {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #rightStick(EventLoop)
+ */
+ public Trigger rightStick() {
+ return rightStick(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the right stick button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the right stick button's digital signal attached to the
+ * given loop.
+ */
+ public Trigger rightStick(EventLoop loop) {
+ return m_hid.rightStick(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the right trigger button's digital signal.
+ *
+ * @return an event instance representing the right trigger button's digital signal attached to
+ * the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #rightTrigger(EventLoop)
+ */
+ public Trigger rightTrigger() {
+ return rightTrigger(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the right trigger button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the right trigger button's digital signal attached to
+ * the given loop.
+ */
+ public Trigger rightTrigger(EventLoop loop) {
+ return m_hid.rightTrigger(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the left trigger button's digital signal.
+ *
+ * @return an event instance representing the left trigger button's digital signal attached to the
+ * {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #leftTrigger(EventLoop)
+ */
+ public Trigger leftTrigger() {
+ return leftTrigger(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the left trigger button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the left trigger button's digital signal attached to the
+ * given loop.
+ */
+ public Trigger leftTrigger(EventLoop loop) {
+ return m_hid.leftTrigger(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the A button's digital signal.
+ *
+ * @return an event instance representing the A button's digital signal attached to the {@link
+ * CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #a(EventLoop)
+ */
+ public Trigger a() {
+ return a(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the A button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the A button's digital signal attached to the given
+ * loop.
+ */
+ public Trigger a(EventLoop loop) {
+ return m_hid.a(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the B button's digital signal.
+ *
+ * @return an event instance representing the B button's digital signal attached to the {@link
+ * CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #b(EventLoop)
+ */
+ public Trigger b() {
+ return b(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the B button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the B button's digital signal attached to the given
+ * loop.
+ */
+ public Trigger b(EventLoop loop) {
+ return m_hid.b(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the X button's digital signal.
+ *
+ * @return an event instance representing the X button's digital signal attached to the {@link
+ * CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #x(EventLoop)
+ */
+ public Trigger x() {
+ return x(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the X button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the X button's digital signal attached to the given
+ * loop.
+ */
+ public Trigger x(EventLoop loop) {
+ return m_hid.x(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the Y button's digital signal.
+ *
+ * @return an event instance representing the Y button's digital signal attached to the {@link
+ * CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #y(EventLoop)
+ */
+ public Trigger y() {
+ return y(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the Y button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the Y button's digital signal attached to the given
+ * loop.
+ */
+ public Trigger y(EventLoop loop) {
+ return m_hid.y(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the ellipses button's digital signal.
+ *
+ * @return an event instance representing the ellipses button's digital signal attached to the
+ * {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #ellipses(EventLoop)
+ */
+ public Trigger ellipses() {
+ return ellipses(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the ellipses button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the ellipses button's digital signal attached to the
+ * given loop.
+ */
+ public Trigger ellipses(EventLoop loop) {
+ return m_hid.ellipses(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the stadia button's digital signal.
+ *
+ * @return an event instance representing the stadia button's digital signal attached to the
+ * {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #stadia(EventLoop)
+ */
+ public Trigger stadia() {
+ return stadia(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the stadia button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the stadia button's digital signal attached to the given
+ * loop.
+ */
+ public Trigger stadia(EventLoop loop) {
+ return m_hid.stadia(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the google button's digital signal.
+ *
+ * @return an event instance representing the google button's digital signal attached to the
+ * {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #google(EventLoop)
+ */
+ public Trigger google() {
+ return google(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the google button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the google button's digital signal attached to the given
+ * loop.
+ */
+ public Trigger google(EventLoop loop) {
+ return m_hid.google(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the frame button's digital signal.
+ *
+ * @return an event instance representing the frame button's digital signal attached to the {@link
+ * CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #frame(EventLoop)
+ */
+ public Trigger frame() {
+ return frame(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the frame button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the frame button's digital signal attached to the given
+ * loop.
+ */
+ public Trigger frame(EventLoop loop) {
+ return m_hid.frame(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Constructs an event instance around the hamburger button's digital signal.
+ *
+ * @return an event instance representing the hamburger button's digital signal attached to the
+ * {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+ * @see #hamburger(EventLoop)
+ */
+ public Trigger hamburger() {
+ return hamburger(CommandScheduler.getInstance().getDefaultButtonLoop());
+ }
+
+ /**
+ * Constructs an event instance around the hamburger button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to.
+ * @return an event instance representing the hamburger button's digital signal attached to the
+ * given loop.
+ */
+ public Trigger hamburger(EventLoop loop) {
+ return m_hid.hamburger(loop).castTo(Trigger::new);
+ }
+
+ /**
+ * Get the X axis value of left side of the controller.
+ *
+ * @return The axis value.
+ */
+ public double getLeftX() {
+ return m_hid.getLeftX();
+ }
+
+ /**
+ * Get the X axis value of right side of the controller.
+ *
+ * @return The axis value.
+ */
+ public double getRightX() {
+ return m_hid.getRightX();
+ }
+
+ /**
+ * Get the Y axis value of left side of the controller.
+ *
+ * @return The axis value.
+ */
+ public double getLeftY() {
+ return m_hid.getLeftY();
+ }
+
+ /**
+ * Get the Y axis value of right side of the controller.
+ *
+ * @return The axis value.
+ */
+ public double getRightY() {
+ return m_hid.getRightY();
+ }
+}
diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
index f4897f2..87c9b73 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
@@ -42,10 +42,20 @@
this.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/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggers.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggers.java
new file mode 100644
index 0000000..9bef66a
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggers.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 edu.wpi.first.wpilibj2.command.button;
+
+import edu.wpi.first.wpilibj.DriverStation;
+
+/**
+ * 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/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java
new file mode 100644
index 0000000..6a049a6
--- /dev/null
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutine.java
@@ -0,0 +1,285 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.sysid;
+
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+import static java.util.Map.entry;
+
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Time;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import java.util.Map;
+import java.util.function.Consumer;
+
+/**
+ * A SysId characterization routine for a single mechanism. Mechanisms may have multiple motors.
+ *
+ * <p>A single subsystem may have multiple mechanisms, but mechanisms should not share test
+ * routines. Each complete test of a mechanism should have its own SysIdRoutine instance, since the
+ * log name of the recorded data is determined by the mechanism name.
+ *
+ * <p>The test state (e.g. "quasistatic-forward") is logged once per iteration during test
+ * execution, and once with state "none" when a test ends. Motor frames are logged every iteration
+ * during test execution.
+ *
+ * <p>Timestamps are not coordinated across data, so motor frames and test state tags may be
+ * recorded on different log frames. Because frame alignment is not guaranteed, SysId parses the log
+ * by using the test state flag to determine the timestamp range for each section of the test, and
+ * then extracts the motor frames within the valid timestamp ranges. If a given test was run
+ * multiple times in a single logfile, the user will need to select which of the tests to use for
+ * the fit in the analysis tool.
+ */
+public class SysIdRoutine extends SysIdRoutineLog {
+ private final Config m_config;
+ private final Mechanism m_mechanism;
+ private final MutableMeasure<Voltage> m_outputVolts = mutable(Volts.of(0));
+ private final Consumer<State> m_recordState;
+
+ /**
+ * Create a new SysId characterization routine.
+ *
+ * @param config Hardware-independent parameters for the SysId routine.
+ * @param mechanism Hardware interface for the SysId routine.
+ */
+ public SysIdRoutine(Config config, Mechanism mechanism) {
+ super(mechanism.m_subsystem.getName());
+ m_config = config;
+ m_mechanism = mechanism;
+ m_recordState = config.m_recordState != null ? config.m_recordState : this::recordState;
+ }
+
+ /** Hardware-independent configuration for a SysId test routine. */
+ public static class Config {
+ /** The voltage ramp rate used for quasistatic test routines. */
+ public final Measure<Velocity<Voltage>> m_rampRate;
+
+ /** The step voltage output used for dynamic test routines. */
+ public final Measure<Voltage> m_stepVoltage;
+
+ /** Safety timeout for the test routine commands. */
+ public final Measure<Time> m_timeout;
+
+ /** Optional handle for recording test state in a third-party logging solution. */
+ public final Consumer<State> m_recordState;
+
+ /**
+ * Create a new configuration for a SysId test routine.
+ *
+ * @param rampRate The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt
+ * per second if left null.
+ * @param stepVoltage The step voltage output used for dynamic test routines. Defaults to 7
+ * volts if left null.
+ * @param timeout Safety timeout for the test routine commands. Defaults to 10 seconds if left
+ * null.
+ * @param recordState Optional handle for recording test state in a third-party logging
+ * solution. If provided, the test routine state will be passed to this callback instead of
+ * logged in WPILog.
+ */
+ public Config(
+ Measure<Velocity<Voltage>> rampRate,
+ Measure<Voltage> stepVoltage,
+ Measure<Time> timeout,
+ Consumer<State> recordState) {
+ m_rampRate = rampRate != null ? rampRate : Volts.of(1).per(Seconds.of(1));
+ m_stepVoltage = stepVoltage != null ? stepVoltage : Volts.of(7);
+ m_timeout = timeout != null ? timeout : Seconds.of(10);
+ m_recordState = recordState;
+ }
+
+ /**
+ * Create a new configuration for a SysId test routine.
+ *
+ * @param rampRate The voltage ramp rate used for quasistatic test routines. Defaults to 1 volt
+ * per second if left null.
+ * @param stepVoltage The step voltage output used for dynamic test routines. Defaults to 7
+ * volts if left null.
+ * @param timeout Safety timeout for the test routine commands. Defaults to 10 seconds if left
+ * null.
+ */
+ public Config(
+ Measure<Velocity<Voltage>> rampRate, Measure<Voltage> stepVoltage, Measure<Time> timeout) {
+ this(rampRate, stepVoltage, timeout, null);
+ }
+
+ /**
+ * Create a default configuration for a SysId test routine with all default settings.
+ *
+ * <p>rampRate: 1 volt/sec
+ *
+ * <p>stepVoltage: 7 volts
+ *
+ * <p>timeout: 10 seconds
+ */
+ public Config() {
+ this(null, null, null, null);
+ }
+ }
+
+ /**
+ * A mechanism to be characterized by a SysId routine. Defines callbacks needed for the SysId test
+ * routine to control and record data from the mechanism.
+ */
+ public static class Mechanism {
+ /** Sends the SysId-specified drive signal to the mechanism motors during test routines. */
+ public final Consumer<Measure<Voltage>> m_drive;
+
+ /**
+ * Returns measured data (voltages, positions, velocities) of the mechanism motors during test
+ * routines.
+ */
+ public final Consumer<SysIdRoutineLog> m_log;
+
+ /** The subsystem containing the motor(s) that is (or are) being characterized. */
+ public final Subsystem m_subsystem;
+
+ /** The name of the mechanism being tested. */
+ public final String m_name;
+
+ /**
+ * Create a new mechanism specification for a SysId routine.
+ *
+ * @param drive Sends the SysId-specified drive signal to the mechanism motors during test
+ * routines.
+ * @param log Returns measured data of the mechanism motors during test routines. To return
+ * data, call `motor(string motorName)` on the supplied `SysIdRoutineLog` instance, and then
+ * call one or more of the chainable logging handles (e.g. `voltage`) on the returned
+ * `MotorLog`. Multiple motors can be logged in a single callback by calling `motor`
+ * multiple times.
+ * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized.
+ * Will be declared as a requirement for the returned test commands.
+ * @param name The name of the mechanism being tested. Will be appended to the log entry title
+ * for the routine's test state, e.g. "sysid-test-state-mechanism". Defaults to the name of
+ * the subsystem if left null.
+ */
+ public Mechanism(
+ Consumer<Measure<Voltage>> drive,
+ Consumer<SysIdRoutineLog> log,
+ Subsystem subsystem,
+ String name) {
+ m_drive = drive;
+ m_log = log != null ? log : l -> {};
+ m_subsystem = subsystem;
+ m_name = name != null ? name : subsystem.getName();
+ }
+
+ /**
+ * Create a new mechanism specification for a SysId routine. Defaults the mechanism name to the
+ * subsystem name.
+ *
+ * @param drive Sends the SysId-specified drive signal to the mechanism motors during test
+ * routines.
+ * @param log Returns measured data of the mechanism motors during test routines. To return
+ * data, call `motor(string motorName)` on the supplied `SysIdRoutineLog` instance, and then
+ * call one or more of the chainable logging handles (e.g. `voltage`) on the returned
+ * `MotorLog`. Multiple motors can be logged in a single callback by calling `motor`
+ * multiple times.
+ * @param subsystem The subsystem containing the motor(s) that is (or are) being characterized.
+ * Will be declared as a requirement for the returned test commands. The subsystem's `name`
+ * will be appended to the log entry title for the routine's test state, e.g.
+ * "sysid-test-state-subsystem".
+ */
+ public Mechanism(
+ Consumer<Measure<Voltage>> drive, Consumer<SysIdRoutineLog> log, Subsystem subsystem) {
+ this(drive, log, subsystem, null);
+ }
+ }
+
+ /** Motor direction for a SysId test. */
+ public enum Direction {
+ /** Forward. */
+ kForward,
+ /** Reverse. */
+ kReverse
+ }
+
+ /**
+ * Returns a command to run a quasistatic test in the specified direction.
+ *
+ * <p>The command will call the `drive` and `log` callbacks supplied at routine construction once
+ * per iteration. Upon command end or interruption, the `drive` callback is called with a value of
+ * 0 volts.
+ *
+ * @param direction The direction in which to run the test.
+ * @return A command to run the test.
+ */
+ public Command quasistatic(Direction direction) {
+ Timer timer = new Timer();
+ double outputSign = direction == Direction.kForward ? 1.0 : -1.0;
+ State state =
+ Map.ofEntries(
+ entry(Direction.kForward, State.kQuasistaticForward),
+ entry(Direction.kReverse, State.kQuasistaticReverse))
+ .get(direction);
+
+ return m_mechanism
+ .m_subsystem
+ .runOnce(timer::start)
+ .andThen(
+ m_mechanism.m_subsystem.run(
+ () -> {
+ m_mechanism.m_drive.accept(
+ m_outputVolts.mut_replace(
+ outputSign * timer.get() * m_config.m_rampRate.in(Volts.per(Second)),
+ Volts));
+ m_mechanism.m_log.accept(this);
+ m_recordState.accept(state);
+ }))
+ .finallyDo(
+ () -> {
+ m_mechanism.m_drive.accept(Volts.of(0));
+ m_recordState.accept(State.kNone);
+ timer.stop();
+ })
+ .withName("sysid-" + state.toString() + "-" + m_mechanism.m_name)
+ .withTimeout(m_config.m_timeout.in(Seconds));
+ }
+
+ /**
+ * Returns a command to run a dynamic test in the specified direction.
+ *
+ * <p>The command will call the `drive` and `log` callbacks supplied at routine construction once
+ * per iteration. Upon command end or interruption, the `drive` callback is called with a value of
+ * 0 volts.
+ *
+ * @param direction The direction in which to run the test.
+ * @return A command to run the test.
+ */
+ public Command dynamic(Direction direction) {
+ double outputSign = direction == Direction.kForward ? 1.0 : -1.0;
+ State state =
+ Map.ofEntries(
+ entry(Direction.kForward, State.kDynamicForward),
+ entry(Direction.kReverse, State.kDynamicReverse))
+ .get(direction);
+
+ return m_mechanism
+ .m_subsystem
+ .runOnce(
+ () -> m_outputVolts.mut_replace(m_config.m_stepVoltage.in(Volts) * outputSign, Volts))
+ .andThen(
+ m_mechanism.m_subsystem.run(
+ () -> {
+ m_mechanism.m_drive.accept(m_outputVolts);
+ m_mechanism.m_log.accept(this);
+ m_recordState.accept(state);
+ }))
+ .finallyDo(
+ () -> {
+ m_mechanism.m_drive.accept(Volts.of(0));
+ m_recordState.accept(State.kNone);
+ })
+ .withName("sysid-" + state.toString() + "-" + m_mechanism.m_name)
+ .withTimeout(m_config.m_timeout.in(Seconds));
+ }
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
index 66de555..de673f2 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
@@ -4,6 +4,7 @@
#include "frc2/command/Command.h"
+#include <wpi/StackTrace.h>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
@@ -31,7 +32,7 @@
}
Command& Command::operator=(const Command& rhs) {
- m_isComposed = false;
+ SetComposed(false);
return *this;
}
@@ -156,11 +157,19 @@
}
bool Command::IsComposed() const {
- return m_isComposed;
+ return GetPreviousCompositionSite().has_value();
}
void Command::SetComposed(bool isComposed) {
- m_isComposed = isComposed;
+ if (isComposed) {
+ m_previousComposition = wpi::GetStackTrace(1);
+ } else {
+ m_previousComposition.reset();
+ }
+}
+
+std::optional<std::string> Command::GetPreviousCompositionSite() const {
+ return m_previousComposition;
}
void Command::InitSendable(wpi::SendableBuilder& builder) {
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp
index 6f7e418..6fa86d3 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp
@@ -22,10 +22,22 @@
using namespace frc2;
+CommandPtr::CommandPtr(std::unique_ptr<Command>&& command)
+ : m_ptr(std::move(command)) {
+ AssertValid();
+}
+
+CommandPtr::CommandPtr(CommandPtr&& rhs) {
+ m_ptr = std::move(rhs.m_ptr);
+ AssertValid();
+ rhs.m_moveOutSite = wpi::GetStackTrace(1);
+}
+
void CommandPtr::AssertValid() const {
if (!m_ptr) {
throw FRC_MakeError(frc::err::CommandIllegalUse,
- "Moved-from CommandPtr object used!");
+ "Moved-from CommandPtr object used!\nMoved out at:\n{}",
+ m_moveOutSite);
}
}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
index d61ffbd..efeee4e 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -183,7 +183,7 @@
if constexpr (frc::RobotBase::IsSimulation()) {
subsystem.getFirst()->SimulationPeriodic();
}
- m_watchdog.AddEpoch("Subsystem Periodic()");
+ m_watchdog.AddEpoch(subsystem.getFirst()->GetName() + ".Periodic()");
}
// Cache the active instance to avoid concurrency problems if SetActiveLoop()
@@ -453,11 +453,13 @@
}
void CommandScheduler::RequireUngrouped(const Command* command) {
- if (command->IsComposed()) {
+ auto stacktrace = command->GetPreviousCompositionSite();
+ if (stacktrace.has_value()) {
throw FRC_MakeError(frc::err::CommandIllegalUse,
"Commands that have been composed may not be added to "
- "another composition or scheduled "
- "individually!");
+ "another composition or scheduled individually!"
+ "\nOriginally composed at:\n{}",
+ stacktrace.value());
}
}
@@ -475,6 +477,29 @@
}
}
+void CommandScheduler::RequireUngroupedAndUnscheduled(const Command* command) {
+ if (IsScheduled(command)) {
+ throw FRC_MakeError(frc::err::CommandIllegalUse,
+ "Commands that have been scheduled individually may "
+ "not be added to another composition!");
+ }
+ RequireUngrouped(command);
+}
+
+void CommandScheduler::RequireUngroupedAndUnscheduled(
+ std::span<const std::unique_ptr<Command>> commands) {
+ for (auto&& command : commands) {
+ RequireUngroupedAndUnscheduled(command.get());
+ }
+}
+
+void CommandScheduler::RequireUngroupedAndUnscheduled(
+ std::initializer_list<const Command*> commands) {
+ for (auto&& command : commands) {
+ RequireUngroupedAndUnscheduled(command);
+ }
+}
+
void CommandScheduler::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("Scheduler");
builder.AddStringArrayProperty(
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
index 4c07c7a..6ce33b7 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
@@ -12,7 +12,7 @@
std::unique_ptr<Command>&& onFalse,
std::function<bool()> condition)
: m_condition{std::move(condition)} {
- CommandScheduler::GetInstance().RequireUngrouped(
+ CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(
{onTrue.get(), onFalse.get()});
m_onTrue = std::move(onTrue);
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
index 902c26b..6059db4 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
@@ -6,7 +6,12 @@
#include <utility>
+#include <units/velocity.h>
+#include <units/voltage.h>
+
using namespace frc2;
+using kv_unit = units::compound_unit<units::volts,
+ units::inverse<units::meters_per_second>>;
MecanumControllerCommand::MecanumControllerCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
@@ -95,6 +100,7 @@
Requirements requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
+ m_feedforward(0_V, units::unit_t<kv_unit>{0}),
m_kinematics(kinematics),
m_controller(xController, yController, thetaController),
m_desiredRotation(std::move(desiredRotation)),
@@ -116,6 +122,7 @@
Requirements requirements)
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
+ m_feedforward(0_V, units::unit_t<kv_unit>{0}),
m_kinematics(kinematics),
m_controller(xController, yController, thetaController),
m_maxWheelVelocity(maxWheelVelocity),
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
index 99e0845..4a234b1 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
@@ -63,7 +63,7 @@
void ParallelCommandGroup::AddCommands(
std::vector<std::unique_ptr<Command>>&& commands) {
- CommandScheduler::GetInstance().RequireUngrouped(commands);
+ CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
if (isRunning) {
throw FRC_MakeError(frc::err::CommandIllegalUse,
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
index a6b5c1c..1c9b700 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
@@ -62,7 +62,7 @@
void ParallelDeadlineGroup::AddCommands(
std::vector<std::unique_ptr<Command>>&& commands) {
- CommandScheduler::GetInstance().RequireUngrouped(commands);
+ CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
if (!m_finished) {
throw FRC_MakeError(frc::err::CommandIllegalUse,
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
index df658bc..334286f 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
@@ -50,7 +50,7 @@
void ParallelRaceGroup::AddCommands(
std::vector<std::unique_ptr<Command>>&& commands) {
- CommandScheduler::GetInstance().RequireUngrouped(commands);
+ CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
if (isRunning) {
throw FRC_MakeError(frc::err::CommandIllegalUse,
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
index 9145985..b78b6ad 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
@@ -6,9 +6,13 @@
#include <utility>
+#include <units/velocity.h>
+#include <units/voltage.h>
#include <wpi/sendable/SendableBuilder.h>
using namespace frc2;
+using kv_unit = units::compound_unit<units::volts,
+ units::inverse<units::meters_per_second>>;
RamseteCommand::RamseteCommand(
frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
@@ -42,6 +46,7 @@
: m_trajectory(std::move(trajectory)),
m_pose(std::move(pose)),
m_controller(controller),
+ m_feedforward(0_V, units::unit_t<kv_unit>{0}),
m_kinematics(std::move(kinematics)),
m_outputVel(std::move(output)),
m_usePID(false) {
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp
index 2e74c8c..2a08f1b 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RepeatCommand.cpp
@@ -9,7 +9,7 @@
using namespace frc2;
RepeatCommand::RepeatCommand(std::unique_ptr<Command>&& command) {
- CommandScheduler::GetInstance().RequireUngrouped(command.get());
+ CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(command.get());
m_command = std::move(command);
m_command->SetComposed(true);
AddRequirements(m_command->GetRequirements());
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
index b9ea3d5..173b9d9 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
@@ -62,7 +62,7 @@
void SequentialCommandGroup::AddCommands(
std::vector<std::unique_ptr<Command>>&& commands) {
- CommandScheduler::GetInstance().RequireUngrouped(commands);
+ CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(commands);
if (m_currentCommandIndex != invalid_index) {
throw FRC_MakeError(frc::err::CommandIllegalUse,
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
index 4c06f1f..a0241bd 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
@@ -4,6 +4,8 @@
#include "frc2/command/Subsystem.h"
+#include <wpi/Demangle.h>
+
#include "frc2/command/CommandPtr.h"
#include "frc2/command/Commands.h"
@@ -16,6 +18,10 @@
void Subsystem::SimulationPeriodic() {}
+std::string Subsystem::GetName() const {
+ return wpi::GetTypeName(*this);
+}
+
void Subsystem::SetDefaultCommand(CommandPtr&& defaultCommand) {
CommandScheduler::GetInstance().SetDefaultCommand(this,
std::move(defaultCommand));
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
index 8216a07..e5a4038 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
@@ -17,6 +17,11 @@
CommandScheduler::GetInstance().RegisterSubsystem({this});
}
+SubsystemBase::SubsystemBase(std::string_view name) {
+ wpi::SendableRegistry::AddLW(this, name);
+ CommandScheduler::GetInstance().RegisterSubsystem({this});
+}
+
void SubsystemBase::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("Subsystem");
builder.AddBooleanProperty(
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp
index f7928c9..6391294 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/WrapperCommand.cpp
@@ -9,7 +9,7 @@
using namespace frc2;
WrapperCommand::WrapperCommand(std::unique_ptr<Command>&& command) {
- CommandScheduler::GetInstance().RequireUngrouped(command.get());
+ CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(command.get());
m_command = std::move(command);
m_command->SetComposed(true);
// copy the wrapped command's name
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandStadiaController.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandStadiaController.cpp
new file mode 100644
index 0000000..86c34ba
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandStadiaController.cpp
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/button/CommandStadiaController.h"
+
+using namespace frc2;
+
+Trigger CommandStadiaController::Button(int button,
+ frc::EventLoop* loop) const {
+ return GenericHID::Button(button, loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::LeftBumper(frc::EventLoop* loop) const {
+ return StadiaController::LeftBumper(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::RightBumper(frc::EventLoop* loop) const {
+ return StadiaController::RightBumper(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::LeftStick(frc::EventLoop* loop) const {
+ return StadiaController::LeftStick(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::RightStick(frc::EventLoop* loop) const {
+ return StadiaController::RightStick(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::A(frc::EventLoop* loop) const {
+ return StadiaController::A(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::B(frc::EventLoop* loop) const {
+ return StadiaController::B(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::X(frc::EventLoop* loop) const {
+ return StadiaController::X(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Y(frc::EventLoop* loop) const {
+ return StadiaController::Y(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Ellipses(frc::EventLoop* loop) const {
+ return StadiaController::Ellipses(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Hamburger(frc::EventLoop* loop) const {
+ return StadiaController::Hamburger(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Stadia(frc::EventLoop* loop) const {
+ return StadiaController::Stadia(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Google(frc::EventLoop* loop) const {
+ return StadiaController::Google(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::Frame(frc::EventLoop* loop) const {
+ return StadiaController::Frame(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::LeftTrigger(frc::EventLoop* loop) const {
+ return StadiaController::LeftTrigger(loop).CastTo<Trigger>();
+}
+
+Trigger CommandStadiaController::RightTrigger(frc::EventLoop* loop) const {
+ return StadiaController::RightTrigger(loop).CastTo<Trigger>();
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/RobotModeTriggers.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/RobotModeTriggers.cpp
new file mode 100644
index 0000000..ab593d8
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/RobotModeTriggers.cpp
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc2/command/button/RobotModeTriggers.h"
+
+#include <frc/DriverStation.h>
+
+using namespace frc2;
+
+Trigger RobotModeTriggers::Autonomous() {
+ return Trigger{&frc::DriverStation::IsAutonomousEnabled};
+}
+
+Trigger RobotModeTriggers::Teleop() {
+ return Trigger{&frc::DriverStation::IsTeleopEnabled};
+}
+
+Trigger RobotModeTriggers::Disabled() {
+ return Trigger{&frc::DriverStation::IsDisabled};
+}
+
+Trigger RobotModeTriggers::Test() {
+ return Trigger{&frc::DriverStation::IsTestEnabled};
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
index 38ec741..190a544 100644
--- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
@@ -212,3 +212,7 @@
return debouncer.Calculate(condition());
});
}
+
+bool Trigger::Get() const {
+ return m_condition();
+}
diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp
new file mode 100644
index 0000000..5f677fe
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/sysid/SysIdRoutine.cpp
@@ -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.
+
+#include "frc2/command/sysid/SysIdRoutine.h"
+
+using namespace frc2::sysid;
+
+frc2::CommandPtr SysIdRoutine::Quasistatic(Direction direction) {
+ std::unordered_map<Direction, frc::sysid::State> stateOptions{
+ {Direction::kForward, frc::sysid::State::kQuasistaticForward},
+ {Direction::kReverse, frc::sysid::State::kQuasistaticReverse},
+ };
+ frc::sysid::State state = stateOptions[direction];
+ double outputSign = direction == Direction::kForward ? 1.0 : -1.0;
+
+ return m_mechanism.m_subsystem
+ ->RunOnce([this] {
+ timer.Reset();
+ timer.Start();
+ })
+ .AndThen(
+ m_mechanism.m_subsystem
+ ->Run([this, state, outputSign] {
+ m_outputVolts = outputSign * timer.Get() * m_config.m_rampRate;
+ m_mechanism.m_drive(m_outputVolts);
+ m_mechanism.m_log(this);
+ m_recordState(state);
+ })
+ .FinallyDo([this] {
+ m_mechanism.m_drive(0_V);
+ m_recordState(frc::sysid::State::kNone);
+ timer.Stop();
+ })
+ .WithName("sysid-" +
+ frc::sysid::SysIdRoutineLog::StateEnumToString(state) +
+ "-" + m_mechanism.m_name)
+ .WithTimeout(m_config.m_timeout));
+}
+
+frc2::CommandPtr SysIdRoutine::Dynamic(Direction direction) {
+ std::unordered_map<Direction, frc::sysid::State> stateOptions{
+ {Direction::kForward, frc::sysid::State::kDynamicForward},
+ {Direction::kReverse, frc::sysid::State::kDynamicReverse},
+ };
+ frc::sysid::State state = stateOptions[direction];
+ double outputSign = direction == Direction::kForward ? 1.0 : -1.0;
+
+ return m_mechanism.m_subsystem
+ ->RunOnce([this] { m_outputVolts = m_config.m_stepVoltage; })
+ .AndThen(m_mechanism.m_subsystem->Run([this, state, outputSign] {
+ m_mechanism.m_drive(m_outputVolts * outputSign);
+ m_mechanism.m_log(this);
+ m_recordState(state);
+ }))
+ .FinallyDo([this] {
+ m_mechanism.m_drive(0_V);
+ m_recordState(frc::sysid::State::kNone);
+ })
+ .WithName("sysid-" +
+ frc::sysid::SysIdRoutineLog::StateEnumToString(state) + "-" +
+ m_mechanism.m_name)
+ .WithTimeout(m_config.m_timeout);
+}
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
index 9a48b2e..ad6aa79 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -6,11 +6,13 @@
#include <functional>
#include <memory>
+#include <optional>
#include <string>
#include <units/time.h>
#include <wpi/Demangle.h>
#include <wpi/SmallSet.h>
+#include <wpi/StackTrace.h>
#include <wpi/sendable/Sendable.h>
#include "frc2/command/Requirements.h"
@@ -18,11 +20,6 @@
namespace frc2 {
-template <typename T>
-std::string GetTypeName(const T& type) {
- return wpi::Demangle(typeid(type).name());
-}
-
/**
* A state machine representing a complete action to be performed by the robot.
* Commands are run by the CommandScheduler, and can be composed into
@@ -393,6 +390,14 @@
void SetComposed(bool isComposed);
/**
+ * Get the stacktrace of where this command was composed, or an empty
+ * optional. Intended for internal use.
+ *
+ * @return optional string representation of the composition site stack trace.
+ */
+ std::optional<std::string> GetPreviousCompositionSite() const;
+
+ /**
* Whether the given command should run when the robot is disabled. Override
* to return true if the command should run when disabled.
*
@@ -421,15 +426,17 @@
protected:
Command();
+ /// Requirements set.
wpi::SmallSet<Subsystem*, 4> m_requirements;
/**
* Transfers ownership of this command to a unique pointer. Used for
* decorator methods.
*/
+ [[deprecated("Use ToPtr() instead")]]
virtual std::unique_ptr<Command> TransferOwnership() && = 0;
- bool m_isComposed = false;
+ std::optional<std::string> m_previousComposition;
};
/**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
index ed93b65..ba6fe92 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
@@ -8,6 +8,8 @@
#include <memory>
#include <utility>
+#include <wpi/deprecated.h>
+
#include "frc2/command/Command.h"
#include "frc2/command/CommandPtr.h"
@@ -34,6 +36,7 @@
}
protected:
+ WPI_DEPRECATED("Use ToPtr() instead")
std::unique_ptr<Command> TransferOwnership() && override {
return std::make_unique<CRTP>(std::move(*static_cast<CRTP*>(this)));
}
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h
index ae8c118..2610684 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h
@@ -7,6 +7,7 @@
#include <concepts>
#include <functional>
#include <memory>
+#include <optional>
#include <string>
#include <utility>
#include <vector>
@@ -27,8 +28,7 @@
*/
class CommandPtr final {
public:
- explicit CommandPtr(std::unique_ptr<Command>&& command)
- : m_ptr(std::move(command)) {}
+ explicit CommandPtr(std::unique_ptr<Command>&& command);
template <std::derived_from<Command> T>
// NOLINTNEXTLINE(bugprone-forwarding-reference-overload)
@@ -36,9 +36,11 @@
: CommandPtr(
std::make_unique<std::decay_t<T>>(std::forward<T>(command))) {}
- CommandPtr(CommandPtr&&) = default;
+ CommandPtr(CommandPtr&&);
CommandPtr& operator=(CommandPtr&&) = default;
+ explicit CommandPtr(std::nullptr_t) = delete;
+
/**
* Decorates this command to run repeatedly, restarting it when it ends, until
* this command is interrupted. The decorated command can still be canceled.
@@ -326,6 +328,7 @@
private:
std::unique_ptr<Command> m_ptr;
+ std::string m_moveOutSite{""};
void AssertValid() const;
};
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
index ce4dc91..1f8d742 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
@@ -374,7 +374,7 @@
void OnCommandFinish(Action action);
/**
- * Requires that the specified command hasn't been already added to a
+ * Requires that the specified command hasn't already been added to a
* composition.
*
* @param command The command to check
@@ -383,7 +383,7 @@
void RequireUngrouped(const Command* command);
/**
- * Requires that the specified commands not have been already added to a
+ * Requires that the specified commands have not already been added to a
* composition.
*
* @param commands The commands to check
@@ -392,7 +392,7 @@
void RequireUngrouped(std::span<const std::unique_ptr<Command>> commands);
/**
- * Requires that the specified commands not have been already added to a
+ * Requires that the specified commands have not already been added to a
* composition.
*
* @param commands The commands to check
@@ -401,6 +401,38 @@
*/
void RequireUngrouped(std::initializer_list<const Command*> commands);
+ /**
+ * Requires that the specified command has not already been added to a
+ * composition and is not currently scheduled.
+ *
+ * @param command The command to check
+ * @throws IllegalArgumentException if the given command has already been
+ * composed or scheduled.
+ */
+ void RequireUngroupedAndUnscheduled(const Command* command);
+
+ /**
+ * Requires that the specified commands have not already been added to a
+ * composition and are not currently scheduled.
+ *
+ * @param commands The commands to check
+ * @throws IllegalArgumentException if the given commands have already been
+ * composed.
+ */
+ void RequireUngroupedAndUnscheduled(
+ std::span<const std::unique_ptr<Command>> commands);
+
+ /**
+ * Requires that the specified commands have not already been added to a
+ * composition and are not currently scheduled.
+ *
+ * @param commands The commands to check
+ * @throws IllegalArgumentException if the given commands have already been
+ * composed or scheduled.
+ */
+ void RequireUngroupedAndUnscheduled(
+ std::initializer_list<const Command*> commands);
+
void InitSendable(wpi::SendableBuilder& builder) override;
private:
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
index bede6d0..ab16e7d 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
@@ -74,9 +74,16 @@
frc::PIDController& GetController();
protected:
+ /// PID controller.
frc::PIDController m_controller;
+
+ /// Measurement getter.
std::function<double()> m_measurement;
+
+ /// Setpoint getter.
std::function<double()> m_setpoint;
+
+ /// PID controller output consumer.
std::function<void(double)> m_useOutput;
};
} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
index af61430..af7d298 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -69,7 +69,10 @@
frc::PIDController& GetController();
protected:
+ /// PID controller.
frc::PIDController m_controller;
+
+ /// Whether PID controller output is enabled.
bool m_enabled{false};
/**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
index 9ea5db5..6fbb12b 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -139,9 +139,16 @@
frc::ProfiledPIDController<Distance>& GetController() { return m_controller; }
protected:
+ /// Profiled PID controller.
frc::ProfiledPIDController<Distance> m_controller;
+
+ /// Measurement getter.
std::function<Distance_t()> m_measurement;
+
+ /// Goal getter.
std::function<State()> m_goal;
+
+ /// Profiled PID controller output consumer.
std::function<void(double, State)> m_useOutput;
};
} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
index cfdfc6b..1dac7fa 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -91,7 +91,10 @@
frc::ProfiledPIDController<Distance>& GetController() { return m_controller; }
protected:
+ /// Profiled PID controller.
frc::ProfiledPIDController<Distance> m_controller;
+
+ /// Whether the profiled PID controller output is enabled.
bool m_enabled{false};
/**
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
index d425d76..cc31106 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
@@ -17,6 +17,7 @@
#include <utility>
#include <vector>
+#include <wpi/deprecated.h>
#include <wpi/sendable/SendableBuilder.h>
#include "frc2/command/Command.h"
@@ -56,7 +57,8 @@
m_defaultCommand.SetComposed(true);
for (auto&& command : foo) {
- CommandScheduler::GetInstance().RequireUngrouped(command.second.get());
+ CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(
+ command.second.get());
command.second.get()->SetComposed(true);
}
@@ -77,7 +79,8 @@
: m_selector{std::move(selector)} {
m_defaultCommand.SetComposed(true);
for (auto&& command : commands) {
- CommandScheduler::GetInstance().RequireUngrouped(command.second.get());
+ CommandScheduler::GetInstance().RequireUngroupedAndUnscheduled(
+ command.second.get());
command.second.get()->SetComposed(true);
}
@@ -132,6 +135,7 @@
}
protected:
+ WPI_DEPRECATED("Use ToPtr() instead")
std::unique_ptr<Command> TransferOwnership() && override {
return std::make_unique<SelectCommand>(std::move(*this));
}
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
index cdac0c0..b95b837 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
@@ -6,6 +6,7 @@
#include <concepts>
#include <functional>
+#include <string>
#include <utility>
#include <wpi/FunctionExtras.h>
@@ -60,6 +61,13 @@
virtual void SimulationPeriodic();
/**
+ * Gets the name of this Subsystem.
+ *
+ * @return Name
+ */
+ virtual std::string GetName() const;
+
+ /**
* Sets the default Command of the subsystem. The default command will be
* automatically scheduled when no other commands are scheduled that require
* the subsystem. Default commands should generally not end on their own, i.e.
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h b/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
index 86fb026..444aca5 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
@@ -30,7 +30,7 @@
*
* @return Name
*/
- std::string GetName() const;
+ std::string GetName() const override;
/**
* Sets the name of this Subsystem.
@@ -63,6 +63,15 @@
void AddChild(std::string name, wpi::Sendable* child);
protected:
+ /**
+ * Constructor. Telemetry/log name defaults to the classname.
+ */
SubsystemBase();
+ /**
+ * Constructor.
+ *
+ * @param name Name of the subsystem for telemetry and logging.
+ */
+ explicit SubsystemBase(std::string_view name);
};
} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
index 69cc8d8..289b1cb 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -79,7 +79,7 @@
void Initialize() override { m_timer.Restart(); }
void Execute() override {
- m_output(m_profile.Calculate(m_timer.Get(), m_goal(), m_currentState()));
+ m_output(m_profile.Calculate(m_timer.Get(), m_currentState(), m_goal()));
}
void End(bool interrupted) override { m_timer.Stop(); }
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
index e28615e..6f6db40 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
@@ -41,6 +41,7 @@
void InitSendable(wpi::SendableBuilder& builder) override;
protected:
+ /// The timer used for waiting.
frc::Timer m_timer;
private:
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h
index 98e8b20..97a8693 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/WrapperCommand.h
@@ -69,6 +69,7 @@
wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
protected:
+ /// Command being wrapped.
std::unique_ptr<Command> m_command;
};
} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandStadiaController.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandStadiaController.h
new file mode 100644
index 0000000..7a1b549
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandStadiaController.h
@@ -0,0 +1,201 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+#include <frc/StadiaController.h>
+
+#include "Trigger.h"
+#include "frc2/command/CommandScheduler.h"
+
+namespace frc2 {
+/**
+ * A version of {@link StadiaController} with {@link Trigger} factories for
+ * command-based.
+ *
+ * @see StadiaController
+ */
+class CommandStadiaController : public frc::StadiaController {
+ public:
+ using StadiaController::StadiaController;
+
+ /**
+ * 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. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the button's digital signal attached
+ * to the given loop.
+ */
+ Trigger Button(int button,
+ frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the left bumper's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the left bumper's digital signal
+ * attached to the given loop.
+ */
+ Trigger LeftBumper(frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the right bumper's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the right bumper's digital signal
+ * attached to the given loop.
+ */
+ Trigger RightBumper(frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the left stick's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the left stick's digital signal
+ * attached to the given loop.
+ */
+ Trigger LeftStick(frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the right stick's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the right stick's digital signal
+ * attached to the given loop.
+ */
+ Trigger RightStick(frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the A button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the A button's digital signal
+ * attached to the given loop.
+ */
+ Trigger A(frc::EventLoop* loop =
+ CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the B button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the B button's digital signal
+ * attached to the given loop.
+ */
+ Trigger B(frc::EventLoop* loop =
+ CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the X button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the X button's digital signal
+ * attached to the given loop.
+ */
+ Trigger X(frc::EventLoop* loop =
+ CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the Y button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the Y button's digital signal
+ * attached to the given loop.
+ */
+ Trigger Y(frc::EventLoop* loop =
+ CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the ellipses button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the ellipses button's digital signal
+ * attached to the given loop.
+ */
+ Trigger Ellipses(frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the hamburger button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the hamburger button's digital
+ * signal attached to the given loop.
+ */
+ Trigger Hamburger(frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the stadia button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the stadia button's digital signal
+ * attached to the given loop.
+ */
+ Trigger Stadia(frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the google button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the google button's digital signal
+ * attached to the given loop.
+ */
+ Trigger Google(frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the frame button's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the frame button's digital signal
+ * attached to the given loop.
+ */
+ Trigger Frame(frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the left trigger's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the left trigger's digital signal
+ * attached to the given loop.
+ */
+ Trigger LeftTrigger(frc::EventLoop* loop = CommandScheduler::GetInstance()
+ .GetDefaultButtonLoop()) const;
+
+ /**
+ * Constructs an event instance around the right trigger's digital signal.
+ *
+ * @param loop the event loop instance to attach the event to. Defaults to the
+ * CommandScheduler's default loop.
+ * @return an event instance representing the right trigger's digital signal
+ * attached to the given loop.
+ */
+ Trigger RightTrigger(
+ frc::EventLoop* loop =
+ CommandScheduler::GetInstance().GetDefaultButtonLoop()) const;
+};
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/RobotModeTriggers.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/RobotModeTriggers.h
new file mode 100644
index 0000000..2fbcc64
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/RobotModeTriggers.h
@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc2/command/button/Trigger.h"
+
+namespace frc2 {
+
+/**
+ * A class containing static Trigger factories for running callbacks when robot
+ * mode changes.
+ */
+class RobotModeTriggers {
+ public:
+ RobotModeTriggers() = delete;
+
+ /**
+ * 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.
+ */
+ static Trigger Autonomous();
+
+ /**
+ * 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.
+ */
+ static Trigger Teleop();
+
+ /**
+ * Returns a trigger that is true when the robot is disabled.
+ *
+ * @return A trigger that is true when the robot is disabled.
+ */
+ static Trigger Disabled();
+
+ /**
+ * 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.
+ */
+ static Trigger Test();
+};
+
+} // namespace frc2
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
index 533e0a6..4438aac 100644
--- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
@@ -268,6 +268,12 @@
frc::Debouncer::DebounceType type =
frc::Debouncer::DebounceType::kRising);
+ /**
+ * Returns the current state of this trigger.
+ * @return A bool representing the current state of the trigger.
+ */
+ bool Get() const;
+
private:
frc::EventLoop* m_loop;
std::function<bool()> m_condition;
diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h b/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h
new file mode 100644
index 0000000..021059f
--- /dev/null
+++ b/wpilibNewCommands/src/main/native/include/frc2/command/sysid/SysIdRoutine.h
@@ -0,0 +1,200 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <string>
+#include <string_view>
+#include <utility>
+
+#include <frc/Timer.h>
+#include <frc/sysid/SysIdRoutineLog.h>
+
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/Subsystem.h"
+
+namespace frc2::sysid {
+
+using ramp_rate_t = units::unit_t<
+ units::compound_unit<units::volt, units::inverse<units::second>>>;
+
+/** Hardware-independent configuration for a SysId test routine. */
+class Config {
+ public:
+ /// The voltage ramp rate used for quasistatic test routines.
+ ramp_rate_t m_rampRate{1_V / 1_s};
+
+ /// The step voltage output used for dynamic test routines.
+ units::volt_t m_stepVoltage{7_V};
+
+ /// Safety timeout for the test routine commands.
+ units::second_t m_timeout{10_s};
+
+ /// Optional handle for recording test state in a third-party logging
+ /// solution.
+ std::function<void(frc::sysid::State)> m_recordState;
+
+ /**
+ * Create a new configuration for a SysId test routine.
+ *
+ * @param rampRate The voltage ramp rate used for quasistatic test routines.
+ * Defaults to 1 volt per second if left null.
+ * @param stepVoltage The step voltage output used for dynamic test routines.
+ * Defaults to 7 volts if left null.
+ * @param timeout Safety timeout for the test routine commands. Defaults to 10
+ * seconds if left null.
+ * @param recordState Optional handle for recording test state in a
+ * third-party logging solution. If provided, the test routine state will be
+ * passed to this callback instead of logged in WPILog.
+ */
+ Config(std::optional<ramp_rate_t> rampRate,
+ std::optional<units::volt_t> stepVoltage,
+ std::optional<units::second_t> timeout,
+ std::optional<std::function<void(frc::sysid::State)>> recordState) {
+ if (rampRate) {
+ m_rampRate = rampRate.value();
+ }
+ if (stepVoltage) {
+ m_stepVoltage = stepVoltage.value();
+ }
+ if (timeout) {
+ m_timeout = timeout.value();
+ }
+ if (recordState) {
+ m_recordState = recordState.value();
+ }
+ }
+};
+
+class Mechanism {
+ public:
+ /// Sends the SysId-specified drive signal to the mechanism motors during test
+ /// routines.
+ std::function<void(units::volt_t)> m_drive;
+
+ /// Returns measured data (voltages, positions, velocities) of the mechanism
+ /// motors during test routines.
+ std::function<void(frc::sysid::SysIdRoutineLog*)> m_log;
+
+ /// The subsystem containing the motor(s) that is (or are) being
+ /// characterized.
+ frc2::Subsystem* m_subsystem;
+
+ /// The name of the mechanism being tested. Will be appended to the log entry
+ /// title for the routine's test state, e.g. "sysid-test-state-mechanism".
+ std::string m_name;
+
+ /**
+ * Create a new mechanism specification for a SysId routine.
+ *
+ * @param drive Sends the SysId-specified drive signal to the mechanism motors
+ * during test routines.
+ * @param log Returns measured data of the mechanism motors during test
+ * routines. To return data, call `Motor(string motorName)` on the supplied
+ * `SysIdRoutineLog` instance, and then call one or more of the chainable
+ * logging handles (e.g. `voltage`) on the returned `MotorLog`. Multiple
+ * motors can be logged in a single callback by calling `Motor` multiple
+ * times.
+ * @param subsystem The subsystem containing the motor(s) that is (or are)
+ * being characterized. Will be declared as a requirement for the returned
+ * test commands.
+ * @param name The name of the mechanism being tested. Will be appended to the
+ * log entry * title for the routine's test state, e.g.
+ * "sysid-test-state-mechanism". Defaults to the name of the subsystem if
+ * left null.
+ */
+ Mechanism(std::function<void(units::volt_t)> drive,
+ std::function<void(frc::sysid::SysIdRoutineLog*)> log,
+ frc2::Subsystem* subsystem, std::string_view name)
+ : m_drive{std::move(drive)},
+ m_log{std::move(log)},
+ m_subsystem{subsystem},
+ m_name{name} {}
+
+ /**
+ * Create a new mechanism specification for a SysId routine. Defaults the
+ * mechanism name to the subsystem name.
+ *
+ * @param drive Sends the SysId-specified drive signal to the mechanism motors
+ * during test routines.
+ * @param log Returns measured data of the mechanism motors during test
+ * routines. To return data, call `Motor(string motorName)` on the supplied
+ * `SysIdRoutineLog` instance, and then call one or more of the chainable
+ * logging handles (e.g. `voltage`) on the returned `MotorLog`. Multiple
+ * motors can be logged in a single callback by calling `Motor` multiple
+ * times.
+ * @param subsystem The subsystem containing the motor(s) that is (or are)
+ * being characterized. Will be declared as a requirement for the returned
+ * test commands. The subsystem's `name` will be appended to the log entry
+ * title for the routine's test state, e.g. "sysid-test-state-subsystem".
+ */
+ Mechanism(std::function<void(units::volt_t)> drive,
+ std::function<void(frc::sysid::SysIdRoutineLog*)> log,
+ frc2::Subsystem* subsystem)
+ : m_drive{std::move(drive)},
+ m_log{std::move(log)},
+ m_subsystem{subsystem},
+ m_name{m_subsystem->GetName()} {}
+};
+
+/**
+ * Motor direction for a SysId test.
+ */
+enum Direction {
+ /// Forward.
+ kForward,
+ /// Reverse.
+ kReverse
+};
+
+/**
+ * A SysId characterization routine for a single mechanism. Mechanisms may have
+ * multiple motors.
+ *
+ * A single subsystem may have multiple mechanisms, but mechanisms should not
+ * share test routines. Each complete test of a mechanism should have its own
+ * SysIdRoutine instance, since the log name of the recorded data is determined
+ * by the mechanism name.
+ *
+ * The test state (e.g. "quasistatic-forward") is logged once per iteration
+ * during test execution, and once with state "none" when a test ends. Motor
+ * frames are logged every iteration during test execution.
+ *
+ * Timestamps are not coordinated across data, so motor frames and test state
+ * tags may be recorded on different log frames. Because frame alignment is not
+ * guaranteed, SysId parses the log by using the test state flag to determine
+ * the timestamp range for each section of the test, and then extracts the motor
+ * frames within the valid timestamp ranges. If a given test was run multiple
+ * times in a single logfile, the user will need to select which of the tests to
+ * use for the fit in the analysis tool.
+ */
+class SysIdRoutine : public frc::sysid::SysIdRoutineLog {
+ public:
+ /**
+ * Create a new SysId characterization routine.
+ *
+ * @param config Hardware-independent parameters for the SysId routine.
+ * @param mechanism Hardware interface for the SysId routine.
+ */
+ SysIdRoutine(Config config, Mechanism mechanism)
+ : SysIdRoutineLog(mechanism.m_subsystem->GetName()),
+ m_config(config),
+ m_mechanism(mechanism),
+ m_recordState(config.m_recordState ? config.m_recordState
+ : [this](frc::sysid::State state) {
+ this->RecordState(state);
+ }) {}
+
+ frc2::CommandPtr Quasistatic(Direction direction);
+ frc2::CommandPtr Dynamic(Direction direction);
+
+ private:
+ Config m_config;
+ Mechanism m_mechanism;
+ units::volt_t m_outputVolts{0};
+ std::function<void(frc::sysid::State)> m_recordState;
+ frc::Timer timer;
+};
+} // namespace frc2::sysid
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
deleted file mode 100644
index ba4a77b..0000000
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
+++ /dev/null
@@ -1,47 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.wpilibj2.command;
-
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-
-import org.junit.jupiter.api.Test;
-
-class CommandGroupErrorTest extends CommandTestBase {
- @Test
- void commandInMultipleGroupsTest() {
- MockCommandHolder command1Holder = new MockCommandHolder(true);
- Command command1 = command1Holder.getMock();
- MockCommandHolder command2Holder = new MockCommandHolder(true);
- Command command2 = command2Holder.getMock();
-
- new ParallelCommandGroup(command1, command2);
- assertThrows(
- IllegalArgumentException.class, () -> new ParallelCommandGroup(command1, command2));
- }
-
- @Test
- void commandInGroupExternallyScheduledTest() {
- MockCommandHolder command1Holder = new MockCommandHolder(true);
- Command command1 = command1Holder.getMock();
- MockCommandHolder command2Holder = new MockCommandHolder(true);
- Command command2 = command2Holder.getMock();
-
- new ParallelCommandGroup(command1, command2);
-
- assertThrows(
- IllegalArgumentException.class, () -> CommandScheduler.getInstance().schedule(command1));
- }
-
- @Test
- void redecoratedCommandErrorTest() {
- Command command = new InstantCommand();
-
- assertDoesNotThrow(() -> command.withTimeout(10).until(() -> false));
- assertThrows(IllegalArgumentException.class, () -> command.withTimeout(10));
- CommandScheduler.getInstance().removeComposedCommand(command);
- assertDoesNotThrow(() -> command.withTimeout(10));
- }
-}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java
index 2b79550..9fafc59 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java
@@ -5,6 +5,7 @@
package edu.wpi.first.wpilibj2.command;
import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.params.provider.Arguments.arguments;
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
@@ -13,11 +14,11 @@
import org.junit.jupiter.params.provider.Arguments;
import org.junit.jupiter.params.provider.MethodSource;
-interface MultiCompositionTestBase<T extends Command> extends SingleCompositionTestBase<T> {
- T compose(Command... members);
+abstract class MultiCompositionTestBase<T extends Command> extends SingleCompositionTestBase<T> {
+ abstract T compose(Command... members);
@Override
- default T composeSingle(Command member) {
+ T composeSingle(Command member) {
return compose(member);
}
@@ -63,7 +64,7 @@
@MethodSource
@ParameterizedTest(name = "interruptible[{index}]: {0}")
- default void interruptible(
+ void interruptible(
@SuppressWarnings("unused") String name,
InterruptionBehavior expected,
Command command1,
@@ -103,7 +104,7 @@
@MethodSource
@ParameterizedTest(name = "runsWhenDisabled[{index}]: {0}")
- default void runsWhenDisabled(
+ void runsWhenDisabled(
@SuppressWarnings("unused") String name,
boolean expected,
Command command1,
@@ -112,4 +113,19 @@
var command = compose(command1, command2, command3);
assertEquals(expected, command.runsWhenDisabled());
}
+
+ static Stream<Arguments> composeDuplicates() {
+ Command a = new InstantCommand(() -> {});
+ Command b = new InstantCommand(() -> {});
+ return Stream.of(
+ arguments("AA", new Command[] {a, a}),
+ arguments("ABA", new Command[] {a, b, a}),
+ arguments("BAA", new Command[] {b, a, a}));
+ }
+
+ @MethodSource
+ @ParameterizedTest(name = "composeDuplicates[{index}]: {0}")
+ void composeDuplicates(@SuppressWarnings("unused") String name, Command[] commands) {
+ assertThrows(IllegalArgumentException.class, () -> compose(commands));
+ }
}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
index 7dc9110..a837baa 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
@@ -14,8 +14,7 @@
import org.junit.jupiter.api.Test;
-class ParallelCommandGroupTest extends CommandTestBase
- implements MultiCompositionTestBase<ParallelCommandGroup> {
+class ParallelCommandGroupTest extends MultiCompositionTestBase<ParallelCommandGroup> {
@Test
void parallelGroupScheduleTest() {
try (CommandScheduler scheduler = new CommandScheduler()) {
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
index 6fa644b..206d801 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj2.command;
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -14,8 +15,7 @@
import java.util.Arrays;
import org.junit.jupiter.api.Test;
-class ParallelDeadlineGroupTest extends CommandTestBase
- implements MultiCompositionTestBase<ParallelDeadlineGroup> {
+class ParallelDeadlineGroupTest extends MultiCompositionTestBase<ParallelDeadlineGroup> {
@Test
void parallelDeadlineScheduleTest() {
try (CommandScheduler scheduler = new CommandScheduler()) {
@@ -125,6 +125,21 @@
IllegalArgumentException.class, () -> new ParallelDeadlineGroup(command1, command2));
}
+ @Test
+ void parallelDeadlineSetDeadlineToDeadlineTest() {
+ Command a = new InstantCommand(() -> {});
+ ParallelDeadlineGroup group = new ParallelDeadlineGroup(a);
+ assertDoesNotThrow(() -> group.setDeadline(a));
+ }
+
+ @Test
+ void parallelDeadlineSetDeadlineDuplicateTest() {
+ Command a = new InstantCommand(() -> {});
+ Command b = new InstantCommand(() -> {});
+ ParallelDeadlineGroup group = new ParallelDeadlineGroup(a, b);
+ assertThrows(IllegalArgumentException.class, () -> group.setDeadline(b));
+ }
+
@Override
public ParallelDeadlineGroup compose(Command... members) {
return new ParallelDeadlineGroup(members[0], Arrays.copyOfRange(members, 1, members.length));
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
index 6752496..ea780c6 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
@@ -16,8 +16,7 @@
import org.junit.jupiter.api.Test;
-class ParallelRaceGroupTest extends CommandTestBase
- implements MultiCompositionTestBase<ParallelRaceGroup> {
+class ParallelRaceGroupTest extends MultiCompositionTestBase<ParallelRaceGroup> {
@Test
void parallelRaceScheduleTest() {
try (CommandScheduler scheduler = new CommandScheduler()) {
@@ -153,7 +152,7 @@
scheduler.run();
command2Holder.setFinished(true);
// at this point the sequential group should be done
- assertDoesNotThrow(() -> scheduler.run());
+ assertDoesNotThrow(scheduler::run);
assertFalse(scheduler.isScheduled(group2));
}
}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java
index 53abdd8..2aaecc6 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java
@@ -10,8 +10,7 @@
import java.util.concurrent.atomic.AtomicInteger;
import org.junit.jupiter.api.Test;
-class RepeatCommandTest extends CommandTestBase
- implements SingleCompositionTestBase<RepeatCommand> {
+class RepeatCommandTest extends SingleCompositionTestBase<RepeatCommand> {
@Test
void callsMethodsCorrectly() {
try (CommandScheduler scheduler = new CommandScheduler()) {
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
index 5e8fd39..a253740 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java
@@ -288,7 +288,7 @@
scheduler.schedule(cCancelsD);
scheduler.schedule(dCancelsAll);
- assertDoesNotThrow(() -> scheduler.run());
+ assertDoesNotThrow(scheduler::run);
assertEquals(4, counter.get());
assertFalse(scheduler.isScheduled(aCancelsB));
assertFalse(scheduler.isScheduled(bCancelsC));
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
index 0ce3b79..e364468 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
@@ -13,8 +13,7 @@
import java.util.Map;
import org.junit.jupiter.api.Test;
-class SelectCommandTest extends CommandTestBase
- implements MultiCompositionTestBase<SelectCommand<Integer>> {
+class SelectCommandTest extends MultiCompositionTestBase<SelectCommand<Integer>> {
@Test
void selectCommandTest() {
try (CommandScheduler scheduler = new CommandScheduler()) {
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
index ff578f4..abbc490 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
@@ -12,8 +12,7 @@
import org.junit.jupiter.api.Test;
-class SequentialCommandGroupTest extends CommandTestBase
- implements MultiCompositionTestBase<SequentialCommandGroup> {
+class SequentialCommandGroupTest extends MultiCompositionTestBase<SequentialCommandGroup> {
@Test
void sequentialGroupScheduleTest() {
try (CommandScheduler scheduler = new CommandScheduler()) {
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java
index 4e54e3e..a063a1d 100644
--- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java
@@ -5,17 +5,19 @@
package edu.wpi.first.wpilibj2.command;
import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import org.junit.jupiter.api.Test;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.EnumSource;
import org.junit.jupiter.params.provider.ValueSource;
-public interface SingleCompositionTestBase<T extends Command> {
- T composeSingle(Command member);
+public abstract class SingleCompositionTestBase<T extends Command> extends CommandTestBase {
+ abstract T composeSingle(Command member);
@EnumSource(Command.InterruptionBehavior.class)
@ParameterizedTest
- default void interruptible(Command.InterruptionBehavior interruptionBehavior) {
+ void interruptible(Command.InterruptionBehavior interruptionBehavior) {
var command =
composeSingle(
new WaitUntilCommand(() -> false).withInterruptBehavior(interruptionBehavior));
@@ -24,9 +26,38 @@
@ValueSource(booleans = {true, false})
@ParameterizedTest
- default void runWhenDisabled(boolean runsWhenDisabled) {
+ void runWhenDisabled(boolean runsWhenDisabled) {
var command =
composeSingle(new WaitUntilCommand(() -> false).ignoringDisable(runsWhenDisabled));
assertEquals(runsWhenDisabled, command.runsWhenDisabled());
}
+
+ @Test
+ void commandInOtherCompositionTest() {
+ var command = new InstantCommand();
+ new WrapperCommand(command) {};
+ assertThrows(IllegalArgumentException.class, () -> composeSingle(command));
+ }
+
+ @Test
+ void commandInMultipleCompositionsTest() {
+ var command = new InstantCommand();
+ composeSingle(command);
+ assertThrows(IllegalArgumentException.class, () -> composeSingle(command));
+ }
+
+ @Test
+ void composeThenScheduleTest() {
+ var command = new InstantCommand();
+ composeSingle(command);
+ assertThrows(
+ IllegalArgumentException.class, () -> CommandScheduler.getInstance().schedule(command));
+ }
+
+ @Test
+ void scheduleThenComposeTest() {
+ var command = new RunCommand(() -> {});
+ CommandScheduler.getInstance().schedule(command);
+ assertThrows(IllegalArgumentException.class, () -> composeSingle(command));
+ }
}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java
new file mode 100644
index 0000000..b2b41c4
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.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 edu.wpi.first.wpilibj2.command.button;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj2.command.CommandTestBase;
+import org.junit.jupiter.api.Test;
+
+class RobotModeTriggersTest extends CommandTestBase {
+ @Test
+ void autonomousTest() {
+ DriverStationSim.resetData();
+ DriverStationSim.setAutonomous(true);
+ DriverStationSim.setTest(false);
+ DriverStationSim.setEnabled(true);
+ DriverStation.refreshData();
+ Trigger auto = RobotModeTriggers.autonomous();
+ assertTrue(auto.getAsBoolean());
+ }
+
+ @Test
+ void teleopTest() {
+ DriverStationSim.resetData();
+ DriverStationSim.setAutonomous(false);
+ DriverStationSim.setTest(false);
+ DriverStationSim.setEnabled(true);
+ DriverStation.refreshData();
+ Trigger teleop = RobotModeTriggers.teleop();
+ assertTrue(teleop.getAsBoolean());
+ }
+
+ @Test
+ void testModeTest() {
+ DriverStationSim.resetData();
+ DriverStationSim.setAutonomous(false);
+ DriverStationSim.setTest(true);
+ DriverStationSim.setEnabled(true);
+ DriverStation.refreshData();
+ Trigger test = RobotModeTriggers.test();
+ assertTrue(test.getAsBoolean());
+ }
+
+ @Test
+ void disabledTest() {
+ DriverStationSim.resetData();
+ DriverStationSim.setAutonomous(false);
+ DriverStationSim.setTest(false);
+ DriverStationSim.setEnabled(false);
+ DriverStation.refreshData();
+ Trigger disabled = RobotModeTriggers.disabled();
+ assertTrue(disabled.getAsBoolean());
+ }
+}
diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java
new file mode 100644
index 0000000..3d885b2
--- /dev/null
+++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java
@@ -0,0 +1,144 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.sysid;
+
+import static edu.wpi.first.units.Units.Volts;
+import static org.mockito.ArgumentMatchers.any;
+import static org.mockito.Mockito.atLeastOnce;
+import static org.mockito.Mockito.clearInvocations;
+import static org.mockito.Mockito.inOrder;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class SysIdRoutineTest {
+ interface Mechanism extends Subsystem {
+ void recordState(SysIdRoutineLog.State state);
+
+ void drive(Measure<Voltage> voltage);
+
+ void log(SysIdRoutineLog log);
+ }
+
+ Mechanism m_mechanism;
+ SysIdRoutine m_sysidRoutine;
+ Command m_quasistaticForward;
+ Command m_quasistaticReverse;
+ Command m_dynamicForward;
+ Command m_dynamicReverse;
+
+ void runCommand(Command command) {
+ command.initialize();
+ command.execute();
+ command.execute();
+ SimHooks.stepTiming(1);
+ command.execute();
+ command.end(true);
+ }
+
+ @BeforeEach
+ void setup() {
+ HAL.initialize(500, 0);
+ SimHooks.pauseTiming();
+ m_mechanism = mock(Mechanism.class);
+ m_sysidRoutine =
+ new SysIdRoutine(
+ new SysIdRoutine.Config(null, null, null, m_mechanism::recordState),
+ new SysIdRoutine.Mechanism(m_mechanism::drive, m_mechanism::log, new Subsystem() {}));
+ m_quasistaticForward = m_sysidRoutine.quasistatic(SysIdRoutine.Direction.kForward);
+ m_quasistaticReverse = m_sysidRoutine.quasistatic(SysIdRoutine.Direction.kReverse);
+ m_dynamicForward = m_sysidRoutine.dynamic(SysIdRoutine.Direction.kForward);
+ m_dynamicReverse = m_sysidRoutine.dynamic(SysIdRoutine.Direction.kReverse);
+ }
+
+ @AfterEach
+ void cleanupAll() {
+ SimHooks.resumeTiming();
+ }
+
+ @Test
+ void recordStateBookendsMotorLogging() {
+ runCommand(m_quasistaticForward);
+
+ var orderCheck = inOrder(m_mechanism);
+
+ orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kQuasistaticForward);
+ orderCheck.verify(m_mechanism).drive(any());
+ orderCheck.verify(m_mechanism).log(any());
+ orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kNone);
+ orderCheck.verifyNoMoreInteractions();
+
+ clearInvocations(m_mechanism);
+ orderCheck = inOrder(m_mechanism);
+ runCommand(m_dynamicForward);
+
+ orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kDynamicForward);
+ orderCheck.verify(m_mechanism).drive(any());
+ orderCheck.verify(m_mechanism).log(any());
+ orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kNone);
+ orderCheck.verifyNoMoreInteractions();
+ }
+
+ @Test
+ void testsDeclareCorrectState() {
+ runCommand(m_quasistaticForward);
+ verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kQuasistaticForward);
+
+ runCommand(m_quasistaticReverse);
+ verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kQuasistaticReverse);
+
+ runCommand(m_dynamicForward);
+ verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kDynamicForward);
+
+ runCommand(m_dynamicReverse);
+ verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kDynamicReverse);
+ }
+
+ @Test
+ void testsOutputCorrectVoltage() {
+ runCommand(m_quasistaticForward);
+ var orderCheck = inOrder(m_mechanism);
+
+ orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(1));
+ orderCheck.verify(m_mechanism).drive(Volts.of(0));
+ orderCheck.verify(m_mechanism, never()).drive(any());
+
+ clearInvocations(m_mechanism);
+ runCommand(m_quasistaticReverse);
+ orderCheck = inOrder(m_mechanism);
+
+ orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(-1));
+ orderCheck.verify(m_mechanism).drive(Volts.of(0));
+ orderCheck.verify(m_mechanism, never()).drive(any());
+
+ clearInvocations(m_mechanism);
+ runCommand(m_dynamicForward);
+ orderCheck = inOrder(m_mechanism);
+
+ orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(7));
+ orderCheck.verify(m_mechanism).drive(Volts.of(0));
+ orderCheck.verify(m_mechanism, never()).drive(any());
+
+ clearInvocations(m_mechanism);
+ runCommand(m_dynamicForward);
+ orderCheck = inOrder(m_mechanism);
+
+ runCommand(m_dynamicReverse);
+ orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(-7));
+ orderCheck.verify(m_mechanism).drive(Volts.of(0));
+ orderCheck.verify(m_mechanism, never()).drive(any());
+ }
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp
index 6c57c7f..75ec040 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp
@@ -34,3 +34,7 @@
EXPECT_EQ(1, counter);
}
+
+TEST_F(CommandPtrTest, NullInitialization) {
+ EXPECT_THROW(CommandPtr{std::unique_ptr<Command>{}}, frc::RuntimeError);
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
index 28d498c..ff5384c 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
@@ -20,7 +20,7 @@
MockCommand* command1 = command1Holder.get();
MockCommand* command2 = command2Holder.get();
- ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+ ParallelCommandGroup group(make_vector<std::unique_ptr<Command>>(
std::move(command1Holder), std::move(command2Holder)));
EXPECT_CALL(*command1, Initialize());
@@ -50,7 +50,7 @@
MockCommand* command1 = command1Holder.get();
MockCommand* command2 = command2Holder.get();
- ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+ ParallelCommandGroup group(make_vector<std::unique_ptr<Command>>(
std::move(command1Holder), std::move(command2Holder)));
EXPECT_CALL(*command1, Initialize());
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
index 4abab33..f35bd00 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
@@ -24,8 +24,8 @@
ParallelDeadlineGroup group(
std::move(command1Holder),
- tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
- std::move(command3Holder)));
+ make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+ std::move(command3Holder)));
EXPECT_CALL(*command1, Initialize());
EXPECT_CALL(*command1, Execute()).Times(2);
@@ -64,8 +64,8 @@
ParallelDeadlineGroup group(
std::move(command1Holder),
- tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
- std::move(command3Holder)));
+ make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+ std::move(command3Holder)));
EXPECT_CALL(*command1, Initialize());
EXPECT_CALL(*command1, Execute()).Times(1);
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
index 3df2147..be18fc3 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
@@ -23,7 +23,7 @@
MockCommand* command2 = command2Holder.get();
MockCommand* command3 = command3Holder.get();
- ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ ParallelRaceGroup group{make_vector<std::unique_ptr<Command>>(
std::move(command1Holder), std::move(command2Holder),
std::move(command3Holder))};
@@ -59,7 +59,7 @@
MockCommand* command2 = command2Holder.get();
MockCommand* command3 = command3Holder.get();
- ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ ParallelRaceGroup group{make_vector<std::unique_ptr<Command>>(
std::move(command1Holder), std::move(command2Holder),
std::move(command3Holder))};
@@ -164,7 +164,7 @@
MockCommand* command2 = command2Holder.get();
MockCommand* command3 = command3Holder.get();
- ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ ParallelRaceGroup group{make_vector<std::unique_ptr<Command>>(
std::move(command1Holder), std::move(command2Holder),
std::move(command3Holder))};
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
index 0048b24..e128313 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
@@ -22,7 +22,7 @@
MockCommand* command2 = command2Holder.get();
MockCommand* command3 = command3Holder.get();
- SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ SequentialCommandGroup group{make_vector<std::unique_ptr<Command>>(
std::move(command1Holder), std::move(command2Holder),
std::move(command3Holder))};
@@ -61,7 +61,7 @@
MockCommand* command2 = command2Holder.get();
MockCommand* command3 = command3Holder.get();
- SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ SequentialCommandGroup group{make_vector<std::unique_ptr<Command>>(
std::move(command1Holder), std::move(command2Holder),
std::move(command3Holder))};
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp
new file mode 100644
index 0000000..cb9e966
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp
@@ -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.
+
+#include <frc/DriverStation.h>
+#include <frc/simulation/DriverStationSim.h>
+
+#include "../CommandTestBase.h"
+#include "frc2/command/button/RobotModeTriggers.h"
+#include "frc2/command/button/Trigger.h"
+
+using namespace frc2;
+using namespace frc::sim;
+class RobotModeTriggersTest : public CommandTestBase {};
+
+TEST(RobotModeTriggersTest, Autonomous) {
+ DriverStationSim::ResetData();
+ DriverStationSim::SetAutonomous(true);
+ DriverStationSim::SetTest(false);
+ DriverStationSim::SetEnabled(true);
+ frc::DriverStation::RefreshData();
+ Trigger autonomous = RobotModeTriggers::Autonomous();
+ EXPECT_TRUE(autonomous.Get());
+}
+
+TEST(RobotModeTriggersTest, Teleop) {
+ DriverStationSim::ResetData();
+ DriverStationSim::SetAutonomous(false);
+ DriverStationSim::SetTest(false);
+ DriverStationSim::SetEnabled(true);
+ frc::DriverStation::RefreshData();
+ Trigger teleop = RobotModeTriggers::Teleop();
+ EXPECT_TRUE(teleop.Get());
+}
+
+TEST(RobotModeTriggersTest, Disabled) {
+ DriverStationSim::ResetData();
+ DriverStationSim::SetAutonomous(false);
+ DriverStationSim::SetTest(false);
+ DriverStationSim::SetEnabled(false);
+ frc::DriverStation::RefreshData();
+ Trigger disabled = RobotModeTriggers::Disabled();
+ EXPECT_TRUE(disabled.Get());
+}
+
+TEST(RobotModeTriggersTest, TestMode) {
+ DriverStationSim::ResetData();
+ DriverStationSim::SetAutonomous(false);
+ DriverStationSim::SetTest(true);
+ DriverStationSim::SetEnabled(true);
+ frc::DriverStation::RefreshData();
+ Trigger test = RobotModeTriggers::Test();
+ EXPECT_TRUE(test.Get());
+}
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
index 996ddba..295eba7 100644
--- a/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
@@ -8,59 +8,27 @@
#include <utility>
#include <vector>
-namespace tcb {
-
-namespace detail {
-
-template <typename T, typename...>
-struct vec_type_helper {
- using type = T;
-};
-
-template <typename... Args>
-struct vec_type_helper<void, Args...> {
- using type = typename std::common_type_t<Args...>;
-};
-
-template <typename T, typename... Args>
-using vec_type_helper_t = typename vec_type_helper<T, Args...>::type;
-
-template <typename, typename...>
-struct all_constructible_and_convertible : std::true_type {};
-
-template <typename T, typename First, typename... Rest>
-struct all_constructible_and_convertible<T, First, Rest...>
- : std::conditional_t<
- std::is_constructible_v<T, First> && std::is_convertible_v<First, T>,
- all_constructible_and_convertible<T, Rest...>, std::false_type> {};
-
-template <typename T, typename First, typename... Rest>
-inline constexpr bool all_constructible_and_convertible_v =
- all_constructible_and_convertible<T, First, Rest...>::value;
-
-template <typename T, typename... Args>
- requires(!std::is_trivially_copyable_v<T>)
-std::vector<T> make_vector_impl(Args&&... args) {
- std::vector<T> vec;
- vec.reserve(sizeof...(Args));
- using arr_t = int[];
- (void)arr_t{0, (vec.emplace_back(std::forward<Args>(args)), 0)...};
- return vec;
-}
-
-template <typename T, typename... Args>
- requires std::is_trivially_copyable_v<T>
-std::vector<T> make_vector_impl(Args&&... args) {
- return std::vector<T>{std::forward<Args>(args)...};
-}
-
-} // namespace detail
+namespace frc2 {
template <typename T = void, typename... Args,
- typename V = detail::vec_type_helper_t<T, Args...>>
- requires detail::all_constructible_and_convertible_v<V, Args...>
-std::vector<V> make_vector(Args&&... args) {
- return detail::make_vector_impl<V>(std::forward<Args>(args)...);
+ typename CommonType = std::conditional_t<
+ std::same_as<T, void>, std::common_type_t<Args...>, T>>
+ requires((std::is_constructible_v<T, Args> &&
+ std::is_convertible_v<Args, T>) &&
+ ...)
+std::vector<CommonType> make_vector(Args&&... args) {
+ if constexpr (std::is_trivially_copyable_v<CommonType>) {
+ return std::vector<CommonType>{std::forward<Args>(args)...};
+ } else {
+ std::vector<CommonType> vec;
+ vec.reserve(sizeof...(Args));
+
+ using arr_t = int[];
+ [[maybe_unused]] arr_t arr{
+ 0, (vec.emplace_back(std::forward<Args>(args)), 0)...};
+
+ return vec;
+ }
}
-} // namespace tcb
+} // namespace frc2
diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp
new file mode 100644
index 0000000..5e4a582
--- /dev/null
+++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp
@@ -0,0 +1,170 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Subsystem.h>
+#include <frc2/command/sysid/SysIdRoutine.h>
+
+#include <numbers>
+
+#include <frc/Timer.h>
+#include <frc/simulation/SimHooks.h>
+#include <gtest/gtest.h>
+#include <units/math.h>
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+enum StateTest {
+ Invalid,
+ InRecordStateQf,
+ InRecordStateQr,
+ InRecordStateDf,
+ InRecordStateDr,
+ InDrive,
+ InLog,
+ DoneWithRecordState
+};
+
+class SysIdRoutineTest : public ::testing::Test {
+ protected:
+ std::vector<StateTest> currentStateList{};
+ std::vector<units::volt_t> sentVoltages{};
+ frc2::Subsystem m_subsystem{};
+ frc2::sysid::SysIdRoutine m_sysidRoutine{
+ frc2::sysid::Config{
+ std::nullopt, std::nullopt, std::nullopt,
+ [this](frc::sysid::State state) {
+ switch (state) {
+ case frc::sysid::State::kQuasistaticForward:
+ currentStateList.emplace_back(StateTest::InRecordStateQf);
+ break;
+ case frc::sysid::State::kQuasistaticReverse:
+ currentStateList.emplace_back(StateTest::InRecordStateQr);
+ break;
+ case frc::sysid::State::kDynamicForward:
+ currentStateList.emplace_back(StateTest::InRecordStateDf);
+ break;
+ case frc::sysid::State::kDynamicReverse:
+ currentStateList.emplace_back(StateTest::InRecordStateDr);
+ break;
+ case frc::sysid::State::kNone:
+ currentStateList.emplace_back(StateTest::DoneWithRecordState);
+ break;
+ }
+ }},
+ frc2::sysid::Mechanism{
+ [this](units::volt_t driveVoltage) {
+ sentVoltages.emplace_back(driveVoltage);
+ currentStateList.emplace_back(StateTest::InDrive);
+ },
+ [this](frc::sysid::SysIdRoutineLog* log) {
+ currentStateList.emplace_back(StateTest::InLog);
+ log->Motor("Mock Motor").position(0_m).velocity(0_mps).voltage(0_V);
+ },
+ &m_subsystem}};
+ frc2::CommandPtr m_quasistaticForward{
+ m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
+ frc2::CommandPtr m_quasistaticReverse{
+ m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kReverse)};
+ frc2::CommandPtr m_dynamicForward{
+ m_sysidRoutine.Dynamic(frc2::sysid::Direction::kForward)};
+ frc2::CommandPtr m_dynamicReverse{
+ m_sysidRoutine.Dynamic(frc2::sysid::Direction::kReverse)};
+
+ void RunCommand(frc2::CommandPtr command) {
+ command.get()->Initialize();
+ command.get()->Execute();
+ frc::sim::StepTiming(1_s);
+ command.get()->Execute();
+ command.get()->End(true);
+ }
+
+ void SetUp() override {
+ frc::sim::PauseTiming();
+ frc2::CommandPtr m_quasistaticForward{
+ m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
+ frc2::CommandPtr m_quasistaticReverse{
+ m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kReverse)};
+ frc2::CommandPtr m_dynamicForward{
+ m_sysidRoutine.Dynamic(frc2::sysid::Direction::kForward)};
+ frc2::CommandPtr m_dynamicReverse{
+ m_sysidRoutine.Dynamic(frc2::sysid::Direction::kReverse)};
+ }
+
+ void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+TEST_F(SysIdRoutineTest, RecordStateBookendsMotorLogging) {
+ RunCommand(std::move(m_quasistaticForward));
+ std::vector<StateTest> expectedOrder{
+ StateTest::InDrive, StateTest::InLog, StateTest::InRecordStateQf,
+ StateTest::InDrive, StateTest::DoneWithRecordState};
+ EXPECT_TRUE(expectedOrder == currentStateList);
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ expectedOrder = std::vector<StateTest>{
+ StateTest::InDrive, StateTest::InLog, StateTest::InRecordStateDf,
+ StateTest::InDrive, StateTest::DoneWithRecordState};
+ RunCommand(std::move(m_dynamicForward));
+ EXPECT_TRUE(expectedOrder == currentStateList);
+ currentStateList.clear();
+ sentVoltages.clear();
+}
+
+TEST_F(SysIdRoutineTest, DeclareCorrectState) {
+ RunCommand(std::move(m_quasistaticForward));
+ EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+ StateTest::InRecordStateQf) != currentStateList.end());
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_quasistaticReverse));
+ EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+ StateTest::InRecordStateQr) != currentStateList.end());
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_dynamicForward));
+ EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+ StateTest::InRecordStateDf) != currentStateList.end());
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_dynamicReverse));
+ EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+ StateTest::InRecordStateDr) != currentStateList.end());
+ currentStateList.clear();
+ sentVoltages.clear();
+}
+
+TEST_F(SysIdRoutineTest, OutputCorrectVoltage) {
+ RunCommand(std::move(m_quasistaticForward));
+ std::vector<units::volt_t> expectedVoltages{1_V, 0_V};
+ EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+ EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_quasistaticReverse));
+ expectedVoltages = std::vector<units::volt_t>{-1_V, 0_V};
+ EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+ EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_dynamicForward));
+ expectedVoltages = std::vector<units::volt_t>{7_V, 0_V};
+ EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+ EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+ currentStateList.clear();
+ sentVoltages.clear();
+
+ RunCommand(std::move(m_dynamicReverse));
+ expectedVoltages = std::vector<units::volt_t>{-7_V, 0_V};
+ EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+ EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+ currentStateList.clear();
+ sentVoltages.clear();
+}
diff --git a/wpilibNewCommands/wpilibNewCommands-config.cmake.in b/wpilibNewCommands/wpilibNewCommands-config.cmake.in
deleted file mode 100644
index 8a8d8d8..0000000
--- a/wpilibNewCommands/wpilibNewCommands-config.cmake.in
+++ /dev/null
@@ -1,11 +0,0 @@
-include(CMakeFindDependencyMacro)
- @WPIUTIL_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}/wpilibNewCommands.cmake)
diff --git a/wpilibNewCommands/wpilibnewcommands-config.cmake.in b/wpilibNewCommands/wpilibnewcommands-config.cmake.in
new file mode 100644
index 0000000..b26b9a7
--- /dev/null
+++ b/wpilibNewCommands/wpilibnewcommands-config.cmake.in
@@ -0,0 +1,14 @@
+include(CMakeFindDependencyMacro)
+@WPIUTIL_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}/wpilibnewcommands.cmake)
+if(@WITH_JAVA@)
+ include(${SELF_DIR}/wpilibNewCommands_jar.cmake)
+endif()