Squashed 'third_party/allwpilib_2019/' changes from 99e4f7dd2..c36bbcc9a
936627bd9 wpilibc: Remove direct CameraServer dependency (#1989)
8e333c0aa Use FPGA Time instead of wall clock time for odometry (#1996)
d4430b765 Gearsbot example: Use standard argument order (#1995)
75438ab2c Add RamseteCommand (#1951)
989df1b46 Bump Native Utils and OpenCV dependencies (#1993)
dbc33b61e Fix Timer usage in TrapezoidProfileCommand (#1992)
79f8c5644 Add TrapezoidProfileCommand (#1962)
9440edf2b Refactor TrajectoryGenerator (#1972)
73a30182c Add frc2::Timer (#1968)
36ea865ed Add toString for geometry and trajectory classes (#1991)
cbe05e7e8 Update ProfiledPIDController API (#1967)
d04eb3546 Deprecate old PID classes (#1964)
02264db69 Add JNI dependencies to myRobotCpp (#1980)
2a76c996e Use VID/PID detection for PS3Eye (#1977)
a3820bbdf Remove HAL_BaseInitialize (#1981)
a83fb4793 Update to 2020v5 image (#1983)
4b0ed910e Make SwerveDriveKinematics.toChassisSpeeds() public (#1976)
103c1b121 Remove DS caching from the HAL level (#1971)
6635ea75e Fix NPE in SendableRegistry.foreachLiveWindow() (#1974)
cfe23c5cd Fix grammar error in comment for configureButtonBindings (#1969)
4bde2654e Fix mac azure build (#1973)
4f034e6c1 generateTrajectory: default reversed param to false (#1953)
acf960f72 Sim GUI: Add option to disable outputs on DS disable
2d3dac99f Sim GUI: Handle low resolutions and scale default window positions
07c86e0cd Sim GUI: Support High DPI monitors
46ad95512 SimDeviceData: Add missing null check
5bce489b9 Add ProggyDotted font to imgui (both cmake and gradle)
55af553ac Simulation GUI: Map gamepad the same way as DS
c59f9cea5 CameraServer: Add VID/PID support for Linux USB devices (#1960)
3fc89c84d Make splinePointsFromSplines public (#1963)
2c5093797 Fix implicitly deleted move constructors (#1954)
f3ad927f4 Update Java SmartDashboard and LiveWindow to match C++
05c25deb7 Fix move handling of C++ Sendable in SmartDashboard and LiveWindow
d726591ce Fix Gazebo sim plugin build (#1959)
2ff694fa4 Unbreak gradle build when other compilers installed (#1958)
53816155b Improve command decorator names (#1945)
a38f183a9 Fix GenResources.cmake so it's usable in a submodule (#1956)
b3398dca3 Set gradlebase correctly for all examples (#1950)
2c311013d Add Aarch64Bionic platform detection (#1922)
c10f2003c Add generateTrajectory overload (#1944)
63cfa64fb Add getters for pose in odometry classes (#1943)
2402c2bad Fix C++ command group recursive constructor bug (#1941)
f4eedf597 Fix ConcurrentModificationException in CommandScheduler (#1938)
bb0b207d2 Fix array out of bounds exception caused by parallel race group (#1935)
7bd69e591 Fix typo in temperature (#1940)
ec9738245 Bump to 2020 v4 image (#1931)
46303a822 Add messaging to extension loading in the HAL (#1926)
d169d6be9 Set extract_static for Doxygen config so that static members show up (#1930)
4e183eb10 Bump to 2020 v3 image (#1929)
84c185803 LiveWindow: catch errors in callback/builder functions (#1921)
0e3b0f3da Remove deprecated free() calls (#1925)
7f839b87c Remove timeouts from azure builds (#1924)
45b766a5d Fix main thread ID being potentially incorrect for simulation (#1923)
56d782b16 Add secondary camera name for PS3Eye (#1920)
2b4894038 Add simulation GUI plugin
f97d16073 Add imgui build to cmake
55a844a3e HAL sim: Add encoder channel B access
10deba854 Remove sendables from SendableRegistry when close() is called (#1917)
a9f0e4668 Implement sim devices for ADXL345, ADXL362, ADXRS450, Ultrasonic
aa9064586 Add ability to associate other devices with a SimDevice
81c2c8a7d Add simulation generic device/value support
e8d6f8a2c Move mockdata/HAL_Value.h to hal/Value.h
1b266717a Add simulation module support to cmake build (#1906)
fb8f3bd06 Add testbench yaml file (#1915)
846d8def0 Update to 2020 v2 image (#1913)
d6ac6e512 Fix PortForwarder package declaration (#1912)
227157086 Fix PS3Eye exposure setting (#1911)
885744d7e Add myRobot C++ version to cmake build (#1907)
366091fa8 Document that ConditionalCommand requires all subsystems (#1909)
c58b072c8 Fix Drive usage reporting order (#1908)
762c88adb Update compiler versions in readme (#1905)
af8ce568d Add Ramsete unicycle controller (#1790)
b2c2934d0 Fix javadoc warnings about invalid usage of ">" (#1904)
cce26ec78 Replace CRLF line endings with LF (#1902)
cb54602d4 Add support for writing RTR CAN Frames to the CAN API (#1900)
9f740e590 Use OS for serial port instead of NI VISA (#1875)
b23baf611 Add ability to run robot main loop in a separate thread (#1895)
457f94ba2 Add trajectory generation using hermite splines (#1843)
fd612052f Update native utils to use new frchome directory (#1884)
8858ec55c Remove periodic can read call (#1868)
41efb8015 Update CANAPITypes.h (#1860)
c93be1b2d Remove LabVIEW HAL support (#1901)
680f8919e Remove eigen, units and libuv from doxygen generation (#1898)
c5812524f Bump GradleJNI plugin version (#1899)
971303da8 Add PortForwarder class (#1890)
50db77bf2 Fix wpiutil cmake eigen install source directory (#1891)
85d42c199 C++ PIDCommand: Add GetMeasurement() and UseOutput() (#1892)
2dfbb855d wpilibj: Fix SwerveDriveKinematics ctor parameter name (#1889)
471f375a3 Simplify Sendable interface (#1864)
1d8c4d016 Replace ::value and ::type with _v and _t suffixes (#1885)
a5650b943 Add Units Utility class for Java (#1829)
904479ad4 Deprecate GearTooth class for removal (#1878)
86b666bba Add equality comparator to geometry classes (#1882)
62f07c182 Make one-arg Rotation2d constructor implicit (#1883)
f405582f8 Add kinematics suite (#1787)
561cbbd14 Deprecate Filter class for removal (#1876)
84e2973aa Remove unused include from Filesystem.h (#1877)
f49859ebf Remove NI VISA headers, as they are now included in NI Libraries (#1879)
bc59db5e6 Rename DEBUG macro to DEBUG0 (#1871)
dd928b4cb Remove JNI logging (#1872)
3e0f7d099 Use units for new NotifierCommand (#1869)
5ffe15d5f Remove ability to build all combined artifacts (#1867)
516cbef2c Remove RoboRIO ifdef from simulation headers (#1859)
9b6ffc201 Replace SetOutputRange() with SetIntegratorRange()
ff8b8f0a8 Remove percent tolerance from PID controller
0ca8d667d Clean up AutoCloseable and other Java warnings (#1866)
7112add67 Watchdog: use units::second_t instead of double (#1863)
761bc3ef8 Change C++ WaitCommand to use units (#1865)
1fb301123 Add MathUtils.clamp() for Java (#1861)
eb3e0c9c9 Fix cmake Eigen include directory (#1862)
2250b7fbe Rename GearsBotNew example to GearsBot
c9f9feff1 Replace deprecated API usage in C++ examples
d6b9c7e14 CONTRIBUTING.md: Point to frc-docs instead of screensteps (#1858)
d10a1a797 Fix eigen build in vcpkg (#1856)
2bdb44325 Add frc2 includes to list of "other lib" regexes (#1855)
4b2b21d24 Replace outdated Java collections (#508)
8993ce5bf Move Eigen headers out of main include folder (#1854)
0f532a117 Add PWMSparkMax (#1751)
f7ad363d8 Add jni cross compile options for aarch64 (#1853)
9afea3340 Add support for aarch64 jetson bionic builds (#1844)
d787b5d60 Add more items to .gitignore (#1850)
5dd0d1b7d Use units in SPI
07ac711b3 Fix units deprecated warning in IterativeRobot
decfd858b Correctly report -1 for POV on disconnected joystick (#1852)
076ed7770 Add new C++ Command framework (#1785)
a0be07c37 Refactor HAL handle move construction/assignment (#1845)
558c38308 Add new Java Command framework (#1682)
1379735af Delete RobotState and SensorUtil constructors (#1847)
e3d86fee4 Move circular buffer class from wpilib to wpiutil (#1840)
4cd8a5667 TimedRobot.cpp: Fix deprecation warning (#1846)
b2861f894 Use 2020 artifacts and artifactory server (#1838)
98cc32703 Update to use artifactory to publish artifacts (#1833)
fa0640300 Move drive integration tests into wpilibj/src/test (#1836)
e716c36b8 Fix Nat.java generation to be incremental (#1831)
9fd2b5e3f Fix MSVC builds on cmake windows in vcpkg (#1835)
7e95010a2 Add compile-time EJML matrix wrapper to wpiutil (#1804)
3ebc5a6d3 Add ProfiledPIDController
fc98a79db Clean up PIDController interface in preparation for ProfiledPIDController
fdc098267 Fix compilation error in elevator trapezoid profile example (#1826)
a3dd84e85 Make XBoxController Button enum public (#1823)
a216b9e9e Add TrapezoidProfile example (#1814)
8f386f6bb wpilibc: Add unit-safety to C++ geometry classes (#1811)
c07ac2353 wpilibc: Add overloads for units (#1815)
f1d71da8a Move GetStackTrace and Demangle to wpiutil, add Windows support (#1819)
ef037457e Make LinearFilter copyable and moveable (#1789)
76930250c Remove objective-cpp support (#1816)
1c246418f Move TrapezoidProfileTest to trajectory folder (#1812)
95a54a0f2 Add java arcade drive example (#1810)
a4530243e HAL sim: Fix incorrectly setting dio port to initialized on cleanup (#1813)
09d00a622 Update Java examples to use new PIDController (#1809)
ba9b51742 Add missing Java examples (#841)
6411bd79c InterruptableSensorBase: Fix callback function deletion (#1807)
810e58ea8 I2C: Add tip about writeBulk() to transaction() (#1806)
607d6c148 Fix wpilibj integration tests jar name (#1808)
c9873e81b Remove PIDControllerRunner and mutex from new PIDController (#1795)
98d0706de Fix cscore build with OpenCV 4 (#1803)
fbe67c90c Make Sendable setters synchronous (#1799)
c67a488a0 Format SendableBuilderImpl javadocs (#1802)
8e93ce892 Fix PIDControllerRunner member destruction order (#1801)
c98ca7310 Add EJML dependency to wpiutil (#1769)
3b12276bc SendableBase: remove unnecessary synchronization (#1797)
e6d348f38 Fix missing default name in Java PIDController (#1792)
df12fc2a8 Java cleanups (#1776)
39561751f Update GradleVSCode version (#1786)
37d316aa0 Add C++20 std::math constants shim (#1788)
dd4310959 Deprecate frc/WPILib.h (#1779)
823174f30 Update native utils to 2020.0.4 (#1783)
37c695266 Squelch -Wdeprecated-copy for Eigen with GCC >= 9
04c9b000f Revert "Fix build of Eigen 3.3.7 with GCC 9"
ca3e71e21 wpiutil: Fix Process::Spawn() (#1778)
d946d5a2b Fix Eigen compilation errors and add tests (#1777)
8b1b9ac75 Fix build of Eigen 3.3.7 with GCC 9
2f680ba99 Add Eigen linear algebra library
a885db7d4 Make MotorEncoderTest use LinearFilter (#1775)
ee2410169 Add geometry classes (#1766)
48fe54271 Add HALSIM_SetSendError implementation (#1773)
dff58c87f Fix unused warning in release build (#1771)
dde61aad3 Remove TimerEventHandler typedef from Notifier class (#1767)
0f6ef80ab Add RobotState#IsEStopped and DriverStation#IsEStopped (#952)
e48886187 Move unit tests from integration test suite (#1170)
dffa1a5cb Make null checks more descriptive (#1688)
fe59d854d Notifier: add null check (#1684)
10731f3d6 Update uv Udp wrapper for latest features
89f7b72b6 Update libuv to 1.30.1 release
85f2f8740 wpiutil: Add unique_function (#1761)
73ec94078 Remove SampleRobot (#1658)
62be0392b Replace std::lock_guard and std::lock with std::scoped_lock (#1758)
24d31df55 Make sure move constructor is generated for TrapezoidProfile (#1757)
841ef5d73 Remove template types from lock RAII wrapper usages (#1756)
e582518ba Fix some move constructors (#1754)
8757bc471 Remove pre-C++17 shims (#1752)
ea9512977 Add replacement PIDController class (#1300)
9b798d228 Add TrapezoidProfile class (#1673)
804926fb5 Unconditionally skip athena builds for sim (#1748)
118e9d29d Add C++14 units library (#1749)
c705953d7 Add usage reporting to LinearFilter (#1750)
852d1b9ca Don't cross-build gazebo for raspbian (#1747)
eedb3a1ad Fix GCC 9 warnings (#1730)
60dce66a4 Remove wpi::ArrayRef std::initializer_list constructor (#1745)
9e19b29c3 Use base azure image for primary wpilib build (#1744)
299425071 Update jni library, fix cross builds of the jni symbol check (#1742)
a6b0e9b85 Only disable execution of cross compile google tests (#1741)
3c2093119 Use docker container to run wpiformat (#1740)
5fe2eebce Revert "Don't build halsim_gazebo on raspbian (#1737)" (#1743)
4b1b92bb7 Replace wpi::optional with C++17 std::optional (#1732)
0fbb0d989 Update to 2020 compilers (#1733)
2dc94e605 Disable google tests on cross compilers (#1738)
d9cb57a42 Don't build halsim_gazebo on raspbian (#1737)
f7cfdd7ce Replace crlf line endings with lf (#1731)
b6d5d90d9 Add JaCoCo Support (#1734)
c7ab2baa6 Add way to disable the jni check tasks from a property (#1736)
0c45c5b7e Fix skip athena and skip raspbian flags (#1735)
3dfb01d45 Update to new Native Utils (#1696)
30e936837 Clean up LinearDigitalFilter class (#782)
311e2de4c Remove deprecated Joystick constants (#1715)
c08fd6682 Update CAN manufacturer list (#1706)
258bba0c2 ErrorBase and WPIError improvements (#1727)
372ca4f45 cmake: Enable googletest unit tests (#1720)
223d47af0 HALSIM: support mocking of HAL_SendError() (#1728)
55cb683db Change compiler flags to C++17 (#1723)
ee8a33c56 wpiutil: SafeThread: Add thread id, support getting shared_ptr (#1722)
61426d08d wpiutil: Signal: make operator() const (#1721)
b630b63ef Remove functions in LiveWindow deprecated since 2018 (#1716)
1d0c05d4f Styleguide fixes for #1718 (#1719)
f07569df1 Fix newer GCC/clang compiler warnings (#1718)
0120f3124 C++ SPI: Fix SetClockRate to take int (#1717)
c2829ed98 Configure gradle to ignore unresolved headers (#1711)
221e66f46 Allow disabling static init of JNI libraries (#1672)
738852e11 cmake: Add cross toolchain files for Rio and Pi (#1710)
27b697b08 Remove frc directory include shims (#1714)
9e45373a7 Remove functions and classes deprecated for 2018 season (#1059)
eeb1025ac SPI: Report port as instance for usage reporting (#1704)
bc6f1e246 Windows compiler options improvements (#1709)
bb48ae391 cmake: Move example programs into folders (#1654)
221011494 Update for C++17 and fix MSVC warnings (#1694)
fb1239a2a Add raw sources and sinks to cscore (#1670)
7de947734 Add lambda overloads for interrupts (#1636)
90957aeea Move libuv to its own subfolder in build (#1661)
47aae502a Styleguide fixes (#1702)
0bff98b5e Correct DifferentialDrive::ArcadeDrive param docs (#1698)
b52e40b80 Allow widgets to be added by passing value suppliers (#1690)
4a00cd77b Add usage reporting for the Shuffleboard API (#1685)
e25e515f2 Publish artifacts on azure (#1678)
322ef9b96 Force Java 11, fix javadoc generation (#1695)
d42ef5df0 Fix Watchdog print formatting (#1693)
f432f65be Update copyright year in license to 2019 (#1524)
1726b77ac wpiutil: uv: Remove copy from SimpleBufferPool (#1680)
620bec9ca wpiutil: uv: Add LoopClosing status to Handle (#1647)
7cd6e2e7f UsbCamera: Solve race in windows initialization (#1638)
7732836bd Completely disable watchdog tests on mac (#1679)
698edfda9 Remove framework load, disable mac timeout test (#1676)
1c454b000 Add Shuffleboard calls to IterativeRobotBase in C++ (#1607)
f42905b32 Include missing headers in HAL.h (#1660)
bdc822fad Only generate passthrough URLs for RoboRIO (#1624)
d3affb16b Make failure of HAL_GetFPGATime() more descriptive (#1633)
2de3bf7f5 Update LLVM from stable upstream (#1653)
3cf4f38f5 Fix build on macos10.14.4 (#1648)
4e0c10f48 Fix CAN Clean using wrong ID (#1668)
3b0631324 Fix Gray to BGR conversion in CameraServer (#1665)
6cd1c73ef Fix GUID comparison creating weird symbol (#1659)
063bbab6f MavenArtifacts.md: update links to HTTPS (#1674)
aab4c494d Fix type in build.gradle (#1604)
bf46af260 Disable extraneous data warnings in libjpeg (#1630)
655763a9a Limit length of message sent to DS SendError call (#1618)
a095ec2d8 Fix linker errors with free functions in Threads.h (#1625)
12ab035aa Fix receive side of LabVIEW USB streams (#1621)
Change-Id: Ibd382e1a48925c200850cf90a8121e35c0fcffe3
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: c36bbcc9a9095489fc078229db4fba3ecd0f9b78
diff --git a/wpilibc/CMakeLists.txt b/wpilibc/CMakeLists.txt
index da35871..977200a 100644
--- a/wpilibc/CMakeLists.txt
+++ b/wpilibc/CMakeLists.txt
@@ -1,11 +1,14 @@
project(wpilibc)
+include(CompileWarnings)
+include(AddTest)
+
find_package( OpenCV REQUIRED )
configure_file(src/generate/WPILibVersion.cpp.in WPILibVersion.cpp)
file(GLOB_RECURSE
- wpilibc_native_src src/main/native/cpp/*.cpp)
+ wpilibc_native_src src/main/native/cpp/*.cpp src/main/native/cppcs/*.cpp)
add_library(wpilibc ${wpilibc_native_src} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
set_target_properties(wpilibc PROPERTIES DEBUG_POSTFIX "d")
@@ -13,6 +16,7 @@
target_include_directories(wpilibc PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
$<INSTALL_INTERFACE:${include_dest}/wpilibc>)
+wpilib_target_warnings(wpilibc)
target_link_libraries(wpilibc PUBLIC cameraserver hal ntcore cscore wpiutil ${OpenCV_LIBS})
set_property(TARGET wpilibc PROPERTY FOLDER "libraries")
@@ -20,11 +24,23 @@
install(TARGETS wpilibc EXPORT wpilibc DESTINATION "${main_lib_dest}")
install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpilibc")
-if (MSVC)
+if (MSVC OR FLAT_INSTALL_WPILIB)
set (wpilibc_config_dir ${wpilib_dest})
else()
set (wpilibc_config_dir share/wpilibc)
endif()
-install(FILES wpilibc-config.cmake DESTINATION ${wpilibc_config_dir})
+configure_file(wpilibc-config.cmake.in ${CMAKE_BINARY_DIR}/wpilibc-config.cmake )
+install(FILES ${CMAKE_BINARY_DIR}/wpilibc-config.cmake DESTINATION ${wpilibc_config_dir})
install(EXPORT wpilibc DESTINATION ${wpilibc_config_dir})
+
+if (WITH_TESTS)
+ wpilib_add_test(wpilibc src/test/native/cpp)
+ target_include_directories(wpilibc_test PRIVATE src/test/native/include)
+ target_link_libraries(wpilibc_test wpilibc gmock_main)
+ if (NOT MSVC)
+ target_compile_options(wpilibc_test PRIVATE -Wno-error)
+ else()
+ target_compile_options(wpilibc_test PRIVATE /WX-)
+ endif()
+endif()
diff --git a/wpilibc/build.gradle b/wpilibc/build.gradle
index 37ff6bd..5f93f7e 100644
--- a/wpilibc/build.gradle
+++ b/wpilibc/build.gradle
@@ -22,22 +22,23 @@
outputs.file wpilibVersionFileOutput
inputs.file wpilibVersionFileInput
- if (WPILibVersion.releaseType.toString().equalsIgnoreCase('official')) {
+ if (wpilibVersioning.releaseMode) {
outputs.upToDateWhen { false }
}
// We follow a simple set of checks to determine whether we should generate a new version file:
- // 1. If the release type is not development, we generate a new verison file
+ // 1. If the release type is not development, we generate a new version file
// 2. If there is no generated version number, we generate a new version file
// 3. If there is a generated build number, and the release type is development, then we will
// only generate if the publish task is run.
doLast {
- println "Writing version ${WPILibVersion.version} to $wpilibVersionFileOutput"
+ def version = wpilibVersioning.version.get()
+ println "Writing version ${version} to $wpilibVersionFileOutput"
if (wpilibVersionFileOutput.exists()) {
wpilibVersionFileOutput.delete()
}
- def read = wpilibVersionFileInput.text.replace('${wpilib_version}', WPILibVersion.version)
+ def read = wpilibVersionFileInput.text.replace('${wpilib_version}', version)
wpilibVersionFileOutput.write(read)
}
}
@@ -81,19 +82,22 @@
apply from: "${rootDir}/shared/nilibraries.gradle"
-model {
- exportsConfigs {
- wpilibc(ExportsConfig) {
- x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
- '_CT??_R0?AVbad_cast',
- '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
- '_TI5?AVfailure']
- x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
- '_CT??_R0?AVbad_cast',
- '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
- '_TI5?AVfailure']
- }
+apply plugin: DisableBuildingGTest
+
+nativeUtils.exportsConfigs {
+ wpilibc {
+ x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+ '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+ '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+ '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+ x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
+ '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
+ '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
+ '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
}
+}
+
+model {
components {
"${nativeName}Base"(NativeLibrarySpec) {
sources {
@@ -112,31 +116,39 @@
it.buildable = false
return
}
+ cppCompiler.define 'DYNAMIC_CAMERA_SERVER'
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
- lib project: ':cscore', library: 'cscore', linkage: 'shared'
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
- lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
}
}
"${nativeName}"(NativeLibrarySpec) {
sources {
cpp {
source {
- srcDirs "${rootDir}/shared/singlelib"
+ srcDirs "src/main/native/cppcs"
include '**/*.cpp'
}
exportedHeaders {
- srcDirs 'src/main/native/include'
+ srcDirs 'src/main/native/include', '../cameraserver/src/main/native/include'
}
}
}
binaries.all {
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
- lib project: ':cscore', library: 'cscore', linkage: 'shared'
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
- lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+
+ if (it instanceof SharedLibraryBinarySpec) {
+ cppCompiler.define 'DYNAMIC_CAMERA_SERVER'
+ if (buildType == buildTypes.debug) {
+ cppCompiler.define 'DYNAMIC_CAMERA_SERVER_DEBUG'
+ }
+ } else {
+ lib project: ':cscore', library: 'cscore', linkage: 'shared'
+ lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ }
+
}
appendDebugPathToBinaries(binaries)
}
@@ -202,16 +214,12 @@
}
}
withType(GoogleTestTestSuiteBinarySpec) {
- if (!project.hasProperty('onlyAthena') && !project.hasProperty('onlyRaspbian')) {
- lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
- lib project: ':cscore', library: 'cscore', linkage: 'shared'
- project(':hal').addHalDependency(it, 'shared')
- lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
- lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
- lib library: nativeName, linkage: 'shared'
- } else {
- it.buildable = false
- }
+ lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+ lib project: ':cscore', library: 'cscore', linkage: 'shared'
+ project(':hal').addHalDependency(it, 'shared')
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+ lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ lib library: nativeName, linkage: 'shared'
}
}
tasks {
diff --git a/wpilibc/publish.gradle b/wpilibc/publish.gradle
index 06f88df..dd7601e 100644
--- a/wpilibc/publish.gradle
+++ b/wpilibc/publish.gradle
@@ -1,12 +1,5 @@
apply plugin: 'maven-publish'
-def pubVersion = ''
-if (project.hasProperty("publishVersion")) {
- pubVersion = project.publishVersion
-} else {
- pubVersion = WPILibVersion.version
-}
-
def baseArtifactId = 'wpilibc-cpp'
def artifactGroupId = 'edu.wpi.first.wpilibc'
def zipBaseName = '_GROUP_edu_wpi_first_wpilibc_ID_wpilibc-cpp_CLS'
@@ -67,7 +60,7 @@
artifactId = baseArtifactId
groupId artifactGroupId
- version pubVersion
+ version wpilibVersioning.version.get()
}
}
}
diff --git a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
index 71407da..5b5762a 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,11 +10,20 @@
#include <hal/HAL.h>
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress)
- : m_i2c(port, deviceAddress) {
+ : m_i2c(port, deviceAddress),
+ m_simDevice("ADXL345_I2C", port, deviceAddress) {
+ if (m_simDevice) {
+ m_simRange =
+ m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
+ m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
+ m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+ m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
+ }
// Turn on the measurements
m_i2c.Write(kPowerCtlRegister, kPowerCtl_Measure);
// Specify the data format to read
@@ -22,7 +31,8 @@
HAL_Report(HALUsageReporting::kResourceType_ADXL345,
HALUsageReporting::kADXL345_I2C, 0);
- SetName("ADXL345_I2C", port);
+
+ SendableRegistry::GetInstance().AddLW(this, "ADXL345_I2C", port);
}
void ADXL345_I2C::SetRange(Range range) {
@@ -37,6 +47,9 @@
double ADXL345_I2C::GetZ() { return GetAcceleration(kAxis_Z); }
double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis) {
+ if (axis == kAxis_X && m_simX) return m_simX.Get();
+ if (axis == kAxis_Y && m_simY) return m_simY.Get();
+ if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
int16_t rawAccel = 0;
m_i2c.Read(kDataRegister + static_cast<int>(axis), sizeof(rawAccel),
reinterpret_cast<uint8_t*>(&rawAccel));
@@ -44,7 +57,13 @@
}
ADXL345_I2C::AllAxes ADXL345_I2C::GetAccelerations() {
- AllAxes data = AllAxes();
+ AllAxes data;
+ if (m_simX && m_simY && m_simZ) {
+ data.XAxis = m_simX.Get();
+ data.YAxis = m_simY.Get();
+ data.ZAxis = m_simZ.Get();
+ return data;
+ }
int16_t rawData[3];
m_i2c.Read(kDataRegister, sizeof(rawData),
reinterpret_cast<uint8_t*>(rawData));
diff --git a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
index 738bc3e..bb9275a 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,11 +10,19 @@
#include <hal/HAL.h>
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range)
- : m_spi(port) {
+ : m_spi(port), m_simDevice("ADXL345_SPI", port) {
+ if (m_simDevice) {
+ m_simRange =
+ m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
+ m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
+ m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+ m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
+ }
m_spi.SetClockRate(500000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnTrailingEdge();
@@ -32,7 +40,7 @@
HAL_Report(HALUsageReporting::kResourceType_ADXL345,
HALUsageReporting::kADXL345_SPI);
- SetName("ADXL345_SPI", port);
+ SendableRegistry::GetInstance().AddLW(this, "ADXL345_SPI", port);
}
void ADXL345_SPI::SetRange(Range range) {
@@ -42,6 +50,8 @@
commands[0] = kDataFormatRegister;
commands[1] = kDataFormat_FullRes | static_cast<uint8_t>(range & 0x03);
m_spi.Transaction(commands, commands, 2);
+
+ if (m_simRange) m_simRange.Set(range);
}
double ADXL345_SPI::GetX() { return GetAcceleration(kAxis_X); }
@@ -51,6 +61,9 @@
double ADXL345_SPI::GetZ() { return GetAcceleration(kAxis_Z); }
double ADXL345_SPI::GetAcceleration(ADXL345_SPI::Axes axis) {
+ if (axis == kAxis_X && m_simX) return m_simX.Get();
+ if (axis == kAxis_Y && m_simY) return m_simY.Get();
+ if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
uint8_t buffer[3];
uint8_t command[3] = {0, 0, 0};
command[0] = (kAddress_Read | kAddress_MultiByte | kDataRegister) +
@@ -63,7 +76,14 @@
}
ADXL345_SPI::AllAxes ADXL345_SPI::GetAccelerations() {
- AllAxes data = AllAxes();
+ AllAxes data;
+ if (m_simX && m_simY && m_simZ) {
+ data.XAxis = m_simX.Get();
+ data.YAxis = m_simY.Get();
+ data.ZAxis = m_simZ.Get();
+ return data;
+ }
+
uint8_t dataBuffer[7] = {0, 0, 0, 0, 0, 0, 0};
int16_t rawData[3];
diff --git a/wpilibc/src/main/native/cpp/ADXL362.cpp b/wpilibc/src/main/native/cpp/ADXL362.cpp
index d709ae4..2f97f1b 100644
--- a/wpilibc/src/main/native/cpp/ADXL362.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL362.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,6 +11,7 @@
#include "frc/DriverStation.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -33,23 +34,34 @@
ADXL362::ADXL362(Range range) : ADXL362(SPI::Port::kOnboardCS1, range) {}
-ADXL362::ADXL362(SPI::Port port, Range range) : m_spi(port) {
+ADXL362::ADXL362(SPI::Port port, Range range)
+ : m_spi(port), m_simDevice("ADXL362", port) {
+ if (m_simDevice) {
+ m_simRange =
+ m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
+ m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
+ m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+ m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
+ }
+
m_spi.SetClockRate(3000000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnTrailingEdge();
m_spi.SetClockActiveLow();
m_spi.SetChipSelectActiveLow();
- // Validate the part ID
uint8_t commands[3];
- commands[0] = kRegRead;
- commands[1] = kPartIdRegister;
- commands[2] = 0;
- m_spi.Transaction(commands, commands, 3);
- if (commands[2] != 0xF2) {
- DriverStation::ReportError("could not find ADXL362");
- m_gsPerLSB = 0.0;
- return;
+ if (!m_simDevice) {
+ // Validate the part ID
+ commands[0] = kRegRead;
+ commands[1] = kPartIdRegister;
+ commands[2] = 0;
+ m_spi.Transaction(commands, commands, 3);
+ if (commands[2] != 0xF2) {
+ DriverStation::ReportError("could not find ADXL362");
+ m_gsPerLSB = 0.0;
+ return;
+ }
}
SetRange(range);
@@ -62,7 +74,7 @@
HAL_Report(HALUsageReporting::kResourceType_ADXL362, port);
- SetName("ADXL362", port);
+ SendableRegistry::GetInstance().AddLW(this, "ADXL362", port);
}
void ADXL362::SetRange(Range range) {
@@ -89,6 +101,8 @@
commands[2] =
kFilterCtl_ODR_100Hz | static_cast<uint8_t>((range & 0x03) << 6);
m_spi.Write(commands, 3);
+
+ if (m_simRange) m_simRange.Set(range);
}
double ADXL362::GetX() { return GetAcceleration(kAxis_X); }
@@ -100,6 +114,10 @@
double ADXL362::GetAcceleration(ADXL362::Axes axis) {
if (m_gsPerLSB == 0.0) return 0.0;
+ if (axis == kAxis_X && m_simX) return m_simX.Get();
+ if (axis == kAxis_Y && m_simY) return m_simY.Get();
+ if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
+
uint8_t buffer[4];
uint8_t command[4] = {0, 0, 0, 0};
command[0] = kRegRead;
@@ -112,11 +130,17 @@
}
ADXL362::AllAxes ADXL362::GetAccelerations() {
- AllAxes data = AllAxes();
+ AllAxes data;
if (m_gsPerLSB == 0.0) {
data.XAxis = data.YAxis = data.ZAxis = 0.0;
return data;
}
+ if (m_simX && m_simY && m_simZ) {
+ data.XAxis = m_simX.Get();
+ data.YAxis = m_simY.Get();
+ data.ZAxis = m_simZ.Get();
+ return data;
+ }
uint8_t dataBuffer[8] = {0, 0, 0, 0, 0, 0, 0, 0};
int16_t rawData[3];
diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
index 875f403..3dde234 100644
--- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
+++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,45 +11,55 @@
#include "frc/DriverStation.h"
#include "frc/Timer.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
-static constexpr double kSamplePeriod = 0.0005;
+static constexpr auto kSamplePeriod = 0.0005_s;
static constexpr double kCalibrationSampleTime = 5.0;
static constexpr double kDegreePerSecondPerLSB = 0.0125;
-static constexpr int kRateRegister = 0x00;
-static constexpr int kTemRegister = 0x02;
-static constexpr int kLoCSTRegister = 0x04;
-static constexpr int kHiCSTRegister = 0x06;
-static constexpr int kQuadRegister = 0x08;
-static constexpr int kFaultRegister = 0x0A;
+// static constexpr int kRateRegister = 0x00;
+// static constexpr int kTemRegister = 0x02;
+// static constexpr int kLoCSTRegister = 0x04;
+// static constexpr int kHiCSTRegister = 0x06;
+// static constexpr int kQuadRegister = 0x08;
+// static constexpr int kFaultRegister = 0x0A;
static constexpr int kPIDRegister = 0x0C;
-static constexpr int kSNHighRegister = 0x0E;
-static constexpr int kSNLowRegister = 0x10;
+// static constexpr int kSNHighRegister = 0x0E;
+// static constexpr int kSNLowRegister = 0x10;
ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
-ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) : m_spi(port) {
+ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port)
+ : m_spi(port), m_simDevice("ADXRS450_Gyro", port) {
+ if (m_simDevice) {
+ m_simAngle = m_simDevice.CreateDouble("Angle", false, 0.0);
+ m_simRate = m_simDevice.CreateDouble("Rate", false, 0.0);
+ }
+
m_spi.SetClockRate(3000000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnLeadingEdge();
m_spi.SetClockActiveHigh();
m_spi.SetChipSelectActiveLow();
- // Validate the part ID
- if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
- DriverStation::ReportError("could not find ADXRS450 gyro");
- return;
+ if (!m_simDevice) {
+ // Validate the part ID
+ if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
+ DriverStation::ReportError("could not find ADXRS450 gyro");
+ return;
+ }
+
+ m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c00000eu,
+ 0x04000000u, 10u, 16u, true, true);
+
+ Calibrate();
}
- m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c00000eu, 0x04000000u,
- 10u, 16u, true, true);
-
- Calibrate();
-
HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port);
- SetName("ADXRS450_Gyro", port);
+
+ SendableRegistry::GetInstance().AddLW(this, "ADXRS450_Gyro", port);
}
static bool CalcParity(int v) {
@@ -86,15 +96,21 @@
}
double ADXRS450_Gyro::GetAngle() const {
+ if (m_simAngle) return m_simAngle.Get();
return m_spi.GetAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
}
double ADXRS450_Gyro::GetRate() const {
+ if (m_simRate) return m_simRate.Get();
return static_cast<double>(m_spi.GetAccumulatorLastValue()) *
kDegreePerSecondPerLSB;
}
-void ADXRS450_Gyro::Reset() { m_spi.ResetAccumulator(); }
+void ADXRS450_Gyro::Reset() {
+ if (m_simAngle) m_simAngle.Set(0.0);
+ if (m_simRate) m_simRate.Set(0.0);
+ m_spi.ResetAccumulator();
+}
void ADXRS450_Gyro::Calibrate() {
Wait(0.1);
diff --git a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
index 3ae8e48..4436bcf 100644
--- a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,12 +11,13 @@
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
AnalogAccelerometer::AnalogAccelerometer(int channel)
: AnalogAccelerometer(std::make_shared<AnalogInput>(channel)) {
- AddChild(m_analogInput);
+ SendableRegistry::GetInstance().AddChild(this, m_analogInput.get());
}
AnalogAccelerometer::AnalogAccelerometer(AnalogInput* channel)
@@ -58,5 +59,7 @@
void AnalogAccelerometer::InitAccelerometer() {
HAL_Report(HALUsageReporting::kResourceType_Accelerometer,
m_analogInput->GetChannel());
- SetName("Accelerometer", m_analogInput->GetChannel());
+
+ SendableRegistry::GetInstance().AddLW(this, "Accelerometer",
+ m_analogInput->GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/AnalogGyro.cpp b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
index 5efe7b3..36cde8a 100644
--- a/wpilibc/src/main/native/cpp/AnalogGyro.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -17,12 +17,13 @@
#include "frc/AnalogInput.h"
#include "frc/Timer.h"
#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
AnalogGyro::AnalogGyro(int channel)
: AnalogGyro(std::make_shared<AnalogInput>(channel)) {
- AddChild(m_analog);
+ SendableRegistry::GetInstance().AddChild(this, m_analog.get());
}
AnalogGyro::AnalogGyro(AnalogInput* channel)
@@ -41,7 +42,7 @@
AnalogGyro::AnalogGyro(int channel, int center, double offset)
: AnalogGyro(std::make_shared<AnalogInput>(channel), center, offset) {
- AddChild(m_analog);
+ SendableRegistry::GetInstance().AddChild(this, m_analog.get());
}
AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel, int center,
@@ -65,20 +66,6 @@
AnalogGyro::~AnalogGyro() { HAL_FreeAnalogGyro(m_gyroHandle); }
-AnalogGyro::AnalogGyro(AnalogGyro&& rhs)
- : GyroBase(std::move(rhs)), m_analog(std::move(rhs.m_analog)) {
- std::swap(m_gyroHandle, rhs.m_gyroHandle);
-}
-
-AnalogGyro& AnalogGyro::operator=(AnalogGyro&& rhs) {
- GyroBase::operator=(std::move(rhs));
-
- m_analog = std::move(rhs.m_analog);
- std::swap(m_gyroHandle, rhs.m_gyroHandle);
-
- return *this;
-}
-
double AnalogGyro::GetAngle() const {
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
@@ -162,7 +149,9 @@
}
HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
- SetName("AnalogGyro", m_analog->GetChannel());
+
+ SendableRegistry::GetInstance().AddLW(this, "AnalogGyro",
+ m_analog->GetChannel());
}
void AnalogGyro::Calibrate() {
diff --git a/wpilibc/src/main/native/cpp/AnalogInput.cpp b/wpilibc/src/main/native/cpp/AnalogInput.cpp
index 52a55d3..a200f4d 100644
--- a/wpilibc/src/main/native/cpp/AnalogInput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogInput.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -18,6 +18,7 @@
#include "frc/Timer.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -42,32 +43,12 @@
}
HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel);
- SetName("AnalogInput", channel);
+
+ SendableRegistry::GetInstance().AddLW(this, "AnalogInput", channel);
}
AnalogInput::~AnalogInput() { HAL_FreeAnalogInputPort(m_port); }
-AnalogInput::AnalogInput(AnalogInput&& rhs)
- : ErrorBase(std::move(rhs)),
- SendableBase(std::move(rhs)),
- PIDSource(std::move(rhs)),
- m_channel(std::move(rhs.m_channel)),
- m_accumulatorOffset(std::move(rhs.m_accumulatorOffset)) {
- std::swap(m_port, rhs.m_port);
-}
-
-AnalogInput& AnalogInput::operator=(AnalogInput&& rhs) {
- ErrorBase::operator=(std::move(rhs));
- SendableBase::operator=(std::move(rhs));
- PIDSource::operator=(std::move(rhs));
-
- m_channel = std::move(rhs.m_channel);
- std::swap(m_port, rhs.m_port);
- m_accumulatorOffset = std::move(rhs.m_accumulatorOffset);
-
- return *this;
-}
-
int AnalogInput::GetValue() const {
if (StatusIsFatal()) return 0;
int32_t status = 0;
@@ -243,6 +224,10 @@
return GetAverageVoltage();
}
+void AnalogInput::SetSimDevice(HAL_SimDeviceHandle device) {
+ HAL_SetAnalogInputSimDevice(m_port, device);
+}
+
void AnalogInput::InitSendable(SendableBuilder& builder) {
builder.SetSmartDashboardType("Analog Input");
builder.AddDoubleProperty("Value", [=]() { return GetAverageVoltage(); },
diff --git a/wpilibc/src/main/native/cpp/AnalogOutput.cpp b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
index b18af5b..5e56efc 100644
--- a/wpilibc/src/main/native/cpp/AnalogOutput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,6 +16,7 @@
#include "frc/SensorUtil.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -42,28 +43,11 @@
}
HAL_Report(HALUsageReporting::kResourceType_AnalogOutput, m_channel);
- SetName("AnalogOutput", m_channel);
+ SendableRegistry::GetInstance().AddLW(this, "AnalogOutput", m_channel);
}
AnalogOutput::~AnalogOutput() { HAL_FreeAnalogOutputPort(m_port); }
-AnalogOutput::AnalogOutput(AnalogOutput&& rhs)
- : ErrorBase(std::move(rhs)),
- SendableBase(std::move(rhs)),
- m_channel(std::move(rhs.m_channel)) {
- std::swap(m_port, rhs.m_port);
-}
-
-AnalogOutput& AnalogOutput::operator=(AnalogOutput&& rhs) {
- ErrorBase::operator=(std::move(rhs));
- SendableBase::operator=(std::move(rhs));
-
- m_channel = std::move(rhs.m_channel);
- std::swap(m_port, rhs.m_port);
-
- return *this;
-}
-
void AnalogOutput::SetVoltage(double voltage) {
int32_t status = 0;
HAL_SetAnalogOutput(m_port, voltage, &status);
diff --git a/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
index 6c42ff9..b650ec7 100644
--- a/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,26 +9,29 @@
#include "frc/RobotController.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
AnalogPotentiometer::AnalogPotentiometer(int channel, double fullRange,
double offset)
- : m_analog_input(std::make_shared<AnalogInput>(channel)),
- m_fullRange(fullRange),
- m_offset(offset) {
- AddChild(m_analog_input);
+ : AnalogPotentiometer(std::make_shared<AnalogInput>(channel), fullRange,
+ offset) {
+ SendableRegistry::GetInstance().AddChild(this, m_analog_input.get());
}
AnalogPotentiometer::AnalogPotentiometer(AnalogInput* input, double fullRange,
double offset)
- : m_analog_input(input, NullDeleter<AnalogInput>()),
- m_fullRange(fullRange),
- m_offset(offset) {}
+ : AnalogPotentiometer(
+ std::shared_ptr<AnalogInput>(input, NullDeleter<AnalogInput>()),
+ fullRange, offset) {}
AnalogPotentiometer::AnalogPotentiometer(std::shared_ptr<AnalogInput> input,
double fullRange, double offset)
- : m_analog_input(input), m_fullRange(fullRange), m_offset(offset) {}
+ : m_analog_input(input), m_fullRange(fullRange), m_offset(offset) {
+ SendableRegistry::GetInstance().AddLW(this, "AnalogPotentiometer",
+ m_analog_input->GetChannel());
+}
double AnalogPotentiometer::Get() const {
return (m_analog_input->GetVoltage() / RobotController::GetVoltage5V()) *
diff --git a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
index aeb58df..60ce04a 100644
--- a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,13 +13,14 @@
#include "frc/AnalogInput.h"
#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
AnalogTrigger::AnalogTrigger(int channel)
: AnalogTrigger(new AnalogInput(channel)) {
m_ownsAnalog = true;
- AddChild(m_analogInput);
+ SendableRegistry::GetInstance().AddChild(this, m_analogInput);
}
AnalogTrigger::AnalogTrigger(AnalogInput* input) {
@@ -36,7 +37,8 @@
m_index = index;
HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, input->m_channel);
- SetName("AnalogTrigger", input->GetChannel());
+ SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger",
+ input->GetChannel());
}
AnalogTrigger::~AnalogTrigger() {
@@ -50,19 +52,19 @@
AnalogTrigger::AnalogTrigger(AnalogTrigger&& rhs)
: ErrorBase(std::move(rhs)),
- SendableBase(std::move(rhs)),
- m_index(std::move(rhs.m_index)) {
- std::swap(m_trigger, rhs.m_trigger);
+ SendableHelper(std::move(rhs)),
+ m_index(std::move(rhs.m_index)),
+ m_trigger(std::move(rhs.m_trigger)) {
std::swap(m_analogInput, rhs.m_analogInput);
std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
}
AnalogTrigger& AnalogTrigger::operator=(AnalogTrigger&& rhs) {
ErrorBase::operator=(std::move(rhs));
- SendableBase::operator=(std::move(rhs));
+ SendableHelper::operator=(std::move(rhs));
m_index = std::move(rhs.m_index);
- std::swap(m_trigger, rhs.m_trigger);
+ m_trigger = std::move(rhs.m_trigger);
std::swap(m_analogInput, rhs.m_analogInput);
std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
diff --git a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
index b2a8cd5..c2c7265 100644
--- a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,26 +14,17 @@
using namespace frc;
-AnalogTriggerOutput::~AnalogTriggerOutput() {
- if (m_interrupt != HAL_kInvalidHandle) {
- int32_t status = 0;
- HAL_CleanInterrupts(m_interrupt, &status);
- // ignore status, as an invalid handle just needs to be ignored.
- m_interrupt = HAL_kInvalidHandle;
- }
-}
-
bool AnalogTriggerOutput::Get() const {
int32_t status = 0;
bool result = HAL_GetAnalogTriggerOutput(
- m_trigger.m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
+ m_trigger->m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
&status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
return result;
}
HAL_Handle AnalogTriggerOutput::GetPortHandleForRouting() const {
- return m_trigger.m_trigger;
+ return m_trigger->m_trigger;
}
AnalogTriggerType AnalogTriggerOutput::GetAnalogTriggerTypeForRouting() const {
@@ -42,13 +33,13 @@
bool AnalogTriggerOutput::IsAnalogTrigger() const { return true; }
-int AnalogTriggerOutput::GetChannel() const { return m_trigger.m_index; }
+int AnalogTriggerOutput::GetChannel() const { return m_trigger->m_index; }
void AnalogTriggerOutput::InitSendable(SendableBuilder&) {}
AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger& trigger,
AnalogTriggerType outputType)
- : m_trigger(trigger), m_outputType(outputType) {
+ : m_trigger(&trigger), m_outputType(outputType) {
HAL_Report(HALUsageReporting::kResourceType_AnalogTriggerOutput,
trigger.GetIndex(), static_cast<uint8_t>(outputType));
}
diff --git a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
index 8452e38..71c8e89 100644
--- a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,6 +12,7 @@
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -20,7 +21,7 @@
HAL_Report(HALUsageReporting::kResourceType_Accelerometer, 0, 0,
"Built-in accelerometer");
- SetName("BuiltInAccel", 0);
+ SendableRegistry::GetInstance().AddLW(this, "BuiltInAccel");
}
void BuiltInAccelerometer::SetRange(Range range) {
diff --git a/wpilibc/src/main/native/cpp/CAN.cpp b/wpilibc/src/main/native/cpp/CAN.cpp
index f01eb3f..35a926a 100644
--- a/wpilibc/src/main/native/cpp/CAN.cpp
+++ b/wpilibc/src/main/native/cpp/CAN.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -52,18 +52,6 @@
}
}
-CAN::CAN(CAN&& rhs) : ErrorBase(std::move(rhs)) {
- std::swap(m_handle, rhs.m_handle);
-}
-
-CAN& CAN::operator=(CAN&& rhs) {
- ErrorBase::operator=(std::move(rhs));
-
- std::swap(m_handle, rhs.m_handle);
-
- return *this;
-}
-
void CAN::WritePacket(const uint8_t* data, int length, int apiId) {
int32_t status = 0;
HAL_WriteCANPacket(m_handle, data, length, apiId, &status);
@@ -77,6 +65,12 @@
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
+void CAN::WriteRTRFrame(int length, int apiId) {
+ int32_t status = 0;
+ HAL_WriteCANRTRFrame(m_handle, length, apiId, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
void CAN::StopPacketRepeating(int apiId) {
int32_t status = 0;
HAL_StopCANPacketRepeating(m_handle, apiId, &status);
@@ -128,20 +122,3 @@
return true;
}
}
-
-bool CAN::ReadPeriodicPacket(int apiId, int timeoutMs, int periodMs,
- CANData* data) {
- int32_t status = 0;
- HAL_ReadCANPeriodicPacket(m_handle, apiId, data->data, &data->length,
- &data->timestamp, timeoutMs, periodMs, &status);
- if (status == HAL_CAN_TIMEOUT ||
- status == HAL_ERR_CANSessionMux_MessageNotFound) {
- return false;
- }
- if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- return false;
- } else {
- return true;
- }
-}
diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp
index 48e1ed6..10d75f1 100644
--- a/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,6 +14,7 @@
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -28,7 +29,7 @@
SetClosedLoopControl(true);
HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID);
- SetName("Compressor", pcmID);
+ SendableRegistry::GetInstance().AddLW(this, "Compressor", pcmID);
}
void Compressor::Start() {
diff --git a/wpilibc/src/main/native/cpp/ControllerPower.cpp b/wpilibc/src/main/native/cpp/ControllerPower.cpp
deleted file mode 100644
index d3012ea..0000000
--- a/wpilibc/src/main/native/cpp/ControllerPower.cpp
+++ /dev/null
@@ -1,115 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/ControllerPower.h"
-
-#include <stdint.h>
-
-#include <hal/HAL.h>
-#include <hal/Power.h>
-
-#include "frc/ErrorBase.h"
-
-using namespace frc;
-
-double ControllerPower::GetInputVoltage() {
- int32_t status = 0;
- double retVal = HAL_GetVinVoltage(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-double ControllerPower::GetInputCurrent() {
- int32_t status = 0;
- double retVal = HAL_GetVinCurrent(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-double ControllerPower::GetVoltage3V3() {
- int32_t status = 0;
- double retVal = HAL_GetUserVoltage3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-double ControllerPower::GetCurrent3V3() {
- int32_t status = 0;
- double retVal = HAL_GetUserCurrent3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-bool ControllerPower::GetEnabled3V3() {
- int32_t status = 0;
- bool retVal = HAL_GetUserActive3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-int ControllerPower::GetFaultCount3V3() {
- int32_t status = 0;
- int retVal = HAL_GetUserCurrentFaults3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-double ControllerPower::GetVoltage5V() {
- int32_t status = 0;
- double retVal = HAL_GetUserVoltage5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-double ControllerPower::GetCurrent5V() {
- int32_t status = 0;
- double retVal = HAL_GetUserCurrent5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-bool ControllerPower::GetEnabled5V() {
- int32_t status = 0;
- bool retVal = HAL_GetUserActive5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-int ControllerPower::GetFaultCount5V() {
- int32_t status = 0;
- int retVal = HAL_GetUserCurrentFaults5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-double ControllerPower::GetVoltage6V() {
- int32_t status = 0;
- double retVal = HAL_GetUserVoltage6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-double ControllerPower::GetCurrent6V() {
- int32_t status = 0;
- double retVal = HAL_GetUserCurrent6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-bool ControllerPower::GetEnabled6V() {
- int32_t status = 0;
- bool retVal = HAL_GetUserActive6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-int ControllerPower::GetFaultCount6V() {
- int32_t status = 0;
- int retVal = HAL_GetUserCurrentFaults6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
diff --git a/wpilibc/src/main/native/cpp/Counter.cpp b/wpilibc/src/main/native/cpp/Counter.cpp
index c97bbc5..1ef40fd 100644
--- a/wpilibc/src/main/native/cpp/Counter.cpp
+++ b/wpilibc/src/main/native/cpp/Counter.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -15,6 +15,7 @@
#include "frc/DigitalInput.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -26,7 +27,7 @@
SetMaxPeriod(.5);
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index, mode);
- SetName("Counter", m_index);
+ SendableRegistry::GetInstance().AddLW(this, "Counter", m_index);
}
Counter::Counter(int channel) : Counter(kTwoPulse) {
@@ -90,36 +91,12 @@
int32_t status = 0;
HAL_FreeCounter(m_counter, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- m_counter = HAL_kInvalidHandle;
-}
-
-Counter::Counter(Counter&& rhs)
- : ErrorBase(std::move(rhs)),
- SendableBase(std::move(rhs)),
- CounterBase(std::move(rhs)),
- m_upSource(std::move(rhs.m_upSource)),
- m_downSource(std::move(rhs.m_downSource)),
- m_index(std::move(rhs.m_index)) {
- std::swap(m_counter, rhs.m_counter);
-}
-
-Counter& Counter::operator=(Counter&& rhs) {
- ErrorBase::operator=(std::move(rhs));
- SendableBase::operator=(std::move(rhs));
- CounterBase::operator=(std::move(rhs));
-
- m_upSource = std::move(rhs.m_upSource);
- m_downSource = std::move(rhs.m_downSource);
- std::swap(m_counter, rhs.m_counter);
- m_index = std::move(rhs.m_index);
-
- return *this;
}
void Counter::SetUpSource(int channel) {
if (StatusIsFatal()) return;
SetUpSource(std::make_shared<DigitalInput>(channel));
- AddChild(m_upSource);
+ SendableRegistry::GetInstance().AddChild(this, m_upSource.get());
}
void Counter::SetUpSource(AnalogTrigger* analogTrigger,
@@ -183,7 +160,7 @@
void Counter::SetDownSource(int channel) {
if (StatusIsFatal()) return;
SetDownSource(std::make_shared<DigitalInput>(channel));
- AddChild(m_downSource);
+ SendableRegistry::GetInstance().AddChild(this, m_downSource.get());
}
void Counter::SetDownSource(AnalogTrigger* analogTrigger,
diff --git a/wpilibc/src/main/native/cpp/DMC60.cpp b/wpilibc/src/main/native/cpp/DMC60.cpp
index 7dc2d8d..d61bd40 100644
--- a/wpilibc/src/main/native/cpp/DMC60.cpp
+++ b/wpilibc/src/main/native/cpp/DMC60.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <hal/HAL.h>
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
DMC60::DMC60(int channel) : PWMSpeedController(channel) {
@@ -32,5 +34,5 @@
SetZeroLatch();
HAL_Report(HALUsageReporting::kResourceType_DigilentDMC60, GetChannel());
- SetName("DMC60", GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "DMC60", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
index 71211b3..9e92da8 100644
--- a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -20,6 +20,7 @@
#include "frc/SensorUtil.h"
#include "frc/Utility.h"
#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -28,7 +29,7 @@
wpi::mutex DigitalGlitchFilter::m_mutex;
DigitalGlitchFilter::DigitalGlitchFilter() {
- std::lock_guard<wpi::mutex> lock(m_mutex);
+ std::scoped_lock lock(m_mutex);
auto index =
std::find(m_filterAllocated.begin(), m_filterAllocated.end(), false);
wpi_assert(index != m_filterAllocated.end());
@@ -38,24 +39,25 @@
HAL_Report(HALUsageReporting::kResourceType_DigitalGlitchFilter,
m_channelIndex);
- SetName("DigitalGlitchFilter", m_channelIndex);
+ SendableRegistry::GetInstance().AddLW(this, "DigitalGlitchFilter",
+ m_channelIndex);
}
DigitalGlitchFilter::~DigitalGlitchFilter() {
if (m_channelIndex >= 0) {
- std::lock_guard<wpi::mutex> lock(m_mutex);
+ std::scoped_lock lock(m_mutex);
m_filterAllocated[m_channelIndex] = false;
}
}
DigitalGlitchFilter::DigitalGlitchFilter(DigitalGlitchFilter&& rhs)
- : ErrorBase(std::move(rhs)), SendableBase(std::move(rhs)) {
+ : ErrorBase(std::move(rhs)), SendableHelper(std::move(rhs)) {
std::swap(m_channelIndex, rhs.m_channelIndex);
}
DigitalGlitchFilter& DigitalGlitchFilter::operator=(DigitalGlitchFilter&& rhs) {
ErrorBase::operator=(std::move(rhs));
- SendableBase::operator=(std::move(rhs));
+ SendableHelper::operator=(std::move(rhs));
std::swap(m_channelIndex, rhs.m_channelIndex);
diff --git a/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp
index 273e9b6..d0fa41b 100644
--- a/wpilibc/src/main/native/cpp/DigitalInput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -17,6 +17,7 @@
#include "frc/SensorUtil.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -40,35 +41,14 @@
}
HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel);
- SetName("DigitalInput", channel);
+ SendableRegistry::GetInstance().AddLW(this, "DigitalInput", channel);
}
DigitalInput::~DigitalInput() {
if (StatusIsFatal()) return;
- if (m_interrupt != HAL_kInvalidHandle) {
- int32_t status = 0;
- HAL_CleanInterrupts(m_interrupt, &status);
- // Ignore status, as an invalid handle just needs to be ignored.
- m_interrupt = HAL_kInvalidHandle;
- }
-
HAL_FreeDIOPort(m_handle);
}
-DigitalInput::DigitalInput(DigitalInput&& rhs)
- : DigitalSource(std::move(rhs)), m_channel(std::move(rhs.m_channel)) {
- std::swap(m_handle, rhs.m_handle);
-}
-
-DigitalInput& DigitalInput::operator=(DigitalInput&& rhs) {
- DigitalSource::operator=(std::move(rhs));
-
- m_channel = std::move(rhs.m_channel);
- std::swap(m_handle, rhs.m_handle);
-
- return *this;
-}
-
bool DigitalInput::Get() const {
if (StatusIsFatal()) return false;
int32_t status = 0;
@@ -85,6 +65,10 @@
bool DigitalInput::IsAnalogTrigger() const { return false; }
+void DigitalInput::SetSimDevice(HAL_SimDeviceHandle device) {
+ HAL_SetDIOSimDevice(m_handle, device);
+}
+
int DigitalInput::GetChannel() const { return m_channel; }
void DigitalInput::InitSendable(SendableBuilder& builder) {
diff --git a/wpilibc/src/main/native/cpp/DigitalOutput.cpp b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
index b05c6b1..2068b5d 100644
--- a/wpilibc/src/main/native/cpp/DigitalOutput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -17,6 +17,7 @@
#include "frc/SensorUtil.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -41,7 +42,7 @@
}
HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel);
- SetName("DigitalOutput", channel);
+ SendableRegistry::GetInstance().AddLW(this, "DigitalOutput", channel);
}
DigitalOutput::~DigitalOutput() {
@@ -52,25 +53,6 @@
HAL_FreeDIOPort(m_handle);
}
-DigitalOutput::DigitalOutput(DigitalOutput&& rhs)
- : ErrorBase(std::move(rhs)),
- SendableBase(std::move(rhs)),
- m_channel(std::move(rhs.m_channel)),
- m_pwmGenerator(std::move(rhs.m_pwmGenerator)) {
- std::swap(m_handle, rhs.m_handle);
-}
-
-DigitalOutput& DigitalOutput::operator=(DigitalOutput&& rhs) {
- ErrorBase::operator=(std::move(rhs));
- SendableBase::operator=(std::move(rhs));
-
- m_channel = std::move(rhs.m_channel);
- std::swap(m_handle, rhs.m_handle);
- m_pwmGenerator = std::move(rhs.m_pwmGenerator);
-
- return *this;
-}
-
void DigitalOutput::Set(bool value) {
if (StatusIsFatal()) return;
@@ -158,6 +140,10 @@
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
+void DigitalOutput::SetSimDevice(HAL_SimDeviceHandle device) {
+ HAL_SetDIOSimDevice(m_handle, device);
+}
+
void DigitalOutput::InitSendable(SendableBuilder& builder) {
builder.SetSmartDashboardType("Digital Output");
builder.AddBooleanProperty("Value", [=]() { return Get(); },
diff --git a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
index 86678aa..0b35ff5 100644
--- a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
+++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,6 +16,7 @@
#include "frc/SensorUtil.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -75,7 +76,9 @@
m_moduleNumber);
HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel,
m_moduleNumber);
- SetName("DoubleSolenoid", m_moduleNumber, m_forwardChannel);
+
+ SendableRegistry::GetInstance().AddLW(this, "DoubleSolenoid", m_moduleNumber,
+ m_forwardChannel);
}
DoubleSolenoid::~DoubleSolenoid() {
@@ -83,29 +86,6 @@
HAL_FreeSolenoidPort(m_reverseHandle);
}
-DoubleSolenoid::DoubleSolenoid(DoubleSolenoid&& rhs)
- : SolenoidBase(std::move(rhs)),
- m_forwardChannel(std::move(rhs.m_forwardChannel)),
- m_reverseChannel(std::move(rhs.m_reverseChannel)),
- m_forwardMask(std::move(rhs.m_forwardMask)),
- m_reverseMask(std::move(rhs.m_reverseMask)) {
- std::swap(m_forwardHandle, rhs.m_forwardHandle);
- std::swap(m_reverseHandle, rhs.m_reverseHandle);
-}
-
-DoubleSolenoid& DoubleSolenoid::operator=(DoubleSolenoid&& rhs) {
- SolenoidBase::operator=(std::move(rhs));
-
- m_forwardChannel = std::move(rhs.m_forwardChannel);
- m_reverseChannel = std::move(rhs.m_reverseChannel);
- m_forwardMask = std::move(rhs.m_forwardMask);
- m_reverseMask = std::move(rhs.m_reverseMask);
- std::swap(m_forwardHandle, rhs.m_forwardHandle);
- std::swap(m_reverseHandle, rhs.m_reverseHandle);
-
- return *this;
-}
-
void DoubleSolenoid::Set(Value value) {
if (StatusIsFatal()) return;
diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp
index 3c274c6..d268de7 100644
--- a/wpilibc/src/main/native/cpp/DriverStation.cpp
+++ b/wpilibc/src/main/native/cpp/DriverStation.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,7 +11,6 @@
#include <hal/HAL.h>
#include <hal/Power.h>
-#include <hal/cpp/Log.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableInstance.h>
@@ -146,7 +145,7 @@
"Joystick Button missing, check if all controllers are plugged in");
return false;
}
- std::unique_lock<wpi::mutex> lock(m_buttonEdgeMutex);
+ std::unique_lock lock(m_buttonEdgeMutex);
// If button was pressed, clear flag and return true
if (m_joystickButtonsPressed[stick] & 1 << (button - 1)) {
m_joystickButtonsPressed[stick] &= ~(1 << (button - 1));
@@ -175,7 +174,7 @@
"Joystick Button missing, check if all controllers are plugged in");
return false;
}
- std::unique_lock<wpi::mutex> lock(m_buttonEdgeMutex);
+ std::unique_lock lock(m_buttonEdgeMutex);
// If button was released, clear flag and return true
if (m_joystickButtonsReleased[stick] & 1 << (button - 1)) {
m_joystickButtonsReleased[stick] &= ~(1 << (button - 1));
@@ -336,6 +335,12 @@
return !(controlWord.enabled && controlWord.dsAttached);
}
+bool DriverStation::IsEStopped() const {
+ HAL_ControlWord controlWord;
+ HAL_GetControlWord(&controlWord);
+ return controlWord.eStop;
+}
+
bool DriverStation::IsAutonomous() const {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
@@ -368,20 +373,6 @@
return controlWord.fmsAttached;
}
-bool DriverStation::IsSysActive() const {
- int32_t status = 0;
- bool retVal = HAL_GetSystemActive(&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
-bool DriverStation::IsBrownedOut() const {
- int32_t status = 0;
- bool retVal = HAL_GetBrownedOut(&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- return retVal;
-}
-
std::string DriverStation::GetGameSpecificMessage() const {
HAL_MatchInfo info;
HAL_GetMatchInfo(&info);
@@ -454,7 +445,7 @@
auto timeoutTime =
std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
- std::unique_lock<wpi::mutex> lock(m_waitForDataMutex);
+ std::unique_lock lock(m_waitForDataMutex);
int currentCount = m_waitForDataCounter;
while (m_waitForDataCounter == currentCount) {
if (timeout > 0) {
@@ -486,7 +477,7 @@
{
// Compute the pressed and released buttons
HAL_JoystickButtons currentButtons;
- std::unique_lock<wpi::mutex> lock(m_buttonEdgeMutex);
+ std::unique_lock lock(m_buttonEdgeMutex);
for (int32_t i = 0; i < kJoystickPorts; i++) {
HAL_GetJoystickButtons(i, ¤tButtons);
@@ -504,7 +495,7 @@
}
{
- std::lock_guard<wpi::mutex> waitLock(m_waitForDataMutex);
+ std::scoped_lock waitLock(m_waitForDataMutex);
// Nofify all threads
m_waitForDataCounter++;
m_waitForDataCond.notify_all();
diff --git a/wpilibc/src/main/native/cpp/Encoder.cpp b/wpilibc/src/main/native/cpp/Encoder.cpp
index 77e7a2a..fb043cc 100644
--- a/wpilibc/src/main/native/cpp/Encoder.cpp
+++ b/wpilibc/src/main/native/cpp/Encoder.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,6 +16,7 @@
#include "frc/DigitalInput.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -24,8 +25,9 @@
m_aSource = std::make_shared<DigitalInput>(aChannel);
m_bSource = std::make_shared<DigitalInput>(bChannel);
InitEncoder(reverseDirection, encodingType);
- AddChild(m_aSource);
- AddChild(m_bSource);
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, m_aSource.get());
+ registry.AddChild(this, m_bSource.get());
}
Encoder::Encoder(DigitalSource* aSource, DigitalSource* bSource,
@@ -61,31 +63,6 @@
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
-Encoder::Encoder(Encoder&& rhs)
- : ErrorBase(std::move(rhs)),
- SendableBase(std::move(rhs)),
- CounterBase(std::move(rhs)),
- PIDSource(std::move(rhs)),
- m_aSource(std::move(rhs.m_aSource)),
- m_bSource(std::move(rhs.m_bSource)),
- m_indexSource(std::move(rhs.m_indexSource)) {
- std::swap(m_encoder, rhs.m_encoder);
-}
-
-Encoder& Encoder::operator=(Encoder&& rhs) {
- ErrorBase::operator=(std::move(rhs));
- SendableBase::operator=(std::move(rhs));
- CounterBase::operator=(std::move(rhs));
- PIDSource::operator=(std::move(rhs));
-
- m_aSource = std::move(rhs.m_aSource);
- m_bSource = std::move(rhs.m_bSource);
- m_indexSource = std::move(rhs.m_indexSource);
- std::swap(m_encoder, rhs.m_encoder);
-
- return *this;
-}
-
int Encoder::Get() const {
if (StatusIsFatal()) return 0;
int32_t status = 0;
@@ -226,7 +203,7 @@
void Encoder::SetIndexSource(int channel, Encoder::IndexingType type) {
// Force digital input if just given an index
m_indexSource = std::make_shared<DigitalInput>(channel);
- AddChild(m_indexSource);
+ SendableRegistry::GetInstance().AddChild(this, m_indexSource.get());
SetIndexSource(*m_indexSource.get(), type);
}
@@ -240,6 +217,10 @@
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
+void Encoder::SetSimDevice(HAL_SimDeviceHandle device) {
+ HAL_SetEncoderSimDevice(m_encoder, device);
+}
+
int Encoder::GetFPGAIndex() const {
int32_t status = 0;
int val = HAL_GetEncoderFPGAIndex(m_encoder, &status);
@@ -275,7 +256,8 @@
HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex(),
encodingType);
- SetName("Encoder", m_aSource->GetChannel());
+ SendableRegistry::GetInstance().AddLW(this, "Encoder",
+ m_aSource->GetChannel());
}
double Encoder::DecodingScaleFactor() const {
diff --git a/wpilibc/src/main/native/cpp/Error.cpp b/wpilibc/src/main/native/cpp/Error.cpp
index f758032..ee7dcbd 100644
--- a/wpilibc/src/main/native/cpp/Error.cpp
+++ b/wpilibc/src/main/native/cpp/Error.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,6 +8,7 @@
#include "frc/Error.h"
#include <wpi/Path.h>
+#include <wpi/StackTrace.h>
#include "frc/DriverStation.h"
#include "frc/Timer.h"
@@ -84,7 +85,7 @@
true, m_code, m_message,
m_function + wpi::Twine(" [") + wpi::sys::path::filename(m_filename) +
wpi::Twine(':') + wpi::Twine(m_lineNumber) + wpi::Twine(']'),
- GetStackTrace(4));
+ wpi::GetStackTrace(4));
}
void Error::Clear() {
diff --git a/wpilibc/src/main/native/cpp/ErrorBase.cpp b/wpilibc/src/main/native/cpp/ErrorBase.cpp
index 947e53b..8e70e61 100644
--- a/wpilibc/src/main/native/cpp/ErrorBase.cpp
+++ b/wpilibc/src/main/native/cpp/ErrorBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -17,13 +17,38 @@
#include <wpi/SmallString.h>
#include <wpi/raw_ostream.h>
-#define WPI_ERRORS_DEFINE_STRINGS
#include "frc/WPIErrors.h"
using namespace frc;
-static wpi::mutex globalErrorsMutex;
-static std::set<Error> globalErrors;
+namespace {
+struct GlobalErrors {
+ wpi::mutex mutex;
+ std::set<Error> errors;
+ const Error* lastError{nullptr};
+
+ static GlobalErrors& GetInstance();
+ static void Insert(const Error& error);
+ static void Insert(Error&& error);
+};
+} // namespace
+
+GlobalErrors& GlobalErrors::GetInstance() {
+ static GlobalErrors inst;
+ return inst;
+}
+
+void GlobalErrors::Insert(const Error& error) {
+ GlobalErrors& inst = GetInstance();
+ std::scoped_lock lock(inst.mutex);
+ inst.lastError = &(*inst.errors.insert(error).first);
+}
+
+void GlobalErrors::Insert(Error&& error) {
+ GlobalErrors& inst = GetInstance();
+ std::scoped_lock lock(inst.mutex);
+ inst.lastError = &(*inst.errors.insert(std::move(error)).first);
+}
ErrorBase::ErrorBase() { HAL_Initialize(500, 0); }
@@ -51,8 +76,7 @@
this);
// Update the global error if there is not one already set.
- std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
- globalErrors.insert(m_error);
+ GlobalErrors::Insert(m_error);
}
void ErrorBase::SetImaqError(int success, const wpi::Twine& contextMessage,
@@ -65,8 +89,7 @@
function, lineNumber, this);
// Update the global error if there is not one already set.
- std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
- globalErrors.insert(m_error);
+ GlobalErrors::Insert(m_error);
}
}
@@ -79,8 +102,7 @@
m_error.Set(code, contextMessage, filename, function, lineNumber, this);
// Update the global error if there is not one already set.
- std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
- globalErrors.insert(m_error);
+ GlobalErrors::Insert(m_error);
}
}
@@ -99,8 +121,7 @@
filename, function, lineNumber, this);
// Update the global error if there is not one already set.
- std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
- globalErrors.insert(m_error);
+ GlobalErrors::Insert(m_error);
}
}
@@ -113,8 +134,7 @@
lineNumber, this);
// Update the global error if there is not one already set.
- std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
- globalErrors.insert(m_error);
+ GlobalErrors::Insert(m_error);
}
void ErrorBase::CloneError(const ErrorBase& rhs) const {
@@ -129,11 +149,9 @@
int lineNumber) {
// If there was an error
if (code != 0) {
- std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
-
// Set the current error information for this object.
- globalErrors.emplace(code, contextMessage, filename, function, lineNumber,
- nullptr);
+ GlobalErrors::Insert(
+ Error(code, contextMessage, filename, function, lineNumber, nullptr));
}
}
@@ -141,12 +159,28 @@
const wpi::Twine& contextMessage,
wpi::StringRef filename,
wpi::StringRef function, int lineNumber) {
- std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
- globalErrors.emplace(-1, errorMessage + ": " + contextMessage, filename,
- function, lineNumber, nullptr);
+ GlobalErrors::Insert(Error(-1, errorMessage + ": " + contextMessage, filename,
+ function, lineNumber, nullptr));
}
-const Error& ErrorBase::GetGlobalError() {
- std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
- return *globalErrors.begin();
+Error ErrorBase::GetGlobalError() {
+ auto& inst = GlobalErrors::GetInstance();
+ std::scoped_lock mutex(inst.mutex);
+ if (!inst.lastError) return Error{};
+ return *inst.lastError;
+}
+
+std::vector<Error> ErrorBase::GetGlobalErrors() {
+ auto& inst = GlobalErrors::GetInstance();
+ std::scoped_lock mutex(inst.mutex);
+ std::vector<Error> rv;
+ for (auto&& error : inst.errors) rv.push_back(error);
+ return rv;
+}
+
+void ErrorBase::ClearGlobalErrors() {
+ auto& inst = GlobalErrors::GetInstance();
+ std::scoped_lock mutex(inst.mutex);
+ inst.errors.clear();
+ inst.lastError = nullptr;
}
diff --git a/wpilibc/src/main/native/cpp/GamepadBase.cpp b/wpilibc/src/main/native/cpp/GamepadBase.cpp
deleted file mode 100644
index 4eeb744..0000000
--- a/wpilibc/src/main/native/cpp/GamepadBase.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/GamepadBase.h"
-
-using namespace frc;
-
-GamepadBase::GamepadBase(int port) : GenericHID(port) {}
diff --git a/wpilibc/src/main/native/cpp/GearTooth.cpp b/wpilibc/src/main/native/cpp/GearTooth.cpp
index fba5a24..5fb5021 100644
--- a/wpilibc/src/main/native/cpp/GearTooth.cpp
+++ b/wpilibc/src/main/native/cpp/GearTooth.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,6 +8,7 @@
#include "frc/GearTooth.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -15,20 +16,22 @@
GearTooth::GearTooth(int channel, bool directionSensitive) : Counter(channel) {
EnableDirectionSensing(directionSensitive);
- SetName("GearTooth", channel);
+ SendableRegistry::GetInstance().SetName(this, "GearTooth", channel);
}
GearTooth::GearTooth(DigitalSource* source, bool directionSensitive)
: Counter(source) {
EnableDirectionSensing(directionSensitive);
- SetName("GearTooth", source->GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "GearTooth",
+ source->GetChannel());
}
GearTooth::GearTooth(std::shared_ptr<DigitalSource> source,
bool directionSensitive)
: Counter(source) {
EnableDirectionSensing(directionSensitive);
- SetName("GearTooth", source->GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "GearTooth",
+ source->GetChannel());
}
void GearTooth::EnableDirectionSensing(bool directionSensitive) {
diff --git a/wpilibc/src/main/native/cpp/GenericHID.cpp b/wpilibc/src/main/native/cpp/GenericHID.cpp
index d1a3112..504d669 100644
--- a/wpilibc/src/main/native/cpp/GenericHID.cpp
+++ b/wpilibc/src/main/native/cpp/GenericHID.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,7 +14,7 @@
using namespace frc;
-GenericHID::GenericHID(int port) : m_ds(DriverStation::GetInstance()) {
+GenericHID::GenericHID(int port) : m_ds(&DriverStation::GetInstance()) {
if (port >= DriverStation::kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
}
@@ -22,39 +22,41 @@
}
bool GenericHID::GetRawButton(int button) const {
- return m_ds.GetStickButton(m_port, button);
+ return m_ds->GetStickButton(m_port, button);
}
bool GenericHID::GetRawButtonPressed(int button) {
- return m_ds.GetStickButtonPressed(m_port, button);
+ return m_ds->GetStickButtonPressed(m_port, button);
}
bool GenericHID::GetRawButtonReleased(int button) {
- return m_ds.GetStickButtonReleased(m_port, button);
+ return m_ds->GetStickButtonReleased(m_port, button);
}
double GenericHID::GetRawAxis(int axis) const {
- return m_ds.GetStickAxis(m_port, axis);
+ return m_ds->GetStickAxis(m_port, axis);
}
-int GenericHID::GetPOV(int pov) const { return m_ds.GetStickPOV(m_port, pov); }
+int GenericHID::GetPOV(int pov) const { return m_ds->GetStickPOV(m_port, pov); }
-int GenericHID::GetAxisCount() const { return m_ds.GetStickAxisCount(m_port); }
+int GenericHID::GetAxisCount() const { return m_ds->GetStickAxisCount(m_port); }
-int GenericHID::GetPOVCount() const { return m_ds.GetStickPOVCount(m_port); }
+int GenericHID::GetPOVCount() const { return m_ds->GetStickPOVCount(m_port); }
int GenericHID::GetButtonCount() const {
- return m_ds.GetStickButtonCount(m_port);
+ return m_ds->GetStickButtonCount(m_port);
}
GenericHID::HIDType GenericHID::GetType() const {
- return static_cast<HIDType>(m_ds.GetJoystickType(m_port));
+ return static_cast<HIDType>(m_ds->GetJoystickType(m_port));
}
-std::string GenericHID::GetName() const { return m_ds.GetJoystickName(m_port); }
+std::string GenericHID::GetName() const {
+ return m_ds->GetJoystickName(m_port);
+}
int GenericHID::GetAxisType(int axis) const {
- return m_ds.GetJoystickAxisType(m_port, axis);
+ return m_ds->GetJoystickAxisType(m_port, axis);
}
int GenericHID::GetPort() const { return m_port; }
diff --git a/wpilibc/src/main/native/cpp/I2C.cpp b/wpilibc/src/main/native/cpp/I2C.cpp
index 4b18f73..44b71af 100644
--- a/wpilibc/src/main/native/cpp/I2C.cpp
+++ b/wpilibc/src/main/native/cpp/I2C.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -27,21 +27,6 @@
I2C::~I2C() { HAL_CloseI2C(m_port); }
-I2C::I2C(I2C&& rhs)
- : ErrorBase(std::move(rhs)),
- m_deviceAddress(std::move(rhs.m_deviceAddress)) {
- std::swap(m_port, rhs.m_port);
-}
-
-I2C& I2C::operator=(I2C&& rhs) {
- ErrorBase::operator=(std::move(rhs));
-
- std::swap(m_port, rhs.m_port);
- m_deviceAddress = std::move(rhs.m_deviceAddress);
-
- return *this;
-}
-
bool I2C::Transaction(uint8_t* dataToSend, int sendSize, uint8_t* dataReceived,
int receiveSize) {
int32_t status = 0;
diff --git a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
index 220043f..36150d4 100644
--- a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
+++ b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,6 +14,13 @@
using namespace frc;
+InterruptableSensorBase::~InterruptableSensorBase() {
+ if (m_interrupt == HAL_kInvalidHandle) return;
+ int32_t status = 0;
+ HAL_CleanInterrupts(m_interrupt, &status);
+ // Ignore status, as an invalid handle just needs to be ignored.
+}
+
void InterruptableSensorBase::RequestInterrupts(
HAL_InterruptHandlerFunction handler, void* param) {
if (StatusIsFatal()) return;
@@ -32,6 +39,39 @@
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
+void InterruptableSensorBase::RequestInterrupts(InterruptEventHandler handler) {
+ if (StatusIsFatal()) return;
+
+ wpi_assert(m_interrupt == HAL_kInvalidHandle);
+ AllocateInterrupts(false);
+ if (StatusIsFatal()) return; // if allocate failed, out of interrupts
+
+ m_interruptHandler =
+ std::make_unique<InterruptEventHandler>(std::move(handler));
+
+ int32_t status = 0;
+ HAL_RequestInterrupts(
+ m_interrupt, GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(GetAnalogTriggerTypeForRouting()),
+ &status);
+ SetUpSourceEdge(true, false);
+ HAL_AttachInterruptHandler(
+ m_interrupt,
+ [](uint32_t mask, void* param) {
+ auto self = reinterpret_cast<InterruptEventHandler*>(param);
+ // Rising edge result is the interrupt bit set in the byte 0xFF
+ // Falling edge result is the interrupt bit set in the byte 0xFF00
+ // Set any bit set to be true for that edge, and AND the 2 results
+ // together to match the existing enum for all interrupts
+ int32_t rising = (mask & 0xFF) ? 0x1 : 0x0;
+ int32_t falling = ((mask & 0xFF00) ? 0x0100 : 0x0);
+ WaitResult res = static_cast<WaitResult>(falling | rising);
+ (*self)(res);
+ },
+ m_interruptHandler.get(), &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
void InterruptableSensorBase::RequestInterrupts() {
if (StatusIsFatal()) return;
@@ -55,6 +95,7 @@
HAL_CleanInterrupts(m_interrupt, &status);
// Ignore status, as an invalid handle just needs to be ignored.
m_interrupt = HAL_kInvalidHandle;
+ m_interruptHandler = nullptr;
}
InterruptableSensorBase::WaitResult InterruptableSensorBase::WaitForInterrupt(
diff --git a/wpilibc/src/main/native/cpp/IterativeRobot.cpp b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
index 0f3add3..950b239 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobot.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,7 +13,7 @@
using namespace frc;
-static constexpr double kPacketPeriod = 0.02;
+static constexpr auto kPacketPeriod = 0.02_s;
IterativeRobot::IterativeRobot() : IterativeRobotBase(kPacketPeriod) {
HAL_Report(HALUsageReporting::kResourceType_Framework,
diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
index 0de2004..90dc54b 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,6 +10,7 @@
#include <cstdio>
#include <hal/HAL.h>
+#include <wpi/Format.h>
#include <wpi/SmallString.h>
#include <wpi/raw_ostream.h>
@@ -17,11 +18,15 @@
#include "frc/Timer.h"
#include "frc/commands/Scheduler.h"
#include "frc/livewindow/LiveWindow.h"
+#include "frc/shuffleboard/Shuffleboard.h"
#include "frc/smartdashboard/SmartDashboard.h"
using namespace frc;
IterativeRobotBase::IterativeRobotBase(double period)
+ : IterativeRobotBase(units::second_t(period)) {}
+
+IterativeRobotBase::IterativeRobotBase(units::second_t period)
: m_period(period),
m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {}
@@ -94,6 +99,7 @@
// either a different mode or from power-on.
if (m_lastMode != Mode::kDisabled) {
LiveWindow::GetInstance()->SetEnabled(false);
+ Shuffleboard::DisableActuatorWidgets();
DisabledInit();
m_watchdog.AddEpoch("DisabledInit()");
m_lastMode = Mode::kDisabled;
@@ -107,6 +113,7 @@
// either a different mode or from power-on.
if (m_lastMode != Mode::kAutonomous) {
LiveWindow::GetInstance()->SetEnabled(false);
+ Shuffleboard::DisableActuatorWidgets();
AutonomousInit();
m_watchdog.AddEpoch("AutonomousInit()");
m_lastMode = Mode::kAutonomous;
@@ -120,6 +127,7 @@
// either a different mode or from power-on.
if (m_lastMode != Mode::kTeleop) {
LiveWindow::GetInstance()->SetEnabled(false);
+ Shuffleboard::DisableActuatorWidgets();
TeleopInit();
m_watchdog.AddEpoch("TeleopInit()");
m_lastMode = Mode::kTeleop;
@@ -134,6 +142,7 @@
// either a different mode or from power-on.
if (m_lastMode != Mode::kTest) {
LiveWindow::GetInstance()->SetEnabled(true);
+ Shuffleboard::EnableActuatorWidgets();
TestInit();
m_watchdog.AddEpoch("TestInit()");
m_lastMode = Mode::kTest;
@@ -146,10 +155,14 @@
RobotPeriodic();
m_watchdog.AddEpoch("RobotPeriodic()");
- m_watchdog.Disable();
- SmartDashboard::UpdateValues();
+ SmartDashboard::UpdateValues();
+ m_watchdog.AddEpoch("SmartDashboard::UpdateValues()");
LiveWindow::GetInstance()->UpdateValues();
+ m_watchdog.AddEpoch("LiveWindow::UpdateValues()");
+ Shuffleboard::Update();
+ m_watchdog.AddEpoch("Shuffleboard::Update()");
+ m_watchdog.Disable();
// Warn on loop time overruns
if (m_watchdog.IsExpired()) {
@@ -161,7 +174,8 @@
wpi::SmallString<128> str;
wpi::raw_svector_ostream buf(str);
- buf << "Loop time of " << m_period << "s overrun\n";
+ buf << "Loop time of " << wpi::format("%.6f", m_period.to<double>())
+ << "s overrun\n";
DriverStation::ReportWarning(str);
}
diff --git a/wpilibc/src/main/native/cpp/Jaguar.cpp b/wpilibc/src/main/native/cpp/Jaguar.cpp
index 7bde6ba..487d489 100644
--- a/wpilibc/src/main/native/cpp/Jaguar.cpp
+++ b/wpilibc/src/main/native/cpp/Jaguar.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <hal/HAL.h>
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
Jaguar::Jaguar(int channel) : PWMSpeedController(channel) {
@@ -26,5 +28,5 @@
SetZeroLatch();
HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel());
- SetName("Jaguar", GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "Jaguar", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/Joystick.cpp b/wpilibc/src/main/native/cpp/Joystick.cpp
index da77d94..34afd0a 100644
--- a/wpilibc/src/main/native/cpp/Joystick.cpp
+++ b/wpilibc/src/main/native/cpp/Joystick.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,14 +10,13 @@
#include <cmath>
#include <hal/HAL.h>
+#include <wpi/math>
#include "frc/DriverStation.h"
#include "frc/WPIErrors.h"
using namespace frc;
-constexpr double kPi = 3.14159265358979323846;
-
Joystick::Joystick(int port) : GenericHID(port) {
m_axes[Axis::kX] = kDefaultXChannel;
m_axes[Axis::kY] = kDefaultYChannel;
@@ -40,10 +39,6 @@
m_axes[Axis::kThrottle] = channel;
}
-void Joystick::SetAxisChannel(AxisType axis, int channel) {
- m_axes[axis] = channel;
-}
-
int Joystick::GetXChannel() const { return m_axes[Axis::kX]; }
int Joystick::GetYChannel() const { return m_axes[Axis::kY]; }
@@ -70,24 +65,6 @@
return GetRawAxis(m_axes[Axis::kThrottle]);
}
-double Joystick::GetAxis(AxisType axis) const {
- switch (axis) {
- case kXAxis:
- return GetX();
- case kYAxis:
- return GetY();
- case kZAxis:
- return GetZ();
- case kTwistAxis:
- return GetTwist();
- case kThrottleAxis:
- return GetThrottle();
- default:
- wpi_setWPIError(BadJoystickAxis);
- return 0.0;
- }
-}
-
bool Joystick::GetTrigger() const { return GetRawButton(Button::kTrigger); }
bool Joystick::GetTriggerPressed() {
@@ -104,22 +81,6 @@
bool Joystick::GetTopReleased() { return GetRawButtonReleased(Button::kTop); }
-Joystick* Joystick::GetStickForPort(int port) {
- static std::array<std::unique_ptr<Joystick>, DriverStation::kJoystickPorts>
- joysticks{};
- auto stick = joysticks[port].get();
- if (stick == nullptr) {
- joysticks[port] = std::make_unique<Joystick>(port);
- stick = joysticks[port].get();
- }
- return stick;
-}
-
-bool Joystick::GetButton(ButtonType button) const {
- int temp = button;
- return GetRawButton(static_cast<Button>(temp));
-}
-
double Joystick::GetMagnitude() const {
return std::sqrt(std::pow(GetX(), 2) + std::pow(GetY(), 2));
}
@@ -129,5 +90,5 @@
}
double Joystick::GetDirectionDegrees() const {
- return (180 / kPi) * GetDirectionRadians();
+ return (180 / wpi::math::pi) * GetDirectionRadians();
}
diff --git a/wpilibc/src/main/native/cpp/JoystickBase.cpp b/wpilibc/src/main/native/cpp/JoystickBase.cpp
deleted file mode 100644
index 8e6858c..0000000
--- a/wpilibc/src/main/native/cpp/JoystickBase.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/JoystickBase.h"
-
-using namespace frc;
-
-JoystickBase::JoystickBase(int port) : GenericHID(port) {}
diff --git a/wpilibc/src/main/native/cpp/LinearFilter.cpp b/wpilibc/src/main/native/cpp/LinearFilter.cpp
new file mode 100644
index 0000000..95df127
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/LinearFilter.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/LinearFilter.h"
+
+#include <cassert>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+LinearFilter::LinearFilter(wpi::ArrayRef<double> ffGains,
+ wpi::ArrayRef<double> fbGains)
+ : m_inputs(ffGains.size()),
+ m_outputs(fbGains.size()),
+ m_inputGains(ffGains),
+ m_outputGains(fbGains) {
+ static int instances = 0;
+ instances++;
+ HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
+}
+
+LinearFilter LinearFilter::SinglePoleIIR(double timeConstant,
+ units::second_t period) {
+ double gain = std::exp(-period.to<double>() / timeConstant);
+ return LinearFilter(1.0 - gain, -gain);
+}
+
+LinearFilter LinearFilter::HighPass(double timeConstant,
+ units::second_t period) {
+ double gain = std::exp(-period.to<double>() / timeConstant);
+ return LinearFilter({gain, -gain}, {-gain});
+}
+
+LinearFilter LinearFilter::MovingAverage(int taps) {
+ assert(taps > 0);
+
+ std::vector<double> gains(taps, 1.0 / taps);
+ return LinearFilter(gains, {});
+}
+
+void LinearFilter::Reset() {
+ m_inputs.reset();
+ m_outputs.reset();
+}
+
+double LinearFilter::Calculate(double input) {
+ double retVal = 0.0;
+
+ // Rotate the inputs
+ m_inputs.push_front(input);
+
+ // Calculate the new value
+ for (size_t i = 0; i < m_inputGains.size(); i++) {
+ retVal += m_inputs[i] * m_inputGains[i];
+ }
+ for (size_t i = 0; i < m_outputGains.size(); i++) {
+ retVal -= m_outputs[i] * m_outputGains[i];
+ }
+
+ // Rotate the outputs
+ m_outputs.push_front(retVal);
+
+ return retVal;
+}
diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp
index aff85d8..1806e86 100644
--- a/wpilibc/src/main/native/cpp/MotorSafety.cpp
+++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -23,12 +23,12 @@
static wpi::mutex listMutex;
MotorSafety::MotorSafety() {
- std::lock_guard<wpi::mutex> lock(listMutex);
+ std::scoped_lock lock(listMutex);
instanceList.insert(this);
}
MotorSafety::~MotorSafety() {
- std::lock_guard<wpi::mutex> lock(listMutex);
+ std::scoped_lock lock(listMutex);
instanceList.erase(this);
}
@@ -39,6 +39,8 @@
m_stopTime(std::move(rhs.m_stopTime)) {}
MotorSafety& MotorSafety::operator=(MotorSafety&& rhs) {
+ std::scoped_lock lock(m_thisMutex, rhs.m_thisMutex);
+
ErrorBase::operator=(std::move(rhs));
m_expiration = std::move(rhs.m_expiration);
@@ -49,32 +51,32 @@
}
void MotorSafety::Feed() {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
}
void MotorSafety::SetExpiration(double expirationTime) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_expiration = expirationTime;
}
double MotorSafety::GetExpiration() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return m_expiration;
}
bool MotorSafety::IsAlive() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
}
void MotorSafety::SetSafetyEnabled(bool enabled) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_enabled = enabled;
}
bool MotorSafety::IsSafetyEnabled() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return m_enabled;
}
@@ -83,7 +85,7 @@
double stopTime;
{
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
enabled = m_enabled;
stopTime = m_stopTime;
}
@@ -104,7 +106,7 @@
}
void MotorSafety::CheckMotors() {
- std::lock_guard<wpi::mutex> lock(listMutex);
+ std::scoped_lock lock(listMutex);
for (auto elem : instanceList) {
elem->Check();
}
diff --git a/wpilibc/src/main/native/cpp/NidecBrushless.cpp b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
index 0ccc7c7..90ce43e 100644
--- a/wpilibc/src/main/native/cpp/NidecBrushless.cpp
+++ b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,13 +10,15 @@
#include <hal/HAL.h>
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
: m_dio(dioChannel), m_pwm(pwmChannel) {
- AddChild(&m_dio);
- AddChild(&m_pwm);
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, &m_dio);
+ registry.AddChild(this, &m_pwm);
SetExpiration(0.0);
SetSafetyEnabled(false);
@@ -25,7 +27,7 @@
m_dio.EnablePWM(0.5);
HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel);
- SetName("Nidec Brushless", pwmChannel);
+ registry.AddLW(this, "Nidec Brushless", pwmChannel);
}
void NidecBrushless::Set(double speed) {
diff --git a/wpilibc/src/main/native/cpp/Notifier.cpp b/wpilibc/src/main/native/cpp/Notifier.cpp
index 69a9f4e..a2f93f9 100644
--- a/wpilibc/src/main/native/cpp/Notifier.cpp
+++ b/wpilibc/src/main/native/cpp/Notifier.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -17,7 +17,7 @@
using namespace frc;
-Notifier::Notifier(TimerEventHandler handler) {
+Notifier::Notifier(std::function<void()> handler) {
if (handler == nullptr)
wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
m_handler = handler;
@@ -33,9 +33,9 @@
uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
if (curTime == 0 || status != 0) break;
- TimerEventHandler handler;
+ std::function<void()> handler;
{
- std::lock_guard<wpi::mutex> lock(m_processMutex);
+ std::scoped_lock lock(m_processMutex);
handler = m_handler;
if (m_periodic) {
m_expirationTime += m_period;
@@ -90,23 +90,31 @@
return *this;
}
-void Notifier::SetHandler(TimerEventHandler handler) {
- std::lock_guard<wpi::mutex> lock(m_processMutex);
+void Notifier::SetHandler(std::function<void()> handler) {
+ std::scoped_lock lock(m_processMutex);
m_handler = handler;
}
void Notifier::StartSingle(double delay) {
- std::lock_guard<wpi::mutex> lock(m_processMutex);
+ StartSingle(units::second_t(delay));
+}
+
+void Notifier::StartSingle(units::second_t delay) {
+ std::scoped_lock lock(m_processMutex);
m_periodic = false;
- m_period = delay;
+ m_period = delay.to<double>();
m_expirationTime = Timer::GetFPGATimestamp() + m_period;
UpdateAlarm();
}
void Notifier::StartPeriodic(double period) {
- std::lock_guard<wpi::mutex> lock(m_processMutex);
+ StartPeriodic(units::second_t(period));
+}
+
+void Notifier::StartPeriodic(units::second_t period) {
+ std::scoped_lock lock(m_processMutex);
m_periodic = true;
- m_period = period;
+ m_period = period.to<double>();
m_expirationTime = Timer::GetFPGATimestamp() + m_period;
UpdateAlarm();
}
diff --git a/wpilibc/src/main/native/cpp/PIDBase.cpp b/wpilibc/src/main/native/cpp/PIDBase.cpp
index 872a45a..a2efe0f 100644
--- a/wpilibc/src/main/native/cpp/PIDBase.cpp
+++ b/wpilibc/src/main/native/cpp/PIDBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,6 +14,7 @@
#include "frc/PIDOutput.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -27,19 +28,14 @@
: PIDBase(Kp, Ki, Kd, 0.0, source, output) {}
PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source,
- PIDOutput& output)
- : SendableBase(false) {
+ PIDOutput& output) {
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
- // Save original source
- m_origSource = std::shared_ptr<PIDSource>(&source, NullDeleter<PIDSource>());
-
- // Create LinearDigitalFilter with original source as its source argument
- m_filter = LinearDigitalFilter::MovingAverage(m_origSource, 1);
- m_pidInput = &m_filter;
+ m_pidInput = &source;
+ m_filter = LinearFilter::MovingAverage(1);
m_pidOutput = &output;
@@ -48,22 +44,22 @@
static int instances = 0;
instances++;
HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
- SetName("PIDController", instances);
+ SendableRegistry::GetInstance().Add(this, "PIDController", instances);
}
double PIDBase::Get() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return m_result;
}
void PIDBase::SetContinuous(bool continuous) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_continuous = continuous;
}
void PIDBase::SetInputRange(double minimumInput, double maximumInput) {
{
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_minimumInput = minimumInput;
m_maximumInput = maximumInput;
m_inputRange = maximumInput - minimumInput;
@@ -73,14 +69,14 @@
}
void PIDBase::SetOutputRange(double minimumOutput, double maximumOutput) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_minimumOutput = minimumOutput;
m_maximumOutput = maximumOutput;
}
void PIDBase::SetPID(double p, double i, double d) {
{
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_P = p;
m_I = i;
m_D = d;
@@ -88,7 +84,7 @@
}
void PIDBase::SetPID(double p, double i, double d, double f) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_P = p;
m_I = i;
m_D = d;
@@ -96,48 +92,48 @@
}
void PIDBase::SetP(double p) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_P = p;
}
void PIDBase::SetI(double i) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_I = i;
}
void PIDBase::SetD(double d) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_D = d;
}
void PIDBase::SetF(double f) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_F = f;
}
double PIDBase::GetP() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return m_P;
}
double PIDBase::GetI() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return m_I;
}
double PIDBase::GetD() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return m_D;
}
double PIDBase::GetF() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return m_F;
}
void PIDBase::SetSetpoint(double setpoint) {
{
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
if (m_maximumInput > m_minimumInput) {
if (setpoint > m_maximumInput)
@@ -153,19 +149,19 @@
}
double PIDBase::GetSetpoint() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return m_setpoint;
}
double PIDBase::GetDeltaSetpoint() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
}
double PIDBase::GetError() const {
double setpoint = GetSetpoint();
{
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return GetContinuousError(setpoint - m_pidInput->PIDGet());
}
}
@@ -181,35 +177,32 @@
}
void PIDBase::SetTolerance(double percent) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
void PIDBase::SetAbsoluteTolerance(double absTolerance) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_toleranceType = kAbsoluteTolerance;
m_tolerance = absTolerance;
}
void PIDBase::SetPercentTolerance(double percent) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_toleranceType = kPercentTolerance;
m_tolerance = percent;
}
void PIDBase::SetToleranceBuffer(int bufLength) {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
-
- // Create LinearDigitalFilter with original source as its source argument
- m_filter = LinearDigitalFilter::MovingAverage(m_origSource, bufLength);
- m_pidInput = &m_filter;
+ std::scoped_lock lock(m_thisMutex);
+ m_filter = LinearFilter::MovingAverage(bufLength);
}
bool PIDBase::OnTarget() const {
double error = GetError();
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
switch (m_toleranceType) {
case kPercentTolerance:
return std::fabs(error) < m_tolerance / 100 * m_inputRange;
@@ -225,7 +218,7 @@
}
void PIDBase::Reset() {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_prevError = 0;
m_totalError = 0;
m_result = 0;
@@ -249,11 +242,11 @@
}
void PIDBase::Calculate() {
- if (m_origSource == nullptr || m_pidOutput == nullptr) return;
+ if (m_pidInput == nullptr || m_pidOutput == nullptr) return;
bool enabled;
{
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
enabled = m_enabled;
}
@@ -275,9 +268,9 @@
double totalError;
{
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
- input = m_pidInput->PIDGet();
+ input = m_filter.Calculate(m_pidInput->PIDGet());
pidSourceType = m_pidInput->GetPIDSourceType();
P = m_P;
@@ -315,8 +308,8 @@
{
// Ensures m_enabled check and PIDWrite() call occur atomically
- std::lock_guard<wpi::mutex> pidWriteLock(m_pidWriteMutex);
- std::unique_lock<wpi::mutex> mainLock(m_thisMutex);
+ std::scoped_lock pidWriteLock(m_pidWriteMutex);
+ std::unique_lock mainLock(m_thisMutex);
if (m_enabled) {
// Don't block other PIDBase operations on PIDWrite()
mainLock.unlock();
@@ -325,7 +318,7 @@
}
}
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_prevError = m_error;
m_error = error;
m_totalError = totalError;
diff --git a/wpilibc/src/main/native/cpp/PIDController.cpp b/wpilibc/src/main/native/cpp/PIDController.cpp
index b1c51c6..0c86f55 100644
--- a/wpilibc/src/main/native/cpp/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/PIDController.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -31,7 +31,7 @@
double period)
: PIDBase(Kp, Ki, Kd, Kf, source, output) {
m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
- m_controlLoop->StartPeriodic(period);
+ m_controlLoop->StartPeriodic(units::second_t(period));
}
PIDController::~PIDController() {
@@ -41,7 +41,7 @@
void PIDController::Enable() {
{
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
m_enabled = true;
}
}
@@ -49,9 +49,9 @@
void PIDController::Disable() {
{
// Ensures m_enabled modification and PIDWrite() call occur atomically
- std::lock_guard<wpi::mutex> pidWriteLock(m_pidWriteMutex);
+ std::scoped_lock pidWriteLock(m_pidWriteMutex);
{
- std::lock_guard<wpi::mutex> mainLock(m_thisMutex);
+ std::scoped_lock mainLock(m_thisMutex);
m_enabled = false;
}
@@ -68,7 +68,7 @@
}
bool PIDController::IsEnabled() const {
- std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ std::scoped_lock lock(m_thisMutex);
return m_enabled;
}
diff --git a/wpilibc/src/main/native/cpp/PWM.cpp b/wpilibc/src/main/native/cpp/PWM.cpp
index 603c94b..c6b398f 100644
--- a/wpilibc/src/main/native/cpp/PWM.cpp
+++ b/wpilibc/src/main/native/cpp/PWM.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -17,6 +17,7 @@
#include "frc/Utility.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -46,7 +47,7 @@
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
HAL_Report(HALUsageReporting::kResourceType_PWM, channel);
- SetName("PWM", channel);
+ SendableRegistry::GetInstance().AddLW(this, "PWM", channel);
SetSafetyEnabled(false);
}
@@ -61,23 +62,6 @@
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
-PWM::PWM(PWM&& rhs)
- : MotorSafety(std::move(rhs)),
- SendableBase(std::move(rhs)),
- m_channel(std::move(rhs.m_channel)) {
- std::swap(m_handle, rhs.m_handle);
-}
-
-PWM& PWM::operator=(PWM&& rhs) {
- ErrorBase::operator=(std::move(rhs));
- SendableBase::operator=(std::move(rhs));
-
- m_channel = std::move(rhs.m_channel);
- std::swap(m_handle, rhs.m_handle);
-
- return *this;
-}
-
void PWM::StopMotor() { SetDisabled(); }
void PWM::GetDescription(wpi::raw_ostream& desc) const {
diff --git a/wpilibc/src/main/native/cpp/PWMSparkMax.cpp b/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
new file mode 100644
index 0000000..d8d3b2a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMSparkMax.h"
+
+#include <hal/HAL.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMSparkMax::PWMSparkMax(int channel) : PWMSpeedController(channel) {
+ /* Note that the SparkMax uses the following bounds for PWM values.
+ *
+ * 2.003ms = full "forward"
+ * 1.55ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.46ms = the "low end" of the deadband range
+ * 0.999ms = full "reverse"
+ */
+ SetBounds(2.003, 1.55, 1.50, 1.46, .999);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_RevSparkMaxPWM, GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "PWMSparkMax", GetChannel());
+}
diff --git a/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
index ccb50b6..0bbc5b9 100644
--- a/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
+++ b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <hal/HAL.h>
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
PWMTalonSRX::PWMTalonSRX(int channel) : PWMSpeedController(channel) {
@@ -30,5 +32,5 @@
SetZeroLatch();
HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel());
- SetName("PWMTalonSRX", GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "PWMTalonSRX", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp b/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
index b4f1fd2..122d219 100644
--- a/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
+++ b/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <hal/HAL.h>
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
PWMVictorSPX::PWMVictorSPX(int channel) : PWMSpeedController(channel) {
@@ -30,5 +32,5 @@
SetZeroLatch();
HAL_Report(HALUsageReporting::kResourceType_PWMVictorSPX, GetChannel());
- SetName("PWMVictorSPX", GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "PWMVictorSPX", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
index 8872028..b3297ea 100644
--- a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
+++ b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,6 +16,7 @@
#include "frc/SensorUtil.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -34,7 +35,7 @@
}
HAL_Report(HALUsageReporting::kResourceType_PDP, module);
- SetName("PowerDistributionPanel", module);
+ SendableRegistry::GetInstance().AddLW(this, "PowerDistributionPanel", module);
}
double PowerDistributionPanel::GetVoltage() const {
diff --git a/wpilibc/src/main/native/cpp/Relay.cpp b/wpilibc/src/main/native/cpp/Relay.cpp
index 9128d20..3a3e772 100644
--- a/wpilibc/src/main/native/cpp/Relay.cpp
+++ b/wpilibc/src/main/native/cpp/Relay.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -17,6 +17,7 @@
#include "frc/SensorUtil.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -76,7 +77,7 @@
}
}
- SetName("Relay", m_channel);
+ SendableRegistry::GetInstance().AddLW(this, "Relay", m_channel);
}
Relay::~Relay() {
@@ -88,27 +89,6 @@
if (m_reverseHandle != HAL_kInvalidHandle) HAL_FreeRelayPort(m_reverseHandle);
}
-Relay::Relay(Relay&& rhs)
- : MotorSafety(std::move(rhs)),
- SendableBase(std::move(rhs)),
- m_channel(std::move(rhs.m_channel)),
- m_direction(std::move(rhs.m_direction)) {
- std::swap(m_forwardHandle, rhs.m_forwardHandle);
- std::swap(m_reverseHandle, rhs.m_reverseHandle);
-}
-
-Relay& Relay::operator=(Relay&& rhs) {
- MotorSafety::operator=(std::move(rhs));
- SendableBase::operator=(std::move(rhs));
-
- m_channel = std::move(rhs.m_channel);
- m_direction = std::move(rhs.m_direction);
- std::swap(m_forwardHandle, rhs.m_forwardHandle);
- std::swap(m_reverseHandle, rhs.m_reverseHandle);
-
- return *this;
-}
-
void Relay::Set(Relay::Value value) {
if (StatusIsFatal()) return;
diff --git a/wpilibc/src/main/native/cpp/Resource.cpp b/wpilibc/src/main/native/cpp/Resource.cpp
index e2c53d4..d546461 100644
--- a/wpilibc/src/main/native/cpp/Resource.cpp
+++ b/wpilibc/src/main/native/cpp/Resource.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,7 +16,7 @@
void Resource::CreateResourceObject(std::unique_ptr<Resource>& r,
uint32_t elements) {
- std::lock_guard<wpi::mutex> lock(m_createMutex);
+ std::scoped_lock lock(m_createMutex);
if (!r) {
r = std::make_unique<Resource>(elements);
}
@@ -27,7 +27,7 @@
}
uint32_t Resource::Allocate(const std::string& resourceDesc) {
- std::lock_guard<wpi::mutex> lock(m_allocateMutex);
+ std::scoped_lock lock(m_allocateMutex);
for (uint32_t i = 0; i < m_isAllocated.size(); i++) {
if (!m_isAllocated[i]) {
m_isAllocated[i] = true;
@@ -39,7 +39,7 @@
}
uint32_t Resource::Allocate(uint32_t index, const std::string& resourceDesc) {
- std::lock_guard<wpi::mutex> lock(m_allocateMutex);
+ std::scoped_lock lock(m_allocateMutex);
if (index >= m_isAllocated.size()) {
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc);
return std::numeric_limits<uint32_t>::max();
@@ -53,7 +53,7 @@
}
void Resource::Free(uint32_t index) {
- std::unique_lock<wpi::mutex> lock(m_allocateMutex);
+ std::unique_lock lock(m_allocateMutex);
if (index == std::numeric_limits<uint32_t>::max()) return;
if (index >= m_isAllocated.size()) {
wpi_setWPIError(NotAllocated);
diff --git a/wpilibc/src/main/native/cpp/RobotDrive.cpp b/wpilibc/src/main/native/cpp/RobotDrive.cpp
index a20eb65..fd43c96 100644
--- a/wpilibc/src/main/native/cpp/RobotDrive.cpp
+++ b/wpilibc/src/main/native/cpp/RobotDrive.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -120,8 +120,8 @@
double leftOutput, rightOutput;
static bool reported = false;
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
- HALUsageReporting::kRobotDrive_ArcadeRatioCurve);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive_ArcadeRatioCurve, GetNumMotors());
reported = true;
}
@@ -180,8 +180,8 @@
bool squaredInputs) {
static bool reported = false;
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
- HALUsageReporting::kRobotDrive_Tank);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive_Tank, GetNumMotors());
reported = true;
}
@@ -230,8 +230,8 @@
bool squaredInputs) {
static bool reported = false;
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
- HALUsageReporting::kRobotDrive_ArcadeStandard);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive_ArcadeStandard, GetNumMotors());
reported = true;
}
@@ -273,8 +273,8 @@
double gyroAngle) {
static bool reported = false;
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
- HALUsageReporting::kRobotDrive_MecanumCartesian);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive_MecanumCartesian, GetNumMotors());
reported = true;
}
@@ -305,8 +305,8 @@
double rotation) {
static bool reported = false;
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
- HALUsageReporting::kRobotDrive_MecanumPolar);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive_MecanumPolar, GetNumMotors());
reported = true;
}
diff --git a/wpilibc/src/main/native/cpp/RobotState.cpp b/wpilibc/src/main/native/cpp/RobotState.cpp
index f5da5c1..530cee3 100644
--- a/wpilibc/src/main/native/cpp/RobotState.cpp
+++ b/wpilibc/src/main/native/cpp/RobotState.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -19,6 +19,10 @@
return DriverStation::GetInstance().IsEnabled();
}
+bool RobotState::IsEStopped() {
+ return DriverStation::GetInstance().IsEStopped();
+}
+
bool RobotState::IsOperatorControl() {
return DriverStation::GetInstance().IsOperatorControl();
}
diff --git a/wpilibc/src/main/native/cpp/SD540.cpp b/wpilibc/src/main/native/cpp/SD540.cpp
index 977ae7b..93733b2 100644
--- a/wpilibc/src/main/native/cpp/SD540.cpp
+++ b/wpilibc/src/main/native/cpp/SD540.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <hal/HAL.h>
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
SD540::SD540(int channel) : PWMSpeedController(channel) {
@@ -31,5 +33,5 @@
SetZeroLatch();
HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel());
- SetName("SD540", GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "SD540", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/SPI.cpp b/wpilibc/src/main/native/cpp/SPI.cpp
index 6b41adb..54cf82b 100644
--- a/wpilibc/src/main/native/cpp/SPI.cpp
+++ b/wpilibc/src/main/native/cpp/SPI.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -28,7 +28,7 @@
Accumulator(HAL_SPIPort port, int xferSize, int validMask, int validValue,
int dataShift, int dataSize, bool isSigned, bool bigEndian)
: m_notifier([=]() {
- std::lock_guard<wpi::mutex> lock(m_mutex);
+ std::scoped_lock lock(m_mutex);
Update();
}),
m_buf(new uint32_t[(xferSize + 1) * kAccumulateDepth]),
@@ -157,35 +157,12 @@
HAL_InitializeSPI(m_port, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- static int instances = 0;
- instances++;
- HAL_Report(HALUsageReporting::kResourceType_SPI, instances);
+ HAL_Report(HALUsageReporting::kResourceType_SPI, port);
}
SPI::~SPI() { HAL_CloseSPI(m_port); }
-SPI::SPI(SPI&& rhs)
- : ErrorBase(std::move(rhs)),
- m_msbFirst(std::move(rhs.m_msbFirst)),
- m_sampleOnTrailing(std::move(rhs.m_sampleOnTrailing)),
- m_clockIdleHigh(std::move(rhs.m_clockIdleHigh)),
- m_accum(std::move(rhs.m_accum)) {
- std::swap(m_port, rhs.m_port);
-}
-
-SPI& SPI::operator=(SPI&& rhs) {
- ErrorBase::operator=(std::move(rhs));
-
- std::swap(m_port, rhs.m_port);
- m_msbFirst = std::move(rhs.m_msbFirst);
- m_sampleOnTrailing = std::move(rhs.m_sampleOnTrailing);
- m_clockIdleHigh = std::move(rhs.m_clockIdleHigh);
- m_accum = std::move(rhs.m_accum);
-
- return *this;
-}
-
-void SPI::SetClockRate(double hz) { HAL_SetSPISpeed(m_port, hz); }
+void SPI::SetClockRate(int hz) { HAL_SetSPISpeed(m_port, hz); }
void SPI::SetMSBFirst() {
m_msbFirst = true;
@@ -282,12 +259,16 @@
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
-void SPI::StartAutoRate(double period) {
+void SPI::StartAutoRate(units::second_t period) {
int32_t status = 0;
- HAL_StartSPIAutoRate(m_port, period, &status);
+ HAL_StartSPIAutoRate(m_port, period.to<double>(), &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
+void SPI::StartAutoRate(double period) {
+ StartAutoRate(units::second_t(period));
+}
+
void SPI::StartAutoTrigger(DigitalSource& source, bool rising, bool falling) {
int32_t status = 0;
HAL_StartSPIAutoTrigger(
@@ -309,14 +290,19 @@
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
-int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout) {
+int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead,
+ units::second_t timeout) {
int32_t status = 0;
- int32_t val =
- HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead, timeout, &status);
+ int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead,
+ timeout.to<double>(), &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
return val;
}
+int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout) {
+ return ReadAutoReceivedData(buffer, numToRead, units::second_t(timeout));
+}
+
int SPI::GetAutoDroppedCount() {
int32_t status = 0;
int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
@@ -324,9 +310,9 @@
return val;
}
-void SPI::InitAccumulator(double period, int cmd, int xferSize, int validMask,
- int validValue, int dataShift, int dataSize,
- bool isSigned, bool bigEndian) {
+void SPI::InitAccumulator(units::second_t period, int cmd, int xferSize,
+ int validMask, int validValue, int dataShift,
+ int dataSize, bool isSigned, bool bigEndian) {
InitAuto(xferSize * kAccumulateDepth);
uint8_t cmdBytes[4] = {0, 0, 0, 0};
if (bigEndian) {
@@ -351,6 +337,13 @@
m_accum->m_notifier.StartPeriodic(period * kAccumulateDepth / 2);
}
+void SPI::InitAccumulator(double period, int cmd, int xferSize, int validMask,
+ int validValue, int dataShift, int dataSize,
+ bool isSigned, bool bigEndian) {
+ InitAccumulator(units::second_t(period), cmd, xferSize, validMask, validValue,
+ dataShift, dataSize, isSigned, bigEndian);
+}
+
void SPI::FreeAccumulator() {
m_accum.reset(nullptr);
FreeAuto();
@@ -358,7 +351,7 @@
void SPI::ResetAccumulator() {
if (!m_accum) return;
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->m_value = 0;
m_accum->m_count = 0;
m_accum->m_lastValue = 0;
@@ -368,40 +361,40 @@
void SPI::SetAccumulatorCenter(int center) {
if (!m_accum) return;
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->m_center = center;
}
void SPI::SetAccumulatorDeadband(int deadband) {
if (!m_accum) return;
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->m_deadband = deadband;
}
int SPI::GetAccumulatorLastValue() const {
if (!m_accum) return 0;
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
return m_accum->m_lastValue;
}
int64_t SPI::GetAccumulatorValue() const {
if (!m_accum) return 0;
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
return m_accum->m_value;
}
int64_t SPI::GetAccumulatorCount() const {
if (!m_accum) return 0;
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
return m_accum->m_count;
}
double SPI::GetAccumulatorAverage() const {
if (!m_accum) return 0;
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
if (m_accum->m_count == 0) return 0.0;
return static_cast<double>(m_accum->m_value) / m_accum->m_count;
@@ -413,7 +406,7 @@
count = 0;
return;
}
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
value = m_accum->m_value;
count = m_accum->m_count;
@@ -421,20 +414,20 @@
void SPI::SetAccumulatorIntegratedCenter(double center) {
if (!m_accum) return;
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->m_integratedCenter = center;
}
double SPI::GetAccumulatorIntegratedValue() const {
if (!m_accum) return 0;
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
return m_accum->m_integratedValue;
}
double SPI::GetAccumulatorIntegratedAverage() const {
if (!m_accum) return 0;
- std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
if (m_accum->m_count <= 1) return 0.0;
// count-1 due to not integrating the first value received
diff --git a/wpilibc/src/main/native/cpp/SampleRobot.cpp b/wpilibc/src/main/native/cpp/SampleRobot.cpp
deleted file mode 100644
index 190f5d8..0000000
--- a/wpilibc/src/main/native/cpp/SampleRobot.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/SampleRobot.h"
-
-#include <hal/DriverStation.h>
-#include <hal/FRCUsageReporting.h>
-#include <hal/HALBase.h>
-#include <networktables/NetworkTable.h>
-#include <wpi/raw_ostream.h>
-
-#include "frc/DriverStation.h"
-#include "frc/Timer.h"
-#include "frc/livewindow/LiveWindow.h"
-
-using namespace frc;
-
-void SampleRobot::StartCompetition() {
- LiveWindow* lw = LiveWindow::GetInstance();
-
- RobotInit();
-
- // Tell the DS that the robot is ready to be enabled
- HAL_ObserveUserProgramStarting();
-
- RobotMain();
-
- if (!m_robotMainOverridden) {
- while (true) {
- if (IsDisabled()) {
- m_ds.InDisabled(true);
- Disabled();
- m_ds.InDisabled(false);
- while (IsDisabled()) m_ds.WaitForData();
- } else if (IsAutonomous()) {
- m_ds.InAutonomous(true);
- Autonomous();
- m_ds.InAutonomous(false);
- while (IsAutonomous() && IsEnabled()) m_ds.WaitForData();
- } else if (IsTest()) {
- lw->SetEnabled(true);
- m_ds.InTest(true);
- Test();
- m_ds.InTest(false);
- while (IsTest() && IsEnabled()) m_ds.WaitForData();
- lw->SetEnabled(false);
- } else {
- m_ds.InOperatorControl(true);
- OperatorControl();
- m_ds.InOperatorControl(false);
- while (IsOperatorControl() && IsEnabled()) m_ds.WaitForData();
- }
- }
- }
-}
-
-void SampleRobot::RobotInit() {
- wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
-
-void SampleRobot::Disabled() {
- wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
-
-void SampleRobot::Autonomous() {
- wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
-
-void SampleRobot::OperatorControl() {
- wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
-
-void SampleRobot::Test() {
- wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
-
-void SampleRobot::RobotMain() { m_robotMainOverridden = false; }
-
-SampleRobot::SampleRobot() {
- HAL_Report(HALUsageReporting::kResourceType_Framework,
- HALUsageReporting::kFramework_Simple);
-}
diff --git a/wpilibc/src/main/native/cpp/SerialPort.cpp b/wpilibc/src/main/native/cpp/SerialPort.cpp
index a399f4d..46e02c8 100644
--- a/wpilibc/src/main/native/cpp/SerialPort.cpp
+++ b/wpilibc/src/main/native/cpp/SerialPort.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,9 +12,6 @@
#include <hal/HAL.h>
#include <hal/SerialPort.h>
-// static ViStatus _VI_FUNCH ioCompleteHandler (ViSession vi, ViEventType
-// eventType, ViEvent event, ViAddr userHandle);
-
using namespace frc;
SerialPort::SerialPort(int baudRate, Port port, int dataBits,
@@ -22,19 +19,18 @@
SerialPort::StopBits stopBits) {
int32_t status = 0;
- m_port = port;
-
- HAL_InitializeSerialPort(static_cast<HAL_SerialPort>(port), &status);
+ m_portHandle =
+ HAL_InitializeSerialPort(static_cast<HAL_SerialPort>(port), &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
// Don't continue if initialization failed
if (status < 0) return;
- HAL_SetSerialBaudRate(static_cast<HAL_SerialPort>(port), baudRate, &status);
+ HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- HAL_SetSerialDataBits(static_cast<HAL_SerialPort>(port), dataBits, &status);
+ HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- HAL_SetSerialParity(static_cast<HAL_SerialPort>(port), parity, &status);
+ HAL_SetSerialParity(m_portHandle, parity, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- HAL_SetSerialStopBits(static_cast<HAL_SerialPort>(port), stopBits, &status);
+ HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
// Set the default timeout to 5 seconds.
@@ -43,11 +39,7 @@
// Don't wait until the buffer is full to transmit.
SetWriteBufferMode(kFlushOnAccess);
- EnableTermination();
-
- // viInstallHandler(m_portHandle, VI_EVENT_IO_COMPLETION, ioCompleteHandler,
- // this);
- // viEnableEvent(m_portHandle, VI_EVENT_IO_COMPLETION, VI_HNDLR, VI_NULL);
+ DisableTermination();
HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
}
@@ -57,23 +49,21 @@
SerialPort::StopBits stopBits) {
int32_t status = 0;
- m_port = port;
-
wpi::SmallVector<char, 64> buf;
const char* portNameC = portName.toNullTerminatedStringRef(buf).data();
- HAL_InitializeSerialPortDirect(static_cast<HAL_SerialPort>(port), portNameC,
- &status);
+ m_portHandle = HAL_InitializeSerialPortDirect(
+ static_cast<HAL_SerialPort>(port), portNameC, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
// Don't continue if initialization failed
if (status < 0) return;
- HAL_SetSerialBaudRate(static_cast<HAL_SerialPort>(port), baudRate, &status);
+ HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- HAL_SetSerialDataBits(static_cast<HAL_SerialPort>(port), dataBits, &status);
+ HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- HAL_SetSerialParity(static_cast<HAL_SerialPort>(port), parity, &status);
+ HAL_SetSerialParity(m_portHandle, parity, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- HAL_SetSerialStopBits(static_cast<HAL_SerialPort>(port), stopBits, &status);
+ HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
// Set the default timeout to 5 seconds.
@@ -82,72 +72,45 @@
// Don't wait until the buffer is full to transmit.
SetWriteBufferMode(kFlushOnAccess);
- EnableTermination();
-
- // viInstallHandler(m_portHandle, VI_EVENT_IO_COMPLETION, ioCompleteHandler,
- // this);
- // viEnableEvent(m_portHandle, VI_EVENT_IO_COMPLETION, VI_HNDLR, VI_NULL);
+ DisableTermination();
HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
}
SerialPort::~SerialPort() {
int32_t status = 0;
- HAL_CloseSerial(static_cast<HAL_SerialPort>(m_port), &status);
+ HAL_CloseSerial(m_portHandle, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
-SerialPort::SerialPort(SerialPort&& rhs)
- : ErrorBase(std::move(rhs)),
- m_resourceManagerHandle(std::move(rhs.m_resourceManagerHandle)),
- m_portHandle(std::move(rhs.m_portHandle)),
- m_consoleModeEnabled(std::move(rhs.m_consoleModeEnabled)) {
- std::swap(m_port, rhs.m_port);
-}
-
-SerialPort& SerialPort::operator=(SerialPort&& rhs) {
- ErrorBase::operator=(std::move(rhs));
-
- m_resourceManagerHandle = std::move(rhs.m_resourceManagerHandle);
- m_portHandle = std::move(rhs.m_portHandle);
- m_consoleModeEnabled = std::move(rhs.m_consoleModeEnabled);
- std::swap(m_port, rhs.m_port);
-
- return *this;
-}
-
void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) {
int32_t status = 0;
- HAL_SetSerialFlowControl(static_cast<HAL_SerialPort>(m_port), flowControl,
- &status);
+ HAL_SetSerialFlowControl(m_portHandle, flowControl, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
void SerialPort::EnableTermination(char terminator) {
int32_t status = 0;
- HAL_EnableSerialTermination(static_cast<HAL_SerialPort>(m_port), terminator,
- &status);
+ HAL_EnableSerialTermination(m_portHandle, terminator, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
void SerialPort::DisableTermination() {
int32_t status = 0;
- HAL_DisableSerialTermination(static_cast<HAL_SerialPort>(m_port), &status);
+ HAL_DisableSerialTermination(m_portHandle, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
int SerialPort::GetBytesReceived() {
int32_t status = 0;
- int retVal =
- HAL_GetSerialBytesReceived(static_cast<HAL_SerialPort>(m_port), &status);
+ int retVal = HAL_GetSerialBytesReceived(m_portHandle, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
return retVal;
}
int SerialPort::Read(char* buffer, int count) {
int32_t status = 0;
- int retVal = HAL_ReadSerial(static_cast<HAL_SerialPort>(m_port), buffer,
- count, &status);
+ int retVal = HAL_ReadSerial(m_portHandle, buffer, count, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
return retVal;
}
@@ -158,46 +121,44 @@
int SerialPort::Write(wpi::StringRef buffer) {
int32_t status = 0;
- int retVal = HAL_WriteSerial(static_cast<HAL_SerialPort>(m_port),
- buffer.data(), buffer.size(), &status);
+ int retVal =
+ HAL_WriteSerial(m_portHandle, buffer.data(), buffer.size(), &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
return retVal;
}
void SerialPort::SetTimeout(double timeout) {
int32_t status = 0;
- HAL_SetSerialTimeout(static_cast<HAL_SerialPort>(m_port), timeout, &status);
+ HAL_SetSerialTimeout(m_portHandle, timeout, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
void SerialPort::SetReadBufferSize(int size) {
int32_t status = 0;
- HAL_SetSerialReadBufferSize(static_cast<HAL_SerialPort>(m_port), size,
- &status);
+ HAL_SetSerialReadBufferSize(m_portHandle, size, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
void SerialPort::SetWriteBufferSize(int size) {
int32_t status = 0;
- HAL_SetSerialWriteBufferSize(static_cast<HAL_SerialPort>(m_port), size,
- &status);
+ HAL_SetSerialWriteBufferSize(m_portHandle, size, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode) {
int32_t status = 0;
- HAL_SetSerialWriteMode(static_cast<HAL_SerialPort>(m_port), mode, &status);
+ HAL_SetSerialWriteMode(m_portHandle, mode, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
void SerialPort::Flush() {
int32_t status = 0;
- HAL_FlushSerial(static_cast<HAL_SerialPort>(m_port), &status);
+ HAL_FlushSerial(m_portHandle, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
void SerialPort::Reset() {
int32_t status = 0;
- HAL_ClearSerial(static_cast<HAL_SerialPort>(m_port), &status);
+ HAL_ClearSerial(m_portHandle, &status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
}
diff --git a/wpilibc/src/main/native/cpp/Servo.cpp b/wpilibc/src/main/native/cpp/Servo.cpp
index b4c9eb5..09e482b 100644
--- a/wpilibc/src/main/native/cpp/Servo.cpp
+++ b/wpilibc/src/main/native/cpp/Servo.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,6 +10,7 @@
#include <hal/HAL.h>
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -27,7 +28,7 @@
SetPeriodMultiplier(kPeriodMultiplier_4X);
HAL_Report(HALUsageReporting::kResourceType_Servo, channel);
- SetName("Servo", channel);
+ SendableRegistry::GetInstance().SetName(this, "Servo", channel);
}
void Servo::Set(double value) { SetPosition(value); }
diff --git a/wpilibc/src/main/native/cpp/Solenoid.cpp b/wpilibc/src/main/native/cpp/Solenoid.cpp
index 3445f5d..c860c44 100644
--- a/wpilibc/src/main/native/cpp/Solenoid.cpp
+++ b/wpilibc/src/main/native/cpp/Solenoid.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,6 +16,7 @@
#include "frc/SensorUtil.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -47,25 +48,12 @@
HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel,
m_moduleNumber);
- SetName("Solenoid", m_moduleNumber, m_channel);
+ SendableRegistry::GetInstance().AddLW(this, "Solenoid", m_moduleNumber,
+ m_channel);
}
Solenoid::~Solenoid() { HAL_FreeSolenoidPort(m_solenoidHandle); }
-Solenoid::Solenoid(Solenoid&& rhs)
- : SolenoidBase(std::move(rhs)), m_channel(std::move(rhs.m_channel)) {
- std::swap(m_solenoidHandle, rhs.m_solenoidHandle);
-}
-
-Solenoid& Solenoid::operator=(Solenoid&& rhs) {
- SolenoidBase::operator=(std::move(rhs));
-
- std::swap(m_solenoidHandle, rhs.m_solenoidHandle);
- m_channel = std::move(rhs.m_channel);
-
- return *this;
-}
-
void Solenoid::Set(bool on) {
if (StatusIsFatal()) return;
int32_t status = 0;
diff --git a/wpilibc/src/main/native/cpp/Spark.cpp b/wpilibc/src/main/native/cpp/Spark.cpp
index fcfcb96..18f8ee6 100644
--- a/wpilibc/src/main/native/cpp/Spark.cpp
+++ b/wpilibc/src/main/native/cpp/Spark.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <hal/HAL.h>
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
Spark::Spark(int channel) : PWMSpeedController(channel) {
@@ -31,5 +33,5 @@
SetZeroLatch();
HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel());
- SetName("Spark", GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "Spark", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/Talon.cpp b/wpilibc/src/main/native/cpp/Talon.cpp
index 34f659a..8dc1928 100644
--- a/wpilibc/src/main/native/cpp/Talon.cpp
+++ b/wpilibc/src/main/native/cpp/Talon.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <hal/HAL.h>
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
Talon::Talon(int channel) : PWMSpeedController(channel) {
@@ -31,5 +33,5 @@
SetZeroLatch();
HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel());
- SetName("Talon", GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "Talon", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/Threads.cpp b/wpilibc/src/main/native/cpp/Threads.cpp
index 7713f66..798e86a 100644
--- a/wpilibc/src/main/native/cpp/Threads.cpp
+++ b/wpilibc/src/main/native/cpp/Threads.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,7 +12,7 @@
#include "frc/ErrorBase.h"
-using namespace frc;
+namespace frc {
int GetThreadPriority(std::thread& thread, bool* isRealTime) {
int32_t status = 0;
@@ -47,3 +47,5 @@
wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
return ret;
}
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp
index 24ab668..ffff2dd 100644
--- a/wpilibc/src/main/native/cpp/TimedRobot.cpp
+++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -25,7 +25,7 @@
// Tell the DS that the robot is ready to be enabled
HAL_ObserveUserProgramStarting();
- m_expirationTime = Timer::GetFPGATimestamp() + m_period;
+ m_expirationTime = units::second_t{Timer::GetFPGATimestamp()} + m_period;
UpdateAlarm();
// Loop forever, calling the appropriate mode-dependent function
@@ -43,9 +43,13 @@
}
}
-double TimedRobot::GetPeriod() const { return m_period; }
+units::second_t TimedRobot::GetPeriod() const {
+ return units::second_t(m_period);
+}
-TimedRobot::TimedRobot(double period) : IterativeRobotBase(period) {
+TimedRobot::TimedRobot(double period) : TimedRobot(units::second_t(period)) {}
+
+TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
@@ -63,22 +67,6 @@
HAL_CleanNotifier(m_notifier, &status);
}
-TimedRobot::TimedRobot(TimedRobot&& rhs)
- : IterativeRobotBase(std::move(rhs)),
- m_expirationTime(std::move(rhs.m_expirationTime)) {
- std::swap(m_notifier, rhs.m_notifier);
-}
-
-TimedRobot& TimedRobot::operator=(TimedRobot&& rhs) {
- IterativeRobotBase::operator=(std::move(rhs));
- ErrorBase::operator=(std::move(rhs));
-
- std::swap(m_notifier, rhs.m_notifier);
- m_expirationTime = std::move(rhs.m_expirationTime);
-
- return *this;
-}
-
void TimedRobot::UpdateAlarm() {
int32_t status = 0;
HAL_UpdateNotifierAlarm(
diff --git a/wpilibc/src/main/native/cpp/Timer.cpp b/wpilibc/src/main/native/cpp/Timer.cpp
index ae4a66e..cdfa9ab 100644
--- a/wpilibc/src/main/native/cpp/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/Timer.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -17,91 +17,34 @@
namespace frc {
-void Wait(double seconds) {
- std::this_thread::sleep_for(std::chrono::duration<double>(seconds));
-}
+void Wait(double seconds) { frc2::Wait(units::second_t(seconds)); }
-double GetClock() { return Timer::GetFPGATimestamp(); }
-
-double GetTime() {
- using std::chrono::duration;
- using std::chrono::duration_cast;
- using std::chrono::system_clock;
-
- return duration_cast<duration<double>>(system_clock::now().time_since_epoch())
- .count();
-}
+double GetTime() { return frc2::GetTime().to<double>(); }
} // namespace frc
using namespace frc;
-// for compatibility with msvc12--see C2864
-const double Timer::kRolloverTime = (1ll << 32) / 1e6;
+const double Timer::kRolloverTime = frc2::Timer::kRolloverTime.to<double>();
Timer::Timer() { Reset(); }
-double Timer::Get() const {
- double result;
- double currentTime = GetFPGATimestamp();
+double Timer::Get() const { return m_timer.Get().to<double>(); }
- std::lock_guard<wpi::mutex> lock(m_mutex);
- if (m_running) {
- // If the current time is before the start time, then the FPGA clock rolled
- // over. Compensate by adding the ~71 minutes that it takes to roll over to
- // the current time.
- if (currentTime < m_startTime) {
- currentTime += kRolloverTime;
- }
+void Timer::Reset() { m_timer.Reset(); }
- result = (currentTime - m_startTime) + m_accumulatedTime;
- } else {
- result = m_accumulatedTime;
- }
+void Timer::Start() { m_timer.Start(); }
- return result;
-}
-
-void Timer::Reset() {
- std::lock_guard<wpi::mutex> lock(m_mutex);
- m_accumulatedTime = 0;
- m_startTime = GetFPGATimestamp();
-}
-
-void Timer::Start() {
- std::lock_guard<wpi::mutex> lock(m_mutex);
- if (!m_running) {
- m_startTime = GetFPGATimestamp();
- m_running = true;
- }
-}
-
-void Timer::Stop() {
- double temp = Get();
-
- std::lock_guard<wpi::mutex> lock(m_mutex);
- if (m_running) {
- m_accumulatedTime = temp;
- m_running = false;
- }
-}
+void Timer::Stop() { m_timer.Stop(); }
bool Timer::HasPeriodPassed(double period) {
- if (Get() > period) {
- std::lock_guard<wpi::mutex> lock(m_mutex);
- // Advance the start time by the period.
- m_startTime += period;
- // Don't set it to the current time... we want to avoid drift.
- return true;
- }
- return false;
+ return m_timer.HasPeriodPassed(units::second_t(period));
}
double Timer::GetFPGATimestamp() {
- // FPGA returns the timestamp in microseconds
- return RobotController::GetFPGATime() * 1.0e-6;
+ return frc2::Timer::GetFPGATimestamp().to<double>();
}
double Timer::GetMatchTime() {
- return DriverStation::GetInstance().GetMatchTime();
+ return frc2::Timer::GetMatchTime().to<double>();
}
diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
index ee4f5cc..35320de 100644
--- a/wpilibc/src/main/native/cpp/Ultrasonic.cpp
+++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,6 +16,7 @@
#include "frc/Utility.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -31,8 +32,9 @@
m_counter(m_echoChannel) {
m_units = units;
Initialize();
- AddChild(m_pingChannel);
- AddChild(m_echoChannel);
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, m_pingChannel.get());
+ registry.AddChild(this, m_echoChannel.get());
}
Ultrasonic::Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel,
@@ -96,7 +98,10 @@
m_pingChannel->Pulse(kPingTime);
}
-bool Ultrasonic::IsRangeValid() const { return m_counter.Get() > 1; }
+bool Ultrasonic::IsRangeValid() const {
+ if (m_simRangeValid) return m_simRangeValid.Get();
+ return m_counter.Get() > 1;
+}
void Ultrasonic::SetAutomaticMode(bool enabling) {
if (enabling == m_automaticEnabled) return; // ignore the case of no change
@@ -133,10 +138,12 @@
}
double Ultrasonic::GetRangeInches() const {
- if (IsRangeValid())
+ if (IsRangeValid()) {
+ if (m_simRange) return m_simRange.Get();
return m_counter.GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
- else
+ } else {
return 0;
+ }
}
double Ultrasonic::GetRangeMM() const { return GetRangeInches() * 25.4; }
@@ -175,6 +182,14 @@
}
void Ultrasonic::Initialize() {
+ m_simDevice = hal::SimDevice("Ultrasonic", m_echoChannel->GetChannel());
+ if (m_simDevice) {
+ m_simRangeValid = m_simDevice.CreateBoolean("Range Valid", false, true);
+ m_simRange = m_simDevice.CreateDouble("Range (in)", false, 0.0);
+ m_pingChannel->SetSimDevice(m_simDevice);
+ m_echoChannel->SetSimDevice(m_simDevice);
+ }
+
bool originalMode = m_automaticEnabled;
SetAutomaticMode(false); // Kill task when adding a new sensor
// Link this instance on the list
@@ -189,7 +204,8 @@
static int instances = 0;
instances++;
HAL_Report(HALUsageReporting::kResourceType_Ultrasonic, instances);
- SetName("Ultrasonic", m_echoChannel->GetChannel());
+ SendableRegistry::GetInstance().AddLW(this, "Ultrasonic",
+ m_echoChannel->GetChannel());
}
void Ultrasonic::UltrasonicChecker() {
diff --git a/wpilibc/src/main/native/cpp/Utility.cpp b/wpilibc/src/main/native/cpp/Utility.cpp
index 503b6d0..499f9b4 100644
--- a/wpilibc/src/main/native/cpp/Utility.cpp
+++ b/wpilibc/src/main/native/cpp/Utility.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -20,6 +20,7 @@
#include <hal/HAL.h>
#include <wpi/Path.h>
#include <wpi/SmallString.h>
+#include <wpi/StackTrace.h>
#include <wpi/raw_ostream.h>
#include "frc/ErrorBase.h"
@@ -47,7 +48,7 @@
errorStream << "failed: " << message << "\n";
}
- std::string stack = GetStackTrace(2);
+ std::string stack = wpi::GetStackTrace(2);
// Print the error and send it to the DriverStation
HAL_SendError(1, 1, 0, errorBuf.c_str(), locBuf.c_str(), stack.c_str(), 1);
@@ -86,7 +87,7 @@
errorStream << "failed: " << message << "\n";
}
- std::string trace = GetStackTrace(3);
+ std::string trace = wpi::GetStackTrace(3);
// Print the error and send it to the DriverStation
HAL_SendError(1, 1, 0, errorBuf.c_str(), locBuf.c_str(), trace.c_str(), 1);
@@ -115,89 +116,3 @@
}
return valueA != valueB;
}
-
-namespace frc {
-
-int GetFPGAVersion() {
- int32_t status = 0;
- int version = HAL_GetFPGAVersion(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return version;
-}
-
-int64_t GetFPGARevision() {
- int32_t status = 0;
- int64_t revision = HAL_GetFPGARevision(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return revision;
-}
-
-uint64_t GetFPGATime() {
- int32_t status = 0;
- uint64_t time = HAL_GetFPGATime(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
- return time;
-}
-
-bool GetUserButton() {
- int32_t status = 0;
-
- bool value = HAL_GetFPGAButton(&status);
- wpi_setGlobalError(status);
-
- return value;
-}
-
-#ifndef _WIN32
-
-/**
- * Demangle a C++ symbol, used for printing stack traces.
- */
-static std::string demangle(char const* mangledSymbol) {
- char buffer[256];
- size_t length;
- int32_t status;
-
- if (std::sscanf(mangledSymbol, "%*[^(]%*[(]%255[^)+]", buffer)) {
- char* symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status);
- if (status == 0) {
- return symbol;
- } else {
- // If the symbol couldn't be demangled, it's probably a C function,
- // so just return it as-is.
- return buffer;
- }
- }
-
- // If everything else failed, just return the mangled symbol
- return mangledSymbol;
-}
-
-std::string GetStackTrace(int offset) {
- void* stackTrace[128];
- int stackSize = backtrace(stackTrace, 128);
- char** mangledSymbols = backtrace_symbols(stackTrace, stackSize);
- wpi::SmallString<1024> buf;
- wpi::raw_svector_ostream trace(buf);
-
- for (int i = offset; i < stackSize; i++) {
- // Only print recursive functions once in a row.
- if (i == 0 || stackTrace[i] != stackTrace[i - 1]) {
- trace << "\tat " << demangle(mangledSymbols[i]) << "\n";
- }
- }
-
- std::free(mangledSymbols);
-
- return trace.str();
-}
-
-#else
-static std::string demangle(char const* mangledSymbol) {
- return "no demangling on windows";
-}
-
-std::string GetStackTrace(int offset) { return "no stack trace on windows"; }
-#endif
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/cpp/Victor.cpp b/wpilibc/src/main/native/cpp/Victor.cpp
index 2c29ece..49dcd57 100644
--- a/wpilibc/src/main/native/cpp/Victor.cpp
+++ b/wpilibc/src/main/native/cpp/Victor.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <hal/HAL.h>
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
Victor::Victor(int channel) : PWMSpeedController(channel) {
@@ -32,5 +34,5 @@
SetZeroLatch();
HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel());
- SetName("Victor", GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "Victor", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/VictorSP.cpp b/wpilibc/src/main/native/cpp/VictorSP.cpp
index 5e2b6b9..82e966f 100644
--- a/wpilibc/src/main/native/cpp/VictorSP.cpp
+++ b/wpilibc/src/main/native/cpp/VictorSP.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <hal/HAL.h>
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
VictorSP::VictorSP(int channel) : PWMSpeedController(channel) {
@@ -31,5 +33,5 @@
SetZeroLatch();
HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel());
- SetName("VictorSP", GetChannel());
+ SendableRegistry::GetInstance().SetName(this, "VictorSP", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/Watchdog.cpp b/wpilibc/src/main/native/cpp/Watchdog.cpp
index f7c1778..e5be4fa 100644
--- a/wpilibc/src/main/native/cpp/Watchdog.cpp
+++ b/wpilibc/src/main/native/cpp/Watchdog.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -32,7 +32,7 @@
};
void Watchdog::Thread::Main() {
- std::unique_lock<wpi::mutex> lock(m_mutex);
+ std::unique_lock lock(m_mutex);
while (m_active) {
if (m_watchdogs.size() > 0) {
@@ -54,7 +54,7 @@
if (!watchdog->m_suppressTimeoutMessage) {
wpi::outs() << "Watchdog not fed within "
<< wpi::format("%.6f",
- watchdog->m_timeout.count() / 1.0e6)
+ watchdog->m_timeout.count() / 1.0e9)
<< "s\n";
}
}
@@ -78,9 +78,10 @@
}
Watchdog::Watchdog(double timeout, std::function<void()> callback)
- : m_timeout(static_cast<int64_t>(timeout * 1.0e6)),
- m_callback(callback),
- m_owner(&GetThreadOwner()) {}
+ : Watchdog(units::second_t{timeout}, callback) {}
+
+Watchdog::Watchdog(units::second_t timeout, std::function<void()> callback)
+ : m_timeout(timeout), m_callback(callback), m_owner(&GetThreadOwner()) {}
Watchdog::~Watchdog() { Disable(); }
@@ -89,6 +90,13 @@
}
void Watchdog::SetTimeout(double timeout) {
+ SetTimeout(units::second_t{timeout});
+}
+
+void Watchdog::SetTimeout(units::second_t timeout) {
+ using std::chrono::duration_cast;
+ using std::chrono::microseconds;
+
m_startTime = hal::fpga_clock::now();
m_epochs.clear();
@@ -96,11 +104,11 @@
auto thr = m_owner->GetThread();
if (!thr) return;
- m_timeout = std::chrono::microseconds(static_cast<int64_t>(timeout * 1.0e6));
+ m_timeout = timeout;
m_isExpired = false;
thr->m_watchdogs.remove(this);
- m_expirationTime = m_startTime + m_timeout;
+ m_expirationTime = m_startTime + duration_cast<microseconds>(m_timeout);
thr->m_watchdogs.emplace(this);
thr->m_cond.notify_all();
}
@@ -109,7 +117,7 @@
// Locks mutex
auto thr = m_owner->GetThread();
- return m_timeout.count() / 1.0e6;
+ return m_timeout.count() / 1.0e9;
}
bool Watchdog::IsExpired() const {
@@ -140,6 +148,9 @@
void Watchdog::Reset() { Enable(); }
void Watchdog::Enable() {
+ using std::chrono::duration_cast;
+ using std::chrono::microseconds;
+
m_startTime = hal::fpga_clock::now();
m_epochs.clear();
@@ -150,7 +161,7 @@
m_isExpired = false;
thr->m_watchdogs.remove(this);
- m_expirationTime = m_startTime + m_timeout;
+ m_expirationTime = m_startTime + duration_cast<microseconds>(m_timeout);
thr->m_watchdogs.emplace(this);
thr->m_cond.notify_all();
}
diff --git a/wpilibc/src/main/native/cpp/buttons/Trigger.cpp b/wpilibc/src/main/native/cpp/buttons/Trigger.cpp
index 2ccaaf2..f215083 100644
--- a/wpilibc/src/main/native/cpp/buttons/Trigger.cpp
+++ b/wpilibc/src/main/native/cpp/buttons/Trigger.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -15,6 +15,27 @@
using namespace frc;
+Trigger::Trigger(const Trigger& rhs) : SendableHelper(rhs) {}
+
+Trigger& Trigger::operator=(const Trigger& rhs) {
+ SendableHelper::operator=(rhs);
+ m_sendablePressed = false;
+ return *this;
+}
+
+Trigger::Trigger(Trigger&& rhs)
+ : SendableHelper(std::move(rhs)),
+ m_sendablePressed(rhs.m_sendablePressed.load()) {
+ rhs.m_sendablePressed = false;
+}
+
+Trigger& Trigger::operator=(Trigger&& rhs) {
+ SendableHelper::operator=(std::move(rhs));
+ m_sendablePressed = rhs.m_sendablePressed.load();
+ rhs.m_sendablePressed = false;
+ return *this;
+}
+
bool Trigger::Grab() { return Get() || m_sendablePressed; }
void Trigger::WhenActive(Command* command) {
diff --git a/wpilibc/src/main/native/cpp/commands/Command.cpp b/wpilibc/src/main/native/cpp/commands/Command.cpp
index ad7824a..a7154c0 100644
--- a/wpilibc/src/main/native/cpp/commands/Command.cpp
+++ b/wpilibc/src/main/native/cpp/commands/Command.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,6 +16,7 @@
#include "frc/commands/Scheduler.h"
#include "frc/livewindow/LiveWindow.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -31,7 +32,7 @@
Requires(&subsystem);
}
-Command::Command(const wpi::Twine& name, double timeout) : SendableBase(false) {
+Command::Command(const wpi::Twine& name, double timeout) {
// We use -1.0 to indicate no timeout.
if (timeout < 0.0 && timeout != -1.0)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
@@ -41,9 +42,10 @@
// If name contains an empty string
if (name.isTriviallyEmpty() ||
(name.isSingleStringRef() && name.getSingleStringRef().empty())) {
- SetName("Command_" + wpi::Twine(typeid(*this).name()));
+ SendableRegistry::GetInstance().Add(
+ this, "Command_" + wpi::Twine(typeid(*this).name()));
} else {
- SetName(name);
+ SendableRegistry::GetInstance().Add(this, name);
}
}
@@ -228,9 +230,27 @@
void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
+std::string Command::GetName() const {
+ return SendableRegistry::GetInstance().GetName(this);
+}
+
+void Command::SetName(const wpi::Twine& name) {
+ SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string Command::GetSubsystem() const {
+ return SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void Command::SetSubsystem(const wpi::Twine& name) {
+ SendableRegistry::GetInstance().SetSubsystem(this, name);
+}
+
void Command::InitSendable(SendableBuilder& builder) {
builder.SetSmartDashboardType("Command");
- builder.AddStringProperty(".name", [=]() { return GetName(); }, nullptr);
+ builder.AddStringProperty(
+ ".name", [=]() { return SendableRegistry::GetInstance().GetName(this); },
+ nullptr);
builder.AddBooleanProperty("running", [=]() { return IsRunning(); },
[=](bool value) {
if (value) {
diff --git a/wpilibc/src/main/native/cpp/commands/Scheduler.cpp b/wpilibc/src/main/native/cpp/commands/Scheduler.cpp
index 8a7af76..f406d74 100644
--- a/wpilibc/src/main/native/cpp/commands/Scheduler.cpp
+++ b/wpilibc/src/main/native/cpp/commands/Scheduler.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -21,6 +21,7 @@
#include "frc/commands/Command.h"
#include "frc/commands/Subsystem.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -51,7 +52,7 @@
}
void Scheduler::AddCommand(Command* command) {
- std::lock_guard<wpi::mutex> lock(m_impl->additionsMutex);
+ std::scoped_lock lock(m_impl->additionsMutex);
if (std::find(m_impl->additions.begin(), m_impl->additions.end(), command) !=
m_impl->additions.end())
return;
@@ -59,7 +60,7 @@
}
void Scheduler::AddButton(ButtonScheduler* button) {
- std::lock_guard<wpi::mutex> lock(m_impl->buttonsMutex);
+ std::scoped_lock lock(m_impl->buttonsMutex);
m_impl->buttons.emplace_back(button);
}
@@ -76,7 +77,7 @@
{
if (!m_impl->enabled) return;
- std::lock_guard<wpi::mutex> lock(m_impl->buttonsMutex);
+ std::scoped_lock lock(m_impl->buttonsMutex);
for (auto& button : m_impl->buttons) {
button->Execute();
}
@@ -103,7 +104,7 @@
// Add the new things
{
- std::lock_guard<wpi::mutex> lock(m_impl->additionsMutex);
+ std::scoped_lock lock(m_impl->additionsMutex);
for (auto& addition : m_impl->additions) {
// Check to make sure no adding during adding
if (m_impl->adding) {
@@ -182,8 +183,9 @@
if (m_impl->runningCommandsChanged) {
m_impl->commandsBuf.resize(0);
m_impl->idsBuf.resize(0);
+ auto& registry = SendableRegistry::GetInstance();
for (const auto& command : m_impl->commands) {
- m_impl->commandsBuf.emplace_back(command->GetName());
+ m_impl->commandsBuf.emplace_back(registry.GetName(command));
m_impl->idsBuf.emplace_back(command->GetID());
}
nt::NetworkTableEntry(namesEntry).SetStringArray(m_impl->commandsBuf);
@@ -195,7 +197,7 @@
Scheduler::Scheduler() : m_impl(new Impl) {
HAL_Report(HALUsageReporting::kResourceType_Command,
HALUsageReporting::kCommand_Scheduler);
- SetName("Scheduler");
+ SendableRegistry::GetInstance().AddLW(this, "Scheduler");
}
Scheduler::~Scheduler() {}
diff --git a/wpilibc/src/main/native/cpp/commands/Subsystem.cpp b/wpilibc/src/main/native/cpp/commands/Subsystem.cpp
index 308642f..6e665ea 100644
--- a/wpilibc/src/main/native/cpp/commands/Subsystem.cpp
+++ b/wpilibc/src/main/native/cpp/commands/Subsystem.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,11 +12,12 @@
#include "frc/commands/Scheduler.h"
#include "frc/livewindow/LiveWindow.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
Subsystem::Subsystem(const wpi::Twine& name) {
- SetName(name, name);
+ SendableRegistry::GetInstance().AddLW(this, name, name);
Scheduler::GetInstance()->RegisterSubsystem(this);
}
@@ -46,7 +47,7 @@
wpi::StringRef Subsystem::GetDefaultCommandName() {
Command* defaultCommand = GetDefaultCommand();
if (defaultCommand) {
- return defaultCommand->GetName();
+ return SendableRegistry::GetInstance().GetName(defaultCommand);
} else {
return wpi::StringRef();
}
@@ -62,7 +63,7 @@
wpi::StringRef Subsystem::GetCurrentCommandName() const {
Command* currentCommand = GetCurrentCommand();
if (currentCommand) {
- return currentCommand->GetName();
+ return SendableRegistry::GetInstance().GetName(currentCommand);
} else {
return wpi::StringRef();
}
@@ -72,6 +73,22 @@
void Subsystem::InitDefaultCommand() {}
+std::string Subsystem::GetName() const {
+ return SendableRegistry::GetInstance().GetName(this);
+}
+
+void Subsystem::SetName(const wpi::Twine& name) {
+ SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string Subsystem::GetSubsystem() const {
+ return SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void Subsystem::SetSubsystem(const wpi::Twine& name) {
+ SendableRegistry::GetInstance().SetSubsystem(this, name);
+}
+
void Subsystem::AddChild(const wpi::Twine& name,
std::shared_ptr<Sendable> child) {
AddChild(name, *child);
@@ -82,8 +99,9 @@
}
void Subsystem::AddChild(const wpi::Twine& name, Sendable& child) {
- child.SetName(GetSubsystem(), name);
- LiveWindow::GetInstance()->Add(&child);
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddLW(&child, registry.GetSubsystem(this), name);
+ registry.AddChild(this, &child);
}
void Subsystem::AddChild(std::shared_ptr<Sendable> child) { AddChild(*child); }
@@ -91,8 +109,10 @@
void Subsystem::AddChild(Sendable* child) { AddChild(*child); }
void Subsystem::AddChild(Sendable& child) {
- child.SetSubsystem(GetSubsystem());
- LiveWindow::GetInstance()->Add(&child);
+ auto& registry = SendableRegistry::GetInstance();
+ registry.SetSubsystem(&child, registry.GetSubsystem(this));
+ registry.EnableLiveWindow(&child);
+ registry.AddChild(this, &child);
}
void Subsystem::ConfirmCommand() {
diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
new file mode 100644
index 0000000..2202d93
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/PIDController.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc2;
+
+PIDController::PIDController(double Kp, double Ki, double Kd,
+ units::second_t period)
+ : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+ static int instances = 0;
+ instances++;
+ HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
+ frc::SendableRegistry::GetInstance().Add(this, "PIDController", instances);
+}
+
+void PIDController::SetP(double Kp) { m_Kp = Kp; }
+
+void PIDController::SetI(double Ki) { m_Ki = Ki; }
+
+void PIDController::SetD(double Kd) { m_Kd = Kd; }
+
+double PIDController::GetP() const { return m_Kp; }
+
+double PIDController::GetI() const { return m_Ki; }
+
+double PIDController::GetD() const { return m_Kd; }
+
+units::second_t PIDController::GetPeriod() const {
+ return units::second_t(m_period);
+}
+
+void PIDController::SetSetpoint(double setpoint) {
+ if (m_maximumInput > m_minimumInput) {
+ m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput);
+ } else {
+ m_setpoint = setpoint;
+ }
+}
+
+double PIDController::GetSetpoint() const { return m_setpoint; }
+
+bool PIDController::AtSetpoint() const {
+ return std::abs(m_positionError) < m_positionTolerance &&
+ std::abs(m_velocityError) < m_velocityTolerance;
+}
+
+void PIDController::EnableContinuousInput(double minimumInput,
+ double maximumInput) {
+ m_continuous = true;
+ SetInputRange(minimumInput, maximumInput);
+}
+
+void PIDController::DisableContinuousInput() { m_continuous = false; }
+
+void PIDController::SetIntegratorRange(double minimumIntegral,
+ double maximumIntegral) {
+ m_minimumIntegral = minimumIntegral;
+ m_maximumIntegral = maximumIntegral;
+}
+
+void PIDController::SetTolerance(double positionTolerance,
+ double velocityTolerance) {
+ m_positionTolerance = positionTolerance;
+ m_velocityTolerance = velocityTolerance;
+}
+
+double PIDController::GetPositionError() const {
+ return GetContinuousError(m_positionError);
+}
+
+double PIDController::GetVelocityError() const { return m_velocityError; }
+
+double PIDController::Calculate(double measurement) {
+ m_prevError = m_positionError;
+ m_positionError = GetContinuousError(m_setpoint - measurement);
+ m_velocityError = (m_positionError - m_prevError) / m_period.to<double>();
+
+ if (m_Ki != 0) {
+ m_totalError =
+ std::clamp(m_totalError + m_positionError * m_period.to<double>(),
+ m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
+ }
+
+ return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError;
+}
+
+double PIDController::Calculate(double measurement, double setpoint) {
+ // Set setpoint to provided value
+ SetSetpoint(setpoint);
+ return Calculate(measurement);
+}
+
+void PIDController::Reset() {
+ m_prevError = 0;
+ m_totalError = 0;
+}
+
+void PIDController::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("PIDController");
+ builder.AddDoubleProperty("p", [this] { return GetP(); },
+ [this](double value) { SetP(value); });
+ builder.AddDoubleProperty("i", [this] { return GetI(); },
+ [this](double value) { SetI(value); });
+ builder.AddDoubleProperty("d", [this] { return GetD(); },
+ [this](double value) { SetD(value); });
+ builder.AddDoubleProperty("setpoint", [this] { return GetSetpoint(); },
+ [this](double value) { SetSetpoint(value); });
+}
+
+double PIDController::GetContinuousError(double error) const {
+ if (m_continuous && m_inputRange > 0) {
+ error = std::fmod(error, m_inputRange);
+ if (std::abs(error) > m_inputRange / 2) {
+ if (error > 0) {
+ return error - m_inputRange;
+ } else {
+ return error + m_inputRange;
+ }
+ }
+ }
+
+ return error;
+}
+
+void PIDController::SetInputRange(double minimumInput, double maximumInput) {
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+ m_inputRange = maximumInput - minimumInput;
+
+ // Clamp setpoint to new input range
+ if (m_maximumInput > m_minimumInput) {
+ m_setpoint = std::clamp(m_setpoint, m_minimumInput, m_maximumInput);
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
new file mode 100644
index 0000000..4ef3bf9
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/ProfiledPIDController.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+ProfiledPIDController::ProfiledPIDController(
+ double Kp, double Ki, double Kd,
+ frc::TrapezoidProfile::Constraints constraints, units::second_t period)
+ : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
+
+void ProfiledPIDController::SetP(double Kp) { m_controller.SetP(Kp); }
+
+void ProfiledPIDController::SetI(double Ki) { m_controller.SetI(Ki); }
+
+void ProfiledPIDController::SetD(double Kd) { m_controller.SetD(Kd); }
+
+double ProfiledPIDController::GetP() const { return m_controller.GetP(); }
+
+double ProfiledPIDController::GetI() const { return m_controller.GetI(); }
+
+double ProfiledPIDController::GetD() const { return m_controller.GetD(); }
+
+units::second_t ProfiledPIDController::GetPeriod() const {
+ return m_controller.GetPeriod();
+}
+
+void ProfiledPIDController::SetGoal(TrapezoidProfile::State goal) {
+ m_goal = goal;
+}
+
+void ProfiledPIDController::SetGoal(units::meter_t goal) {
+ m_goal = {goal, 0_mps};
+}
+
+TrapezoidProfile::State ProfiledPIDController::GetGoal() const {
+ return m_goal;
+}
+
+bool ProfiledPIDController::AtGoal() const {
+ return AtSetpoint() && m_goal == m_setpoint;
+}
+
+void ProfiledPIDController::SetConstraints(
+ frc::TrapezoidProfile::Constraints constraints) {
+ m_constraints = constraints;
+}
+
+TrapezoidProfile::State ProfiledPIDController::GetSetpoint() const {
+ return m_setpoint;
+}
+
+bool ProfiledPIDController::AtSetpoint() const {
+ return m_controller.AtSetpoint();
+}
+
+void ProfiledPIDController::EnableContinuousInput(double minimumInput,
+ double maximumInput) {
+ m_controller.EnableContinuousInput(minimumInput, maximumInput);
+}
+
+void ProfiledPIDController::DisableContinuousInput() {
+ m_controller.DisableContinuousInput();
+}
+
+void ProfiledPIDController::SetIntegratorRange(double minimumIntegral,
+ double maximumIntegral) {
+ m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
+}
+
+void ProfiledPIDController::SetTolerance(double positionTolerance,
+ double velocityTolerance) {
+ m_controller.SetTolerance(positionTolerance, velocityTolerance);
+}
+
+double ProfiledPIDController::GetPositionError() const {
+ return m_controller.GetPositionError();
+}
+
+double ProfiledPIDController::GetVelocityError() const {
+ return m_controller.GetVelocityError();
+}
+
+double ProfiledPIDController::Calculate(units::meter_t measurement) {
+ frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
+ m_setpoint = profile.Calculate(GetPeriod());
+ return m_controller.Calculate(measurement.to<double>(),
+ m_setpoint.position.to<double>());
+}
+
+double ProfiledPIDController::Calculate(units::meter_t measurement,
+ TrapezoidProfile::State goal) {
+ SetGoal(goal);
+ return Calculate(measurement);
+}
+
+double ProfiledPIDController::Calculate(units::meter_t measurement,
+ units::meter_t goal) {
+ SetGoal(goal);
+ return Calculate(measurement);
+}
+
+double ProfiledPIDController::Calculate(
+ units::meter_t measurement, units::meter_t goal,
+ frc::TrapezoidProfile::Constraints constraints) {
+ SetConstraints(constraints);
+ return Calculate(measurement, goal);
+}
+
+void ProfiledPIDController::Reset() { m_controller.Reset(); }
+
+void ProfiledPIDController::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("ProfiledPIDController");
+ builder.AddDoubleProperty("p", [this] { return GetP(); },
+ [this](double value) { SetP(value); });
+ builder.AddDoubleProperty("i", [this] { return GetI(); },
+ [this](double value) { SetI(value); });
+ builder.AddDoubleProperty("d", [this] { return GetD(); },
+ [this](double value) { SetD(value); });
+ builder.AddDoubleProperty(
+ "goal", [this] { return GetGoal().position.to<double>(); },
+ [this](double value) { SetGoal(units::meter_t{value}); });
+}
diff --git a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
new file mode 100644
index 0000000..c55c2e8
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/RamseteController.h"
+
+#include <cmath>
+
+using namespace frc;
+
+/**
+ * Returns sin(x) / x.
+ *
+ * @param x Value of which to take sinc(x).
+ */
+static double Sinc(double x) {
+ if (std::abs(x) < 1e-9) {
+ return 1.0 - 1.0 / 6.0 * x * x;
+ } else {
+ return std::sin(x) / x;
+ }
+}
+
+RamseteController::RamseteController(double b, double zeta)
+ : m_b{b}, m_zeta{zeta} {}
+
+bool RamseteController::AtReference() const {
+ const auto& eTranslate = m_poseError.Translation();
+ const auto& eRotate = m_poseError.Rotation();
+ const auto& tolTranslate = m_poseTolerance.Translation();
+ const auto& tolRotate = m_poseTolerance.Rotation();
+ return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+ units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+ units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+}
+
+void RamseteController::SetTolerance(const Pose2d& poseTolerance) {
+ m_poseTolerance = poseTolerance;
+}
+
+ChassisSpeeds RamseteController::Calculate(
+ const Pose2d& currentPose, const Pose2d& poseRef,
+ units::meters_per_second_t linearVelocityRef,
+ units::radians_per_second_t angularVelocityRef) {
+ m_poseError = poseRef.RelativeTo(currentPose);
+
+ // Aliases for equation readability
+ double eX = m_poseError.Translation().X().to<double>();
+ double eY = m_poseError.Translation().Y().to<double>();
+ double eTheta = m_poseError.Rotation().Radians().to<double>();
+ double vRef = linearVelocityRef.to<double>();
+ double omegaRef = angularVelocityRef.to<double>();
+
+ double k =
+ 2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2));
+
+ units::meters_per_second_t v{vRef * m_poseError.Rotation().Cos() + k * eX};
+ units::radians_per_second_t omega{omegaRef + k * eTheta +
+ m_b * vRef * Sinc(eTheta) * eY};
+ return ChassisSpeeds{v, 0_mps, omega};
+}
+
+ChassisSpeeds RamseteController::Calculate(
+ const Pose2d& currentPose, const Trajectory::State& desiredState) {
+ return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+ desiredState.velocity * desiredState.curvature);
+}
diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index 5ad65b8..ee23a5e 100644
--- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,32 +14,34 @@
#include "frc/SpeedController.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
DifferentialDrive::DifferentialDrive(SpeedController& leftMotor,
SpeedController& rightMotor)
- : m_leftMotor(leftMotor), m_rightMotor(rightMotor) {
- AddChild(&m_leftMotor);
- AddChild(&m_rightMotor);
+ : m_leftMotor(&leftMotor), m_rightMotor(&rightMotor) {
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, m_leftMotor);
+ registry.AddChild(this, m_rightMotor);
static int instances = 0;
++instances;
- SetName("DifferentialDrive", instances);
+ registry.AddLW(this, "DifferentialDrive", instances);
}
void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
bool squareInputs) {
static bool reported = false;
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
- HALUsageReporting::kRobotDrive2_DifferentialArcade);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_DifferentialArcade, 2);
reported = true;
}
- xSpeed = Limit(xSpeed);
+ xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
- zRotation = Limit(zRotation);
+ zRotation = std::clamp(zRotation, -1.0, 1.0);
zRotation = ApplyDeadband(zRotation, m_deadband);
// Square the inputs (while preserving the sign) to increase fine control
@@ -75,9 +77,9 @@
}
}
- m_leftMotor.Set(Limit(leftMotorOutput) * m_maxOutput);
- m_rightMotor.Set(Limit(rightMotorOutput) * m_maxOutput *
- m_rightSideInvertMultiplier);
+ m_leftMotor->Set(std::clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
+ double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
+ m_rightMotor->Set(std::clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
Feed();
}
@@ -86,15 +88,15 @@
bool isQuickTurn) {
static bool reported = false;
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
- HALUsageReporting::kRobotDrive2_DifferentialCurvature);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_DifferentialCurvature, 2);
reported = true;
}
- xSpeed = Limit(xSpeed);
+ xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
- zRotation = Limit(zRotation);
+ zRotation = std::clamp(zRotation, -1.0, 1.0);
zRotation = ApplyDeadband(zRotation, m_deadband);
double angularPower;
@@ -102,8 +104,9 @@
if (isQuickTurn) {
if (std::abs(xSpeed) < m_quickStopThreshold) {
- m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator +
- m_quickStopAlpha * Limit(zRotation) * 2;
+ m_quickStopAccumulator =
+ (1 - m_quickStopAlpha) * m_quickStopAccumulator +
+ m_quickStopAlpha * std::clamp(zRotation, -1.0, 1.0) * 2;
}
overPower = true;
angularPower = zRotation;
@@ -148,9 +151,9 @@
rightMotorOutput /= maxMagnitude;
}
- m_leftMotor.Set(leftMotorOutput * m_maxOutput);
- m_rightMotor.Set(rightMotorOutput * m_maxOutput *
- m_rightSideInvertMultiplier);
+ m_leftMotor->Set(leftMotorOutput * m_maxOutput);
+ m_rightMotor->Set(rightMotorOutput * m_maxOutput *
+ m_rightSideInvertMultiplier);
Feed();
}
@@ -159,15 +162,15 @@
bool squareInputs) {
static bool reported = false;
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
- HALUsageReporting::kRobotDrive2_DifferentialTank);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_DifferentialTank, 2);
reported = true;
}
- leftSpeed = Limit(leftSpeed);
+ leftSpeed = std::clamp(leftSpeed, -1.0, 1.0);
leftSpeed = ApplyDeadband(leftSpeed, m_deadband);
- rightSpeed = Limit(rightSpeed);
+ rightSpeed = std::clamp(rightSpeed, -1.0, 1.0);
rightSpeed = ApplyDeadband(rightSpeed, m_deadband);
// Square the inputs (while preserving the sign) to increase fine control
@@ -177,8 +180,8 @@
rightSpeed = std::copysign(rightSpeed * rightSpeed, rightSpeed);
}
- m_leftMotor.Set(leftSpeed * m_maxOutput);
- m_rightMotor.Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
+ m_leftMotor->Set(leftSpeed * m_maxOutput);
+ m_rightMotor->Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
Feed();
}
@@ -200,8 +203,8 @@
}
void DifferentialDrive::StopMotor() {
- m_leftMotor.StopMotor();
- m_rightMotor.StopMotor();
+ m_leftMotor->StopMotor();
+ m_rightMotor->StopMotor();
Feed();
}
@@ -214,12 +217,12 @@
builder.SetActuator(true);
builder.SetSafeState([=] { StopMotor(); });
builder.AddDoubleProperty("Left Motor Speed",
- [=]() { return m_leftMotor.Get(); },
- [=](double value) { m_leftMotor.Set(value); });
+ [=]() { return m_leftMotor->Get(); },
+ [=](double value) { m_leftMotor->Set(value); });
builder.AddDoubleProperty(
"Right Motor Speed",
- [=]() { return m_rightMotor.Get() * m_rightSideInvertMultiplier; },
+ [=]() { return m_rightMotor->Get() * m_rightSideInvertMultiplier; },
[=](double value) {
- m_rightMotor.Set(value * m_rightSideInvertMultiplier);
+ m_rightMotor->Set(value * m_rightSideInvertMultiplier);
});
}
diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
index b1af5de..4c15de8 100644
--- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,14 +11,14 @@
#include <cmath>
#include <hal/HAL.h>
+#include <wpi/math>
#include "frc/SpeedController.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
-constexpr double kPi = 3.14159265358979323846;
-
KilloughDrive::KilloughDrive(SpeedController& leftMotor,
SpeedController& rightMotor,
SpeedController& backMotor)
@@ -29,33 +29,36 @@
SpeedController& rightMotor,
SpeedController& backMotor, double leftMotorAngle,
double rightMotorAngle, double backMotorAngle)
- : m_leftMotor(leftMotor), m_rightMotor(rightMotor), m_backMotor(backMotor) {
- m_leftVec = {std::cos(leftMotorAngle * (kPi / 180.0)),
- std::sin(leftMotorAngle * (kPi / 180.0))};
- m_rightVec = {std::cos(rightMotorAngle * (kPi / 180.0)),
- std::sin(rightMotorAngle * (kPi / 180.0))};
- m_backVec = {std::cos(backMotorAngle * (kPi / 180.0)),
- std::sin(backMotorAngle * (kPi / 180.0))};
- AddChild(&m_leftMotor);
- AddChild(&m_rightMotor);
- AddChild(&m_backMotor);
+ : m_leftMotor(&leftMotor),
+ m_rightMotor(&rightMotor),
+ m_backMotor(&backMotor) {
+ m_leftVec = {std::cos(leftMotorAngle * (wpi::math::pi / 180.0)),
+ std::sin(leftMotorAngle * (wpi::math::pi / 180.0))};
+ m_rightVec = {std::cos(rightMotorAngle * (wpi::math::pi / 180.0)),
+ std::sin(rightMotorAngle * (wpi::math::pi / 180.0))};
+ m_backVec = {std::cos(backMotorAngle * (wpi::math::pi / 180.0)),
+ std::sin(backMotorAngle * (wpi::math::pi / 180.0))};
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, m_leftMotor);
+ registry.AddChild(this, m_rightMotor);
+ registry.AddChild(this, m_backMotor);
static int instances = 0;
++instances;
- SetName("KilloughDrive", instances);
+ registry.AddLW(this, "KilloughDrive", instances);
}
void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
double zRotation, double gyroAngle) {
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
- HALUsageReporting::kRobotDrive2_KilloughCartesian);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_KilloughCartesian, 3);
reported = true;
}
- ySpeed = Limit(ySpeed);
+ ySpeed = std::clamp(ySpeed, -1.0, 1.0);
ySpeed = ApplyDeadband(ySpeed, m_deadband);
- xSpeed = Limit(xSpeed);
+ xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.
@@ -69,9 +72,9 @@
Normalize(wheelSpeeds);
- m_leftMotor.Set(wheelSpeeds[kLeft] * m_maxOutput);
- m_rightMotor.Set(wheelSpeeds[kRight] * m_maxOutput);
- m_backMotor.Set(wheelSpeeds[kBack] * m_maxOutput);
+ m_leftMotor->Set(wheelSpeeds[kLeft] * m_maxOutput);
+ m_rightMotor->Set(wheelSpeeds[kRight] * m_maxOutput);
+ m_backMotor->Set(wheelSpeeds[kBack] * m_maxOutput);
Feed();
}
@@ -79,19 +82,20 @@
void KilloughDrive::DrivePolar(double magnitude, double angle,
double zRotation) {
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
- HALUsageReporting::kRobotDrive2_KilloughPolar);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_KilloughPolar, 3);
reported = true;
}
- DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)),
- magnitude * std::cos(angle * (kPi / 180.0)), zRotation, 0.0);
+ DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)),
+ magnitude * std::cos(angle * (wpi::math::pi / 180.0)),
+ zRotation, 0.0);
}
void KilloughDrive::StopMotor() {
- m_leftMotor.StopMotor();
- m_rightMotor.StopMotor();
- m_backMotor.StopMotor();
+ m_leftMotor->StopMotor();
+ m_rightMotor->StopMotor();
+ m_backMotor->StopMotor();
Feed();
}
@@ -104,12 +108,12 @@
builder.SetActuator(true);
builder.SetSafeState([=] { StopMotor(); });
builder.AddDoubleProperty("Left Motor Speed",
- [=]() { return m_leftMotor.Get(); },
- [=](double value) { m_leftMotor.Set(value); });
+ [=]() { return m_leftMotor->Get(); },
+ [=](double value) { m_leftMotor->Set(value); });
builder.AddDoubleProperty("Right Motor Speed",
- [=]() { return m_rightMotor.Get(); },
- [=](double value) { m_rightMotor.Set(value); });
+ [=]() { return m_rightMotor->Get(); },
+ [=](double value) { m_rightMotor->Set(value); });
builder.AddDoubleProperty("Back Motor Speed",
- [=]() { return m_backMotor.Get(); },
- [=](double value) { m_backMotor.Set(value); });
+ [=]() { return m_backMotor->Get(); },
+ [=](double value) { m_backMotor->Set(value); });
}
diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index c74ba19..473e033 100644
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,44 +11,45 @@
#include <cmath>
#include <hal/HAL.h>
+#include <wpi/math>
#include "frc/SpeedController.h"
#include "frc/drive/Vector2d.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
-constexpr double kPi = 3.14159265358979323846;
-
MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor,
SpeedController& rearLeftMotor,
SpeedController& frontRightMotor,
SpeedController& rearRightMotor)
- : m_frontLeftMotor(frontLeftMotor),
- m_rearLeftMotor(rearLeftMotor),
- m_frontRightMotor(frontRightMotor),
- m_rearRightMotor(rearRightMotor) {
- AddChild(&m_frontLeftMotor);
- AddChild(&m_rearLeftMotor);
- AddChild(&m_frontRightMotor);
- AddChild(&m_rearRightMotor);
+ : m_frontLeftMotor(&frontLeftMotor),
+ m_rearLeftMotor(&rearLeftMotor),
+ m_frontRightMotor(&frontRightMotor),
+ m_rearRightMotor(&rearRightMotor) {
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, m_frontLeftMotor);
+ registry.AddChild(this, m_rearLeftMotor);
+ registry.AddChild(this, m_frontRightMotor);
+ registry.AddChild(this, m_rearRightMotor);
static int instances = 0;
++instances;
- SetName("MecanumDrive", instances);
+ registry.AddLW(this, "MecanumDrive", instances);
}
void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
double zRotation, double gyroAngle) {
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
- HALUsageReporting::kRobotDrive2_MecanumCartesian);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_MecanumCartesian, 4);
reported = true;
}
- ySpeed = Limit(ySpeed);
+ ySpeed = std::clamp(ySpeed, -1.0, 1.0);
ySpeed = ApplyDeadband(ySpeed, m_deadband);
- xSpeed = Limit(xSpeed);
+ xSpeed = std::clamp(xSpeed, -1.0, 1.0);
xSpeed = ApplyDeadband(xSpeed, m_deadband);
// Compensate for gyro angle.
@@ -63,12 +64,12 @@
Normalize(wheelSpeeds);
- m_frontLeftMotor.Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
- m_frontRightMotor.Set(wheelSpeeds[kFrontRight] * m_maxOutput *
+ m_frontLeftMotor->Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRight] * m_maxOutput *
+ m_rightSideInvertMultiplier);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeft] * m_maxOutput);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRight] * m_maxOutput *
m_rightSideInvertMultiplier);
- m_rearLeftMotor.Set(wheelSpeeds[kRearLeft] * m_maxOutput);
- m_rearRightMotor.Set(wheelSpeeds[kRearRight] * m_maxOutput *
- m_rightSideInvertMultiplier);
Feed();
}
@@ -76,13 +77,14 @@
void MecanumDrive::DrivePolar(double magnitude, double angle,
double zRotation) {
if (!reported) {
- HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
- HALUsageReporting::kRobotDrive2_MecanumPolar);
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_MecanumPolar, 4);
reported = true;
}
- DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)),
- magnitude * std::cos(angle * (kPi / 180.0)), zRotation, 0.0);
+ DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)),
+ magnitude * std::cos(angle * (wpi::math::pi / 180.0)),
+ zRotation, 0.0);
}
bool MecanumDrive::IsRightSideInverted() const {
@@ -94,10 +96,10 @@
}
void MecanumDrive::StopMotor() {
- m_frontLeftMotor.StopMotor();
- m_frontRightMotor.StopMotor();
- m_rearLeftMotor.StopMotor();
- m_rearRightMotor.StopMotor();
+ m_frontLeftMotor->StopMotor();
+ m_frontRightMotor->StopMotor();
+ m_rearLeftMotor->StopMotor();
+ m_rearRightMotor->StopMotor();
Feed();
}
@@ -109,22 +111,22 @@
builder.SetSmartDashboardType("MecanumDrive");
builder.SetActuator(true);
builder.SetSafeState([=] { StopMotor(); });
- builder.AddDoubleProperty("Front Left Motor Speed",
- [=]() { return m_frontLeftMotor.Get(); },
- [=](double value) { m_frontLeftMotor.Set(value); });
+ builder.AddDoubleProperty(
+ "Front Left Motor Speed", [=]() { return m_frontLeftMotor->Get(); },
+ [=](double value) { m_frontLeftMotor->Set(value); });
builder.AddDoubleProperty(
"Front Right Motor Speed",
- [=]() { return m_frontRightMotor.Get() * m_rightSideInvertMultiplier; },
+ [=]() { return m_frontRightMotor->Get() * m_rightSideInvertMultiplier; },
[=](double value) {
- m_frontRightMotor.Set(value * m_rightSideInvertMultiplier);
+ m_frontRightMotor->Set(value * m_rightSideInvertMultiplier);
});
builder.AddDoubleProperty("Rear Left Motor Speed",
- [=]() { return m_rearLeftMotor.Get(); },
- [=](double value) { m_rearLeftMotor.Set(value); });
+ [=]() { return m_rearLeftMotor->Get(); },
+ [=](double value) { m_rearLeftMotor->Set(value); });
builder.AddDoubleProperty(
"Rear Right Motor Speed",
- [=]() { return m_rearRightMotor.Get() * m_rightSideInvertMultiplier; },
+ [=]() { return m_rearRightMotor->Get() * m_rightSideInvertMultiplier; },
[=](double value) {
- m_rearRightMotor.Set(value * m_rightSideInvertMultiplier);
+ m_rearRightMotor->Set(value * m_rightSideInvertMultiplier);
});
}
diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
index c22cb2a..bae8868 100644
--- a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
+++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -26,16 +26,6 @@
void RobotDriveBase::FeedWatchdog() { Feed(); }
-double RobotDriveBase::Limit(double value) {
- if (value > 1.0) {
- return 1.0;
- }
- if (value < -1.0) {
- return -1.0;
- }
- return value;
-}
-
double RobotDriveBase::ApplyDeadband(double value, double deadband) {
if (std::abs(value) > deadband) {
if (value > 0.0) {
diff --git a/wpilibc/src/main/native/cpp/drive/Vector2d.cpp b/wpilibc/src/main/native/cpp/drive/Vector2d.cpp
index 3d4b689..a6e68a6 100644
--- a/wpilibc/src/main/native/cpp/drive/Vector2d.cpp
+++ b/wpilibc/src/main/native/cpp/drive/Vector2d.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,9 +9,9 @@
#include <cmath>
-using namespace frc;
+#include <wpi/math>
-constexpr double kPi = 3.14159265358979323846;
+using namespace frc;
Vector2d::Vector2d(double x, double y) {
this->x = x;
@@ -19,8 +19,8 @@
}
void Vector2d::Rotate(double angle) {
- double cosA = std::cos(angle * (kPi / 180.0));
- double sinA = std::sin(angle * (kPi / 180.0));
+ double cosA = std::cos(angle * (wpi::math::pi / 180.0));
+ double sinA = std::sin(angle * (wpi::math::pi / 180.0));
double out[2];
out[0] = x * cosA - y * sinA;
out[1] = x * sinA + y * cosA;
diff --git a/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
index db4cebe..7dfc8f0 100644
--- a/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
+++ b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -27,6 +27,13 @@
HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
}
+LinearDigitalFilter::LinearDigitalFilter(PIDSource& source,
+ std::initializer_list<double> ffGains,
+ std::initializer_list<double> fbGains)
+ : LinearDigitalFilter(source,
+ wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
+ wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
+
LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
wpi::ArrayRef<double> ffGains,
wpi::ArrayRef<double> fbGains)
@@ -40,6 +47,13 @@
HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
}
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ std::initializer_list<double> ffGains,
+ std::initializer_list<double> fbGains)
+ : LinearDigitalFilter(source,
+ wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
+ wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
+
LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(PIDSource& source,
double timeConstant,
double period) {
diff --git a/wpilibc/src/main/native/cpp/frc2/Timer.cpp b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
new file mode 100644
index 0000000..126cfdd
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/Timer.h"
+
+#include <chrono>
+#include <thread>
+
+#include <hal/HAL.h>
+
+#include "frc/DriverStation.h"
+#include "frc/RobotController.h"
+
+namespace frc2 {
+
+void Wait(units::second_t seconds) {
+ std::this_thread::sleep_for(
+ std::chrono::duration<double>(seconds.to<double>()));
+}
+
+units::second_t GetTime() {
+ using std::chrono::duration;
+ using std::chrono::duration_cast;
+ using std::chrono::system_clock;
+
+ return units::second_t(
+ duration_cast<duration<double>>(system_clock::now().time_since_epoch())
+ .count());
+}
+
+} // namespace frc2
+
+using namespace frc2;
+
+// for compatibility with msvc12--see C2864
+const units::second_t Timer::kRolloverTime = units::second_t((1ll << 32) / 1e6);
+
+Timer::Timer() { Reset(); }
+
+Timer::Timer(const Timer& rhs)
+ : m_startTime(rhs.m_startTime),
+ m_accumulatedTime(rhs.m_accumulatedTime),
+ m_running(rhs.m_running) {}
+
+Timer& Timer::operator=(const Timer& rhs) {
+ std::scoped_lock lock(m_mutex, rhs.m_mutex);
+
+ m_startTime = rhs.m_startTime;
+ m_accumulatedTime = rhs.m_accumulatedTime;
+ m_running = rhs.m_running;
+
+ return *this;
+}
+
+Timer::Timer(Timer&& rhs)
+ : m_startTime(std::move(rhs.m_startTime)),
+ m_accumulatedTime(std::move(rhs.m_accumulatedTime)),
+ m_running(std::move(rhs.m_running)) {}
+
+Timer& Timer::operator=(Timer&& rhs) {
+ std::scoped_lock lock(m_mutex, rhs.m_mutex);
+
+ m_startTime = std::move(rhs.m_startTime);
+ m_accumulatedTime = std::move(rhs.m_accumulatedTime);
+ m_running = std::move(rhs.m_running);
+
+ return *this;
+}
+
+units::second_t Timer::Get() const {
+ units::second_t result;
+ units::second_t currentTime = GetFPGATimestamp();
+
+ std::scoped_lock lock(m_mutex);
+ if (m_running) {
+ // If the current time is before the start time, then the FPGA clock rolled
+ // over. Compensate by adding the ~71 minutes that it takes to roll over to
+ // the current time.
+ if (currentTime < m_startTime) {
+ currentTime += kRolloverTime;
+ }
+
+ result = (currentTime - m_startTime) + m_accumulatedTime;
+ } else {
+ result = m_accumulatedTime;
+ }
+
+ return result;
+}
+
+void Timer::Reset() {
+ std::scoped_lock lock(m_mutex);
+ m_accumulatedTime = 0_s;
+ m_startTime = GetFPGATimestamp();
+}
+
+void Timer::Start() {
+ std::scoped_lock lock(m_mutex);
+ if (!m_running) {
+ m_startTime = GetFPGATimestamp();
+ m_running = true;
+ }
+}
+
+void Timer::Stop() {
+ units::second_t temp = Get();
+
+ std::scoped_lock lock(m_mutex);
+ if (m_running) {
+ m_accumulatedTime = temp;
+ m_running = false;
+ }
+}
+
+bool Timer::HasPeriodPassed(units::second_t period) {
+ if (Get() > period) {
+ std::scoped_lock lock(m_mutex);
+ // Advance the start time by the period.
+ m_startTime += period;
+ // Don't set it to the current time... we want to avoid drift.
+ return true;
+ }
+ return false;
+}
+
+units::second_t Timer::GetFPGATimestamp() {
+ // FPGA returns the timestamp in microseconds
+ return units::second_t(frc::RobotController::GetFPGATime()) * 1.0e-6;
+}
+
+units::second_t Timer::GetMatchTime() {
+ return units::second_t(frc::DriverStation::GetInstance().GetMatchTime());
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/Command.cpp b/wpilibc/src/main/native/cpp/frc2/command/Command.cpp
new file mode 100644
index 0000000..692879c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/Command.cpp
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/Command.h"
+
+#include <iostream>
+
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PerpetualCommand.h"
+#include "frc2/command/ProxyScheduleCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+
+Command::~Command() { CommandScheduler::GetInstance().Cancel(this); }
+
+Command::Command(const Command& rhs) : ErrorBase(rhs) {}
+
+Command& Command::operator=(const Command& rhs) {
+ ErrorBase::operator=(rhs);
+ m_isGrouped = false;
+ return *this;
+}
+
+void Command::Initialize() {}
+void Command::Execute() {}
+void Command::End(bool interrupted) {}
+
+ParallelRaceGroup Command::WithTimeout(units::second_t duration) && {
+ std::vector<std::unique_ptr<Command>> temp;
+ temp.emplace_back(std::make_unique<WaitCommand>(duration));
+ temp.emplace_back(std::move(*this).TransferOwnership());
+ return ParallelRaceGroup(std::move(temp));
+}
+
+ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
+ std::vector<std::unique_ptr<Command>> temp;
+ temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
+ temp.emplace_back(std::move(*this).TransferOwnership());
+ return ParallelRaceGroup(std::move(temp));
+}
+
+SequentialCommandGroup Command::BeforeStarting(std::function<void()> toRun) && {
+ std::vector<std::unique_ptr<Command>> temp;
+ temp.emplace_back(std::make_unique<InstantCommand>(
+ std::move(toRun), std::initializer_list<Subsystem*>{}));
+ temp.emplace_back(std::move(*this).TransferOwnership());
+ return SequentialCommandGroup(std::move(temp));
+}
+
+SequentialCommandGroup Command::AndThen(std::function<void()> toRun) && {
+ std::vector<std::unique_ptr<Command>> temp;
+ temp.emplace_back(std::move(*this).TransferOwnership());
+ temp.emplace_back(std::make_unique<InstantCommand>(
+ std::move(toRun), std::initializer_list<Subsystem*>{}));
+ return SequentialCommandGroup(std::move(temp));
+}
+
+PerpetualCommand Command::Perpetually() && {
+ return PerpetualCommand(std::move(*this).TransferOwnership());
+}
+
+ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); }
+
+void Command::Schedule(bool interruptible) {
+ CommandScheduler::GetInstance().Schedule(interruptible, this);
+}
+
+void Command::Cancel() { CommandScheduler::GetInstance().Cancel(this); }
+
+bool Command::IsScheduled() const {
+ return CommandScheduler::GetInstance().IsScheduled(this);
+}
+
+bool Command::HasRequirement(Subsystem* requirement) const {
+ bool hasRequirement = false;
+ for (auto&& subsystem : GetRequirements()) {
+ hasRequirement |= requirement == subsystem;
+ }
+ return hasRequirement;
+}
+
+std::string Command::GetName() const { return GetTypeName(*this); }
+
+bool Command::IsGrouped() const { return m_isGrouped; }
+
+void Command::SetGrouped(bool grouped) { m_isGrouped = grouped; }
+
+namespace frc2 {
+bool RequirementsDisjoint(Command* first, Command* second) {
+ bool disjoint = true;
+ auto&& requirements = second->GetRequirements();
+ for (auto&& requirement : first->GetRequirements()) {
+ disjoint &= requirements.find(requirement) == requirements.end();
+ }
+ return disjoint;
+}
+} // namespace frc2
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp
new file mode 100644
index 0000000..aeba26b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandBase.h"
+
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+#include <frc2/command/CommandScheduler.h>
+#include <frc2/command/SetUtilities.h>
+
+using namespace frc2;
+
+CommandBase::CommandBase() {
+ frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
+}
+
+void CommandBase::AddRequirements(
+ std::initializer_list<Subsystem*> requirements) {
+ m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+void CommandBase::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
+ m_requirements.insert(requirements.begin(), requirements.end());
+}
+
+wpi::SmallSet<Subsystem*, 4> CommandBase::GetRequirements() const {
+ return m_requirements;
+}
+
+void CommandBase::SetName(const wpi::Twine& name) {
+ frc::SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string CommandBase::GetName() const {
+ return frc::SendableRegistry::GetInstance().GetName(this);
+}
+
+std::string CommandBase::GetSubsystem() const {
+ return frc::SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void CommandBase::SetSubsystem(const wpi::Twine& subsystem) {
+ frc::SendableRegistry::GetInstance().SetSubsystem(this, subsystem);
+}
+
+void CommandBase::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Command");
+ builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr);
+ builder.AddBooleanProperty("running", [this] { return IsScheduled(); },
+ [this](bool value) {
+ bool isScheduled = IsScheduled();
+ if (value && !isScheduled) {
+ Schedule();
+ } else if (!value && isScheduled) {
+ Cancel();
+ }
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
new file mode 100644
index 0000000..ba53397
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandGroupBase.h"
+
+#include <set>
+
+#include "frc/WPIErrors.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+template <typename TMap, typename TKey>
+static bool ContainsKey(const TMap& map, TKey keyToCheck) {
+ return map.find(keyToCheck) != map.end();
+}
+bool CommandGroupBase::RequireUngrouped(Command& command) {
+ if (command.IsGrouped()) {
+ wpi_setGlobalWPIErrorWithContext(
+ CommandIllegalUse,
+ "Commands cannot be added to more than one CommandGroup");
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool CommandGroupBase::RequireUngrouped(
+ wpi::ArrayRef<std::unique_ptr<Command>> commands) {
+ bool allUngrouped = true;
+ for (auto&& command : commands) {
+ allUngrouped &= !command.get()->IsGrouped();
+ }
+ if (!allUngrouped) {
+ wpi_setGlobalWPIErrorWithContext(
+ CommandIllegalUse,
+ "Commands cannot be added to more than one CommandGroup");
+ }
+ return allUngrouped;
+}
+
+bool CommandGroupBase::RequireUngrouped(
+ std::initializer_list<Command*> commands) {
+ bool allUngrouped = true;
+ for (auto&& command : commands) {
+ allUngrouped &= !command->IsGrouped();
+ }
+ if (!allUngrouped) {
+ wpi_setGlobalWPIErrorWithContext(
+ CommandIllegalUse,
+ "Commands cannot be added to more than one CommandGroup");
+ }
+ return allUngrouped;
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp
new file mode 100644
index 0000000..464a4a6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -0,0 +1,346 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandScheduler.h"
+
+#include <frc/RobotState.h>
+#include <frc/WPIErrors.h>
+#include <frc/commands/Scheduler.h>
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+#include <frc2/command/CommandGroupBase.h>
+#include <frc2/command/Subsystem.h>
+
+#include <hal/HAL.h>
+
+using namespace frc2;
+template <typename TMap, typename TKey>
+static bool ContainsKey(const TMap& map, TKey keyToCheck) {
+ return map.find(keyToCheck) != map.end();
+}
+
+CommandScheduler::CommandScheduler() {
+ frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
+}
+
+CommandScheduler& CommandScheduler::GetInstance() {
+ static CommandScheduler scheduler;
+ return scheduler;
+}
+
+void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
+ m_buttons.emplace_back(std::move(button));
+}
+
+void CommandScheduler::ClearButtons() { m_buttons.clear(); }
+
+void CommandScheduler::Schedule(bool interruptible, Command* command) {
+ if (m_inRunLoop) {
+ m_toSchedule.try_emplace(command, interruptible);
+ return;
+ }
+
+ if (command->IsGrouped()) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "A command that is part of a command group "
+ "cannot be independently scheduled");
+ return;
+ }
+ if (m_disabled ||
+ (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
+ ContainsKey(m_scheduledCommands, command)) {
+ return;
+ }
+
+ const auto& requirements = command->GetRequirements();
+
+ wpi::SmallVector<Command*, 8> intersection;
+
+ bool isDisjoint = true;
+ bool allInterruptible = true;
+ for (auto&& i1 : m_requirements) {
+ if (requirements.find(i1.first) != requirements.end()) {
+ isDisjoint = false;
+ allInterruptible &= m_scheduledCommands[i1.second].IsInterruptible();
+ intersection.emplace_back(i1.second);
+ }
+ }
+
+ if (isDisjoint || allInterruptible) {
+ if (allInterruptible) {
+ for (auto&& cmdToCancel : intersection) {
+ Cancel(cmdToCancel);
+ }
+ }
+ command->Initialize();
+ m_scheduledCommands[command] = CommandState{interruptible};
+ for (auto&& action : m_initActions) {
+ action(*command);
+ }
+ for (auto&& requirement : requirements) {
+ m_requirements[requirement] = command;
+ }
+ }
+}
+
+void CommandScheduler::Schedule(Command* command) { Schedule(true, command); }
+
+void CommandScheduler::Schedule(bool interruptible,
+ wpi::ArrayRef<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(interruptible, command);
+ }
+}
+
+void CommandScheduler::Schedule(bool interruptible,
+ std::initializer_list<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(interruptible, command);
+ }
+}
+
+void CommandScheduler::Schedule(wpi::ArrayRef<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(true, command);
+ }
+}
+
+void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
+ for (auto command : commands) {
+ Schedule(true, command);
+ }
+}
+
+void CommandScheduler::Run() {
+ if (m_disabled) {
+ return;
+ }
+
+ // Run the periodic method of all registered subsystems.
+ for (auto&& subsystem : m_subsystems) {
+ subsystem.getFirst()->Periodic();
+ }
+
+ // Poll buttons for new commands to add.
+ for (auto&& button : m_buttons) {
+ button();
+ }
+
+ m_inRunLoop = true;
+ // Run scheduled commands, remove finished commands.
+ for (auto iterator = m_scheduledCommands.begin();
+ iterator != m_scheduledCommands.end(); iterator++) {
+ Command* command = iterator->getFirst();
+
+ if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
+ Cancel(command);
+ continue;
+ }
+
+ command->Execute();
+ for (auto&& action : m_executeActions) {
+ action(*command);
+ }
+
+ if (command->IsFinished()) {
+ command->End(false);
+ for (auto&& action : m_finishActions) {
+ action(*command);
+ }
+
+ for (auto&& requirement : command->GetRequirements()) {
+ m_requirements.erase(requirement);
+ }
+
+ m_scheduledCommands.erase(iterator);
+ }
+ }
+ m_inRunLoop = false;
+
+ for (auto&& commandInterruptible : m_toSchedule) {
+ Schedule(commandInterruptible.second, commandInterruptible.first);
+ }
+
+ for (auto&& command : m_toCancel) {
+ Cancel(command);
+ }
+
+ m_toSchedule.clear();
+ m_toCancel.clear();
+
+ // Add default commands for un-required registered subsystems.
+ for (auto&& subsystem : m_subsystems) {
+ auto s = m_requirements.find(subsystem.getFirst());
+ if (s == m_requirements.end()) {
+ Schedule({subsystem.getSecond().get()});
+ }
+ }
+}
+
+void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
+ m_subsystems[subsystem] = nullptr;
+}
+
+void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
+ auto s = m_subsystems.find(subsystem);
+ if (s != m_subsystems.end()) {
+ m_subsystems.erase(s);
+ }
+}
+
+void CommandScheduler::RegisterSubsystem(
+ std::initializer_list<Subsystem*> subsystems) {
+ for (auto* subsystem : subsystems) {
+ RegisterSubsystem(subsystem);
+ }
+}
+
+void CommandScheduler::UnregisterSubsystem(
+ std::initializer_list<Subsystem*> subsystems) {
+ for (auto* subsystem : subsystems) {
+ UnregisterSubsystem(subsystem);
+ }
+}
+
+Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
+ auto&& find = m_subsystems.find(subsystem);
+ if (find != m_subsystems.end()) {
+ return find->second.get();
+ } else {
+ return nullptr;
+ }
+}
+
+void CommandScheduler::Cancel(Command* command) {
+ if (m_inRunLoop) {
+ m_toCancel.emplace_back(command);
+ return;
+ }
+
+ auto find = m_scheduledCommands.find(command);
+ if (find == m_scheduledCommands.end()) return;
+ command->End(true);
+ for (auto&& action : m_interruptActions) {
+ action(*command);
+ }
+ m_scheduledCommands.erase(find);
+ for (auto&& requirement : m_requirements) {
+ if (requirement.second == command) {
+ m_requirements.erase(requirement.first);
+ }
+ }
+}
+
+void CommandScheduler::Cancel(wpi::ArrayRef<Command*> commands) {
+ for (auto command : commands) {
+ Cancel(command);
+ }
+}
+
+void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
+ for (auto command : commands) {
+ Cancel(command);
+ }
+}
+
+void CommandScheduler::CancelAll() {
+ for (auto&& command : m_scheduledCommands) {
+ Cancel(command.first);
+ }
+}
+
+double CommandScheduler::TimeSinceScheduled(const Command* command) const {
+ auto find = m_scheduledCommands.find(command);
+ if (find != m_scheduledCommands.end()) {
+ return find->second.TimeSinceInitialized();
+ } else {
+ return -1;
+ }
+}
+bool CommandScheduler::IsScheduled(
+ wpi::ArrayRef<const Command*> commands) const {
+ for (auto command : commands) {
+ if (!IsScheduled(command)) {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool CommandScheduler::IsScheduled(
+ std::initializer_list<const Command*> commands) const {
+ for (auto command : commands) {
+ if (!IsScheduled(command)) {
+ return false;
+ }
+ }
+ return true;
+}
+
+bool CommandScheduler::IsScheduled(const Command* command) const {
+ return m_scheduledCommands.find(command) != m_scheduledCommands.end();
+}
+
+Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
+ auto find = m_requirements.find(subsystem);
+ if (find != m_requirements.end()) {
+ return find->second;
+ } else {
+ return nullptr;
+ }
+}
+
+void CommandScheduler::Disable() { m_disabled = true; }
+
+void CommandScheduler::Enable() { m_disabled = false; }
+
+void CommandScheduler::OnCommandInitialize(Action action) {
+ m_initActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandExecute(Action action) {
+ m_executeActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandInterrupt(Action action) {
+ m_interruptActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::OnCommandFinish(Action action) {
+ m_finishActions.emplace_back(std::move(action));
+}
+
+void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Scheduler");
+ m_namesEntry = builder.GetEntry("Names");
+ m_idsEntry = builder.GetEntry("Ids");
+ m_cancelEntry = builder.GetEntry("Cancel");
+
+ builder.SetUpdateTable([this] {
+ double tmp[1];
+ tmp[0] = 0;
+ auto toCancel = m_cancelEntry.GetDoubleArray(tmp);
+ for (auto cancel : toCancel) {
+ uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
+ Command* command = reinterpret_cast<Command*>(ptrTmp);
+ if (m_scheduledCommands.find(command) != m_scheduledCommands.end()) {
+ Cancel(command);
+ }
+ m_cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
+ }
+
+ wpi::SmallVector<std::string, 8> names;
+ wpi::SmallVector<double, 8> ids;
+ for (auto&& command : m_scheduledCommands) {
+ names.emplace_back(command.first->GetName());
+ uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
+ ids.emplace_back(static_cast<double>(ptrTmp));
+ }
+ m_namesEntry.SetStringArray(names);
+ m_idsEntry.SetDoubleArray(ids);
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp
new file mode 100644
index 0000000..78ae006
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/CommandState.h"
+
+#include "frc/Timer.h"
+
+using namespace frc2;
+CommandState::CommandState(bool interruptible)
+ : m_interruptible{interruptible} {
+ StartTiming();
+ StartRunning();
+}
+
+void CommandState::StartTiming() {
+ m_startTime = frc::Timer::GetFPGATimestamp();
+}
+void CommandState::StartRunning() { m_startTime = -1; }
+double CommandState::TimeSinceInitialized() const {
+ return m_startTime != -1 ? frc::Timer::GetFPGATimestamp() - m_startTime : -1;
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
new file mode 100644
index 0000000..2344513
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ConditionalCommand.h"
+
+using namespace frc2;
+
+ConditionalCommand::ConditionalCommand(std::unique_ptr<Command>&& onTrue,
+ std::unique_ptr<Command>&& onFalse,
+ std::function<bool()> condition)
+ : m_condition{std::move(condition)} {
+ if (!CommandGroupBase::RequireUngrouped({onTrue.get(), onFalse.get()})) {
+ return;
+ }
+
+ m_onTrue = std::move(onTrue);
+ m_onFalse = std::move(onFalse);
+
+ m_onTrue->SetGrouped(true);
+ m_onFalse->SetGrouped(true);
+
+ m_runsWhenDisabled &= m_onTrue->RunsWhenDisabled();
+ m_runsWhenDisabled &= m_onFalse->RunsWhenDisabled();
+
+ AddRequirements(m_onTrue->GetRequirements());
+ AddRequirements(m_onFalse->GetRequirements());
+}
+
+void ConditionalCommand::Initialize() {
+ if (m_condition()) {
+ m_selectedCommand = m_onTrue.get();
+ } else {
+ m_selectedCommand = m_onFalse.get();
+ }
+ m_selectedCommand->Initialize();
+}
+
+void ConditionalCommand::Execute() { m_selectedCommand->Execute(); }
+
+void ConditionalCommand::End(bool interrupted) {
+ m_selectedCommand->End(interrupted);
+}
+
+bool ConditionalCommand::IsFinished() {
+ return m_selectedCommand->IsFinished();
+}
+
+bool ConditionalCommand::RunsWhenDisabled() const { return m_runsWhenDisabled; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
new file mode 100644
index 0000000..63c3179
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+
+FunctionalCommand::FunctionalCommand(std::function<void()> onInit,
+ std::function<void()> onExecute,
+ std::function<void(bool)> onEnd,
+ std::function<bool()> isFinished)
+ : m_onInit{std::move(onInit)},
+ m_onExecute{std::move(onExecute)},
+ m_onEnd{std::move(onEnd)},
+ m_isFinished{std::move(isFinished)} {}
+
+void FunctionalCommand::Initialize() { m_onInit(); }
+
+void FunctionalCommand::Execute() { m_onExecute(); }
+
+void FunctionalCommand::End(bool interrupted) { m_onEnd(interrupted); }
+
+bool FunctionalCommand::IsFinished() { return m_isFinished(); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp
new file mode 100644
index 0000000..b199074
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+
+InstantCommand::InstantCommand(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements)
+ : m_toRun{std::move(toRun)} {
+ AddRequirements(requirements);
+}
+
+InstantCommand::InstantCommand() : m_toRun{[] {}} {}
+
+void InstantCommand::Initialize() { m_toRun(); }
+
+bool InstantCommand::IsFinished() { return true; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp
new file mode 100644
index 0000000..c4eac81
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/NotifierCommand.h"
+
+using namespace frc2;
+
+NotifierCommand::NotifierCommand(std::function<void()> toRun,
+ units::second_t period,
+ std::initializer_list<Subsystem*> requirements)
+ : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} {
+ AddRequirements(requirements);
+}
+
+NotifierCommand::NotifierCommand(NotifierCommand&& other)
+ : CommandHelper(std::move(other)),
+ m_toRun(other.m_toRun),
+ m_notifier(other.m_toRun),
+ m_period(other.m_period) {}
+
+NotifierCommand::NotifierCommand(const NotifierCommand& other)
+ : CommandHelper(other),
+ m_toRun(other.m_toRun),
+ m_notifier(frc::Notifier(other.m_toRun)),
+ m_period(other.m_period) {}
+
+void NotifierCommand::Initialize() { m_notifier.StartPeriodic(m_period); }
+
+void NotifierCommand::End(bool interrupted) { m_notifier.Stop(); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp
new file mode 100644
index 0000000..4391e60
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PIDCommand.h"
+
+using namespace frc2;
+
+PIDCommand::PIDCommand(PIDController controller,
+ std::function<double()> measurementSource,
+ std::function<double()> setpointSource,
+ std::function<void(double)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : m_controller{controller},
+ m_measurement{std::move(measurementSource)},
+ m_setpoint{std::move(setpointSource)},
+ m_useOutput{std::move(useOutput)} {
+ AddRequirements(requirements);
+}
+
+PIDCommand::PIDCommand(PIDController controller,
+ std::function<double()> measurementSource,
+ double setpoint, std::function<void(double)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
+ useOutput, requirements) {}
+
+void PIDCommand::Initialize() { m_controller.Reset(); }
+
+void PIDCommand::Execute() {
+ UseOutput(m_controller.Calculate(GetMeasurement(), m_setpoint()));
+}
+
+void PIDCommand::End(bool interrupted) { UseOutput(0); }
+
+void PIDCommand::SetOutput(std::function<void(double)> useOutput) {
+ m_useOutput = useOutput;
+}
+
+void PIDCommand::SetSetpoint(std::function<double()> setpointSource) {
+ m_setpoint = setpointSource;
+}
+
+void PIDCommand::SetSetpoint(double setpoint) {
+ m_setpoint = [setpoint] { return setpoint; };
+}
+
+void PIDCommand::SetSetpointRelative(double relativeSetpoint) {
+ SetSetpoint(m_setpoint() + relativeSetpoint);
+}
+
+double PIDCommand::GetMeasurement() { return m_measurement(); }
+
+void PIDCommand::UseOutput(double output) { m_useOutput(output); }
+
+PIDController& PIDCommand::getController() { return m_controller; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
new file mode 100644
index 0000000..b81f6f6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PIDSubsystem.h"
+
+using namespace frc2;
+
+PIDSubsystem::PIDSubsystem(PIDController controller)
+ : m_controller{controller} {}
+
+void PIDSubsystem::Periodic() {
+ if (m_enabled) {
+ UseOutput(m_controller.Calculate(GetMeasurement(), GetSetpoint()));
+ }
+}
+
+void PIDSubsystem::Enable() {
+ m_controller.Reset();
+ m_enabled = true;
+}
+
+void PIDSubsystem::Disable() {
+ UseOutput(0);
+ m_enabled = false;
+}
+
+PIDController& PIDSubsystem::GetController() { return m_controller; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
new file mode 100644
index 0000000..d8a4159
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelCommandGroup.h"
+
+using namespace frc2;
+
+ParallelCommandGroup::ParallelCommandGroup(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ AddCommands(std::move(commands));
+}
+
+void ParallelCommandGroup::Initialize() {
+ for (auto& commandRunning : m_commands) {
+ commandRunning.first->Initialize();
+ commandRunning.second = true;
+ }
+ isRunning = true;
+}
+
+void ParallelCommandGroup::Execute() {
+ for (auto& commandRunning : m_commands) {
+ if (!commandRunning.second) continue;
+ commandRunning.first->Execute();
+ if (commandRunning.first->IsFinished()) {
+ commandRunning.first->End(false);
+ commandRunning.second = false;
+ }
+ }
+}
+
+void ParallelCommandGroup::End(bool interrupted) {
+ if (interrupted) {
+ for (auto& commandRunning : m_commands) {
+ if (commandRunning.second) {
+ commandRunning.first->End(true);
+ }
+ }
+ }
+ isRunning = false;
+}
+
+bool ParallelCommandGroup::IsFinished() {
+ for (auto& command : m_commands) {
+ if (command.second) return false;
+ }
+ return true;
+}
+
+bool ParallelCommandGroup::RunsWhenDisabled() const {
+ return m_runWhenDisabled;
+}
+
+void ParallelCommandGroup::AddCommands(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ for (auto&& command : commands) {
+ if (!RequireUngrouped(*command)) return;
+ }
+
+ if (isRunning) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Commands cannot be added to a CommandGroup "
+ "while the group is running");
+ }
+
+ for (auto&& command : commands) {
+ if (RequirementsDisjoint(this, command.get())) {
+ command->SetGrouped(true);
+ AddRequirements(command->GetRequirements());
+ m_runWhenDisabled &= command->RunsWhenDisabled();
+ m_commands[std::move(command)] = false;
+ } else {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Multiple commands in a parallel group cannot "
+ "require the same subsystems");
+ return;
+ }
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
new file mode 100644
index 0000000..d967e67
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelDeadlineGroup.h"
+
+using namespace frc2;
+
+ParallelDeadlineGroup::ParallelDeadlineGroup(
+ std::unique_ptr<Command>&& deadline,
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ SetDeadline(std::move(deadline));
+ AddCommands(std::move(commands));
+}
+
+void ParallelDeadlineGroup::Initialize() {
+ for (auto& commandRunning : m_commands) {
+ commandRunning.first->Initialize();
+ commandRunning.second = true;
+ }
+ isRunning = true;
+}
+
+void ParallelDeadlineGroup::Execute() {
+ for (auto& commandRunning : m_commands) {
+ if (!commandRunning.second) continue;
+ commandRunning.first->Execute();
+ if (commandRunning.first->IsFinished()) {
+ commandRunning.first->End(false);
+ commandRunning.second = false;
+ }
+ }
+}
+
+void ParallelDeadlineGroup::End(bool interrupted) {
+ for (auto& commandRunning : m_commands) {
+ if (commandRunning.second) {
+ commandRunning.first->End(true);
+ }
+ }
+ isRunning = false;
+}
+
+bool ParallelDeadlineGroup::IsFinished() { return m_deadline->IsFinished(); }
+
+bool ParallelDeadlineGroup::RunsWhenDisabled() const {
+ return m_runWhenDisabled;
+}
+
+void ParallelDeadlineGroup::AddCommands(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ if (!RequireUngrouped(commands)) {
+ return;
+ }
+
+ if (isRunning) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Commands cannot be added to a CommandGroup "
+ "while the group is running");
+ }
+
+ for (auto&& command : commands) {
+ if (RequirementsDisjoint(this, command.get())) {
+ command->SetGrouped(true);
+ AddRequirements(command->GetRequirements());
+ m_runWhenDisabled &= command->RunsWhenDisabled();
+ m_commands[std::move(command)] = false;
+ } else {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Multiple commands in a parallel group cannot "
+ "require the same subsystems");
+ return;
+ }
+ }
+}
+
+void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
+ m_deadline = deadline.get();
+ m_deadline->SetGrouped(true);
+ m_commands[std::move(deadline)] = false;
+ AddRequirements(m_deadline->GetRequirements());
+ m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
new file mode 100644
index 0000000..8a02717
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ParallelRaceGroup.h"
+
+using namespace frc2;
+
+ParallelRaceGroup::ParallelRaceGroup(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ AddCommands(std::move(commands));
+}
+
+void ParallelRaceGroup::Initialize() {
+ for (auto& commandRunning : m_commands) {
+ commandRunning->Initialize();
+ }
+ isRunning = true;
+}
+
+void ParallelRaceGroup::Execute() {
+ for (auto& commandRunning : m_commands) {
+ commandRunning->Execute();
+ if (commandRunning->IsFinished()) {
+ m_finished = true;
+ }
+ }
+}
+
+void ParallelRaceGroup::End(bool interrupted) {
+ for (auto& commandRunning : m_commands) {
+ commandRunning->End(!commandRunning->IsFinished());
+ }
+ isRunning = false;
+}
+
+bool ParallelRaceGroup::IsFinished() { return m_finished; }
+
+bool ParallelRaceGroup::RunsWhenDisabled() const { return m_runWhenDisabled; }
+
+void ParallelRaceGroup::AddCommands(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ if (!RequireUngrouped(commands)) {
+ return;
+ }
+
+ if (isRunning) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Commands cannot be added to a CommandGroup "
+ "while the group is running");
+ }
+
+ for (auto&& command : commands) {
+ if (RequirementsDisjoint(this, command.get())) {
+ command->SetGrouped(true);
+ AddRequirements(command->GetRequirements());
+ m_runWhenDisabled &= command->RunsWhenDisabled();
+ m_commands.emplace(std::move(command));
+ } else {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Multiple commands in a parallel group cannot "
+ "require the same subsystems");
+ return;
+ }
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
new file mode 100644
index 0000000..f29850b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PerpetualCommand.h"
+
+using namespace frc2;
+
+PerpetualCommand::PerpetualCommand(std::unique_ptr<Command>&& command) {
+ if (!CommandGroupBase::RequireUngrouped(command)) {
+ return;
+ }
+ m_command = std::move(command);
+ m_command->SetGrouped(true);
+ AddRequirements(m_command->GetRequirements());
+}
+
+void PerpetualCommand::Initialize() { m_command->Initialize(); }
+
+void PerpetualCommand::Execute() { m_command->Execute(); }
+
+void PerpetualCommand::End(bool interrupted) { m_command->End(interrupted); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
new file mode 100644
index 0000000..8bd62ea
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/PrintCommand.h"
+
+using namespace frc2;
+
+PrintCommand::PrintCommand(const wpi::Twine& message)
+ : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
+}
+
+bool PrintCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
new file mode 100644
index 0000000..c0cc19b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ProfiledPIDCommand.h"
+
+using namespace frc2;
+using State = frc::TrapezoidProfile::State;
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+ frc::ProfiledPIDController controller,
+ std::function<units::meter_t()> measurementSource,
+ std::function<State()> goalSource,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : m_controller{controller},
+ m_measurement{std::move(measurementSource)},
+ m_goal{std::move(goalSource)},
+ m_useOutput{std::move(useOutput)} {
+ AddRequirements(requirements);
+}
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+ frc::ProfiledPIDController controller,
+ std::function<units::meter_t()> measurementSource,
+ std::function<units::meter_t()> goalSource,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : ProfiledPIDCommand(controller, measurementSource,
+ [&goalSource]() {
+ return State{goalSource(), 0_mps};
+ },
+ useOutput, requirements) {}
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+ frc::ProfiledPIDController controller,
+ std::function<units::meter_t()> measurementSource, State goal,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
+ useOutput, requirements) {}
+
+ProfiledPIDCommand::ProfiledPIDCommand(
+ frc::ProfiledPIDController controller,
+ std::function<units::meter_t()> measurementSource, units::meter_t goal,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements)
+ : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
+ useOutput, requirements) {}
+
+void ProfiledPIDCommand::Initialize() { m_controller.Reset(); }
+
+void ProfiledPIDCommand::Execute() {
+ UseOutput(m_controller.Calculate(GetMeasurement(), m_goal()),
+ m_controller.GetSetpoint());
+}
+
+void ProfiledPIDCommand::End(bool interrupted) {
+ UseOutput(0, State{0_m, 0_mps});
+}
+
+units::meter_t ProfiledPIDCommand::GetMeasurement() { return m_measurement(); }
+
+void ProfiledPIDCommand::UseOutput(double output, State state) {
+ m_useOutput(output, state);
+}
+
+frc::ProfiledPIDController& ProfiledPIDCommand::GetController() {
+ return m_controller;
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
new file mode 100644
index 0000000..469e153
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ProfiledPIDSubsystem.h"
+
+using namespace frc2;
+using State = frc::TrapezoidProfile::State;
+
+ProfiledPIDSubsystem::ProfiledPIDSubsystem(
+ frc::ProfiledPIDController controller)
+ : m_controller{controller} {}
+
+void ProfiledPIDSubsystem::Periodic() {
+ if (m_enabled) {
+ UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
+ m_controller.GetSetpoint());
+ }
+}
+
+void ProfiledPIDSubsystem::Enable() {
+ m_controller.Reset();
+ m_enabled = true;
+}
+
+void ProfiledPIDSubsystem::Disable() {
+ UseOutput(0, State{0_m, 0_mps});
+ m_enabled = false;
+}
+
+frc::ProfiledPIDController& ProfiledPIDSubsystem::GetController() {
+ return m_controller;
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
new file mode 100644
index 0000000..6f96315
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ProxyScheduleCommand.h"
+
+using namespace frc2;
+
+ProxyScheduleCommand::ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
+ SetInsert(m_toSchedule, toSchedule);
+}
+
+void ProxyScheduleCommand::Initialize() {
+ for (auto* command : m_toSchedule) {
+ command->Schedule();
+ }
+}
+
+void ProxyScheduleCommand::End(bool interrupted) {
+ if (interrupted) {
+ for (auto* command : m_toSchedule) {
+ command->Cancel();
+ }
+ }
+}
+
+void ProxyScheduleCommand::Execute() {
+ m_finished = true;
+ for (auto* command : m_toSchedule) {
+ m_finished &= !command->IsScheduled();
+ }
+}
+
+bool ProxyScheduleCommand::IsFinished() { return m_finished; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp
new file mode 100644
index 0000000..0de739e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/RamseteCommand.h"
+
+using namespace frc2;
+using namespace units;
+
+template <typename T>
+int sgn(T val) {
+ return (T(0) < val) - (val < T(0));
+}
+
+RamseteCommand::RamseteCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller, volt_t ks,
+ units::unit_t<voltsecondspermeter> kv,
+ units::unit_t<voltsecondssquaredpermeter> ka,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<units::meters_per_second_t()> leftSpeed,
+ std::function<units::meters_per_second_t()> rightSpeed,
+ frc2::PIDController leftController, frc2::PIDController rightController,
+ std::function<void(volt_t, volt_t)> output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_controller(controller),
+ m_ks(ks),
+ m_kv(kv),
+ m_ka(ka),
+ m_kinematics(kinematics),
+ m_leftSpeed(leftSpeed),
+ m_rightSpeed(rightSpeed),
+ m_leftController(std::make_unique<frc2::PIDController>(leftController)),
+ m_rightController(std::make_unique<frc2::PIDController>(rightController)),
+ m_outputVolts(output) {
+ AddRequirements(requirements);
+}
+
+RamseteCommand::RamseteCommand(
+ frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_trajectory(trajectory),
+ m_pose(pose),
+ m_controller(controller),
+ m_ks(0),
+ m_kv(0),
+ m_ka(0),
+ m_kinematics(kinematics),
+ m_outputVel(output) {
+ AddRequirements(requirements);
+}
+
+void RamseteCommand::Initialize() {
+ m_prevTime = 0_s;
+ auto initialState = m_trajectory.Sample(0_s);
+ m_prevSpeeds = m_kinematics.ToWheelSpeeds(
+ frc::ChassisSpeeds{initialState.velocity, 0_mps,
+ initialState.velocity * initialState.curvature});
+ m_timer.Reset();
+ m_timer.Start();
+ m_leftController->Reset();
+ m_rightController->Reset();
+}
+
+void RamseteCommand::Execute() {
+ auto curTime = m_timer.Get();
+ auto dt = curTime - m_prevTime;
+
+ auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
+ m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
+
+ if (m_leftController.get() != nullptr) {
+ auto leftFeedforward =
+ m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
+ m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
+
+ auto rightFeedforward =
+ m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
+ m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
+
+ auto leftOutput =
+ volt_t(m_leftController->Calculate(
+ m_leftSpeed().to<double>(), targetWheelSpeeds.left.to<double>())) +
+ leftFeedforward;
+
+ auto rightOutput = volt_t(m_rightController->Calculate(
+ m_rightSpeed().to<double>(),
+ targetWheelSpeeds.right.to<double>())) +
+ rightFeedforward;
+
+ m_outputVolts(leftOutput, rightOutput);
+ } else {
+ m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
+ }
+
+ m_prevTime = curTime;
+ m_prevSpeeds = targetWheelSpeeds;
+}
+
+void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool RamseteCommand::IsFinished() {
+ return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp
new file mode 100644
index 0000000..5c2c755
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+
+RunCommand::RunCommand(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements)
+ : m_toRun{std::move(toRun)} {
+ AddRequirements(requirements);
+}
+
+void RunCommand::Execute() { m_toRun(); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
new file mode 100644
index 0000000..ea1ea8d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/ScheduleCommand.h"
+
+using namespace frc2;
+
+ScheduleCommand::ScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
+ SetInsert(m_toSchedule, toSchedule);
+}
+
+void ScheduleCommand::Initialize() {
+ for (auto command : m_toSchedule) {
+ command->Schedule();
+ }
+}
+
+bool ScheduleCommand::IsFinished() { return true; }
+
+bool ScheduleCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
new file mode 100644
index 0000000..1aa19e4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+
+SequentialCommandGroup::SequentialCommandGroup(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ AddCommands(std::move(commands));
+}
+
+void SequentialCommandGroup::Initialize() {
+ m_currentCommandIndex = 0;
+
+ if (!m_commands.empty()) {
+ m_commands[0]->Initialize();
+ }
+}
+
+void SequentialCommandGroup::Execute() {
+ if (m_commands.empty()) return;
+
+ auto& currentCommand = m_commands[m_currentCommandIndex];
+
+ currentCommand->Execute();
+ if (currentCommand->IsFinished()) {
+ currentCommand->End(false);
+ m_currentCommandIndex++;
+ if (m_currentCommandIndex < m_commands.size()) {
+ m_commands[m_currentCommandIndex]->Initialize();
+ }
+ }
+}
+
+void SequentialCommandGroup::End(bool interrupted) {
+ if (interrupted && !m_commands.empty() &&
+ m_currentCommandIndex != invalid_index &&
+ m_currentCommandIndex < m_commands.size()) {
+ m_commands[m_currentCommandIndex]->End(interrupted);
+ }
+ m_currentCommandIndex = invalid_index;
+}
+
+bool SequentialCommandGroup::IsFinished() {
+ return m_currentCommandIndex == m_commands.size();
+}
+
+bool SequentialCommandGroup::RunsWhenDisabled() const {
+ return m_runWhenDisabled;
+}
+
+void SequentialCommandGroup::AddCommands(
+ std::vector<std::unique_ptr<Command>>&& commands) {
+ if (!RequireUngrouped(commands)) {
+ return;
+ }
+
+ if (m_currentCommandIndex != invalid_index) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Commands cannot be added to a CommandGroup "
+ "while the group is running");
+ }
+
+ for (auto&& command : commands) {
+ command->SetGrouped(true);
+ AddRequirements(command->GetRequirements());
+ m_runWhenDisabled &= command->RunsWhenDisabled();
+ m_commands.emplace_back(std::move(command));
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp
new file mode 100644
index 0000000..33407bf
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/StartEndCommand.h"
+
+using namespace frc2;
+
+StartEndCommand::StartEndCommand(std::function<void()> onInit,
+ std::function<void()> onEnd,
+ std::initializer_list<Subsystem*> requirements)
+ : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} {
+ AddRequirements(requirements);
+}
+
+StartEndCommand::StartEndCommand(const StartEndCommand& other)
+ : CommandHelper(other) {
+ m_onInit = other.m_onInit;
+ m_onEnd = other.m_onEnd;
+}
+
+void StartEndCommand::Initialize() { m_onInit(); }
+
+void StartEndCommand::End(bool interrupted) { m_onEnd(); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp
new file mode 100644
index 0000000..cccb55b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/Subsystem.h"
+
+using namespace frc2;
+Subsystem::~Subsystem() {
+ CommandScheduler::GetInstance().UnregisterSubsystem(this);
+}
+
+void Subsystem::Periodic() {}
+
+Command* Subsystem::GetDefaultCommand() const {
+ return CommandScheduler::GetInstance().GetDefaultCommand(this);
+}
+
+Command* Subsystem::GetCurrentCommand() const {
+ return CommandScheduler::GetInstance().Requiring(this);
+}
+
+void Subsystem::Register() {
+ return CommandScheduler::GetInstance().RegisterSubsystem(this);
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp
new file mode 100644
index 0000000..226f080
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/SubsystemBase.h"
+
+#include <frc/smartdashboard/SendableBuilder.h>
+#include <frc/smartdashboard/SendableRegistry.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandScheduler.h>
+
+using namespace frc2;
+
+SubsystemBase::SubsystemBase() {
+ frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
+ CommandScheduler::GetInstance().RegisterSubsystem({this});
+}
+
+void SubsystemBase::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Subsystem");
+ builder.AddBooleanProperty(".hasDefault",
+ [this] { return GetDefaultCommand() != nullptr; },
+ nullptr);
+ builder.AddStringProperty(".default",
+ [this]() -> std::string {
+ auto command = GetDefaultCommand();
+ if (command == nullptr) return "none";
+ return command->GetName();
+ },
+ nullptr);
+ builder.AddBooleanProperty(".hasCommand",
+ [this] { return GetCurrentCommand() != nullptr; },
+ nullptr);
+ builder.AddStringProperty(".command",
+ [this]() -> std::string {
+ auto command = GetCurrentCommand();
+ if (command == nullptr) return "none";
+ return command->GetName();
+ },
+ nullptr);
+}
+
+std::string SubsystemBase::GetName() const {
+ return frc::SendableRegistry::GetInstance().GetName(this);
+}
+
+void SubsystemBase::SetName(const wpi::Twine& name) {
+ frc::SendableRegistry::GetInstance().SetName(this, name);
+}
+
+std::string SubsystemBase::GetSubsystem() const {
+ return frc::SendableRegistry::GetInstance().GetSubsystem(this);
+}
+
+void SubsystemBase::SetSubsystem(const wpi::Twine& name) {
+ frc::SendableRegistry::GetInstance().SetSubsystem(this, name);
+}
+
+void SubsystemBase::AddChild(std::string name, frc::Sendable* child) {
+ auto& registry = frc::SendableRegistry::GetInstance();
+ registry.AddLW(child, GetSubsystem(), name);
+ registry.AddChild(this, child);
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
new file mode 100644
index 0000000..bb17edc
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/TrapezoidProfileCommand.h"
+
+#include <units/units.h>
+
+using namespace frc2;
+
+TrapezoidProfileCommand::TrapezoidProfileCommand(
+ frc::TrapezoidProfile profile,
+ std::function<void(frc::TrapezoidProfile::State)> output,
+ std::initializer_list<Subsystem*> requirements)
+ : m_profile(profile), m_output(output) {
+ AddRequirements(requirements);
+}
+
+void TrapezoidProfileCommand::Initialize() {
+ m_timer.Reset();
+ m_timer.Start();
+}
+
+void TrapezoidProfileCommand::Execute() {
+ m_output(m_profile.Calculate(m_timer.Get()));
+}
+
+void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool TrapezoidProfileCommand::IsFinished() {
+ return m_timer.HasPeriodPassed(m_profile.TotalTime());
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp
new file mode 100644
index 0000000..1279d66
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/WaitCommand.h"
+
+using namespace frc2;
+
+WaitCommand::WaitCommand(units::second_t duration) : m_duration{duration} {
+ auto durationStr = std::to_string(duration.to<double>());
+ SetName(wpi::Twine(GetName()) + ": " + wpi::Twine(durationStr) + " seconds");
+}
+
+void WaitCommand::Initialize() {
+ m_timer.Reset();
+ m_timer.Start();
+}
+
+void WaitCommand::End(bool interrupted) { m_timer.Stop(); }
+
+bool WaitCommand::IsFinished() { return m_timer.HasPeriodPassed(m_duration); }
+
+bool WaitCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
new file mode 100644
index 0000000..d7a0daf
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+
+WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
+ : m_condition{std::move(condition)} {}
+
+WaitUntilCommand::WaitUntilCommand(double time)
+ : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0; }} {}
+
+bool WaitUntilCommand::IsFinished() { return m_condition(); }
+
+bool WaitUntilCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp b/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp
new file mode 100644
index 0000000..e519a9f
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/button/Button.h"
+
+using namespace frc2;
+
+Button::Button(std::function<bool()> isPressed) : Trigger(isPressed) {}
+
+Button Button::WhenPressed(Command* command, bool interruptible) {
+ WhenActive(command, interruptible);
+ return *this;
+}
+
+Button Button::WhenPressed(std::function<void()> toRun) {
+ WhenActive(std::move(toRun));
+ return *this;
+}
+
+Button Button::WhileHeld(Command* command, bool interruptible) {
+ WhileActiveContinous(command, interruptible);
+ return *this;
+}
+
+Button Button::WhileHeld(std::function<void()> toRun) {
+ WhileActiveContinous(std::move(toRun));
+ return *this;
+}
+
+Button Button::WhenHeld(Command* command, bool interruptible) {
+ WhileActiveOnce(command, interruptible);
+ return *this;
+}
+
+Button Button::WhenReleased(Command* command, bool interruptible) {
+ WhenInactive(command, interruptible);
+ return *this;
+}
+
+Button Button::WhenReleased(std::function<void()> toRun) {
+ WhenInactive(std::move(toRun));
+ return *this;
+}
+
+Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
+ ToggleWhenActive(command, interruptible);
+ return *this;
+}
+
+Button Button::CancelWhenPressed(Command* command) {
+ CancelWhenActive(command);
+ return *this;
+}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp
new file mode 100644
index 0000000..304bf98
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/command/button/Trigger.h"
+
+#include <frc2/command/InstantCommand.h>
+
+using namespace frc2;
+
+Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {}
+
+Trigger Trigger::WhenActive(Command* command, bool interruptible) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command, interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ command->Schedule(interruptible);
+ }
+
+ pressedLast = pressed;
+ });
+
+ return *this;
+}
+
+Trigger Trigger::WhenActive(std::function<void()> toRun) {
+ return WhenActive(InstantCommand(std::move(toRun), {}));
+}
+
+Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command, interruptible]() mutable {
+ bool pressed = Get();
+
+ if (pressed) {
+ command->Schedule(interruptible);
+ } else if (pressedLast && !pressed) {
+ command->Cancel();
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+}
+
+Trigger Trigger::WhileActiveContinous(std::function<void()> toRun) {
+ return WhileActiveContinous(InstantCommand(std::move(toRun), {}));
+}
+
+Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command, interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ command->Schedule(interruptible);
+ } else if (pressedLast && !pressed) {
+ command->Cancel();
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+}
+
+Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command, interruptible]() mutable {
+ bool pressed = Get();
+
+ if (pressedLast && !pressed) {
+ command->Schedule(interruptible);
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+}
+
+Trigger Trigger::WhenInactive(std::function<void()> toRun) {
+ return WhenInactive(InstantCommand(std::move(toRun), {}));
+}
+
+Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command, interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ if (command->IsScheduled()) {
+ command->Cancel();
+ } else {
+ command->Schedule(interruptible);
+ }
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+}
+
+Trigger Trigger::CancelWhenActive(Command* command) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this, command]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ command->Cancel();
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
new file mode 100644
index 0000000..42d20f7
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Pose2d.h"
+
+#include <cmath>
+
+using namespace frc;
+
+Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
+ : m_translation(translation), m_rotation(rotation) {}
+
+Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
+ : m_translation(x, y), m_rotation(rotation) {}
+
+Pose2d Pose2d::operator+(const Transform2d& other) const {
+ return TransformBy(other);
+}
+
+Pose2d& Pose2d::operator+=(const Transform2d& other) {
+ m_translation += other.Translation().RotateBy(m_rotation);
+ m_rotation += other.Rotation();
+ return *this;
+}
+
+Transform2d Pose2d::operator-(const Pose2d& other) const {
+ const auto pose = this->RelativeTo(other);
+ return Transform2d(pose.Translation(), pose.Rotation());
+}
+
+bool Pose2d::operator==(const Pose2d& other) const {
+ return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Pose2d::operator!=(const Pose2d& other) const {
+ return !operator==(other);
+}
+
+Pose2d Pose2d::TransformBy(const Transform2d& other) const {
+ return {m_translation + (other.Translation().RotateBy(m_rotation)),
+ m_rotation + other.Rotation()};
+}
+
+Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
+ const Transform2d transform{other, *this};
+ return {transform.Translation(), transform.Rotation()};
+}
+
+Pose2d Pose2d::Exp(const Twist2d& twist) const {
+ const auto dx = twist.dx;
+ const auto dy = twist.dy;
+ const auto dtheta = twist.dtheta.to<double>();
+
+ const auto sinTheta = std::sin(dtheta);
+ const auto cosTheta = std::cos(dtheta);
+
+ double s, c;
+ if (std::abs(dtheta) < 1E-9) {
+ s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+ c = 0.5 * dtheta;
+ } else {
+ s = sinTheta / dtheta;
+ c = (1 - cosTheta) / dtheta;
+ }
+
+ const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
+ Rotation2d{cosTheta, sinTheta}};
+
+ return *this + transform;
+}
+
+Twist2d Pose2d::Log(const Pose2d& end) const {
+ const auto transform = end.RelativeTo(*this);
+ const auto dtheta = transform.Rotation().Radians().to<double>();
+ const auto halfDtheta = dtheta / 2.0;
+
+ const auto cosMinusOne = transform.Rotation().Cos() - 1;
+
+ double halfThetaByTanOfHalfDtheta;
+
+ if (std::abs(cosMinusOne) < 1E-9) {
+ halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+ } else {
+ halfThetaByTanOfHalfDtheta =
+ -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
+ }
+
+ const Translation2d translationPart =
+ transform.Translation().RotateBy(
+ {halfThetaByTanOfHalfDtheta, -halfDtheta}) *
+ std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
+
+ return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
new file mode 100644
index 0000000..2723442
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Rotation2d.h"
+
+#include <cmath>
+
+using namespace frc;
+
+Rotation2d::Rotation2d(units::radian_t value)
+ : m_value(value),
+ m_cos(units::math::cos(value)),
+ m_sin(units::math::sin(value)) {}
+
+Rotation2d::Rotation2d(double x, double y) {
+ const auto magnitude = std::hypot(x, y);
+ if (magnitude > 1e-6) {
+ m_sin = y / magnitude;
+ m_cos = x / magnitude;
+ } else {
+ m_sin = 0.0;
+ m_cos = 1.0;
+ }
+ m_value = units::radian_t(std::atan2(m_sin, m_cos));
+}
+
+Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
+ return RotateBy(other);
+}
+
+Rotation2d& Rotation2d::operator+=(const Rotation2d& other) {
+ double cos = Cos() * other.Cos() - Sin() * other.Sin();
+ double sin = Cos() * other.Sin() + Sin() * other.Cos();
+ m_cos = cos;
+ m_sin = sin;
+ m_value = units::radian_t(std::atan2(m_sin, m_cos));
+ return *this;
+}
+
+Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
+ return *this + -other;
+}
+
+Rotation2d& Rotation2d::operator-=(const Rotation2d& other) {
+ *this += -other;
+ return *this;
+}
+
+Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); }
+
+Rotation2d Rotation2d::operator*(double scalar) const {
+ return Rotation2d(m_value * scalar);
+}
+
+bool Rotation2d::operator==(const Rotation2d& other) const {
+ return units::math::abs(m_value - other.m_value) < 1E-9_rad;
+}
+
+bool Rotation2d::operator!=(const Rotation2d& other) const {
+ return !operator==(other);
+}
+
+Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
+ return {Cos() * other.Cos() - Sin() * other.Sin(),
+ Cos() * other.Sin() + Sin() * other.Cos()};
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp b/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp
new file mode 100644
index 0000000..04f0419
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Transform2d.h"
+
+#include "frc/geometry/Pose2d.h"
+
+using namespace frc;
+
+Transform2d::Transform2d(Pose2d initial, Pose2d final) {
+ // We are rotating the difference between the translations
+ // using a clockwise rotation matrix. This transforms the global
+ // delta into a local delta (relative to the initial pose).
+ m_translation = (final.Translation() - initial.Translation())
+ .RotateBy(-initial.Rotation());
+
+ m_rotation = final.Rotation() - initial.Rotation();
+}
+
+Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
+ : m_translation(translation), m_rotation(rotation) {}
+
+bool Transform2d::operator==(const Transform2d& other) const {
+ return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Transform2d::operator!=(const Transform2d& other) const {
+ return !operator==(other);
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
new file mode 100644
index 0000000..ca287d1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Translation2d.h"
+
+using namespace frc;
+
+Translation2d::Translation2d(units::meter_t x, units::meter_t y)
+ : m_x(x), m_y(y) {}
+
+units::meter_t Translation2d::Distance(const Translation2d& other) const {
+ return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
+}
+
+units::meter_t Translation2d::Norm() const {
+ return units::math::hypot(m_x, m_y);
+}
+
+Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
+ return {m_x * other.Cos() - m_y * other.Sin(),
+ m_x * other.Sin() + m_y * other.Cos()};
+}
+
+Translation2d Translation2d::operator+(const Translation2d& other) const {
+ return {X() + other.X(), Y() + other.Y()};
+}
+
+Translation2d& Translation2d::operator+=(const Translation2d& other) {
+ m_x += other.m_x;
+ m_y += other.m_y;
+ return *this;
+}
+
+Translation2d Translation2d::operator-(const Translation2d& other) const {
+ return *this + -other;
+}
+
+Translation2d& Translation2d::operator-=(const Translation2d& other) {
+ *this += -other;
+ return *this;
+}
+
+Translation2d Translation2d::operator-() const { return {-m_x, -m_y}; }
+
+Translation2d Translation2d::operator*(double scalar) const {
+ return {scalar * m_x, scalar * m_y};
+}
+
+Translation2d& Translation2d::operator*=(double scalar) {
+ m_x *= scalar;
+ m_y *= scalar;
+ return *this;
+}
+
+Translation2d Translation2d::operator/(double scalar) const {
+ return *this * (1.0 / scalar);
+}
+
+bool Translation2d::operator==(const Translation2d& other) const {
+ return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+ units::math::abs(m_y - other.m_y) < 1E-9_m;
+}
+
+bool Translation2d::operator!=(const Translation2d& other) const {
+ return !operator==(other);
+}
+
+Translation2d& Translation2d::operator/=(double scalar) {
+ *this *= (1.0 / scalar);
+ return *this;
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp
new file mode 100644
index 0000000..8301481
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+
+using namespace frc;
+
+DifferentialDriveKinematics::DifferentialDriveKinematics(
+ units::meter_t trackWidth)
+ : m_trackWidth(trackWidth) {}
+
+ChassisSpeeds DifferentialDriveKinematics::ToChassisSpeeds(
+ const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
+ return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
+ (wheelSpeeds.right - wheelSpeeds.left) / m_trackWidth * 1_rad};
+}
+
+DifferentialDriveWheelSpeeds DifferentialDriveKinematics::ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds) const {
+ return {chassisSpeeds.vx - m_trackWidth / 2 * chassisSpeeds.omega / 1_rad,
+ chassisSpeeds.vx + m_trackWidth / 2 * chassisSpeeds.omega / 1_rad};
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
new file mode 100644
index 0000000..418dd0f
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/DifferentialDriveOdometry.h"
+
+using namespace frc;
+
+DifferentialDriveOdometry::DifferentialDriveOdometry(
+ DifferentialDriveKinematics kinematics, const Pose2d& initialPose)
+ : m_kinematics(kinematics), m_pose(initialPose) {
+ m_previousAngle = m_pose.Rotation();
+}
+
+const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
+ units::second_t currentTime, const Rotation2d& angle,
+ const DifferentialDriveWheelSpeeds& wheelSpeeds) {
+ units::second_t deltaTime =
+ (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
+ m_previousTime = currentTime;
+
+ auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+ static_cast<void>(dtheta);
+
+ auto newPose = m_pose.Exp(
+ {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+
+ m_previousAngle = angle;
+ m_pose = {newPose.Translation(), angle};
+
+ return m_pose;
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
new file mode 100644
index 0000000..5b13f0e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+using namespace frc;
+
+void DifferentialDriveWheelSpeeds::Normalize(
+ units::meters_per_second_t attainableMaxSpeed) {
+ auto realMaxSpeed =
+ units::math::max(units::math::abs(left), units::math::abs(right));
+
+ if (realMaxSpeed > attainableMaxSpeed) {
+ left = left / realMaxSpeed * attainableMaxSpeed;
+ right = right / realMaxSpeed * attainableMaxSpeed;
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
new file mode 100644
index 0000000..cf6ebb5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+
+using namespace frc;
+
+MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
+ // We have a new center of rotation. We need to compute the matrix again.
+ if (centerOfRotation != m_previousCoR) {
+ auto fl = m_frontLeftWheel - centerOfRotation;
+ auto fr = m_frontRightWheel - centerOfRotation;
+ auto rl = m_rearLeftWheel - centerOfRotation;
+ auto rr = m_rearRightWheel - centerOfRotation;
+
+ SetInverseKinematics(fl, fr, rl, rr);
+
+ m_previousCoR = centerOfRotation;
+ }
+
+ Eigen::Vector3d chassisSpeedsVector;
+ chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
+ chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+
+ Eigen::Matrix<double, 4, 1> wheelsMatrix =
+ m_inverseKinematics * chassisSpeedsVector;
+
+ MecanumDriveWheelSpeeds wheelSpeeds;
+ wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
+ wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
+ wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
+ wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
+ return wheelSpeeds;
+}
+
+ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
+ const MecanumDriveWheelSpeeds& wheelSpeeds) {
+ Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
+ // clang-format off
+ wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
+ wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
+ // clang-format on
+
+ Eigen::Vector3d chassisSpeedsVector =
+ m_forwardKinematics.solve(wheelSpeedsMatrix);
+
+ return {units::meters_per_second_t{chassisSpeedsVector(0)},
+ units::meters_per_second_t{chassisSpeedsVector(1)},
+ units::radians_per_second_t{chassisSpeedsVector(2)}};
+}
+
+void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
+ Translation2d fr,
+ Translation2d rl,
+ Translation2d rr) {
+ // clang-format off
+ m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
+ 1, 1, (fr.X() - fr.Y()).template to<double>(),
+ 1, 1, (rl.X() - rl.Y()).template to<double>(),
+ 1, -1, (-(rr.X() + rr.Y())).template to<double>();
+ // clang-format on
+ m_inverseKinematics /= std::sqrt(2);
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
new file mode 100644
index 0000000..18e5635
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveOdometry.h"
+
+using namespace frc;
+
+MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+ const Pose2d& initialPose)
+ : m_kinematics(kinematics), m_pose(initialPose) {
+ m_previousAngle = m_pose.Rotation();
+}
+
+const Pose2d& MecanumDriveOdometry::UpdateWithTime(
+ units::second_t currentTime, const Rotation2d& angle,
+ MecanumDriveWheelSpeeds wheelSpeeds) {
+ units::second_t deltaTime =
+ (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
+ m_previousTime = currentTime;
+
+ auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+ static_cast<void>(dtheta);
+
+ auto newPose = m_pose.Exp(
+ {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+
+ m_previousAngle = angle;
+ m_pose = {newPose.Translation(), angle};
+
+ return m_pose;
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
new file mode 100644
index 0000000..1641ba8
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+
+using namespace frc;
+
+void MecanumDriveWheelSpeeds::Normalize(
+ units::meters_per_second_t attainableMaxSpeed) {
+ std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
+ rearLeft, rearRight};
+ units::meters_per_second_t realMaxSpeed = *std::max_element(
+ wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
+ return units::math::abs(a) < units::math::abs(b);
+ });
+
+ if (realMaxSpeed > attainableMaxSpeed) {
+ for (int i = 0; i < 4; ++i) {
+ wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
+ }
+ frontLeft = wheelSpeeds[0];
+ frontRight = wheelSpeeds[1];
+ rearLeft = wheelSpeeds[2];
+ rearRight = wheelSpeeds[3];
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
index 135d044..1f011f5 100644
--- a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2012-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2012-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,18 +7,14 @@
#include "frc/livewindow/LiveWindow.h"
-#include <algorithm>
-
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableInstance.h>
-#include <wpi/DenseMap.h>
-#include <wpi/SmallString.h>
#include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
#include "frc/commands/Scheduler.h"
#include "frc/smartdashboard/SendableBuilderImpl.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -28,16 +24,14 @@
Impl();
struct Component {
- std::shared_ptr<Sendable> sendable;
- Sendable* parent = nullptr;
- SendableBuilderImpl builder;
bool firstTime = true;
bool telemetryEnabled = true;
};
wpi::mutex mutex;
- wpi::DenseMap<void*, Component> components;
+ SendableRegistry& registry;
+ int dataHandle;
std::shared_ptr<nt::NetworkTable> liveWindowTable;
std::shared_ptr<nt::NetworkTable> statusTable;
@@ -46,133 +40,64 @@
bool startLiveWindow = false;
bool liveWindowEnabled = false;
bool telemetryEnabled = true;
+
+ std::shared_ptr<Component> GetOrAdd(Sendable* sendable);
};
LiveWindow::Impl::Impl()
- : liveWindowTable(
+ : registry(SendableRegistry::GetInstance()),
+ dataHandle(registry.GetDataHandle()),
+ liveWindowTable(
nt::NetworkTableInstance::GetDefault().GetTable("LiveWindow")) {
statusTable = liveWindowTable->GetSubTable(".status");
enabledEntry = statusTable->GetEntry("LW Enabled");
}
+std::shared_ptr<LiveWindow::Impl::Component> LiveWindow::Impl::GetOrAdd(
+ Sendable* sendable) {
+ auto data = std::static_pointer_cast<Component>(
+ registry.GetData(sendable, dataHandle));
+ if (!data) {
+ data = std::make_shared<Component>();
+ registry.SetData(sendable, dataHandle, data);
+ }
+ return data;
+}
+
LiveWindow* LiveWindow::GetInstance() {
static LiveWindow instance;
return &instance;
}
-void LiveWindow::Run() { UpdateValues(); }
-
-void LiveWindow::AddSensor(const wpi::Twine& subsystem, const wpi::Twine& name,
- Sendable* component) {
- Add(component);
- component->SetName(subsystem, name);
-}
-
-void LiveWindow::AddSensor(const wpi::Twine& subsystem, const wpi::Twine& name,
- Sendable& component) {
- Add(&component);
- component.SetName(subsystem, name);
-}
-
-void LiveWindow::AddSensor(const wpi::Twine& subsystem, const wpi::Twine& name,
- std::shared_ptr<Sendable> component) {
- Add(component);
- component->SetName(subsystem, name);
-}
-
-void LiveWindow::AddActuator(const wpi::Twine& subsystem,
- const wpi::Twine& name, Sendable* component) {
- Add(component);
- component->SetName(subsystem, name);
-}
-
-void LiveWindow::AddActuator(const wpi::Twine& subsystem,
- const wpi::Twine& name, Sendable& component) {
- Add(&component);
- component.SetName(subsystem, name);
-}
-
-void LiveWindow::AddActuator(const wpi::Twine& subsystem,
- const wpi::Twine& name,
- std::shared_ptr<Sendable> component) {
- Add(component);
- component->SetName(subsystem, name);
-}
-
-void LiveWindow::AddSensor(const wpi::Twine& type, int channel,
- Sendable* component) {
- Add(component);
- component->SetName("Ungrouped",
- type + Twine('[') + Twine(channel) + Twine(']'));
-}
-
-void LiveWindow::AddActuator(const wpi::Twine& type, int channel,
- Sendable* component) {
- Add(component);
- component->SetName("Ungrouped",
- type + Twine('[') + Twine(channel) + Twine(']'));
-}
-
-void LiveWindow::AddActuator(const wpi::Twine& type, int module, int channel,
- Sendable* component) {
- Add(component);
- component->SetName("Ungrouped", type + Twine('[') + Twine(module) +
- Twine(',') + Twine(channel) + Twine(']'));
-}
-
-void LiveWindow::Add(std::shared_ptr<Sendable> sendable) {
- std::lock_guard<wpi::mutex> lock(m_impl->mutex);
- auto& comp = m_impl->components[sendable.get()];
- comp.sendable = sendable;
-}
-
-void LiveWindow::Add(Sendable* sendable) {
- Add(std::shared_ptr<Sendable>(sendable, NullDeleter<Sendable>()));
-}
-
-void LiveWindow::AddChild(Sendable* parent, std::shared_ptr<Sendable> child) {
- AddChild(parent, child.get());
-}
-
-void LiveWindow::AddChild(Sendable* parent, void* child) {
- std::lock_guard<wpi::mutex> lock(m_impl->mutex);
- auto& comp = m_impl->components[child];
- comp.parent = parent;
- comp.telemetryEnabled = false;
-}
-
-void LiveWindow::Remove(Sendable* sendable) {
- std::lock_guard<wpi::mutex> lock(m_impl->mutex);
- m_impl->components.erase(sendable);
-}
-
void LiveWindow::EnableTelemetry(Sendable* sendable) {
- std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ std::scoped_lock lock(m_impl->mutex);
// Re-enable global setting in case DisableAllTelemetry() was called.
m_impl->telemetryEnabled = true;
- auto i = m_impl->components.find(sendable);
- if (i != m_impl->components.end()) i->getSecond().telemetryEnabled = true;
+ m_impl->GetOrAdd(sendable)->telemetryEnabled = true;
}
void LiveWindow::DisableTelemetry(Sendable* sendable) {
- std::lock_guard<wpi::mutex> lock(m_impl->mutex);
- auto i = m_impl->components.find(sendable);
- if (i != m_impl->components.end()) i->getSecond().telemetryEnabled = false;
+ std::scoped_lock lock(m_impl->mutex);
+ m_impl->GetOrAdd(sendable)->telemetryEnabled = false;
}
void LiveWindow::DisableAllTelemetry() {
- std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ std::scoped_lock lock(m_impl->mutex);
m_impl->telemetryEnabled = false;
- for (auto& i : m_impl->components) i.getSecond().telemetryEnabled = false;
+ m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
+ if (!cbdata.data) cbdata.data = std::make_shared<Impl::Component>();
+ std::static_pointer_cast<Impl::Component>(cbdata.data)->telemetryEnabled =
+ false;
+ });
}
bool LiveWindow::IsEnabled() const {
- std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ std::scoped_lock lock(m_impl->mutex);
return m_impl->liveWindowEnabled;
}
void LiveWindow::SetEnabled(bool enabled) {
- std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ std::scoped_lock lock(m_impl->mutex);
if (m_impl->liveWindowEnabled == enabled) return;
Scheduler* scheduler = Scheduler::GetInstance();
m_impl->startLiveWindow = enabled;
@@ -183,16 +108,16 @@
scheduler->SetEnabled(false);
scheduler->RemoveAll();
} else {
- for (auto& i : m_impl->components) {
- i.getSecond().builder.StopLiveWindowMode();
- }
+ m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
+ cbdata.builder.StopLiveWindowMode();
+ });
scheduler->SetEnabled(true);
}
m_impl->enabledEntry.SetBoolean(enabled);
}
void LiveWindow::UpdateValues() {
- std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ std::scoped_lock lock(m_impl->mutex);
UpdateValuesUnsafe();
}
@@ -200,37 +125,39 @@
// Only do this if either LiveWindow mode or telemetry is enabled.
if (!m_impl->liveWindowEnabled && !m_impl->telemetryEnabled) return;
- for (auto& i : m_impl->components) {
- auto& comp = i.getSecond();
- if (comp.sendable && !comp.parent &&
- (m_impl->liveWindowEnabled || comp.telemetryEnabled)) {
- if (comp.firstTime) {
- // By holding off creating the NetworkTable entries, it allows the
- // components to be redefined. This allows default sensor and actuator
- // values to be created that are replaced with the custom names from
- // users calling setName.
- auto name = comp.sendable->GetName();
- if (name.empty()) continue;
- auto subsystem = comp.sendable->GetSubsystem();
- auto ssTable = m_impl->liveWindowTable->GetSubTable(subsystem);
- std::shared_ptr<NetworkTable> table;
- // Treat name==subsystem as top level of subsystem
- if (name == subsystem)
- table = ssTable;
- else
- table = ssTable->GetSubTable(name);
- table->GetEntry(".name").SetString(name);
- comp.builder.SetTable(table);
- comp.sendable->InitSendable(comp.builder);
- ssTable->GetEntry(".type").SetString("LW Subsystem");
+ m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
+ if (!cbdata.sendable || cbdata.parent) return;
- comp.firstTime = false;
- }
+ if (!cbdata.data) cbdata.data = std::make_shared<Impl::Component>();
- if (m_impl->startLiveWindow) comp.builder.StartLiveWindowMode();
- comp.builder.UpdateTable();
+ auto& comp = *std::static_pointer_cast<Impl::Component>(cbdata.data);
+
+ if (!m_impl->liveWindowEnabled && !comp.telemetryEnabled) return;
+
+ if (comp.firstTime) {
+ // By holding off creating the NetworkTable entries, it allows the
+ // components to be redefined. This allows default sensor and actuator
+ // values to be created that are replaced with the custom names from
+ // users calling setName.
+ if (cbdata.name.empty()) return;
+ auto ssTable = m_impl->liveWindowTable->GetSubTable(cbdata.subsystem);
+ std::shared_ptr<NetworkTable> table;
+ // Treat name==subsystem as top level of subsystem
+ if (cbdata.name == cbdata.subsystem)
+ table = ssTable;
+ else
+ table = ssTable->GetSubTable(cbdata.name);
+ table->GetEntry(".name").SetString(cbdata.name);
+ cbdata.builder.SetTable(table);
+ cbdata.sendable->InitSendable(cbdata.builder);
+ ssTable->GetEntry(".type").SetString("LW Subsystem");
+
+ comp.firstTime = false;
}
- }
+
+ if (m_impl->startLiveWindow) cbdata.builder.StartLiveWindowMode();
+ cbdata.builder.UpdateTable();
+ });
m_impl->startLiveWindow = false;
}
diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindowSendable.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindowSendable.cpp
deleted file mode 100644
index 7f03f52..0000000
--- a/wpilibc/src/main/native/cpp/livewindow/LiveWindowSendable.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/livewindow/LiveWindowSendable.h"
-
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
-
-std::string LiveWindowSendable::GetName() const { return std::string(); }
-
-void LiveWindowSendable::SetName(const wpi::Twine&) {}
-
-std::string LiveWindowSendable::GetSubsystem() const { return std::string(); }
-
-void LiveWindowSendable::SetSubsystem(const wpi::Twine&) {}
-
-void LiveWindowSendable::InitSendable(SendableBuilder& builder) {
- builder.SetUpdateTable([=]() { UpdateTable(); });
-}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp b/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp
index 294be79..d80001e 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -44,7 +44,6 @@
DriverStation::ReportError("Shuffleboard event name was not specified");
return;
}
- auto arr = wpi::ArrayRef<std::string>{
- description, ShuffleboardEventImportanceName(importance)};
- m_eventsTable->GetSubTable(name)->GetEntry("Info").SetStringArray(arr);
+ m_eventsTable->GetSubTable(name)->GetEntry("Info").SetStringArray(
+ {description, ShuffleboardEventImportanceName(importance)});
}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
index 87d7e9d..b6f8ce9 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,40 +7,30 @@
#include "frc/shuffleboard/SendableCameraWrapper.h"
-#include <cscore_oo.h>
+#include <functional>
+#include <memory>
+#include <string>
+
#include <wpi/DenseMap.h>
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
-using namespace frc;
-
-namespace {
-constexpr const char* kProtocol = "camera_server://";
-wpi::DenseMap<int, std::unique_ptr<SendableCameraWrapper>> wrappers;
-} // namespace
-
-SendableCameraWrapper& SendableCameraWrapper::Wrap(
- const cs::VideoSource& source) {
- return Wrap(source.GetHandle());
+namespace frc {
+namespace detail {
+std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
+ CS_Source source) {
+ static wpi::DenseMap<int, std::shared_ptr<SendableCameraWrapper>> wrappers;
+ return wrappers[static_cast<int>(source)];
}
-SendableCameraWrapper& SendableCameraWrapper::Wrap(CS_Source source) {
- auto& wrapper = wrappers[static_cast<int>(source)];
- if (!wrapper)
- wrapper = std::make_unique<SendableCameraWrapper>(source, private_init{});
- return *wrapper;
+void AddToSendableRegistry(frc::Sendable* sendable, std::string name) {
+ SendableRegistry::GetInstance().Add(sendable, name);
}
-
-SendableCameraWrapper::SendableCameraWrapper(CS_Source source,
- const private_init&)
- : SendableBase(false), m_uri(kProtocol) {
- CS_Status status = 0;
- auto name = cs::GetSourceName(source, &status);
- SetName(name);
- m_uri += name;
-}
+} // namespace detail
void SendableCameraWrapper::InitSendable(SendableBuilder& builder) {
builder.AddStringProperty(".ShuffleboardURI", [this] { return m_uri; },
nullptr);
}
+} // namespace frc
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
index f13116d..bb9dc9e 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,10 +11,10 @@
#include <wpi/raw_ostream.h>
#include "frc/shuffleboard/ComplexWidget.h"
-#include "frc/shuffleboard/SendableCameraWrapper.h"
#include "frc/shuffleboard/ShuffleboardComponent.h"
#include "frc/shuffleboard/ShuffleboardLayout.h"
#include "frc/shuffleboard/SimpleWidget.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
@@ -74,20 +74,12 @@
return *ptr;
}
-ComplexWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
- const cs::VideoSource& video) {
- return Add(title, SendableCameraWrapper::Wrap(video));
-}
-
ComplexWidget& ShuffleboardContainer::Add(Sendable& sendable) {
- if (sendable.GetName().empty()) {
+ auto name = SendableRegistry::GetInstance().GetName(&sendable);
+ if (name.empty()) {
wpi::outs() << "Sendable must have a name\n";
}
- return Add(sendable.GetName(), sendable);
-}
-
-ComplexWidget& ShuffleboardContainer::Add(const cs::VideoSource& video) {
- return Add(SendableCameraWrapper::Wrap(video));
+ return Add(name, sendable);
}
SimpleWidget& ShuffleboardContainer::Add(
@@ -141,6 +133,108 @@
return Add(title, nt::Value::MakeStringArray(defaultValue));
}
+SuppliedValueWidget<std::string>& ShuffleboardContainer::AddString(
+ const wpi::Twine& title, std::function<std::string()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry, std::string value) {
+ entry.SetString(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<std::string>>(
+ *this, title, supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<double>& ShuffleboardContainer::AddNumber(
+ const wpi::Twine& title, std::function<double()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry, double value) {
+ entry.SetDouble(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<double>>(*this, title,
+ supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<bool>& ShuffleboardContainer::AddBoolean(
+ const wpi::Twine& title, std::function<bool()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry, bool value) {
+ entry.SetBoolean(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<bool>>(*this, title,
+ supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<std::vector<std::string>>&
+ShuffleboardContainer::AddStringArray(
+ const wpi::Twine& title,
+ std::function<std::vector<std::string>()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry,
+ std::vector<std::string> value) {
+ entry.SetStringArray(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<std::vector<std::string>>>(
+ *this, title, supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<std::vector<double>>& ShuffleboardContainer::AddNumberArray(
+ const wpi::Twine& title, std::function<std::vector<double>()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry,
+ std::vector<double> value) {
+ entry.SetDoubleArray(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<std::vector<double>>>(
+ *this, title, supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<std::vector<int>>& ShuffleboardContainer::AddBooleanArray(
+ const wpi::Twine& title, std::function<std::vector<int>()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry, std::vector<int> value) {
+ entry.SetBooleanArray(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<std::vector<int>>>(
+ *this, title, supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<wpi::StringRef>& ShuffleboardContainer::AddRaw(
+ const wpi::Twine& title, std::function<wpi::StringRef()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry, wpi::StringRef value) {
+ entry.SetRaw(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<wpi::StringRef>>(
+ *this, title, supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
SimpleWidget& ShuffleboardContainer::AddPersistent(
const wpi::Twine& title, std::shared_ptr<nt::Value> defaultValue) {
auto& widget = Add(title, defaultValue);
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
index 39e5778..a50b77f 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,7 @@
#include "frc/shuffleboard/ShuffleboardInstance.h"
+#include <hal/HAL.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/StringMap.h>
@@ -27,6 +28,7 @@
: m_impl(new Impl) {
m_impl->rootTable = ntInstance.GetTable(Shuffleboard::kBaseTableName);
m_impl->rootMetaTable = m_impl->rootTable->GetSubTable(".metadata");
+ HAL_Report(HALUsageReporting::kResourceType_Shuffleboard, 0);
}
ShuffleboardInstance::~ShuffleboardInstance() {}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
index 4573516..3b79a9c 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
@@ -1,41 +1,41 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/shuffleboard/ShuffleboardWidget.h"
-
-using namespace frc;
-
-static constexpr const char* widgetStrings[] = {
- "Text View",
- "Number Slider",
- "Number Bar",
- "Simple Dial",
- "Graph",
- "Boolean Box",
- "Toggle Button",
- "Toggle Switch",
- "Voltage View",
- "PDP",
- "ComboBox Chooser",
- "Split Button Chooser",
- "Encoder",
- "Speed Controller",
- "Command",
- "PID Command",
- "PID Controller",
- "Accelerometer",
- "3-Axis Accelerometer",
- "Gyro",
- "Relay",
- "Differential Drivebase",
- "Mecanum Drivebase",
- "Camera Stream",
-};
-
-const char* detail::GetStringForWidgetType(BuiltInWidgets type) {
- return widgetStrings[static_cast<int>(type)];
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/ShuffleboardWidget.h"
+
+using namespace frc;
+
+static constexpr const char* widgetStrings[] = {
+ "Text View",
+ "Number Slider",
+ "Number Bar",
+ "Simple Dial",
+ "Graph",
+ "Boolean Box",
+ "Toggle Button",
+ "Toggle Switch",
+ "Voltage View",
+ "PDP",
+ "ComboBox Chooser",
+ "Split Button Chooser",
+ "Encoder",
+ "Speed Controller",
+ "Command",
+ "PID Command",
+ "PID Controller",
+ "Accelerometer",
+ "3-Axis Accelerometer",
+ "Gyro",
+ "Relay",
+ "Differential Drivebase",
+ "Mecanum Drivebase",
+ "Camera Stream",
+};
+
+const char* detail::GetStringForWidgetType(BuiltInWidgets type) {
+ return widgetStrings[static_cast<int>(type)];
+}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/ListenerExecutor.cpp b/wpilibc/src/main/native/cpp/smartdashboard/ListenerExecutor.cpp
new file mode 100644
index 0000000..75b373a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/ListenerExecutor.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/ListenerExecutor.h"
+
+using namespace frc::detail;
+
+void ListenerExecutor::Execute(std::function<void()> task) {
+ std::scoped_lock lock(m_lock);
+ m_tasks.emplace_back(task);
+}
+
+void ListenerExecutor::RunListenerTasks() {
+ // Locally copy tasks from internal list; minimizes blocking time
+ {
+ std::scoped_lock lock(m_lock);
+ std::swap(m_tasks, m_runningTasks);
+ }
+
+ for (auto&& task : m_runningTasks) {
+ task();
+ }
+ m_runningTasks.clear();
+}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/NamedSendable.cpp b/wpilibc/src/main/native/cpp/smartdashboard/NamedSendable.cpp
deleted file mode 100644
index 61028b3..0000000
--- a/wpilibc/src/main/native/cpp/smartdashboard/NamedSendable.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/smartdashboard/NamedSendable.h"
-
-using namespace frc;
-
-void NamedSendable::SetName(const wpi::Twine&) {}
-
-std::string NamedSendable::GetSubsystem() const { return std::string(); }
-
-void NamedSendable::SetSubsystem(const wpi::Twine&) {}
-
-void NamedSendable::InitSendable(SendableBuilder&) {}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp
index 28bd7fd..7fc8635 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,66 +7,13 @@
#include "frc/smartdashboard/SendableBase.h"
-#include <utility>
-
-#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
SendableBase::SendableBase(bool addLiveWindow) {
- if (addLiveWindow) LiveWindow::GetInstance()->Add(this);
-}
-
-SendableBase::~SendableBase() { LiveWindow::GetInstance()->Remove(this); }
-
-SendableBase::SendableBase(SendableBase&& rhs) {
- m_name = std::move(rhs.m_name);
- m_subsystem = std::move(rhs.m_subsystem);
-}
-
-SendableBase& SendableBase::operator=(SendableBase&& rhs) {
- Sendable::operator=(std::move(rhs));
-
- m_name = std::move(rhs.m_name);
- m_subsystem = std::move(rhs.m_subsystem);
-
- return *this;
-}
-
-std::string SendableBase::GetName() const {
- std::lock_guard<wpi::mutex> lock(m_mutex);
- return m_name;
-}
-
-void SendableBase::SetName(const wpi::Twine& name) {
- std::lock_guard<wpi::mutex> lock(m_mutex);
- m_name = name.str();
-}
-
-std::string SendableBase::GetSubsystem() const {
- std::lock_guard<wpi::mutex> lock(m_mutex);
- return m_subsystem;
-}
-
-void SendableBase::SetSubsystem(const wpi::Twine& subsystem) {
- std::lock_guard<wpi::mutex> lock(m_mutex);
- m_subsystem = subsystem.str();
-}
-
-void SendableBase::AddChild(std::shared_ptr<Sendable> child) {
- LiveWindow::GetInstance()->AddChild(this, child);
-}
-
-void SendableBase::AddChild(void* child) {
- LiveWindow::GetInstance()->AddChild(this, child);
-}
-
-void SendableBase::SetName(const wpi::Twine& moduleType, int channel) {
- SetName(moduleType + wpi::Twine('[') + wpi::Twine(channel) + wpi::Twine(']'));
-}
-
-void SendableBase::SetName(const wpi::Twine& moduleType, int moduleNumber,
- int channel) {
- SetName(moduleType + wpi::Twine('[') + wpi::Twine(moduleNumber) +
- wpi::Twine(',') + wpi::Twine(channel) + wpi::Twine(']'));
+ if (addLiveWindow)
+ SendableRegistry::GetInstance().AddLW(this, "");
+ else
+ SendableRegistry::GetInstance().Add(this, "");
}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
index bce03b5..d075deb 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,6 +10,8 @@
#include <ntcore_cpp.h>
#include <wpi/SmallString.h>
+#include "frc/smartdashboard/SmartDashboard.h"
+
using namespace frc;
void SendableBuilderImpl::SetTable(std::shared_ptr<nt::NetworkTable> table) {
@@ -21,6 +23,8 @@
return m_table;
}
+bool SendableBuilderImpl::HasTable() const { return m_table != nullptr; }
+
bool SendableBuilderImpl::IsActuator() const { return m_actuator; }
void SendableBuilderImpl::UpdateTable() {
@@ -51,6 +55,8 @@
if (m_safeState) m_safeState();
}
+void SendableBuilderImpl::ClearProperties() { m_properties.clear(); }
+
void SendableBuilderImpl::SetSmartDashboardType(const wpi::Twine& type) {
m_table->GetEntry(".type").SetString(type);
}
@@ -88,7 +94,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsBoolean()) return;
- setter(event.value->GetBoolean());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetBoolean()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -111,7 +118,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsDouble()) return;
- setter(event.value->GetDouble());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetDouble()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -134,7 +142,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsString()) return;
- setter(event.value->GetString());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetString()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -157,7 +166,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsBooleanArray()) return;
- setter(event.value->GetBooleanArray());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetBooleanArray()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -180,7 +190,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsDoubleArray()) return;
- setter(event.value->GetDoubleArray());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetDoubleArray()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -203,7 +214,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsStringArray()) return;
- setter(event.value->GetStringArray());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetStringArray()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -226,7 +238,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsRaw()) return;
- setter(event.value->GetRaw());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetRaw()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -247,7 +260,9 @@
m_properties.back().createListener =
[=](nt::NetworkTableEntry entry) -> NT_EntryListener {
return entry.AddListener(
- [=](const nt::EntryNotification& event) { setter(event.value); },
+ [=](const nt::EntryNotification& event) {
+ SmartDashboard::PostListenerTask([=] { setter(event.value); });
+ },
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
}
@@ -271,7 +286,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsString()) return;
- setter(event.value->GetString());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetString()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -296,7 +312,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsBooleanArray()) return;
- setter(event.value->GetBooleanArray());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetBooleanArray()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -322,7 +339,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsDoubleArray()) return;
- setter(event.value->GetDoubleArray());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetDoubleArray()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -349,7 +367,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsStringArray()) return;
- setter(event.value->GetStringArray());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetStringArray()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
@@ -374,7 +393,8 @@
return entry.AddListener(
[=](const nt::EntryNotification& event) {
if (!event.value->IsRaw()) return;
- setter(event.value->GetRaw());
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetRaw()); });
},
NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
};
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp
index 2cdf92c..2f7af9c 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,9 +7,31 @@
#include "frc/smartdashboard/SendableChooserBase.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
std::atomic_int SendableChooserBase::s_instances{0};
-SendableChooserBase::SendableChooserBase()
- : SendableBase(false), m_instance{s_instances++} {}
+SendableChooserBase::SendableChooserBase() : m_instance{s_instances++} {
+ SendableRegistry::GetInstance().Add(this, "SendableChooser", m_instance);
+}
+
+SendableChooserBase::SendableChooserBase(SendableChooserBase&& oth)
+ : SendableHelper(std::move(oth)),
+ m_defaultChoice(std::move(oth.m_defaultChoice)),
+ m_selected(std::move(oth.m_selected)),
+ m_haveSelected(std::move(oth.m_haveSelected)),
+ m_activeEntries(std::move(oth.m_activeEntries)),
+ m_instance(std::move(oth.m_instance)) {}
+
+SendableChooserBase& SendableChooserBase::operator=(SendableChooserBase&& oth) {
+ SendableHelper::operator=(oth);
+ std::scoped_lock lock(m_mutex, oth.m_mutex);
+ m_defaultChoice = std::move(oth.m_defaultChoice);
+ m_selected = std::move(oth.m_selected);
+ m_haveSelected = std::move(oth.m_haveSelected);
+ m_activeEntries = std::move(oth.m_activeEntries);
+ m_instance = std::move(oth.m_instance);
+ return *this;
+}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
new file mode 100644
index 0000000..56a27dc
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
@@ -0,0 +1,325 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+#include <memory>
+
+#include <wpi/DenseMap.h>
+#include <wpi/SmallVector.h>
+#include <wpi/UidVector.h>
+#include <wpi/mutex.h>
+
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilderImpl.h"
+
+using namespace frc;
+
+struct SendableRegistry::Impl {
+ struct Component {
+ Sendable* sendable = nullptr;
+ SendableBuilderImpl builder;
+ std::string name;
+ std::string subsystem = "Ungrouped";
+ Sendable* parent = nullptr;
+ bool liveWindow = false;
+ wpi::SmallVector<std::shared_ptr<void>, 2> data;
+
+ void SetName(const wpi::Twine& moduleType, int channel) {
+ name =
+ (moduleType + wpi::Twine('[') + wpi::Twine(channel) + wpi::Twine(']'))
+ .str();
+ }
+
+ void SetName(const wpi::Twine& moduleType, int moduleNumber, int channel) {
+ name = (moduleType + wpi::Twine('[') + wpi::Twine(moduleNumber) +
+ wpi::Twine(',') + wpi::Twine(channel) + wpi::Twine(']'))
+ .str();
+ }
+ };
+
+ wpi::mutex mutex;
+
+ wpi::UidVector<std::unique_ptr<Component>, 32> components;
+ wpi::DenseMap<void*, UID> componentMap;
+ int nextDataHandle = 0;
+
+ Component& GetOrAdd(void* sendable, UID* uid = nullptr);
+};
+
+SendableRegistry::Impl::Component& SendableRegistry::Impl::GetOrAdd(
+ void* sendable, UID* uid) {
+ UID& compUid = componentMap[sendable];
+ if (compUid == 0)
+ compUid = components.emplace_back(std::make_unique<Component>()) + 1;
+ if (uid) *uid = compUid;
+
+ return *components[compUid - 1];
+}
+
+SendableRegistry& SendableRegistry::GetInstance() {
+ static SendableRegistry instance;
+ return instance;
+}
+
+void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.name = name.str();
+}
+
+void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& moduleType,
+ int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.SetName(moduleType, channel);
+}
+
+void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& moduleType,
+ int moduleNumber, int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.SetName(moduleType, moduleNumber, channel);
+}
+
+void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& subsystem,
+ const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.name = name.str();
+ comp.subsystem = subsystem.str();
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.liveWindow = true;
+ comp.name = name.str();
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& moduleType,
+ int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.liveWindow = true;
+ comp.SetName(moduleType, channel);
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& moduleType,
+ int moduleNumber, int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.liveWindow = true;
+ comp.SetName(moduleType, moduleNumber, channel);
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& subsystem,
+ const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.liveWindow = true;
+ comp.name = name.str();
+ comp.subsystem = subsystem.str();
+}
+
+void SendableRegistry::AddChild(Sendable* parent, void* child) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(child);
+ comp.parent = parent;
+}
+
+bool SendableRegistry::Remove(Sendable* sendable) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return false;
+ UID compUid = it->getSecond();
+ m_impl->components.erase(compUid - 1);
+ m_impl->componentMap.erase(it);
+ return true;
+}
+
+void SendableRegistry::Move(Sendable* to, Sendable* from) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(from);
+ if (it == m_impl->componentMap.end()) return;
+ UID compUid = it->getSecond();
+ m_impl->componentMap.erase(it);
+ m_impl->componentMap[to] = compUid;
+ auto& comp = *m_impl->components[compUid - 1];
+ comp.sendable = to;
+ if (comp.builder.HasTable()) {
+ // rebuild builder, as lambda captures can point to "from"
+ comp.builder.ClearProperties();
+ to->InitSendable(comp.builder);
+ }
+}
+
+bool SendableRegistry::Contains(const Sendable* sendable) const {
+ std::scoped_lock lock(m_impl->mutex);
+ return m_impl->componentMap.count(sendable) != 0;
+}
+
+std::string SendableRegistry::GetName(const Sendable* sendable) const {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return std::string{};
+ return m_impl->components[it->getSecond() - 1]->name;
+}
+
+void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->name = name.str();
+}
+
+void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
+ int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->SetName(moduleType, channel);
+}
+
+void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
+ int moduleNumber, int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->SetName(moduleType, moduleNumber,
+ channel);
+}
+
+void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& subsystem,
+ const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ auto& comp = *m_impl->components[it->getSecond() - 1];
+ comp.name = name.str();
+ comp.subsystem = subsystem.str();
+}
+
+std::string SendableRegistry::GetSubsystem(const Sendable* sendable) const {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return std::string{};
+ return m_impl->components[it->getSecond() - 1]->subsystem;
+}
+
+void SendableRegistry::SetSubsystem(Sendable* sendable,
+ const wpi::Twine& subsystem) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->subsystem = subsystem.str();
+}
+
+int SendableRegistry::GetDataHandle() {
+ std::scoped_lock lock(m_impl->mutex);
+ return m_impl->nextDataHandle++;
+}
+
+std::shared_ptr<void> SendableRegistry::SetData(Sendable* sendable, int handle,
+ std::shared_ptr<void> data) {
+ assert(handle >= 0);
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return nullptr;
+ auto& comp = *m_impl->components[it->getSecond() - 1];
+ std::shared_ptr<void> rv;
+ if (static_cast<size_t>(handle) < comp.data.size())
+ rv = std::move(comp.data[handle]);
+ else
+ comp.data.resize(handle + 1);
+ comp.data[handle] = std::move(data);
+ return rv;
+}
+
+std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
+ int handle) {
+ assert(handle >= 0);
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return nullptr;
+ auto& comp = *m_impl->components[it->getSecond() - 1];
+ if (static_cast<size_t>(handle) >= comp.data.size()) return nullptr;
+ return comp.data[handle];
+}
+
+void SendableRegistry::EnableLiveWindow(Sendable* sendable) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->liveWindow = true;
+}
+
+void SendableRegistry::DisableLiveWindow(Sendable* sendable) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->liveWindow = false;
+}
+
+SendableRegistry::UID SendableRegistry::GetUniqueId(Sendable* sendable) {
+ std::scoped_lock lock(m_impl->mutex);
+ UID uid;
+ auto& comp = m_impl->GetOrAdd(sendable, &uid);
+ comp.sendable = sendable;
+ return uid;
+}
+
+Sendable* SendableRegistry::GetSendable(UID uid) {
+ if (uid == 0) return nullptr;
+ std::scoped_lock lock(m_impl->mutex);
+ return m_impl->components[uid - 1]->sendable;
+}
+
+void SendableRegistry::Publish(UID sendableUid,
+ std::shared_ptr<NetworkTable> table) {
+ std::scoped_lock lock(m_impl->mutex);
+ if (sendableUid == 0) return;
+ auto& comp = *m_impl->components[sendableUid - 1];
+ comp.builder = SendableBuilderImpl{}; // clear any current builder
+ comp.builder.SetTable(table);
+ comp.sendable->InitSendable(comp.builder);
+ comp.builder.UpdateTable();
+ comp.builder.StartListeners();
+}
+
+void SendableRegistry::Update(UID sendableUid) {
+ if (sendableUid == 0) return;
+ std::scoped_lock lock(m_impl->mutex);
+ m_impl->components[sendableUid - 1]->builder.UpdateTable();
+}
+
+void SendableRegistry::ForeachLiveWindow(
+ int dataHandle,
+ wpi::function_ref<void(CallbackData& data)> callback) const {
+ assert(dataHandle >= 0);
+ std::scoped_lock lock(m_impl->mutex);
+ for (auto&& comp : m_impl->components) {
+ if (comp->sendable && comp->liveWindow) {
+ if (static_cast<size_t>(dataHandle) >= comp->data.size())
+ comp->data.resize(dataHandle + 1);
+ CallbackData cbdata{comp->sendable, comp->name,
+ comp->subsystem, comp->parent,
+ comp->data[dataHandle], comp->builder};
+ callback(cbdata);
+ }
+ }
+}
+
+SendableRegistry::SendableRegistry() : m_impl(new Impl) {}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
index 98e5568..e8c9700 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,27 +14,17 @@
#include <wpi/mutex.h>
#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableBuilderImpl.h"
+#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
namespace {
-class SmartDashboardData {
- public:
- SmartDashboardData() = default;
- explicit SmartDashboardData(Sendable* sendable_) : sendable(sendable_) {}
-
- Sendable* sendable = nullptr;
- SendableBuilderImpl builder;
-};
-
class Singleton {
public:
static Singleton& GetInstance();
std::shared_ptr<nt::NetworkTable> table;
- wpi::StringMap<SmartDashboardData> tablesToData;
+ wpi::StringMap<SendableRegistry::UID> tablesToData;
wpi::mutex tablesToDataMutex;
private:
@@ -97,15 +87,14 @@
return;
}
auto& inst = Singleton::GetInstance();
- std::lock_guard<wpi::mutex> lock(inst.tablesToDataMutex);
- auto& sddata = inst.tablesToData[key];
- if (!sddata.sendable || sddata.sendable != data) {
- sddata = SmartDashboardData(data);
+ std::scoped_lock lock(inst.tablesToDataMutex);
+ auto& uid = inst.tablesToData[key];
+ auto& registry = SendableRegistry::GetInstance();
+ Sendable* sddata = registry.GetSendable(uid);
+ if (sddata != data) {
+ uid = registry.GetUniqueId(data);
auto dataTable = inst.table->GetSubTable(key);
- sddata.builder.SetTable(dataTable);
- data->InitSendable(sddata.builder);
- sddata.builder.UpdateTable();
- sddata.builder.StartListeners();
+ registry.Publish(uid, dataTable);
dataTable->GetEntry(".name").SetString(key);
}
}
@@ -115,18 +104,19 @@
wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
return;
}
- PutData(value->GetName(), value);
+ auto name = SendableRegistry::GetInstance().GetName(value);
+ if (!name.empty()) PutData(name, value);
}
Sendable* SmartDashboard::GetData(wpi::StringRef key) {
auto& inst = Singleton::GetInstance();
- std::lock_guard<wpi::mutex> lock(inst.tablesToDataMutex);
- auto data = inst.tablesToData.find(key);
- if (data == inst.tablesToData.end()) {
+ std::scoped_lock lock(inst.tablesToDataMutex);
+ auto it = inst.tablesToData.find(key);
+ if (it == inst.tablesToData.end()) {
wpi_setGlobalWPIErrorWithContext(SmartDashboardMissingKey, key);
return nullptr;
}
- return data->getValue().sendable;
+ return SendableRegistry::GetInstance().GetSendable(it->getValue());
}
bool SmartDashboard::PutBoolean(wpi::StringRef keyName, bool value) {
@@ -254,10 +244,16 @@
return Singleton::GetInstance().table->GetEntry(keyName).GetValue();
}
+detail::ListenerExecutor SmartDashboard::listenerExecutor;
+
+void SmartDashboard::PostListenerTask(std::function<void()> task) {
+ listenerExecutor.Execute(task);
+}
+
void SmartDashboard::UpdateValues() {
+ auto& registry = SendableRegistry::GetInstance();
auto& inst = Singleton::GetInstance();
- std::lock_guard<wpi::mutex> lock(inst.tablesToDataMutex);
- for (auto& i : inst.tablesToData) {
- i.getValue().builder.UpdateTable();
- }
+ std::scoped_lock lock(inst.tablesToDataMutex);
+ for (auto& i : inst.tablesToData) registry.Update(i.getValue());
+ listenerExecutor.RunListenerTasks();
}
diff --git a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
new file mode 100644
index 0000000..3578b1d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/CubicHermiteSpline.h"
+
+using namespace frc;
+
+CubicHermiteSpline::CubicHermiteSpline(
+ std::array<double, 2> xInitialControlVector,
+ std::array<double, 2> xFinalControlVector,
+ std::array<double, 2> yInitialControlVector,
+ std::array<double, 2> yFinalControlVector) {
+ const auto hermite = MakeHermiteBasis();
+ const auto x =
+ ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+ const auto y =
+ ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+ // Populate first two rows with coefficients.
+ m_coefficients.template block<1, 4>(0, 0) = hermite * x;
+ m_coefficients.template block<1, 4>(1, 0) = hermite * y;
+
+ // Populate Row 2 and Row 3 with the derivatives of the equations above.
+ // Then populate row 4 and 5 with the second derivatives.
+ for (int i = 0; i < 4; i++) {
+ m_coefficients.template block<2, 1>(2, i) =
+ m_coefficients.template block<2, 1>(0, i) * (3 - i);
+
+ m_coefficients.template block<2, 1>(4, i) =
+ m_coefficients.template block<2, 1>(2, i) * (3 - i);
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
new file mode 100644
index 0000000..f714c6f
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/QuinticHermiteSpline.h"
+
+using namespace frc;
+
+QuinticHermiteSpline::QuinticHermiteSpline(
+ std::array<double, 3> xInitialControlVector,
+ std::array<double, 3> xFinalControlVector,
+ std::array<double, 3> yInitialControlVector,
+ std::array<double, 3> yFinalControlVector) {
+ const auto hermite = MakeHermiteBasis();
+ const auto x =
+ ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+ const auto y =
+ ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+ // Populate first two rows with coefficients.
+ m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose();
+ m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose();
+
+ // Populate Row 2 and Row 3 with the derivatives of the equations above.
+ // Then populate row 4 and 5 with the second derivatives.
+ for (int i = 0; i < 6; i++) {
+ m_coefficients.template block<2, 1>(2, i) =
+ m_coefficients.template block<2, 1>(0, i) * (5 - i);
+
+ m_coefficients.template block<2, 1>(4, i) =
+ m_coefficients.template block<2, 1>(2, i) * (5 - i);
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
new file mode 100644
index 0000000..7c3fdc1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
@@ -0,0 +1,194 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/SplineHelper.h"
+
+#include <cstddef>
+
+using namespace frc;
+
+std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
+ const Spline<3>::ControlVector& start, std::vector<Translation2d> waypoints,
+ const Spline<3>::ControlVector& end) {
+ std::vector<CubicHermiteSpline> splines;
+
+ std::array<double, 2> xInitial = start.x;
+ std::array<double, 2> yInitial = start.y;
+ std::array<double, 2> xFinal = end.x;
+ std::array<double, 2> yFinal = end.y;
+
+ if (waypoints.size() > 1) {
+ waypoints.emplace(waypoints.begin(),
+ Translation2d{units::meter_t(xInitial[0]),
+ units::meter_t(yInitial[0])});
+ waypoints.emplace_back(
+ Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
+
+ std::vector<double> a;
+ std::vector<double> b(waypoints.size() - 2, 4.0);
+ std::vector<double> c;
+ std::vector<double> dx, dy;
+ std::vector<double> fx(waypoints.size() - 2, 0.0),
+ fy(waypoints.size() - 2, 0.0);
+
+ a.emplace_back(0);
+ for (size_t i = 0; i < waypoints.size() - 3; ++i) {
+ a.emplace_back(1);
+ c.emplace_back(1);
+ }
+ c.emplace_back(0);
+
+ dx.emplace_back(
+ 3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
+ xInitial[1]);
+ dy.emplace_back(
+ 3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
+ yInitial[1]);
+ if (waypoints.size() > 4) {
+ for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
+ dx.emplace_back(3 * (waypoints[i + 1].X().to<double>() -
+ waypoints[i - 1].X().to<double>()));
+ dy.emplace_back(3 * (waypoints[i + 1].Y().to<double>() -
+ waypoints[i - 1].Y().to<double>()));
+ }
+ }
+ dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
+ waypoints[waypoints.size() - 3].X().to<double>()) -
+ xFinal[1]);
+ dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
+ waypoints[waypoints.size() - 3].Y().to<double>()) -
+ yFinal[1]);
+
+ ThomasAlgorithm(a, b, c, dx, &fx);
+ ThomasAlgorithm(a, b, c, dy, &fy);
+
+ fx.emplace(fx.begin(), xInitial[1]);
+ fx.emplace_back(xFinal[1]);
+ fy.emplace(fy.begin(), yInitial[1]);
+ fy.emplace_back(yFinal[1]);
+
+ for (size_t i = 0; i < fx.size() - 1; ++i) {
+ // Create the spline.
+ const CubicHermiteSpline spline{
+ {waypoints[i].X().to<double>(), fx[i]},
+ {waypoints[i + 1].X().to<double>(), fx[i + 1]},
+ {waypoints[i].Y().to<double>(), fy[i]},
+ {waypoints[i + 1].Y().to<double>(), fy[i + 1]}};
+
+ splines.push_back(spline);
+ }
+ } else if (waypoints.size() == 1) {
+ const double xDeriv =
+ (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
+ const double yDeriv =
+ (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
+
+ std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
+ xDeriv};
+ std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
+ yDeriv};
+
+ splines.emplace_back(xInitial, midXControlVector, yInitial,
+ midYControlVector);
+ splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal);
+
+ } else {
+ // Create the spline.
+ const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal};
+ splines.push_back(spline);
+ }
+
+ return splines;
+}
+
+std::vector<QuinticHermiteSpline>
+SplineHelper::QuinticSplinesFromControlVectors(
+ const std::vector<Spline<5>::ControlVector>& controlVectors) {
+ std::vector<QuinticHermiteSpline> splines;
+ for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
+ auto& xInitial = controlVectors[i].x;
+ auto& yInitial = controlVectors[i].y;
+ auto& xFinal = controlVectors[i + 1].x;
+ auto& yFinal = controlVectors[i + 1].y;
+ splines.emplace_back(xInitial, xFinal, yInitial, yFinal);
+ }
+ return splines;
+}
+
+std::array<Spline<3>::ControlVector, 2>
+SplineHelper::CubicControlVectorsFromWaypoints(
+ const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+ const Pose2d& end) {
+ double scalar;
+ if (interiorWaypoints.empty()) {
+ scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
+ } else {
+ scalar =
+ 1.2 *
+ start.Translation().Distance(interiorWaypoints.front()).to<double>();
+ }
+ const auto initialCV = CubicControlVector(scalar, start);
+ if (!interiorWaypoints.empty()) {
+ scalar =
+ 1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
+ }
+ const auto finalCV = CubicControlVector(scalar, end);
+ return {initialCV, finalCV};
+}
+
+std::vector<Spline<5>::ControlVector>
+SplineHelper::QuinticControlVectorsFromWaypoints(
+ const std::vector<Pose2d>& waypoints) {
+ std::vector<Spline<5>::ControlVector> vectors;
+ for (size_t i = 0; i < waypoints.size() - 1; ++i) {
+ auto& p0 = waypoints[i];
+ auto& p1 = waypoints[i + 1];
+
+ // This just makes the splines look better.
+ const auto scalar =
+ 1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
+
+ vectors.push_back(QuinticControlVector(scalar, p0));
+ vectors.push_back(QuinticControlVector(scalar, p1));
+ }
+ return vectors;
+}
+
+void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
+ const std::vector<double>& b,
+ const std::vector<double>& c,
+ const std::vector<double>& d,
+ std::vector<double>* solutionVector) {
+ auto& f = *solutionVector;
+ size_t N = d.size();
+
+ // Create the temporary vectors
+ // Note that this is inefficient as it is possible to call
+ // this function many times. A better implementation would
+ // pass these temporary matrices by non-const reference to
+ // save excess allocation and deallocation
+ std::vector<double> c_star(N, 0.0);
+ std::vector<double> d_star(N, 0.0);
+
+ // This updates the coefficients in the first row
+ // Note that we should be checking for division by zero here
+ c_star[0] = c[0] / b[0];
+ d_star[0] = d[0] / b[0];
+
+ // Create the c_star and d_star coefficients in the forward sweep
+ for (size_t i = 1; i < N; ++i) {
+ double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
+ c_star[i] = c[i] * m;
+ d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;
+ }
+
+ f[N - 1] = d_star[N - 1];
+ // This is the reverse sweep, used to update the solution vector f
+ for (int i = N - 2; i >= 0; i--) {
+ f[i] = d_star[i] - c_star[i] * f[i + 1];
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp b/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp
new file mode 100644
index 0000000..f3c42c6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/SplineParameterizer.h"
+
+constexpr units::meter_t frc::SplineParameterizer::kMaxDx;
+constexpr units::meter_t frc::SplineParameterizer::kMaxDy;
+constexpr units::radian_t frc::SplineParameterizer::kMaxDtheta;
diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
new file mode 100644
index 0000000..9b4b4f5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/Trajectory.h"
+
+using namespace frc;
+
+Trajectory::State Trajectory::State::Interpolate(State endValue,
+ double i) const {
+ // Find the new [t] value.
+ const auto newT = Lerp(t, endValue.t, i);
+
+ // Find the delta time between the current state and the interpolated state.
+ const auto deltaT = newT - t;
+
+ // If delta time is negative, flip the order of interpolation.
+ if (deltaT < 0_s) return endValue.Interpolate(*this, 1.0 - i);
+
+ // Check whether the robot is reversing at this stage.
+ const auto reversing =
+ velocity < 0_mps ||
+ (units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq);
+
+ // Calculate the new velocity.
+ // v = v_0 + at
+ const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
+
+ // Calculate the change in position.
+ // delta_s = v_0 t + 0.5 at^2
+ const units::meter_t newS =
+ (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
+ (reversing ? -1.0 : 1.0);
+
+ // Return the new state. To find the new position for the new state, we need
+ // to interpolate between the two endpoint poses. The fraction for
+ // interpolation is the change in position (delta s) divided by the total
+ // distance between the two endpoints.
+ const double interpolationFrac =
+ newS / endValue.pose.Translation().Distance(pose.Translation());
+
+ return {newT, newV, acceleration,
+ Lerp(pose, endValue.pose, interpolationFrac),
+ Lerp(curvature, endValue.curvature, interpolationFrac)};
+}
+
+Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
+ m_totalTime = states.back().t;
+}
+
+Trajectory::State Trajectory::Sample(units::second_t t) const {
+ if (t <= m_states.front().t) return m_states.front();
+ if (t >= m_totalTime) return m_states.back();
+
+ // To get the element that we want, we will use a binary search algorithm
+ // instead of iterating over a for-loop. A binary search is O(std::log(n))
+ // whereas searching using a loop is O(n).
+
+ // This starts at 1 because we use the previous state later on for
+ // interpolation.
+ int low = 1;
+ int high = m_states.size() - 1;
+
+ while (low != high) {
+ int mid = (low + high) / 2;
+ if (m_states[mid].t < t) {
+ // This index and everything under it are less than the requested
+ // timestamp. Therefore, we can discard them.
+ low = mid + 1;
+ } else {
+ // t is at least as large as the element at this index. This means that
+ // anything after it cannot be what we are looking for.
+ high = mid;
+ }
+ }
+
+ // High and Low should be the same.
+
+ // The sample's timestamp is now greater than or equal to the requested
+ // timestamp. If it is greater, we need to interpolate between the
+ // previous state and the current state to get the exact state that we
+ // want.
+ const auto sample = m_states[low];
+ const auto prevSample = m_states[low - 1];
+
+ // If the difference in states is negligible, then we are spot on!
+ if (units::math::abs(sample.t - prevSample.t) < 1E-9_s) {
+ return sample;
+ }
+ // Interpolate between the two states for the state that we want.
+ return prevSample.Interpolate(sample,
+ (t - prevSample.t) / (sample.t - prevSample.t));
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
new file mode 100644
index 0000000..92c21f6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryGenerator.h"
+
+#include <utility>
+
+#include "frc/spline/SplineHelper.h"
+#include "frc/trajectory/TrajectoryParameterizer.h"
+
+using namespace frc;
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+ Spline<3>::ControlVector initial,
+ const std::vector<Translation2d>& interiorWaypoints,
+ Spline<3>::ControlVector end, const TrajectoryConfig& config) {
+ const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+
+ // Make theta normal for trajectory generation if path is reversed.
+ // Flip the headings.
+ if (config.IsReversed()) {
+ initial.x[1] *= -1;
+ initial.y[1] *= -1;
+ end.x[1] *= -1;
+ end.y[1] *= -1;
+ }
+
+ auto points =
+ SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
+ initial, interiorWaypoints, end));
+
+ // After trajectory generation, flip theta back so it's relative to the
+ // field. Also fix curvature.
+ if (config.IsReversed()) {
+ for (auto& point : points) {
+ point = {point.first + flip, -point.second};
+ }
+ }
+
+ return TrajectoryParameterizer::TimeParameterizeTrajectory(
+ points, config.Constraints(), config.StartVelocity(),
+ config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+ config.IsReversed());
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+ const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+ const Pose2d& end, const TrajectoryConfig& config) {
+ auto [startCV, endCV] = SplineHelper::CubicControlVectorsFromWaypoints(
+ start, interiorWaypoints, end);
+ return GenerateTrajectory(startCV, interiorWaypoints, endCV, config);
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+ std::vector<Spline<5>::ControlVector> controlVectors,
+ const TrajectoryConfig& config) {
+ const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+ // Make theta normal for trajectory generation if path is reversed.
+ if (config.IsReversed()) {
+ for (auto& vector : controlVectors) {
+ // Flip the headings.
+ vector.x[1] *= -1;
+ vector.y[1] *= -1;
+ }
+ }
+
+ auto points = SplinePointsFromSplines(
+ SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
+
+ // After trajectory generation, flip theta back so it's relative to the
+ // field. Also fix curvature.
+ if (config.IsReversed()) {
+ for (auto& point : points) {
+ point = {point.first + flip, -point.second};
+ }
+ }
+
+ return TrajectoryParameterizer::TimeParameterizeTrajectory(
+ points, config.Constraints(), config.StartVelocity(),
+ config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+ config.IsReversed());
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+ const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
+ return GenerateTrajectory(
+ SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config);
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
new file mode 100644
index 0000000..131ae8a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
@@ -0,0 +1,224 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "frc/trajectory/TrajectoryParameterizer.h"
+
+using namespace frc;
+
+Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
+ const std::vector<PoseWithCurvature>& points,
+ const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+ units::meters_per_second_t startVelocity,
+ units::meters_per_second_t endVelocity,
+ units::meters_per_second_t maxVelocity,
+ units::meters_per_second_squared_t maxAcceleration, bool reversed) {
+ std::vector<ConstrainedState> constrainedStates(points.size());
+
+ ConstrainedState predecessor{points.front(), 0_m, startVelocity,
+ -maxAcceleration, maxAcceleration};
+
+ constrainedStates[0] = predecessor;
+
+ // Forward pass
+ for (unsigned int i = 0; i < points.size(); i++) {
+ auto& constrainedState = constrainedStates[i];
+ constrainedState.pose = points[i];
+
+ // Begin constraining based on predecessor
+ units::meter_t ds = constrainedState.pose.first.Translation().Distance(
+ predecessor.pose.first.Translation());
+ constrainedState.distance = ds + predecessor.distance;
+
+ // We may need to iterate to find the maximum end velocity and common
+ // acceleration, since acceleration limits may be a function of velocity.
+ while (true) {
+ // Enforce global max velocity and max reachable velocity by global
+ // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
+
+ constrainedState.maxVelocity = units::math::min(
+ maxVelocity,
+ units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity +
+ predecessor.maxAcceleration * ds * 2.0));
+
+ constrainedState.minAcceleration = -maxAcceleration;
+ constrainedState.maxAcceleration = maxAcceleration;
+
+ // At this point, the constrained state is fully constructed apart from
+ // all the custom-defined user constraints.
+ for (const auto& constraint : constraints) {
+ constrainedState.maxVelocity = units::math::min(
+ constrainedState.maxVelocity,
+ constraint->MaxVelocity(constrainedState.pose.first,
+ constrainedState.pose.second,
+ constrainedState.maxVelocity));
+ }
+
+ // Now enforce all acceleration limits.
+ EnforceAccelerationLimits(reversed, constraints, &constrainedState);
+
+ if (ds.to<double>() < kEpsilon) break;
+
+ // If the actual acceleration for this state is higher than the max
+ // acceleration that we applied, then we need to reduce the max
+ // acceleration of the predecessor and try again.
+ units::meters_per_second_squared_t actualAcceleration =
+ (constrainedState.maxVelocity * constrainedState.maxVelocity -
+ predecessor.maxVelocity * predecessor.maxVelocity) /
+ (ds * 2.0);
+
+ // If we violate the max acceleration constraint, let's modify the
+ // predecessor.
+ if (constrainedState.maxAcceleration < actualAcceleration - 1E-6_mps_sq) {
+ predecessor.maxAcceleration = constrainedState.maxAcceleration;
+ } else {
+ // Constrain the predecessor's max acceleration to the current
+ // acceleration.
+ if (actualAcceleration > predecessor.minAcceleration + 1E-6_mps_sq) {
+ predecessor.maxAcceleration = actualAcceleration;
+ }
+ // If the actual acceleration is less than the predecessor's min
+ // acceleration, it will be repaired in the backward pass.
+ break;
+ }
+ }
+ predecessor = constrainedState;
+ }
+
+ ConstrainedState successor{points.back(), constrainedStates.back().distance,
+ endVelocity, -maxAcceleration, maxAcceleration};
+
+ // Backward pass
+ for (int i = points.size() - 1; i >= 0; i--) {
+ auto& constrainedState = constrainedStates[i];
+ units::meter_t ds =
+ constrainedState.distance - successor.distance; // negative
+
+ while (true) {
+ // Enforce max velocity limit (reverse)
+ // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+ units::meters_per_second_t newMaxVelocity =
+ units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
+ successor.minAcceleration * ds * 2.0);
+
+ // No more limits to impose! This state can be finalized.
+ if (newMaxVelocity >= constrainedState.maxVelocity) break;
+
+ constrainedState.maxVelocity = newMaxVelocity;
+
+ // Check all acceleration constraints with the new max velocity.
+ EnforceAccelerationLimits(reversed, constraints, &constrainedState);
+
+ if (ds.to<double>() > -kEpsilon) break;
+
+ // If the actual acceleration for this state is lower than the min
+ // acceleration, then we need to lower the min acceleration of the
+ // successor and try again.
+ units::meters_per_second_squared_t actualAcceleration =
+ (constrainedState.maxVelocity * constrainedState.maxVelocity -
+ successor.maxVelocity * successor.maxVelocity) /
+ (ds * 2.0);
+ if (constrainedState.minAcceleration > actualAcceleration + 1E-6_mps_sq) {
+ successor.minAcceleration = constrainedState.minAcceleration;
+ } else {
+ successor.minAcceleration = actualAcceleration;
+ break;
+ }
+ }
+ successor = constrainedState;
+ }
+
+ // Now we can integrate the constrained states forward in time to obtain our
+ // trajectory states.
+
+ std::vector<Trajectory::State> states(points.size());
+ units::second_t t = 0_s;
+ units::meter_t s = 0_m;
+ units::meters_per_second_t v = 0_mps;
+
+ for (unsigned int i = 0; i < constrainedStates.size(); i++) {
+ auto state = constrainedStates[i];
+
+ // Calculate the change in position between the current state and the
+ // previous state.
+ units::meter_t ds = state.distance - s;
+
+ // Calculate the acceleration between the current state and the previous
+ // state.
+ units::meters_per_second_squared_t accel =
+ (state.maxVelocity * state.maxVelocity - v * v) / (ds * 2);
+
+ // Calculate dt.
+ units::second_t dt = 0_s;
+ if (i > 0) {
+ states.at(i - 1).acceleration = reversed ? -accel : accel;
+ if (units::math::abs(accel) > 1E-6_mps_sq) {
+ // v_f = v_0 + a * t
+ dt = (state.maxVelocity - v) / accel;
+ } else if (units::math::abs(v) > 1E-6_mps) {
+ // delta_x = v * t
+ dt = ds / v;
+ } else {
+ throw std::runtime_error(
+ "Something went wrong during trajectory generation.");
+ }
+ }
+
+ v = state.maxVelocity;
+ s = state.distance;
+
+ t += dt;
+
+ states[i] = {t, reversed ? -v : v, reversed ? -accel : accel,
+ state.pose.first, state.pose.second};
+ }
+
+ return Trajectory(states);
+}
+
+void TrajectoryParameterizer::EnforceAccelerationLimits(
+ bool reverse,
+ const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+ ConstrainedState* state) {
+ for (auto&& constraint : constraints) {
+ double factor = reverse ? -1.0 : 1.0;
+
+ auto minMaxAccel = constraint->MinMaxAcceleration(
+ state->pose.first, state->pose.second, state->maxVelocity * factor);
+
+ state->minAcceleration = units::math::max(
+ state->minAcceleration,
+ reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
+
+ state->maxAcceleration = units::math::min(
+ state->maxAcceleration,
+ reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration);
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp b/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
new file mode 100644
index 0000000..6026aa5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
@@ -0,0 +1,158 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrapezoidProfile.h"
+
+using namespace frc;
+
+TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
+ State initial)
+ : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
+ m_constraints(constraints),
+ m_initial(Direct(initial)),
+ m_goal(Direct(goal)) {
+ if (m_initial.velocity > m_constraints.maxVelocity) {
+ m_initial.velocity = m_constraints.maxVelocity;
+ }
+
+ // Deal with a possibly truncated motion profile (with nonzero initial or
+ // final velocity) by calculating the parameters as if the profile began and
+ // ended at zero velocity
+ units::second_t cutoffBegin =
+ m_initial.velocity / m_constraints.maxAcceleration;
+ units::meter_t cutoffDistBegin =
+ cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+ units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+ units::meter_t cutoffDistEnd =
+ cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
+
+ // Now we can calculate the parameters as if it was a full trapezoid instead
+ // of a truncated one
+
+ units::meter_t fullTrapezoidDist =
+ cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+ units::second_t accelerationTime =
+ m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+ units::meter_t fullSpeedDist =
+ fullTrapezoidDist -
+ accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+ // Handle the case where the profile never reaches full speed
+ if (fullSpeedDist < 0_m) {
+ accelerationTime =
+ units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+ fullSpeedDist = 0_m;
+ }
+
+ m_endAccel = accelerationTime - cutoffBegin;
+ m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+ m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+}
+
+TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
+ State result = m_initial;
+
+ if (t < m_endAccel) {
+ result.velocity += t * m_constraints.maxAcceleration;
+ result.position +=
+ (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
+ } else if (t < m_endFullSpeed) {
+ result.velocity = m_constraints.maxVelocity;
+ result.position += (m_initial.velocity +
+ m_endAccel * m_constraints.maxAcceleration / 2.0) *
+ m_endAccel +
+ m_constraints.maxVelocity * (t - m_endAccel);
+ } else if (t <= m_endDeccel) {
+ result.velocity =
+ m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
+ units::second_t timeLeft = m_endDeccel - t;
+ result.position =
+ m_goal.position -
+ (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
+ timeLeft;
+ } else {
+ result = m_goal;
+ }
+
+ return Direct(result);
+}
+
+units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
+ units::meter_t position = m_initial.position * m_direction;
+ units::meters_per_second_t velocity = m_initial.velocity * m_direction;
+
+ units::second_t endAccel = m_endAccel * m_direction;
+ units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
+
+ if (target < position) {
+ endAccel *= -1.0;
+ endFullSpeed *= -1.0;
+ velocity *= -1.0;
+ }
+
+ endAccel = units::math::max(endAccel, 0_s);
+ endFullSpeed = units::math::max(endFullSpeed, 0_s);
+ units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
+ endDeccel = units::math::max(endDeccel, 0_s);
+
+ const units::meters_per_second_squared_t acceleration =
+ m_constraints.maxAcceleration;
+ const units::meters_per_second_squared_t decceleration =
+ -m_constraints.maxAcceleration;
+
+ units::meter_t distToTarget = units::math::abs(target - position);
+
+ if (distToTarget < 1e-6_m) {
+ return 0_s;
+ }
+
+ units::meter_t accelDist =
+ velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
+
+ units::meters_per_second_t deccelVelocity;
+ if (endAccel > 0_s) {
+ deccelVelocity = units::math::sqrt(
+ units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
+ } else {
+ deccelVelocity = velocity;
+ }
+
+ units::meter_t deccelDist =
+ deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
+
+ deccelDist = units::math::max(deccelDist, 0_m);
+
+ units::meter_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+
+ if (accelDist > distToTarget) {
+ accelDist = distToTarget;
+ fullSpeedDist = 0_m;
+ deccelDist = 0_m;
+ } else if (accelDist + fullSpeedDist > distToTarget) {
+ fullSpeedDist = distToTarget - accelDist;
+ deccelDist = 0_m;
+ } else {
+ deccelDist = distToTarget - fullSpeedDist - accelDist;
+ }
+
+ units::second_t accelTime =
+ (-velocity + units::math::sqrt(units::math::abs(
+ velocity * velocity + 2 * acceleration * accelDist))) /
+ acceleration;
+
+ units::second_t deccelTime =
+ (-deccelVelocity +
+ units::math::sqrt(units::math::abs(deccelVelocity * deccelVelocity +
+ 2 * decceleration * deccelDist))) /
+ decceleration;
+
+ units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
+
+ return accelTime + fullSpeedTime + deccelTime;
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
new file mode 100644
index 0000000..bf45c34
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+
+using namespace frc;
+
+CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
+ units::meters_per_second_squared_t maxCentripetalAcceleration)
+ : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
+
+units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) {
+ // ac = v^2 / r
+ // k (curvature) = 1 / r
+
+ // therefore, ac = v^2 * k
+ // ac / k = v^2
+ // v = std::sqrt(ac / k)
+
+ // We have to multiply by 1_rad here to get the units to cancel out nicely.
+ // The units library defines a unit for radians although it is technically
+ // unitless.
+ return units::math::sqrt(m_maxCentripetalAcceleration /
+ units::math::abs(curvature) * 1_rad);
+}
+
+TrajectoryConstraint::MinMax
+CentripetalAccelerationConstraint::MinMaxAcceleration(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) {
+ // The acceleration of the robot has no impact on the centripetal acceleration
+ // of the robot.
+ return {};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
new file mode 100644
index 0000000..8b88bf4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+
+using namespace frc;
+
+DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
+ DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+ : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) {
+ auto wheelSpeeds =
+ m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
+ wheelSpeeds.Normalize(m_maxSpeed);
+
+ return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) {
+ return {};
+}
diff --git a/wpilibc/src/main/native/cpp/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp
similarity index 73%
rename from wpilibc/src/main/native/cpp/RobotBase.cpp
rename to wpilibc/src/main/native/cppcs/RobotBase.cpp
index bc316f4..65c072d 100644
--- a/wpilibc/src/main/native/cpp/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,10 +7,13 @@
#include "frc/RobotBase.h"
+#ifdef __FRC_ROBORIO__
+#include <dlfcn.h>
+#endif
+
#include <cstdio>
#include <cameraserver/CameraServerShared.h>
-#include <cscore.h>
#include <hal/HAL.h>
#include <networktables/NetworkTableInstance.h>
@@ -22,6 +25,8 @@
#include "frc/livewindow/LiveWindow.h"
#include "frc/smartdashboard/SmartDashboard.h"
+typedef void (*SetCameraServerSharedFP)(frc::CameraServerShared* shared);
+
using namespace frc;
int frc::RunHALInitialization() {
@@ -65,7 +70,36 @@
} // namespace
static void SetupCameraServerShared() {
- SetCameraServerShared(std::make_unique<WPILibCameraServerShared>());
+#ifdef __FRC_ROBORIO__
+#ifdef DYNAMIC_CAMERA_SERVER
+#ifdef DYNAMIC_CAMERA_SERVER_DEBUG
+ auto cameraServerLib = dlopen("libcameraserverd.so", RTLD_NOW);
+#else
+ auto cameraServerLib = dlopen("libcameraserver.so", RTLD_NOW);
+#endif
+
+ if (!cameraServerLib) {
+ wpi::outs() << "Camera Server Library Not Found\n";
+ wpi::outs().flush();
+ return;
+ }
+ auto symbol = dlsym(cameraServerLib, "CameraServer_SetCameraServerShared");
+ if (symbol) {
+ auto setCameraServerShared = (SetCameraServerSharedFP)symbol;
+ setCameraServerShared(new WPILibCameraServerShared{});
+ wpi::outs() << "Set Camera Server Shared\n";
+ wpi::outs().flush();
+ } else {
+ wpi::outs() << "Camera Server Shared Symbol Missing\n";
+ wpi::outs().flush();
+ }
+#else
+ CameraServer_SetCameraServerShared(new WPILibCameraServerShared{});
+#endif
+#else
+ wpi::outs() << "Not loading CameraServerShared\n";
+ wpi::outs().flush();
+#endif
}
bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
@@ -118,8 +152,9 @@
LiveWindow::GetInstance()->SetEnabled(false);
}
-RobotBase::RobotBase(RobotBase&&) : m_ds(DriverStation::GetInstance()) {}
+RobotBase::RobotBase(RobotBase&&) noexcept
+ : m_ds(DriverStation::GetInstance()) {}
-RobotBase::~RobotBase() { cs::Shutdown(); }
+RobotBase::~RobotBase() {}
-RobotBase& RobotBase::operator=(RobotBase&&) { return *this; }
+RobotBase& RobotBase::operator=(RobotBase&&) noexcept { return *this; }
diff --git a/wpilibc/src/main/native/include/ADXL345_I2C.h b/wpilibc/src/main/native/include/ADXL345_I2C.h
deleted file mode 100644
index a155d6c0..0000000
--- a/wpilibc/src/main/native/include/ADXL345_I2C.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: ADXL345_I2C.h is deprecated; include frc/ADXL345_I2C.h instead"
-#else
-#warning "ADXL345_I2C.h is deprecated; include frc/ADXL345_I2C.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/ADXL345_I2C.h"
diff --git a/wpilibc/src/main/native/include/ADXL345_SPI.h b/wpilibc/src/main/native/include/ADXL345_SPI.h
deleted file mode 100644
index 490594c..0000000
--- a/wpilibc/src/main/native/include/ADXL345_SPI.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: ADXL345_SPI.h is deprecated; include frc/ADXL345_SPI.h instead"
-#else
-#warning "ADXL345_SPI.h is deprecated; include frc/ADXL345_SPI.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/ADXL345_SPI.h"
diff --git a/wpilibc/src/main/native/include/ADXL362.h b/wpilibc/src/main/native/include/ADXL362.h
deleted file mode 100644
index 238ee10..0000000
--- a/wpilibc/src/main/native/include/ADXL362.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: ADXL362.h is deprecated; include frc/ADXL362.h instead"
-#else
-#warning "ADXL362.h is deprecated; include frc/ADXL362.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/ADXL362.h"
diff --git a/wpilibc/src/main/native/include/ADXRS450_Gyro.h b/wpilibc/src/main/native/include/ADXRS450_Gyro.h
deleted file mode 100644
index 46b337f..0000000
--- a/wpilibc/src/main/native/include/ADXRS450_Gyro.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: ADXRS450_Gyro.h is deprecated; include frc/ADXRS450_Gyro.h instead"
-#else
-#warning "ADXRS450_Gyro.h is deprecated; include frc/ADXRS450_Gyro.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/ADXRS450_Gyro.h"
diff --git a/wpilibc/src/main/native/include/AnalogAccelerometer.h b/wpilibc/src/main/native/include/AnalogAccelerometer.h
deleted file mode 100644
index 926005e..0000000
--- a/wpilibc/src/main/native/include/AnalogAccelerometer.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: AnalogAccelerometer.h is deprecated; include frc/AnalogAccelerometer.h instead"
-#else
-#warning "AnalogAccelerometer.h is deprecated; include frc/AnalogAccelerometer.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/AnalogAccelerometer.h"
diff --git a/wpilibc/src/main/native/include/AnalogGyro.h b/wpilibc/src/main/native/include/AnalogGyro.h
deleted file mode 100644
index 287bf46..0000000
--- a/wpilibc/src/main/native/include/AnalogGyro.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: AnalogGyro.h is deprecated; include frc/AnalogGyro.h instead"
-#else
-#warning "AnalogGyro.h is deprecated; include frc/AnalogGyro.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/AnalogGyro.h"
diff --git a/wpilibc/src/main/native/include/AnalogInput.h b/wpilibc/src/main/native/include/AnalogInput.h
deleted file mode 100644
index 64bacdf..0000000
--- a/wpilibc/src/main/native/include/AnalogInput.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: AnalogInput.h is deprecated; include frc/AnalogInput.h instead"
-#else
-#warning "AnalogInput.h is deprecated; include frc/AnalogInput.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/AnalogInput.h"
diff --git a/wpilibc/src/main/native/include/AnalogOutput.h b/wpilibc/src/main/native/include/AnalogOutput.h
deleted file mode 100644
index b08d3ee..0000000
--- a/wpilibc/src/main/native/include/AnalogOutput.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: AnalogOutput.h is deprecated; include frc/AnalogOutput.h instead"
-#else
-#warning "AnalogOutput.h is deprecated; include frc/AnalogOutput.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/AnalogOutput.h"
diff --git a/wpilibc/src/main/native/include/AnalogPotentiometer.h b/wpilibc/src/main/native/include/AnalogPotentiometer.h
deleted file mode 100644
index 5b01b97..0000000
--- a/wpilibc/src/main/native/include/AnalogPotentiometer.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: AnalogPotentiometer.h is deprecated; include frc/AnalogPotentiometer.h instead"
-#else
-#warning "AnalogPotentiometer.h is deprecated; include frc/AnalogPotentiometer.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/AnalogPotentiometer.h"
diff --git a/wpilibc/src/main/native/include/AnalogTrigger.h b/wpilibc/src/main/native/include/AnalogTrigger.h
deleted file mode 100644
index e88547d..0000000
--- a/wpilibc/src/main/native/include/AnalogTrigger.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: AnalogTrigger.h is deprecated; include frc/AnalogTrigger.h instead"
-#else
-#warning "AnalogTrigger.h is deprecated; include frc/AnalogTrigger.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/AnalogTrigger.h"
diff --git a/wpilibc/src/main/native/include/AnalogTriggerOutput.h b/wpilibc/src/main/native/include/AnalogTriggerOutput.h
deleted file mode 100644
index d8754a0..0000000
--- a/wpilibc/src/main/native/include/AnalogTriggerOutput.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: AnalogTriggerOutput.h is deprecated; include frc/AnalogTriggerOutput.h instead"
-#else
-#warning "AnalogTriggerOutput.h is deprecated; include frc/AnalogTriggerOutput.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/AnalogTriggerOutput.h"
diff --git a/wpilibc/src/main/native/include/AnalogTriggerType.h b/wpilibc/src/main/native/include/AnalogTriggerType.h
deleted file mode 100644
index 6f55772..0000000
--- a/wpilibc/src/main/native/include/AnalogTriggerType.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: AnalogTriggerType.h is deprecated; include frc/AnalogTriggerType.h instead"
-#else
-#warning "AnalogTriggerType.h is deprecated; include frc/AnalogTriggerType.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/AnalogTriggerType.h"
diff --git a/wpilibc/src/main/native/include/Base.h b/wpilibc/src/main/native/include/Base.h
deleted file mode 100644
index c6fc91a..0000000
--- a/wpilibc/src/main/native/include/Base.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Base.h is deprecated; include frc/Base.h instead"
-#else
-#warning "Base.h is deprecated; include frc/Base.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Base.h"
diff --git a/wpilibc/src/main/native/include/BuiltInAccelerometer.h b/wpilibc/src/main/native/include/BuiltInAccelerometer.h
deleted file mode 100644
index e394037..0000000
--- a/wpilibc/src/main/native/include/BuiltInAccelerometer.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: BuiltInAccelerometer.h is deprecated; include frc/BuiltInAccelerometer.h instead"
-#else
-#warning "BuiltInAccelerometer.h is deprecated; include frc/BuiltInAccelerometer.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/BuiltInAccelerometer.h"
diff --git a/wpilibc/src/main/native/include/Buttons/Button.h b/wpilibc/src/main/native/include/Buttons/Button.h
deleted file mode 100644
index f71bee2..0000000
--- a/wpilibc/src/main/native/include/Buttons/Button.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Buttons/Button.h is deprecated; include frc/buttons/Button.h instead"
-#else
-#warning "Buttons/Button.h is deprecated; include frc/buttons/Button.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/Button.h"
diff --git a/wpilibc/src/main/native/include/Buttons/ButtonScheduler.h b/wpilibc/src/main/native/include/Buttons/ButtonScheduler.h
deleted file mode 100644
index 48392c7..0000000
--- a/wpilibc/src/main/native/include/Buttons/ButtonScheduler.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: buttons/ButtonScheduler.h is deprecated; include frc/buttons/ButtonScheduler.h instead"
-#else
-#warning "buttons/ButtonScheduler.h is deprecated; include frc/buttons/ButtonScheduler.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/ButtonScheduler.h"
diff --git a/wpilibc/src/main/native/include/Buttons/CancelButtonScheduler.h b/wpilibc/src/main/native/include/Buttons/CancelButtonScheduler.h
deleted file mode 100644
index 201d024..0000000
--- a/wpilibc/src/main/native/include/Buttons/CancelButtonScheduler.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: buttons/CancelButtonScheduler.h is deprecated; include frc/buttons/CancelButtonScheduler.h instead"
-#else
-#warning "buttons/CancelButtonScheduler.h is deprecated; include frc/buttons/CancelButtonScheduler.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/CancelButtonScheduler.h"
diff --git a/wpilibc/src/main/native/include/Buttons/HeldButtonScheduler.h b/wpilibc/src/main/native/include/Buttons/HeldButtonScheduler.h
deleted file mode 100644
index 4886832..0000000
--- a/wpilibc/src/main/native/include/Buttons/HeldButtonScheduler.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: buttons/HeldButtonScheduler.h is deprecated; include frc/buttons/HeldButtonScheduler.h instead"
-#else
-#warning "buttons/HeldButtonScheduler.h is deprecated; include frc/buttons/HeldButtonScheduler.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/HeldButtonScheduler.h"
diff --git a/wpilibc/src/main/native/include/Buttons/InternalButton.h b/wpilibc/src/main/native/include/Buttons/InternalButton.h
deleted file mode 100644
index 4cde15a..0000000
--- a/wpilibc/src/main/native/include/Buttons/InternalButton.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: buttons/InternalButton.h is deprecated; include frc/buttons/InternalButton.h instead"
-#else
-#warning "buttons/InternalButton.h is deprecated; include frc/buttons/InternalButton.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/InternalButton.h"
diff --git a/wpilibc/src/main/native/include/Buttons/JoystickButton.h b/wpilibc/src/main/native/include/Buttons/JoystickButton.h
deleted file mode 100644
index 6ad611e..0000000
--- a/wpilibc/src/main/native/include/Buttons/JoystickButton.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: buttons/JoystickButton.h is deprecated; include frc/buttons/JoystickButton.h instead"
-#else
-#warning "buttons/JoystickButton.h is deprecated; include frc/buttons/JoystickButton.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/JoystickButton.h"
diff --git a/wpilibc/src/main/native/include/Buttons/NetworkButton.h b/wpilibc/src/main/native/include/Buttons/NetworkButton.h
deleted file mode 100644
index 8d23810..0000000
--- a/wpilibc/src/main/native/include/Buttons/NetworkButton.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: buttons/NetworkButton.h is deprecated; include frc/buttons/NetworkButton.h instead"
-#else
-#warning "buttons/NetworkButton.h is deprecated; include frc/buttons/NetworkButton.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/NetworkButton.h"
diff --git a/wpilibc/src/main/native/include/Buttons/PressedButtonScheduler.h b/wpilibc/src/main/native/include/Buttons/PressedButtonScheduler.h
deleted file mode 100644
index 323d9e2..0000000
--- a/wpilibc/src/main/native/include/Buttons/PressedButtonScheduler.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: buttons/PressedButtonScheduler.h is deprecated; include frc/buttons/PressedButtonScheduler.h instead"
-#else
-#warning "buttons/PressedButtonScheduler.h is deprecated; include frc/buttons/PressedButtonScheduler.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/PressedButtonScheduler.h"
diff --git a/wpilibc/src/main/native/include/Buttons/ReleasedButtonScheduler.h b/wpilibc/src/main/native/include/Buttons/ReleasedButtonScheduler.h
deleted file mode 100644
index 88ef25c..0000000
--- a/wpilibc/src/main/native/include/Buttons/ReleasedButtonScheduler.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: buttons/ReleasedButtonScheduler.h is deprecated; include frc/buttons/ReleasedButtonScheduler.h instead"
-#else
-#warning "buttons/ReleasedButtonScheduler.h is deprecated; include frc/buttons/ReleasedButtonScheduler.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/ReleasedButtonScheduler.h"
diff --git a/wpilibc/src/main/native/include/Buttons/ToggleButtonScheduler.h b/wpilibc/src/main/native/include/Buttons/ToggleButtonScheduler.h
deleted file mode 100644
index 7848f91..0000000
--- a/wpilibc/src/main/native/include/Buttons/ToggleButtonScheduler.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: buttons/ToggleButtonScheduler.h is deprecated; include frc/buttons/ToggleButtonScheduler.h instead"
-#else
-#warning "buttons/ToggleButtonScheduler.h is deprecated; include frc/buttons/ToggleButtonScheduler.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/ToggleButtonScheduler.h"
diff --git a/wpilibc/src/main/native/include/Buttons/Trigger.h b/wpilibc/src/main/native/include/Buttons/Trigger.h
deleted file mode 100644
index 7fb8889..0000000
--- a/wpilibc/src/main/native/include/Buttons/Trigger.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: buttons/Trigger.h is deprecated; include frc/buttons/Trigger.h instead"
-#else
-#warning "buttons/Trigger.h is deprecated; include frc/buttons/Trigger.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/buttons/Trigger.h"
diff --git a/wpilibc/src/main/native/include/CAN.h b/wpilibc/src/main/native/include/CAN.h
deleted file mode 100644
index 28d49b8..0000000
--- a/wpilibc/src/main/native/include/CAN.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: CAN.h is deprecated; include frc/CAN.h instead"
-#else
-#warning "CAN.h is deprecated; include frc/CAN.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/CAN.h"
diff --git a/wpilibc/src/main/native/include/Commands/Command.h b/wpilibc/src/main/native/include/Commands/Command.h
deleted file mode 100644
index f170ced..0000000
--- a/wpilibc/src/main/native/include/Commands/Command.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/Command.h is deprecated; include frc/commands/Command.h instead"
-#else
-#warning "Commands/Command.h is deprecated; include frc/commands/Command.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/Command.h"
diff --git a/wpilibc/src/main/native/include/Commands/CommandGroup.h b/wpilibc/src/main/native/include/Commands/CommandGroup.h
deleted file mode 100644
index 0e124f2..0000000
--- a/wpilibc/src/main/native/include/Commands/CommandGroup.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/CommandGroup.h is deprecated; include frc/commands/CommandGroup.h instead"
-#else
-#warning "Commands/CommandGroup.h is deprecated; include frc/commands/CommandGroup.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/CommandGroup.h"
diff --git a/wpilibc/src/main/native/include/Commands/CommandGroupEntry.h b/wpilibc/src/main/native/include/Commands/CommandGroupEntry.h
deleted file mode 100644
index 1f5ee67..0000000
--- a/wpilibc/src/main/native/include/Commands/CommandGroupEntry.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/CommandGroupEntry.h is deprecated; include frc/commands/CommandGroupEntry.h instead"
-#else
-#warning "Commands/CommandGroupEntry.h is deprecated; include frc/commands/CommandGroupEntry.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/CommandGroupEntry.h"
diff --git a/wpilibc/src/main/native/include/Commands/ConditionalCommand.h b/wpilibc/src/main/native/include/Commands/ConditionalCommand.h
deleted file mode 100644
index a0d3df8..0000000
--- a/wpilibc/src/main/native/include/Commands/ConditionalCommand.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/ConditionalCommand.h is deprecated; include frc/commands/ConditionalCommand.h instead"
-#else
-#warning "Commands/ConditionalCommand.h is deprecated; include frc/commands/ConditionalCommand.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/ConditionalCommand.h"
diff --git a/wpilibc/src/main/native/include/Commands/InstantCommand.h b/wpilibc/src/main/native/include/Commands/InstantCommand.h
deleted file mode 100644
index c097c1a..0000000
--- a/wpilibc/src/main/native/include/Commands/InstantCommand.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/InstantCommand.h is deprecated; include frc/commands/InstantCommand.h instead"
-#else
-#warning "Commands/InstantCommand.h is deprecated; include frc/commands/InstantCommand.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/InstantCommand.h"
diff --git a/wpilibc/src/main/native/include/Commands/PIDCommand.h b/wpilibc/src/main/native/include/Commands/PIDCommand.h
deleted file mode 100644
index 462e807..0000000
--- a/wpilibc/src/main/native/include/Commands/PIDCommand.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/PIDCommand.h is deprecated; include frc/commands/PIDCommand.h instead"
-#else
-#warning "Commands/PIDCommand.h is deprecated; include frc/commands/PIDCommand.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/PIDCommand.h"
diff --git a/wpilibc/src/main/native/include/Commands/PIDSubsystem.h b/wpilibc/src/main/native/include/Commands/PIDSubsystem.h
deleted file mode 100644
index d4bc1ee..0000000
--- a/wpilibc/src/main/native/include/Commands/PIDSubsystem.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/PIDSubsystem.h is deprecated; include frc/commands/PIDSubsystem.h instead"
-#else
-#warning "Commands/PIDSubsystem.h is deprecated; include frc/commands/PIDSubsystem.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/PIDSubsystem.h"
diff --git a/wpilibc/src/main/native/include/Commands/PrintCommand.h b/wpilibc/src/main/native/include/Commands/PrintCommand.h
deleted file mode 100644
index 801a539..0000000
--- a/wpilibc/src/main/native/include/Commands/PrintCommand.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/PrintCommand.h is deprecated; include frc/commands/PrintCommand.h instead"
-#else
-#warning "Commands/PrintCommand.h is deprecated; include frc/commands/PrintCommand.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/PrintCommand.h"
diff --git a/wpilibc/src/main/native/include/Commands/Scheduler.h b/wpilibc/src/main/native/include/Commands/Scheduler.h
deleted file mode 100644
index df7591c..0000000
--- a/wpilibc/src/main/native/include/Commands/Scheduler.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/Scheduler.h is deprecated; include frc/commands/Scheduler.h instead"
-#else
-#warning "Commands/Scheduler.h is deprecated; include frc/commands/Scheduler.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/Scheduler.h"
diff --git a/wpilibc/src/main/native/include/Commands/StartCommand.h b/wpilibc/src/main/native/include/Commands/StartCommand.h
deleted file mode 100644
index 7ae1603..0000000
--- a/wpilibc/src/main/native/include/Commands/StartCommand.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/StartCommand.h is deprecated; include frc/commands/StartCommand.h instead"
-#else
-#warning "Commands/StartCommand.h is deprecated; include frc/commands/StartCommand.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/StartCommand.h"
diff --git a/wpilibc/src/main/native/include/Commands/Subsystem.h b/wpilibc/src/main/native/include/Commands/Subsystem.h
deleted file mode 100644
index ffd0697..0000000
--- a/wpilibc/src/main/native/include/Commands/Subsystem.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/Subsystem.h is deprecated; include frc/commands/Subsystem.h instead"
-#else
-#warning "Commands/Subsystem.h is deprecated; include frc/commands/Subsystem.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/Subsystem.h"
diff --git a/wpilibc/src/main/native/include/Commands/TimedCommand.h b/wpilibc/src/main/native/include/Commands/TimedCommand.h
deleted file mode 100644
index 848f4b6..0000000
--- a/wpilibc/src/main/native/include/Commands/TimedCommand.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/TimedCommand.h is deprecated; include frc/commands/TimedCommand.h instead"
-#else
-#warning "Commands/TimedCommand.h is deprecated; include frc/commands/TimedCommand.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/TimedCommand.h"
diff --git a/wpilibc/src/main/native/include/Commands/WaitCommand.h b/wpilibc/src/main/native/include/Commands/WaitCommand.h
deleted file mode 100644
index 1901fba..0000000
--- a/wpilibc/src/main/native/include/Commands/WaitCommand.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/WaitCommand.h is deprecated; include frc/commands/WaitCommand.h instead"
-#else
-#warning "Commands/WaitCommand.h is deprecated; include frc/commands/WaitCommand.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/WaitCommand.h"
diff --git a/wpilibc/src/main/native/include/Commands/WaitForChildren.h b/wpilibc/src/main/native/include/Commands/WaitForChildren.h
deleted file mode 100644
index 178c835..0000000
--- a/wpilibc/src/main/native/include/Commands/WaitForChildren.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/WaitForChildren.h is deprecated; include frc/commands/WaitForChildren.h instead"
-#else
-#warning "Commands/WaitForChildren.h is deprecated; include frc/commands/WaitForChildren.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/WaitForChildren.h"
diff --git a/wpilibc/src/main/native/include/Commands/WaitUntilCommand.h b/wpilibc/src/main/native/include/Commands/WaitUntilCommand.h
deleted file mode 100644
index b338d5f..0000000
--- a/wpilibc/src/main/native/include/Commands/WaitUntilCommand.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Commands/WaitUntilCommand.h is deprecated; include frc/commands/WaitUntilCommand.h instead"
-#else
-#warning "Commands/WaitUntilCommand.h is deprecated; include frc/commands/WaitUntilCommand.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/commands/WaitUntilCommand.h"
diff --git a/wpilibc/src/main/native/include/Compressor.h b/wpilibc/src/main/native/include/Compressor.h
deleted file mode 100644
index bfb018e..0000000
--- a/wpilibc/src/main/native/include/Compressor.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Compressor.h is deprecated; include frc/Compressor.h instead"
-#else
-#warning "Compressor.h is deprecated; include frc/Compressor.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Compressor.h"
diff --git a/wpilibc/src/main/native/include/Controller.h b/wpilibc/src/main/native/include/Controller.h
deleted file mode 100644
index b869373..0000000
--- a/wpilibc/src/main/native/include/Controller.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Controller.h is deprecated; include frc/Controller.h instead"
-#else
-#warning "Controller.h is deprecated; include frc/Controller.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Controller.h"
diff --git a/wpilibc/src/main/native/include/ControllerPower.h b/wpilibc/src/main/native/include/ControllerPower.h
deleted file mode 100644
index 1469d19..0000000
--- a/wpilibc/src/main/native/include/ControllerPower.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: ControllerPower.h is deprecated; include frc/ControllerPower.h instead"
-#else
-#warning "ControllerPower.h is deprecated; include frc/ControllerPower.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/ControllerPower.h"
diff --git a/wpilibc/src/main/native/include/Counter.h b/wpilibc/src/main/native/include/Counter.h
deleted file mode 100644
index e088897..0000000
--- a/wpilibc/src/main/native/include/Counter.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Counter.h is deprecated; include frc/Counter.h instead"
-#else
-#warning "Counter.h is deprecated; include frc/Counter.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Counter.h"
diff --git a/wpilibc/src/main/native/include/CounterBase.h b/wpilibc/src/main/native/include/CounterBase.h
deleted file mode 100644
index 736577f..0000000
--- a/wpilibc/src/main/native/include/CounterBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: CounterBase.h is deprecated; include frc/CounterBase.h instead"
-#else
-#warning "CounterBase.h is deprecated; include frc/CounterBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/CounterBase.h"
diff --git a/wpilibc/src/main/native/include/DMC60.h b/wpilibc/src/main/native/include/DMC60.h
deleted file mode 100644
index 0fe0415..0000000
--- a/wpilibc/src/main/native/include/DMC60.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: DMC60.h is deprecated; include frc/DMC60.h instead"
-#else
-#warning "DMC60.h is deprecated; include frc/DMC60.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/DMC60.h"
diff --git a/wpilibc/src/main/native/include/DigitalGlitchFilter.h b/wpilibc/src/main/native/include/DigitalGlitchFilter.h
deleted file mode 100644
index d35c82a..0000000
--- a/wpilibc/src/main/native/include/DigitalGlitchFilter.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: DigitalGlitchFilter.h is deprecated; include frc/DigitalGlitchFilter.h instead"
-#else
-#warning "DigitalGlitchFilter.h is deprecated; include frc/DigitalGlitchFilter.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/DigitalGlitchFilter.h"
diff --git a/wpilibc/src/main/native/include/DigitalInput.h b/wpilibc/src/main/native/include/DigitalInput.h
deleted file mode 100644
index 2b2a9b1..0000000
--- a/wpilibc/src/main/native/include/DigitalInput.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: DigitalInput.h is deprecated; include frc/DigitalInput.h instead"
-#else
-#warning "DigitalInput.h is deprecated; include frc/DigitalInput.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/DigitalInput.h"
diff --git a/wpilibc/src/main/native/include/DigitalOutput.h b/wpilibc/src/main/native/include/DigitalOutput.h
deleted file mode 100644
index 56983c6..0000000
--- a/wpilibc/src/main/native/include/DigitalOutput.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: DigitalOutput.h is deprecated; include frc/DigitalOutput.h instead"
-#else
-#warning "DigitalOutput.h is deprecated; include frc/DigitalOutput.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/DigitalOutput.h"
diff --git a/wpilibc/src/main/native/include/DigitalSource.h b/wpilibc/src/main/native/include/DigitalSource.h
deleted file mode 100644
index 9eae461..0000000
--- a/wpilibc/src/main/native/include/DigitalSource.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: DigitalSource.h is deprecated; include frc/DigitalSource.h instead"
-#else
-#warning "DigitalSource.h is deprecated; include frc/DigitalSource.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/DigitalSource.h"
diff --git a/wpilibc/src/main/native/include/DoubleSolenoid.h b/wpilibc/src/main/native/include/DoubleSolenoid.h
deleted file mode 100644
index 3c1bf96..0000000
--- a/wpilibc/src/main/native/include/DoubleSolenoid.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: DoubleSolenoid.h is deprecated; include frc/DoubleSolenoid.h instead"
-#else
-#warning "DoubleSolenoid.h is deprecated; include frc/DoubleSolenoid.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/DoubleSolenoid.h"
diff --git a/wpilibc/src/main/native/include/Drive/DifferentialDrive.h b/wpilibc/src/main/native/include/Drive/DifferentialDrive.h
deleted file mode 100644
index 4071d60..0000000
--- a/wpilibc/src/main/native/include/Drive/DifferentialDrive.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Drive/DifferentialDrive.h is deprecated; include frc/drive/DifferentialDrive.h instead"
-#else
-#warning "Drive/DifferentialDrive.h is deprecated; include frc/drive/DifferentialDrive.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/drive/DifferentialDrive.h"
diff --git a/wpilibc/src/main/native/include/Drive/KilloughDrive.h b/wpilibc/src/main/native/include/Drive/KilloughDrive.h
deleted file mode 100644
index fbe54a4..0000000
--- a/wpilibc/src/main/native/include/Drive/KilloughDrive.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Drive/KilloughDrive.h is deprecated; include frc/drive/KilloughDrive.h instead"
-#else
-#warning "Drive/KilloughDrive.h is deprecated; include frc/drive/KilloughDrive.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/drive/KilloughDrive.h"
diff --git a/wpilibc/src/main/native/include/Drive/MecanumDrive.h b/wpilibc/src/main/native/include/Drive/MecanumDrive.h
deleted file mode 100644
index b5a22aa..0000000
--- a/wpilibc/src/main/native/include/Drive/MecanumDrive.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Drive/MecanumDrive.h is deprecated; include frc/drive/MecanumDrive.h instead"
-#else
-#warning "Drive/MecanumDrive.h is deprecated; include frc/drive/MecanumDrive.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/drive/MecanumDrive.h"
diff --git a/wpilibc/src/main/native/include/Drive/RobotDriveBase.h b/wpilibc/src/main/native/include/Drive/RobotDriveBase.h
deleted file mode 100644
index f37ec0c..0000000
--- a/wpilibc/src/main/native/include/Drive/RobotDriveBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Drive/RobotDriveBase.h is deprecated; include frc/drive/RobotDriveBase.h instead"
-#else
-#warning "Drive/RobotDriveBase.h is deprecated; include frc/drive/RobotDriveBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/drive/RobotDriveBase.h"
diff --git a/wpilibc/src/main/native/include/Drive/Vector2d.h b/wpilibc/src/main/native/include/Drive/Vector2d.h
deleted file mode 100644
index 01516ed..0000000
--- a/wpilibc/src/main/native/include/Drive/Vector2d.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Drive/Vector2d.h is deprecated; include frc/drive/Vector2d.h instead"
-#else
-#warning "Drive/Vector2d.h is deprecated; include frc/drive/Vector2d.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/drive/Vector2d.h"
diff --git a/wpilibc/src/main/native/include/DriverStation.h b/wpilibc/src/main/native/include/DriverStation.h
deleted file mode 100644
index c00adbc..0000000
--- a/wpilibc/src/main/native/include/DriverStation.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: DriverStation.h is deprecated; include frc/DriverStation.h instead"
-#else
-#warning "DriverStation.h is deprecated; include frc/DriverStation.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/DriverStation.h"
diff --git a/wpilibc/src/main/native/include/Encoder.h b/wpilibc/src/main/native/include/Encoder.h
deleted file mode 100644
index 214100d..0000000
--- a/wpilibc/src/main/native/include/Encoder.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Encoder.h is deprecated; include frc/Encoder.h instead"
-#else
-#warning "Encoder.h is deprecated; include frc/Encoder.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Encoder.h"
diff --git a/wpilibc/src/main/native/include/Error.h b/wpilibc/src/main/native/include/Error.h
deleted file mode 100644
index f3fb5de..0000000
--- a/wpilibc/src/main/native/include/Error.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Error.h is deprecated; include frc/Error.h instead"
-#else
-#warning "Error.h is deprecated; include frc/Error.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Error.h"
diff --git a/wpilibc/src/main/native/include/ErrorBase.h b/wpilibc/src/main/native/include/ErrorBase.h
deleted file mode 100644
index eb300d9..0000000
--- a/wpilibc/src/main/native/include/ErrorBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: ErrorBase.h is deprecated; include frc/ErrorBase.h instead"
-#else
-#warning "ErrorBase.h is deprecated; include frc/ErrorBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/ErrorBase.h"
diff --git a/wpilibc/src/main/native/include/Filters/Filter.h b/wpilibc/src/main/native/include/Filters/Filter.h
deleted file mode 100644
index 4fd382f..0000000
--- a/wpilibc/src/main/native/include/Filters/Filter.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Filters/Filter.h is deprecated; include frc/filters/Filter.h instead"
-#else
-#warning "Filters/Filter.h is deprecated; include frc/filters/Filter.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/filters/Filter.h"
diff --git a/wpilibc/src/main/native/include/Filters/LinearDigitalFilter.h b/wpilibc/src/main/native/include/Filters/LinearDigitalFilter.h
deleted file mode 100644
index b0da22c..0000000
--- a/wpilibc/src/main/native/include/Filters/LinearDigitalFilter.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Filters/LinearDigitalFilter.h is deprecated; include frc/filters/LinearDigitalFilter.h instead"
-#else
-#warning "Filters/LinearDigitalFilter.h is deprecated; include frc/filters/LinearDigitalFilter.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/filters/LinearDigitalFilter.h"
diff --git a/wpilibc/src/main/native/include/GamepadBase.h b/wpilibc/src/main/native/include/GamepadBase.h
deleted file mode 100644
index bc8d809..0000000
--- a/wpilibc/src/main/native/include/GamepadBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: GamepadBase.h is deprecated; include frc/GamepadBase.h instead"
-#else
-#warning "GamepadBase.h is deprecated; include frc/GamepadBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/GamepadBase.h"
diff --git a/wpilibc/src/main/native/include/GearTooth.h b/wpilibc/src/main/native/include/GearTooth.h
deleted file mode 100644
index 2c56f95..0000000
--- a/wpilibc/src/main/native/include/GearTooth.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: GearTooth.h is deprecated; include frc/GearTooth.h instead"
-#else
-#warning "GearTooth.h is deprecated; include frc/GearTooth.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/GearTooth.h"
diff --git a/wpilibc/src/main/native/include/GenericHID.h b/wpilibc/src/main/native/include/GenericHID.h
deleted file mode 100644
index d305692..0000000
--- a/wpilibc/src/main/native/include/GenericHID.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: GenericHID.h is deprecated; include frc/GenericHID.h instead"
-#else
-#warning "GenericHID.h is deprecated; include frc/GenericHID.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/GenericHID.h"
diff --git a/wpilibc/src/main/native/include/GyroBase.h b/wpilibc/src/main/native/include/GyroBase.h
deleted file mode 100644
index f2b9097..0000000
--- a/wpilibc/src/main/native/include/GyroBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: GyroBase.h is deprecated; include frc/GyroBase.h instead"
-#else
-#warning "GyroBase.h is deprecated; include frc/GyroBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/GyroBase.h"
diff --git a/wpilibc/src/main/native/include/I2C.h b/wpilibc/src/main/native/include/I2C.h
deleted file mode 100644
index 594fdaf..0000000
--- a/wpilibc/src/main/native/include/I2C.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: I2C.h is deprecated; include frc/I2C.h instead"
-#else
-#warning "I2C.h is deprecated; include frc/I2C.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/I2C.h"
diff --git a/wpilibc/src/main/native/include/InterruptableSensorBase.h b/wpilibc/src/main/native/include/InterruptableSensorBase.h
deleted file mode 100644
index 43e4f74..0000000
--- a/wpilibc/src/main/native/include/InterruptableSensorBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: InterruptableSensorBase.h is deprecated; include frc/InterruptableSensorBase.h instead"
-#else
-#warning "InterruptableSensorBase.h is deprecated; include frc/InterruptableSensorBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/InterruptableSensorBase.h"
diff --git a/wpilibc/src/main/native/include/IterativeRobot.h b/wpilibc/src/main/native/include/IterativeRobot.h
deleted file mode 100644
index 7ad7973..0000000
--- a/wpilibc/src/main/native/include/IterativeRobot.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: IterativeRobot.h is deprecated; include frc/IterativeRobot.h instead"
-#else
-#warning "IterativeRobot.h is deprecated; include frc/IterativeRobot.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/IterativeRobot.h"
diff --git a/wpilibc/src/main/native/include/IterativeRobotBase.h b/wpilibc/src/main/native/include/IterativeRobotBase.h
deleted file mode 100644
index e87c547..0000000
--- a/wpilibc/src/main/native/include/IterativeRobotBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: IterativeRobotBase.h is deprecated; include frc/IterativeRobotBase.h instead"
-#else
-#warning "IterativeRobotBase.h is deprecated; include frc/IterativeRobotBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/IterativeRobotBase.h"
diff --git a/wpilibc/src/main/native/include/Jaguar.h b/wpilibc/src/main/native/include/Jaguar.h
deleted file mode 100644
index 7eea49d..0000000
--- a/wpilibc/src/main/native/include/Jaguar.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Jaguar.h is deprecated; include frc/Jaguar.h instead"
-#else
-#warning "Jaguar.h is deprecated; include frc/Jaguar.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Jaguar.h"
diff --git a/wpilibc/src/main/native/include/Joystick.h b/wpilibc/src/main/native/include/Joystick.h
deleted file mode 100644
index febc148..0000000
--- a/wpilibc/src/main/native/include/Joystick.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Joystick.h is deprecated; include frc/Joystick.h instead"
-#else
-#warning "Joystick.h is deprecated; include frc/Joystick.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Joystick.h"
diff --git a/wpilibc/src/main/native/include/JoystickBase.h b/wpilibc/src/main/native/include/JoystickBase.h
deleted file mode 100644
index 4aa2a59..0000000
--- a/wpilibc/src/main/native/include/JoystickBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: JoystickBase.h is deprecated; include frc/JoystickBase.h instead"
-#else
-#warning "JoystickBase.h is deprecated; include frc/JoystickBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/JoystickBase.h"
diff --git a/wpilibc/src/main/native/include/LiveWindow/LiveWindow.h b/wpilibc/src/main/native/include/LiveWindow/LiveWindow.h
deleted file mode 100644
index 99e7628..0000000
--- a/wpilibc/src/main/native/include/LiveWindow/LiveWindow.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: LiveWindow/LiveWindow.h is deprecated; include frc/livewindow/LiveWindow.h instead"
-#else
-#warning "LiveWindow/LiveWindow.h is deprecated; include frc/livewindow/LiveWindow.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/livewindow/LiveWindow.h"
diff --git a/wpilibc/src/main/native/include/LiveWindow/LiveWindowSendable.h b/wpilibc/src/main/native/include/LiveWindow/LiveWindowSendable.h
deleted file mode 100644
index cc8082b..0000000
--- a/wpilibc/src/main/native/include/LiveWindow/LiveWindowSendable.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: LiveWindow/LiveWindowSendable.h is deprecated; include frc/livewindow/LiveWindowSendable.h instead"
-#else
-#warning "LiveWindow/LiveWindowSendable.h is deprecated; include frc/livewindow/LiveWindowSendable.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/livewindow/LiveWindowSendable.h"
diff --git a/wpilibc/src/main/native/include/MotorSafety.h b/wpilibc/src/main/native/include/MotorSafety.h
deleted file mode 100644
index a33fd0b..0000000
--- a/wpilibc/src/main/native/include/MotorSafety.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: MotorSafety.h is deprecated; include frc/MotorSafety.h instead"
-#else
-#warning "MotorSafety.h is deprecated; include frc/MotorSafety.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/MotorSafety.h"
diff --git a/wpilibc/src/main/native/include/NidecBrushless.h b/wpilibc/src/main/native/include/NidecBrushless.h
deleted file mode 100644
index 0937787..0000000
--- a/wpilibc/src/main/native/include/NidecBrushless.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: NidecBrushless.h is deprecated; include frc/NidecBrushless.h instead"
-#else
-#warning "NidecBrushless.h is deprecated; include frc/NidecBrushless.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/NidecBrushless.h"
diff --git a/wpilibc/src/main/native/include/Notifier.h b/wpilibc/src/main/native/include/Notifier.h
deleted file mode 100644
index 28e6a20..0000000
--- a/wpilibc/src/main/native/include/Notifier.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Notifier.h is deprecated; include frc/Notifier.h instead"
-#else
-#warning "Notifier.h is deprecated; include frc/Notifier.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Notifier.h"
diff --git a/wpilibc/src/main/native/include/PIDBase.h b/wpilibc/src/main/native/include/PIDBase.h
deleted file mode 100644
index b9e2d50..0000000
--- a/wpilibc/src/main/native/include/PIDBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: PIDBase.h is deprecated; include frc/PIDBase.h instead"
-#else
-#warning "PIDBase.h is deprecated; include frc/PIDBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/PIDBase.h"
diff --git a/wpilibc/src/main/native/include/PIDController.h b/wpilibc/src/main/native/include/PIDController.h
deleted file mode 100644
index 38777ea..0000000
--- a/wpilibc/src/main/native/include/PIDController.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: PIDController.h is deprecated; include frc/PIDController.h instead"
-#else
-#warning "PIDController.h is deprecated; include frc/PIDController.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/PIDController.h"
diff --git a/wpilibc/src/main/native/include/PIDInterface.h b/wpilibc/src/main/native/include/PIDInterface.h
deleted file mode 100644
index 8e0f10e..0000000
--- a/wpilibc/src/main/native/include/PIDInterface.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: PIDInterface.h is deprecated; include frc/PIDInterface.h instead"
-#else
-#warning "PIDInterface.h is deprecated; include frc/PIDInterface.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/PIDInterface.h"
diff --git a/wpilibc/src/main/native/include/PIDOutput.h b/wpilibc/src/main/native/include/PIDOutput.h
deleted file mode 100644
index 857ce0f..0000000
--- a/wpilibc/src/main/native/include/PIDOutput.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: PIDOutput.h is deprecated; include frc/PIDOutput.h instead"
-#else
-#warning "PIDOutput.h is deprecated; include frc/PIDOutput.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/PIDOutput.h"
diff --git a/wpilibc/src/main/native/include/PIDSource.h b/wpilibc/src/main/native/include/PIDSource.h
deleted file mode 100644
index 29b8469..0000000
--- a/wpilibc/src/main/native/include/PIDSource.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: PIDSource.h is deprecated; include frc/PIDSource.h instead"
-#else
-#warning "PIDSource.h is deprecated; include frc/PIDSource.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/PIDSource.h"
diff --git a/wpilibc/src/main/native/include/PWM.h b/wpilibc/src/main/native/include/PWM.h
deleted file mode 100644
index 989c4f7..0000000
--- a/wpilibc/src/main/native/include/PWM.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: PWM.h is deprecated; include frc/PWM.h instead"
-#else
-#warning "PWM.h is deprecated; include frc/PWM.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/PWM.h"
diff --git a/wpilibc/src/main/native/include/PWMSpeedController.h b/wpilibc/src/main/native/include/PWMSpeedController.h
deleted file mode 100644
index f15f384..0000000
--- a/wpilibc/src/main/native/include/PWMSpeedController.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: PWMSpeedController.h is deprecated; include frc/PWMSpeedController.h instead"
-#else
-#warning "PWMSpeedController.h is deprecated; include frc/PWMSpeedController.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/PWMSpeedController.h"
diff --git a/wpilibc/src/main/native/include/PWMTalonSRX.h b/wpilibc/src/main/native/include/PWMTalonSRX.h
deleted file mode 100644
index 43a132a..0000000
--- a/wpilibc/src/main/native/include/PWMTalonSRX.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: PWMTalonSRX.h is deprecated; include frc/PWMTalonSRX.h instead"
-#else
-#warning "PWMTalonSRX.h is deprecated; include frc/PWMTalonSRX.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/PWMTalonSRX.h"
diff --git a/wpilibc/src/main/native/include/PWMVictorSPX.h b/wpilibc/src/main/native/include/PWMVictorSPX.h
deleted file mode 100644
index 0fd6a1e..0000000
--- a/wpilibc/src/main/native/include/PWMVictorSPX.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: PWMVictorSPX.h is deprecated; include frc/PWMVictorSPX.h instead"
-#else
-#warning "PWMVictorSPX.h is deprecated; include frc/PWMVictorSPX.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/PWMVictorSPX.h"
diff --git a/wpilibc/src/main/native/include/PowerDistributionPanel.h b/wpilibc/src/main/native/include/PowerDistributionPanel.h
deleted file mode 100644
index fa4cae5..0000000
--- a/wpilibc/src/main/native/include/PowerDistributionPanel.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: PowerDistributionPanel.h is deprecated; include frc/PowerDistributionPanel.h instead"
-#else
-#warning "PowerDistributionPanel.h is deprecated; include frc/PowerDistributionPanel.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/PowerDistributionPanel.h"
diff --git a/wpilibc/src/main/native/include/Preferences.h b/wpilibc/src/main/native/include/Preferences.h
deleted file mode 100644
index 3a857f3..0000000
--- a/wpilibc/src/main/native/include/Preferences.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Preferences.h is deprecated; include frc/Preferences.h instead"
-#else
-#warning "Preferences.h is deprecated; include frc/Preferences.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Preferences.h"
diff --git a/wpilibc/src/main/native/include/Relay.h b/wpilibc/src/main/native/include/Relay.h
deleted file mode 100644
index 22896fe..0000000
--- a/wpilibc/src/main/native/include/Relay.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Relay.h is deprecated; include frc/Relay.h instead"
-#else
-#warning "Relay.h is deprecated; include frc/Relay.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Relay.h"
diff --git a/wpilibc/src/main/native/include/Resource.h b/wpilibc/src/main/native/include/Resource.h
deleted file mode 100644
index c314eb3..0000000
--- a/wpilibc/src/main/native/include/Resource.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Resource.h is deprecated; include frc/Resource.h instead"
-#else
-#warning "Resource.h is deprecated; include frc/Resource.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Resource.h"
diff --git a/wpilibc/src/main/native/include/RobotBase.h b/wpilibc/src/main/native/include/RobotBase.h
deleted file mode 100644
index f6faa81..0000000
--- a/wpilibc/src/main/native/include/RobotBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: RobotBase.h is deprecated; include frc/RobotBase.h instead"
-#else
-#warning "RobotBase.h is deprecated; include frc/RobotBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/RobotBase.h"
diff --git a/wpilibc/src/main/native/include/RobotController.h b/wpilibc/src/main/native/include/RobotController.h
deleted file mode 100644
index 0cf64b2..0000000
--- a/wpilibc/src/main/native/include/RobotController.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: RobotController.h is deprecated; include frc/RobotController.h instead"
-#else
-#warning "RobotController.h is deprecated; include frc/RobotController.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/RobotController.h"
diff --git a/wpilibc/src/main/native/include/RobotDrive.h b/wpilibc/src/main/native/include/RobotDrive.h
deleted file mode 100644
index 7b34cc4..0000000
--- a/wpilibc/src/main/native/include/RobotDrive.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: RobotDrive.h is deprecated; include frc/RobotDrive.h instead"
-#else
-#warning "RobotDrive.h is deprecated; include frc/RobotDrive.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/RobotDrive.h"
diff --git a/wpilibc/src/main/native/include/RobotState.h b/wpilibc/src/main/native/include/RobotState.h
deleted file mode 100644
index 329b8ed..0000000
--- a/wpilibc/src/main/native/include/RobotState.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: RobotState.h is deprecated; include frc/RobotState.h instead"
-#else
-#warning "RobotState.h is deprecated; include frc/RobotState.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/RobotState.h"
diff --git a/wpilibc/src/main/native/include/SD540.h b/wpilibc/src/main/native/include/SD540.h
deleted file mode 100644
index 8518703..0000000
--- a/wpilibc/src/main/native/include/SD540.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SD540.h is deprecated; include frc/SD540.h instead"
-#else
-#warning "SD540.h is deprecated; include frc/SD540.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/SD540.h"
diff --git a/wpilibc/src/main/native/include/SPI.h b/wpilibc/src/main/native/include/SPI.h
deleted file mode 100644
index 827a10c..0000000
--- a/wpilibc/src/main/native/include/SPI.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SPI.h is deprecated; include frc/SPI.h instead"
-#else
-#warning "SPI.h is deprecated; include frc/SPI.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/SPI.h"
diff --git a/wpilibc/src/main/native/include/SampleRobot.h b/wpilibc/src/main/native/include/SampleRobot.h
deleted file mode 100644
index 16ca36a..0000000
--- a/wpilibc/src/main/native/include/SampleRobot.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SampleRobot.h is deprecated; include frc/SampleRobot.h instead"
-#else
-#warning "SampleRobot.h is deprecated; include frc/SampleRobot.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/SampleRobot.h"
diff --git a/wpilibc/src/main/native/include/SensorUtil.h b/wpilibc/src/main/native/include/SensorUtil.h
deleted file mode 100644
index 8fd7b36..0000000
--- a/wpilibc/src/main/native/include/SensorUtil.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SensorUtil.h is deprecated; include frc/SensorUtil.h instead"
-#else
-#warning "SensorUtil.h is deprecated; include frc/SensorUtil.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/SensorUtil.h"
diff --git a/wpilibc/src/main/native/include/SerialPort.h b/wpilibc/src/main/native/include/SerialPort.h
deleted file mode 100644
index 4df02ca..0000000
--- a/wpilibc/src/main/native/include/SerialPort.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SerialPort.h is deprecated; include frc/SerialPort.h instead"
-#else
-#warning "SerialPort.h is deprecated; include frc/SerialPort.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/SerialPort.h"
diff --git a/wpilibc/src/main/native/include/Servo.h b/wpilibc/src/main/native/include/Servo.h
deleted file mode 100644
index 99cf2f2..0000000
--- a/wpilibc/src/main/native/include/Servo.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Servo.h is deprecated; include frc/Servo.h instead"
-#else
-#warning "Servo.h is deprecated; include frc/Servo.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Servo.h"
diff --git a/wpilibc/src/main/native/include/SmartDashboard/NamedSendable.h b/wpilibc/src/main/native/include/SmartDashboard/NamedSendable.h
deleted file mode 100644
index 7cbcbe7..0000000
--- a/wpilibc/src/main/native/include/SmartDashboard/NamedSendable.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SmartDashboard/NamedSendable.h is deprecated; include frc/smartdashboard/NamedSendable.h instead"
-#else
-#warning "SmartDashboard/NamedSendable.h is deprecated; include frc/smartdashboard/NamedSendable.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/smartdashboard/NamedSendable.h"
diff --git a/wpilibc/src/main/native/include/SmartDashboard/Sendable.h b/wpilibc/src/main/native/include/SmartDashboard/Sendable.h
deleted file mode 100644
index 187c1ea..0000000
--- a/wpilibc/src/main/native/include/SmartDashboard/Sendable.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: smartdashboard/Sendable.h is deprecated; include frc/smartdashboard/Sendable.h instead"
-#else
-#warning "smartdashboard/Sendable.h is deprecated; include frc/smartdashboard/Sendable.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/smartdashboard/Sendable.h"
diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableBase.h b/wpilibc/src/main/native/include/SmartDashboard/SendableBase.h
deleted file mode 100644
index d898040..0000000
--- a/wpilibc/src/main/native/include/SmartDashboard/SendableBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: smartdashboard/SendableBase.h is deprecated; include frc/smartdashboard/SendableBase.h instead"
-#else
-#warning "smartdashboard/SendableBase.h is deprecated; include frc/smartdashboard/SendableBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/smartdashboard/SendableBase.h"
diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableBuilder.h b/wpilibc/src/main/native/include/SmartDashboard/SendableBuilder.h
deleted file mode 100644
index 7102c35..0000000
--- a/wpilibc/src/main/native/include/SmartDashboard/SendableBuilder.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: smartdashboard/SendableBuilder.h is deprecated; include frc/smartdashboard/SendableBuilder.h instead"
-#else
-#warning "smartdashboard/SendableBuilder.h is deprecated; include frc/smartdashboard/SendableBuilder.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/smartdashboard/SendableBuilder.h"
diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableBuilderImpl.h b/wpilibc/src/main/native/include/SmartDashboard/SendableBuilderImpl.h
deleted file mode 100644
index b956309..0000000
--- a/wpilibc/src/main/native/include/SmartDashboard/SendableBuilderImpl.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: smartdashboard/SendableBuilderImpl.h is deprecated; include frc/smartdashboard/SendableBuilderImpl.h instead"
-#else
-#warning "smartdashboard/SendableBuilderImpl.h is deprecated; include frc/smartdashboard/SendableBuilderImpl.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/smartdashboard/SendableBuilderImpl.h"
diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.h b/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.h
deleted file mode 100644
index 5e4683f..0000000
--- a/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: smartdashboard/SendableChooser.h is deprecated; include frc/smartdashboard/SendableChooser.h instead"
-#else
-#warning "smartdashboard/SendableChooser.h is deprecated; include frc/smartdashboard/SendableChooser.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/smartdashboard/SendableChooser.h"
diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.inc b/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.inc
deleted file mode 100644
index 79e8b16..0000000
--- a/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.inc
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: smartdashboard/SendableChooser.inc is deprecated; include frc/smartdashboard/SendableChooser.inc instead"
-#else
-#warning "smartdashboard/SendableChooser.inc is deprecated; include frc/smartdashboard/SendableChooser.inc instead"
-#endif
-
-// clang-format on
-
-#include "frc/smartdashboard/SendableChooser.inc"
diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableChooserBase.h b/wpilibc/src/main/native/include/SmartDashboard/SendableChooserBase.h
deleted file mode 100644
index 436675c..0000000
--- a/wpilibc/src/main/native/include/SmartDashboard/SendableChooserBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: smartdashboard/SendableChooserBase.h is deprecated; include frc/smartdashboard/SendableChooserBase.h instead"
-#else
-#warning "smartdashboard/SendableChooserBase.h is deprecated; include frc/smartdashboard/SendableChooserBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/smartdashboard/SendableChooserBase.h"
diff --git a/wpilibc/src/main/native/include/SmartDashboard/SmartDashboard.h b/wpilibc/src/main/native/include/SmartDashboard/SmartDashboard.h
deleted file mode 100644
index cc7c75f..0000000
--- a/wpilibc/src/main/native/include/SmartDashboard/SmartDashboard.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: smartdashboard/SmartDashboard.h is deprecated; include frc/smartdashboard/SmartDashboard.h instead"
-#else
-#warning "smartdashboard/SmartDashboard.h is deprecated; include frc/smartdashboard/SmartDashboard.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/smartdashboard/SmartDashboard.h"
diff --git a/wpilibc/src/main/native/include/Solenoid.h b/wpilibc/src/main/native/include/Solenoid.h
deleted file mode 100644
index b565188..0000000
--- a/wpilibc/src/main/native/include/Solenoid.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Solenoid.h is deprecated; include frc/Solenoid.h instead"
-#else
-#warning "Solenoid.h is deprecated; include frc/Solenoid.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Solenoid.h"
diff --git a/wpilibc/src/main/native/include/SolenoidBase.h b/wpilibc/src/main/native/include/SolenoidBase.h
deleted file mode 100644
index 9678200..0000000
--- a/wpilibc/src/main/native/include/SolenoidBase.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SolenoidBase.h is deprecated; include frc/SolenoidBase.h instead"
-#else
-#warning "SolenoidBase.h is deprecated; include frc/SolenoidBase.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/SolenoidBase.h"
diff --git a/wpilibc/src/main/native/include/Spark.h b/wpilibc/src/main/native/include/Spark.h
deleted file mode 100644
index b01c2c9..0000000
--- a/wpilibc/src/main/native/include/Spark.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Spark.h is deprecated; include frc/Spark.h instead"
-#else
-#warning "Spark.h is deprecated; include frc/Spark.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Spark.h"
diff --git a/wpilibc/src/main/native/include/SpeedController.h b/wpilibc/src/main/native/include/SpeedController.h
deleted file mode 100644
index 27f0c9d..0000000
--- a/wpilibc/src/main/native/include/SpeedController.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SpeedController.h is deprecated; include frc/SpeedController.h instead"
-#else
-#warning "SpeedController.h is deprecated; include frc/SpeedController.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/SpeedController.h"
diff --git a/wpilibc/src/main/native/include/SpeedControllerGroup.h b/wpilibc/src/main/native/include/SpeedControllerGroup.h
deleted file mode 100644
index ae387b6..0000000
--- a/wpilibc/src/main/native/include/SpeedControllerGroup.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SpeedControllerGroup.h is deprecated; include frc/SpeedControllerGroup.h instead"
-#else
-#warning "SpeedControllerGroup.h is deprecated; include frc/SpeedControllerGroup.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/SpeedControllerGroup.h"
diff --git a/wpilibc/src/main/native/include/SpeedControllerGroup.inc b/wpilibc/src/main/native/include/SpeedControllerGroup.inc
deleted file mode 100644
index f1f6271..0000000
--- a/wpilibc/src/main/native/include/SpeedControllerGroup.inc
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SpeedControllerGroup.inc is deprecated; include frc/SpeedControllerGroup.inc instead"
-#else
-#warning "SpeedControllerGroup.inc is deprecated; include frc/SpeedControllerGroup.inc instead"
-#endif
-
-// clang-format on
-
-#include "frc/SpeedControllerGroup.inc"
diff --git a/wpilibc/src/main/native/include/SynchronousPID.h b/wpilibc/src/main/native/include/SynchronousPID.h
deleted file mode 100644
index f2a9c53..0000000
--- a/wpilibc/src/main/native/include/SynchronousPID.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: SynchronousPID.h is deprecated; include frc/SynchronousPID.h instead"
-#else
-#warning "SynchronousPID.h is deprecated; include frc/SynchronousPID.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/SynchronousPID.h"
diff --git a/wpilibc/src/main/native/include/Talon.h b/wpilibc/src/main/native/include/Talon.h
deleted file mode 100644
index 59c431c..0000000
--- a/wpilibc/src/main/native/include/Talon.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Talon.h is deprecated; include frc/Talon.h instead"
-#else
-#warning "Talon.h is deprecated; include frc/Talon.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Talon.h"
diff --git a/wpilibc/src/main/native/include/Threads.h b/wpilibc/src/main/native/include/Threads.h
deleted file mode 100644
index 3a9561f..0000000
--- a/wpilibc/src/main/native/include/Threads.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Threads.h is deprecated; include frc/Threads.h instead"
-#else
-#warning "Threads.h is deprecated; include frc/Threads.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Threads.h"
diff --git a/wpilibc/src/main/native/include/TimedRobot.h b/wpilibc/src/main/native/include/TimedRobot.h
deleted file mode 100644
index 3c5f393..0000000
--- a/wpilibc/src/main/native/include/TimedRobot.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: TimedRobot.h is deprecated; include frc/TimedRobot.h instead"
-#else
-#warning "TimedRobot.h is deprecated; include frc/TimedRobot.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/TimedRobot.h"
diff --git a/wpilibc/src/main/native/include/Timer.h b/wpilibc/src/main/native/include/Timer.h
deleted file mode 100644
index 2df6097..0000000
--- a/wpilibc/src/main/native/include/Timer.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Timer.h is deprecated; include frc/Timer.h instead"
-#else
-#warning "Timer.h is deprecated; include frc/Timer.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Timer.h"
diff --git a/wpilibc/src/main/native/include/Ultrasonic.h b/wpilibc/src/main/native/include/Ultrasonic.h
deleted file mode 100644
index 8c32ad8..0000000
--- a/wpilibc/src/main/native/include/Ultrasonic.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Ultrasonic.h is deprecated; include frc/Ultrasonic.h instead"
-#else
-#warning "Ultrasonic.h is deprecated; include frc/Ultrasonic.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Ultrasonic.h"
diff --git a/wpilibc/src/main/native/include/Utility.h b/wpilibc/src/main/native/include/Utility.h
deleted file mode 100644
index 2294238..0000000
--- a/wpilibc/src/main/native/include/Utility.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Utility.h is deprecated; include frc/Utility.h instead"
-#else
-#warning "Utility.h is deprecated; include frc/Utility.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Utility.h"
diff --git a/wpilibc/src/main/native/include/Victor.h b/wpilibc/src/main/native/include/Victor.h
deleted file mode 100644
index d17df6d..0000000
--- a/wpilibc/src/main/native/include/Victor.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Victor.h is deprecated; include frc/Victor.h instead"
-#else
-#warning "Victor.h is deprecated; include frc/Victor.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Victor.h"
diff --git a/wpilibc/src/main/native/include/VictorSP.h b/wpilibc/src/main/native/include/VictorSP.h
deleted file mode 100644
index a0c8616..0000000
--- a/wpilibc/src/main/native/include/VictorSP.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: VictorSP.h is deprecated; include frc/VictorSP.h instead"
-#else
-#warning "VictorSP.h is deprecated; include frc/VictorSP.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/VictorSP.h"
diff --git a/wpilibc/src/main/native/include/WPIErrors.h b/wpilibc/src/main/native/include/WPIErrors.h
deleted file mode 100644
index 6893bce..0000000
--- a/wpilibc/src/main/native/include/WPIErrors.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: WPIErrors.h is deprecated; include frc/WPIErrors.h instead"
-#else
-#warning "WPIErrors.h is deprecated; include frc/WPIErrors.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/WPIErrors.h"
diff --git a/wpilibc/src/main/native/include/WPILib.h b/wpilibc/src/main/native/include/WPILib.h
deleted file mode 100644
index ab3e608..0000000
--- a/wpilibc/src/main/native/include/WPILib.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: WPILib.h is deprecated; include frc/WPILib.h instead"
-#else
-#warning "WPILib.h is deprecated; include frc/WPILib.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/WPILib.h"
diff --git a/wpilibc/src/main/native/include/Watchdog.h b/wpilibc/src/main/native/include/Watchdog.h
deleted file mode 100644
index 7ba2bf8..0000000
--- a/wpilibc/src/main/native/include/Watchdog.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: Watchdog.h is deprecated; include frc/Watchdog.h instead"
-#else
-#warning "Watchdog.h is deprecated; include frc/Watchdog.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/Watchdog.h"
diff --git a/wpilibc/src/main/native/include/XboxController.h b/wpilibc/src/main/native/include/XboxController.h
deleted file mode 100644
index b0abca9..0000000
--- a/wpilibc/src/main/native/include/XboxController.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: XboxController.h is deprecated; include frc/XboxController.h instead"
-#else
-#warning "XboxController.h is deprecated; include frc/XboxController.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/XboxController.h"
diff --git a/wpilibc/src/main/native/include/circular_buffer.h b/wpilibc/src/main/native/include/circular_buffer.h
deleted file mode 100644
index a150058..0000000
--- a/wpilibc/src/main/native/include/circular_buffer.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: circular_buffer.h is deprecated; include frc/circular_buffer.h instead"
-#else
-#warning "circular_buffer.h is deprecated; include frc/circular_buffer.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/circular_buffer.h"
diff --git a/wpilibc/src/main/native/include/circular_buffer.inc b/wpilibc/src/main/native/include/circular_buffer.inc
deleted file mode 100644
index 417286a..0000000
--- a/wpilibc/src/main/native/include/circular_buffer.inc
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: circular_buffer.inc is deprecated; include frc/circular_buffer.inc instead"
-#else
-#warning "circular_buffer.inc is deprecated; include frc/circular_buffer.inc instead"
-#endif
-
-// clang-format on
-
-#include "frc/circular_buffer.inc"
diff --git a/wpilibc/src/main/native/include/frc/ADXL345_I2C.h b/wpilibc/src/main/native/include/frc/ADXL345_I2C.h
index 5d5fed4..202acbb 100644
--- a/wpilibc/src/main/native/include/frc/ADXL345_I2C.h
+++ b/wpilibc/src/main/native/include/frc/ADXL345_I2C.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,13 +7,18 @@
#pragma once
+#include <hal/SimDevice.h>
+
#include "frc/ErrorBase.h"
#include "frc/I2C.h"
#include "frc/interfaces/Accelerometer.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* ADXL345 Accelerometer on I2C.
*
@@ -22,8 +27,9 @@
* 0x1D (7-bit address).
*/
class ADXL345_I2C : public ErrorBase,
- public SendableBase,
- public Accelerometer {
+ public Accelerometer,
+ public Sendable,
+ public SendableHelper<ADXL345_I2C> {
public:
enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 };
@@ -74,6 +80,12 @@
protected:
I2C m_i2c;
+ hal::SimDevice m_simDevice;
+ hal::SimEnum m_simRange;
+ hal::SimDouble m_simX;
+ hal::SimDouble m_simY;
+ hal::SimDouble m_simZ;
+
static constexpr int kAddress = 0x1D;
static constexpr int kPowerCtlRegister = 0x2D;
static constexpr int kDataFormatRegister = 0x31;
diff --git a/wpilibc/src/main/native/include/frc/ADXL345_SPI.h b/wpilibc/src/main/native/include/frc/ADXL345_SPI.h
index ac5d6d9..90454c0 100644
--- a/wpilibc/src/main/native/include/frc/ADXL345_SPI.h
+++ b/wpilibc/src/main/native/include/frc/ADXL345_SPI.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,10 +7,13 @@
#pragma once
+#include <hal/SimDevice.h>
+
#include "frc/ErrorBase.h"
#include "frc/SPI.h"
#include "frc/interfaces/Accelerometer.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -21,8 +24,9 @@
* via SPI. This class assumes the sensor is wired in 4-wire SPI mode.
*/
class ADXL345_SPI : public ErrorBase,
- public SendableBase,
- public Accelerometer {
+ public Accelerometer,
+ public Sendable,
+ public SendableHelper<ADXL345_SPI> {
public:
enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 };
@@ -72,6 +76,12 @@
protected:
SPI m_spi;
+ hal::SimDevice m_simDevice;
+ hal::SimEnum m_simRange;
+ hal::SimDouble m_simX;
+ hal::SimDouble m_simY;
+ hal::SimDouble m_simZ;
+
static constexpr int kPowerCtlRegister = 0x2D;
static constexpr int kDataFormatRegister = 0x31;
static constexpr int kDataRegister = 0x32;
diff --git a/wpilibc/src/main/native/include/frc/ADXL362.h b/wpilibc/src/main/native/include/frc/ADXL362.h
index 896af15..e1d659b 100644
--- a/wpilibc/src/main/native/include/frc/ADXL362.h
+++ b/wpilibc/src/main/native/include/frc/ADXL362.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,19 +7,27 @@
#pragma once
+#include <hal/SimDevice.h>
+
#include "frc/ErrorBase.h"
#include "frc/SPI.h"
#include "frc/interfaces/Accelerometer.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* ADXL362 SPI Accelerometer.
*
* This class allows access to an Analog Devices ADXL362 3-axis accelerometer.
*/
-class ADXL362 : public ErrorBase, public SendableBase, public Accelerometer {
+class ADXL362 : public ErrorBase,
+ public Accelerometer,
+ public Sendable,
+ public SendableHelper<ADXL362> {
public:
enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 };
struct AllAxes {
@@ -44,7 +52,7 @@
*/
explicit ADXL362(SPI::Port port, Range range = kRange_2G);
- virtual ~ADXL362() = default;
+ ~ADXL362() override = default;
ADXL362(ADXL362&&) = default;
ADXL362& operator=(ADXL362&&) = default;
@@ -75,6 +83,11 @@
private:
SPI m_spi;
+ hal::SimDevice m_simDevice;
+ hal::SimEnum m_simRange;
+ hal::SimDouble m_simX;
+ hal::SimDouble m_simY;
+ hal::SimDouble m_simZ;
double m_gsPerLSB = 0.001;
};
diff --git a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
index e1b70e2..ccdb75c 100644
--- a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
+++ b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <stdint.h>
+#include <hal/SimDevice.h>
+
#include "frc/GyroBase.h"
#include "frc/SPI.h"
@@ -40,7 +42,7 @@
*/
explicit ADXRS450_Gyro(SPI::Port port);
- virtual ~ADXRS450_Gyro() = default;
+ ~ADXRS450_Gyro() override = default;
ADXRS450_Gyro(ADXRS450_Gyro&&) = default;
ADXRS450_Gyro& operator=(ADXRS450_Gyro&&) = default;
@@ -94,6 +96,10 @@
private:
SPI m_spi;
+ hal::SimDevice m_simDevice;
+ hal::SimDouble m_simAngle;
+ hal::SimDouble m_simRate;
+
uint16_t ReadRegister(int reg);
};
diff --git a/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h b/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h
index 80dc98e..27015e4 100644
--- a/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h
+++ b/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,10 +12,13 @@
#include "frc/AnalogInput.h"
#include "frc/ErrorBase.h"
#include "frc/PIDSource.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Handle operation of an analog accelerometer.
*
@@ -24,8 +27,9 @@
* calibrated by finding the center value over a period of time.
*/
class AnalogAccelerometer : public ErrorBase,
- public SendableBase,
- public PIDSource {
+ public PIDSource,
+ public Sendable,
+ public SendableHelper<AnalogAccelerometer> {
public:
/**
* Create a new instance of an accelerometer.
diff --git a/wpilibc/src/main/native/include/frc/AnalogGyro.h b/wpilibc/src/main/native/include/frc/AnalogGyro.h
index 9e18d89..bcc67c9 100644
--- a/wpilibc/src/main/native/include/frc/AnalogGyro.h
+++ b/wpilibc/src/main/native/include/frc/AnalogGyro.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,6 +12,8 @@
#include <hal/Types.h>
#include "frc/GyroBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -100,7 +102,7 @@
*/
AnalogGyro(std::shared_ptr<AnalogInput> channel, int center, double offset);
- virtual ~AnalogGyro();
+ ~AnalogGyro() override;
AnalogGyro(AnalogGyro&& rhs);
AnalogGyro& operator=(AnalogGyro&& rhs);
@@ -188,7 +190,7 @@
std::shared_ptr<AnalogInput> m_analog;
private:
- HAL_GyroHandle m_gyroHandle = HAL_kInvalidHandle;
+ hal::Handle<HAL_GyroHandle> m_gyroHandle;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/AnalogInput.h b/wpilibc/src/main/native/include/frc/AnalogInput.h
index 97ce2a8..6567542 100644
--- a/wpilibc/src/main/native/include/frc/AnalogInput.h
+++ b/wpilibc/src/main/native/include/frc/AnalogInput.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,10 +13,13 @@
#include "frc/ErrorBase.h"
#include "frc/PIDSource.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Analog input class.
*
@@ -29,7 +32,10 @@
* are divided by the number of samples to retain the resolution, but get more
* stable values.
*/
-class AnalogInput : public ErrorBase, public SendableBase, public PIDSource {
+class AnalogInput : public ErrorBase,
+ public PIDSource,
+ public Sendable,
+ public SendableHelper<AnalogInput> {
friend class AnalogTrigger;
friend class AnalogGyro;
@@ -48,8 +54,8 @@
~AnalogInput() override;
- AnalogInput(AnalogInput&& rhs);
- AnalogInput& operator=(AnalogInput&& rhs);
+ AnalogInput(AnalogInput&&) = default;
+ AnalogInput& operator=(AnalogInput&&) = default;
/**
* Get a sample straight from this channel.
@@ -280,11 +286,18 @@
*/
double PIDGet() override;
+ /**
+ * Indicates this input is used by a simulated device.
+ *
+ * @param device simulated device handle
+ */
+ void SetSimDevice(HAL_SimDeviceHandle device);
+
void InitSendable(SendableBuilder& builder) override;
private:
int m_channel;
- HAL_AnalogInputHandle m_port = HAL_kInvalidHandle;
+ hal::Handle<HAL_AnalogInputHandle> m_port;
int64_t m_accumulatorOffset;
};
diff --git a/wpilibc/src/main/native/include/frc/AnalogOutput.h b/wpilibc/src/main/native/include/frc/AnalogOutput.h
index 3c7b44e..1cecd70 100644
--- a/wpilibc/src/main/native/include/frc/AnalogOutput.h
+++ b/wpilibc/src/main/native/include/frc/AnalogOutput.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,14 +10,19 @@
#include <hal/Types.h>
#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* MXP analog output class.
*/
-class AnalogOutput : public ErrorBase, public SendableBase {
+class AnalogOutput : public ErrorBase,
+ public Sendable,
+ public SendableHelper<AnalogOutput> {
public:
/**
* Construct an analog output on the given channel.
@@ -30,8 +35,8 @@
~AnalogOutput() override;
- AnalogOutput(AnalogOutput&& rhs);
- AnalogOutput& operator=(AnalogOutput&& rhs);
+ AnalogOutput(AnalogOutput&&) = default;
+ AnalogOutput& operator=(AnalogOutput&&) = default;
/**
* Set the value of the analog output.
@@ -56,7 +61,7 @@
protected:
int m_channel;
- HAL_AnalogOutputHandle m_port = HAL_kInvalidHandle;
+ hal::Handle<HAL_AnalogOutputHandle> m_port;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h b/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
index a937c69..446a920 100644
--- a/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
+++ b/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,10 +12,13 @@
#include "frc/AnalogInput.h"
#include "frc/ErrorBase.h"
#include "frc/interfaces/Potentiometer.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Class for reading analog potentiometers. Analog potentiometers read in an
* analog voltage that corresponds to a position. The position is in whichever
@@ -23,8 +26,9 @@
* constructor.
*/
class AnalogPotentiometer : public ErrorBase,
- public SendableBase,
- public Potentiometer {
+ public Potentiometer,
+ public Sendable,
+ public SendableHelper<AnalogPotentiometer> {
public:
/**
* Construct an Analog Potentiometer object from a channel number.
diff --git a/wpilibc/src/main/native/include/frc/AnalogTrigger.h b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
index beb48fc..6a57f8a 100644
--- a/wpilibc/src/main/native/include/frc/AnalogTrigger.h
+++ b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,13 +13,17 @@
#include "frc/AnalogTriggerOutput.h"
#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
class AnalogInput;
+class SendableBuilder;
-class AnalogTrigger : public ErrorBase, public SendableBase {
+class AnalogTrigger : public ErrorBase,
+ public Sendable,
+ public SendableHelper<AnalogTrigger> {
friend class AnalogTriggerOutput;
public:
@@ -136,7 +140,7 @@
private:
int m_index;
- HAL_AnalogTriggerHandle m_trigger = HAL_kInvalidHandle;
+ hal::Handle<HAL_AnalogTriggerHandle> m_trigger;
AnalogInput* m_analogInput = nullptr;
bool m_ownsAnalog = false;
};
diff --git a/wpilibc/src/main/native/include/frc/AnalogTriggerOutput.h b/wpilibc/src/main/native/include/frc/AnalogTriggerOutput.h
index fc3d8f2..989a93f 100644
--- a/wpilibc/src/main/native/include/frc/AnalogTriggerOutput.h
+++ b/wpilibc/src/main/native/include/frc/AnalogTriggerOutput.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,6 +8,8 @@
#pragma once
#include "frc/DigitalSource.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -44,15 +46,12 @@
* rollover transition is not sharp / clean enough. Using the averaging engine
* may help with this, but rotational speeds of the sensor will then be limited.
*/
-class AnalogTriggerOutput : public DigitalSource {
+class AnalogTriggerOutput : public DigitalSource,
+ public Sendable,
+ public SendableHelper<AnalogTriggerOutput> {
friend class AnalogTrigger;
public:
- ~AnalogTriggerOutput() override;
-
- AnalogTriggerOutput(AnalogTriggerOutput&&) = default;
- AnalogTriggerOutput& operator=(AnalogTriggerOutput&&) = default;
-
/**
* Get the state of the analog trigger output.
*
@@ -99,10 +98,10 @@
AnalogTriggerType outputType);
private:
- // Uses reference rather than smart pointer because a user can not construct
+ // Uses pointer rather than smart pointer because a user can not construct
// an AnalogTriggerOutput themselves and because the AnalogTriggerOutput
// should always be in scope at the same time as an AnalogTrigger.
- const AnalogTrigger& m_trigger;
+ const AnalogTrigger* m_trigger;
AnalogTriggerType m_outputType;
};
diff --git a/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h b/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h
index 3c9fc4a..8148e72 100644
--- a/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h
+++ b/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,18 +9,22 @@
#include "frc/ErrorBase.h"
#include "frc/interfaces/Accelerometer.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Built-in accelerometer.
*
* This class allows access to the roboRIO's internal accelerometer.
*/
class BuiltInAccelerometer : public ErrorBase,
- public SendableBase,
- public Accelerometer {
+ public Accelerometer,
+ public Sendable,
+ public SendableHelper<BuiltInAccelerometer> {
public:
/**
* Constructor.
diff --git a/wpilibc/src/main/native/include/frc/CAN.h b/wpilibc/src/main/native/include/frc/CAN.h
index 4cd06e9..ed861fb 100644
--- a/wpilibc/src/main/native/include/frc/CAN.h
+++ b/wpilibc/src/main/native/include/frc/CAN.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -58,8 +58,8 @@
*/
~CAN() override;
- CAN(CAN&& rhs);
- CAN& operator=(CAN&& rhs);
+ CAN(CAN&&) = default;
+ CAN& operator=(CAN&&) = default;
/**
* Write a packet to the CAN device with a specific ID. This ID is 10 bits.
@@ -84,6 +84,16 @@
int repeatMs);
/**
+ * Write an RTR frame to the CAN device with a specific ID. This ID is 10
+ * bits. The length by spec must match what is returned by the responding
+ * device
+ *
+ * @param length The length to request (0 to 8)
+ * @param apiId The API ID to write.
+ */
+ void WriteRTRFrame(int length, int apiId);
+
+ /**
* Stop a repeating packet with a specific ID. This ID is 10 bits.
*
* @param apiId The API ID to stop repeating
@@ -122,28 +132,11 @@
*/
bool ReadPacketTimeout(int apiId, int timeoutMs, CANData* data);
- /**
- * Read a CAN packet. The will return the last packet received until the
- * packet is older then the requested timeout. Then it will return false. The
- * period parameter is used when you know the packet is sent at specific
- * intervals, so calls will not attempt to read a new packet from the network
- * until that period has passed. We do not recommend users use this API unless
- * they know the implications.
- *
- * @param apiId The API ID to read.
- * @param timeoutMs The timeout time for the packet
- * @param periodMs The usual period for the packet
- * @param data Storage for the received data.
- * @return True if the data is valid, otherwise false.
- */
- bool ReadPeriodicPacket(int apiId, int timeoutMs, int periodMs,
- CANData* data);
-
static constexpr HAL_CANManufacturer kTeamManufacturer = HAL_CAN_Man_kTeamUse;
static constexpr HAL_CANDeviceType kTeamDeviceType =
HAL_CAN_Dev_kMiscellaneous;
private:
- HAL_CANHandle m_handle = HAL_kInvalidHandle;
+ hal::Handle<HAL_CANHandle> m_handle;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Compressor.h b/wpilibc/src/main/native/include/frc/Compressor.h
index 97f64d1..a5e512c 100644
--- a/wpilibc/src/main/native/include/frc/Compressor.h
+++ b/wpilibc/src/main/native/include/frc/Compressor.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,10 +11,13 @@
#include "frc/ErrorBase.h"
#include "frc/SensorUtil.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Class for operating a compressor connected to a %PCM (Pneumatic Control
* Module).
@@ -30,7 +33,9 @@
* loop control. You can only turn off closed loop control, thereby stopping
* the compressor from operating.
*/
-class Compressor : public ErrorBase, public SendableBase {
+class Compressor : public ErrorBase,
+ public Sendable,
+ public SendableHelper<Compressor> {
public:
/**
* Constructor. The default PCM ID is 0.
@@ -169,7 +174,7 @@
void InitSendable(SendableBuilder& builder) override;
protected:
- HAL_CompressorHandle m_compressorHandle = HAL_kInvalidHandle;
+ hal::Handle<HAL_CompressorHandle> m_compressorHandle;
private:
void SetCompressor(bool on);
diff --git a/wpilibc/src/main/native/include/frc/Controller.h b/wpilibc/src/main/native/include/frc/Controller.h
index b327723..4124046 100644
--- a/wpilibc/src/main/native/include/frc/Controller.h
+++ b/wpilibc/src/main/native/include/frc/Controller.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,8 @@
#pragma once
+#include <wpi/deprecated.h>
+
namespace frc {
/**
@@ -18,6 +20,7 @@
*/
class Controller {
public:
+ WPI_DEPRECATED("None of the 2020 FRC controllers use this.")
Controller() = default;
virtual ~Controller() = default;
diff --git a/wpilibc/src/main/native/include/frc/ControllerPower.h b/wpilibc/src/main/native/include/frc/ControllerPower.h
deleted file mode 100644
index 1359961..0000000
--- a/wpilibc/src/main/native/include/frc/ControllerPower.h
+++ /dev/null
@@ -1,152 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/deprecated.h>
-
-namespace frc {
-
-class ControllerPower {
- public:
- /**
- * Get the input voltage to the robot controller.
- *
- * @return The controller input voltage value in Volts
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static double GetInputVoltage();
-
- /**
- * Get the input current to the robot controller.
- *
- * @return The controller input current value in Amps
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static double GetInputCurrent();
-
- /**
- * Get the voltage of the 3.3V rail.
- *
- * @return The controller 3.3V rail voltage value in Volts
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static double GetVoltage3V3();
-
- /**
- * Get the current output of the 3.3V rail.
- *
- * @return The controller 3.3V rail output current value in Amps
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static double GetCurrent3V3();
-
- /**
- * Get the enabled state of the 3.3V rail. The rail may be disabled due to a
- * controller brownout, a short circuit on the rail, or controller
- * over-voltage.
- *
- * @return The controller 3.3V rail enabled value. True for enabled.
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static bool GetEnabled3V3();
-
- /**
- * Get the count of the total current faults on the 3.3V rail since the
- * controller has booted.
- *
- * @return The number of faults
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static int GetFaultCount3V3();
-
- /**
- * Get the voltage of the 5V rail.
- *
- * @return The controller 5V rail voltage value in Volts
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static double GetVoltage5V();
-
- /**
- * Get the current output of the 5V rail.
- *
- * @return The controller 5V rail output current value in Amps
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static double GetCurrent5V();
-
- /**
- * Get the enabled state of the 5V rail. The rail may be disabled due to a
- * controller brownout, a short circuit on the rail, or controller
- * over-voltage.
- *
- * @return The controller 5V rail enabled value. True for enabled.
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static bool GetEnabled5V();
-
- /**
- * Get the count of the total current faults on the 5V rail since the
- * controller has booted.
- *
- * @return The number of faults
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static int GetFaultCount5V();
-
- /**
- * Get the voltage of the 6V rail.
- *
- * @return The controller 6V rail voltage value in Volts
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static double GetVoltage6V();
-
- /**
- * Get the current output of the 6V rail.
- *
- * @return The controller 6V rail output current value in Amps
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static double GetCurrent6V();
-
- /**
- * Get the enabled state of the 6V rail. The rail may be disabled due to a
- * controller brownout, a short circuit on the rail, or controller
- * over-voltage.
- *
- * @return The controller 6V rail enabled value. True for enabled.
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static bool GetEnabled6V();
-
- /**
- * Get the count of the total current faults on the 6V rail since the
- * controller has booted.
- *
- * @return The number of faults.
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- static int GetFaultCount6V();
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Counter.h b/wpilibc/src/main/native/include/frc/Counter.h
index 2705a39..50e3948 100644
--- a/wpilibc/src/main/native/include/frc/Counter.h
+++ b/wpilibc/src/main/native/include/frc/Counter.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,11 +14,13 @@
#include "frc/AnalogTrigger.h"
#include "frc/CounterBase.h"
#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
class DigitalGlitchFilter;
+class SendableBuilder;
/**
* Class for counting the number of ticks on a digital input channel.
@@ -30,7 +32,10 @@
* All counters will immediately start counting - Reset() them if you need them
* to be zeroed before use.
*/
-class Counter : public ErrorBase, public SendableBase, public CounterBase {
+class Counter : public ErrorBase,
+ public CounterBase,
+ public Sendable,
+ public SendableHelper<Counter> {
public:
enum Mode {
kTwoPulse = 0,
@@ -139,8 +144,8 @@
~Counter() override;
- Counter(Counter&& rhs);
- Counter& operator=(Counter&& rhs);
+ Counter(Counter&&) = default;
+ Counter& operator=(Counter&&) = default;
/**
* Set the upsource for the counter as a digital input channel.
@@ -425,7 +430,7 @@
std::shared_ptr<DigitalSource> m_downSource;
// The FPGA counter object
- HAL_CounterHandle m_counter = HAL_kInvalidHandle;
+ hal::Handle<HAL_CounterHandle> m_counter;
private:
int m_index = 0; // The index of this counter.
diff --git a/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h b/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h
index f33955e..0690e53 100644
--- a/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h
+++ b/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -15,7 +15,8 @@
#include "frc/DigitalSource.h"
#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -29,7 +30,9 @@
* filter. The filter lets the user configure the time that an input must remain
* high or low before it is classified as high or low.
*/
-class DigitalGlitchFilter : public ErrorBase, public SendableBase {
+class DigitalGlitchFilter : public ErrorBase,
+ public Sendable,
+ public SendableHelper<DigitalGlitchFilter> {
public:
DigitalGlitchFilter();
~DigitalGlitchFilter() override;
diff --git a/wpilibc/src/main/native/include/frc/DigitalInput.h b/wpilibc/src/main/native/include/frc/DigitalInput.h
index af191aa..33aa716 100644
--- a/wpilibc/src/main/native/include/frc/DigitalInput.h
+++ b/wpilibc/src/main/native/include/frc/DigitalInput.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,10 +8,13 @@
#pragma once
#include "frc/DigitalSource.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
class DigitalGlitchFilter;
+class SendableBuilder;
/**
* Class to read a digital input.
@@ -22,7 +25,9 @@
* as required. This class is only for devices like switches etc. that aren't
* implemented anywhere else.
*/
-class DigitalInput : public DigitalSource {
+class DigitalInput : public DigitalSource,
+ public Sendable,
+ public SendableHelper<DigitalInput> {
public:
/**
* Create an instance of a Digital Input class.
@@ -35,8 +40,8 @@
~DigitalInput() override;
- DigitalInput(DigitalInput&& rhs);
- DigitalInput& operator=(DigitalInput&& rhs);
+ DigitalInput(DigitalInput&&) = default;
+ DigitalInput& operator=(DigitalInput&&) = default;
/**
* Get the value from a digital input channel.
@@ -66,11 +71,18 @@
*/
int GetChannel() const override;
+ /**
+ * Indicates this input is used by a simulated device.
+ *
+ * @param device simulated device handle
+ */
+ void SetSimDevice(HAL_SimDeviceHandle device);
+
void InitSendable(SendableBuilder& builder) override;
private:
int m_channel;
- HAL_DigitalHandle m_handle = HAL_kInvalidHandle;
+ hal::Handle<HAL_DigitalHandle> m_handle;
friend class DigitalGlitchFilter;
};
diff --git a/wpilibc/src/main/native/include/frc/DigitalOutput.h b/wpilibc/src/main/native/include/frc/DigitalOutput.h
index 49cb67d..45727a4 100644
--- a/wpilibc/src/main/native/include/frc/DigitalOutput.h
+++ b/wpilibc/src/main/native/include/frc/DigitalOutput.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,10 +10,13 @@
#include <hal/Types.h>
#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Class to write to digital outputs.
*
@@ -21,7 +24,9 @@
* elsewhere will allocate channels automatically so for those devices it
* shouldn't be done here.
*/
-class DigitalOutput : public ErrorBase, public SendableBase {
+class DigitalOutput : public ErrorBase,
+ public Sendable,
+ public SendableHelper<DigitalOutput> {
public:
/**
* Create an instance of a digital output.
@@ -35,8 +40,8 @@
~DigitalOutput() override;
- DigitalOutput(DigitalOutput&& rhs);
- DigitalOutput& operator=(DigitalOutput&& rhs);
+ DigitalOutput(DigitalOutput&&) = default;
+ DigitalOutput& operator=(DigitalOutput&&) = default;
/**
* Set the value of a digital output.
@@ -120,12 +125,19 @@
*/
void UpdateDutyCycle(double dutyCycle);
+ /**
+ * Indicates this output is used by a simulated device.
+ *
+ * @param device simulated device handle
+ */
+ void SetSimDevice(HAL_SimDeviceHandle device);
+
void InitSendable(SendableBuilder& builder) override;
private:
int m_channel;
- HAL_DigitalHandle m_handle = HAL_kInvalidHandle;
- HAL_DigitalPWMHandle m_pwmGenerator = HAL_kInvalidHandle;
+ hal::Handle<HAL_DigitalHandle> m_handle;
+ hal::Handle<HAL_DigitalPWMHandle> m_pwmGenerator;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DoubleSolenoid.h b/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
index 87c0c85..6722976 100644
--- a/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
+++ b/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,9 +10,13 @@
#include <hal/Types.h>
#include "frc/SolenoidBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* DoubleSolenoid class for running 2 channels of high voltage Digital Output
* (PCM).
@@ -20,7 +24,9 @@
* The DoubleSolenoid class is typically used for pneumatics solenoids that
* have two positions controlled by two separate channels.
*/
-class DoubleSolenoid : public SolenoidBase {
+class DoubleSolenoid : public SolenoidBase,
+ public Sendable,
+ public SendableHelper<DoubleSolenoid> {
public:
enum Value { kOff, kForward, kReverse };
@@ -45,8 +51,8 @@
~DoubleSolenoid() override;
- DoubleSolenoid(DoubleSolenoid&& rhs);
- DoubleSolenoid& operator=(DoubleSolenoid&& rhs);
+ DoubleSolenoid(DoubleSolenoid&&) = default;
+ DoubleSolenoid& operator=(DoubleSolenoid&&) = default;
/**
* Set the value of a solenoid.
@@ -91,8 +97,8 @@
int m_reverseChannel; // The reverse channel on the module to control.
int m_forwardMask; // The mask for the forward channel.
int m_reverseMask; // The mask for the reverse channel.
- HAL_SolenoidHandle m_forwardHandle = HAL_kInvalidHandle;
- HAL_SolenoidHandle m_reverseHandle = HAL_kInvalidHandle;
+ hal::Handle<HAL_SolenoidHandle> m_forwardHandle;
+ hal::Handle<HAL_SolenoidHandle> m_reverseHandle;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h
index ddbe27a..8abffde 100644
--- a/wpilibc/src/main/native/include/frc/DriverStation.h
+++ b/wpilibc/src/main/native/include/frc/DriverStation.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -198,6 +198,13 @@
bool IsDisabled() const;
/**
+ * Check if the robot is e-stopped.
+ *
+ * @return True if the robot is e-stopped
+ */
+ bool IsEStopped() const;
+
+ /**
* Check if the DS is commanding autonomous mode.
*
* @return True if the robot is being commanded to be in autonomous mode
@@ -245,27 +252,6 @@
bool IsFMSAttached() const;
/**
- * Check if the FPGA outputs are enabled.
- *
- * The outputs may be disabled if the robot is disabled or e-stopped, the
- * watchdog has expired, or if the roboRIO browns out.
- *
- * @return True if the FPGA outputs are enabled.
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- bool IsSysActive() const;
-
- /**
- * Check if the system is browned out.
- *
- * @return True if the system is browned out
- * @deprecated Use RobotController static class method
- */
- WPI_DEPRECATED("Use RobotController static class method")
- bool IsBrownedOut() const;
-
- /**
* Returns the game specific message provided by the FMS.
*
* @return A string containing the game specific message.
diff --git a/wpilibc/src/main/native/include/frc/Encoder.h b/wpilibc/src/main/native/include/frc/Encoder.h
index 7096eeb..074cc5e 100644
--- a/wpilibc/src/main/native/include/frc/Encoder.h
+++ b/wpilibc/src/main/native/include/frc/Encoder.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -15,12 +15,14 @@
#include "frc/CounterBase.h"
#include "frc/ErrorBase.h"
#include "frc/PIDSource.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
class DigitalSource;
class DigitalGlitchFilter;
+class SendableBuilder;
/**
* Class to read quad encoders.
@@ -38,9 +40,10 @@
* to be zeroed before use.
*/
class Encoder : public ErrorBase,
- public SendableBase,
public CounterBase,
- public PIDSource {
+ public PIDSource,
+ public Sendable,
+ public SendableHelper<Encoder> {
public:
enum IndexingType {
kResetWhileHigh,
@@ -133,8 +136,8 @@
~Encoder() override;
- Encoder(Encoder&& rhs);
- Encoder& operator=(Encoder&& rhs);
+ Encoder(Encoder&&) = default;
+ Encoder& operator=(Encoder&&) = default;
// CounterBase interface
/**
@@ -329,6 +332,13 @@
void SetIndexSource(const DigitalSource& source,
IndexingType type = kResetOnRisingEdge);
+ /**
+ * Indicates this encoder is used by a simulated device.
+ *
+ * @param device simulated device handle
+ */
+ void SetSimDevice(HAL_SimDeviceHandle device);
+
int GetFPGAIndex() const;
void InitSendable(SendableBuilder& builder) override;
@@ -362,7 +372,7 @@
std::shared_ptr<DigitalSource> m_aSource; // The A phase of the quad encoder
std::shared_ptr<DigitalSource> m_bSource; // The B phase of the quad encoder
std::shared_ptr<DigitalSource> m_indexSource = nullptr;
- HAL_EncoderHandle m_encoder = HAL_kInvalidHandle;
+ hal::Handle<HAL_EncoderHandle> m_encoder;
friend class DigitalGlitchFilter;
};
diff --git a/wpilibc/src/main/native/include/frc/ErrorBase.h b/wpilibc/src/main/native/include/frc/ErrorBase.h
index 3be9765..7235352 100644
--- a/wpilibc/src/main/native/include/frc/ErrorBase.h
+++ b/wpilibc/src/main/native/include/frc/ErrorBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,8 @@
#pragma once
+#include <vector>
+
#include <wpi/StringRef.h>
#include <wpi/Twine.h>
#include <wpi/mutex.h>
@@ -49,16 +51,16 @@
} while (0)
#define wpi_setGlobalError(code) wpi_setGlobalErrorWithContext(code, "")
#define wpi_setWPIErrorWithContext(error, context) \
- this->SetWPIError((wpi_error_s_##error), (wpi_error_value_##error), \
+ this->SetWPIError(wpi_error_s_##error(), wpi_error_value_##error(), \
(context), __FILE__, __FUNCTION__, __LINE__)
#define wpi_setWPIError(error) (wpi_setWPIErrorWithContext(error, ""))
#define wpi_setStaticWPIErrorWithContext(object, error, context) \
- object->SetWPIError((wpi_error_s_##error), (context), __FILE__, \
+ object->SetWPIError(wpi_error_s_##error(), (context), __FILE__, \
__FUNCTION__, __LINE__)
#define wpi_setStaticWPIError(object, error) \
wpi_setStaticWPIErrorWithContext(object, error, "")
#define wpi_setGlobalWPIErrorWithContext(error, context) \
- ::frc::ErrorBase::SetGlobalWPIError((wpi_error_s_##error), (context), \
+ ::frc::ErrorBase::SetGlobalWPIError(wpi_error_s_##error(), (context), \
__FILE__, __FUNCTION__, __LINE__)
#define wpi_setGlobalWPIError(error) wpi_setGlobalWPIErrorWithContext(error, "")
@@ -77,6 +79,8 @@
ErrorBase();
virtual ~ErrorBase() = default;
+ ErrorBase(const ErrorBase&) = default;
+ ErrorBase& operator=(const ErrorBase&) = default;
ErrorBase(ErrorBase&&) = default;
ErrorBase& operator=(ErrorBase&&) = default;
@@ -191,9 +195,19 @@
wpi::StringRef function, int lineNumber);
/**
- * Retrieve the current global error.
+ * Retrieve the last global error.
*/
- static const Error& GetGlobalError();
+ static Error GetGlobalError();
+
+ /**
+ * Retrieve all global errors.
+ */
+ static std::vector<Error> GetGlobalErrors();
+
+ /**
+ * Clear global errors.
+ */
+ void ClearGlobalErrors();
protected:
mutable Error m_error;
diff --git a/wpilibc/src/main/native/include/frc/Filesystem.h b/wpilibc/src/main/native/include/frc/Filesystem.h
index d7675d6..b7ef3f1 100644
--- a/wpilibc/src/main/native/include/frc/Filesystem.h
+++ b/wpilibc/src/main/native/include/frc/Filesystem.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,8 +9,6 @@
#include <wpi/SmallVector.h>
-#include "frc/RobotBase.h"
-
namespace frc {
/** WPILib FileSystem namespace */
namespace filesystem {
diff --git a/wpilibc/src/main/native/include/frc/GamepadBase.h b/wpilibc/src/main/native/include/frc/GamepadBase.h
deleted file mode 100644
index 9681c84..0000000
--- a/wpilibc/src/main/native/include/frc/GamepadBase.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/deprecated.h>
-
-#include "frc/GenericHID.h"
-
-namespace frc {
-
-/**
- * Gamepad Interface.
- */
-class GamepadBase : public GenericHID {
- public:
- WPI_DEPRECATED("Inherit directly from GenericHID instead.")
- explicit GamepadBase(int port);
- virtual ~GamepadBase() = default;
-
- GamepadBase(GamepadBase&&) = default;
- GamepadBase& operator=(GamepadBase&&) = default;
-
- virtual bool GetBumper(JoystickHand hand = kRightHand) const = 0;
- virtual bool GetStickButton(JoystickHand hand) const = 0;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/GearTooth.h b/wpilibc/src/main/native/include/frc/GearTooth.h
index 230f6d1..1c3df5b 100644
--- a/wpilibc/src/main/native/include/frc/GearTooth.h
+++ b/wpilibc/src/main/native/include/frc/GearTooth.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,6 +10,8 @@
#include <memory>
#include <string>
+#include <wpi/deprecated.h>
+
#include "frc/Counter.h"
namespace frc {
@@ -34,6 +36,9 @@
* @param directionSensitive True to enable the pulse length decoding in
* hardware to specify count direction.
*/
+ WPI_DEPRECATED(
+ "The only sensor this works with is no longer available and no teams use "
+ "it according to FMS usage reporting.")
explicit GearTooth(int channel, bool directionSensitive = false);
/**
@@ -46,6 +51,9 @@
* @param directionSensitive True to enable the pulse length decoding in
* hardware to specify count direction.
*/
+ WPI_DEPRECATED(
+ "The only sensor this works with is no longer available and no teams use "
+ "it according to FMS usage reporting.")
explicit GearTooth(DigitalSource* source, bool directionSensitive = false);
/**
@@ -58,6 +66,9 @@
* @param directionSensitive True to enable the pulse length decoding in
* hardware to specify count direction.
*/
+ WPI_DEPRECATED(
+ "The only sensor this works with is no longer available and no teams use "
+ "it according to FMS usage reporting.")
explicit GearTooth(std::shared_ptr<DigitalSource> source,
bool directionSensitive = false);
diff --git a/wpilibc/src/main/native/include/frc/GenericHID.h b/wpilibc/src/main/native/include/frc/GenericHID.h
index 74e5271..aae10cf 100644
--- a/wpilibc/src/main/native/include/frc/GenericHID.h
+++ b/wpilibc/src/main/native/include/frc/GenericHID.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -179,7 +179,7 @@
void SetRumble(RumbleType type, double value);
private:
- DriverStation& m_ds;
+ DriverStation* m_ds;
int m_port;
int m_outputs = 0;
uint16_t m_leftRumble = 0;
diff --git a/wpilibc/src/main/native/include/frc/GyroBase.h b/wpilibc/src/main/native/include/frc/GyroBase.h
index d27c7d9..037686f 100644
--- a/wpilibc/src/main/native/include/frc/GyroBase.h
+++ b/wpilibc/src/main/native/include/frc/GyroBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,7 +10,8 @@
#include "frc/ErrorBase.h"
#include "frc/PIDSource.h"
#include "frc/interfaces/Gyro.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -20,8 +21,9 @@
*/
class GyroBase : public Gyro,
public ErrorBase,
- public SendableBase,
- public PIDSource {
+ public PIDSource,
+ public Sendable,
+ public SendableHelper<GyroBase> {
public:
GyroBase() = default;
GyroBase(GyroBase&&) = default;
diff --git a/wpilibc/src/main/native/include/frc/I2C.h b/wpilibc/src/main/native/include/frc/I2C.h
index 4623c44..2f12615 100644
--- a/wpilibc/src/main/native/include/frc/I2C.h
+++ b/wpilibc/src/main/native/include/frc/I2C.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -35,14 +35,16 @@
~I2C() override;
- I2C(I2C&& rhs);
- I2C& operator=(I2C&& rhs);
+ I2C(I2C&&) = default;
+ I2C& operator=(I2C&&) = default;
/**
* Generic transaction.
*
* This is a lower-level interface to the I2C hardware giving you more control
- * over each transaction.
+ * over each transaction. If you intend to write multiple bytes in the same
+ * transaction and do not plan to receive anything back, use writeBulk()
+ * instead. Calling this with a receiveSize of 0 will result in an error.
*
* @param dataToSend Buffer of data to send as part of the transaction.
* @param sendSize Number of bytes to send as part of the transaction.
@@ -135,7 +137,7 @@
bool VerifySensor(int registerAddress, int count, const uint8_t* expected);
private:
- HAL_I2CPort m_port = HAL_I2C_kInvalid;
+ hal::I2CPort m_port;
int m_deviceAddress;
};
diff --git a/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h b/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
index 7c9e364..8c2a564 100644
--- a/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
+++ b/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,15 +7,17 @@
#pragma once
+#include <functional>
+#include <memory>
+
#include <hal/Interrupts.h>
#include "frc/AnalogTriggerType.h"
#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/SendableBase.h"
namespace frc {
-class InterruptableSensorBase : public ErrorBase, public SendableBase {
+class InterruptableSensorBase : public ErrorBase {
public:
enum WaitResult {
kTimeout = 0x0,
@@ -24,8 +26,20 @@
kBoth = 0x101,
};
+ /**
+ * Handler for interrupts.
+ *
+ * First parameter is if rising, 2nd is if falling.
+ */
+ using InterruptEventHandler = std::function<void(WaitResult)>;
+
InterruptableSensorBase() = default;
+ /**
+ * Free the resources for an interrupt event.
+ */
+ virtual ~InterruptableSensorBase();
+
InterruptableSensorBase(InterruptableSensorBase&&) = default;
InterruptableSensorBase& operator=(InterruptableSensorBase&&) = default;
@@ -44,6 +58,16 @@
void* param);
/**
+ * Request one of the 8 interrupts asynchronously on this digital input.
+ *
+ * Request interrupts in asynchronous mode where the user's interrupt handler
+ * will be called when the interrupt fires. Users that want control over the
+ * thread priority should use the synchronous method with their own spawned
+ * thread. The default is interrupt on rising edges only.
+ */
+ virtual void RequestInterrupts(InterruptEventHandler handler);
+
+ /**
* Request one of the 8 interrupts synchronously on this digital input.
*
* Request interrupts in synchronous mode where the user program will have to
@@ -119,7 +143,8 @@
virtual void SetUpSourceEdge(bool risingEdge, bool fallingEdge);
protected:
- HAL_InterruptHandle m_interrupt = HAL_kInvalidHandle;
+ hal::Handle<HAL_InterruptHandle> m_interrupt;
+ std::unique_ptr<InterruptEventHandler> m_interruptHandler{nullptr};
void AllocateInterrupts(bool watcher);
};
diff --git a/wpilibc/src/main/native/include/frc/IterativeRobotBase.h b/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
index 6ff5816..b78765a 100644
--- a/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
+++ b/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,9 @@
#pragma once
+#include <units/units.h>
+#include <wpi/deprecated.h>
+
#include "frc/RobotBase.h"
#include "frc/Watchdog.h"
@@ -141,8 +144,16 @@
*
* @param period Period in seconds.
*/
+ WPI_DEPRECATED("Use ctor with unit-safety instead.")
explicit IterativeRobotBase(double period);
+ /**
+ * Constructor for IterativeRobotBase.
+ *
+ * @param period Period.
+ */
+ explicit IterativeRobotBase(units::second_t period);
+
virtual ~IterativeRobotBase() = default;
IterativeRobotBase(IterativeRobotBase&&) = default;
@@ -150,7 +161,7 @@
void LoopFunc();
- double m_period;
+ units::second_t m_period;
private:
enum class Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest };
diff --git a/wpilibc/src/main/native/include/frc/Joystick.h b/wpilibc/src/main/native/include/frc/Joystick.h
index 4c17589..4ae398c 100644
--- a/wpilibc/src/main/native/include/frc/Joystick.h
+++ b/wpilibc/src/main/native/include/frc/Joystick.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -31,17 +31,6 @@
static constexpr int kDefaultTwistChannel = 2;
static constexpr int kDefaultThrottleChannel = 3;
- WPI_DEPRECATED("Use kDefaultXChannel instead.")
- static constexpr int kDefaultXAxis = 0;
- WPI_DEPRECATED("Use kDefaultYChannel instead.")
- static constexpr int kDefaultYAxis = 1;
- WPI_DEPRECATED("Use kDefaultZChannel instead.")
- static constexpr int kDefaultZAxis = 2;
- WPI_DEPRECATED("Use kDefaultTwistChannel instead.")
- static constexpr int kDefaultTwistAxis = 2;
- WPI_DEPRECATED("Use kDefaultThrottleChannel instead.")
- static constexpr int kDefaultThrottleAxis = 3;
-
enum AxisType { kXAxis, kYAxis, kZAxis, kTwistAxis, kThrottleAxis };
enum ButtonType { kTriggerButton, kTopButton };
@@ -100,15 +89,6 @@
void SetThrottleChannel(int channel);
/**
- * Set the channel associated with a specified axis.
- *
- * @param axis The axis to set the channel for.
- * @param channel The channel to set the axis to.
- */
- WPI_DEPRECATED("Use the more specific axis channel setter functions.")
- void SetAxisChannel(AxisType axis, int channel);
-
- /**
* Get the channel currently associated with the X axis.
*
* @return The channel for the axis.
@@ -185,19 +165,6 @@
double GetThrottle() const;
/**
- * For the current joystick, return the axis determined by the argument.
- *
- * This is for cases where the joystick axis is returned programatically,
- * otherwise one of the previous functions would be preferable (for example
- * GetX()).
- *
- * @param axis The axis to read.
- * @return The value of the axis.
- */
- WPI_DEPRECATED("Use the more specific axis channel getter functions.")
- double GetAxis(AxisType axis) const;
-
- /**
* Read the state of the trigger on the joystick.
*
* Look up which button has been assigned to the trigger and read its state.
@@ -243,20 +210,6 @@
*/
bool GetTopReleased();
- WPI_DEPRECATED("Use Joystick instances instead.")
- static Joystick* GetStickForPort(int port);
-
- /**
- * Get buttons based on an enumerated type.
- *
- * The button type will be looked up in the list of buttons and then read.
- *
- * @param button The type of button to read.
- * @return The state of the button.
- */
- WPI_DEPRECATED("Use the more specific button getter functions.")
- bool GetButton(ButtonType button) const;
-
/**
* Get the magnitude of the direction vector formed by the joystick's
* current position relative to its origin.
diff --git a/wpilibc/src/main/native/include/frc/JoystickBase.h b/wpilibc/src/main/native/include/frc/JoystickBase.h
deleted file mode 100644
index cba5a7e..0000000
--- a/wpilibc/src/main/native/include/frc/JoystickBase.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/deprecated.h>
-
-#include "frc/GenericHID.h"
-
-namespace frc {
-
-/**
- * Joystick Interface.
- */
-class JoystickBase : public GenericHID {
- public:
- WPI_DEPRECATED("Inherit directly from GenericHID instead.")
- explicit JoystickBase(int port);
- virtual ~JoystickBase() = default;
-
- JoystickBase(JoystickBase&&) = default;
- JoystickBase& operator=(JoystickBase&&) = default;
-
- virtual double GetZ(JoystickHand hand = kRightHand) const = 0;
- virtual double GetTwist() const = 0;
- virtual double GetThrottle() const = 0;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/LinearFilter.h b/wpilibc/src/main/native/include/frc/LinearFilter.h
new file mode 100644
index 0000000..de52003
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/LinearFilter.h
@@ -0,0 +1,150 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <initializer_list>
+#include <vector>
+
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/circular_buffer.h>
+
+namespace frc {
+
+/**
+ * This class implements a linear, digital filter. All types of FIR and IIR
+ * filters are supported. Static factory methods are provided to create commonly
+ * used types of filters.
+ *
+ * Filters are of the form:<br>
+ * y[n] = (b0 * x[n] + b1 * x[n-1] + … + bP * x[n-P]) -
+ * (a0 * y[n-1] + a2 * y[n-2] + … + aQ * y[n-Q])
+ *
+ * Where:<br>
+ * y[n] is the output at time "n"<br>
+ * x[n] is the input at time "n"<br>
+ * y[n-1] is the output from the LAST time step ("n-1")<br>
+ * x[n-1] is the input from the LAST time step ("n-1")<br>
+ * b0 … bP are the "feedforward" (FIR) gains<br>
+ * a0 … aQ are the "feedback" (IIR) gains<br>
+ * IMPORTANT! Note the "-" sign in front of the feedback term! This is a common
+ * convention in signal processing.
+ *
+ * What can linear filters do? Basically, they can filter, or diminish, the
+ * effects of undesirable input frequencies. High frequencies, or rapid changes,
+ * can be indicative of sensor noise or be otherwise undesirable. A "low pass"
+ * filter smooths out the signal, reducing the impact of these high frequency
+ * components. Likewise, a "high pass" filter gets rid of slow-moving signal
+ * components, letting you detect large changes more easily.
+ *
+ * Example FRC applications of filters:
+ * - Getting rid of noise from an analog sensor input (note: the roboRIO's FPGA
+ * can do this faster in hardware)
+ * - Smoothing out joystick input to prevent the wheels from slipping or the
+ * robot from tipping
+ * - Smoothing motor commands so that unnecessary strain isn't put on
+ * electrical or mechanical components
+ * - If you use clever gains, you can make a PID controller out of this class!
+ *
+ * For more on filters, we highly recommend the following articles:<br>
+ * https://en.wikipedia.org/wiki/Linear_filter<br>
+ * https://en.wikipedia.org/wiki/Iir_filter<br>
+ * https://en.wikipedia.org/wiki/Fir_filter<br>
+ *
+ * Note 1: Calculate() should be called by the user on a known, regular period.
+ * You can use a Notifier for this or do it "inline" with code in a
+ * periodic function.
+ *
+ * Note 2: For ALL filters, gains are necessarily a function of frequency. If
+ * you make a filter that works well for you at, say, 100Hz, you will most
+ * definitely need to adjust the gains if you then want to run it at 200Hz!
+ * Combining this with Note 1 - the impetus is on YOU as a developer to make
+ * sure Calculate() gets called at the desired, constant frequency!
+ */
+class LinearFilter {
+ public:
+ /**
+ * Create a linear FIR or IIR filter.
+ *
+ * @param ffGains The "feed forward" or FIR gains.
+ * @param fbGains The "feed back" or IIR gains.
+ */
+ LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains);
+
+ /**
+ * Create a linear FIR or IIR filter.
+ *
+ * @param ffGains The "feed forward" or FIR gains.
+ * @param fbGains The "feed back" or IIR gains.
+ */
+ LinearFilter(std::initializer_list<double> ffGains,
+ std::initializer_list<double> fbGains)
+ : LinearFilter(wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
+ wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
+
+ // Static methods to create commonly used filters
+ /**
+ * Creates a one-pole IIR low-pass filter of the form:<br>
+ * y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
+ * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
+ *
+ * This filter is stable for time constants greater than zero.
+ *
+ * @param timeConstant The discrete-time time constant in seconds.
+ * @param period The period in seconds between samples taken by the
+ * user.
+ */
+ static LinearFilter SinglePoleIIR(double timeConstant,
+ units::second_t period);
+
+ /**
+ * Creates a first-order high-pass filter of the form:<br>
+ * y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
+ * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
+ *
+ * This filter is stable for time constants greater than zero.
+ *
+ * @param timeConstant The discrete-time time constant in seconds.
+ * @param period The period in seconds between samples taken by the
+ * user.
+ */
+ static LinearFilter HighPass(double timeConstant, units::second_t period);
+
+ /**
+ * Creates a K-tap FIR moving average filter of the form:<br>
+ * y[n] = 1/k * (x[k] + x[k-1] + … + x[0])
+ *
+ * This filter is always stable.
+ *
+ * @param taps The number of samples to average over. Higher = smoother but
+ * slower
+ */
+ static LinearFilter MovingAverage(int taps);
+
+ /**
+ * Reset the filter state.
+ */
+ void Reset();
+
+ /**
+ * Calculates the next value of the filter.
+ *
+ * @param input Current input value.
+ *
+ * @return The filtered value at this step
+ */
+ double Calculate(double input);
+
+ private:
+ wpi::circular_buffer<double> m_inputs{0};
+ wpi::circular_buffer<double> m_outputs{0};
+ std::vector<double> m_inputGains;
+ std::vector<double> m_outputGains;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/NidecBrushless.h b/wpilibc/src/main/native/include/frc/NidecBrushless.h
index 1d63c31..fa77e28 100644
--- a/wpilibc/src/main/native/include/frc/NidecBrushless.h
+++ b/wpilibc/src/main/native/include/frc/NidecBrushless.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,23 +7,25 @@
#pragma once
-#include <atomic>
-
#include "frc/DigitalOutput.h"
#include "frc/ErrorBase.h"
#include "frc/MotorSafety.h"
#include "frc/PWM.h"
#include "frc/SpeedController.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Nidec Brushless Motor.
*/
-class NidecBrushless : public SendableBase,
- public SpeedController,
- public MotorSafety {
+class NidecBrushless : public SpeedController,
+ public MotorSafety,
+ public Sendable,
+ public SendableHelper<NidecBrushless> {
public:
/**
* Constructor.
@@ -98,7 +100,7 @@
private:
bool m_isInverted = false;
- std::atomic_bool m_disabled{false};
+ bool m_disabled = false;
DigitalOutput m_dio;
PWM m_pwm;
double m_speed = 0.0;
diff --git a/wpilibc/src/main/native/include/frc/Notifier.h b/wpilibc/src/main/native/include/frc/Notifier.h
index 47380bc..f2f37f1 100644
--- a/wpilibc/src/main/native/include/frc/Notifier.h
+++ b/wpilibc/src/main/native/include/frc/Notifier.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -15,14 +15,14 @@
#include <utility>
#include <hal/Types.h>
+#include <units/units.h>
+#include <wpi/deprecated.h>
#include <wpi/mutex.h>
#include "frc/ErrorBase.h"
namespace frc {
-using TimerEventHandler = std::function<void()>;
-
class Notifier : public ErrorBase {
public:
/**
@@ -31,7 +31,7 @@
* @param handler The handler is called at the notification time which is set
* using StartSingle or StartPeriodic.
*/
- explicit Notifier(TimerEventHandler handler);
+ explicit Notifier(std::function<void()> handler);
template <typename Callable, typename Arg, typename... Args>
Notifier(Callable&& f, Arg&& arg, Args&&... args)
@@ -51,7 +51,7 @@
*
* @param handler Handler
*/
- void SetHandler(TimerEventHandler handler);
+ void SetHandler(std::function<void()> handler);
/**
* Register for single event notification.
@@ -60,9 +60,19 @@
*
* @param delay Seconds to wait before the handler is called.
*/
+ WPI_DEPRECATED("Use unit-safe StartSingle method instead.")
void StartSingle(double delay);
/**
+ * Register for single event notification.
+ *
+ * A timer event is queued for a single event after the specified delay.
+ *
+ * @param delay Amount of time to wait before the handler is called.
+ */
+ void StartSingle(units::second_t delay);
+
+ /**
* Register for periodic event notification.
*
* A timer event is queued for periodic event notification. Each time the
@@ -72,9 +82,22 @@
* @param period Period in seconds to call the handler starting one period
* after the call to this method.
*/
+ WPI_DEPRECATED("Use unit-safe StartPeriodic method instead.")
void StartPeriodic(double period);
/**
+ * Register for periodic event notification.
+ *
+ * A timer event is queued for periodic event notification. Each time the
+ * interrupt occurs, the event will be immediately requeued for the same time
+ * interval.
+ *
+ * @param period Period to call the handler starting one period
+ * after the call to this method.
+ */
+ void StartPeriodic(units::second_t period);
+
+ /**
* Stop timer events from occuring.
*
* Stop any repeating timer events from occuring. This will also remove any
@@ -108,7 +131,7 @@
std::atomic<HAL_NotifierHandle> m_notifier{0};
// Address of the handler
- TimerEventHandler m_handler;
+ std::function<void()> m_handler;
// The absolute expiration time
double m_expirationTime = 0;
diff --git a/wpilibc/src/main/native/include/frc/PIDBase.h b/wpilibc/src/main/native/include/frc/PIDBase.h
index f29b56e..098718f 100644
--- a/wpilibc/src/main/native/include/frc/PIDBase.h
+++ b/wpilibc/src/main/native/include/frc/PIDBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,15 +14,18 @@
#include <wpi/mutex.h>
#include "frc/Base.h"
+#include "frc/LinearFilter.h"
#include "frc/PIDInterface.h"
#include "frc/PIDOutput.h"
#include "frc/PIDSource.h"
#include "frc/Timer.h"
-#include "frc/filters/LinearDigitalFilter.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Class implements a PID Control Loop.
*
@@ -33,7 +36,10 @@
* in the integral and derivative calculations. Therefore, the sample rate
* affects the controller's behavior for a given set of PID constants.
*/
-class PIDBase : public SendableBase, public PIDInterface, public PIDOutput {
+class PIDBase : public PIDInterface,
+ public PIDOutput,
+ public Sendable,
+ public SendableHelper<PIDBase> {
public:
/**
* Allocate a PID object with the given constants for P, I, D.
@@ -44,6 +50,7 @@
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
*/
+ WPI_DEPRECATED("All APIs which use this have been deprecated.")
PIDBase(double p, double i, double d, PIDSource& source, PIDOutput& output);
/**
@@ -55,13 +62,11 @@
* @param source The PIDSource object that is used to get values
* @param output The PIDOutput object that is set to the output value
*/
+ WPI_DEPRECATED("All APIs which use this have been deprecated.")
PIDBase(double p, double i, double d, double f, PIDSource& source,
PIDOutput& output);
- ~PIDBase() override = default;
-
- PIDBase(PIDBase&&) = default;
- PIDBase& operator=(PIDBase&&) = default;
+ virtual ~PIDBase() = default;
/**
* Return the current PID result.
@@ -215,7 +220,7 @@
*
* @return the average error
*/
- WPI_DEPRECATED("Use a LinearDigitalFilter as the input and GetError().")
+ WPI_DEPRECATED("Use a LinearFilter as the input and GetError().")
virtual double GetAvgError() const;
/**
@@ -397,8 +402,7 @@
double m_error = 0;
double m_result = 0;
- std::shared_ptr<PIDSource> m_origSource;
- LinearDigitalFilter m_filter{nullptr, {}, {}};
+ LinearFilter m_filter{{}, {}};
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/PIDController.h b/wpilibc/src/main/native/include/frc/PIDController.h
index e9eea8b..88b0786 100644
--- a/wpilibc/src/main/native/include/frc/PIDController.h
+++ b/wpilibc/src/main/native/include/frc/PIDController.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -19,7 +19,6 @@
#include "frc/PIDBase.h"
#include "frc/PIDSource.h"
#include "frc/Timer.h"
-#include "frc/filters/LinearDigitalFilter.h"
namespace frc {
@@ -49,6 +48,7 @@
* particularly affects calculations of the integral and
* differental terms. The default is 0.05 (50ms).
*/
+ WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, PIDSource* source,
PIDOutput* output, double period = 0.05);
@@ -64,6 +64,7 @@
* particularly affects calculations of the integral and
* differental terms. The default is 0.05 (50ms).
*/
+ WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, double f, PIDSource* source,
PIDOutput* output, double period = 0.05);
@@ -79,6 +80,7 @@
* particularly affects calculations of the integral and
* differental terms. The default is 0.05 (50ms).
*/
+ WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, PIDSource& source,
PIDOutput& output, double period = 0.05);
@@ -94,14 +96,12 @@
* particularly affects calculations of the integral and
* differental terms. The default is 0.05 (50ms).
*/
+ WPI_DEPRECATED("Use frc2::PIDController class instead.")
PIDController(double p, double i, double d, double f, PIDSource& source,
PIDOutput& output, double period = 0.05);
~PIDController() override;
- PIDController(PIDController&&) = default;
- PIDController& operator=(PIDController&&) = default;
-
/**
* Begin running the PIDController.
*/
diff --git a/wpilibc/src/main/native/include/frc/PIDInterface.h b/wpilibc/src/main/native/include/frc/PIDInterface.h
index 72d8beb..8162aa5 100644
--- a/wpilibc/src/main/native/include/frc/PIDInterface.h
+++ b/wpilibc/src/main/native/include/frc/PIDInterface.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,10 +7,13 @@
#pragma once
+#include <wpi/deprecated.h>
+
namespace frc {
class PIDInterface {
public:
+ WPI_DEPRECATED("All APIs which use this have been deprecated.")
PIDInterface() = default;
PIDInterface(PIDInterface&&) = default;
PIDInterface& operator=(PIDInterface&&) = default;
diff --git a/wpilibc/src/main/native/include/frc/PWM.h b/wpilibc/src/main/native/include/frc/PWM.h
index 99f213e..58e18d2 100644
--- a/wpilibc/src/main/native/include/frc/PWM.h
+++ b/wpilibc/src/main/native/include/frc/PWM.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,10 +13,13 @@
#include <wpi/raw_ostream.h>
#include "frc/MotorSafety.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Class implements the PWM generation in the FPGA.
*
@@ -34,7 +37,7 @@
* - 1 = minimum pulse width (currently .5ms)
* - 0 = disabled (i.e. PWM output is held low)
*/
-class PWM : public MotorSafety, public SendableBase {
+class PWM : public MotorSafety, public Sendable, public SendableHelper<PWM> {
public:
/**
* Represents the amount to multiply the minimum servo-pulse pwm period by.
@@ -73,8 +76,8 @@
*/
~PWM() override;
- PWM(PWM&& rhs);
- PWM& operator=(PWM&& rhs);
+ PWM(PWM&&) = default;
+ PWM& operator=(PWM&&) = default;
// MotorSafety interface
void StopMotor() override;
@@ -231,7 +234,7 @@
private:
int m_channel;
- HAL_DigitalHandle m_handle = HAL_kInvalidHandle;
+ hal::Handle<HAL_DigitalHandle> m_handle;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/PWMSparkMax.h b/wpilibc/src/main/native/include/frc/PWMSparkMax.h
new file mode 100644
index 0000000..c8b22d7
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/PWMSparkMax.h
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/PWMSpeedController.h"
+
+namespace frc {
+
+/**
+ * REV Robotics SparkMax Speed Controller.
+ */
+class PWMSparkMax : public PWMSpeedController {
+ public:
+ /**
+ * Constructor for a SparkMax.
+ *
+ * @param channel The PWM channel that the Spark is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ explicit PWMSparkMax(int channel);
+
+ PWMSparkMax(PWMSparkMax&&) = default;
+ PWMSparkMax& operator=(PWMSparkMax&&) = default;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/PWMSpeedController.h b/wpilibc/src/main/native/include/frc/PWMSpeedController.h
index 5222559..b827d30 100644
--- a/wpilibc/src/main/native/include/frc/PWMSpeedController.h
+++ b/wpilibc/src/main/native/include/frc/PWMSpeedController.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
diff --git a/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h b/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
index 2d7f65c..433874b 100644
--- a/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
+++ b/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,15 +10,20 @@
#include <hal/Types.h>
#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Class for getting voltage, current, temperature, power and energy from the
* CAN PDP.
*/
-class PowerDistributionPanel : public ErrorBase, public SendableBase {
+class PowerDistributionPanel : public ErrorBase,
+ public Sendable,
+ public SendableHelper<PowerDistributionPanel> {
public:
PowerDistributionPanel();
explicit PowerDistributionPanel(int module);
@@ -83,7 +88,7 @@
void InitSendable(SendableBuilder& builder) override;
private:
- HAL_PDPHandle m_handle = HAL_kInvalidHandle;
+ hal::Handle<HAL_PDPHandle> m_handle;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Relay.h b/wpilibc/src/main/native/include/frc/Relay.h
index 6fb21b5..c903fc0 100644
--- a/wpilibc/src/main/native/include/frc/Relay.h
+++ b/wpilibc/src/main/native/include/frc/Relay.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,10 +14,13 @@
#include "frc/ErrorBase.h"
#include "frc/MotorSafety.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Class for Spike style relay outputs.
*
@@ -30,7 +33,9 @@
* independently for something that does not care about voltage polarity (like
* a solenoid).
*/
-class Relay : public MotorSafety, public SendableBase {
+class Relay : public MotorSafety,
+ public Sendable,
+ public SendableHelper<Relay> {
public:
enum Value { kOff, kOn, kForward, kReverse };
enum Direction { kBothDirections, kForwardOnly, kReverseOnly };
@@ -53,8 +58,8 @@
*/
~Relay() override;
- Relay(Relay&& rhs);
- Relay& operator=(Relay&& rhs);
+ Relay(Relay&&) = default;
+ Relay& operator=(Relay&&) = default;
/**
* Set the relay state.
@@ -98,8 +103,8 @@
int m_channel;
Direction m_direction;
- HAL_RelayHandle m_forwardHandle = HAL_kInvalidHandle;
- HAL_RelayHandle m_reverseHandle = HAL_kInvalidHandle;
+ hal::Handle<HAL_RelayHandle> m_forwardHandle;
+ hal::Handle<HAL_RelayHandle> m_reverseHandle;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Resource.h b/wpilibc/src/main/native/include/frc/Resource.h
index 9df2196..e9759d5 100644
--- a/wpilibc/src/main/native/include/frc/Resource.h
+++ b/wpilibc/src/main/native/include/frc/Resource.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -33,9 +33,6 @@
public:
virtual ~Resource() = default;
- Resource(Resource&&) = default;
- Resource& operator=(Resource&&) = default;
-
/**
* Factory method to create a Resource allocation-tracker *if* needed.
*
diff --git a/wpilibc/src/main/native/include/frc/RobotBase.h b/wpilibc/src/main/native/include/frc/RobotBase.h
index eee07d6..85a9d12 100644
--- a/wpilibc/src/main/native/include/frc/RobotBase.h
+++ b/wpilibc/src/main/native/include/frc/RobotBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,7 @@
#include <thread>
+#include <hal/Main.h>
#include <wpi/raw_ostream.h>
#include "frc/Base.h"
@@ -19,14 +20,37 @@
int RunHALInitialization();
+namespace impl {
+
+template <class Robot>
+void RunRobot() {
+ static Robot robot;
+ robot.StartCompetition();
+}
+
+} // namespace impl
+
template <class Robot>
int StartRobot() {
int halInit = RunHALInitialization();
if (halInit != 0) {
return halInit;
}
- static Robot robot;
- robot.StartCompetition();
+ if (HAL_HasMain()) {
+ std::thread([] {
+ try {
+ impl::RunRobot<Robot>();
+ } catch (...) {
+ HAL_ExitMain();
+ throw;
+ }
+ HAL_ExitMain();
+ })
+ .detach();
+ HAL_RunMain();
+ } else {
+ impl::RunRobot<Robot>();
+ }
return 0;
}
@@ -131,8 +155,8 @@
// m_ds isn't moved in these because DriverStation is a singleton; every
// instance of RobotBase has a reference to the same object.
- RobotBase(RobotBase&&);
- RobotBase& operator=(RobotBase&&);
+ RobotBase(RobotBase&&) noexcept;
+ RobotBase& operator=(RobotBase&&) noexcept;
DriverStation& m_ds;
diff --git a/wpilibc/src/main/native/include/frc/RobotState.h b/wpilibc/src/main/native/include/frc/RobotState.h
index 6b91692..60e608a 100644
--- a/wpilibc/src/main/native/include/frc/RobotState.h
+++ b/wpilibc/src/main/native/include/frc/RobotState.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,8 +13,11 @@
class RobotState {
public:
+ RobotState() = delete;
+
static bool IsDisabled();
static bool IsEnabled();
+ static bool IsEStopped();
static bool IsOperatorControl();
static bool IsAutonomous();
static bool IsTest();
diff --git a/wpilibc/src/main/native/include/frc/SPI.h b/wpilibc/src/main/native/include/frc/SPI.h
index 88cd4d9..fb4835e 100644
--- a/wpilibc/src/main/native/include/frc/SPI.h
+++ b/wpilibc/src/main/native/include/frc/SPI.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,6 +12,7 @@
#include <memory>
#include <hal/SPITypes.h>
+#include <units/units.h>
#include <wpi/ArrayRef.h>
#include <wpi/deprecated.h>
@@ -41,8 +42,8 @@
~SPI() override;
- SPI(SPI&& rhs);
- SPI& operator=(SPI&& rhs);
+ SPI(SPI&&) = default;
+ SPI& operator=(SPI&&) = default;
/**
* Configure the rate of the generated clock signal.
@@ -52,7 +53,7 @@
*
* @param hz The clock rate in Hertz.
*/
- void SetClockRate(double hz);
+ void SetClockRate(int hz);
/**
* Configure the order that bits are sent and received on the wire
@@ -179,8 +180,19 @@
* InitAuto() and SetAutoTransmitData() must be called before calling this
* function.
*
+ * @param period period between transfers (us resolution)
+ */
+ void StartAutoRate(units::second_t period);
+
+ /**
+ * Start running the automatic SPI transfer engine at a periodic rate.
+ *
+ * InitAuto() and SetAutoTransmitData() must be called before calling this
+ * function.
+ *
* @param period period between transfers, in seconds (us resolution)
*/
+ WPI_DEPRECATED("Use StartAutoRate with unit-safety instead")
void StartAutoRate(double period);
/**
@@ -221,9 +233,32 @@
*
* @param buffer buffer where read words are stored
* @param numToRead number of words to read
+ * @param timeout timeout (ms resolution)
+ * @return Number of words remaining to be read
+ */
+ int ReadAutoReceivedData(uint32_t* buffer, int numToRead,
+ units::second_t timeout);
+
+ /**
+ * Read data that has been transferred by the automatic SPI transfer engine.
+ *
+ * Transfers may be made a byte at a time, so it's necessary for the caller
+ * to handle cases where an entire transfer has not been completed.
+ *
+ * Each received data sequence consists of a timestamp followed by the
+ * received data bytes, one byte per word (in the least significant byte).
+ * The length of each received data sequence is the same as the combined
+ * size of the data and zeroSize set in SetAutoTransmitData().
+ *
+ * Blocks until numToRead words have been read or timeout expires.
+ * May be called with numToRead=0 to retrieve how many words are available.
+ *
+ * @param buffer buffer where read words are stored
+ * @param numToRead number of words to read
* @param timeout timeout in seconds (ms resolution)
* @return Number of words remaining to be read
*/
+ WPI_DEPRECATED("Use ReadAutoReceivedData with unit-safety instead")
int ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout);
/**
@@ -249,6 +284,26 @@
* @param isSigned Is data field signed?
* @param bigEndian Is device big endian?
*/
+ void InitAccumulator(units::second_t period, int cmd, int xferSize,
+ int validMask, int validValue, int dataShift,
+ int dataSize, bool isSigned, bool bigEndian);
+
+ /**
+ * Initialize the accumulator.
+ *
+ * @param period Time between reads
+ * @param cmd SPI command to send to request data
+ * @param xferSize SPI transfer size, in bytes
+ * @param validMask Mask to apply to received data for validity checking
+ * @param validData After valid_mask is applied, required matching value for
+ * validity checking
+ * @param dataShift Bit shift to apply to received data to get actual data
+ * value
+ * @param dataSize Size (in bits) of data field
+ * @param isSigned Is data field signed?
+ * @param bigEndian Is device big endian?
+ */
+ WPI_DEPRECATED("Use InitAccumulator with unit-safety instead")
void InitAccumulator(double period, int cmd, int xferSize, int validMask,
int validValue, int dataShift, int dataSize,
bool isSigned, bool bigEndian);
@@ -345,7 +400,7 @@
double GetAccumulatorIntegratedAverage() const;
protected:
- HAL_SPIPort m_port = HAL_SPI_kInvalid;
+ hal::SPIPort m_port;
bool m_msbFirst = false; // Default little-endian
bool m_sampleOnTrailing = false; // Default data updated on falling edge
bool m_clockIdleHigh = false; // Default clock active high
diff --git a/wpilibc/src/main/native/include/frc/SampleRobot.h b/wpilibc/src/main/native/include/frc/SampleRobot.h
deleted file mode 100644
index 4bce0f3..0000000
--- a/wpilibc/src/main/native/include/frc/SampleRobot.h
+++ /dev/null
@@ -1,109 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/deprecated.h>
-
-#include "frc/RobotBase.h"
-
-namespace frc {
-
-class SampleRobot : public RobotBase {
- public:
- /**
- * Start a competition.
- *
- * This code needs to track the order of the field starting to ensure that
- * everything happens in the right order. Repeatedly run the correct method,
- * either Autonomous or OperatorControl or Test when the robot is enabled.
- * After running the correct method, wait for some state to change, either the
- * other mode starts or the robot is disabled. Then go back and wait for the
- * robot to be enabled again.
- */
- void StartCompetition() override;
-
- /**
- * Robot-wide initialization code should go here.
- *
- * Users should override this method for default Robot-wide initialization
- * which will be called when the robot is first powered on. It will be called
- * exactly one time.
- *
- * Warning: the Driver Station "Robot Code" light and FMS "Robot Ready"
- * indicators will be off until RobotInit() exits. Code in RobotInit() that
- * waits for enable will cause the robot to never indicate that the code is
- * ready, causing the robot to be bypassed in a match.
- */
- virtual void RobotInit();
-
- /**
- * Disabled should go here.
- *
- * Programmers should override this method to run code that should run while
- * the field is disabled.
- */
- virtual void Disabled();
-
- /**
- * Autonomous should go here.
- *
- * Programmers should override this method to run code that should run while
- * the field is in the autonomous period. This will be called once each time
- * the robot enters the autonomous state.
- */
- virtual void Autonomous();
-
- /**
- * Operator control (tele-operated) code should go here.
- *
- * Programmers should override this method to run code that should run while
- * the field is in the Operator Control (tele-operated) period. This is called
- * once each time the robot enters the teleop state.
- */
- virtual void OperatorControl();
-
- /**
- * Test program should go here.
- *
- * Programmers should override this method to run code that executes while the
- * robot is in test mode. This will be called once whenever the robot enters
- * test mode
- */
- virtual void Test();
-
- /**
- * Robot main program for free-form programs.
- *
- * This should be overridden by user subclasses if the intent is to not use
- * the Autonomous() and OperatorControl() methods. In that case, the program
- * is responsible for sensing when to run the autonomous and operator control
- * functions in their program.
- *
- * This method will be called immediately after the constructor is called. If
- * it has not been overridden by a user subclass (i.e. the default version
- * runs), then the Autonomous() and OperatorControl() methods will be called.
- */
- virtual void RobotMain();
-
- protected:
- WPI_DEPRECATED(
- "WARNING: While it may look like a good choice to use for your code if "
- "you're inexperienced, don't. Unless you know what you are doing, "
- "complex code will be much more difficult under this system. Use "
- "TimedRobot or Command-Based instead.")
- SampleRobot();
- virtual ~SampleRobot() = default;
-
- SampleRobot(SampleRobot&&) = default;
- SampleRobot& operator=(SampleRobot&&) = default;
-
- private:
- bool m_robotMainOverridden = true;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/SensorUtil.h b/wpilibc/src/main/native/include/frc/SensorUtil.h
index a38b542..ab471b1 100644
--- a/wpilibc/src/main/native/include/frc/SensorUtil.h
+++ b/wpilibc/src/main/native/include/frc/SensorUtil.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -15,6 +15,8 @@
*/
class SensorUtil final {
public:
+ SensorUtil() = delete;
+
/**
* Get the number of the default solenoid module.
*
@@ -109,9 +111,6 @@
static const int kPwmChannels;
static const int kRelayChannels;
static const int kPDPChannels;
-
- private:
- SensorUtil() = default;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/SerialPort.h b/wpilibc/src/main/native/include/frc/SerialPort.h
index e9e38ef..f9edb84 100644
--- a/wpilibc/src/main/native/include/frc/SerialPort.h
+++ b/wpilibc/src/main/native/include/frc/SerialPort.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,7 @@
#include <string>
+#include <hal/Types.h>
#include <wpi/StringRef.h>
#include <wpi/Twine.h>
#include <wpi/deprecated.h>
@@ -73,6 +74,9 @@
/**
* Create an instance of a Serial Port class.
*
+ * Prefer to use the constructor that doesn't take a port name, but in some
+ * cases the automatic detection might not work correctly.
+ *
* @param baudRate The baud rate to configure the serial port.
* @param port The physical port to use
* @param portName The direct port name to use
@@ -82,15 +86,14 @@
* @param stopBits The number of stop bits to use as defined by the enum
* StopBits.
*/
- WPI_DEPRECATED("Will be removed for 2020")
SerialPort(int baudRate, const wpi::Twine& portName, Port port = kOnboard,
int dataBits = 8, Parity parity = kParity_None,
StopBits stopBits = kStopBits_One);
~SerialPort();
- SerialPort(SerialPort&& rhs);
- SerialPort& operator=(SerialPort&& rhs);
+ SerialPort(SerialPort&& rhs) = default;
+ SerialPort& operator=(SerialPort&& rhs) = default;
/**
* Set the type of flow control to enable on this port.
@@ -214,10 +217,7 @@
void Reset();
private:
- int m_resourceManagerHandle = 0;
- int m_portHandle = 0;
- bool m_consoleModeEnabled = false;
- Port m_port = kOnboard;
+ hal::Handle<HAL_SerialPortHandle> m_portHandle;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Solenoid.h b/wpilibc/src/main/native/include/frc/Solenoid.h
index 8a90b26..86a2839 100644
--- a/wpilibc/src/main/native/include/frc/Solenoid.h
+++ b/wpilibc/src/main/native/include/frc/Solenoid.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,16 +10,22 @@
#include <hal/Types.h>
#include "frc/SolenoidBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableBuilder;
+
/**
* Solenoid class for running high voltage Digital Output (PCM).
*
* The Solenoid class is typically used for pneumatics solenoids, but could be
* used for any device within the current spec of the PCM.
*/
-class Solenoid : public SolenoidBase {
+class Solenoid : public SolenoidBase,
+ public Sendable,
+ public SendableHelper<Solenoid> {
public:
/**
* Constructor using the default PCM ID (0).
@@ -38,8 +44,8 @@
~Solenoid() override;
- Solenoid(Solenoid&& rhs);
- Solenoid& operator=(Solenoid&& rhs);
+ Solenoid(Solenoid&&) = default;
+ Solenoid& operator=(Solenoid&&) = default;
/**
* Set the value of a solenoid.
@@ -90,7 +96,7 @@
void InitSendable(SendableBuilder& builder) override;
private:
- HAL_SolenoidHandle m_solenoidHandle = HAL_kInvalidHandle;
+ hal::Handle<HAL_SolenoidHandle> m_solenoidHandle;
int m_channel; // The channel on the module to control
};
diff --git a/wpilibc/src/main/native/include/frc/SolenoidBase.h b/wpilibc/src/main/native/include/frc/SolenoidBase.h
index fe9f32e..314df5c 100644
--- a/wpilibc/src/main/native/include/frc/SolenoidBase.h
+++ b/wpilibc/src/main/native/include/frc/SolenoidBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,7 +8,6 @@
#pragma once
#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/SendableBase.h"
namespace frc {
@@ -16,7 +15,7 @@
* SolenoidBase class is the common base class for the Solenoid and
* DoubleSolenoid classes.
*/
-class SolenoidBase : public ErrorBase, public SendableBase {
+class SolenoidBase : public ErrorBase {
public:
/**
* Read all 8 solenoids as a single byte
diff --git a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
index 3f0c699..80adf1c 100644
--- a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
+++ b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -11,11 +11,14 @@
#include <vector>
#include "frc/SpeedController.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
-class SpeedControllerGroup : public SendableBase, public SpeedController {
+class SpeedControllerGroup : public Sendable,
+ public SpeedController,
+ public SendableHelper<SpeedControllerGroup> {
public:
template <class... SpeedControllers>
explicit SpeedControllerGroup(SpeedController& speedController,
diff --git a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
index ba4b766..5848746 100644
--- a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
+++ b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,8 @@
#pragma once
+#include "frc/smartdashboard/SendableRegistry.h"
+
namespace frc {
template <class... SpeedControllers>
@@ -14,10 +16,10 @@
SpeedController& speedController, SpeedControllers&... speedControllers)
: m_speedControllers{speedController, speedControllers...} {
for (auto& speedController : m_speedControllers)
- AddChild(&speedController.get());
+ SendableRegistry::GetInstance().AddChild(this, &speedController.get());
static int instances = 0;
++instances;
- SetName("SpeedControllerGroup", instances);
+ SendableRegistry::GetInstance().Add(this, "SpeedControllerGroup", instances);
}
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h
index a6da5ee..1c8ff80 100644
--- a/wpilibc/src/main/native/include/frc/TimedRobot.h
+++ b/wpilibc/src/main/native/include/frc/TimedRobot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,6 +8,8 @@
#pragma once
#include <hal/Types.h>
+#include <units/units.h>
+#include <wpi/deprecated.h>
#include "frc/ErrorBase.h"
#include "frc/IterativeRobotBase.h"
@@ -25,7 +27,7 @@
*/
class TimedRobot : public IterativeRobotBase, public ErrorBase {
public:
- static constexpr double kDefaultPeriod = 0.02;
+ static constexpr units::second_t kDefaultPeriod = 20_ms;
/**
* Provide an alternate "main loop" via StartCompetition().
@@ -35,25 +37,33 @@
/**
* Get the time period between calls to Periodic() functions.
*/
- double GetPeriod() const;
+ units::second_t GetPeriod() const;
/**
* Constructor for TimedRobot.
*
* @param period Period in seconds.
*/
- explicit TimedRobot(double period = kDefaultPeriod);
+ WPI_DEPRECATED("Use constructor with unit-safety instead.")
+ explicit TimedRobot(double period);
+
+ /**
+ * Constructor for TimedRobot.
+ *
+ * @param period Period.
+ */
+ explicit TimedRobot(units::second_t period = kDefaultPeriod);
~TimedRobot() override;
- TimedRobot(TimedRobot&& rhs);
- TimedRobot& operator=(TimedRobot&& rhs);
+ TimedRobot(TimedRobot&&) = default;
+ TimedRobot& operator=(TimedRobot&&) = default;
private:
- HAL_NotifierHandle m_notifier{0};
+ hal::Handle<HAL_NotifierHandle> m_notifier;
// The absolute expiration time
- double m_expirationTime = 0;
+ units::second_t m_expirationTime{0};
/**
* Update the HAL alarm time.
diff --git a/wpilibc/src/main/native/include/frc/Timer.h b/wpilibc/src/main/native/include/frc/Timer.h
index f665c83..99caa47 100644
--- a/wpilibc/src/main/native/include/frc/Timer.h
+++ b/wpilibc/src/main/native/include/frc/Timer.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,15 +7,15 @@
#pragma once
+#include <units/units.h>
#include <wpi/deprecated.h>
#include <wpi/mutex.h>
#include "frc/Base.h"
+#include "frc2/Timer.h"
namespace frc {
-using TimerInterruptHandler = void (*)(void* param);
-
/**
* Pause the task for a specified time.
*
@@ -29,16 +29,6 @@
void Wait(double seconds);
/**
- * Return the FPGA system clock time in seconds.
- *
- * This is deprecated and just forwards to Timer::GetFPGATimestamp().
- *
- * @return Robot running time in seconds.
- */
-WPI_DEPRECATED("Use Timer::GetFPGATimestamp() instead.")
-double GetClock();
-
-/**
* @brief Gives real-time clock system time with nanosecond resolution
* @return The time, just in case you want the robot to start autonomous at 8pm
* on Saturday.
@@ -66,8 +56,10 @@
virtual ~Timer() = default;
- Timer(Timer&&) = default;
- Timer& operator=(Timer&&) = default;
+ Timer(const Timer& rhs) = default;
+ Timer& operator=(const Timer& rhs) = default;
+ Timer(Timer&& rhs) = default;
+ Timer& operator=(Timer&& rhs) = default;
/**
* Get the current time from the timer. If the clock is running it is derived
@@ -144,10 +136,7 @@
static const double kRolloverTime;
private:
- double m_startTime = 0.0;
- double m_accumulatedTime = 0.0;
- bool m_running = false;
- mutable wpi::mutex m_mutex;
+ frc2::Timer m_timer;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Ultrasonic.h b/wpilibc/src/main/native/include/frc/Ultrasonic.h
index 82ae6ca..637c6fc 100644
--- a/wpilibc/src/main/native/include/frc/Ultrasonic.h
+++ b/wpilibc/src/main/native/include/frc/Ultrasonic.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,10 +12,13 @@
#include <thread>
#include <vector>
+#include <hal/SimDevice.h>
+
#include "frc/Counter.h"
#include "frc/ErrorBase.h"
#include "frc/PIDSource.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -34,7 +37,10 @@
* received. The time that the line is high determines the round trip distance
* (time of flight).
*/
-class Ultrasonic : public ErrorBase, public SendableBase, public PIDSource {
+class Ultrasonic : public ErrorBase,
+ public Sendable,
+ public PIDSource,
+ public SendableHelper<Ultrasonic> {
public:
enum DistanceUnit { kInches = 0, kMilliMeters = 1 };
@@ -226,6 +232,10 @@
bool m_enabled = false;
Counter m_counter;
DistanceUnit m_units;
+
+ hal::SimDevice m_simDevice;
+ hal::SimBoolean m_simRangeValid;
+ hal::SimDouble m_simRange;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Utility.h b/wpilibc/src/main/native/include/frc/Utility.h
index 9cb7abb..3caaf84 100644
--- a/wpilibc/src/main/native/include/frc/Utility.h
+++ b/wpilibc/src/main/native/include/frc/Utility.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -71,57 +71,3 @@
const wpi::Twine& valueBString,
const wpi::Twine& message, wpi::StringRef fileName,
int lineNumber, wpi::StringRef funcName);
-
-namespace frc {
-
-/**
- * Return the FPGA Version number.
- *
- * For now, expect this to be competition year.
- *
- * @return FPGA Version number.
- * @deprecated Use RobotController static class method
- */
-WPI_DEPRECATED("Use RobotController static class method")
-int GetFPGAVersion();
-
-/**
- * Return the FPGA Revision number.
- *
- * The format of the revision is 3 numbers. The 12 most significant bits are the
- * Major Revision. The next 8 bits are the Minor Revision. The 12 least
- * significant bits are the Build Number.
- *
- * @return FPGA Revision number.
- * @deprecated Use RobotController static class method
- */
-WPI_DEPRECATED("Use RobotController static class method")
-int64_t GetFPGARevision();
-
-/**
- * Read the microsecond-resolution timer on the FPGA.
- *
- * @return The current time in microseconds according to the FPGA (since FPGA
- * reset).
- * @deprecated Use RobotController static class method
- */
-WPI_DEPRECATED("Use RobotController static class method")
-uint64_t GetFPGATime();
-
-/**
- * Get the state of the "USER" button on the roboRIO.
- *
- * @return True if the button is currently pressed down
- * @deprecated Use RobotController static class method
- */
-WPI_DEPRECATED("Use RobotController static class method")
-bool GetUserButton();
-
-/**
- * Get a stack trace, ignoring the first "offset" symbols.
- *
- * @param offset The number of symbols at the top of the stack to ignore
- */
-std::string GetStackTrace(int offset);
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/WPIErrors.h b/wpilibc/src/main/native/include/frc/WPIErrors.h
index a7c4f16..8325c92 100644
--- a/wpilibc/src/main/native/include/frc/WPIErrors.h
+++ b/wpilibc/src/main/native/include/frc/WPIErrors.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,93 +9,87 @@
#include <stdint.h>
-#ifdef WPI_ERRORS_DEFINE_STRINGS
-#define S(label, offset, message) \
- const char* wpi_error_s_##label = message; \
- constexpr int wpi_error_value_##label = offset
-#else
-#define S(label, offset, message) \
- extern const char* wpi_error_s_##label; \
- constexpr int wpi_error_value_##label = offset
-#endif
+#define S(label, offset, message) \
+ constexpr inline const char* wpi_error_s_##label() { return message; } \
+ constexpr inline int wpi_error_value_##label() { return offset; }
// Fatal errors
S(ModuleIndexOutOfRange, -1,
- "Allocating module that is out of range or not found");
-S(ChannelIndexOutOfRange, -1, "Allocating channel that is out of range");
-S(NotAllocated, -2, "Attempting to free unallocated resource");
-S(ResourceAlreadyAllocated, -3, "Attempted to reuse an allocated resource");
-S(NoAvailableResources, -4, "No available resources to allocate");
-S(NullParameter, -5, "A pointer parameter to a method is nullptr");
-S(Timeout, -6, "A timeout has been exceeded");
-S(CompassManufacturerError, -7, "Compass manufacturer doesn't match HiTechnic");
+ "Allocating module that is out of range or not found")
+S(ChannelIndexOutOfRange, -1, "Allocating channel that is out of range")
+S(NotAllocated, -2, "Attempting to free unallocated resource")
+S(ResourceAlreadyAllocated, -3, "Attempted to reuse an allocated resource")
+S(NoAvailableResources, -4, "No available resources to allocate")
+S(NullParameter, -5, "A pointer parameter to a method is nullptr")
+S(Timeout, -6, "A timeout has been exceeded")
+S(CompassManufacturerError, -7, "Compass manufacturer doesn't match HiTechnic")
S(CompassTypeError, -8,
- "Compass type doesn't match expected type for HiTechnic compass");
-S(IncompatibleMode, -9, "The object is in an incompatible mode");
+ "Compass type doesn't match expected type for HiTechnic compass")
+S(IncompatibleMode, -9, "The object is in an incompatible mode")
S(AnalogTriggerLimitOrderError, -10,
- "AnalogTrigger limits error. Lower limit > Upper Limit");
+ "AnalogTrigger limits error. Lower limit > Upper Limit")
S(AnalogTriggerPulseOutputError, -11,
- "Attempted to read AnalogTrigger pulse output.");
-S(TaskError, -12, "Task can't be started");
-S(TaskIDError, -13, "Task error: Invalid ID.");
-S(TaskDeletedError, -14, "Task error: Task already deleted.");
-S(TaskOptionsError, -15, "Task error: Invalid options.");
-S(TaskMemoryError, -16, "Task can't be started due to insufficient memory.");
-S(TaskPriorityError, -17, "Task error: Invalid priority [1-255].");
-S(DriveUninitialized, -18, "RobotDrive not initialized for the C interface");
+ "Attempted to read AnalogTrigger pulse output.")
+S(TaskError, -12, "Task can't be started")
+S(TaskIDError, -13, "Task error: Invalid ID.")
+S(TaskDeletedError, -14, "Task error: Task already deleted.")
+S(TaskOptionsError, -15, "Task error: Invalid options.")
+S(TaskMemoryError, -16, "Task can't be started due to insufficient memory.")
+S(TaskPriorityError, -17, "Task error: Invalid priority [1-255].")
+S(DriveUninitialized, -18, "RobotDrive not initialized for the C interface")
S(CompressorNonMatching, -19,
- "Compressor slot/channel doesn't match previous instance");
-S(CompressorAlreadyDefined, -20, "Creating a second compressor instance");
+ "Compressor slot/channel doesn't match previous instance")
+S(CompressorAlreadyDefined, -20, "Creating a second compressor instance")
S(CompressorUndefined, -21,
- "Using compressor functions without defining compressor");
+ "Using compressor functions without defining compressor")
S(InconsistentArrayValueAdded, -22,
"When packing data into an array to the dashboard, not all values added were "
- "of the same type.");
+ "of the same type.")
S(MismatchedComplexTypeClose, -23,
"When packing data to the dashboard, a Close for a complex type was called "
- "without a matching Open.");
+ "without a matching Open.")
S(DashboardDataOverflow, -24,
"When packing data to the dashboard, too much data was packed and the buffer "
- "overflowed.");
+ "overflowed.")
S(DashboardDataCollision, -25,
- "The same buffer was used for packing data and for printing.");
-S(EnhancedIOMissing, -26, "IO is not attached or Enhanced IO is not enabled.");
+ "The same buffer was used for packing data and for printing.")
+S(EnhancedIOMissing, -26, "IO is not attached or Enhanced IO is not enabled.")
S(LineNotOutput, -27,
- "Cannot SetDigitalOutput for a line not configured for output.");
-S(ParameterOutOfRange, -28, "A parameter is out of range.");
-S(SPIClockRateTooLow, -29, "SPI clock rate was below the minimum supported");
-S(JaguarVersionError, -30, "Jaguar firmware version error");
-S(JaguarMessageNotFound, -31, "Jaguar message not found");
-S(NetworkTablesReadError, -40, "Error reading NetworkTables socket");
-S(NetworkTablesBufferFull, -41, "Buffer full writing to NetworkTables socket");
+ "Cannot SetDigitalOutput for a line not configured for output.")
+S(ParameterOutOfRange, -28, "A parameter is out of range.")
+S(SPIClockRateTooLow, -29, "SPI clock rate was below the minimum supported")
+S(JaguarVersionError, -30, "Jaguar firmware version error")
+S(JaguarMessageNotFound, -31, "Jaguar message not found")
+S(NetworkTablesReadError, -40, "Error reading NetworkTables socket")
+S(NetworkTablesBufferFull, -41, "Buffer full writing to NetworkTables socket")
S(NetworkTablesWrongType, -42,
- "The wrong type was read from the NetworkTables entry");
-S(NetworkTablesCorrupt, -43, "NetworkTables data stream is corrupt");
-S(SmartDashboardMissingKey, -43, "SmartDashboard data does not exist");
-S(CommandIllegalUse, -50, "Illegal use of Command");
-S(UnsupportedInSimulation, -80, "Unsupported in simulation");
-S(CameraServerError, -90, "CameraServer error");
-S(InvalidParameter, -100, "Invalid parameter value");
+ "The wrong type was read from the NetworkTables entry")
+S(NetworkTablesCorrupt, -43, "NetworkTables data stream is corrupt")
+S(SmartDashboardMissingKey, -43, "SmartDashboard data does not exist")
+S(CommandIllegalUse, -50, "Illegal use of Command")
+S(UnsupportedInSimulation, -80, "Unsupported in simulation")
+S(CameraServerError, -90, "CameraServer error")
+S(InvalidParameter, -100, "Invalid parameter value")
// Warnings
-S(SampleRateTooHigh, 1, "Analog module sample rate is too high");
+S(SampleRateTooHigh, 1, "Analog module sample rate is too high")
S(VoltageOutOfRange, 2,
- "Voltage to convert to raw value is out of range [-10; 10]");
-S(CompressorTaskError, 3, "Compressor task won't start");
-S(LoopTimingError, 4, "Digital module loop timing is not the expected value");
-S(NonBinaryDigitalValue, 5, "Digital output value is not 0 or 1");
+ "Voltage to convert to raw value is out of range [-10; 10]")
+S(CompressorTaskError, 3, "Compressor task won't start")
+S(LoopTimingError, 4, "Digital module loop timing is not the expected value")
+S(NonBinaryDigitalValue, 5, "Digital output value is not 0 or 1")
S(IncorrectBatteryChannel, 6,
- "Battery measurement channel is not correct value");
-S(BadJoystickIndex, 7, "Joystick index is out of range, should be 0-3");
-S(BadJoystickAxis, 8, "Joystick axis or POV is out of range");
-S(InvalidMotorIndex, 9, "Motor index is out of range, should be 0-3");
-S(DriverStationTaskError, 10, "Driver Station task won't start");
+ "Battery measurement channel is not correct value")
+S(BadJoystickIndex, 7, "Joystick index is out of range, should be 0-3")
+S(BadJoystickAxis, 8, "Joystick axis or POV is out of range")
+S(InvalidMotorIndex, 9, "Motor index is out of range, should be 0-3")
+S(DriverStationTaskError, 10, "Driver Station task won't start")
S(EnhancedIOPWMPeriodOutOfRange, 11,
- "Driver Station Enhanced IO PWM Output period out of range.");
-S(SPIWriteNoMOSI, 12, "Cannot write to SPI port with no MOSI output");
-S(SPIReadNoMISO, 13, "Cannot read from SPI port with no MISO input");
-S(SPIReadNoData, 14, "No data available to read from SPI");
+ "Driver Station Enhanced IO PWM Output period out of range.")
+S(SPIWriteNoMOSI, 12, "Cannot write to SPI port with no MOSI output")
+S(SPIReadNoMISO, 13, "Cannot read from SPI port with no MISO input")
+S(SPIReadNoData, 14, "No data available to read from SPI")
S(IncompatibleState, 15,
- "Incompatible State: The operation cannot be completed");
+ "Incompatible State: The operation cannot be completed")
#undef S
diff --git a/wpilibc/src/main/native/include/frc/WPILib.h b/wpilibc/src/main/native/include/frc/WPILib.h
index 27c7b69..69fa061 100644
--- a/wpilibc/src/main/native/include/frc/WPILib.h
+++ b/wpilibc/src/main/native/include/frc/WPILib.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,15 @@
#pragma once
+// clang-format off
+#ifdef _MSC_VER
+#pragma message "warning: Including this header drastically increases compilation times and is bad style. Include only what you use instead."
+#else
+#warning "Including this header drastically increases compilation times and is bad style. Include only what you use instead."
+#endif
+
+// clang-format on
+
#include <cameraserver/CameraServer.h>
#include <vision/VisionRunner.h>
@@ -23,7 +32,6 @@
#include "frc/AnalogTriggerOutput.h"
#include "frc/BuiltInAccelerometer.h"
#include "frc/Compressor.h"
-#include "frc/ControllerPower.h"
#include "frc/Counter.h"
#include "frc/DMC60.h"
#include "frc/DigitalInput.h"
@@ -57,7 +65,6 @@
#include "frc/RobotDrive.h"
#include "frc/SD540.h"
#include "frc/SPI.h"
-#include "frc/SampleRobot.h"
#include "frc/SensorUtil.h"
#include "frc/SerialPort.h"
#include "frc/Servo.h"
diff --git a/wpilibc/src/main/native/include/frc/Watchdog.h b/wpilibc/src/main/native/include/frc/Watchdog.h
index e15da1f..b36cf23 100644
--- a/wpilibc/src/main/native/include/frc/Watchdog.h
+++ b/wpilibc/src/main/native/include/frc/Watchdog.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,9 +12,11 @@
#include <utility>
#include <hal/cpp/fpga_clock.h>
+#include <units/units.h>
#include <wpi/SafeThread.h>
#include <wpi/StringMap.h>
#include <wpi/StringRef.h>
+#include <wpi/deprecated.h>
namespace frc {
@@ -36,10 +38,25 @@
* resolution.
* @param callback This function is called when the timeout expires.
*/
+ WPI_DEPRECATED("Use unit-safe version instead")
Watchdog(double timeout, std::function<void()> callback);
+ /**
+ * Watchdog constructor.
+ *
+ * @param timeout The watchdog's timeout in seconds with microsecond
+ * resolution.
+ * @param callback This function is called when the timeout expires.
+ */
+ Watchdog(units::second_t timeout, std::function<void()> callback);
+
template <typename Callable, typename Arg, typename... Args>
+ WPI_DEPRECATED("Use unit-safe version instead")
Watchdog(double timeout, Callable&& f, Arg&& arg, Args&&... args)
+ : Watchdog(units::second_t{timeout}, arg, args...) {}
+
+ template <typename Callable, typename Arg, typename... Args>
+ Watchdog(units::second_t timeout, Callable&& f, Arg&& arg, Args&&... args)
: Watchdog(timeout,
std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
std::forward<Args>(args)...)) {}
@@ -60,9 +77,18 @@
* @param timeout The watchdog's timeout in seconds with microsecond
* resolution.
*/
+ WPI_DEPRECATED("Use unit-safe version instead")
void SetTimeout(double timeout);
/**
+ * Sets the watchdog's timeout.
+ *
+ * @param timeout The watchdog's timeout in seconds with microsecond
+ * resolution.
+ */
+ void SetTimeout(units::second_t timeout);
+
+ /**
* Returns the watchdog's timeout in seconds.
*/
double GetTimeout() const;
@@ -119,13 +145,13 @@
static constexpr std::chrono::milliseconds kMinPrintPeriod{1000};
hal::fpga_clock::time_point m_startTime;
- std::chrono::microseconds m_timeout;
+ std::chrono::nanoseconds m_timeout;
hal::fpga_clock::time_point m_expirationTime;
std::function<void()> m_callback;
hal::fpga_clock::time_point m_lastTimeoutPrintTime = hal::fpga_clock::epoch();
hal::fpga_clock::time_point m_lastEpochsPrintTime = hal::fpga_clock::epoch();
- wpi::StringMap<std::chrono::microseconds> m_epochs;
+ wpi::StringMap<std::chrono::nanoseconds> m_epochs;
bool m_isExpired = false;
bool m_suppressTimeoutMessage = false;
diff --git a/wpilibc/src/main/native/include/frc/XboxController.h b/wpilibc/src/main/native/include/frc/XboxController.h
index 8aa2d5b..3ca2f4b 100644
--- a/wpilibc/src/main/native/include/frc/XboxController.h
+++ b/wpilibc/src/main/native/include/frc/XboxController.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -232,7 +232,6 @@
*/
bool GetStartButtonReleased();
- private:
enum class Button {
kBumperLeft = 5,
kBumperRight = 6,
diff --git a/wpilibc/src/main/native/include/frc/buttons/Trigger.h b/wpilibc/src/main/native/include/frc/buttons/Trigger.h
index 8f8c477..56700e9 100644
--- a/wpilibc/src/main/native/include/frc/buttons/Trigger.h
+++ b/wpilibc/src/main/native/include/frc/buttons/Trigger.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,7 +9,8 @@
#include <atomic>
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -28,13 +29,15 @@
* only have to write the {@link Trigger#Get()} method to get the full
* functionality of the Trigger class.
*/
-class Trigger : public SendableBase {
+class Trigger : public Sendable, public SendableHelper<Trigger> {
public:
Trigger() = default;
~Trigger() override = default;
- Trigger(Trigger&&) = default;
- Trigger& operator=(Trigger&&) = default;
+ Trigger(const Trigger& rhs);
+ Trigger& operator=(const Trigger& rhs);
+ Trigger(Trigger&& rhs);
+ Trigger& operator=(Trigger&& rhs);
bool Grab();
virtual bool Get() = 0;
diff --git a/wpilibc/src/main/native/include/frc/circular_buffer.h b/wpilibc/src/main/native/include/frc/circular_buffer.h
deleted file mode 100644
index b5ebd16..0000000
--- a/wpilibc/src/main/native/include/frc/circular_buffer.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <cstddef>
-#include <vector>
-
-namespace frc {
-
-/**
- * This is a simple circular buffer so we don't need to "bucket brigade" copy
- * old values.
- */
-template <class T>
-class circular_buffer {
- public:
- explicit circular_buffer(size_t size);
-
- using value_type = T;
- using reference = value_type&;
- using const_reference = const value_type&;
- using pointer = value_type*;
- using size_type = size_t;
- using iterator_category = std::forward_iterator_tag;
- using difference_type = std::ptrdiff_t;
-
- size_type size() const;
- T& front();
- const T& front() const;
- T& back();
- const T& back() const;
- void push_front(T value);
- void push_back(T value);
- T pop_front();
- T pop_back();
- void resize(size_t size);
- void reset();
-
- T& operator[](size_t index);
- const T& operator[](size_t index) const;
-
- private:
- std::vector<T> m_data;
-
- // Index of element at front of buffer
- size_t m_front = 0;
-
- // Number of elements used in buffer
- size_t m_length = 0;
-
- size_t ModuloInc(size_t index);
- size_t ModuloDec(size_t index);
-};
-
-} // namespace frc
-
-#include "frc/circular_buffer.inc"
diff --git a/wpilibc/src/main/native/include/frc/circular_buffer.inc b/wpilibc/src/main/native/include/frc/circular_buffer.inc
deleted file mode 100644
index 4af5040..0000000
--- a/wpilibc/src/main/native/include/frc/circular_buffer.inc
+++ /dev/null
@@ -1,239 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <algorithm>
-
-namespace frc {
-
-template <class T>
-circular_buffer<T>::circular_buffer(size_t size) : m_data(size, 0) {}
-
-/**
- * Returns number of elements in buffer
- */
-template <class T>
-typename circular_buffer<T>::size_type circular_buffer<T>::size() const {
- return m_length;
-}
-
-/**
- * Returns value at front of buffer
- */
-template <class T>
-T& circular_buffer<T>::front() {
- return (*this)[0];
-}
-
-/**
- * Returns value at front of buffer
- */
-template <class T>
-const T& circular_buffer<T>::front() const {
- return (*this)[0];
-}
-
-/**
- * Returns value at back of buffer
- */
-template <class T>
-T& circular_buffer<T>::back() {
- // If there are no elements in the buffer, do nothing
- if (m_length == 0) {
- return 0;
- }
-
- return m_data[(m_front + m_length - 1) % m_data.size()];
-}
-
-/**
- * Returns value at back of buffer
- */
-template <class T>
-const T& circular_buffer<T>::back() const {
- // If there are no elements in the buffer, do nothing
- if (m_length == 0) {
- return 0;
- }
-
- return m_data[(m_front + m_length - 1) % m_data.size()];
-}
-
-/**
- * Push new value onto front of the buffer. The value at the back is overwritten
- * if the buffer is full.
- */
-template <class T>
-void circular_buffer<T>::push_front(T value) {
- if (m_data.size() == 0) {
- return;
- }
-
- m_front = ModuloDec(m_front);
-
- m_data[m_front] = value;
-
- if (m_length < m_data.size()) {
- m_length++;
- }
-}
-
-/**
- * Push new value onto back of the buffer. The value at the front is overwritten
- * if the buffer is full.
- */
-template <class T>
-void circular_buffer<T>::push_back(T value) {
- if (m_data.size() == 0) {
- return;
- }
-
- m_data[(m_front + m_length) % m_data.size()] = value;
-
- if (m_length < m_data.size()) {
- m_length++;
- } else {
- // Increment front if buffer is full to maintain size
- m_front = ModuloInc(m_front);
- }
-}
-
-/**
- * Pop value at front of buffer.
- */
-template <class T>
-T circular_buffer<T>::pop_front() {
- // If there are no elements in the buffer, do nothing
- if (m_length == 0) {
- return 0;
- }
-
- T& temp = m_data[m_front];
- m_front = ModuloInc(m_front);
- m_length--;
- return temp;
-}
-
-/**
- * Pop value at back of buffer.
- */
-template <class T>
-T circular_buffer<T>::pop_back() {
- // If there are no elements in the buffer, do nothing
- if (m_length == 0) {
- return 0;
- }
-
- m_length--;
- return m_data[(m_front + m_length) % m_data.size()];
-}
-
-/**
- * Resizes internal buffer to given size.
- */
-template <class T>
-void circular_buffer<T>::resize(size_t size) {
- if (size > m_data.size()) {
- // Find end of buffer
- size_t insertLocation = (m_front + m_length) % m_data.size();
-
- // If insertion location precedes front of buffer, push front index back
- if (insertLocation <= m_front) {
- m_front += size - m_data.size();
- }
-
- // Add elements to end of buffer
- m_data.insert(m_data.begin() + insertLocation, size - m_data.size(), 0);
- } else if (size < m_data.size()) {
- /* 1) Shift element block start at "front" left as many blocks as were
- * removed up to but not exceeding buffer[0]
- * 2) Shrink buffer, which will remove even more elements automatically if
- * necessary
- */
- size_t elemsToRemove = m_data.size() - size;
- auto frontIter = m_data.begin() + m_front;
- if (m_front < elemsToRemove) {
- /* Remove elements from end of buffer before shifting start of element
- * block. Doing so saves a few copies.
- */
- m_data.erase(frontIter + size, m_data.end());
-
- // Shift start of element block to left
- m_data.erase(m_data.begin(), frontIter);
-
- // Update metadata
- m_front = 0;
- } else {
- // Shift start of element block to left
- m_data.erase(frontIter - elemsToRemove, frontIter);
-
- // Update metadata
- m_front -= elemsToRemove;
- }
-
- /* Length only changes during a shrink if all unused spaces have been
- * removed. Length decreases as used spaces are removed to meet the
- * required size.
- */
- if (m_length > size) {
- m_length = size;
- }
- }
-}
-
-/**
- * Sets internal buffer contents to zero.
- */
-template <class T>
-void circular_buffer<T>::reset() {
- std::fill(m_data.begin(), m_data.end(), 0);
- m_front = 0;
- m_length = 0;
-}
-
-/**
- * @return Element at index starting from front of buffer.
- */
-template <class T>
-T& circular_buffer<T>::operator[](size_t index) {
- return m_data[(m_front + index) % m_data.size()];
-}
-
-/**
- * @return Element at index starting from front of buffer.
- */
-template <class T>
-const T& circular_buffer<T>::operator[](size_t index) const {
- return m_data[(m_front + index) % m_data.size()];
-}
-
-/**
- * Increment an index modulo the length of the buffer.
- *
- * @return The result of the modulo operation.
- */
-template <class T>
-size_t circular_buffer<T>::ModuloInc(size_t index) {
- return (index + 1) % m_data.size();
-}
-
-/**
- * Decrement an index modulo the length of the buffer.
- *
- * @return The result of the modulo operation.
- */
-template <class T>
-size_t circular_buffer<T>::ModuloDec(size_t index) {
- if (index == 0) {
- return m_data.size() - 1;
- } else {
- return index - 1;
- }
-}
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/Command.h b/wpilibc/src/main/native/include/frc/commands/Command.h
index f1990c7..d40bb00 100644
--- a/wpilibc/src/main/native/include/frc/commands/Command.h
+++ b/wpilibc/src/main/native/include/frc/commands/Command.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -15,7 +15,8 @@
#include "frc/ErrorBase.h"
#include "frc/commands/Subsystem.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -45,7 +46,9 @@
* @see CommandGroup
* @see Subsystem
*/
-class Command : public ErrorBase, public SendableBase {
+class Command : public ErrorBase,
+ public Sendable,
+ public SendableHelper<Command> {
friend class CommandGroup;
friend class Scheduler;
@@ -385,6 +388,34 @@
friend class ConditionalCommand;
+ /**
+ * Gets the name of this Command.
+ *
+ * @return Name
+ */
+ std::string GetName() const;
+
+ /**
+ * Sets the name of this Command.
+ *
+ * @param name name
+ */
+ void SetName(const wpi::Twine& name);
+
+ /**
+ * Gets the subsystem name of this Command.
+ *
+ * @return Subsystem name
+ */
+ std::string GetSubsystem() const;
+
+ /**
+ * Sets the subsystem name of this Command.
+ *
+ * @param subsystem subsystem name
+ */
+ void SetSubsystem(const wpi::Twine& subsystem);
+
private:
/**
* Prevents further changes from being made.
diff --git a/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h b/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h
index 142c5b0..f5cd738 100644
--- a/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h
+++ b/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -26,7 +26,7 @@
* If no Command is specified for onFalse, the occurrence of that condition
* will be a no-op.
*
- * A CondtionalCommand will require the superset of subsystems of the onTrue
+ * A ConditionalCommand will require the superset of subsystems of the onTrue
* and onFalse commands.
*
* @see Command
diff --git a/wpilibc/src/main/native/include/frc/commands/Scheduler.h b/wpilibc/src/main/native/include/frc/commands/Scheduler.h
index c5c97ba..d15ea39 100644
--- a/wpilibc/src/main/native/include/frc/commands/Scheduler.h
+++ b/wpilibc/src/main/native/include/frc/commands/Scheduler.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,7 +10,8 @@
#include <memory>
#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -18,7 +19,9 @@
class Command;
class Subsystem;
-class Scheduler : public ErrorBase, public SendableBase {
+class Scheduler : public ErrorBase,
+ public Sendable,
+ public SendableHelper<Scheduler> {
public:
/**
* Returns the Scheduler, creating it if one does not exist.
diff --git a/wpilibc/src/main/native/include/frc/commands/Subsystem.h b/wpilibc/src/main/native/include/frc/commands/Subsystem.h
index bb6d166..2637dd0 100644
--- a/wpilibc/src/main/native/include/frc/commands/Subsystem.h
+++ b/wpilibc/src/main/native/include/frc/commands/Subsystem.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -8,19 +8,22 @@
#pragma once
#include <memory>
+#include <string>
#include <wpi/StringRef.h>
#include <wpi/Twine.h>
#include "frc/ErrorBase.h"
#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
class Command;
-class Subsystem : public ErrorBase, public SendableBase {
+class Subsystem : public ErrorBase,
+ public Sendable,
+ public SendableHelper<Subsystem> {
friend class Scheduler;
public:
@@ -98,6 +101,34 @@
virtual void InitDefaultCommand();
/**
+ * Gets the name of this Subsystem.
+ *
+ * @return Name
+ */
+ std::string GetName() const;
+
+ /**
+ * Sets the name of this Subsystem.
+ *
+ * @param name name
+ */
+ void SetName(const wpi::Twine& name);
+
+ /**
+ * Gets the subsystem name of this Subsystem.
+ *
+ * @return Subsystem name
+ */
+ std::string GetSubsystem() const;
+
+ /**
+ * Sets the subsystem name of this Subsystem.
+ *
+ * @param subsystem subsystem name
+ */
+ void SetSubsystem(const wpi::Twine& subsystem);
+
+ /**
* Associate a Sendable with this Subsystem.
* Also update the child's name.
*
diff --git a/wpilibc/src/main/native/include/frc/controller/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h
new file mode 100644
index 0000000..66a36fb
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h
@@ -0,0 +1,261 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <limits>
+
+#include <units/units.h>
+
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc2 {
+
+/**
+ * Implements a PID control loop.
+ */
+class PIDController : public frc::Sendable,
+ public frc::SendableHelper<PIDController> {
+ public:
+ /**
+ * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
+ *
+ * @param Kp The proportional coefficient.
+ * @param Ki The integral coefficient.
+ * @param Kd The derivative coefficient.
+ * @param period The period between controller updates in seconds. The
+ * default is 20 milliseconds.
+ */
+ PIDController(double Kp, double Ki, double Kd,
+ units::second_t period = 20_ms);
+
+ ~PIDController() override = default;
+
+ PIDController(const PIDController&) = default;
+ PIDController& operator=(const PIDController&) = default;
+ PIDController(PIDController&&) = default;
+ PIDController& operator=(PIDController&&) = default;
+
+ /**
+ * Sets the PID Controller gain parameters.
+ *
+ * Sets the proportional, integral, and differential coefficients.
+ *
+ * @param Kp Proportional coefficient
+ * @param Ki Integral coefficient
+ * @param Kd Differential coefficient
+ */
+ void SetPID(double Kp, double Ki, double Kd);
+
+ /**
+ * Sets the proportional coefficient of the PID controller gain.
+ *
+ * @param Kp proportional coefficient
+ */
+ void SetP(double Kp);
+
+ /**
+ * Sets the integral coefficient of the PID controller gain.
+ *
+ * @param Ki integral coefficient
+ */
+ void SetI(double Ki);
+
+ /**
+ * Sets the differential coefficient of the PID controller gain.
+ *
+ * @param Kd differential coefficient
+ */
+ void SetD(double Kd);
+
+ /**
+ * Gets the proportional coefficient.
+ *
+ * @return proportional coefficient
+ */
+ double GetP() const;
+
+ /**
+ * Gets the integral coefficient.
+ *
+ * @return integral coefficient
+ */
+ double GetI() const;
+
+ /**
+ * Gets the differential coefficient.
+ *
+ * @return differential coefficient
+ */
+ double GetD() const;
+
+ /**
+ * Gets the period of this controller.
+ *
+ * @return The period of the controller.
+ */
+ units::second_t GetPeriod() const;
+
+ /**
+ * Sets the setpoint for the PIDController.
+ *
+ * @param setpoint The desired setpoint.
+ */
+ void SetSetpoint(double setpoint);
+
+ /**
+ * Returns the current setpoint of the PIDController.
+ *
+ * @return The current setpoint.
+ */
+ double GetSetpoint() const;
+
+ /**
+ * Returns true if the error is within the tolerance of the error.
+ *
+ * This will return false until at least one input value has been computed.
+ */
+ bool AtSetpoint() const;
+
+ /**
+ * Enables continuous input.
+ *
+ * Rather then using the max and min input range as constraints, it considers
+ * them to be the same point and automatically calculates the shortest route
+ * to the setpoint.
+ *
+ * @param minimumInput The minimum value expected from the input.
+ * @param maximumInput The maximum value expected from the input.
+ */
+ void EnableContinuousInput(double minimumInput, double maximumInput);
+
+ /**
+ * Disables continuous input.
+ */
+ void DisableContinuousInput();
+
+ /**
+ * Sets the minimum and maximum values for the integrator.
+ *
+ * When the cap is reached, the integrator value is added to the controller
+ * output rather than the integrator value times the integral gain.
+ *
+ * @param minimumIntegral The minimum value of the integrator.
+ * @param maximumIntegral The maximum value of the integrator.
+ */
+ void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
+
+ /**
+ * Sets the error which is considered tolerable for use with AtSetpoint().
+ *
+ * @param positionTolerance Position error which is tolerable.
+ * @param velociytTolerance Velocity error which is tolerable.
+ */
+ void SetTolerance(
+ double positionTolerance,
+ double velocityTolerance = std::numeric_limits<double>::infinity());
+
+ /**
+ * Returns the difference between the setpoint and the measurement.
+ */
+ double GetPositionError() const;
+
+ /**
+ * Returns the velocity error.
+ */
+ double GetVelocityError() const;
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ */
+ double Calculate(double measurement);
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ * @param setpoint The new setpoint of the controller.
+ */
+ double Calculate(double measurement, double setpoint);
+
+ /**
+ * Reset the previous error, the integral term, and disable the controller.
+ */
+ void Reset();
+
+ void InitSendable(frc::SendableBuilder& builder) override;
+
+ protected:
+ /**
+ * Wraps error around for continuous inputs. The original error is returned if
+ * continuous mode is disabled.
+ *
+ * @param error The current error of the PID controller.
+ * @return Error for continuous inputs.
+ */
+ double GetContinuousError(double error) const;
+
+ private:
+ // Factor for "proportional" control
+ double m_Kp;
+
+ // Factor for "integral" control
+ double m_Ki;
+
+ // Factor for "derivative" control
+ double m_Kd;
+
+ // The period (in seconds) of the control loop running this controller
+ units::second_t m_period;
+
+ double m_maximumIntegral = 1.0;
+
+ double m_minimumIntegral = -1.0;
+
+ // Maximum input - limit setpoint to this
+ double m_maximumInput = 0;
+
+ // Minimum input - limit setpoint to this
+ double m_minimumInput = 0;
+
+ // Input range - difference between maximum and minimum
+ double m_inputRange = 0;
+
+ // Do the endpoints wrap around? eg. Absolute encoder
+ bool m_continuous = false;
+
+ // The error at the time of the most recent call to Calculate()
+ double m_positionError = 0;
+ double m_velocityError = 0;
+
+ // The error at the time of the second-most-recent call to Calculate() (used
+ // to compute velocity)
+ double m_prevError = 0;
+
+ // The sum of the errors for use in the integral calc
+ double m_totalError = 0;
+
+ // The error that is considered at setpoint.
+ double m_positionTolerance = 0.05;
+ double m_velocityTolerance = std::numeric_limits<double>::infinity();
+
+ double m_setpoint = 0;
+
+ /**
+ * Sets the minimum and maximum values expected from the input.
+ *
+ * @param minimumInput The minimum value expected from the input.
+ * @param maximumInput The maximum value expected from the input.
+ */
+ void SetInputRange(double minimumInput, double maximumInput);
+};
+
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
new file mode 100644
index 0000000..64da9eb
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -0,0 +1,260 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <limits>
+
+#include <units/units.h>
+
+#include "frc/controller/PIDController.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+#include "frc/trajectory/TrapezoidProfile.h"
+
+namespace frc {
+
+/**
+ * Implements a PID control loop whose setpoint is constrained by a trapezoid
+ * profile.
+ */
+class ProfiledPIDController : public Sendable,
+ public SendableHelper<ProfiledPIDController> {
+ public:
+ /**
+ * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
+ * Kd.
+ *
+ * @param Kp The proportional coefficient.
+ * @param Ki The integral coefficient.
+ * @param Kd The derivative coefficient.
+ * @param constraints Velocity and acceleration constraints for goal.
+ * @param period The period between controller updates in seconds. The
+ * default is 20 milliseconds.
+ */
+ ProfiledPIDController(double Kp, double Ki, double Kd,
+ frc::TrapezoidProfile::Constraints constraints,
+ units::second_t period = 20_ms);
+
+ ~ProfiledPIDController() override = default;
+
+ ProfiledPIDController(const ProfiledPIDController&) = default;
+ ProfiledPIDController& operator=(const ProfiledPIDController&) = default;
+ ProfiledPIDController(ProfiledPIDController&&) = default;
+ ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
+
+ /**
+ * Sets the PID Controller gain parameters.
+ *
+ * Sets the proportional, integral, and differential coefficients.
+ *
+ * @param Kp Proportional coefficient
+ * @param Ki Integral coefficient
+ * @param Kd Differential coefficient
+ */
+ void SetPID(double Kp, double Ki, double Kd);
+
+ /**
+ * Sets the proportional coefficient of the PID controller gain.
+ *
+ * @param Kp proportional coefficient
+ */
+ void SetP(double Kp);
+
+ /**
+ * Sets the integral coefficient of the PID controller gain.
+ *
+ * @param Ki integral coefficient
+ */
+ void SetI(double Ki);
+
+ /**
+ * Sets the differential coefficient of the PID controller gain.
+ *
+ * @param Kd differential coefficient
+ */
+ void SetD(double Kd);
+
+ /**
+ * Gets the proportional coefficient.
+ *
+ * @return proportional coefficient
+ */
+ double GetP() const;
+
+ /**
+ * Gets the integral coefficient.
+ *
+ * @return integral coefficient
+ */
+ double GetI() const;
+
+ /**
+ * Gets the differential coefficient.
+ *
+ * @return differential coefficient
+ */
+ double GetD() const;
+
+ /**
+ * Gets the period of this controller.
+ *
+ * @return The period of the controller.
+ */
+ units::second_t GetPeriod() const;
+
+ /**
+ * Sets the goal for the ProfiledPIDController.
+ *
+ * @param goal The desired unprofiled setpoint.
+ */
+ void SetGoal(TrapezoidProfile::State goal);
+
+ /**
+ * Sets the goal for the ProfiledPIDController.
+ *
+ * @param goal The desired unprofiled setpoint.
+ */
+ void SetGoal(units::meter_t goal);
+
+ /**
+ * Gets the goal for the ProfiledPIDController.
+ */
+ TrapezoidProfile::State GetGoal() const;
+
+ /**
+ * Returns true if the error is within the tolerance of the error.
+ *
+ * This will return false until at least one input value has been computed.
+ */
+ bool AtGoal() const;
+
+ /**
+ * Set velocity and acceleration constraints for goal.
+ *
+ * @param constraints Velocity and acceleration constraints for goal.
+ */
+ void SetConstraints(frc::TrapezoidProfile::Constraints constraints);
+
+ /**
+ * Returns the current setpoint of the ProfiledPIDController.
+ *
+ * @return The current setpoint.
+ */
+ TrapezoidProfile::State GetSetpoint() const;
+
+ /**
+ * Returns true if the error is within the tolerance of the error.
+ *
+ * Currently this just reports on target as the actual value passes through
+ * the setpoint. Ideally it should be based on being within the tolerance for
+ * some period of time.
+ *
+ * This will return false until at least one input value has been computed.
+ */
+ bool AtSetpoint() const;
+
+ /**
+ * Enables continuous input.
+ *
+ * Rather then using the max and min input range as constraints, it considers
+ * them to be the same point and automatically calculates the shortest route
+ * to the setpoint.
+ *
+ * @param minimumInput The minimum value expected from the input.
+ * @param maximumInput The maximum value expected from the input.
+ */
+ void EnableContinuousInput(double minimumInput, double maximumInput);
+
+ /**
+ * Disables continuous input.
+ */
+ void DisableContinuousInput();
+
+ /**
+ * Sets the minimum and maximum values for the integrator.
+ *
+ * When the cap is reached, the integrator value is added to the controller
+ * output rather than the integrator value times the integral gain.
+ *
+ * @param minimumIntegral The minimum value of the integrator.
+ * @param maximumIntegral The maximum value of the integrator.
+ */
+ void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
+
+ /**
+ * Sets the error which is considered tolerable for use with
+ * AtSetpoint().
+ *
+ * @param positionTolerance Position error which is tolerable.
+ * @param velocityTolerance Velocity error which is tolerable.
+ */
+ void SetTolerance(
+ double positionTolerance,
+ double velocityTolerance = std::numeric_limits<double>::infinity());
+
+ /**
+ * Returns the difference between the setpoint and the measurement.
+ *
+ * @return The error.
+ */
+ double GetPositionError() const;
+
+ /**
+ * Returns the change in error per second.
+ */
+ double GetVelocityError() const;
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ */
+ double Calculate(units::meter_t measurement);
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ * @param goal The new goal of the controller.
+ */
+ double Calculate(units::meter_t measurement, TrapezoidProfile::State goal);
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ * @param goal The new goal of the controller.
+ */
+ double Calculate(units::meter_t measurement, units::meter_t goal);
+
+ /**
+ * Returns the next output of the PID controller.
+ *
+ * @param measurement The current measurement of the process variable.
+ * @param goal The new goal of the controller.
+ * @param constraints Velocity and acceleration constraints for goal.
+ */
+ double Calculate(units::meter_t measurement, units::meter_t goal,
+ frc::TrapezoidProfile::Constraints constraints);
+
+ /**
+ * Reset the previous error, the integral term, and disable the controller.
+ */
+ void Reset();
+
+ void InitSendable(frc::SendableBuilder& builder) override;
+
+ private:
+ frc2::PIDController m_controller;
+ frc::TrapezoidProfile::State m_goal;
+ frc::TrapezoidProfile::State m_setpoint;
+ frc::TrapezoidProfile::Constraints m_constraints;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/RamseteController.h b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
new file mode 100644
index 0000000..3a564aa
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/trajectory/Trajectory.h"
+
+namespace frc {
+
+/**
+ * Ramsete is a nonlinear time-varying feedback controller for unicycle models
+ * that drives the model to a desired pose along a two-dimensional trajectory.
+ * Why would we need a nonlinear control law in addition to the linear ones we
+ * have used so far like PID? If we use the original approach with PID
+ * controllers for left and right position and velocity states, the controllers
+ * only deal with the local pose. If the robot deviates from the path, there is
+ * no way for the controllers to correct and the robot may not reach the desired
+ * global pose. This is due to multiple endpoints existing for the robot which
+ * have the same encoder path arc lengths.
+ *
+ * Instead of using wheel path arc lengths (which are in the robot's local
+ * coordinate frame), nonlinear controllers like pure pursuit and Ramsete use
+ * global pose. The controller uses this extra information to guide a linear
+ * reference tracker like the PID controllers back in by adjusting the
+ * references of the PID controllers.
+ *
+ * The paper "Control of Wheeled Mobile Robots: An Experimental Overview"
+ * describes a nonlinear controller for a wheeled vehicle with unicycle-like
+ * kinematics; a global pose consisting of x, y, and theta; and a desired pose
+ * consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the
+ * acronym for the title of the book it came from in Italian ("Robotica
+ * Articolata e Mobile per i SErvizi e le TEcnologie").
+ *
+ * See <https://file.tavsys.net/control/controls-engineering-in-frc.pdf> section
+ * on Ramsete unicycle controller for a derivation and analysis.
+ */
+class RamseteController {
+ public:
+ /**
+ * Construct a Ramsete unicycle controller.
+ *
+ * @param b Tuning parameter (b > 0) for which larger values make
+ * convergence more aggressive like a proportional term.
+ * @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide
+ * more damping in response.
+ */
+ RamseteController(double b, double zeta);
+
+ /**
+ * Returns true if the pose error is within tolerance of the reference.
+ */
+ bool AtReference() const;
+
+ /**
+ * Sets the pose error which is considered tolerable for use with
+ * AtReference().
+ *
+ * @param poseTolerance Pose error which is tolerable.
+ */
+ void SetTolerance(const Pose2d& poseTolerance);
+
+ /**
+ * Returns the next output of the Ramsete controller.
+ *
+ * The reference pose, linear velocity, and angular velocity should come from
+ * a drivetrain trajectory.
+ *
+ * @param currentPose The current pose.
+ * @param poseRef The desired pose.
+ * @param linearVelocityRef The desired linear velocity.
+ * @param angularVelocityRef The desired angular velocity.
+ */
+ ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+ units::meters_per_second_t linearVelocityRef,
+ units::radians_per_second_t angularVelocityRef);
+
+ /**
+ * Returns the next output of the Ramsete controller.
+ *
+ * The reference pose, linear velocity, and angular velocity should come from
+ * a drivetrain trajectory.
+ *
+ * @param currentPose The current pose.
+ * @param desiredState The desired pose, linear velocity, and angular velocity
+ * from a trajectory.
+ */
+ ChassisSpeeds Calculate(const Pose2d& currentPose,
+ const Trajectory::State& desiredState);
+
+ private:
+ double m_b;
+ double m_zeta;
+
+ Pose2d m_poseError;
+ Pose2d m_poseTolerance;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
index 1120659..192d21b 100644
--- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -10,6 +10,8 @@
#include <wpi/raw_ostream.h>
#include "frc/drive/RobotDriveBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -96,7 +98,9 @@
* RobotDrive#Drive(double, double) with the addition of a quick turn
* mode. However, it is not designed to give exactly the same response.
*/
-class DifferentialDrive : public RobotDriveBase {
+class DifferentialDrive : public RobotDriveBase,
+ public Sendable,
+ public SendableHelper<DifferentialDrive> {
public:
static constexpr double kDefaultQuickStopThreshold = 0.2;
static constexpr double kDefaultQuickStopAlpha = 0.1;
@@ -121,7 +125,7 @@
* by negating the value passed for rotation.
*
* @param xSpeed The speed at which the robot should drive along the X
- * axis [-1.0..1.0]. Forward is negative.
+ * axis [-1.0..1.0]. Forward is positive.
* @param zRotation The rotation rate of the robot around the Z axis
* [-1.0..1.0]. Clockwise is positive.
* @param squareInputs If set, decreases the input sensitivity at low speeds.
@@ -208,8 +212,8 @@
void InitSendable(SendableBuilder& builder) override;
private:
- SpeedController& m_leftMotor;
- SpeedController& m_rightMotor;
+ SpeedController* m_leftMotor;
+ SpeedController* m_rightMotor;
double m_quickStopThreshold = kDefaultQuickStopThreshold;
double m_quickStopAlpha = kDefaultQuickStopAlpha;
diff --git a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
index 2fa356d..2cc1c91 100644
--- a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,6 +13,8 @@
#include "frc/drive/RobotDriveBase.h"
#include "frc/drive/Vector2d.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -44,7 +46,9 @@
* and the positive Z axis points down. Rotations follow the right-hand rule, so
* clockwise rotation around the Z axis is positive.
*/
-class KilloughDrive : public RobotDriveBase {
+class KilloughDrive : public RobotDriveBase,
+ public Sendable,
+ public SendableHelper<KilloughDrive> {
public:
static constexpr double kDefaultLeftMotorAngle = 60.0;
static constexpr double kDefaultRightMotorAngle = 120.0;
@@ -128,9 +132,9 @@
void InitSendable(SendableBuilder& builder) override;
private:
- SpeedController& m_leftMotor;
- SpeedController& m_rightMotor;
- SpeedController& m_backMotor;
+ SpeedController* m_leftMotor;
+ SpeedController* m_rightMotor;
+ SpeedController* m_backMotor;
Vector2d m_leftVec;
Vector2d m_rightVec;
diff --git a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
index e0acaca..9ddb57e 100644
--- a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,6 +12,8 @@
#include <wpi/raw_ostream.h>
#include "frc/drive/RobotDriveBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -62,7 +64,9 @@
* RobotDrive#MecanumDrive_Polar(double, double, double) if a
* deadband of 0 is used.
*/
-class MecanumDrive : public RobotDriveBase {
+class MecanumDrive : public RobotDriveBase,
+ public Sendable,
+ public SendableHelper<MecanumDrive> {
public:
/**
* Construct a MecanumDrive.
@@ -133,10 +137,10 @@
void InitSendable(SendableBuilder& builder) override;
private:
- SpeedController& m_frontLeftMotor;
- SpeedController& m_rearLeftMotor;
- SpeedController& m_frontRightMotor;
- SpeedController& m_rearRightMotor;
+ SpeedController* m_frontLeftMotor;
+ SpeedController* m_rearLeftMotor;
+ SpeedController* m_frontRightMotor;
+ SpeedController* m_rearRightMotor;
double m_rightSideInvertMultiplier = -1.0;
diff --git a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
index 206434e..ce9cbc5 100644
--- a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
+++ b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,7 +13,6 @@
#include <wpi/raw_ostream.h>
#include "frc/MotorSafety.h"
-#include "frc/smartdashboard/SendableBase.h"
namespace frc {
@@ -22,7 +21,7 @@
/**
* Common base class for drive platforms.
*/
-class RobotDriveBase : public MotorSafety, public SendableBase {
+class RobotDriveBase : public MotorSafety {
public:
/**
* The location of a motor on the robot for the purpose of driving.
@@ -76,11 +75,6 @@
protected:
/**
- * Limit motor values to the -1.0 to +1.0 range.
- */
- double Limit(double number);
-
- /**
* Returns 0.0 if the given value is within the specified range around zero.
* The remaining range between the deadband and 1.0 is scaled from 0.0 to 1.0.
*
diff --git a/wpilibc/src/main/native/include/frc/filters/Filter.h b/wpilibc/src/main/native/include/frc/filters/Filter.h
index b0c0c11..b200407 100644
--- a/wpilibc/src/main/native/include/frc/filters/Filter.h
+++ b/wpilibc/src/main/native/include/frc/filters/Filter.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,6 +9,8 @@
#include <memory>
+#include <wpi/deprecated.h>
+
#include "frc/PIDSource.h"
namespace frc {
@@ -18,7 +20,9 @@
*/
class Filter : public PIDSource {
public:
+ WPI_DEPRECATED("This class is no longer used.")
explicit Filter(PIDSource& source);
+ WPI_DEPRECATED("This class is no longer used.")
explicit Filter(std::shared_ptr<PIDSource> source);
virtual ~Filter() = default;
diff --git a/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h b/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
index 472d53b..0a2afd2 100644
--- a/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
+++ b/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,12 +7,14 @@
#pragma once
+#include <initializer_list>
#include <memory>
#include <vector>
#include <wpi/ArrayRef.h>
+#include <wpi/circular_buffer.h>
+#include <wpi/deprecated.h>
-#include "frc/circular_buffer.h"
#include "frc/filters/Filter.h"
namespace frc {
@@ -76,6 +78,7 @@
* @param ffGains The "feed forward" or FIR gains
* @param fbGains The "feed back" or IIR gains
*/
+ WPI_DEPRECATED("Use LinearFilter class instead.")
LinearDigitalFilter(PIDSource& source, wpi::ArrayRef<double> ffGains,
wpi::ArrayRef<double> fbGains);
@@ -86,10 +89,34 @@
* @param ffGains The "feed forward" or FIR gains
* @param fbGains The "feed back" or IIR gains
*/
+ WPI_DEPRECATED("Use LinearFilter class instead.")
+ LinearDigitalFilter(PIDSource& source, std::initializer_list<double> ffGains,
+ std::initializer_list<double> fbGains);
+
+ /**
+ * Create a linear FIR or IIR filter.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+ WPI_DEPRECATED("Use LinearFilter class instead.")
LinearDigitalFilter(std::shared_ptr<PIDSource> source,
wpi::ArrayRef<double> ffGains,
wpi::ArrayRef<double> fbGains);
+ /**
+ * Create a linear FIR or IIR filter.
+ *
+ * @param source The PIDSource object that is used to get values
+ * @param ffGains The "feed forward" or FIR gains
+ * @param fbGains The "feed back" or IIR gains
+ */
+ WPI_DEPRECATED("Use LinearFilter class instead.")
+ LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ std::initializer_list<double> ffGains,
+ std::initializer_list<double> fbGains);
+
LinearDigitalFilter(LinearDigitalFilter&&) = default;
LinearDigitalFilter& operator=(LinearDigitalFilter&&) = default;
@@ -188,8 +215,8 @@
double PIDGet() override;
private:
- circular_buffer<double> m_inputs;
- circular_buffer<double> m_outputs;
+ wpi::circular_buffer<double> m_inputs;
+ wpi::circular_buffer<double> m_outputs;
std::vector<double> m_inputGains;
std::vector<double> m_outputGains;
};
diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
new file mode 100644
index 0000000..2161e82
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
@@ -0,0 +1,170 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Transform2d.h"
+#include "Translation2d.h"
+#include "Twist2d.h"
+
+namespace frc {
+
+/**
+ * Represents a 2d pose containing translational and rotational elements.
+ */
+class Pose2d {
+ public:
+ /**
+ * Constructs a pose at the origin facing toward the positive X axis.
+ * (Translation2d{0, 0} and Rotation{0})
+ */
+ constexpr Pose2d() = default;
+
+ /**
+ * Constructs a pose with the specified translation and rotation.
+ *
+ * @param translation The translational component of the pose.
+ * @param rotation The rotational component of the pose.
+ */
+ Pose2d(Translation2d translation, Rotation2d rotation);
+
+ /**
+ * Convenience constructors that takes in x and y values directly instead of
+ * having to construct a Translation2d.
+ *
+ * @param x The x component of the translational component of the pose.
+ * @param y The y component of the translational component of the pose.
+ * @param rotation The rotational component of the pose.
+ */
+ Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
+
+ /**
+ * Transforms the pose by the given transformation and returns the new
+ * transformed pose.
+ *
+ * [x_new] [cos, -sin, 0][transform.x]
+ * [y_new] += [sin, cos, 0][transform.y]
+ * [t_new] [0, 0, 1][transform.t]
+ *
+ * @param other The transform to transform the pose by.
+ *
+ * @return The transformed pose.
+ */
+ Pose2d operator+(const Transform2d& other) const;
+
+ /**
+ * Transforms the current pose by the transformation.
+ *
+ * This is similar to the + operator, except that it mutates the current
+ * object.
+ *
+ * @param other The transform to transform the pose by.
+ *
+ * @return Reference to the new mutated object.
+ */
+ Pose2d& operator+=(const Transform2d& other);
+
+ /**
+ * Returns the Transform2d that maps the one pose to another.
+ *
+ * @param other The initial pose of the transformation.
+ * @return The transform that maps the other pose to the current pose.
+ */
+ Transform2d operator-(const Pose2d& other) const;
+
+ /**
+ * Checks equality between this Pose2d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const Pose2d& other) const;
+
+ /**
+ * Checks inequality between this Pose2d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const Pose2d& other) const;
+
+ /**
+ * Returns the underlying translation.
+ *
+ * @return Reference to the translational component of the pose.
+ */
+ const Translation2d& Translation() const { return m_translation; }
+
+ /**
+ * Returns the underlying rotation.
+ *
+ * @return Reference to the rotational component of the pose.
+ */
+ const Rotation2d& Rotation() const { return m_rotation; }
+
+ /**
+ * Transforms the pose by the given transformation and returns the new pose.
+ * See + operator for the matrix multiplication performed.
+ *
+ * @param other The transform to transform the pose by.
+ *
+ * @return The transformed pose.
+ */
+ Pose2d TransformBy(const Transform2d& other) const;
+
+ /**
+ * Returns the other pose relative to the current pose.
+ *
+ * This function can often be used for trajectory tracking or pose
+ * stabilization algorithms to get the error between the reference and the
+ * current pose.
+ *
+ * @param other The pose that is the origin of the new coordinate frame that
+ * the current pose will be converted into.
+ *
+ * @return The current pose relative to the new origin pose.
+ */
+ Pose2d RelativeTo(const Pose2d& other) const;
+
+ /**
+ * Obtain a new Pose2d from a (constant curvature) velocity.
+ *
+ * See <https://file.tavsys.net/control/state-space-guide.pdf> section on
+ * nonlinear pose estimation for derivation.
+ *
+ * The twist is a change in pose in the robot's coordinate frame since the
+ * previous pose update. When the user runs exp() on the previous known
+ * field-relative pose with the argument being the twist, the user will
+ * receive the new field-relative pose.
+ *
+ * "Exp" represents the pose exponential, which is solving a differential
+ * equation moving the pose forward in time.
+ *
+ * @param twist The change in pose in the robot's coordinate frame since the
+ * previous pose update. For example, if a non-holonomic robot moves forward
+ * 0.01 meters and changes angle by .5 degrees since the previous pose update,
+ * the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+ *
+ * @return The new pose of the robot.
+ */
+ Pose2d Exp(const Twist2d& twist) const;
+
+ /**
+ * Returns a Twist2d that maps this pose to the end pose. If c is the output
+ * of a.Log(b), then a.Exp(c) would yield b.
+ *
+ * @param end The end pose for the transformation.
+ *
+ * @return The twist that maps this to end.
+ */
+ Twist2d Log(const Pose2d& end) const;
+
+ private:
+ Translation2d m_translation;
+ Rotation2d m_rotation;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
new file mode 100644
index 0000000..9453e0c
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
@@ -0,0 +1,178 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/math>
+
+namespace frc {
+
+/**
+ * A rotation in a 2d coordinate frame represented a point on the unit circle
+ * (cosine and sine).
+ */
+class Rotation2d {
+ public:
+ /**
+ * Constructs a Rotation2d with a default angle of 0 degrees.
+ */
+ constexpr Rotation2d() = default;
+
+ /**
+ * Constructs a Rotation2d with the given radian value.
+ *
+ * @param value The value of the angle in radians.
+ */
+ Rotation2d(units::radian_t value); // NOLINT(runtime/explicit)
+
+ /**
+ * Constructs a Rotation2d with the given x and y (cosine and sine)
+ * components. The x and y don't have to be normalized.
+ *
+ * @param x The x component or cosine of the rotation.
+ * @param y The y component or sine of the rotation.
+ */
+ Rotation2d(double x, double y);
+
+ /**
+ * Adds two rotations together, with the result being bounded between -pi and
+ * pi.
+ *
+ * For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) =
+ * Rotation2d{-pi/2}
+ *
+ * @param other The rotation to add.
+ *
+ * @return The sum of the two rotations.
+ */
+ Rotation2d operator+(const Rotation2d& other) const;
+
+ /**
+ * Adds a rotation to the current rotation.
+ *
+ * This is similar to the + operator except that it mutates the current
+ * object.
+ *
+ * @param other The rotation to add.
+ *
+ * @return The reference to the new mutated object.
+ */
+ Rotation2d& operator+=(const Rotation2d& other);
+
+ /**
+ * Subtracts the new rotation from the current rotation and returns the new
+ * rotation.
+ *
+ * For example, Rotation2d.FromDegrees(10) - Rotation2d.FromDegrees(100) =
+ * Rotation2d{-pi/2}
+ *
+ * @param other The rotation to subtract.
+ *
+ * @return The difference between the two rotations.
+ */
+ Rotation2d operator-(const Rotation2d& other) const;
+
+ /**
+ * Subtracts the new rotation from the current rotation.
+ *
+ * This is similar to the - operator except that it mutates the current
+ * object.
+ *
+ * @param other The rotation to subtract.
+ *
+ * @return The reference to the new mutated object.
+ */
+ Rotation2d& operator-=(const Rotation2d& other);
+
+ /**
+ * Takes the inverse of the current rotation. This is simply the negative of
+ * the current angular value.
+ *
+ * @return The inverse of the current rotation.
+ */
+ Rotation2d operator-() const;
+
+ /**
+ * Multiplies the current rotation by a scalar.
+ * @param scalar The scalar.
+ *
+ * @return The new scaled Rotation2d.
+ */
+ Rotation2d operator*(double scalar) const;
+
+ /**
+ * Checks equality between this Rotation2d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const Rotation2d& other) const;
+
+ /**
+ * Checks inequality between this Rotation2d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const Rotation2d& other) const;
+
+ /**
+ * Adds the new rotation to the current rotation using a rotation matrix.
+ *
+ * [cos_new] [other.cos, -other.sin][cos]
+ * [sin_new] = [other.sin, other.cos][sin]
+ *
+ * value_new = std::atan2(cos_new, sin_new)
+ *
+ * @param other The rotation to rotate by.
+ *
+ * @return The new rotated Rotation2d.
+ */
+ Rotation2d RotateBy(const Rotation2d& other) const;
+
+ /**
+ * Returns the radian value of the rotation.
+ *
+ * @return The radian value of the rotation.
+ */
+ units::radian_t Radians() const { return m_value; }
+
+ /**
+ * Returns the degree value of the rotation.
+ *
+ * @return The degree value of the rotation.
+ */
+ units::degree_t Degrees() const { return m_value; }
+
+ /**
+ * Returns the cosine of the rotation.
+ *
+ * @return The cosine of the rotation.
+ */
+ double Cos() const { return m_cos; }
+
+ /**
+ * Returns the sine of the rotation.
+ *
+ * @return The sine of the rotation.
+ */
+ double Sin() const { return m_sin; }
+
+ /**
+ * Returns the tangent of the rotation.
+ *
+ * @return The tangent of the rotation.
+ */
+ double Tan() const { return m_sin / m_cos; }
+
+ private:
+ units::radian_t m_value = 0_deg;
+ double m_cos = 1;
+ double m_sin = 0;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h
new file mode 100644
index 0000000..c75fbeb
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "Translation2d.h"
+
+namespace frc {
+
+class Pose2d;
+
+/**
+ * Represents a transformation for a Pose2d.
+ */
+class Transform2d {
+ public:
+ /**
+ * Constructs the transform that maps the initial pose to the final pose.
+ *
+ * @param initial The initial pose for the transformation.
+ * @param final The final pose for the transformation.
+ */
+ Transform2d(Pose2d initial, Pose2d final);
+
+ /**
+ * Constructs a transform with the given translation and rotation components.
+ *
+ * @param translation Translational component of the transform.
+ * @param rotation Rotational component of the transform.
+ */
+ Transform2d(Translation2d translation, Rotation2d rotation);
+
+ /**
+ * Constructs the identity transform -- maps an initial pose to itself.
+ */
+ constexpr Transform2d() = default;
+
+ /**
+ * Returns the translation component of the transformation.
+ *
+ * @return Reference to the translational component of the transform.
+ */
+ const Translation2d& Translation() const { return m_translation; }
+
+ /**
+ * Returns the rotational component of the transformation.
+ *
+ * @return Reference to the rotational component of the transform.
+ */
+ const Rotation2d& Rotation() const { return m_rotation; }
+
+ /**
+ * Scales the transform by the scalar.
+ *
+ * @param scalar The scalar.
+ * @return The scaled Transform2d.
+ */
+ Transform2d operator*(double scalar) const {
+ return Transform2d(m_translation * scalar, m_rotation * scalar);
+ }
+
+ /**
+ * Checks equality between this Transform2d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const Transform2d& other) const;
+
+ /**
+ * Checks inequality between this Transform2d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const Transform2d& other) const;
+
+ private:
+ Translation2d m_translation;
+ Rotation2d m_rotation;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
new file mode 100644
index 0000000..a53abf1
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
@@ -0,0 +1,214 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "Rotation2d.h"
+
+namespace frc {
+
+/**
+ * Represents a translation in 2d space.
+ * This object can be used to represent a point or a vector.
+ *
+ * This assumes that you are using conventional mathematical axes.
+ * When the robot is placed on the origin, facing toward the X direction,
+ * moving forward increases the X, whereas moving to the left increases the Y.
+ */
+class Translation2d {
+ public:
+ /**
+ * Constructs a Translation2d with X and Y components equal to zero.
+ */
+ constexpr Translation2d() = default;
+
+ /**
+ * Constructs a Translation2d with the X and Y components equal to the
+ * provided values.
+ *
+ * @param x The x component of the translation.
+ * @param y The y component of the translation.
+ */
+ Translation2d(units::meter_t x, units::meter_t y);
+
+ /**
+ * Calculates the distance between two translations in 2d space.
+ *
+ * This function uses the pythagorean theorem to calculate the distance.
+ * distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
+ *
+ * @param other The translation to compute the distance to.
+ *
+ * @return The distance between the two translations.
+ */
+ units::meter_t Distance(const Translation2d& other) const;
+
+ /**
+ * Returns the X component of the translation.
+ *
+ * @return The x component of the translation.
+ */
+ units::meter_t X() const { return m_x; }
+
+ /**
+ * Returns the Y component of the translation.
+ *
+ * @return The y component of the translation.
+ */
+ units::meter_t Y() const { return m_y; }
+
+ /**
+ * Returns the norm, or distance from the origin to the translation.
+ *
+ * @return The norm of the translation.
+ */
+ units::meter_t Norm() const;
+
+ /**
+ * Applies a rotation to the translation in 2d space.
+ *
+ * This multiplies the translation vector by a counterclockwise rotation
+ * matrix of the given angle.
+ *
+ * [x_new] [other.cos, -other.sin][x]
+ * [y_new] = [other.sin, other.cos][y]
+ *
+ * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
+ * Translation2d of {0, 2}.
+ *
+ * @param other The rotation to rotate the translation by.
+ *
+ * @return The new rotated translation.
+ */
+ Translation2d RotateBy(const Rotation2d& other) const;
+
+ /**
+ * Adds two translations in 2d space and returns the sum. This is similar to
+ * vector addition.
+ *
+ * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
+ * Translation2d{3.0, 8.0}
+ *
+ * @param other The translation to add.
+ *
+ * @return The sum of the translations.
+ */
+ Translation2d operator+(const Translation2d& other) const;
+
+ /**
+ * Adds the new translation to the current translation.
+ *
+ * This is similar to the + operator, except that the current object is
+ * mutated.
+ *
+ * @param other The translation to add.
+ *
+ * @return The reference to the new mutated object.
+ */
+ Translation2d& operator+=(const Translation2d& other);
+
+ /**
+ * Subtracts the other translation from the other translation and returns the
+ * difference.
+ *
+ * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
+ * Translation2d{4.0, 2.0}
+ *
+ * @param other The translation to subtract.
+ *
+ * @return The difference between the two translations.
+ */
+ Translation2d operator-(const Translation2d& other) const;
+
+ /**
+ * Subtracts the new translation from the current translation.
+ *
+ * This is similar to the - operator, except that the current object is
+ * mutated.
+ *
+ * @param other The translation to subtract.
+ *
+ * @return The reference to the new mutated object.
+ */
+ Translation2d& operator-=(const Translation2d& other);
+
+ /**
+ * Returns the inverse of the current translation. This is equivalent to
+ * rotating by 180 degrees, flipping the point over both axes, or simply
+ * negating both components of the translation.
+ *
+ * @return The inverse of the current translation.
+ */
+ Translation2d operator-() const;
+
+ /**
+ * Multiplies the translation by a scalar and returns the new translation.
+ *
+ * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+ *
+ * @param scalar The scalar to multiply by.
+ *
+ * @return The scaled translation.
+ */
+ Translation2d operator*(double scalar) const;
+
+ /**
+ * Multiplies the current translation by a scalar.
+ *
+ * This is similar to the * operator, except that current object is mutated.
+ *
+ * @param scalar The scalar to multiply by.
+ *
+ * @return The reference to the new mutated object.
+ */
+ Translation2d& operator*=(double scalar);
+
+ /**
+ * Divides the translation by a scalar and returns the new translation.
+ *
+ * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+ *
+ * @param scalar The scalar to divide by.
+ *
+ * @return The scaled translation.
+ */
+ Translation2d operator/(double scalar) const;
+
+ /**
+ * Checks equality between this Translation2d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const Translation2d& other) const;
+
+ /**
+ * Checks inequality between this Translation2d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const Translation2d& other) const;
+
+ /*
+ * Divides the current translation by a scalar.
+ *
+ * This is similar to the / operator, except that current object is mutated.
+ *
+ * @param scalar The scalar to divide by.
+ *
+ * @return The reference to the new mutated object.
+ */
+ Translation2d& operator/=(double scalar);
+
+ private:
+ units::meter_t m_x = 0_m;
+ units::meter_t m_y = 0_m;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/geometry/Twist2d.h b/wpilibc/src/main/native/include/frc/geometry/Twist2d.h
new file mode 100644
index 0000000..ab246a0
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/geometry/Twist2d.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <units/units.h>
+
+namespace frc {
+/**
+ * A change in distance along arc since the last pose update. We can use ideas
+ * from differential calculus to create new Pose2ds from a Twist2d and vise
+ * versa.
+ *
+ * A Twist can be used to represent a difference between two poses.
+ */
+struct Twist2d {
+ /**
+ * Linear "dx" component
+ */
+ units::meter_t dx = 0_m;
+
+ /**
+ * Linear "dy" component
+ */
+ units::meter_t dy = 0_m;
+
+ /**
+ * Angular "dtheta" component (radians)
+ */
+ units::radian_t dtheta = 0_rad;
+
+ /**
+ * Checks equality between this Twist2d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const Twist2d& other) const {
+ return units::math::abs(dx - other.dx) < 1E-9_m &&
+ units::math::abs(dy - other.dy) < 1E-9_m &&
+ units::math::abs(dtheta - other.dtheta) < 1E-9_rad;
+ }
+
+ /**
+ * Checks inequality between this Twist2d and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const Twist2d& other) const { return !operator==(other); }
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h
new file mode 100644
index 0000000..8c772c0
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+/**
+ * Represents the speed of a robot chassis. Although this struct contains
+ * similar members compared to a Twist2d, they do NOT represent the same thing.
+ * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
+ * reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot
+ * frame of reference.
+ *
+ * A strictly non-holonomic drivetrain, such as a differential drive, should
+ * never have a dy component because it can never move sideways. Holonomic
+ * drivetrains such as swerve and mecanum will often have all three components.
+ */
+struct ChassisSpeeds {
+ /**
+ * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
+ */
+ units::meters_per_second_t vx = 0_mps;
+
+ /**
+ * Represents strafe velocity w.r.t the robot frame of reference. (Left is +)
+ */
+ units::meters_per_second_t vy = 0_mps;
+
+ /**
+ * Represents the angular velocity of the robot frame. (CCW is +)
+ */
+ units::radians_per_second_t omega = 0_rad_per_s;
+
+ /**
+ * Converts a user provided field-relative set of speeds into a robot-relative
+ * ChassisSpeeds object.
+ *
+ * @param vx The component of speed in the x direction relative to the field.
+ * Positive x is away from your alliance wall.
+ * @param vy The component of speed in the y direction relative to the field.
+ * Positive y is to your left when standing behind the alliance wall.
+ * @param omega The angular rate of the robot.
+ * @param robotAngle The angle of the robot as measured by a gyroscope. The
+ * robot's angle is considered to be zero when it is facing directly away from
+ * your alliance station wall. Remember that this should be CCW positive.
+ *
+ * @return ChassisSpeeds object representing the speeds in the robot's frame
+ * of reference.
+ */
+ static ChassisSpeeds FromFieldRelativeSpeeds(
+ units::meters_per_second_t vx, units::meters_per_second_t vy,
+ units::radians_per_second_t omega, const Rotation2d& robotAngle) {
+ return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
+ -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
+ }
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
new file mode 100644
index 0000000..34407b9
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) to
+ * left and right wheel velocities for a differential drive.
+ *
+ * Inverse kinematics converts a desired chassis speed into left and right
+ * velocity components whereas forward kinematics converts left and right
+ * component velocities into a linear and angular chassis speed.
+ */
+class DifferentialDriveKinematics {
+ public:
+ /**
+ * Constructs a differential drive kinematics object.
+ *
+ * @param trackWidth The track width of the drivetrain. Theoretically, this is
+ * the distance between the left wheels and right wheels. However, the
+ * empirical value may be larger than the physical measured value due to
+ * scrubbing effects.
+ */
+ explicit DifferentialDriveKinematics(units::meter_t trackWidth);
+
+ /**
+ * Returns a chassis speed from left and right component velocities using
+ * forward kinematics.
+ *
+ * @param wheelSpeeds The left and right velocities.
+ * @return The chassis speed.
+ */
+ ChassisSpeeds ToChassisSpeeds(
+ const DifferentialDriveWheelSpeeds& wheelSpeeds) const;
+
+ /**
+ * Returns left and right component velocities from a chassis speed using
+ * inverse kinematics.
+ *
+ * @param chassisSpeeds The linear and angular (dx and dtheta) components that
+ * represent the chassis' speed.
+ * @return The left and right velocities.
+ */
+ DifferentialDriveWheelSpeeds ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds) const;
+
+ private:
+ units::meter_t m_trackWidth;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
new file mode 100644
index 0000000..fd41d15
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "DifferentialDriveKinematics.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc2/Timer.h"
+
+namespace frc {
+/**
+ * Class for differential drive odometry. Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from
+ * 2 encoders and a gyroscope.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ *
+ * Note: It is important to reset both your encoders to zero before you start
+ * using this class. Only reset your encoders ONCE. You should not reset your
+ * encoders even if you want to reset your robot's pose.
+ */
+class DifferentialDriveOdometry {
+ public:
+ /**
+ * Constructs a DifferentialDriveOdometry object.
+ *
+ * @param kinematics The differential drive kinematics for your drivetrain.
+ * @param initialPose The starting position of the robot on the field.
+ */
+ explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
+ const Pose2d& initialPose = Pose2d());
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ * @param pose The position on the field that your robot is at.
+ */
+ void ResetPosition(const Pose2d& pose) {
+ m_pose = pose;
+ m_previousAngle = pose.Rotation();
+ }
+
+ /**
+ * Returns the position of the robot on the field.
+ * @return The pose of the robot.
+ */
+ const Pose2d& GetPose() const { return m_pose; }
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method takes in the current time as
+ * a parameter to calculate period (difference between two timestamps). The
+ * period is used to calculate the change in distance from a velocity. This
+ * also takes in an angle parameter which is used instead of the
+ * angular rate that is calculated from forward kinematics.
+ *
+ * @param currentTime The current time.
+ * @param angle The angle of the robot.
+ * @param wheelSpeeds The current wheel speeds.
+ *
+ * @return The new pose of the robot.
+ */
+ const Pose2d& UpdateWithTime(units::second_t currentTime,
+ const Rotation2d& angle,
+ const DifferentialDriveWheelSpeeds& wheelSpeeds);
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method automatically calculates
+ * the current time to calculate period (difference between two timestamps).
+ * The period is used to calculate the change in distance from a velocity.
+ * This also takes in an angle parameter which is used instead of the
+ * angular rate that is calculated from forward kinematics.
+ *
+ * @param angle The angle of the robot.
+ * @param wheelSpeeds The current wheel speeds.
+ *
+ * @return The new pose of the robot.
+ */
+ const Pose2d& Update(const Rotation2d& angle,
+ const DifferentialDriveWheelSpeeds& wheelSpeeds) {
+ return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
+ }
+
+ private:
+ DifferentialDriveKinematics m_kinematics;
+ Pose2d m_pose;
+
+ units::second_t m_previousTime = -1_s;
+ Rotation2d m_previousAngle;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
new file mode 100644
index 0000000..66bd84e
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+namespace frc {
+/**
+ * Represents the wheel speeds for a differential drive drivetrain.
+ */
+struct DifferentialDriveWheelSpeeds {
+ /**
+ * Speed of the left side of the robot.
+ */
+ units::meters_per_second_t left = 0_mps;
+
+ /**
+ * Speed of the right side of the robot.
+ */
+ units::meters_per_second_t right = 0_mps;
+
+ /**
+ * Normalizes the wheel speeds using some max attainable speed. Sometimes,
+ * after inverse kinematics, the requested speed from a/several modules may be
+ * above the max attainable speed for the driving motor on that module. To fix
+ * this issue, one can "normalize" all the wheel speeds to make sure that all
+ * requested module speeds are below the absolute threshold, while maintaining
+ * the ratio of speeds between modules.
+ *
+ * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
+ */
+ void Normalize(units::meters_per_second_t attainableMaxSpeed);
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
new file mode 100644
index 0000000..6b0329d
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -0,0 +1,141 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/QR>
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+namespace frc {
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual wheel speeds.
+ *
+ * The inverse kinematics (converting from a desired chassis velocity to
+ * individual wheel speeds) uses the relative locations of the wheels with
+ * respect to the center of rotation. The center of rotation for inverse
+ * kinematics is also variable. This means that you can set your set your center
+ * of rotation in a corner of the robot to perform special evasion manuevers.
+ *
+ * Forward kinematics (converting an array of wheel speeds into the overall
+ * chassis motion) is performs the exact opposite of what inverse kinematics
+ * does. Since this is an overdetermined system (more equations than variables),
+ * we use a least-squares approximation.
+ *
+ * The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
+ * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
+ * multiply by [wheelSpeeds] to get our chassis speeds.
+ *
+ * Forward kinematics is also used for odometry -- determining the position of
+ * the robot on the field using encoders and a gyro.
+ */
+class MecanumDriveKinematics {
+ public:
+ /**
+ * Constructs a mecanum drive kinematics object.
+ *
+ * @param frontLeftWheel The location of the front-left wheel relative to the
+ * physical center of the robot.
+ * @param frontRightWheel The location of the front-right wheel relative to
+ * the physical center of the robot.
+ * @param rearLeftWheel The location of the rear-left wheel relative to the
+ * physical center of the robot.
+ * @param rearRightWheel The location of the rear-right wheel relative to the
+ * physical center of the robot.
+ */
+ explicit MecanumDriveKinematics(Translation2d frontLeftWheel,
+ Translation2d frontRightWheel,
+ Translation2d rearLeftWheel,
+ Translation2d rearRightWheel)
+ : m_frontLeftWheel{frontLeftWheel},
+ m_frontRightWheel{frontRightWheel},
+ m_rearLeftWheel{rearLeftWheel},
+ m_rearRightWheel{rearRightWheel} {
+ SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
+ rearRightWheel);
+ m_forwardKinematics = m_inverseKinematics.householderQr();
+ }
+
+ MecanumDriveKinematics(const MecanumDriveKinematics&) = default;
+
+ /**
+ * Performs inverse kinematics to return the wheel speeds from a desired
+ * chassis velocity. This method is often used to convert joystick values into
+ * wheel speeds.
+ *
+ * This function also supports variable centers of rotation. During normal
+ * operations, the center of rotation is usually the same as the physical
+ * center of the robot; therefore, the argument is defaulted to that use case.
+ * However, if you wish to change the center of rotation for evasive
+ * manuevers, vision alignment, or for any other use case, you can do so.
+ *
+ * @param chassisSpeeds The desired chassis speed.
+ * @param centerOfRotation The center of rotation. For example, if you set the
+ * center of rotation at one corner of the robot and
+ * provide a chassis speed that only has a dtheta
+ * component, the robot will rotate around that
+ * corner.
+ *
+ * @return The wheel speeds. Use caution because they are not normalized.
+ * Sometimes, a user input may cause one of the wheel speeds to go
+ * above the attainable max velocity. Use the
+ * MecanumDriveWheelSpeeds::Normalize() function to rectify this
+ * issue. In addition, you can leverage the power of C++17 to directly
+ * assign the wheel speeds to variables:
+ *
+ * @code{.cpp}
+ * auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(chassisSpeeds);
+ * @endcode
+ */
+ MecanumDriveWheelSpeeds ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds,
+ const Translation2d& centerOfRotation = Translation2d());
+
+ /**
+ * Performs forward kinematics to return the resulting chassis state from the
+ * given wheel speeds. This method is often used for odometry -- determining
+ * the robot's position on the field using data from the real-world speed of
+ * each wheel on the robot.
+ *
+ * @param wheelSpeeds The current mecanum drive wheel speeds.
+ *
+ * @return The resulting chassis speed.
+ */
+ ChassisSpeeds ToChassisSpeeds(const MecanumDriveWheelSpeeds& wheelSpeeds);
+
+ private:
+ Eigen::Matrix<double, 4, 3> m_inverseKinematics;
+ Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
+ Translation2d m_frontLeftWheel;
+ Translation2d m_frontRightWheel;
+ Translation2d m_rearLeftWheel;
+ Translation2d m_rearRightWheel;
+
+ Translation2d m_previousCoR;
+
+ /**
+ * Construct inverse kinematics matrix from wheel locations.
+ *
+ * @param fl The location of the front-left wheel relative to the physical
+ * center of the robot.
+ * @param fr The location of the front-right wheel relative to the physical
+ * center of the robot.
+ * @param rl The location of the rear-left wheel relative to the physical
+ * center of the robot.
+ * @param rr The location of the rear-right wheel relative to the physical
+ * center of the robot.
+ */
+ void SetInverseKinematics(Translation2d fl, Translation2d fr,
+ Translation2d rl, Translation2d rr);
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
new file mode 100644
index 0000000..e8816e1
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "frc2/Timer.h"
+
+namespace frc {
+
+/**
+ * Class for mecanum drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * mecanum wheel encoders.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+class MecanumDriveOdometry {
+ public:
+ /**
+ * Constructs a MecanumDriveOdometry object.
+ *
+ * @param kinematics The mecanum drive kinematics for your drivetrain.
+ * @param initialPose The starting position of the robot on the field.
+ */
+ explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+ const Pose2d& initialPose = Pose2d());
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ * @param pose The position on the field that your robot is at.
+ */
+ void ResetPosition(const Pose2d& pose) {
+ m_pose = pose;
+ m_previousAngle = pose.Rotation();
+ }
+
+ /**
+ * Returns the position of the robot on the field.
+ * @return The pose of the robot.
+ */
+ const Pose2d& GetPose() const { return m_pose; }
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method takes in the current time as
+ * a parameter to calculate period (difference between two timestamps). The
+ * period is used to calculate the change in distance from a velocity. This
+ * also takes in an angle parameter which is used instead of the
+ * angular rate that is calculated from forward kinematics.
+ *
+ * @param currentTime The current time.
+ * @param angle The angle of the robot.
+ * @param wheelSpeeds The current wheel speeds.
+ *
+ * @return The new pose of the robot.
+ */
+ const Pose2d& UpdateWithTime(units::second_t currentTime,
+ const Rotation2d& angle,
+ MecanumDriveWheelSpeeds wheelSpeeds);
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method automatically calculates
+ * the current time to calculate period (difference between two timestamps).
+ * The period is used to calculate the change in distance from a velocity.
+ * This also takes in an angle parameter which is used instead of the
+ * angular rate that is calculated from forward kinematics.
+ *
+ * @param angle The angle of the robot.
+ * @param wheelSpeeds The current wheel speeds.
+ *
+ * @return The new pose of the robot.
+ */
+ const Pose2d& Update(const Rotation2d& angle,
+ MecanumDriveWheelSpeeds wheelSpeeds) {
+ return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
+ }
+
+ private:
+ MecanumDriveKinematics m_kinematics;
+ Pose2d m_pose;
+
+ units::second_t m_previousTime = -1_s;
+ Rotation2d m_previousAngle;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
new file mode 100644
index 0000000..159f7f0
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+namespace frc {
+/**
+ * Represents the wheel speeds for a mecanum drive drivetrain.
+ */
+struct MecanumDriveWheelSpeeds {
+ /**
+ * Speed of the front-left wheel.
+ */
+ units::meters_per_second_t frontLeft = 0_mps;
+
+ /**
+ * Speed of the front-right wheel.
+ */
+ units::meters_per_second_t frontRight = 0_mps;
+
+ /**
+ * Speed of the rear-left wheel.
+ */
+ units::meters_per_second_t rearLeft = 0_mps;
+
+ /**
+ * Speed of the rear-right wheel.
+ */
+ units::meters_per_second_t rearRight = 0_mps;
+
+ /**
+ * Normalizes the wheel speeds using some max attainable speed. Sometimes,
+ * after inverse kinematics, the requested speed from a/several modules may be
+ * above the max attainable speed for the driving motor on that module. To fix
+ * this issue, one can "normalize" all the wheel speeds to make sure that all
+ * requested module speeds are below the absolute threshold, while maintaining
+ * the ratio of speeds between modules.
+ *
+ * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
+ */
+ void Normalize(units::meters_per_second_t attainableMaxSpeed);
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
new file mode 100644
index 0000000..fcd8087
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -0,0 +1,150 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <cstddef>
+
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <units/units.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/SwerveModuleState.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual module states (speed and angle).
+ *
+ * The inverse kinematics (converting from a desired chassis velocity to
+ * individual module states) uses the relative locations of the modules with
+ * respect to the center of rotation. The center of rotation for inverse
+ * kinematics is also variable. This means that you can set your set your center
+ * of rotation in a corner of the robot to perform special evasion manuevers.
+ *
+ * Forward kinematics (converting an array of module states into the overall
+ * chassis motion) is performs the exact opposite of what inverse kinematics
+ * does. Since this is an overdetermined system (more equations than variables),
+ * we use a least-squares approximation.
+ *
+ * The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
+ * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
+ * multiply by [moduleStates] to get our chassis speeds.
+ *
+ * Forward kinematics is also used for odometry -- determining the position of
+ * the robot on the field using encoders and a gyro.
+ */
+template <size_t NumModules>
+class SwerveDriveKinematics {
+ public:
+ /**
+ * Constructs a swerve drive kinematics object. This takes in a variable
+ * number of wheel locations as Translation2ds. The order in which you pass in
+ * the wheel locations is the same order that you will recieve the module
+ * states when performing inverse kinematics. It is also expected that you
+ * pass in the module states in the same order when calling the forward
+ * kinematics methods.
+ *
+ * @param wheels The locations of the wheels relative to the physical center
+ * of the robot.
+ */
+ template <typename... Wheels>
+ explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
+ : m_modules{wheel, wheels...} {
+ static_assert(sizeof...(wheels) >= 1,
+ "A swerve drive requires at least two modules");
+
+ for (size_t i = 0; i < NumModules; i++) {
+ // clang-format off
+ m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
+ 1, 0, (-m_modules[i].Y()).template to<double>(),
+ 0, 1, (+m_modules[i].X()).template to<double>();
+ // clang-format on
+ }
+
+ m_forwardKinematics = m_inverseKinematics.householderQr();
+ }
+
+ SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
+
+ /**
+ * Performs inverse kinematics to return the module states from a desired
+ * chassis velocity. This method is often used to convert joystick values into
+ * module speeds and angles.
+ *
+ * This function also supports variable centers of rotation. During normal
+ * operations, the center of rotation is usually the same as the physical
+ * center of the robot; therefore, the argument is defaulted to that use case.
+ * However, if you wish to change the center of rotation for evasive
+ * manuevers, vision alignment, or for any other use case, you can do so.
+ *
+ * @param chassisSpeeds The desired chassis speed.
+ * @param centerOfRotation The center of rotation. For example, if you set the
+ * center of rotation at one corner of the robot and provide a chassis speed
+ * that only has a dtheta component, the robot will rotate around that corner.
+ *
+ * @return An array containing the module states. Use caution because these
+ * module states are not normalized. Sometimes, a user input may cause one of
+ * the module speeds to go above the attainable max velocity. Use the
+ * <NormalizeWheelSpeeds> function to rectify this issue. In addition, you can
+ * leverage the power of C++17 to directly assign the module states to
+ * variables:
+ *
+ * @code{.cpp}
+ * auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds);
+ * @endcode
+ */
+ std::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
+ const ChassisSpeeds& chassisSpeeds,
+ const Translation2d& centerOfRotation = Translation2d());
+
+ /**
+ * Performs forward kinematics to return the resulting chassis state from the
+ * given module states. This method is often used for odometry -- determining
+ * the robot's position on the field using data from the real-world speed and
+ * angle of each module on the robot.
+ *
+ * @param wheelStates The state of the modules (as a SwerveModuleState type)
+ * as measured from respective encoders and gyros. The order of the swerve
+ * module states should be same as passed into the constructor of this class.
+ *
+ * @return The resulting chassis speed.
+ */
+ template <typename... ModuleStates>
+ ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates);
+
+ /**
+ * Normalizes the wheel speeds using some max attainable speed. Sometimes,
+ * after inverse kinematics, the requested speed from a/several modules may be
+ * above the max attainable speed for the driving motor on that module. To fix
+ * this issue, one can "normalize" all the wheel speeds to make sure that all
+ * requested module speeds are below the absolute threshold, while maintaining
+ * the ratio of speeds between modules.
+ *
+ * @param moduleStates Reference to array of module states. The array will be
+ * mutated with the normalized speeds!
+ * @param attainableMaxSpeed The absolute max speed that a module can reach.
+ */
+ static void NormalizeWheelSpeeds(
+ std::array<SwerveModuleState, NumModules>* moduleStates,
+ units::meters_per_second_t attainableMaxSpeed);
+
+ private:
+ Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
+ Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
+ m_forwardKinematics;
+ std::array<Translation2d, NumModules> m_modules;
+
+ Translation2d m_previousCoR;
+};
+} // namespace frc
+
+#include "SwerveDriveKinematics.inc"
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
new file mode 100644
index 0000000..138954d
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+
+namespace frc {
+
+template <class... Wheels>
+SwerveDriveKinematics(Translation2d, Wheels...)
+ ->SwerveDriveKinematics<1 + sizeof...(Wheels)>;
+
+template <size_t NumModules>
+std::array<SwerveModuleState, NumModules>
+SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
+ const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
+ // We have a new center of rotation. We need to compute the matrix again.
+ if (centerOfRotation != m_previousCoR) {
+ for (size_t i = 0; i < NumModules; i++) {
+ // clang-format off
+ m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
+ 1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).template to<double>(),
+ 0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to<double>();
+ // clang-format on
+ }
+ m_previousCoR = centerOfRotation;
+ }
+
+ Eigen::Vector3d chassisSpeedsVector;
+ chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
+ chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+
+ Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
+ m_inverseKinematics * chassisSpeedsVector;
+ std::array<SwerveModuleState, NumModules> moduleStates;
+
+ for (size_t i = 0; i < NumModules; i++) {
+ units::meters_per_second_t x =
+ units::meters_per_second_t{moduleStatesMatrix(i * 2, 0)};
+ units::meters_per_second_t y =
+ units::meters_per_second_t{moduleStatesMatrix(i * 2 + 1, 0)};
+
+ auto speed = units::math::hypot(x, y);
+ Rotation2d rotation{x.to<double>(), y.to<double>()};
+
+ moduleStates[i] = {speed, rotation};
+ }
+
+ return moduleStates;
+}
+
+template <size_t NumModules>
+template <typename... ModuleStates>
+ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
+ ModuleStates&&... wheelStates) {
+ static_assert(sizeof...(wheelStates) == NumModules,
+ "Number of modules is not consistent with number of wheel "
+ "locations provided in constructor.");
+
+ std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
+ Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
+
+ for (size_t i = 0; i < NumModules; i++) {
+ SwerveModuleState module = moduleStates[i];
+ moduleStatesMatrix.row(i * 2)
+ << module.speed.to<double>() * module.angle.Cos();
+ moduleStatesMatrix.row(i * 2 + 1)
+ << module.speed.to<double>() * module.angle.Sin();
+ }
+
+ Eigen::Vector3d chassisSpeedsVector =
+ m_forwardKinematics.solve(moduleStatesMatrix);
+
+ return {units::meters_per_second_t{chassisSpeedsVector(0)},
+ units::meters_per_second_t{chassisSpeedsVector(1)},
+ units::radians_per_second_t{chassisSpeedsVector(2)}};
+}
+
+template <size_t NumModules>
+void SwerveDriveKinematics<NumModules>::NormalizeWheelSpeeds(
+ std::array<SwerveModuleState, NumModules>* moduleStates,
+ units::meters_per_second_t attainableMaxSpeed) {
+ auto& states = *moduleStates;
+ auto realMaxSpeed = std::max_element(states.begin(), states.end(),
+ [](const auto& a, const auto& b) {
+ return units::math::abs(a.speed) <
+ units::math::abs(b.speed);
+ })
+ ->speed;
+
+ if (realMaxSpeed > attainableMaxSpeed) {
+ for (auto& module : states) {
+ module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
+ }
+ }
+}
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
new file mode 100644
index 0000000..1dbe178
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <chrono>
+#include <cstddef>
+#include <ctime>
+
+#include <units/units.h>
+
+#include "SwerveDriveKinematics.h"
+#include "SwerveModuleState.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc2/Timer.h"
+
+namespace frc {
+
+/**
+ * Class for swerve drive odometry. Odometry allows you to track the robot's
+ * position on the field over a course of a match using readings from your
+ * swerve drive encoders and swerve azimuth encoders.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+template <size_t NumModules>
+class SwerveDriveOdometry {
+ public:
+ /**
+ * Constructs a SwerveDriveOdometry object.
+ *
+ * @param kinematics The swerve drive kinematics for your drivetrain.
+ * @param initialPose The starting position of the robot on the field.
+ */
+ SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
+ const Pose2d& initialPose = Pose2d());
+
+ /**
+ * Resets the robot's position on the field.
+ *
+ * @param pose The position on the field that your robot is at.
+ */
+ void ResetPosition(const Pose2d& pose) {
+ m_pose = pose;
+ m_previousAngle = pose.Rotation();
+ }
+
+ /**
+ * Returns the position of the robot on the field.
+ * @return The pose of the robot.
+ */
+ const Pose2d& GetPose() const { return m_pose; }
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method takes in the current time as
+ * a parameter to calculate period (difference between two timestamps). The
+ * period is used to calculate the change in distance from a velocity. This
+ * also takes in an angle parameter which is used instead of the
+ * angular rate that is calculated from forward kinematics.
+ *
+ * @param currentTime The current time.
+ * @param angle The angle of the robot.
+ * @param moduleStates The current state of all swerve modules. Please provide
+ * the states in the same order in which you instantiated
+ * your SwerveDriveKinematics.
+ *
+ * @return The new pose of the robot.
+ */
+ template <typename... ModuleStates>
+ const Pose2d& UpdateWithTime(units::second_t currentTime,
+ const Rotation2d& angle,
+ ModuleStates&&... moduleStates);
+
+ /**
+ * Updates the robot's position on the field using forward kinematics and
+ * integration of the pose over time. This method automatically calculates
+ * the current time to calculate period (difference between two timestamps).
+ * The period is used to calculate the change in distance from a velocity.
+ * This also takes in an angle parameter which is used instead of the
+ * angular rate that is calculated from forward kinematics.
+ *
+ * @param angle The angle of the robot.
+ * @param moduleStates The current state of all swerve modules. Please provide
+ * the states in the same order in which you instantiated
+ * your SwerveDriveKinematics.
+ *
+ * @return The new pose of the robot.
+ */
+ template <typename... ModuleStates>
+ const Pose2d& Update(const Rotation2d& angle,
+ ModuleStates&&... moduleStates) {
+ return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle,
+ moduleStates...);
+ }
+
+ private:
+ SwerveDriveKinematics<NumModules> m_kinematics;
+ Pose2d m_pose;
+
+ units::second_t m_previousTime = -1_s;
+ Rotation2d m_previousAngle;
+};
+
+} // namespace frc
+
+#include "SwerveDriveOdometry.inc"
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
new file mode 100644
index 0000000..1ff75ac
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc {
+template <size_t NumModules>
+SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
+ SwerveDriveKinematics<NumModules> kinematics, const Pose2d& initialPose)
+ : m_kinematics(kinematics), m_pose(initialPose) {
+ m_previousAngle = m_pose.Rotation();
+}
+
+template <size_t NumModules>
+template <typename... ModuleStates>
+const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
+ units::second_t currentTime, const Rotation2d& angle,
+ ModuleStates&&... moduleStates) {
+ units::second_t deltaTime =
+ (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
+ m_previousTime = currentTime;
+
+ auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
+ static_cast<void>(dtheta);
+
+ auto newPose = m_pose.Exp(
+ {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+
+ m_previousAngle = angle;
+ m_pose = {newPose.Translation(), angle};
+
+ return m_pose;
+}
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h
new file mode 100644
index 0000000..fbfe0d1
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+/**
+ * Represents the state of one swerve module.
+ */
+struct SwerveModuleState {
+ /**
+ * Speed of the wheel of the module.
+ */
+ units::meters_per_second_t speed = 0_mps;
+
+ /**
+ * Angle of the module.
+ */
+ Rotation2d angle;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h b/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
index 465a4bb..c7e0ec2 100644
--- a/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
+++ b/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2012-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2012-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,13 +9,10 @@
#include <memory>
-#include <wpi/Twine.h>
-#include <wpi/deprecated.h>
-
-#include "frc/smartdashboard/Sendable.h"
-
namespace frc {
+class Sendable;
+
/**
* The LiveWindow class is the public interface for putting sensors and
* actuators on the LiveWindow.
@@ -33,140 +30,6 @@
*/
static LiveWindow* GetInstance();
- WPI_DEPRECATED("no longer required")
- void Run();
-
- /**
- * Add a Sensor associated with the subsystem and call it by the given name.
- *
- * @param subsystem The subsystem this component is part of.
- * @param name The name of this component.
- * @param component A Sendable component that represents a sensor.
- */
- WPI_DEPRECATED("use Sendable::SetName() instead")
- void AddSensor(const wpi::Twine& subsystem, const wpi::Twine& name,
- Sendable* component);
-
- /**
- * Add a Sensor associated with the subsystem and call it by the given name.
- *
- * @param subsystem The subsystem this component is part of.
- * @param name The name of this component.
- * @param component A Sendable component that represents a sensor.
- */
- WPI_DEPRECATED("use Sendable::SetName() instead")
- void AddSensor(const wpi::Twine& subsystem, const wpi::Twine& name,
- Sendable& component);
-
- /**
- * Add a Sensor associated with the subsystem and call it by the given name.
- *
- * @param subsystem The subsystem this component is part of.
- * @param name The name of this component.
- * @param component A Sendable component that represents a sensor.
- */
- WPI_DEPRECATED("use Sendable::SetName() instead")
- void AddSensor(const wpi::Twine& subsystem, const wpi::Twine& name,
- std::shared_ptr<Sendable> component);
-
- /**
- * Add an Actuator associated with the subsystem and call it by the given
- * name.
- *
- * @param subsystem The subsystem this component is part of.
- * @param name The name of this component.
- * @param component A Sendable component that represents a actuator.
- */
- WPI_DEPRECATED("use Sendable::SetName() instead")
- void AddActuator(const wpi::Twine& subsystem, const wpi::Twine& name,
- Sendable* component);
-
- /**
- * Add an Actuator associated with the subsystem and call it by the given
- * name.
- *
- * @param subsystem The subsystem this component is part of.
- * @param name The name of this component.
- * @param component A Sendable component that represents a actuator.
- */
- WPI_DEPRECATED("use Sendable::SetName() instead")
- void AddActuator(const wpi::Twine& subsystem, const wpi::Twine& name,
- Sendable& component);
-
- /**
- * Add an Actuator associated with the subsystem and call it by the given
- * name.
- *
- * @param subsystem The subsystem this component is part of.
- * @param name The name of this component.
- * @param component A Sendable component that represents a actuator.
- */
- WPI_DEPRECATED("use Sendable::SetName() instead")
- void AddActuator(const wpi::Twine& subsystem, const wpi::Twine& name,
- std::shared_ptr<Sendable> component);
-
- /**
- * Meant for internal use in other WPILib classes.
- *
- * @deprecated Use SendableBase::SetName() instead.
- */
- WPI_DEPRECATED("use SensorUtil::SetName() instead")
- void AddSensor(const wpi::Twine& type, int channel, Sendable* component);
-
- /**
- * Meant for internal use in other WPILib classes.
- *
- * @deprecated Use SendableBase::SetName() instead.
- */
- WPI_DEPRECATED("use SensorUtil::SetName() instead")
- void AddActuator(const wpi::Twine& type, int channel, Sendable* component);
-
- /**
- * Meant for internal use in other WPILib classes.
- *
- * @deprecated Use SendableBase::SetName() instead.
- */
- WPI_DEPRECATED("use SensorUtil::SetName() instead")
- void AddActuator(const wpi::Twine& type, int module, int channel,
- Sendable* component);
-
- /**
- * Add a component to the LiveWindow.
- *
- * @param sendable component to add
- */
- void Add(std::shared_ptr<Sendable> component);
-
- /**
- * Add a component to the LiveWindow.
- *
- * @param sendable component to add
- */
- void Add(Sendable* component);
-
- /**
- * Add a child component to a component.
- *
- * @param parent parent component
- * @param child child component
- */
- void AddChild(Sendable* parent, std::shared_ptr<Sendable> component);
-
- /**
- * Add a child component to a component.
- *
- * @param parent parent component
- * @param child child component
- */
- void AddChild(Sendable* parent, void* component);
-
- /**
- * Remove the component from the LiveWindow.
- *
- * @param sendable component to remove
- */
- void Remove(Sendable* component);
-
/**
* Enable telemetry for a single component.
*
diff --git a/wpilibc/src/main/native/include/frc/livewindow/LiveWindowSendable.h b/wpilibc/src/main/native/include/frc/livewindow/LiveWindowSendable.h
deleted file mode 100644
index 172e0cb..0000000
--- a/wpilibc/src/main/native/include/frc/livewindow/LiveWindowSendable.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2012-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <string>
-
-#include <wpi/deprecated.h>
-
-#include "frc/smartdashboard/Sendable.h"
-
-namespace frc {
-
-/**
- * Live Window Sendable is a special type of object sendable to the live window.
- * @deprecated Use Sendable directly instead
- */
-class LiveWindowSendable : public Sendable {
- public:
- WPI_DEPRECATED("use Sendable directly instead")
- LiveWindowSendable() = default;
- LiveWindowSendable(LiveWindowSendable&&) = default;
- LiveWindowSendable& operator=(LiveWindowSendable&&) = default;
-
- /**
- * Update the table for this sendable object with the latest values.
- */
- virtual void UpdateTable() = 0;
-
- /**
- * Start having this sendable object automatically respond to value changes
- * reflect the value on the table.
- */
- virtual void StartLiveWindowMode() = 0;
-
- /**
- * Stop having this sendable object automatically respond to value changes.
- */
- virtual void StopLiveWindowMode() = 0;
-
- std::string GetName() const override;
- void SetName(const wpi::Twine& name) override;
- std::string GetSubsystem() const override;
- void SetSubsystem(const wpi::Twine& subsystem) override;
- void InitSendable(SendableBuilder& builder) override;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInLayouts.h b/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInLayouts.h
index c958baa..1d1ea5f 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInLayouts.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInLayouts.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h b/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
index bdd2011..8e666b7 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/LayoutType.h b/wpilibc/src/main/native/include/frc/shuffleboard/LayoutType.h
index 50e448b..00a5e36 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/LayoutType.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/LayoutType.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h b/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
index 291e64d..5610cf8 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,22 +7,39 @@
#pragma once
+#include <functional>
+#include <memory>
#include <string>
-#include <cscore_c.h>
-
-#include "frc/smartdashboard/SendableBase.h"
-
+#ifndef DYNAMIC_CAMERA_SERVER
+#include <cscore_oo.h>
+#else
namespace cs {
class VideoSource;
} // namespace cs
+typedef int CS_Handle;
+typedef CS_Handle CS_Source;
+#endif
+
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
+class SendableCameraWrapper;
+
+namespace detail {
+constexpr const char* kProtocol = "camera_server://";
+std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
+ CS_Source source);
+void AddToSendableRegistry(Sendable* sendable, std::string name);
+} // namespace detail
+
/**
* A wrapper to make video sources sendable and usable from Shuffleboard.
*/
-class SendableCameraWrapper : public SendableBase {
+class SendableCameraWrapper : public Sendable,
+ public SendableHelper<SendableCameraWrapper> {
private:
struct private_init {};
@@ -52,4 +69,27 @@
std::string m_uri;
};
+#ifndef DYNAMIC_CAMERA_SERVER
+inline SendableCameraWrapper::SendableCameraWrapper(CS_Source source,
+ const private_init&)
+ : m_uri(detail::kProtocol) {
+ CS_Status status = 0;
+ auto name = cs::GetSourceName(source, &status);
+ detail::AddToSendableRegistry(this, name);
+ m_uri += name;
+}
+
+inline SendableCameraWrapper& SendableCameraWrapper::Wrap(
+ const cs::VideoSource& source) {
+ return Wrap(source.GetHandle());
+}
+
+inline SendableCameraWrapper& SendableCameraWrapper::Wrap(CS_Source source) {
+ auto& wrapper = detail::GetSendableCameraWrapper(source);
+ if (!wrapper)
+ wrapper = std::make_shared<SendableCameraWrapper>(source, private_init{});
+ return *wrapper;
+}
+#endif
+
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
index ab03099..7b8b49d 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,7 @@
#pragma once
+#include <functional>
#include <memory>
#include <string>
#include <vector>
@@ -24,6 +25,7 @@
#include "frc/shuffleboard/LayoutType.h"
#include "frc/shuffleboard/ShuffleboardComponentBase.h"
#include "frc/shuffleboard/ShuffleboardValue.h"
+#include "frc/shuffleboard/SuppliedValueWidget.h"
namespace cs {
class VideoSource;
@@ -263,6 +265,98 @@
wpi::ArrayRef<std::string> defaultValue);
/**
+ * Adds a widget to this container. The widget will display the data provided
+ * by the value supplier. Changes made on the dashboard will not propagate to
+ * the widget object, and will be overridden by values from the value
+ * supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ */
+ SuppliedValueWidget<std::string>& AddString(
+ const wpi::Twine& title, std::function<std::string()> supplier);
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided
+ * by the value supplier. Changes made on the dashboard will not propagate to
+ * the widget object, and will be overridden by values from the value
+ * supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ */
+ SuppliedValueWidget<double>& AddNumber(const wpi::Twine& title,
+ std::function<double()> supplier);
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided
+ * by the value supplier. Changes made on the dashboard will not propagate to
+ * the widget object, and will be overridden by values from the value
+ * supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ */
+ SuppliedValueWidget<bool>& AddBoolean(const wpi::Twine& title,
+ std::function<bool()> supplier);
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided
+ * by the value supplier. Changes made on the dashboard will not propagate to
+ * the widget object, and will be overridden by values from the value
+ * supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ */
+ SuppliedValueWidget<std::vector<std::string>>& AddStringArray(
+ const wpi::Twine& title,
+ std::function<std::vector<std::string>()> supplier);
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided
+ * by the value supplier. Changes made on the dashboard will not propagate to
+ * the widget object, and will be overridden by values from the value
+ * supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ */
+ SuppliedValueWidget<std::vector<double>>& AddNumberArray(
+ const wpi::Twine& title, std::function<std::vector<double>()> supplier);
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided
+ * by the value supplier. Changes made on the dashboard will not propagate to
+ * the widget object, and will be overridden by values from the value
+ * supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ */
+ SuppliedValueWidget<std::vector<int>>& AddBooleanArray(
+ const wpi::Twine& title, std::function<std::vector<int>()> supplier);
+
+ /**
+ * Adds a widget to this container. The widget will display the data provided
+ * by the value supplier. Changes made on the dashboard will not propagate to
+ * the widget object, and will be overridden by values from the value
+ * supplier.
+ *
+ * @param title the title of the widget
+ * @param valueSupplier the supplier for values
+ * @return a widget to display data
+ */
+ SuppliedValueWidget<wpi::StringRef>& AddRaw(
+ const wpi::Twine& title, std::function<wpi::StringRef()> supplier);
+
+ /**
* Adds a widget to this container to display a simple piece of data.
*
* Unlike {@link #add(String, Object)}, the value in the widget will be saved
@@ -407,3 +501,17 @@
#include "frc/shuffleboard/ComplexWidget.h"
#include "frc/shuffleboard/ShuffleboardLayout.h"
#include "frc/shuffleboard/SimpleWidget.h"
+
+#ifndef DYNAMIC_CAMERA_SERVER
+#include "frc/shuffleboard/SendableCameraWrapper.h"
+
+inline frc::ComplexWidget& frc::ShuffleboardContainer::Add(
+ const cs::VideoSource& video) {
+ return Add(frc::SendableCameraWrapper::Wrap(video));
+}
+
+inline frc::ComplexWidget& frc::ShuffleboardContainer::Add(
+ const wpi::Twine& title, const cs::VideoSource& video) {
+ return Add(title, frc::SendableCameraWrapper::Wrap(video));
+}
+#endif
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardLayout.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardLayout.h
index 0b5d459..effa3da 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardLayout.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardLayout.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,7 +14,11 @@
#include "frc/shuffleboard/ShuffleboardComponent.h"
#include "frc/shuffleboard/ShuffleboardContainer.h"
-#include "frc/smartdashboard/Sendable.h"
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4250)
+#endif
namespace frc {
@@ -33,3 +37,7 @@
};
} // namespace frc
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardTab.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardTab.h
index 16e6f92..75896ae 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardTab.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardTab.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,7 +13,6 @@
#include <wpi/StringRef.h>
#include "frc/shuffleboard/ShuffleboardContainer.h"
-#include "frc/smartdashboard/Sendable.h"
namespace frc {
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
index 3b1b0a8..ea92b93 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/SuppliedValueWidget.h b/wpilibc/src/main/native/include/frc/shuffleboard/SuppliedValueWidget.h
new file mode 100644
index 0000000..c806db4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/SuppliedValueWidget.h
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <memory>
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/Twine.h>
+
+#include "frc/shuffleboard/ShuffleboardComponent.h"
+#include "frc/shuffleboard/ShuffleboardComponent.inc"
+#include "frc/shuffleboard/ShuffleboardComponentBase.h"
+#include "frc/shuffleboard/ShuffleboardContainer.h"
+#include "frc/shuffleboard/ShuffleboardWidget.h"
+
+namespace frc {
+template <typename T>
+class SuppliedValueWidget : public ShuffleboardWidget<SuppliedValueWidget<T> > {
+ public:
+ SuppliedValueWidget(ShuffleboardContainer& parent, const wpi::Twine& title,
+ std::function<T()> supplier,
+ std::function<void(nt::NetworkTableEntry, T)> setter)
+ : ShuffleboardValue(title),
+ ShuffleboardWidget<SuppliedValueWidget<T> >(parent, title),
+ m_supplier(supplier),
+ m_setter(setter) {}
+
+ void BuildInto(std::shared_ptr<nt::NetworkTable> parentTable,
+ std::shared_ptr<nt::NetworkTable> metaTable) override {
+ this->BuildMetadata(metaTable);
+ metaTable->GetEntry("Controllable").SetBoolean(false);
+
+ auto entry = parentTable->GetEntry(this->GetTitle());
+ m_setter(entry, m_supplier());
+ }
+
+ private:
+ std::function<T()> m_supplier;
+ std::function<void(nt::NetworkTableEntry, T)> m_setter;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/WidgetType.h b/wpilibc/src/main/native/include/frc/shuffleboard/WidgetType.h
index 057d594..ff92fe7 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/WidgetType.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/WidgetType.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/ListenerExecutor.h b/wpilibc/src/main/native/include/frc/smartdashboard/ListenerExecutor.h
new file mode 100644
index 0000000..9500278
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/ListenerExecutor.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include <wpi/mutex.h>
+
+namespace frc::detail {
+/**
+ * An executor for running listener tasks posted by Sendable listeners
+ * synchronously from the main application thread.
+ *
+ * @see Sendable
+ */
+class ListenerExecutor {
+ public:
+ /**
+ * Posts a task to the executor to be run synchronously from the main thread.
+ *
+ * @param task The task to run synchronously from the main thread.
+ */
+ void Execute(std::function<void()> task);
+
+ /**
+ * Runs all posted tasks. Called periodically from main thread.
+ */
+ void RunListenerTasks();
+
+ private:
+ std::vector<std::function<void()>> m_tasks;
+ std::vector<std::function<void()>> m_runningTasks;
+ wpi::mutex m_lock;
+};
+} // namespace frc::detail
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/NamedSendable.h b/wpilibc/src/main/native/include/frc/smartdashboard/NamedSendable.h
deleted file mode 100644
index 604d2e0..0000000
--- a/wpilibc/src/main/native/include/frc/smartdashboard/NamedSendable.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2012-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <string>
-
-#include <wpi/deprecated.h>
-
-#include "frc/smartdashboard/Sendable.h"
-
-namespace frc {
-
-/**
- * The interface for sendable objects that gives the sendable a default name in
- * the Smart Dashboard.
- * @deprecated Use Sendable directly instead
- */
-class NamedSendable : public Sendable {
- public:
- WPI_DEPRECATED("use Sendable directly instead")
- NamedSendable() = default;
-
- void SetName(const wpi::Twine& name) override;
- std::string GetSubsystem() const override;
- void SetSubsystem(const wpi::Twine& subsystem) override;
- void InitSendable(SendableBuilder& builder) override;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h b/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h
index 383c169..5000855 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,61 +7,17 @@
#pragma once
-#include <string>
-
-#include <wpi/Twine.h>
-
namespace frc {
class SendableBuilder;
+/**
+ * Interface for Sendable objects.
+ */
class Sendable {
public:
- Sendable() = default;
virtual ~Sendable() = default;
- Sendable(Sendable&&) = default;
- Sendable& operator=(Sendable&&) = default;
-
- /**
- * Gets the name of this Sendable object.
- *
- * @return Name
- */
- virtual std::string GetName() const = 0;
-
- /**
- * Sets the name of this Sendable object.
- *
- * @param name name
- */
- virtual void SetName(const wpi::Twine& name) = 0;
-
- /**
- * Sets both the subsystem name and device name of this Sendable object.
- *
- * @param subsystem subsystem name
- * @param name device name
- */
- void SetName(const wpi::Twine& subsystem, const wpi::Twine& name) {
- SetSubsystem(subsystem);
- SetName(name);
- }
-
- /**
- * Gets the subsystem name of this Sendable object.
- *
- * @return Subsystem name
- */
- virtual std::string GetSubsystem() const = 0;
-
- /**
- * Sets the subsystem name of this Sendable object.
- *
- * @param subsystem subsystem name
- */
- virtual void SetSubsystem(const wpi::Twine& subsystem) = 0;
-
/**
* Initializes this Sendable object.
*
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
index 6d2fbbe..a0ea0f9 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,75 +7,22 @@
#pragma once
-#include <memory>
-#include <string>
-
-#include <wpi/mutex.h>
+#include <wpi/deprecated.h>
#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
-class SendableBase : public Sendable {
+class SendableBase : public Sendable, public SendableHelper<SendableBase> {
public:
/**
* Creates an instance of the sensor base.
*
* @param addLiveWindow if true, add this Sendable to LiveWindow
*/
+ WPI_DEPRECATED("use Sendable and SendableHelper")
explicit SendableBase(bool addLiveWindow = true);
-
- ~SendableBase() override;
-
- SendableBase(SendableBase&& rhs);
- SendableBase& operator=(SendableBase&& rhs);
-
- using Sendable::SetName;
-
- std::string GetName() const final;
- void SetName(const wpi::Twine& name) final;
- std::string GetSubsystem() const final;
- void SetSubsystem(const wpi::Twine& subsystem) final;
-
- protected:
- /**
- * Add a child component.
- *
- * @param child child component
- */
- void AddChild(std::shared_ptr<Sendable> child);
-
- /**
- * Add a child component.
- *
- * @param child child component
- */
- void AddChild(void* child);
-
- /**
- * Sets the name of the sensor with a channel number.
- *
- * @param moduleType A string that defines the module name in the label for
- * the value
- * @param channel The channel number the device is plugged into
- */
- void SetName(const wpi::Twine& moduleType, int channel);
-
- /**
- * Sets the name of the sensor with a module and channel number.
- *
- * @param moduleType A string that defines the module name in the label for
- * the value
- * @param moduleNumber The number of the particular module type
- * @param channel The channel number the device is plugged into (usually
- * PWM)
- */
- void SetName(const wpi::Twine& moduleType, int moduleNumber, int channel);
-
- private:
- mutable wpi::mutex m_mutex;
- std::string m_name;
- std::string m_subsystem = "Ungrouped";
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
index e10f085..eb69dcd 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -46,6 +46,12 @@
std::shared_ptr<nt::NetworkTable> GetTable();
/**
+ * Return whether this sendable has an associated table.
+ * @return True if it has a table, false if not.
+ */
+ bool HasTable() const;
+
+ /**
* Return whether this sendable should be treated as an actuator.
* @return True if actuator, false if not.
*/
@@ -78,6 +84,11 @@
*/
void StopLiveWindowMode();
+ /**
+ * Clear properties.
+ */
+ void ClearProperties();
+
void SetSmartDashboardType(const wpi::Twine& type) override;
void SetActuator(bool value) override;
void SetSafeState(std::function<void()> func) override;
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
index 42b4a65..295d263 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -62,7 +62,7 @@
-> decltype(_unwrap_smart_ptr(m_choices[""])) {
std::string selected = m_defaultChoice;
{
- std::lock_guard<wpi::mutex> lock(m_mutex);
+ std::scoped_lock lock(m_mutex);
if (m_haveSelected) selected = m_selected;
}
if (selected.empty()) {
@@ -99,7 +99,7 @@
builder.AddSmallStringProperty(
kActive,
[=](wpi::SmallVectorImpl<char>& buf) -> wpi::StringRef {
- std::lock_guard<wpi::mutex> lock(m_mutex);
+ std::scoped_lock lock(m_mutex);
if (m_haveSelected) {
buf.assign(m_selected.begin(), m_selected.end());
return wpi::StringRef(buf.data(), buf.size());
@@ -109,11 +109,11 @@
},
nullptr);
{
- std::lock_guard<wpi::mutex> lock(m_mutex);
+ std::scoped_lock lock(m_mutex);
m_activeEntries.emplace_back(builder.GetEntry(kActive));
}
builder.AddStringProperty(kSelected, nullptr, [=](wpi::StringRef val) {
- std::lock_guard<wpi::mutex> lock(m_mutex);
+ std::scoped_lock lock(m_mutex);
m_haveSelected = true;
m_selected = val;
for (auto& entry : m_activeEntries) entry.SetString(val);
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h
index ae7908e..2a5f5ab 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,7 +14,8 @@
#include <wpi/SmallVector.h>
#include <wpi/mutex.h>
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
@@ -24,13 +25,14 @@
* It contains static, non-templated variables to avoid their duplication in the
* template class.
*/
-class SendableChooserBase : public SendableBase {
+class SendableChooserBase : public Sendable,
+ public SendableHelper<SendableChooserBase> {
public:
SendableChooserBase();
~SendableChooserBase() override = default;
- SendableChooserBase(SendableChooserBase&&) = default;
- SendableChooserBase& operator=(SendableChooserBase&&) = default;
+ SendableChooserBase(SendableChooserBase&& oth);
+ SendableChooserBase& operator=(SendableChooserBase&& oth);
protected:
static constexpr const char* kDefault = "default";
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h
new file mode 100644
index 0000000..6b0b28b
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h
@@ -0,0 +1,161 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <wpi/Twine.h>
+#include <wpi/deprecated.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+namespace frc {
+
+/**
+ * A helper class for use with objects that add themselves to SendableRegistry.
+ * It takes care of properly calling Move() and Remove() on move and
+ * destruction. No action is taken if the object is copied.
+ * Use public inheritance with CRTP when using this class.
+ * @tparam CRTP derived class
+ */
+template <typename Derived>
+class SendableHelper {
+ public:
+ SendableHelper(const SendableHelper& rhs) = default;
+ SendableHelper& operator=(const SendableHelper& rhs) = default;
+
+ SendableHelper(SendableHelper&& rhs) {
+ // it is safe to call Move() multiple times with the same rhs
+ SendableRegistry::GetInstance().Move(static_cast<Derived*>(this),
+ static_cast<Derived*>(&rhs));
+ }
+
+ SendableHelper& operator=(SendableHelper&& rhs) {
+ // it is safe to call Move() multiple times with the same rhs
+ SendableRegistry::GetInstance().Move(static_cast<Derived*>(this),
+ static_cast<Derived*>(&rhs));
+ return *this;
+ }
+
+ /**
+ * Gets the name of this Sendable object.
+ *
+ * @return Name
+ */
+ WPI_DEPRECATED("use SendableRegistry::GetName()")
+ std::string GetName() const {
+ return SendableRegistry::GetInstance().GetName(
+ static_cast<const Derived*>(this));
+ }
+
+ /**
+ * Sets the name of this Sendable object.
+ *
+ * @param name name
+ */
+ WPI_DEPRECATED("use SendableRegistry::SetName()")
+ void SetName(const wpi::Twine& name) {
+ SendableRegistry::GetInstance().SetName(static_cast<Derived*>(this), name);
+ }
+
+ /**
+ * Sets both the subsystem name and device name of this Sendable object.
+ *
+ * @param subsystem subsystem name
+ * @param name device name
+ */
+ WPI_DEPRECATED("use SendableRegistry::SetName()")
+ void SetName(const wpi::Twine& subsystem, const wpi::Twine& name) {
+ SendableRegistry::GetInstance().SetName(static_cast<Derived*>(this),
+ subsystem, name);
+ }
+
+ /**
+ * Gets the subsystem name of this Sendable object.
+ *
+ * @return Subsystem name
+ */
+ WPI_DEPRECATED("use SendableRegistry::GetSubsystem()")
+ std::string GetSubsystem() const {
+ return SendableRegistry::GetInstance().GetSubsystem(
+ static_cast<const Derived*>(this));
+ }
+
+ /**
+ * Sets the subsystem name of this Sendable object.
+ *
+ * @param subsystem subsystem name
+ */
+ WPI_DEPRECATED("use SendableRegistry::SetSubsystem()")
+ void SetSubsystem(const wpi::Twine& subsystem) {
+ SendableRegistry::GetInstance().SetSubsystem(static_cast<Derived*>(this),
+ subsystem);
+ }
+
+ protected:
+ /**
+ * Add a child component.
+ *
+ * @param child child component
+ */
+ WPI_DEPRECATED("use SendableRegistry::AddChild()")
+ void AddChild(std::shared_ptr<Sendable> child) {
+ SendableRegistry::GetInstance().AddChild(static_cast<Derived*>(this),
+ child.get());
+ }
+
+ /**
+ * Add a child component.
+ *
+ * @param child child component
+ */
+ WPI_DEPRECATED("use SendableRegistry::AddChild()")
+ void AddChild(void* child) {
+ SendableRegistry::GetInstance().AddChild(static_cast<Derived*>(this),
+ child);
+ }
+
+ /**
+ * Sets the name of the sensor with a channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param channel The channel number the device is plugged into
+ */
+ WPI_DEPRECATED("use SendableRegistry::SetName()")
+ void SetName(const wpi::Twine& moduleType, int channel) {
+ SendableRegistry::GetInstance().SetName(static_cast<Derived*>(this),
+ moduleType, channel);
+ }
+
+ /**
+ * Sets the name of the sensor with a module and channel number.
+ *
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into (usually
+ * PWM)
+ */
+ WPI_DEPRECATED("use SendableRegistry::SetName()")
+ void SetName(const wpi::Twine& moduleType, int moduleNumber, int channel) {
+ SendableRegistry::GetInstance().SetName(static_cast<Derived*>(this),
+ moduleType, moduleNumber, channel);
+ }
+
+ protected:
+ SendableHelper() = default;
+
+ ~SendableHelper() {
+ // it is safe to call Remove() multiple times with the same object
+ SendableRegistry::GetInstance().Remove(static_cast<Derived*>(this));
+ }
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h
new file mode 100644
index 0000000..53dbba3
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h
@@ -0,0 +1,332 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <networktables/NetworkTable.h>
+#include <wpi/STLExtras.h>
+#include <wpi/Twine.h>
+
+namespace frc {
+
+class Sendable;
+class SendableBuilderImpl;
+
+/**
+ * The SendableRegistry class is the public interface for registering sensors
+ * and actuators for use on dashboards and LiveWindow.
+ */
+class SendableRegistry {
+ public:
+ SendableRegistry(const SendableRegistry&) = delete;
+ SendableRegistry& operator=(const SendableRegistry&) = delete;
+
+ using UID = size_t;
+
+ /**
+ * Gets an instance of the SendableRegistry class.
+ *
+ * This is a singleton to guarantee that there is only a single instance
+ * regardless of how many times GetInstance is called.
+ */
+ static SendableRegistry& GetInstance();
+
+ /**
+ * Adds an object to the registry.
+ *
+ * @param sendable object to add
+ * @param name component name
+ */
+ void Add(Sendable* sendable, const wpi::Twine& name);
+
+ /**
+ * Adds an object to the registry.
+ *
+ * @param sendable object to add
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param channel The channel number the device is plugged into
+ */
+ void Add(Sendable* sendable, const wpi::Twine& moduleType, int channel);
+
+ /**
+ * Adds an object to the registry.
+ *
+ * @param sendable object to add
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into
+ */
+ void Add(Sendable* sendable, const wpi::Twine& moduleType, int moduleNumber,
+ int channel);
+
+ /**
+ * Adds an object to the registry.
+ *
+ * @param sendable object to add
+ * @param subsystem subsystem name
+ * @param name component name
+ */
+ void Add(Sendable* sendable, const wpi::Twine& subsystem,
+ const wpi::Twine& name);
+
+ /**
+ * Adds an object to the registry and LiveWindow.
+ *
+ * @param sendable object to add
+ * @param name component name
+ */
+ void AddLW(Sendable* sendable, const wpi::Twine& name);
+
+ /**
+ * Adds an object to the registry and LiveWindow.
+ *
+ * @param sendable object to add
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param channel The channel number the device is plugged into
+ */
+ void AddLW(Sendable* sendable, const wpi::Twine& moduleType, int channel);
+
+ /**
+ * Adds an object to the registry and LiveWindow.
+ *
+ * @param sendable object to add
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into
+ */
+ void AddLW(Sendable* sendable, const wpi::Twine& moduleType, int moduleNumber,
+ int channel);
+
+ /**
+ * Adds an object to the registry and LiveWindow.
+ *
+ * @param sendable object to add
+ * @param subsystem subsystem name
+ * @param name component name
+ */
+ void AddLW(Sendable* sendable, const wpi::Twine& subsystem,
+ const wpi::Twine& name);
+
+ /**
+ * Adds a child object to an object. Adds the child object to the registry
+ * if it's not already present.
+ *
+ * @param parent parent object
+ * @param child child object
+ */
+ void AddChild(Sendable* parent, void* child);
+
+ /**
+ * Removes an object from the registry.
+ *
+ * @param sendable object to remove
+ * @return true if the object was removed; false if it was not present
+ */
+ bool Remove(Sendable* sendable);
+
+ /**
+ * Moves an object in the registry (for use in move constructors/assignments).
+ *
+ * @param to new object
+ * @param from old object
+ */
+ void Move(Sendable* to, Sendable* from);
+
+ /**
+ * Determines if an object is in the registry.
+ *
+ * @param sendable object to check
+ * @return True if in registry, false if not.
+ */
+ bool Contains(const Sendable* sendable) const;
+
+ /**
+ * Gets the name of an object.
+ *
+ * @param sendable object
+ * @return Name (empty if object is not in registry)
+ */
+ std::string GetName(const Sendable* sendable) const;
+
+ /**
+ * Sets the name of an object.
+ *
+ * @param sendable object
+ * @param name name
+ */
+ void SetName(Sendable* sendable, const wpi::Twine& name);
+
+ /**
+ * Sets the name of an object with a channel number.
+ *
+ * @param sendable object
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param channel The channel number the device is plugged into
+ */
+ void SetName(Sendable* sendable, const wpi::Twine& moduleType, int channel);
+
+ /**
+ * Sets the name of an object with a module and channel number.
+ *
+ * @param sendable object
+ * @param moduleType A string that defines the module name in the label for
+ * the value
+ * @param moduleNumber The number of the particular module type
+ * @param channel The channel number the device is plugged into
+ */
+ void SetName(Sendable* sendable, const wpi::Twine& moduleType,
+ int moduleNumber, int channel);
+
+ /**
+ * Sets both the subsystem name and device name of an object.
+ *
+ * @param sendable object
+ * @param subsystem subsystem name
+ * @param name device name
+ */
+ void SetName(Sendable* sendable, const wpi::Twine& subsystem,
+ const wpi::Twine& name);
+
+ /**
+ * Gets the subsystem name of an object.
+ *
+ * @param sendable object
+ * @return Subsystem name (empty if object is not in registry)
+ */
+ std::string GetSubsystem(const Sendable* sendable) const;
+
+ /**
+ * Sets the subsystem name of an object.
+ *
+ * @param sendable object
+ * @param subsystem subsystem name
+ */
+ void SetSubsystem(Sendable* sendable, const wpi::Twine& subsystem);
+
+ /**
+ * Gets a unique handle for setting/getting data with SetData() and GetData().
+ *
+ * @return Handle
+ */
+ int GetDataHandle();
+
+ /**
+ * Associates arbitrary data with an object in the registry.
+ *
+ * @param sendable object
+ * @param handle data handle returned by GetDataHandle()
+ * @param data data to set
+ * @return Previous data (may be null)
+ */
+ std::shared_ptr<void> SetData(Sendable* sendable, int handle,
+ std::shared_ptr<void> data);
+
+ /**
+ * Gets arbitrary data associated with an object in the registry.
+ *
+ * @param sendable object
+ * @param handle data handle returned by GetDataHandle()
+ * @return data (may be null if none associated)
+ */
+ std::shared_ptr<void> GetData(Sendable* sendable, int handle);
+
+ /**
+ * Enables LiveWindow for an object.
+ *
+ * @param sendable object
+ */
+ void EnableLiveWindow(Sendable* sendable);
+
+ /**
+ * Disables LiveWindow for an object.
+ *
+ * @param sendable object
+ */
+ void DisableLiveWindow(Sendable* sendable);
+
+ /**
+ * Get unique id for an object. Since objects can move, use this instead
+ * of storing Sendable* directly if ownership is in question.
+ *
+ * @param sendable object
+ * @return unique id
+ */
+ UID GetUniqueId(Sendable* sendable);
+
+ /**
+ * Get sendable object for a given unique id.
+ *
+ * @param uid unique id
+ * @return sendable object (may be null)
+ */
+ Sendable* GetSendable(UID uid);
+
+ /**
+ * Publishes an object in the registry to a network table.
+ *
+ * @param sendableUid sendable unique id
+ * @param table network table
+ */
+ void Publish(UID sendableUid, std::shared_ptr<NetworkTable> table);
+
+ /**
+ * Updates network table information from an object.
+ *
+ * @param sendableUid sendable unique id
+ */
+ void Update(UID sendableUid);
+
+ /**
+ * Data passed to ForeachLiveWindow() callback function
+ */
+ struct CallbackData {
+ CallbackData(Sendable* sendable_, wpi::StringRef name_,
+ wpi::StringRef subsystem_, Sendable* parent_,
+ std::shared_ptr<void>& data_, SendableBuilderImpl& builder_)
+ : sendable(sendable_),
+ name(name_),
+ subsystem(subsystem_),
+ parent(parent_),
+ data(data_),
+ builder(builder_) {}
+
+ Sendable* sendable;
+ wpi::StringRef name;
+ wpi::StringRef subsystem;
+ Sendable* parent;
+ std::shared_ptr<void>& data;
+ SendableBuilderImpl& builder;
+ };
+
+ /**
+ * Iterates over LiveWindow-enabled objects in the registry.
+ * It is *not* safe to call other SendableRegistry functions from the
+ * callback (this will likely deadlock).
+ *
+ * @param dataHandle data handle to get data pointer passed to callback
+ * @param callback function to call for each object
+ */
+ void ForeachLiveWindow(
+ int dataHandle,
+ wpi::function_ref<void(CallbackData& cbdata)> callback) const;
+
+ private:
+ SendableRegistry();
+
+ struct Impl;
+ std::unique_ptr<Impl> m_impl;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h b/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
index 903fb8b..97805ba 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,13 +14,15 @@
#include <networktables/NetworkTableValue.h>
#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/ListenerExecutor.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
-class Sendable;
-
-class SmartDashboard : public ErrorBase, public SendableBase {
+class SmartDashboard : public ErrorBase,
+ public Sendable,
+ public SendableHelper<SmartDashboard> {
public:
static void init();
@@ -100,6 +102,9 @@
* The value can be retrieved by calling the get method with a key that is
* equal to the original key.
*
+ * In order for the value to appear in the dashboard, it must be registered
+ * with SendableRegistry. WPILib components do this automatically.
+ *
* @param keyName the key
* @param value the value
*/
@@ -112,6 +117,9 @@
* The value can be retrieved by calling the get method with a key that is
* equal to the original key.
*
+ * In order for the value to appear in the dashboard, it must be registered
+ * with SendableRegistry. WPILib components do this automatically.
+ *
* @param value the value
*/
static void PutData(Sendable* value);
@@ -402,12 +410,23 @@
static std::shared_ptr<nt::Value> GetValue(wpi::StringRef keyName);
/**
+ * Posts a task from a listener to the ListenerExecutor, so that it can be run
+ * synchronously from the main loop on the next call to {@link
+ * SmartDashboard#updateValues()}.
+ *
+ * @param task The task to run synchronously from the main thread.
+ */
+ static void PostListenerTask(std::function<void()> task);
+
+ /**
* Puts all sendable data to the dashboard.
*/
static void UpdateValues();
private:
virtual ~SmartDashboard() = default;
+
+ static detail::ListenerExecutor listenerExecutor;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
new file mode 100644
index 0000000..a8979c1
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include <Eigen/Core>
+
+#include "frc/spline/Spline.h"
+
+namespace frc {
+/**
+ * Represents a hermite spline of degree 3.
+ */
+class CubicHermiteSpline : public Spline<3> {
+ public:
+ /**
+ * Constructs a cubic hermite spline with the specified control vectors. Each
+ * control vector contains info about the location of the point and its first
+ * derivative.
+ *
+ * @param xInitialControlVector The control vector for the initial point in
+ * the x dimension.
+ * @param xFinalControlVector The control vector for the final point in
+ * the x dimension.
+ * @param yInitialControlVector The control vector for the initial point in
+ * the y dimension.
+ * @param yFinalControlVector The control vector for the final point in
+ * the y dimension.
+ */
+ CubicHermiteSpline(std::array<double, 2> xInitialControlVector,
+ std::array<double, 2> xFinalControlVector,
+ std::array<double, 2> yInitialControlVector,
+ std::array<double, 2> yFinalControlVector);
+
+ protected:
+ /**
+ * Returns the coefficients matrix.
+ * @return The coefficients matrix.
+ */
+ Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
+ return m_coefficients;
+ }
+
+ private:
+ Eigen::Matrix<double, 6, 4> m_coefficients;
+
+ /**
+ * Returns the hermite basis matrix for cubic hermite spline interpolation.
+ * @return The hermite basis matrix for cubic hermite spline interpolation.
+ */
+ static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
+ // clang-format off
+ static auto basis = (Eigen::Matrix<double, 4, 4>() <<
+ +2.0, +1.0, -2.0, +1.0,
+ -3.0, -2.0, +3.0, -1.0,
+ +0.0, +1.0, +0.0, +0.0,
+ +1.0, +0.0, +0.0, +0.0).finished();
+ // clang-format on
+ return basis;
+ }
+
+ /**
+ * Returns the control vector for each dimension as a matrix from the
+ * user-provided arrays in the constructor.
+ *
+ * @param initialVector The control vector for the initial point.
+ * @param finalVector The control vector for the final point.
+ *
+ * @return The control vector matrix for a dimension.
+ */
+ static Eigen::Vector4d ControlVectorFromArrays(
+ std::array<double, 2> initialVector, std::array<double, 2> finalVector) {
+ return (Eigen::Vector4d() << initialVector[0], initialVector[1],
+ finalVector[0], finalVector[1])
+ .finished();
+ }
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
new file mode 100644
index 0000000..730eab4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include <Eigen/Core>
+
+#include "frc/spline/Spline.h"
+
+namespace frc {
+/**
+ * Represents a hermite spline of degree 5.
+ */
+class QuinticHermiteSpline : public Spline<5> {
+ public:
+ /**
+ * Constructs a quintic hermite spline with the specified control vectors.
+ * Each control vector contains into about the location of the point, its
+ * first derivative, and its second derivative.
+ *
+ * @param xInitialControlVector The control vector for the initial point in
+ * the x dimension.
+ * @param xFinalControlVector The control vector for the final point in
+ * the x dimension.
+ * @param yInitialControlVector The control vector for the initial point in
+ * the y dimension.
+ * @param yFinalControlVector The control vector for the final point in
+ * the y dimension.
+ */
+ QuinticHermiteSpline(std::array<double, 3> xInitialControlVector,
+ std::array<double, 3> xFinalControlVector,
+ std::array<double, 3> yInitialControlVector,
+ std::array<double, 3> yFinalControlVector);
+
+ protected:
+ /**
+ * Returns the coefficients matrix.
+ * @return The coefficients matrix.
+ */
+ Eigen::Matrix<double, 6, 6> Coefficients() const override {
+ return m_coefficients;
+ }
+
+ private:
+ Eigen::Matrix<double, 6, 6> m_coefficients;
+
+ /**
+ * Returns the hermite basis matrix for quintic hermite spline interpolation.
+ * @return The hermite basis matrix for quintic hermite spline interpolation.
+ */
+ static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
+ // clang-format off
+ static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
+ -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+ +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+ -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+ +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+ +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
+ +01.0, +00.0, +00.0, +00.0, +00.0, +00.0).finished();
+ // clang-format on
+ return basis;
+ }
+
+ /**
+ * Returns the control vector for each dimension as a matrix from the
+ * user-provided arrays in the constructor.
+ *
+ * @param initialVector The control vector for the initial point.
+ * @param finalVector The control vector for the final point.
+ *
+ * @return The control vector matrix for a dimension.
+ */
+ static Eigen::Matrix<double, 6, 1> ControlVectorFromArrays(
+ std::array<double, 3> initialVector, std::array<double, 3> finalVector) {
+ return (Eigen::Matrix<double, 6, 1>() << initialVector[0], initialVector[1],
+ initialVector[2], finalVector[0], finalVector[1], finalVector[2])
+ .finished();
+ }
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/spline/Spline.h b/wpilibc/src/main/native/include/frc/spline/Spline.h
new file mode 100644
index 0000000..e8f2372
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/spline/Spline.h
@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "frc/geometry/Pose2d.h"
+
+namespace frc {
+
+/**
+ * Define a unit for curvature.
+ */
+using curvature_t = units::unit_t<
+ units::compound_unit<units::radian, units::inverse<units::meter>>>;
+
+/**
+ * Represents a two-dimensional parametric spline that interpolates between two
+ * points.
+ *
+ * @tparam Degree The degree of the spline.
+ */
+template <int Degree>
+class Spline {
+ public:
+ using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
+
+ Spline() = default;
+
+ Spline(const Spline&) = default;
+ Spline& operator=(const Spline&) = default;
+
+ Spline(Spline&&) = default;
+ Spline& operator=(Spline&&) = default;
+
+ virtual ~Spline() = default;
+
+ /**
+ * Represents a control vector for a spline.
+ *
+ * Each element in each array represents the value of the derivative at the
+ * index. For example, the value of x[2] is the second derivative in the x
+ * dimension.
+ */
+ struct ControlVector {
+ std::array<double, (Degree + 1) / 2> x;
+ std::array<double, (Degree + 1) / 2> y;
+ };
+
+ /**
+ * Gets the pose and curvature at some point t on the spline.
+ *
+ * @param t The point t
+ * @return The pose and curvature at that point.
+ */
+ PoseWithCurvature GetPoint(double t) const {
+ Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
+
+ // Populate the polynomial bases
+ for (int i = 0; i <= Degree; i++) {
+ polynomialBases(i) = std::pow(t, Degree - i);
+ }
+
+ // This simply multiplies by the coefficients. We need to divide out t some
+ // n number of times where n is the derivative we want to take.
+ Eigen::Matrix<double, 6, 1> combined = Coefficients() * polynomialBases;
+
+ double dx, dy, ddx, ddy;
+
+ // If t = 0, all other terms in the equation cancel out to zero. We can use
+ // the last x^0 term in the equation.
+ if (t == 0.0) {
+ dx = Coefficients()(2, Degree - 1);
+ dy = Coefficients()(3, Degree - 1);
+ ddx = Coefficients()(4, Degree - 2);
+ ddy = Coefficients()(5, Degree - 2);
+ } else {
+ // Divide out t for first derivative.
+ dx = combined(2) / t;
+ dy = combined(3) / t;
+
+ // Divide out t for second derivative.
+ ddx = combined(4) / t / t;
+ ddy = combined(5) / t / t;
+ }
+
+ // Find the curvature.
+ const auto curvature =
+ (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
+
+ return {
+ {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)},
+ curvature_t(curvature)};
+ }
+
+ protected:
+ /**
+ * Returns the coefficients of the spline.
+ *
+ * @return The coefficients of the spline.
+ */
+ virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
+
+ /**
+ * Converts a Translation2d into a vector that is compatible with Eigen.
+ *
+ * @param translation The Translation2d to convert.
+ * @return The vector.
+ */
+ static Eigen::Vector2d ToVector(const Translation2d& translation) {
+ return (Eigen::Vector2d() << translation.X().to<double>(),
+ translation.Y().to<double>())
+ .finished();
+ }
+
+ /**
+ * Converts an Eigen vector into a Translation2d.
+ *
+ * @param vector The vector to convert.
+ * @return The Translation2d.
+ */
+ static Translation2d FromVector(const Eigen::Vector2d& vector) {
+ return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));
+ }
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h b/wpilibc/src/main/native/include/frc/spline/SplineHelper.h
new file mode 100644
index 0000000..4ed1cf5
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/spline/SplineHelper.h
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+#include <utility>
+#include <vector>
+
+#include "frc/spline/CubicHermiteSpline.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+
+namespace frc {
+/**
+ * Helper class that is used to generate cubic and quintic splines from user
+ * provided waypoints.
+ */
+class SplineHelper {
+ public:
+ /**
+ * Returns 2 cubic control vectors from a set of exterior waypoints and
+ * interior translations.
+ *
+ * @param start The starting pose.
+ * @param interiorWaypoints The interior waypoints.
+ * @param end The ending pose.
+ * @return 2 cubic control vectors.
+ */
+ static std::array<Spline<3>::ControlVector, 2>
+ CubicControlVectorsFromWaypoints(
+ const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+ const Pose2d& end);
+
+ /**
+ * Returns quintic control vectors from a set of waypoints.
+ *
+ * @param waypoints The waypoints
+ * @return List of control vectors
+ */
+ static std::vector<Spline<5>::ControlVector>
+ QuinticControlVectorsFromWaypoints(const std::vector<Pose2d>& waypoints);
+
+ /**
+ * Returns a set of cubic splines corresponding to the provided control
+ * vectors. The user is free to set the direction of the start and end
+ * point. The directions for the middle waypoints are determined
+ * automatically to ensure continuous curvature throughout the path.
+ *
+ * The derivation for the algorithm used can be found here:
+ * <https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08/undervisningsmateriale/chap7alecture.pdf>
+ *
+ * @param start The starting control vector.
+ * @param waypoints The middle waypoints. This can be left blank if you
+ * only wish to create a path with two waypoints.
+ * @param end The ending control vector.
+ *
+ * @return A vector of cubic hermite splines that interpolate through the
+ * provided waypoints.
+ */
+ static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
+ const Spline<3>::ControlVector& start,
+ std::vector<Translation2d> waypoints,
+ const Spline<3>::ControlVector& end);
+
+ /**
+ * Returns a set of quintic splines corresponding to the provided control
+ * vectors. The user is free to set the direction of all waypoints. Continuous
+ * curvature is guaranteed throughout the path.
+ *
+ * @param controlVectors The control vectors.
+ * @return A vector of quintic hermite splines that interpolate through the
+ * provided waypoints.
+ */
+ static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
+ const std::vector<Spline<5>::ControlVector>& controlVectors);
+
+ private:
+ static Spline<3>::ControlVector CubicControlVector(double scalar,
+ const Pose2d& point) {
+ return {
+ {point.Translation().X().to<double>(), scalar * point.Rotation().Cos()},
+ {point.Translation().Y().to<double>(),
+ scalar * point.Rotation().Sin()}};
+ }
+
+ static Spline<5>::ControlVector QuinticControlVector(double scalar,
+ const Pose2d& point) {
+ return {{point.Translation().X().to<double>(),
+ scalar * point.Rotation().Cos(), 0.0},
+ {point.Translation().Y().to<double>(),
+ scalar * point.Rotation().Sin(), 0.0}};
+ }
+
+ /**
+ * Thomas algorithm for solving tridiagonal systems Af = d.
+ *
+ * @param a the values of A above the diagonal
+ * @param b the values of A on the diagonal
+ * @param c the values of A below the diagonal
+ * @param d the vector on the rhs
+ * @param solutionVector the unknown (solution) vector, modified in-place
+ */
+ static void ThomasAlgorithm(const std::vector<double>& a,
+ const std::vector<double>& b,
+ const std::vector<double>& c,
+ const std::vector<double>& d,
+ std::vector<double>* solutionVector);
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
new file mode 100644
index 0000000..34cf026
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <frc/spline/Spline.h>
+
+#include <utility>
+#include <vector>
+
+#include <units/units.h>
+
+namespace frc {
+
+/**
+ * Class used to parameterize a spline by its arc length.
+ */
+class SplineParameterizer {
+ public:
+ using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
+
+ /**
+ * Parameterizes the spline. This method breaks up the spline into various
+ * arcs until their dx, dy, and dtheta are within specific tolerances.
+ *
+ * @param spline The spline to parameterize.
+ * @param t0 Starting internal spline parameter. It is recommended to leave
+ * this as default.
+ * @param t1 Ending internal spline parameter. It is recommended to leave this
+ * as default.
+ *
+ * @return A vector of poses and curvatures that represents various points on
+ * the spline.
+ */
+ template <int Dim>
+ static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
+ double t0 = 0.0,
+ double t1 = 1.0) {
+ std::vector<PoseWithCurvature> arr;
+
+ // The parameterization does not add the first initial point. Let's add
+ // that.
+ arr.push_back(spline.GetPoint(t0));
+
+ GetSegmentArc(spline, &arr, t0, t1);
+ return arr;
+ }
+
+ private:
+ // Constraints for spline parameterization.
+ static constexpr units::meter_t kMaxDx = 5_in;
+ static constexpr units::meter_t kMaxDy = 0.05_in;
+ static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
+
+ /**
+ * Breaks up the spline into arcs until the dx, dy, and theta of each arc is
+ * within tolerance.
+ *
+ * @param spline The spline to parameterize.
+ * @param vector Pointer to vector of poses.
+ * @param t0 Starting point for arc.
+ * @param t1 Ending point for arc.
+ */
+ template <int Dim>
+ static void GetSegmentArc(const Spline<Dim>& spline,
+ std::vector<PoseWithCurvature>* vector, double t0,
+ double t1) {
+ const auto start = spline.GetPoint(t0);
+ const auto end = spline.GetPoint(t1);
+
+ const auto twist = start.first.Log(end.first);
+
+ if (units::math::abs(twist.dy) > kMaxDy ||
+ units::math::abs(twist.dx) > kMaxDx ||
+ units::math::abs(twist.dtheta) > kMaxDtheta) {
+ GetSegmentArc(spline, vector, t0, (t0 + t1) / 2);
+ GetSegmentArc(spline, vector, (t0 + t1) / 2, t1);
+ } else {
+ vector->push_back(spline.GetPoint(t1));
+ }
+ }
+
+ friend class CubicHermiteSplineTest;
+ friend class QuinticHermiteSplineTest;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
new file mode 100644
index 0000000..0a384fb
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+
+namespace frc {
+
+/**
+ * Define a unit for curvature.
+ */
+using curvature_t = units::unit_t<
+ units::compound_unit<units::radian, units::inverse<units::meter>>>;
+
+/**
+ * Represents a time-parameterized trajectory. The trajectory contains of
+ * various States that represent the pose, curvature, time elapsed, velocity,
+ * and acceleration at that point.
+ */
+class Trajectory {
+ public:
+ /**
+ * Represents one point on the trajectory.
+ */
+ struct State {
+ // The time elapsed since the beginning of the trajectory.
+ units::second_t t = 0_s;
+
+ // The speed at that point of the trajectory.
+ units::meters_per_second_t velocity = 0_mps;
+
+ // The acceleration at that point of the trajectory.
+ units::meters_per_second_squared_t acceleration = 0_mps_sq;
+
+ // The pose at that point of the trajectory.
+ Pose2d pose;
+
+ // The curvature at that point of the trajectory.
+ curvature_t curvature{0.0};
+
+ /**
+ * Interpolates between two States.
+ *
+ * @param endValue The end value for the interpolation.
+ * @param i The interpolant (fraction).
+ *
+ * @return The interpolated state.
+ */
+ State Interpolate(State endValue, double i) const;
+ };
+
+ Trajectory() = default;
+
+ /**
+ * Constructs a trajectory from a vector of states.
+ */
+ explicit Trajectory(const std::vector<State>& states);
+
+ /**
+ * Returns the overall duration of the trajectory.
+ * @return The duration of the trajectory.
+ */
+ units::second_t TotalTime() const { return m_totalTime; }
+
+ /**
+ * Return the states of the trajectory.
+ * @return The states of the trajectory.
+ */
+ const std::vector<State>& States() const { return m_states; }
+
+ /**
+ * Sample the trajectory at a point in time.
+ *
+ * @param t The point in time since the beginning of the trajectory to sample.
+ * @return The state at that point in time.
+ */
+ State Sample(units::second_t t) const;
+
+ private:
+ std::vector<State> m_states;
+ units::second_t m_totalTime;
+
+ /**
+ * Linearly interpolates between two values.
+ *
+ * @param startValue The start value.
+ * @param endValue The end value.
+ * @param t The fraction for interpolation.
+ *
+ * @return The interpolated value.
+ */
+ template <typename T>
+ static T Lerp(const T& startValue, const T& endValue, const double t) {
+ return startValue + (endValue - startValue) * t;
+ }
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
new file mode 100644
index 0000000..a739070
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
@@ -0,0 +1,140 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Represents the configuration for generating a trajectory. This class stores
+ * the start velocity, end velocity, max velocity, max acceleration, custom
+ * constraints, and the reversed flag.
+ *
+ * The class must be constructed with a max velocity and max acceleration.
+ * The other parameters (start velocity, end velocity, constraints, reversed)
+ * have been defaulted to reasonable values (0, 0, {}, false). These values can
+ * be changed via the SetXXX methods.
+ */
+class TrajectoryConfig {
+ public:
+ /**
+ * Constructs a config object.
+ * @param maxVelocity The max velocity of the trajectory.
+ * @param maxAcceleration The max acceleration of the trajectory.
+ */
+ TrajectoryConfig(units::meters_per_second_t maxVelocity,
+ units::meters_per_second_squared_t maxAcceleration)
+ : m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {}
+
+ TrajectoryConfig(const TrajectoryConfig&) = delete;
+ TrajectoryConfig& operator=(const TrajectoryConfig&) = delete;
+
+ TrajectoryConfig(TrajectoryConfig&&) = default;
+ TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
+
+ /**
+ * Sets the start velocity of the trajectory.
+ * @param startVelocity The start velocity of the trajectory.
+ */
+ void SetStartVelocity(units::meters_per_second_t startVelocity) {
+ m_startVelocity = startVelocity;
+ }
+
+ /**
+ * Sets the end velocity of the trajectory.
+ * @param endVelocity The end velocity of the trajectory.
+ */
+ void SetEndVelocity(units::meters_per_second_t endVelocity) {
+ m_endVelocity = endVelocity;
+ }
+
+ /**
+ * Sets the reversed flag of the trajectory.
+ * @param reversed Whether the trajectory should be reversed or not.
+ */
+ void SetReversed(bool reversed) { m_reversed = reversed; }
+
+ /**
+ * Adds a user-defined constraint to the trajectory.
+ * @param constraint The user-defined constraint.
+ */
+ template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
+ TrajectoryConstraint, Constraint>>>
+ void AddConstraint(Constraint constraint) {
+ m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
+ }
+
+ /**
+ * Adds a differential drive kinematics constraint to ensure that
+ * no wheel velocity of a differential drive goes above the max velocity.
+ *
+ * @param kinematics The differential drive kinematics.
+ */
+ void SetKinematics(const DifferentialDriveKinematics& kinematics) {
+ AddConstraint(
+ DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
+ }
+
+ /**
+ * Returns the starting velocity of the trajectory.
+ * @return The starting velocity of the trajectory.
+ */
+ units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
+
+ /**
+ * Returns the ending velocity of the trajectory.
+ * @return The ending velocity of the trajectory.
+ */
+ units::meters_per_second_t EndVelocity() const { return m_endVelocity; }
+
+ /**
+ * Returns the maximum velocity of the trajectory.
+ * @return The maximum velocity of the trajectory.
+ */
+ units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; }
+
+ /**
+ * Returns the maximum acceleration of the trajectory.
+ * @return The maximum acceleration of the trajectory.
+ */
+ units::meters_per_second_squared_t MaxAcceleration() const {
+ return m_maxAcceleration;
+ }
+
+ /**
+ * Returns the user-defined constraints of the trajectory.
+ * @return The user-defined constraints of the trajectory.
+ */
+ const std::vector<std::unique_ptr<TrajectoryConstraint>>& Constraints()
+ const {
+ return m_constraints;
+ }
+
+ /**
+ * Returns whether the trajectory is reversed or not.
+ * @return whether the trajectory is reversed or not.
+ */
+ bool IsReversed() const { return m_reversed; }
+
+ private:
+ units::meters_per_second_t m_startVelocity = 0_mps;
+ units::meters_per_second_t m_endVelocity = 0_mps;
+ units::meters_per_second_t m_maxVelocity;
+ units::meters_per_second_squared_t m_maxAcceleration;
+ std::vector<std::unique_ptr<TrajectoryConstraint>> m_constraints;
+ bool m_reversed = false;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
new file mode 100644
index 0000000..4d6eff6
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
@@ -0,0 +1,117 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc/spline/SplineParameterizer.h"
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Helper class used to generate trajectories with various constraints.
+ */
+class TrajectoryGenerator {
+ public:
+ using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
+
+ /**
+ * Generates a trajectory from the given control vectors and config. This
+ * method uses clamped cubic splines -- a method in which the exterior control
+ * vectors and interior waypoints are provided. The headings are automatically
+ * determined at the interior points to ensure continuous curvature.
+ *
+ * @param initial The initial control vector.
+ * @param interiorWaypoints The interior waypoints.
+ * @param end The ending control vector.
+ * @param config The configuration for the trajectory.
+ * @return The generated trajectory.
+ */
+ static Trajectory GenerateTrajectory(
+ Spline<3>::ControlVector initial,
+ const std::vector<Translation2d>& interiorWaypoints,
+ Spline<3>::ControlVector end, const TrajectoryConfig& config);
+
+ /**
+ * Generates a trajectory from the given waypoints and config. This method
+ * uses clamped cubic splines -- a method in which the initial pose, final
+ * pose, and interior waypoints are provided. The headings are automatically
+ * determined at the interior points to ensure continuous curvature.
+ *
+ * @param start The starting pose.
+ * @param interiorWaypoints The interior waypoints.
+ * @param end The ending pose.
+ * @param config The configuration for the trajectory.
+ * @return The generated trajectory.
+ */
+ static Trajectory GenerateTrajectory(
+ const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+ const Pose2d& end, const TrajectoryConfig& config);
+
+ /**
+ * Generates a trajectory from the given quintic control vectors and config.
+ * This method uses quintic hermite splines -- therefore, all points must be
+ * represented by control vectors. Continuous curvature is guaranteed in this
+ * method.
+ *
+ * @param controlVectors List of quintic control vectors.
+ * @param config The configuration for the trajectory.
+ * @return The generated trajectory.
+ */
+ static Trajectory GenerateTrajectory(
+ std::vector<Spline<5>::ControlVector> controlVectors,
+ const TrajectoryConfig& config);
+
+ /**
+ * Generates a trajectory from the given waypoints and config. This method
+ * uses quintic hermite splines -- therefore, all points must be represented
+ * by Pose2d objects. Continuous curvature is guaranteed in this method.
+ *
+ * @param waypoints List of waypoints..
+ * @param config The configuration for the trajectory.
+ * @return The generated trajectory.
+ */
+ static Trajectory GenerateTrajectory(const std::vector<Pose2d>& waypoints,
+ const TrajectoryConfig& config);
+
+ /**
+ * Generate spline points from a vector of splines by parameterizing the
+ * splines.
+ *
+ * @param splines The splines to parameterize.
+ *
+ * @return The spline points for use in time parameterization of a trajectory.
+ */
+ template <typename Spline>
+ static std::vector<PoseWithCurvature> SplinePointsFromSplines(
+ const std::vector<Spline>& splines) {
+ // Create the vector of spline points.
+ std::vector<PoseWithCurvature> splinePoints;
+
+ // Add the first point to the vector.
+ splinePoints.push_back(splines.front().GetPoint(0.0));
+
+ // Iterate through the vector and parameterize each spline, adding the
+ // parameterized points to the final vector.
+ for (auto&& spline : splines) {
+ auto points = SplineParameterizer::Parameterize(spline);
+ // Append the array of poses to the vector. We are removing the first
+ // point because it's a duplicate of the last point from the previous
+ // spline.
+ splinePoints.insert(std::end(splinePoints), std::begin(points) + 1,
+ std::end(points));
+ }
+ return splinePoints;
+ }
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
new file mode 100644
index 0000000..b8ea8da
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Class used to parameterize a trajectory by time.
+ */
+class TrajectoryParameterizer {
+ public:
+ using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
+
+ /**
+ * Parameterize the trajectory by time. This is where the velocity profile is
+ * generated.
+ *
+ * The derivation of the algorithm used can be found here:
+ * <http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf>
+ *
+ * @param points Reference to the spline points.
+ * @param constraints A vector of various velocity and acceleration
+ * constraints.
+ * @param startVelocity The start velocity for the trajectory.
+ * @param endVelocity The end velocity for the trajectory.
+ * @param maxVelocity The max velocity for the trajectory.
+ * @param maxAcceleration The max acceleration for the trajectory.
+ * @param reversed Whether the robot should move backwards. Note that the
+ * robot will still move from a -> b -> ... -> z as defined in the waypoints.
+ *
+ * @return The trajectory.
+ */
+ static Trajectory TimeParameterizeTrajectory(
+ const std::vector<PoseWithCurvature>& points,
+ const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+ units::meters_per_second_t startVelocity,
+ units::meters_per_second_t endVelocity,
+ units::meters_per_second_t maxVelocity,
+ units::meters_per_second_squared_t maxAcceleration, bool reversed);
+
+ private:
+ constexpr static double kEpsilon = 1E-6;
+
+ /**
+ * Represents a constrained state that is used when time parameterizing a
+ * trajectory. Each state has the pose, curvature, distance from the start of
+ * the trajectory, max velocity, min acceleration and max acceleration.
+ */
+ struct ConstrainedState {
+ PoseWithCurvature pose = {Pose2d(), curvature_t(0.0)};
+ units::meter_t distance = 0_m;
+ units::meters_per_second_t maxVelocity = 0_mps;
+ units::meters_per_second_squared_t minAcceleration = 0_mps_sq;
+ units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
+ };
+
+ /**
+ * Enforces acceleration limits as defined by the constraints. This function
+ * is used when time parameterizing a trajectory.
+ *
+ * @param reverse Whether the robot is traveling backwards.
+ * @param constraints A vector of the user-defined velocity and acceleration
+ * constraints.
+ * @param state Pointer to the constrained state that we are operating on.
+ * This is mutated in place.
+ */
+ static void EnforceAccelerationLimits(
+ bool reverse,
+ const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+ ConstrainedState* state);
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
new file mode 100644
index 0000000..c528c9e
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -0,0 +1,139 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+namespace frc {
+
+/**
+ * A trapezoid-shaped velocity profile.
+ *
+ * While this class can be used for a profiled movement from start to finish,
+ * the intended usage is to filter a reference's dynamics based on trapezoidal
+ * velocity constraints. To compute the reference obeying this constraint, do
+ * the following.
+ *
+ * Initialization:
+ * @code{.cpp}
+ * TrapezoidalMotionProfile::Constraints constraints{kMaxV, kMaxA};
+ * double previousProfiledReference = initialReference;
+ * @endcode
+ *
+ * Run on update:
+ * @code{.cpp}
+ * TrapezoidalMotionProfile profile{constraints, unprofiledReference,
+ * previousProfiledReference};
+ * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
+ * @endcode
+ *
+ * where `unprofiledReference` is free to change between calls. Note that when
+ * the unprofiled reference is within the constraints, `Calculate()` returns the
+ * unprofiled reference unchanged.
+ *
+ * Otherwise, a timer can be started to provide monotonic values for
+ * `Calculate()` and to determine when the profile has completed via
+ * `IsFinished()`.
+ */
+class TrapezoidProfile {
+ public:
+ class Constraints {
+ public:
+ units::meters_per_second_t maxVelocity = 0_mps;
+ units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
+ };
+
+ class State {
+ public:
+ units::meter_t position = 0_m;
+ units::meters_per_second_t velocity = 0_mps;
+ bool operator==(const State& rhs) const {
+ return position == rhs.position && velocity == rhs.velocity;
+ }
+ bool operator!=(const State& rhs) const { return !(*this == rhs); }
+ };
+
+ /**
+ * Construct a TrapezoidProfile.
+ *
+ * @param constraints The constraints on the profile, like maximum velocity.
+ * @param goal The desired state when the profile is complete.
+ * @param initial The initial state (usually the current state).
+ */
+ TrapezoidProfile(Constraints constraints, State goal,
+ State initial = State{0_m, 0_mps});
+
+ TrapezoidProfile(const TrapezoidProfile&) = default;
+ TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
+ TrapezoidProfile(TrapezoidProfile&&) = default;
+ TrapezoidProfile& operator=(TrapezoidProfile&&) = default;
+
+ /**
+ * Calculate the correct position and velocity for the profile at a time t
+ * where the beginning of the profile was at time t = 0.
+ *
+ * @param t The time since the beginning of the profile.
+ */
+ State Calculate(units::second_t t) const;
+
+ /**
+ * Returns the time left until a target distance in the profile is reached.
+ *
+ * @param target The target distance.
+ */
+ units::second_t TimeLeftUntil(units::meter_t target) const;
+
+ /**
+ * Returns the total time the profile takes to reach the goal.
+ */
+ units::second_t TotalTime() const { return m_endDeccel; }
+
+ /**
+ * Returns true if the profile has reached the goal.
+ *
+ * The profile has reached the goal if the time since the profile started
+ * has exceeded the profile's total time.
+ *
+ * @param t The time since the beginning of the profile.
+ */
+ bool IsFinished(units::second_t t) const { return t >= TotalTime(); }
+
+ private:
+ /**
+ * Returns true if the profile inverted.
+ *
+ * The profile is inverted if goal position is less than the initial position.
+ *
+ * @param initial The initial state (usually the current state).
+ * @param goal The desired state when the profile is complete.
+ */
+ static bool ShouldFlipAcceleration(const State& initial, const State& goal) {
+ return initial.position > goal.position;
+ }
+
+ // Flip the sign of the velocity and position if the profile is inverted
+ State Direct(const State& in) const {
+ State result = in;
+ result.position *= m_direction;
+ result.velocity *= m_direction;
+ return result;
+ }
+
+ // The direction of the profile, either 1 for forwards or -1 for inverted
+ int m_direction;
+
+ Constraints m_constraints;
+ State m_initial;
+ State m_goal;
+
+ units::second_t m_endAccel;
+ units::second_t m_endFullSpeed;
+ units::second_t m_endDeccel;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
new file mode 100644
index 0000000..de25738
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+
+/**
+ * A constraint on the maximum absolute centripetal acceleration allowed when
+ * traversing a trajectory. The centripetal acceleration of a robot is defined
+ * as the velocity squared divided by the radius of curvature.
+ *
+ * Effectively, limiting the maximum centripetal acceleration will cause the
+ * robot to slow down around tight turns, making it easier to track trajectories
+ * with sharp turns.
+ */
+class CentripetalAccelerationConstraint : public TrajectoryConstraint {
+ public:
+ explicit CentripetalAccelerationConstraint(
+ units::meters_per_second_squared_t maxCentripetalAcceleration);
+
+ units::meters_per_second_t MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) override;
+
+ MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) override;
+
+ private:
+ units::meters_per_second_squared_t m_maxCentripetalAcceleration;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
new file mode 100644
index 0000000..6259c96
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+namespace frc {
+class DifferentialDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+ DifferentialDriveKinematicsConstraint(DifferentialDriveKinematics kinematics,
+ units::meters_per_second_t maxSpeed);
+
+ units::meters_per_second_t MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) override;
+
+ MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) override;
+
+ private:
+ DifferentialDriveKinematics m_kinematics;
+ units::meters_per_second_t m_maxSpeed;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
new file mode 100644
index 0000000..dcde8c4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <limits>
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/spline/Spline.h"
+
+namespace frc {
+/**
+ * An interface for defining user-defined velocity and acceleration constraints
+ * while generating trajectories.
+ */
+class TrajectoryConstraint {
+ public:
+ TrajectoryConstraint() = default;
+
+ TrajectoryConstraint(const TrajectoryConstraint&) = default;
+ TrajectoryConstraint& operator=(const TrajectoryConstraint&) = default;
+
+ TrajectoryConstraint(TrajectoryConstraint&&) = default;
+ TrajectoryConstraint& operator=(TrajectoryConstraint&&) = default;
+
+ virtual ~TrajectoryConstraint() = default;
+
+ /**
+ * Represents a minimum and maximum acceleration.
+ */
+ struct MinMax {
+ /**
+ * The minimum acceleration.
+ */
+ units::meters_per_second_squared_t minAcceleration{
+ -std::numeric_limits<double>::max()};
+
+ /**
+ * The maximum acceleration.
+ */
+ units::meters_per_second_squared_t maxAcceleration{
+ std::numeric_limits<double>::max()};
+ };
+
+ /**
+ * Returns the max velocity given the current pose and curvature.
+ *
+ * @param pose The pose at the current point in the trajectory.
+ * @param curvature The curvature at the current point in the trajectory.
+ * @param velocity The velocity at the current point in the trajectory before
+ * constraints are applied.
+ *
+ * @return The absolute maximum velocity.
+ */
+ virtual units::meters_per_second_t MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) = 0;
+
+ /**
+ * Returns the minimum and maximum allowable acceleration for the trajectory
+ * given pose, curvature, and speed.
+ *
+ * @param pose The pose at the current point in the trajectory.
+ * @param curvature The curvature at the current point in the trajectory.
+ * @param speed The speed at the current point in the trajectory.
+ *
+ * @return The min and max acceleration bounds.
+ */
+ virtual MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) = 0;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc2/Timer.h b/wpilibc/src/main/native/include/frc2/Timer.h
new file mode 100644
index 0000000..c1eeb16
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/Timer.h
@@ -0,0 +1,138 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/deprecated.h>
+#include <wpi/mutex.h>
+
+#include "frc/Base.h"
+
+namespace frc2 {
+
+/**
+ * Pause the task for a specified time.
+ *
+ * Pause the execution of the program for a specified period of time given in
+ * seconds. Motors will continue to run at their last assigned values, and
+ * sensors will continue to update. Only the task containing the wait will pause
+ * until the wait time is expired.
+ *
+ * @param seconds Length of time to pause, in seconds.
+ */
+void Wait(units::second_t seconds);
+
+/**
+ * @brief Gives real-time clock system time with nanosecond resolution
+ * @return The time, just in case you want the robot to start autonomous at 8pm
+ * on Saturday.
+ */
+units::second_t GetTime();
+
+/**
+ * A wrapper for the frc::Timer class that returns unit-typed values.
+ */
+class Timer {
+ public:
+ /**
+ * Create a new timer object.
+ *
+ * Create a new timer object and reset the time to zero. The timer is
+ * initially not running and must be started.
+ */
+ Timer();
+
+ virtual ~Timer() = default;
+
+ Timer(const Timer& rhs);
+ Timer& operator=(const Timer& rhs);
+ Timer(Timer&& rhs);
+ Timer& operator=(Timer&& rhs);
+
+ /**
+ * Get the current time from the timer. If the clock is running it is derived
+ * from the current system clock the start time stored in the timer class. If
+ * the clock is not running, then return the time when it was last stopped.
+ *
+ * @return Current time value for this timer in seconds
+ */
+ units::second_t Get() const;
+
+ /**
+ * Reset the timer by setting the time to 0.
+ *
+ * Make the timer startTime the current time so new requests will be relative
+ * to now.
+ */
+ void Reset();
+
+ /**
+ * Start the timer running.
+ *
+ * Just set the running flag to true indicating that all time requests should
+ * be relative to the system clock.
+ */
+ void Start();
+
+ /**
+ * Stop the timer.
+ *
+ * This computes the time as of now and clears the running flag, causing all
+ * subsequent time requests to be read from the accumulated time rather than
+ * looking at the system clock.
+ */
+ void Stop();
+
+ /**
+ * Check if the period specified has passed and if it has, advance the start
+ * time by that period. This is useful to decide if it's time to do periodic
+ * work without drifting later by the time it took to get around to checking.
+ *
+ * @param period The period to check for.
+ * @return True if the period has passed.
+ */
+ bool HasPeriodPassed(units::second_t period);
+
+ /**
+ * Return the FPGA system clock time in seconds.
+ *
+ * Return the time from the FPGA hardware clock in seconds since the FPGA
+ * started. Rolls over after 71 minutes.
+ *
+ * @returns Robot running time in seconds.
+ */
+ static units::second_t GetFPGATimestamp();
+
+ /**
+ * Return the approximate match time.
+ *
+ * The FMS does not send an official match time to the robots, but does send
+ * an approximate match time. The value will count down the time remaining in
+ * the current period (auto or teleop).
+ *
+ * Warning: This is not an official time (so it cannot be used to dispute ref
+ * calls or guarantee that a function will trigger before the match ends).
+ *
+ * The Practice Match function of the DS approximates the behavior seen on the
+ * field.
+ *
+ * @return Time remaining in current match period (auto or teleop)
+ */
+ static units::second_t GetMatchTime();
+
+ // The time, in seconds, at which the 32-bit FPGA timestamp rolls over to 0
+ static const units::second_t kRolloverTime;
+
+ private:
+ units::second_t m_startTime = 0_s;
+ units::second_t m_accumulatedTime = 0_s;
+ bool m_running = false;
+ mutable wpi::mutex m_mutex;
+};
+
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/Command.h b/wpilibc/src/main/native/include/frc2/command/Command.h
new file mode 100644
index 0000000..49e904e
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/Command.h
@@ -0,0 +1,242 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ErrorBase.h>
+#include <frc/WPIErrors.h>
+#include <frc2/command/Subsystem.h>
+
+#include <memory>
+#include <string>
+
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/Demangle.h>
+#include <wpi/SmallSet.h>
+#include <wpi/Twine.h>
+
+namespace frc2 {
+
+template <typename T>
+std::string GetTypeName(const T& type) {
+ return wpi::Demangle(typeid(type).name());
+}
+
+class ParallelCommandGroup;
+class ParallelRaceGroup;
+class ParallelDeadlineGroup;
+class SequentialCommandGroup;
+class PerpetualCommand;
+class ProxyScheduleCommand;
+
+/**
+ * A state machine representing a complete action to be performed by the robot.
+ * Commands are run by the CommandScheduler, and can be composed into
+ * CommandGroups to allow users to build complicated multi-step actions without
+ * the need to roll the state machine logic themselves.
+ *
+ * <p>Commands are run synchronously from the main robot loop; no multithreading
+ * is used, unless specified explicitly from the command implementation.
+ *
+ * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
+ * or decorators will not function!
+ *
+ * @see CommandScheduler
+ * @see CommandHelper
+ */
+class Command : public frc::ErrorBase {
+ public:
+ Command() = default;
+ virtual ~Command();
+
+ Command(const Command&);
+ Command& operator=(const Command&);
+ Command(Command&&) = default;
+ Command& operator=(Command&&) = default;
+
+ /**
+ * The initial subroutine of a command. Called once when the command is
+ * initially scheduled.
+ */
+ virtual void Initialize();
+
+ /**
+ * The main body of a command. Called repeatedly while the command is
+ * scheduled.
+ */
+ virtual void Execute();
+
+ /**
+ * The action to take when the command ends. Called when either the command
+ * finishes normally, or when it interrupted/canceled.
+ *
+ * @param interrupted whether the command was interrupted/canceled
+ */
+ virtual void End(bool interrupted);
+
+ /**
+ * Whether the command has finished. Once a command finishes, the scheduler
+ * will call its end() method and un-schedule it.
+ *
+ * @return whether the command has finished.
+ */
+ virtual bool IsFinished() { return false; }
+
+ /**
+ * Specifies the set of subsystems used by this command. Two commands cannot
+ * use the same subsystem at the same time. If the command is scheduled as
+ * interruptible and another command is scheduled that shares a requirement,
+ * the command will be interrupted. Else, the command will not be scheduled.
+ * If no subsystems are required, return an empty set.
+ *
+ * <p>Note: it is recommended that user implementations contain the
+ * requirements as a field, and return that field here, rather than allocating
+ * a new set every time this is called.
+ *
+ * @return the set of subsystems that are required
+ */
+ virtual wpi::SmallSet<Subsystem*, 4> GetRequirements() const = 0;
+
+ /**
+ * Decorates this command with a timeout. If the specified timeout is
+ * exceeded before the command finishes normally, the command will be
+ * interrupted and un-scheduled. Note that the timeout only applies to the
+ * command returned by this method; the calling command is not itself changed.
+ *
+ * @param duration the timeout duration
+ * @return the command with the timeout added
+ */
+ ParallelRaceGroup WithTimeout(units::second_t duration) &&;
+
+ /**
+ * Decorates this command with an interrupt condition. If the specified
+ * condition becomes true before the command finishes normally, the command
+ * will be interrupted and un-scheduled. Note that this only applies to the
+ * command returned by this method; the calling command is not itself changed.
+ *
+ * @param condition the interrupt condition
+ * @return the command with the interrupt condition added
+ */
+ ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
+
+ /**
+ * Decorates this command with a runnable to run before this command starts.
+ *
+ * @param toRun the Runnable to run
+ * @return the decorated command
+ */
+ SequentialCommandGroup BeforeStarting(std::function<void()> toRun) &&;
+
+ /**
+ * Decorates this command with a runnable to run after the command finishes.
+ *
+ * @param toRun the Runnable to run
+ * @return the decorated command
+ */
+ SequentialCommandGroup AndThen(std::function<void()> toRun) &&;
+
+ /**
+ * Decorates this command to run perpetually, ignoring its ordinary end
+ * conditions. The decorated command can still be interrupted or canceled.
+ *
+ * @return the decorated command
+ */
+ PerpetualCommand Perpetually() &&;
+
+ /**
+ * Decorates this command to run "by proxy" by wrapping it in a {@link
+ * ProxyScheduleCommand}. This is useful for "forking off" from command groups
+ * when the user does not wish to extend the command's requirements to the
+ * entire command group.
+ *
+ * @return the decorated command
+ */
+ ProxyScheduleCommand AsProxy();
+
+ /**
+ * Schedules this command.
+ *
+ * @param interruptible whether this command can be interrupted by another
+ * command that shares one of its requirements
+ */
+ void Schedule(bool interruptible);
+
+ /**
+ * Schedules this command, defaulting to interruptible.
+ */
+ void Schedule() { Schedule(true); }
+
+ /**
+ * Cancels this command. Will call the command's interrupted() method.
+ * Commands will be canceled even if they are not marked as interruptible.
+ */
+ void Cancel();
+
+ /**
+ * Whether or not the command is currently scheduled. Note that this does not
+ * detect whether the command is being run by a CommandGroup, only whether it
+ * is directly being run by the scheduler.
+ *
+ * @return Whether the command is scheduled.
+ */
+ bool IsScheduled() const;
+
+ /**
+ * Whether the command requires a given subsystem. Named "hasRequirement"
+ * rather than "requires" to avoid confusion with
+ * {@link
+ * edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
+ * - this may be able to be changed in a few years.
+ *
+ * @param requirement the subsystem to inquire about
+ * @return whether the subsystem is required
+ */
+ bool HasRequirement(Subsystem* requirement) const;
+
+ /**
+ * Whether the command is currently grouped in a command group. Used as extra
+ * insurance to prevent accidental independent use of grouped commands.
+ */
+ bool IsGrouped() const;
+
+ /**
+ * Sets whether the command is currently grouped in a command group. Can be
+ * used to "reclaim" a command if a group is no longer going to use it. NOT
+ * ADVISED!
+ */
+ void SetGrouped(bool grouped);
+
+ /**
+ * Whether the given command should run when the robot is disabled. Override
+ * to return true if the command should run when disabled.
+ *
+ * @return whether the command should run when the robot is disabled
+ */
+ virtual bool RunsWhenDisabled() const { return false; }
+
+ virtual std::string GetName() const;
+
+ protected:
+ /**
+ * Transfers ownership of this command to a unique pointer. Used for
+ * decorator methods.
+ */
+ virtual std::unique_ptr<Command> TransferOwnership() && = 0;
+
+ bool m_isGrouped = false;
+};
+
+/**
+ * Checks if two commands have disjoint requirement sets.
+ *
+ * @param first The first command to check.
+ * @param second The second command to check.
+ * @return False if first and second share a requirement.
+ */
+bool RequirementsDisjoint(Command* first, Command* second);
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandBase.h b/wpilibc/src/main/native/include/frc2/command/CommandBase.h
new file mode 100644
index 0000000..3b1698f
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/CommandBase.h
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+
+#include <string>
+
+#include <wpi/SmallSet.h>
+#include <wpi/Twine.h>
+
+#include "Command.h"
+
+namespace frc2 {
+/**
+ * A Sendable base class for Commands.
+ */
+class CommandBase : public Command,
+ public frc::Sendable,
+ public frc::SendableHelper<CommandBase> {
+ public:
+ /**
+ * Adds the specified requirements to the command.
+ *
+ * @param requirements the requirements to add
+ */
+ void AddRequirements(std::initializer_list<Subsystem*> requirements);
+
+ void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
+
+ wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
+
+ /**
+ * Sets the name of this Command.
+ *
+ * @param name name
+ */
+ void SetName(const wpi::Twine& name);
+
+ /**
+ * Gets the name of this Command.
+ *
+ * @return Name
+ */
+ std::string GetName() const override;
+
+ /**
+ * Gets the subsystem name of this Command.
+ *
+ * @return Subsystem name
+ */
+ std::string GetSubsystem() const;
+
+ /**
+ * Sets the subsystem name of this Command.
+ *
+ * @param subsystem subsystem name
+ */
+ void SetSubsystem(const wpi::Twine& subsystem);
+
+ void InitSendable(frc::SendableBuilder& builder) override;
+
+ protected:
+ CommandBase();
+ wpi::SmallSet<Subsystem*, 4> m_requirements;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h b/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h
new file mode 100644
index 0000000..9d265b4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ErrorBase.h>
+
+#include <memory>
+#include <set>
+#include <vector>
+
+#include "CommandBase.h"
+
+namespace frc2 {
+
+/**
+ * A base for CommandGroups. Statically tracks commands that have been
+ * allocated to groups to ensure those commands are not also used independently,
+ * which can result in inconsistent command state and unpredictable execution.
+ */
+class CommandGroupBase : public CommandBase {
+ public:
+ /**
+ * Requires that the specified command not have been already allocated to a
+ * CommandGroup. Reports an error if the command is already grouped.
+ *
+ * @param commands The command to check
+ * @return True if all the command is ungrouped.
+ */
+ static bool RequireUngrouped(Command& command);
+
+ /**
+ * Requires that the specified commands not have been already allocated to a
+ * CommandGroup. Reports an error if any of the commands are already grouped.
+ *
+ * @param commands The commands to check
+ * @return True if all the commands are ungrouped.
+ */
+ static bool RequireUngrouped(wpi::ArrayRef<std::unique_ptr<Command>>);
+
+ /**
+ * Requires that the specified commands not have been already allocated to a
+ * CommandGroup. Reports an error if any of the commands are already grouped.
+ *
+ * @param commands The commands to check
+ * @return True if all the commands are ungrouped.
+ */
+ static bool RequireUngrouped(std::initializer_list<Command*>);
+
+ /**
+ * Adds the given commands to the command group.
+ *
+ * @param commands The commands to add.
+ */
+ virtual void AddCommands(
+ std::vector<std::unique_ptr<Command>>&& commands) = 0;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandHelper.h b/wpilibc/src/main/native/include/frc2/command/CommandHelper.h
new file mode 100644
index 0000000..ec15f88
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/CommandHelper.h
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+#include "Command.h"
+
+namespace frc2 {
+
+/**
+ * CRTP implementation to allow polymorphic decorator functions in Command.
+ *
+ * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
+ * or decorators will not function!
+ */
+template <typename Base, typename CRTP,
+ typename = std::enable_if_t<std::is_base_of_v<Command, Base>>>
+class CommandHelper : public Base {
+ using Base::Base;
+
+ public:
+ CommandHelper() = default;
+
+ protected:
+ std::unique_ptr<Command> TransferOwnership() && override {
+ return std::make_unique<CRTP>(std::move(*static_cast<CRTP*>(this)));
+ }
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h
new file mode 100644
index 0000000..2e9cdd5
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h
@@ -0,0 +1,372 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/ErrorBase.h>
+#include <frc/RobotState.h>
+#include <frc/WPIErrors.h>
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/DenseMap.h>
+#include <wpi/FunctionExtras.h>
+#include <wpi/SmallSet.h>
+
+#include "CommandState.h"
+
+namespace frc2 {
+class Command;
+class Subsystem;
+
+/**
+ * The scheduler responsible for running Commands. A Command-based robot should
+ * call Run() on the singleton instance in its periodic block in order to run
+ * commands synchronously from the main loop. Subsystems should be registered
+ * with the scheduler using RegisterSubsystem() in order for their Periodic()
+ * methods to be called and for their default commands to be scheduled.
+ */
+class CommandScheduler final : public frc::Sendable,
+ public frc::ErrorBase,
+ public frc::SendableHelper<CommandScheduler> {
+ public:
+ /**
+ * Returns the Scheduler instance.
+ *
+ * @return the instance
+ */
+ static CommandScheduler& GetInstance();
+
+ using Action = std::function<void(const Command&)>;
+
+ /**
+ * Adds a button binding to the scheduler, which will be polled to schedule
+ * commands.
+ *
+ * @param button The button to add
+ */
+ void AddButton(wpi::unique_function<void()> button);
+
+ /**
+ * Removes all button bindings from the scheduler.
+ */
+ void ClearButtons();
+
+ /**
+ * Schedules a command for execution. Does nothing if the command is already
+ * scheduled. If a command's requirements are not available, it will only be
+ * started if all the commands currently using those requirements have been
+ * scheduled as interruptible. If this is the case, they will be interrupted
+ * and the command will be scheduled.
+ *
+ * @param interruptible whether this command can be interrupted
+ * @param command the command to schedule
+ */
+ void Schedule(bool interruptible, Command* command);
+
+ /**
+ * Schedules a command for execution, with interruptible defaulted to true.
+ * Does nothing if the command is already scheduled.
+ *
+ * @param command the command to schedule
+ */
+ void Schedule(Command* command);
+
+ /**
+ * Schedules multiple commands for execution. Does nothing if the command is
+ * already scheduled. If a command's requirements are not available, it will
+ * only be started if all the commands currently using those requirements have
+ * been scheduled as interruptible. If this is the case, they will be
+ * interrupted and the command will be scheduled.
+ *
+ * @param interruptible whether the commands should be interruptible
+ * @param commands the commands to schedule
+ */
+ void Schedule(bool interruptible, wpi::ArrayRef<Command*> commands);
+
+ /**
+ * Schedules multiple commands for execution. Does nothing if the command is
+ * already scheduled. If a command's requirements are not available, it will
+ * only be started if all the commands currently using those requirements have
+ * been scheduled as interruptible. If this is the case, they will be
+ * interrupted and the command will be scheduled.
+ *
+ * @param interruptible whether the commands should be interruptible
+ * @param commands the commands to schedule
+ */
+ void Schedule(bool interruptible, std::initializer_list<Command*> commands);
+
+ /**
+ * Schedules multiple commands for execution, with interruptible defaulted to
+ * true. Does nothing if the command is already scheduled.
+ *
+ * @param commands the commands to schedule
+ */
+ void Schedule(wpi::ArrayRef<Command*> commands);
+
+ /**
+ * Schedules multiple commands for execution, with interruptible defaulted to
+ * true. Does nothing if the command is already scheduled.
+ *
+ * @param commands the commands to schedule
+ */
+ void Schedule(std::initializer_list<Command*> commands);
+
+ /**
+ * Runs a single iteration of the scheduler. The execution occurs in the
+ * following order:
+ *
+ * <p>Subsystem periodic methods are called.
+ *
+ * <p>Button bindings are polled, and new commands are scheduled from them.
+ *
+ * <p>Currently-scheduled commands are executed.
+ *
+ * <p>End conditions are checked on currently-scheduled commands, and commands
+ * that are finished have their end methods called and are removed.
+ *
+ * <p>Any subsystems not being used as requirements have their default methods
+ * started.
+ */
+ void Run();
+
+ /**
+ * Registers subsystems with the scheduler. This must be called for the
+ * subsystem's periodic block to run when the scheduler is run, and for the
+ * subsystem's default command to be scheduled. It is recommended to call
+ * this from the constructor of your subsystem implementations.
+ *
+ * @param subsystem the subsystem to register
+ */
+ void RegisterSubsystem(Subsystem* subsystem);
+
+ /**
+ * Un-registers subsystems with the scheduler. The subsystem will no longer
+ * have its periodic block called, and will not have its default command
+ * scheduled.
+ *
+ * @param subsystem the subsystem to un-register
+ */
+ void UnregisterSubsystem(Subsystem* subsystem);
+
+ void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+
+ void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
+
+ /**
+ * Sets the default command for a subsystem. Registers that subsystem if it
+ * is not already registered. Default commands will run whenever there is no
+ * other command currently scheduled that requires the subsystem. Default
+ * commands should be written to never end (i.e. their IsFinished() method
+ * should return false), as they would simply be re-scheduled if they do.
+ * Default commands must also require their subsystem.
+ *
+ * @param subsystem the subsystem whose default command will be set
+ * @param defaultCommand the default command to associate with the subsystem
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) {
+ if (!defaultCommand.HasRequirement(subsystem)) {
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse, "Default commands must require their subsystem!");
+ return;
+ }
+ if (defaultCommand.IsFinished()) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Default commands should not end!");
+ return;
+ }
+ m_subsystems[subsystem] = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(defaultCommand));
+ }
+
+ /**
+ * Gets the default command associated with this subsystem. Null if this
+ * subsystem has no default command associated with it.
+ *
+ * @param subsystem the subsystem to inquire about
+ * @return the default command associated with the subsystem
+ */
+ Command* GetDefaultCommand(const Subsystem* subsystem) const;
+
+ /**
+ * Cancels a command. The scheduler will only call the interrupted method of
+ * a canceled command, not the end method (though the interrupted method may
+ * itself call the end method). Commands will be canceled even if they are
+ * not scheduled as interruptible.
+ *
+ * @param command the command to cancel
+ */
+ void Cancel(Command* command);
+
+ /**
+ * Cancels commands. The scheduler will only call the interrupted method of a
+ * canceled command, not the end method (though the interrupted method may
+ * itself call the end method). Commands will be canceled even if they are
+ * not scheduled as interruptible.
+ *
+ * @param commands the commands to cancel
+ */
+ void Cancel(wpi::ArrayRef<Command*> commands);
+
+ /**
+ * Cancels commands. The scheduler will only call the interrupted method of a
+ * canceled command, not the end method (though the interrupted method may
+ * itself call the end method). Commands will be canceled even if they are
+ * not scheduled as interruptible.
+ *
+ * @param commands the commands to cancel
+ */
+ void Cancel(std::initializer_list<Command*> commands);
+
+ /**
+ * Cancels all commands that are currently scheduled.
+ */
+ void CancelAll();
+
+ /**
+ * Returns the time since a given command was scheduled. Note that this only
+ * works on commands that are directly scheduled by the scheduler; it will not
+ * work on commands inside of commandgroups, as the scheduler does not see
+ * them.
+ *
+ * @param command the command to query
+ * @return the time since the command was scheduled, in seconds
+ */
+ double TimeSinceScheduled(const Command* command) const;
+
+ /**
+ * Whether the given commands are running. Note that this only works on
+ * commands that are directly scheduled by the scheduler; it will not work on
+ * commands inside of CommandGroups, as the scheduler does not see them.
+ *
+ * @param commands the command to query
+ * @return whether the command is currently scheduled
+ */
+ bool IsScheduled(wpi::ArrayRef<const Command*> commands) const;
+
+ /**
+ * Whether the given commands are running. Note that this only works on
+ * commands that are directly scheduled by the scheduler; it will not work on
+ * commands inside of CommandGroups, as the scheduler does not see them.
+ *
+ * @param commands the command to query
+ * @return whether the command is currently scheduled
+ */
+ bool IsScheduled(std::initializer_list<const Command*> commands) const;
+
+ /**
+ * Whether a given command is running. Note that this only works on commands
+ * that are directly scheduled by the scheduler; it will not work on commands
+ * inside of CommandGroups, as the scheduler does not see them.
+ *
+ * @param commands the command to query
+ * @return whether the command is currently scheduled
+ */
+ bool IsScheduled(const Command* command) const;
+
+ /**
+ * Returns the command currently requiring a given subsystem. Null if no
+ * command is currently requiring the subsystem
+ *
+ * @param subsystem the subsystem to be inquired about
+ * @return the command currently requiring the subsystem
+ */
+ Command* Requiring(const Subsystem* subsystem) const;
+
+ /**
+ * Disables the command scheduler.
+ */
+ void Disable();
+
+ /**
+ * Enables the command scheduler.
+ */
+ void Enable();
+
+ /**
+ * Adds an action to perform on the initialization of any command by the
+ * scheduler.
+ *
+ * @param action the action to perform
+ */
+ void OnCommandInitialize(Action action);
+
+ /**
+ * Adds an action to perform on the execution of any command by the scheduler.
+ *
+ * @param action the action to perform
+ */
+ void OnCommandExecute(Action action);
+
+ /**
+ * Adds an action to perform on the interruption of any command by the
+ * scheduler.
+ *
+ * @param action the action to perform
+ */
+ void OnCommandInterrupt(Action action);
+
+ /**
+ * Adds an action to perform on the finishing of any command by the scheduler.
+ *
+ * @param action the action to perform
+ */
+ void OnCommandFinish(Action action);
+
+ void InitSendable(frc::SendableBuilder& builder) override;
+
+ private:
+ // Constructor; private as this is a singleton
+ CommandScheduler();
+
+ // A map from commands to their scheduling state. Also used as a set of the
+ // currently-running commands.
+ wpi::DenseMap<Command*, CommandState> m_scheduledCommands;
+
+ // A map from required subsystems to their requiring commands. Also used as a
+ // set of the currently-required subsystems.
+ wpi::DenseMap<Subsystem*, Command*> m_requirements;
+
+ // A map from subsystems registered with the scheduler to their default
+ // commands. Also used as a list of currently-registered subsystems.
+ wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> m_subsystems;
+
+ // The set of currently-registered buttons that will be polled every
+ // iteration.
+ wpi::SmallVector<wpi::unique_function<void()>, 4> m_buttons;
+
+ bool m_disabled{false};
+
+ // NetworkTable entries for use in Sendable impl
+ nt::NetworkTableEntry m_namesEntry;
+ nt::NetworkTableEntry m_idsEntry;
+ nt::NetworkTableEntry m_cancelEntry;
+
+ // Lists of user-supplied actions to be executed on scheduling events for
+ // every command.
+ wpi::SmallVector<Action, 4> m_initActions;
+ wpi::SmallVector<Action, 4> m_executeActions;
+ wpi::SmallVector<Action, 4> m_interruptActions;
+ wpi::SmallVector<Action, 4> m_finishActions;
+
+ // Flag and queues for avoiding concurrent modification if commands are
+ // scheduled/canceled during run
+
+ bool m_inRunLoop = false;
+ wpi::DenseMap<Command*, bool> m_toSchedule;
+ wpi::SmallVector<Command*, 4> m_toCancel;
+
+ friend class CommandTestBase;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandState.h b/wpilibc/src/main/native/include/frc2/command/CommandState.h
new file mode 100644
index 0000000..41fc5d0
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/CommandState.h
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc2 {
+/**
+ * Class that holds scheduling state for a command. Used internally by the
+ * CommandScheduler
+ */
+class CommandState final {
+ public:
+ CommandState() = default;
+
+ explicit CommandState(bool interruptible);
+
+ bool IsInterruptible() const { return m_interruptible; }
+
+ // The time since this command was initialized.
+ double TimeSinceInitialized() const;
+
+ private:
+ double m_startTime = -1;
+ bool m_interruptible;
+
+ void StartTiming();
+ void StartRunning();
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h b/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h
new file mode 100644
index 0000000..0419cf9
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <iostream>
+#include <memory>
+#include <utility>
+
+#include "CommandBase.h"
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * Runs one of two commands, depending on the value of the given condition when
+ * this command is initialized. Does not actually schedule the selected command
+ * - rather, the command is run through this command; this ensures that the
+ * command will behave as expected if used as part of a CommandGroup. Requires
+ * the requirements of both commands, again to ensure proper functioning when
+ * used in a CommandGroup. If this is undesired, consider using
+ * ScheduleCommand.
+ *
+ * <p>As this command contains multiple component commands within it, it is
+ * technically a command group; the command instances that are passed to it
+ * cannot be added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ *
+ * @see ScheduleCommand
+ */
+class ConditionalCommand
+ : public CommandHelper<CommandBase, ConditionalCommand> {
+ public:
+ /**
+ * Creates a new ConditionalCommand.
+ *
+ * @param onTrue the command to run if the condition is true
+ * @param onFalse the command to run if the condition is false
+ * @param condition the condition to determine which command to run
+ */
+ template <class T1, class T2,
+ typename = std::enable_if_t<
+ std::is_base_of_v<Command, std::remove_reference_t<T1>>>,
+ typename = std::enable_if_t<
+ std::is_base_of_v<Command, std::remove_reference_t<T2>>>>
+ ConditionalCommand(T1&& onTrue, T2&& onFalse, std::function<bool()> condition)
+ : ConditionalCommand(std::make_unique<std::remove_reference_t<T1>>(
+ std::forward<T1>(onTrue)),
+ std::make_unique<std::remove_reference_t<T2>>(
+ std::forward<T2>(onFalse)),
+ condition) {}
+
+ /**
+ * Creates a new ConditionalCommand.
+ *
+ * @param onTrue the command to run if the condition is true
+ * @param onFalse the command to run if the condition is false
+ * @param condition the condition to determine which command to run
+ */
+ ConditionalCommand(std::unique_ptr<Command>&& onTrue,
+ std::unique_ptr<Command>&& onFalse,
+ std::function<bool()> condition);
+
+ ConditionalCommand(ConditionalCommand&& other) = default;
+
+ // No copy constructors for command groups
+ ConditionalCommand(const ConditionalCommand& other) = delete;
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ std::unique_ptr<Command> m_onTrue;
+ std::unique_ptr<Command> m_onFalse;
+ std::function<bool()> m_condition;
+ Command* m_selectedCommand{nullptr};
+ bool m_runsWhenDisabled = true;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h
new file mode 100644
index 0000000..b47135c
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that allows the user to pass in functions for each of the basic
+ * command methods through the constructor. Useful for inline definitions of
+ * complex commands - note, however, that if a command is beyond a certain
+ * complexity it is usually better practice to write a proper class for it than
+ * to inline it.
+ */
+class FunctionalCommand : public CommandHelper<CommandBase, FunctionalCommand> {
+ public:
+ /**
+ * Creates a new FunctionalCommand.
+ *
+ * @param onInit the function to run on command initialization
+ * @param onExecute the function to run on command execution
+ * @param onEnd the function to run on command end
+ * @param isFinished the function that determines whether the command has
+ * finished
+ * @param requirements the subsystems required by this command
+ */
+ FunctionalCommand(std::function<void()> onInit,
+ std::function<void()> onExecute,
+ std::function<void(bool)> onEnd,
+ std::function<bool()> isFinished);
+
+ FunctionalCommand(FunctionalCommand&& other) = default;
+
+ FunctionalCommand(const FunctionalCommand& other) = default;
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ private:
+ std::function<void()> m_onInit;
+ std::function<void()> m_onExecute;
+ std::function<void(bool)> m_onEnd;
+ std::function<bool()> m_isFinished;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/InstantCommand.h b/wpilibc/src/main/native/include/frc2/command/InstantCommand.h
new file mode 100644
index 0000000..ec28a11
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/InstantCommand.h
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A Command that runs instantly; it will initialize, execute once, and end on
+ * the same iteration of the scheduler. Users can either pass in a Runnable and
+ * a set of requirements, or else subclass this command if desired.
+ */
+class InstantCommand : public CommandHelper<CommandBase, InstantCommand> {
+ public:
+ /**
+ * Creates a new InstantCommand that runs the given Runnable with the given
+ * requirements.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the subsystems required by this command
+ */
+ InstantCommand(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements);
+
+ InstantCommand(InstantCommand&& other) = default;
+
+ InstantCommand(const InstantCommand& other) = default;
+
+ /**
+ * Creates a new InstantCommand with a Runnable that does nothing. Useful
+ * only as a no-arg constructor to call implicitly from subclass constructors.
+ */
+ InstantCommand();
+
+ void Initialize() override;
+
+ bool IsFinished() final;
+
+ private:
+ std::function<void()> m_toRun;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h
new file mode 100644
index 0000000..3365c04
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/Notifier.h>
+
+#include <units/units.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that starts a notifier to run the given runnable periodically in a
+ * separate thread. Has no end condition as-is; either subclass it or use {@link
+ * Command#withTimeout(double)} or
+ * {@link Command#withInterrupt(BooleanSupplier)} to give it one.
+ *
+ * <p>WARNING: Do not use this class unless you are confident in your ability to
+ * make the executed code thread-safe. If you do not know what "thread-safe"
+ * means, that is a good sign that you should not use this class.
+ */
+class NotifierCommand : public CommandHelper<CommandBase, NotifierCommand> {
+ public:
+ /**
+ * Creates a new NotifierCommand.
+ *
+ * @param toRun the runnable for the notifier to run
+ * @param period the period at which the notifier should run
+ * @param requirements the subsystems required by this command
+ */
+ NotifierCommand(std::function<void()> toRun, units::second_t period,
+ std::initializer_list<Subsystem*> requirements);
+
+ NotifierCommand(NotifierCommand&& other);
+
+ NotifierCommand(const NotifierCommand& other);
+
+ void Initialize() override;
+
+ void End(bool interrupted) override;
+
+ private:
+ std::function<void()> m_toRun;
+ frc::Notifier m_notifier;
+ units::second_t m_period;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h b/wpilibc/src/main/native/include/frc2/command/PIDCommand.h
new file mode 100644
index 0000000..6e553ff
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/PIDCommand.h
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/controller/PIDController.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that controls an output with a PIDController. Runs forever by
+ * default - to add exit conditions and/or other behavior, subclass this class.
+ * The controller calculation and output are performed synchronously in the
+ * command's execute() method.
+ *
+ * @see PIDController
+ */
+class PIDCommand : public CommandHelper<CommandBase, PIDCommand> {
+ public:
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * PIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param setpointSource the controller's reference (aka setpoint)
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ PIDCommand(PIDController controller,
+ std::function<double()> measurementSource,
+ std::function<double()> setpointSource,
+ std::function<void(double)> useOutput,
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * PIDController with a constant setpoint.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param setpoint the controller's setpoint (aka setpoint)
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ PIDCommand(PIDController controller,
+ std::function<double()> measurementSource, double setpoint,
+ std::function<void(double)> useOutput,
+ std::initializer_list<Subsystem*> requirements);
+
+ PIDCommand(PIDCommand&& other) = default;
+
+ PIDCommand(const PIDCommand& other) = default;
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ /**
+ * Sets the function that uses the output of the PIDController.
+ *
+ * @param useOutput The function that uses the output.
+ */
+ void SetOutput(std::function<void(double)> useOutput);
+
+ /**
+ * Sets the setpoint for the controller to track the given source.
+ *
+ * @param setpointSource The setpoint source
+ */
+ void SetSetpoint(std::function<double()> setpointSource);
+
+ /**
+ * Sets the setpoint for the controller to a constant value.
+ *
+ * @param setpoint The setpoint
+ */
+ void SetSetpoint(double setpoint);
+
+ /**
+ * Sets the setpoint for the controller to a constant value relative (i.e.
+ * added to) the current setpoint.
+ *
+ * @param relativeReference The change in setpoint
+ */
+ void SetSetpointRelative(double relativeSetpoint);
+
+ /**
+ * Gets the measurement of the process variable. Wraps the passed-in function
+ * for readability.
+ *
+ * @return The measurement of the process variable
+ */
+ virtual double GetMeasurement();
+
+ /**
+ * Gets the measurement of the process variable. Wraps the passed-in function
+ * for readability.
+ *
+ * @return The measurement of the process variable
+ */
+ virtual void UseOutput(double output);
+
+ /**
+ * Returns the PIDController used by the command.
+ *
+ * @return The PIDController
+ */
+ PIDController& getController();
+
+ protected:
+ PIDController m_controller;
+ std::function<double()> m_measurement;
+ std::function<double()> m_setpoint;
+ std::function<void(double)> m_useOutput;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h
new file mode 100644
index 0000000..7aa21c0
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/controller/PIDController.h"
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that uses a PIDController to control an output. The controller
+ * is run synchronously from the subsystem's periodic() method.
+ *
+ * @see PIDController
+ */
+class PIDSubsystem : public SubsystemBase {
+ public:
+ /**
+ * Creates a new PIDSubsystem.
+ *
+ * @param controller the PIDController to use
+ */
+ explicit PIDSubsystem(PIDController controller);
+
+ void Periodic() override;
+
+ /**
+ * Uses the output from the PIDController.
+ *
+ * @param output the output of the PIDController
+ */
+ virtual void UseOutput(double output) = 0;
+
+ /**
+ * Returns the reference (setpoint) used by the PIDController.
+ *
+ * @return the reference (setpoint) to be used by the controller
+ */
+ virtual double GetSetpoint() = 0;
+
+ /**
+ * Returns the measurement of the process variable used by the PIDController.
+ *
+ * @return the measurement of the process variable
+ */
+ virtual double GetMeasurement() = 0;
+
+ /**
+ * Enables the PID control. Resets the controller.
+ */
+ virtual void Enable();
+
+ /**
+ * Disables the PID control. Sets output to zero.
+ */
+ virtual void Disable();
+
+ /**
+ * Returns the PIDController.
+ *
+ * @return The controller.
+ */
+ PIDController& GetController();
+
+ protected:
+ PIDController m_controller;
+ bool m_enabled;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h
new file mode 100644
index 0000000..322e7b1
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending when the last
+ * command ends.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class ParallelCommandGroup
+ : public CommandHelper<CommandGroupBase, ParallelCommandGroup> {
+ public:
+ /**
+ * Creates a new ParallelCommandGroup. The given commands will be executed
+ * simultaneously. The command group will finish when the last command
+ * finishes. If the CommandGroup is interrupted, only the commands that are
+ * still running will be interrupted.
+ *
+ * @param commands the commands to include in this group.
+ */
+ explicit ParallelCommandGroup(
+ std::vector<std::unique_ptr<Command>>&& commands);
+
+ /**
+ * Creates a new ParallelCommandGroup. The given commands will be executed
+ * simultaneously. The command group will finish when the last command
+ * finishes. If the CommandGroup is interrupted, only the commands that are
+ * still running will be interrupted.
+ *
+ * @param commands the commands to include in this group.
+ */
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ explicit ParallelCommandGroup(Types&&... commands) {
+ AddCommands(std::forward<Types>(commands)...);
+ }
+
+ ParallelCommandGroup(ParallelCommandGroup&& other) = default;
+
+ // No copy constructors for commandgroups
+ ParallelCommandGroup(const ParallelCommandGroup&) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ ParallelCommandGroup(ParallelCommandGroup&) = delete;
+
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ void AddCommands(Types&&... commands) {
+ std::vector<std::unique_ptr<Command>> foo;
+ ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+ std::forward<Types>(commands))),
+ ...);
+ AddCommands(std::move(foo));
+ }
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+ std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+ bool m_runWhenDisabled{true};
+ bool isRunning = false;
+};
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
new file mode 100644
index 0000000..168d0f8
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending only when a
+ * specific command (the "deadline") ends, interrupting all other commands that
+ * are still running at that point.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class ParallelDeadlineGroup
+ : public CommandHelper<CommandGroupBase, ParallelDeadlineGroup> {
+ public:
+ /**
+ * Creates a new ParallelDeadlineGroup. The given commands (including the
+ * deadline) will be executed simultaneously. The CommandGroup will finish
+ * when the deadline finishes, interrupting all other still-running commands.
+ * If the CommandGroup is interrupted, only the commands still running will be
+ * interrupted.
+ *
+ * @param deadline the command that determines when the group ends
+ * @param commands the commands to be executed
+ */
+ ParallelDeadlineGroup(std::unique_ptr<Command>&& deadline,
+ std::vector<std::unique_ptr<Command>>&& commands);
+ /**
+ * Creates a new ParallelDeadlineGroup. The given commands (including the
+ * deadline) will be executed simultaneously. The CommandGroup will finish
+ * when the deadline finishes, interrupting all other still-running commands.
+ * If the CommandGroup is interrupted, only the commands still running will be
+ * interrupted.
+ *
+ * @param deadline the command that determines when the group ends
+ * @param commands the commands to be executed
+ */
+ template <class T, class... Types,
+ typename = std::enable_if_t<
+ std::is_base_of_v<Command, std::remove_reference_t<T>>>,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ explicit ParallelDeadlineGroup(T&& deadline, Types&&... commands) {
+ SetDeadline(std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(deadline)));
+ AddCommands(std::forward<Types>(commands)...);
+ }
+
+ ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default;
+
+ // No copy constructors for command groups
+ ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ ParallelDeadlineGroup(ParallelDeadlineGroup&) = delete;
+
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ void AddCommands(Types&&... commands) {
+ std::vector<std::unique_ptr<Command>> foo;
+ ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+ std::forward<Types>(commands))),
+ ...);
+ AddCommands(std::move(foo));
+ }
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+ void SetDeadline(std::unique_ptr<Command>&& deadline);
+
+ std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
+ Command* m_deadline;
+ bool m_runWhenDisabled{true};
+ bool isRunning = false;
+};
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h
new file mode 100644
index 0000000..d7411e9
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <set>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A CommandGroup that runs a set of commands in parallel, ending when any one
+ * of the commands ends and interrupting all the others.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class ParallelRaceGroup
+ : public CommandHelper<CommandGroupBase, ParallelRaceGroup> {
+ public:
+ /**
+ * Creates a new ParallelCommandRace. The given commands will be executed
+ * simultaneously, and will "race to the finish" - the first command to finish
+ * ends the entire command, with all other commands being interrupted.
+ *
+ * @param commands the commands to include in this group.
+ */
+ explicit ParallelRaceGroup(std::vector<std::unique_ptr<Command>>&& commands);
+
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ explicit ParallelRaceGroup(Types&&... commands) {
+ AddCommands(std::forward<Types>(commands)...);
+ }
+
+ ParallelRaceGroup(ParallelRaceGroup&& other) = default;
+
+ // No copy constructors for command groups
+ ParallelRaceGroup(const ParallelRaceGroup&) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ ParallelRaceGroup(ParallelRaceGroup&) = delete;
+
+ template <class... Types>
+ void AddCommands(Types&&... commands) {
+ std::vector<std::unique_ptr<Command>> foo;
+ ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+ std::forward<Types>(commands))),
+ ...);
+ AddCommands(std::move(foo));
+ }
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+
+ std::set<std::unique_ptr<Command>> m_commands;
+ bool m_runWhenDisabled{true};
+ bool m_finished{false};
+ bool isRunning = false;
+};
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h b/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h
new file mode 100644
index 0000000..3f3c9e7
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <utility>
+
+#include "CommandBase.h"
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs another command in perpetuity, ignoring that command's
+ * end conditions. While this class does not extend {@link CommandGroupBase},
+ * it is still considered a CommandGroup, as it allows one to compose another
+ * command within it; the command instances that are passed to it cannot be
+ * added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class PerpetualCommand : public CommandHelper<CommandBase, PerpetualCommand> {
+ public:
+ /**
+ * Creates a new PerpetualCommand. Will run another command in perpetuity,
+ * ignoring that command's end conditions, unless this command itself is
+ * interrupted.
+ *
+ * @param command the command to run perpetually
+ */
+ explicit PerpetualCommand(std::unique_ptr<Command>&& command);
+
+ /**
+ * Creates a new PerpetualCommand. Will run another command in perpetuity,
+ * ignoring that command's end conditions, unless this command itself is
+ * interrupted.
+ *
+ * @param command the command to run perpetually
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ explicit PerpetualCommand(T&& command)
+ : PerpetualCommand(std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command))) {}
+
+ PerpetualCommand(PerpetualCommand&& other) = default;
+
+ // No copy constructors for command groups
+ PerpetualCommand(const PerpetualCommand& other) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ PerpetualCommand(PerpetualCommand&) = delete;
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ private:
+ std::unique_ptr<Command> m_command;
+};
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/PrintCommand.h b/wpilibc/src/main/native/include/frc2/command/PrintCommand.h
new file mode 100644
index 0000000..fb420cb
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/PrintCommand.h
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/Twine.h>
+#include <wpi/raw_ostream.h>
+
+#include "CommandHelper.h"
+#include "InstantCommand.h"
+
+namespace frc2 {
+/**
+ * A command that prints a string when initialized.
+ */
+class PrintCommand : public CommandHelper<InstantCommand, PrintCommand> {
+ public:
+ /**
+ * Creates a new a PrintCommand.
+ *
+ * @param message the message to print
+ */
+ explicit PrintCommand(const wpi::Twine& message);
+
+ PrintCommand(PrintCommand&& other) = default;
+
+ PrintCommand(const PrintCommand& other) = default;
+
+ bool RunsWhenDisabled() const override;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h
new file mode 100644
index 0000000..b1ff9e6
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that controls an output with a ProfiledPIDController. Runs forever
+ * by default - to add exit conditions and/or other behavior, subclass this
+ * class. The controller calculation and output are performed synchronously in
+ * the command's execute() method.
+ *
+ * @see ProfiledPIDController
+ */
+class ProfiledPIDCommand
+ : public CommandHelper<CommandBase, ProfiledPIDCommand> {
+ using State = frc::TrapezoidProfile::State;
+
+ public:
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goalSource the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController controller,
+ std::function<units::meter_t()> measurementSource,
+ std::function<State()> goalSource,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goalSource the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController controller,
+ std::function<units::meter_t()> measurementSource,
+ std::function<units::meter_t()> goalSource,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController with a constant goal.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goal the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController controller,
+ std::function<units::meter_t()> measurementSource,
+ State goal, std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * Creates a new PIDCommand, which controls the given output with a
+ * ProfiledPIDController with a constant goal.
+ *
+ * @param controller the controller that controls the output.
+ * @param measurementSource the measurement of the process variable
+ * @param goal the controller's goal
+ * @param useOutput the controller's output
+ * @param requirements the subsystems required by this command
+ */
+ ProfiledPIDCommand(frc::ProfiledPIDController controller,
+ std::function<units::meter_t()> measurementSource,
+ units::meter_t goal,
+ std::function<void(double, State)> useOutput,
+ std::initializer_list<Subsystem*> requirements);
+
+ ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
+
+ ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ /**
+ * Gets the measurement of the process variable. Wraps the passed-in function
+ * for readability.
+ *
+ * @return The measurement of the process variable
+ */
+ virtual units::meter_t GetMeasurement();
+
+ /**
+ * Gets the measurement of the process variable. Wraps the passed-in function
+ * for readability.
+ *
+ * @return The measurement of the process variable
+ */
+ virtual void UseOutput(double output, State state);
+
+ /**
+ * Returns the ProfiledPIDController used by the command.
+ *
+ * @return The ProfiledPIDController
+ */
+ frc::ProfiledPIDController& GetController();
+
+ protected:
+ frc::ProfiledPIDController m_controller;
+ std::function<units::meter_t()> m_measurement;
+ std::function<State()> m_goal;
+ std::function<void(double, State)> m_useOutput;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
new file mode 100644
index 0000000..4fa47b2
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "frc2/command/SubsystemBase.h"
+
+namespace frc2 {
+/**
+ * A subsystem that uses a ProfiledPIDController to control an output. The
+ * controller is run synchronously from the subsystem's periodic() method.
+ *
+ * @see ProfiledPIDController
+ */
+class ProfiledPIDSubsystem : public SubsystemBase {
+ using State = frc::TrapezoidProfile::State;
+
+ public:
+ /**
+ * Creates a new ProfiledPIDSubsystem.
+ *
+ * @param controller the ProfiledPIDController to use
+ */
+ explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller);
+
+ void Periodic() override;
+
+ /**
+ * Uses the output from the ProfiledPIDController.
+ *
+ * @param output the output of the ProfiledPIDController
+ */
+ virtual void UseOutput(double output, State state) = 0;
+
+ /**
+ * Returns the goal used by the ProfiledPIDController.
+ *
+ * @return the goal to be used by the controller
+ */
+ virtual State GetGoal() = 0;
+
+ /**
+ * Returns the measurement of the process variable used by the
+ * ProfiledPIDController.
+ *
+ * @return the measurement of the process variable
+ */
+ virtual units::meter_t GetMeasurement() = 0;
+
+ /**
+ * Enables the PID control. Resets the controller.
+ */
+ virtual void Enable();
+
+ /**
+ * Disables the PID control. Sets output to zero.
+ */
+ virtual void Disable();
+
+ /**
+ * Returns the ProfiledPIDController.
+ *
+ * @return The controller.
+ */
+ frc::ProfiledPIDController& GetController();
+
+ protected:
+ frc::ProfiledPIDController m_controller;
+ bool m_enabled;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h b/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h
new file mode 100644
index 0000000..8906424
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/SmallVector.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "SetUtilities.h"
+
+namespace frc2 {
+/**
+ * Schedules the given commands when this command is initialized, and ends when
+ * all the commands are no longer scheduled. Useful for forking off from
+ * CommandGroups. If this command is interrupted, it will cancel all of the
+ * commands.
+ */
+class ProxyScheduleCommand
+ : public CommandHelper<CommandBase, ProxyScheduleCommand> {
+ public:
+ /**
+ * Creates a new ProxyScheduleCommand that schedules the given commands when
+ * initialized, and ends when they are all no longer scheduled.
+ *
+ * @param toSchedule the commands to schedule
+ */
+ explicit ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
+
+ ProxyScheduleCommand(ProxyScheduleCommand&& other) = default;
+
+ ProxyScheduleCommand(const ProxyScheduleCommand& other) = default;
+
+ void Initialize() override;
+
+ void End(bool interrupted) override;
+
+ void Execute() override;
+
+ bool IsFinished() override;
+
+ private:
+ wpi::SmallVector<Command*, 4> m_toSchedule;
+ bool m_finished{false};
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h
new file mode 100644
index 0000000..592f194
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <functional>
+#include <memory>
+
+#include <units/units.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc/controller/PIDController.h"
+#include "frc/controller/RamseteController.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/Trajectory.h"
+#include "frc2/Timer.h"
+
+#pragma once
+
+namespace frc2 {
+/**
+ * A command that uses a RAMSETE controller to follow a trajectory
+ * with a differential drive.
+ *
+ * <p>The command handles trajectory-following, PID calculations, and
+ * feedforwards internally. This is intended to be a more-or-less "complete
+ * solution" that can be used by teams without a great deal of controls
+ * expertise.
+ *
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to
+ * use the onboard PID functionality of a "smart" motor controller) may use the
+ * secondary constructor that omits the PID and feedforward functionality,
+ * returning only the raw wheel speeds from the RAMSETE controller.
+ *
+ * @see RamseteController
+ * @see Trajectory
+ */
+class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
+ using voltsecondspermeter =
+ units::compound_unit<units::volt, units::second,
+ units::inverse<units::meter>>;
+ using voltsecondssquaredpermeter =
+ units::compound_unit<units::volt, units::squared<units::second>,
+ units::inverse<units::meter>>;
+
+ public:
+ /**
+ * Constructs a new RamseteCommand that, when executed, will follow the
+ * provided trajectory. PID control and feedforward are handled internally,
+ * and outputs are scaled -1 to 1 for easy consumption by speed controllers.
+ *
+ * <p>Note: The controller will *not* set the outputVolts to zero upon
+ * completion of the path - this is left to the user, since it is not
+ * appropriate for paths with nonstationary endstates.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param controller The RAMSETE controller used to follow the
+ * trajectory.
+ * @param ks Constant feedforward term for the robot drive.
+ * @param kv Velocity-proportional feedforward term for the robot
+ * drive.
+ * @param ka Acceleration-proportional feedforward term for the
+ * robot drive.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param leftSpeed A function that supplies the speed of the left side
+ * of the robot drive.
+ * @param rightSpeed A function that supplies the speed of the right side
+ * of the robot drive.
+ * @param leftController The PIDController for the left side of the robot
+ * drive.
+ * @param rightController The PIDController for the right side of the robot
+ * drive.
+ * @param output A function that consumes the computed left and right
+ * outputs (in volts) for the robot drive.
+ * @param requirements The subsystems to require.
+ */
+ RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller, units::volt_t ks,
+ units::unit_t<voltsecondspermeter> kv,
+ units::unit_t<voltsecondssquaredpermeter> ka,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<units::meters_per_second_t()> leftSpeed,
+ std::function<units::meters_per_second_t()> rightSpeed,
+ frc2::PIDController leftController,
+ frc2::PIDController rightController,
+ std::function<void(units::volt_t, units::volt_t)> output,
+ std::initializer_list<Subsystem*> requirements);
+
+ /**
+ * Constructs a new RamseteCommand that, when executed, will follow the
+ * provided trajectory. Performs no PID control and calculates no
+ * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller,
+ * and will need to be converted into a usable form by the user.
+ *
+ * @param trajectory The trajectory to follow.
+ * @param pose A function that supplies the robot pose - use one of
+ * the odometry classes to provide this.
+ * @param controller The RAMSETE controller used to follow the
+ * trajectory.
+ * @param kinematics The kinematics for the robot drivetrain.
+ * @param output A function that consumes the computed left and right
+ * outputs (in volts) for the robot drive.
+ * @param requirements The subsystems to require.
+ */
+ RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
+ frc::RamseteController controller,
+ frc::DifferentialDriveKinematics kinematics,
+ std::function<void(units::meters_per_second_t,
+ units::meters_per_second_t)>
+ output,
+ std::initializer_list<Subsystem*> requirements);
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ private:
+ frc::Trajectory m_trajectory;
+ std::function<frc::Pose2d()> m_pose;
+ frc::RamseteController m_controller;
+ const units::volt_t m_ks;
+ const units::unit_t<voltsecondspermeter> m_kv;
+ const units::unit_t<voltsecondssquaredpermeter> m_ka;
+ frc::DifferentialDriveKinematics m_kinematics;
+ std::function<units::meters_per_second_t()> m_leftSpeed;
+ std::function<units::meters_per_second_t()> m_rightSpeed;
+ std::unique_ptr<frc2::PIDController> m_leftController;
+ std::unique_ptr<frc2::PIDController> m_rightController;
+ std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
+ std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
+ m_outputVel;
+
+ Timer m_timer;
+ units::second_t m_prevTime;
+ frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/RunCommand.h b/wpilibc/src/main/native/include/frc2/command/RunCommand.h
new file mode 100644
index 0000000..5a0524a
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/RunCommand.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs a Runnable continuously. Has no end condition as-is;
+ * either subclass it or use Command.WithTimeout() or
+ * Command.WithInterrupt() to give it one. If you only wish
+ * to execute a Runnable once, use InstantCommand.
+ */
+class RunCommand : public CommandHelper<CommandBase, RunCommand> {
+ public:
+ /**
+ * Creates a new RunCommand. The Runnable will be run continuously until the
+ * command ends. Does not run when disabled.
+ *
+ * @param toRun the Runnable to run
+ * @param requirements the subsystems to require
+ */
+ RunCommand(std::function<void()> toRun,
+ std::initializer_list<Subsystem*> requirements);
+
+ RunCommand(RunCommand&& other) = default;
+
+ RunCommand(const RunCommand& other) = default;
+
+ void Execute();
+
+ protected:
+ std::function<void()> m_toRun;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h b/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h
new file mode 100644
index 0000000..422823b
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/SmallVector.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "SetUtilities.h"
+
+namespace frc2 {
+/**
+ * Schedules the given commands when this command is initialized. Useful for
+ * forking off from CommandGroups. Note that if run from a CommandGroup, the
+ * group will not know about the status of the scheduled commands, and will
+ * treat this command as finishing instantly.
+ */
+class ScheduleCommand : public CommandHelper<CommandBase, ScheduleCommand> {
+ public:
+ /**
+ * Creates a new ScheduleCommand that schedules the given commands when
+ * initialized.
+ *
+ * @param toSchedule the commands to schedule
+ */
+ explicit ScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
+
+ ScheduleCommand(ScheduleCommand&& other) = default;
+
+ ScheduleCommand(const ScheduleCommand& other) = default;
+
+ void Initialize() override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ wpi::SmallVector<Command*, 4> m_toSchedule;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/SelectCommand.h b/wpilibc/src/main/native/include/frc2/command/SelectCommand.h
new file mode 100644
index 0000000..8dba378
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/SelectCommand.h
@@ -0,0 +1,153 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <memory>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+#include "CommandBase.h"
+#include "CommandGroupBase.h"
+#include "PrintCommand.h"
+
+namespace frc2 {
+template <typename Key>
+/**
+ * Runs one of a selection of commands, either using a selector and a key to
+ * command mapping, or a supplier that returns the command directly at runtime.
+ * Does not actually schedule the selected command - rather, the command is run
+ * through this command; this ensures that the command will behave as expected
+ * if used as part of a CommandGroup. Requires the requirements of all included
+ * commands, again to ensure proper functioning when used in a CommandGroup. If
+ * this is undesired, consider using ScheduleCommand.
+ *
+ * <p>As this command contains multiple component commands within it, it is
+ * technically a command group; the command instances that are passed to it
+ * cannot be added to any other groups, or scheduled individually.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
+ public:
+ /**
+ * Creates a new selectcommand.
+ *
+ * @param commands the map of commands to choose from
+ * @param selector the selector to determine which command to run
+ */
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ SelectCommand(std::function<Key()> selector,
+ std::pair<Key, Types>... commands)
+ : m_selector{std::move(selector)} {
+ std::vector<std::pair<Key, std::unique_ptr<Command>>> foo;
+
+ ((void)foo.emplace_back(commands.first,
+ std::make_unique<std::remove_reference_t<Types>>(
+ std::move(commands.second))),
+ ...);
+
+ for (auto&& command : foo) {
+ if (!CommandGroupBase::RequireUngrouped(command.second)) {
+ return;
+ }
+ }
+
+ for (auto&& command : foo) {
+ this->AddRequirements(command.second->GetRequirements());
+ m_runsWhenDisabled &= command.second->RunsWhenDisabled();
+ m_commands.emplace(std::move(command.first), std::move(command.second));
+ }
+ }
+
+ SelectCommand(
+ std::function<Key()> selector,
+ std::vector<std::pair<Key, std::unique_ptr<Command>>>&& commands)
+ : m_selector{std::move(selector)} {
+ for (auto&& command : commands) {
+ if (!CommandGroupBase::RequireUngrouped(command.second)) {
+ return;
+ }
+ }
+
+ for (auto&& command : commands) {
+ this->AddRequirements(command.second->GetRequirements());
+ m_runsWhenDisabled &= command.second->RunsWhenDisabled();
+ m_commands.emplace(std::move(command.first), std::move(command.second));
+ }
+ }
+
+ // No copy constructors for command groups
+ SelectCommand(const SelectCommand& other) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ SelectCommand(SelectCommand&) = delete;
+
+ /**
+ * Creates a new selectcommand.
+ *
+ * @param toRun a supplier providing the command to run
+ */
+ explicit SelectCommand(std::function<Command*()> toRun) : m_toRun{toRun} {}
+
+ SelectCommand(SelectCommand&& other) = default;
+
+ void Initialize() override;
+
+ void Execute() override { m_selectedCommand->Execute(); }
+
+ void End(bool interrupted) override {
+ return m_selectedCommand->End(interrupted);
+ }
+
+ bool IsFinished() override { return m_selectedCommand->IsFinished(); }
+
+ bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
+
+ protected:
+ std::unique_ptr<Command> TransferOwnership() && override {
+ return std::make_unique<SelectCommand>(std::move(*this));
+ }
+
+ private:
+ std::unordered_map<Key, std::unique_ptr<Command>> m_commands;
+ std::function<Key()> m_selector;
+ std::function<Command*()> m_toRun;
+ Command* m_selectedCommand;
+ bool m_runsWhenDisabled = true;
+};
+
+template <typename T>
+void SelectCommand<T>::Initialize() {
+ if (m_selector) {
+ auto find = m_commands.find(m_selector());
+ if (find == m_commands.end()) {
+ m_selectedCommand = new PrintCommand(
+ "SelectCommand selector value does not correspond to any command!");
+ return;
+ }
+ m_selectedCommand = find->second.get();
+ } else {
+ m_selectedCommand = m_toRun();
+ }
+ m_selectedCommand->Initialize();
+}
+
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h
new file mode 100644
index 0000000..dd1f2ce
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4521)
+#endif
+
+#include <limits>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <wpi/ArrayRef.h>
+
+#include "CommandGroupBase.h"
+#include "CommandHelper.h"
+#include "frc/ErrorBase.h"
+#include "frc/WPIErrors.h"
+
+namespace frc2 {
+
+const size_t invalid_index = std::numeric_limits<size_t>::max();
+
+/**
+ * A CommandGroups that runs a list of commands in sequence.
+ *
+ * <p>As a rule, CommandGroups require the union of the requirements of their
+ * component commands.
+ */
+class SequentialCommandGroup
+ : public CommandHelper<CommandGroupBase, SequentialCommandGroup> {
+ public:
+ /**
+ * Creates a new SequentialCommandGroup. The given commands will be run
+ * sequentially, with the CommandGroup finishing when the last command
+ * finishes.
+ *
+ * @param commands the commands to include in this group.
+ */
+ explicit SequentialCommandGroup(
+ std::vector<std::unique_ptr<Command>>&& commands);
+
+ /**
+ * Creates a new SequentialCommandGroup. The given commands will be run
+ * sequentially, with the CommandGroup finishing when the last command
+ * finishes.
+ *
+ * @param commands the commands to include in this group.
+ */
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ explicit SequentialCommandGroup(Types&&... commands) {
+ AddCommands(std::forward<Types>(commands)...);
+ }
+
+ SequentialCommandGroup(SequentialCommandGroup&& other) = default;
+
+ // No copy constructors for command groups
+ SequentialCommandGroup(const SequentialCommandGroup&) = delete;
+
+ // Prevent template expansion from emulating copy ctor
+ SequentialCommandGroup(SequentialCommandGroup&) = delete;
+
+ template <class... Types,
+ typename = std::enable_if_t<std::conjunction_v<
+ std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
+ void AddCommands(Types&&... commands) {
+ std::vector<std::unique_ptr<Command>> foo;
+ ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
+ std::forward<Types>(commands))),
+ ...);
+ AddCommands(std::move(foo));
+ }
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
+
+ wpi::SmallVector<std::unique_ptr<Command>, 4> m_commands;
+ size_t m_currentCommandIndex{invalid_index};
+ bool m_runWhenDisabled{true};
+};
+} // namespace frc2
+
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/SetUtilities.h b/wpilibc/src/main/native/include/frc2/command/SetUtilities.h
new file mode 100644
index 0000000..d21f572
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/SetUtilities.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+
+namespace frc2 {
+template <typename T>
+void SetInsert(wpi::SmallVectorImpl<T*>& vector, wpi::ArrayRef<T*> toAdd) {
+ for (auto addCommand : toAdd) {
+ bool exists = false;
+ for (auto existingCommand : vector) {
+ if (addCommand == existingCommand) {
+ exists = true;
+ break;
+ }
+ }
+ if (!exists) {
+ vector.emplace_back(addCommand);
+ }
+ }
+}
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h
new file mode 100644
index 0000000..987b9db
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that runs a given runnable when it is initalized, and another
+ * runnable when it ends. Useful for running and then stopping a motor, or
+ * extending and then retracting a solenoid. Has no end condition as-is; either
+ * subclass it or use Command.WithTimeout() or Command.WithInterrupt() to give
+ * it one.
+ */
+class StartEndCommand : public CommandHelper<CommandBase, StartEndCommand> {
+ public:
+ /**
+ * Creates a new StartEndCommand. Will run the given runnables when the
+ * command starts and when it ends.
+ *
+ * @param onInit the Runnable to run on command init
+ * @param onEnd the Runnable to run on command end
+ * @param requirements the subsystems required by this command
+ */
+ StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
+ std::initializer_list<Subsystem*> requirements);
+
+ StartEndCommand(StartEndCommand&& other) = default;
+
+ StartEndCommand(const StartEndCommand& other);
+
+ void Initialize() override;
+
+ void End(bool interrupted) override;
+
+ protected:
+ std::function<void()> m_onInit;
+ std::function<void()> m_onEnd;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/Subsystem.h b/wpilibc/src/main/native/include/frc2/command/Subsystem.h
new file mode 100644
index 0000000..5eb10bc
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/Subsystem.h
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/CommandScheduler.h>
+
+#include <utility>
+
+namespace frc2 {
+class Command;
+/**
+ * A robot subsystem. Subsystems are the basic unit of robot organization in
+ * the Command-based framework; they encapsulate low-level hardware objects
+ * (motor controllers, sensors, etc) and provide methods through which they can
+ * be used by Commands. Subsystems are used by the CommandScheduler's resource
+ * management system to ensure multiple robot actions are not "fighting" over
+ * the same hardware; Commands that use a subsystem should include that
+ * subsystem in their GetRequirements() method, and resources used within a
+ * subsystem should generally remain encapsulated and not be shared by other
+ * parts of the robot.
+ *
+ * <p>Subsystems must be registered with the scheduler with the
+ * CommandScheduler.RegisterSubsystem() method in order for the
+ * Periodic() method to be called. It is recommended that this method be called
+ * from the constructor of users' Subsystem implementations. The
+ * SubsystemBase class offers a simple base for user implementations
+ * that handles this.
+ *
+ * @see Command
+ * @see CommandScheduler
+ * @see SubsystemBase
+ */
+class Subsystem {
+ public:
+ ~Subsystem();
+ /**
+ * This method is called periodically by the CommandScheduler. Useful for
+ * updating subsystem-specific state that you don't want to offload to a
+ * Command. Teams should try to be consistent within their own codebases
+ * about which responsibilities will be handled by Commands, and which will be
+ * handled here.
+ */
+ virtual void Periodic();
+
+ /**
+ * 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.
+ * their IsFinished() method should always return false. Will automatically
+ * register this subsystem with the CommandScheduler.
+ *
+ * @param defaultCommand the default command to associate with this subsystem
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ void SetDefaultCommand(T&& defaultCommand) {
+ CommandScheduler::GetInstance().SetDefaultCommand(
+ this, std::forward<T>(defaultCommand));
+ }
+
+ /**
+ * Gets the default command for this subsystem. Returns null if no default
+ * command is currently associated with the subsystem.
+ *
+ * @return the default command associated with this subsystem
+ */
+ Command* GetDefaultCommand() const;
+
+ /**
+ * Returns the command currently running on this subsystem. Returns null if
+ * no command is currently scheduled that requires this subsystem.
+ *
+ * @return the scheduled command currently requiring this subsystem
+ */
+ Command* GetCurrentCommand() const;
+
+ /**
+ * Registers this subsystem with the CommandScheduler, allowing its
+ * Periodic() method to be called when the scheduler runs.
+ */
+ void Register();
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h b/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h
new file mode 100644
index 0000000..e7dffc7
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc/smartdashboard/Sendable.h>
+#include <frc/smartdashboard/SendableHelper.h>
+
+#include <string>
+
+#include "Subsystem.h"
+
+namespace frc2 {
+/**
+ * A base for subsystems that handles registration in the constructor, and
+ * provides a more intuitive method for setting the default command.
+ */
+class SubsystemBase : public Subsystem,
+ public frc::Sendable,
+ public frc::SendableHelper<SubsystemBase> {
+ public:
+ void InitSendable(frc::SendableBuilder& builder) override;
+
+ /**
+ * Gets the name of this Subsystem.
+ *
+ * @return Name
+ */
+ std::string GetName() const;
+
+ /**
+ * Sets the name of this Subsystem.
+ *
+ * @param name name
+ */
+ void SetName(const wpi::Twine& name);
+
+ /**
+ * Gets the subsystem name of this Subsystem.
+ *
+ * @return Subsystem name
+ */
+ std::string GetSubsystem() const;
+
+ /**
+ * Sets the subsystem name of this Subsystem.
+ *
+ * @param subsystem subsystem name
+ */
+ void SetSubsystem(const wpi::Twine& name);
+
+ /**
+ * Associate a Sendable with this Subsystem.
+ * Also update the child's name.
+ *
+ * @param name name to give child
+ * @param child sendable
+ */
+ void AddChild(std::string name, frc::Sendable* child);
+
+ protected:
+ SubsystemBase();
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
new file mode 100644
index 0000000..6864f53
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <frc2/Timer.h>
+
+#include <functional>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+
+#pragma once
+
+namespace frc2 {
+/**
+ * A command that runs a TrapezoidProfile. Useful for smoothly controlling
+ * mechanism motion.
+ *
+ * @see TrapezoidProfile
+ */
+class TrapezoidProfileCommand
+ : public CommandHelper<CommandBase, TrapezoidProfileCommand> {
+ public:
+ /**
+ * Creates a new TrapezoidProfileCommand that will execute the given
+ * TrapezoidalProfile. Output will be piped to the provided consumer function.
+ *
+ * @param profile The motion profile to execute.
+ * @param output The consumer for the profile output.
+ */
+ TrapezoidProfileCommand(
+ frc::TrapezoidProfile profile,
+ std::function<void(frc::TrapezoidProfile::State)> output,
+ std::initializer_list<Subsystem*> requirements);
+
+ void Initialize() override;
+
+ void Execute() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ private:
+ frc::TrapezoidProfile m_profile;
+ std::function<void(frc::TrapezoidProfile::State)> m_output;
+
+ Timer m_timer;
+};
+
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/WaitCommand.h b/wpilibc/src/main/native/include/frc2/command/WaitCommand.h
new file mode 100644
index 0000000..1544592
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/WaitCommand.h
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/Twine.h>
+
+#include "CommandBase.h"
+#include "CommandHelper.h"
+#include "frc2/Timer.h"
+
+namespace frc2 {
+/**
+ * A command that does nothing but takes a specified amount of time to finish.
+ * Useful for CommandGroups. Can also be subclassed to make a command with an
+ * internal timer.
+ */
+class WaitCommand : public CommandHelper<CommandBase, WaitCommand> {
+ public:
+ /**
+ * Creates a new WaitCommand. This command will do nothing, and end after the
+ * specified duration.
+ *
+ * @param duration the time to wait
+ */
+ explicit WaitCommand(units::second_t duration);
+
+ WaitCommand(WaitCommand&& other) = default;
+
+ WaitCommand(const WaitCommand& other) = default;
+
+ void Initialize() override;
+
+ void End(bool interrupted) override;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ protected:
+ Timer m_timer;
+
+ private:
+ units::second_t m_duration;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h b/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h
new file mode 100644
index 0000000..8da18e8
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "CommandBase.h"
+#include "frc/Timer.h"
+#include "frc2/command/CommandHelper.h"
+
+namespace frc2 {
+/**
+ * A command that does nothing but ends after a specified match time or
+ * condition. Useful for CommandGroups.
+ */
+class WaitUntilCommand : public CommandHelper<CommandBase, WaitUntilCommand> {
+ public:
+ /**
+ * Creates a new WaitUntilCommand that ends after a given condition becomes
+ * true.
+ *
+ * @param condition the condition to determine when to end
+ */
+ explicit WaitUntilCommand(std::function<bool()> condition);
+
+ /**
+ * Creates a new WaitUntilCommand that ends after a given match time.
+ *
+ * <p>NOTE: The match timer used for this command is UNOFFICIAL. Using this
+ * command does NOT guarantee that the time at which the action is performed
+ * will be judged to be legal by the referees. When in doubt, add a safety
+ * factor or time the action manually.
+ *
+ * @param time the match time after which to end, in seconds
+ */
+ explicit WaitUntilCommand(double time);
+
+ WaitUntilCommand(WaitUntilCommand&& other) = default;
+
+ WaitUntilCommand(const WaitUntilCommand& other) = default;
+
+ bool IsFinished() override;
+
+ bool RunsWhenDisabled() const override;
+
+ private:
+ std::function<bool()> m_condition;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/Button.h b/wpilibc/src/main/native/include/frc2/command/button/Button.h
new file mode 100644
index 0000000..3e0f7fe
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/button/Button.h
@@ -0,0 +1,207 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <utility>
+
+#include "Trigger.h"
+
+namespace frc2 {
+class Command;
+/**
+ * A class used to bind command scheduling to button presses. Can be composed
+ * with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class Button : public Trigger {
+ public:
+ /**
+ * Create a new button that is pressed when the given condition is true.
+ *
+ * @param isActive Whether the button is pressed.
+ */
+ explicit Button(std::function<bool()> isPressed);
+
+ /**
+ * Create a new button that is pressed active (default constructor) - activity
+ * can be further determined by subclass code.
+ */
+ Button() = default;
+
+ /**
+ * Binds a command to start when the button is pressed. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Button WhenPressed(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the button is pressed. Transfers
+ * command ownership to the button scheduler, so the user does not have to
+ * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+ * *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Button WhenPressed(T&& command, bool interruptible = true) {
+ WhenActive(std::forward<T>(command), interruptible);
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute when the button is pressed.
+ *
+ * @param toRun the runnable to execute.
+ */
+ Button WhenPressed(std::function<void()> toRun);
+
+ /**
+ * Binds a command to be started repeatedly while the button is pressed, and
+ * cancelled when it is released. Takes a raw pointer, and so is non-owning;
+ * users are responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ Button WhileHeld(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to be started repeatedly while the button is pressed, and
+ * cancelled when it is released. Transfers command ownership to the button
+ * scheduler, so the user does not have to worry about lifespan - rvalue refs
+ * will be *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Button WhileHeld(T&& command, bool interruptible = true) {
+ WhileActiveContinous(std::forward<T>(command), interruptible);
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute repeatedly while the button is pressed.
+ *
+ * @param toRun the runnable to execute.
+ */
+ Button WhileHeld(std::function<void()> toRun);
+
+ /**
+ * Binds a command to be started when the button is pressed, and cancelled
+ * when it is released. Takes a raw pointer, and so is non-owning; users are
+ * responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ Button WhenHeld(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to be started when the button is pressed, and cancelled
+ * when it is released. Transfers command ownership to the button scheduler,
+ * so the user does not have to worry about lifespan - rvalue refs will be
+ * *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Button WhenHeld(T&& command, bool interruptible = true) {
+ WhileActiveOnce(std::forward<T>(command), interruptible);
+ return *this;
+ }
+
+ /**
+ * Binds a command to start when the button is released. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ Button WhenReleased(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the button is pressed. Transfers
+ * command ownership to the button scheduler, so the user does not have to
+ * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+ * *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Button WhenReleased(T&& command, bool interruptible = true) {
+ WhenInactive(std::forward<T>(command), interruptible);
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute when the button is released.
+ *
+ * @param toRun the runnable to execute.
+ */
+ Button WhenReleased(std::function<void()> toRun);
+
+ /**
+ * Binds a command to start when the button is pressed, and be cancelled when
+ * it is pressed again. Takes a raw pointer, and so is non-owning; users are
+ * responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ Button ToggleWhenPressed(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the button is pressed, and be cancelled when
+ * it is pessed again. Transfers command ownership to the button scheduler,
+ * so the user does not have to worry about lifespan - rvalue refs will be
+ * *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The button, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Button ToggleWhenPressed(T&& command, bool interruptible = true) {
+ ToggleWhenActive(std::forward<T>(command), interruptible);
+ return *this;
+ }
+
+ /**
+ * Binds a command to be cancelled when the button is pressed. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * and scheduling of the command.
+ *
+ * @param command The command to bind.
+ * @return The button, for chained calls.
+ */
+ Button CancelWhenPressed(Command* command);
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h b/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h
new file mode 100644
index 0000000..a23738b
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <frc/GenericHID.h>
+
+#include "Button.h"
+
+namespace frc2 {
+/**
+ * A class used to bind command scheduling to joystick button presses. Can be
+ * composed with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class JoystickButton : public Button {
+ public:
+ /**
+ * Creates a JoystickButton that commands can be bound to.
+ *
+ * @param joystick The joystick on which the button is located.
+ * @param buttonNumber The number of the button on the joystic.
+ */
+ explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber)
+ : m_joystick{joystick}, m_buttonNumber{buttonNumber} {}
+
+ bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); }
+
+ private:
+ frc::GenericHID* m_joystick;
+ int m_buttonNumber;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/POVButton.h b/wpilibc/src/main/native/include/frc2/command/button/POVButton.h
new file mode 100644
index 0000000..758cab2
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/button/POVButton.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+#include <frc/GenericHID.h>
+
+#include "Button.h"
+
+namespace frc2 {
+/**
+ * A class used to bind command scheduling to joystick POV presses. Can be
+ * composed with other buttons with the operators in Trigger.
+ *
+ * @see Trigger
+ */
+class POVButton : public Button {
+ public:
+ /**
+ * Creates a POVButton that commands can be bound to.
+ *
+ * @param joystick The joystick on which the button is located.
+ * @param angle The angle of the POV corresponding to a button press.
+ * @param povNumber The number of the POV on the joystick.
+ */
+ POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0)
+ : m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {}
+
+ bool Get() const override {
+ return m_joystick->GetPOV(m_povNumber) == m_angle;
+ }
+
+ private:
+ frc::GenericHID* m_joystick;
+ int m_angle;
+ int m_povNumber;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/Trigger.h b/wpilibc/src/main/native/include/frc2/command/button/Trigger.h
new file mode 100644
index 0000000..1a65ff6
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc2/command/button/Trigger.h
@@ -0,0 +1,325 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandScheduler.h>
+
+#include <atomic>
+#include <memory>
+#include <utility>
+
+namespace frc2 {
+class Command;
+/**
+ * A class used to bind command scheduling to events. The
+ * Trigger class is a base for all command-event-binding classes, and so the
+ * methods are named fairly abstractly; for purpose-specific wrappers, see
+ * Button.
+ *
+ * @see Button
+ */
+class Trigger {
+ public:
+ /**
+ * Create a new trigger that is active when the given condition is true.
+ *
+ * @param isActive Whether the trigger is active.
+ */
+ explicit Trigger(std::function<bool()> isActive)
+ : m_isActive{std::move(isActive)} {}
+
+ /**
+ * Create a new trigger that is never active (default constructor) - activity
+ * can be further determined by subclass code.
+ */
+ Trigger() {
+ m_isActive = [] { return false; };
+ }
+
+ Trigger(const Trigger& other);
+
+ /**
+ * Returns whether the trigger is active. Can be overridden by a subclass.
+ *
+ * @return Whether the trigger is active.
+ */
+ virtual bool Get() const { return m_isActive(); }
+
+ /**
+ * Binds a command to start when the trigger becomes active. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Trigger WhenActive(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the trigger becomes active. Transfers
+ * command ownership to the button scheduler, so the user does not have to
+ * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+ * *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Trigger WhenActive(T&& command, bool interruptible = true) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this,
+ command = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command)),
+ interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ command->Schedule(interruptible);
+ }
+
+ pressedLast = pressed;
+ });
+
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute when the trigger becomes active.
+ *
+ * @param toRun the runnable to execute.
+ */
+ Trigger WhenActive(std::function<void()> toRun);
+
+ /**
+ * Binds a command to be started repeatedly while the trigger is active, and
+ * cancelled when it becomes inactive. Takes a raw pointer, and so is
+ * non-owning; users are responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Trigger WhileActiveContinous(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to be started repeatedly while the trigger is active, and
+ * cancelled when it becomes inactive. Transfers command ownership to the
+ * button scheduler, so the user does not have to worry about lifespan -
+ * rvalue refs will be *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this,
+ command = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command)),
+ interruptible]() mutable {
+ bool pressed = Get();
+
+ if (pressed) {
+ command->Schedule(interruptible);
+ } else if (pressedLast && !pressed) {
+ command->Cancel();
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute repeatedly while the trigger is active.
+ *
+ * @param toRun the runnable to execute.
+ */
+ Trigger WhileActiveContinous(std::function<void()> toRun);
+
+ /**
+ * Binds a command to be started when the trigger becomes active, and
+ * cancelled when it becomes inactive. Takes a raw pointer, and so is
+ * non-owning; users are responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Trigger WhileActiveOnce(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to be started when the trigger becomes active, and
+ * cancelled when it becomes inactive. Transfers command ownership to the
+ * button scheduler, so the user does not have to worry about lifespan -
+ * rvalue refs will be *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this,
+ command = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command)),
+ interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ command->Schedule(interruptible);
+ } else if (pressedLast && !pressed) {
+ command->Cancel();
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+ }
+
+ /**
+ * Binds a command to start when the trigger becomes inactive. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Trigger WhenInactive(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the trigger becomes inactive. Transfers
+ * command ownership to the button scheduler, so the user does not have to
+ * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
+ * *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Trigger WhenInactive(T&& command, bool interruptible = true) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this,
+ command = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command)),
+ interruptible]() mutable {
+ bool pressed = Get();
+
+ if (pressedLast && !pressed) {
+ command->Schedule(interruptible);
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+ }
+
+ /**
+ * Binds a runnable to execute when the trigger becomes inactive.
+ *
+ * @param toRun the runnable to execute.
+ */
+ Trigger WhenInactive(std::function<void()> toRun);
+
+ /**
+ * Binds a command to start when the trigger becomes active, and be cancelled
+ * when it again becomes active. Takes a raw pointer, and so is non-owning;
+ * users are responsible for the lifespan of the command.
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ Trigger ToggleWhenActive(Command* command, bool interruptible = true);
+
+ /**
+ * Binds a command to start when the trigger becomes active, and be cancelled
+ * when it again becomes active. Transfers command ownership to the button
+ * scheduler, so the user does not have to worry about lifespan - rvalue refs
+ * will be *moved*, lvalue refs will be *copied.*
+ *
+ * @param command The command to bind.
+ * @param interruptible Whether the command should be interruptible.
+ * @return The trigger, for chained calls.
+ */
+ template <class T, typename = std::enable_if_t<std::is_base_of_v<
+ Command, std::remove_reference_t<T>>>>
+ Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
+ CommandScheduler::GetInstance().AddButton(
+ [pressedLast = Get(), *this,
+ command = std::make_unique<std::remove_reference_t<T>>(
+ std::forward<T>(command)),
+ interruptible]() mutable {
+ bool pressed = Get();
+
+ if (!pressedLast && pressed) {
+ if (command->IsScheduled()) {
+ command->Cancel();
+ } else {
+ command->Schedule(interruptible);
+ }
+ }
+
+ pressedLast = pressed;
+ });
+ return *this;
+ }
+
+ /**
+ * Binds a command to be cancelled when the trigger becomes active. Takes a
+ * raw pointer, and so is non-owning; users are responsible for the lifespan
+ * and scheduling of the command.
+ *
+ * @param command The command to bind.
+ * @return The trigger, for chained calls.
+ */
+ Trigger CancelWhenActive(Command* command);
+
+ /**
+ * Composes two triggers with logical AND.
+ *
+ * @return A trigger which is active when both component triggers are active.
+ */
+ Trigger operator&&(Trigger rhs) {
+ return Trigger([*this, rhs] { return Get() && rhs.Get(); });
+ }
+
+ /**
+ * Composes two triggers with logical OR.
+ *
+ * @return A trigger which is active when either component trigger is active.
+ */
+ Trigger operator||(Trigger rhs) {
+ return Trigger([*this, rhs] { return Get() || rhs.Get(); });
+ }
+
+ /**
+ * Composes a trigger with logical NOT.
+ *
+ * @return A trigger which is active when the component trigger is inactive,
+ * and vice-versa.
+ */
+ Trigger operator!() {
+ return Trigger([*this] { return !Get(); });
+ }
+
+ private:
+ std::function<bool()> m_isActive;
+};
+} // namespace frc2
diff --git a/wpilibc/src/main/native/include/interfaces/Accelerometer.h b/wpilibc/src/main/native/include/interfaces/Accelerometer.h
deleted file mode 100644
index 74b3c38..0000000
--- a/wpilibc/src/main/native/include/interfaces/Accelerometer.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: interfaces/Accelerometer.h is deprecated; include frc/interfaces/Accelerometer.h instead"
-#else
-#warning "interfaces/Accelerometer.h is deprecated; include frc/interfaces/Accelerometer.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/interfaces/Accelerometer.h"
diff --git a/wpilibc/src/main/native/include/interfaces/Gyro.h b/wpilibc/src/main/native/include/interfaces/Gyro.h
deleted file mode 100644
index f88ec5d..0000000
--- a/wpilibc/src/main/native/include/interfaces/Gyro.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: interfaces/Gyro.h is deprecated; include frc/interfaces/Gyro.h instead"
-#else
-#warning "interfaces/Gyro.h is deprecated; include frc/interfaces/Gyro.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/interfaces/Gyro.h"
diff --git a/wpilibc/src/main/native/include/interfaces/Potentiometer.h b/wpilibc/src/main/native/include/interfaces/Potentiometer.h
deleted file mode 100644
index d73d3db..0000000
--- a/wpilibc/src/main/native/include/interfaces/Potentiometer.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// clang-format off
-#ifdef _MSC_VER
-#pragma message "warning: interfaces/Potentiometer.h is deprecated; include frc/interfaces/Potentiometer.h instead"
-#else
-#warning "interfaces/Potentiometer.h is deprecated; include frc/interfaces/Potentiometer.h instead"
-#endif
-
-// clang-format on
-
-#include "frc/interfaces/Potentiometer.h"
diff --git a/wpilibc/src/test/native/cpp/CircularBufferTest.cpp b/wpilibc/src/test/native/cpp/CircularBufferTest.cpp
deleted file mode 100644
index ada4f9a..0000000
--- a/wpilibc/src/test/native/cpp/CircularBufferTest.cpp
+++ /dev/null
@@ -1,211 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/circular_buffer.h" // NOLINT(build/include_order)
-
-#include <array>
-
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static const std::array<double, 10> values = {
- {751.848, 766.366, 342.657, 234.252, 716.126, 132.344, 445.697, 22.727,
- 421.125, 799.913}};
-
-static const std::array<double, 8> pushFrontOut = {
- {799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657}};
-
-static const std::array<double, 8> pushBackOut = {
- {342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913}};
-
-TEST(CircularBufferTest, PushFrontTest) {
- circular_buffer<double> queue(8);
-
- for (auto& value : values) {
- queue.push_front(value);
- }
-
- for (size_t i = 0; i < pushFrontOut.size(); i++) {
- EXPECT_EQ(pushFrontOut[i], queue[i]);
- }
-}
-
-TEST(CircularBufferTest, PushBackTest) {
- circular_buffer<double> queue(8);
-
- for (auto& value : values) {
- queue.push_back(value);
- }
-
- for (size_t i = 0; i < pushBackOut.size(); i++) {
- EXPECT_EQ(pushBackOut[i], queue[i]);
- }
-}
-
-TEST(CircularBufferTest, PushPopTest) {
- circular_buffer<double> queue(3);
-
- // Insert three elements into the buffer
- queue.push_back(1.0);
- queue.push_back(2.0);
- queue.push_back(3.0);
-
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
- EXPECT_EQ(3.0, queue[2]);
-
- /*
- * The buffer is full now, so pushing subsequent elements will overwrite the
- * front-most elements.
- */
-
- queue.push_back(4.0); // Overwrite 1 with 4
-
- // The buffer now contains 2, 3 and 4
- EXPECT_EQ(2.0, queue[0]);
- EXPECT_EQ(3.0, queue[1]);
- EXPECT_EQ(4.0, queue[2]);
-
- queue.push_back(5.0); // Overwrite 2 with 5
-
- // The buffer now contains 3, 4 and 5
- EXPECT_EQ(3.0, queue[0]);
- EXPECT_EQ(4.0, queue[1]);
- EXPECT_EQ(5.0, queue[2]);
-
- EXPECT_EQ(5.0, queue.pop_back()); // 5 is removed
-
- // The buffer now contains 3 and 4
- EXPECT_EQ(3.0, queue[0]);
- EXPECT_EQ(4.0, queue[1]);
-
- EXPECT_EQ(3.0, queue.pop_front()); // 3 is removed
-
- // Leaving only one element with value == 4
- EXPECT_EQ(4.0, queue[0]);
-}
-
-TEST(CircularBufferTest, ResetTest) {
- circular_buffer<double> queue(5);
-
- for (size_t i = 1; i < 6; i++) {
- queue.push_back(i);
- }
-
- queue.reset();
-
- for (size_t i = 0; i < 5; i++) {
- EXPECT_EQ(0.0, queue[i]);
- }
-}
-
-TEST(CircularBufferTest, ResizeTest) {
- circular_buffer<double> queue(5);
-
- /* Buffer contains {1, 2, 3, _, _}
- * ^ front
- */
- queue.push_back(1.0);
- queue.push_back(2.0);
- queue.push_back(3.0);
-
- queue.resize(2);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
-
- queue.resize(5);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
-
- queue.reset();
-
- /* Buffer contains {_, 1, 2, 3, _}
- * ^ front
- */
- queue.push_back(0.0);
- queue.push_back(1.0);
- queue.push_back(2.0);
- queue.push_back(3.0);
- queue.pop_front();
-
- queue.resize(2);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
-
- queue.resize(5);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
-
- queue.reset();
-
- /* Buffer contains {_, _, 1, 2, 3}
- * ^ front
- */
- queue.push_back(0.0);
- queue.push_back(0.0);
- queue.push_back(1.0);
- queue.push_back(2.0);
- queue.push_back(3.0);
- queue.pop_front();
- queue.pop_front();
-
- queue.resize(2);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
-
- queue.resize(5);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
-
- queue.reset();
-
- /* Buffer contains {3, _, _, 1, 2}
- * ^ front
- */
- queue.push_back(3.0);
- queue.push_front(2.0);
- queue.push_front(1.0);
-
- queue.resize(2);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
-
- queue.resize(5);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
-
- queue.reset();
-
- /* Buffer contains {2, 3, _, _, 1}
- * ^ front
- */
- queue.push_back(2.0);
- queue.push_back(3.0);
- queue.push_front(1.0);
-
- queue.resize(2);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
-
- queue.resize(5);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
-
- // Test push_back() after resize
- queue.push_back(3.0);
- EXPECT_EQ(1.0, queue[0]);
- EXPECT_EQ(2.0, queue[1]);
- EXPECT_EQ(3.0, queue[2]);
-
- // Test push_front() after resize
- queue.push_front(4.0);
- EXPECT_EQ(4.0, queue[0]);
- EXPECT_EQ(1.0, queue[1]);
- EXPECT_EQ(2.0, queue[2]);
- EXPECT_EQ(3.0, queue[3]);
-}
diff --git a/wpilibc/src/test/native/cpp/FilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/FilterNoiseTest.cpp
deleted file mode 100644
index ea14f32..0000000
--- a/wpilibc/src/test/native/cpp/FilterNoiseTest.cpp
+++ /dev/null
@@ -1,137 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/filters/LinearDigitalFilter.h" // NOLINT(build/include_order)
-
-#include <cmath>
-#include <functional>
-#include <memory>
-#include <random>
-#include <thread>
-
-#include "frc/Base.h"
-#include "gtest/gtest.h"
-
-/* Filter constants */
-static constexpr double kFilterStep = 0.005;
-static constexpr double kFilterTime = 2.0;
-static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
-static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
-static constexpr double kHighPassTimeConstant = 0.006631;
-static constexpr double kHighPassExpectedOutput = 10.074717;
-static constexpr int32_t kMovAvgTaps = 6;
-static constexpr double kMovAvgExpectedOutput = -10.191644;
-static constexpr double kPi = 3.14159265358979323846;
-
-using namespace frc;
-
-enum FilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
-
-std::ostream& operator<<(std::ostream& os, const FilterNoiseTestType& type) {
- switch (type) {
- case TEST_SINGLE_POLE_IIR:
- os << "LinearDigitalFilter SinglePoleIIR";
- break;
- case TEST_MOVAVG:
- os << "LinearDigitalFilter MovingAverage";
- break;
- }
-
- return os;
-}
-
-constexpr double kStdDev = 10.0;
-
-/**
- * Adds Gaussian white noise to a function returning data. The noise will have
- * the standard deviation provided in the constructor.
- */
-class NoiseGenerator : public PIDSource {
- public:
- NoiseGenerator(double (*dataFunc)(double), double stdDev)
- : m_distr(0.0, stdDev) {
- m_dataFunc = dataFunc;
- }
-
- void SetPIDSourceType(PIDSourceType pidSource) override {}
-
- double Get() { return m_dataFunc(m_count) + m_noise; }
-
- double PIDGet() override {
- m_noise = m_distr(m_gen);
- m_count += kFilterStep;
- return m_dataFunc(m_count) + m_noise;
- }
-
- void Reset() { m_count = -kFilterStep; }
-
- private:
- std::function<double(double)> m_dataFunc;
- double m_noise = 0.0;
-
- // Make sure first call to PIDGet() uses m_count == 0
- double m_count = -kFilterStep;
-
- std::random_device m_rd;
- std::mt19937 m_gen{m_rd()};
- std::normal_distribution<double> m_distr;
-};
-
-/**
- * A fixture that includes a noise generator wrapped in a filter
- */
-class FilterNoiseTest : public testing::TestWithParam<FilterNoiseTestType> {
- protected:
- std::unique_ptr<PIDSource> m_filter;
- std::shared_ptr<NoiseGenerator> m_noise;
-
- static double GetData(double t) { return 100.0 * std::sin(2.0 * kPi * t); }
-
- void SetUp() override {
- m_noise = std::make_shared<NoiseGenerator>(GetData, kStdDev);
-
- switch (GetParam()) {
- case TEST_SINGLE_POLE_IIR: {
- m_filter = std::make_unique<LinearDigitalFilter>(
- LinearDigitalFilter::SinglePoleIIR(
- m_noise, kSinglePoleIIRTimeConstant, kFilterStep));
- break;
- }
-
- case TEST_MOVAVG: {
- m_filter = std::make_unique<LinearDigitalFilter>(
- LinearDigitalFilter::MovingAverage(m_noise, kMovAvgTaps));
- break;
- }
- }
- }
-};
-
-/**
- * Test if the filter reduces the noise produced by a signal generator
- */
-TEST_P(FilterNoiseTest, NoiseReduce) {
- double theoryData = 0.0;
- double noiseGenError = 0.0;
- double filterError = 0.0;
-
- m_noise->Reset();
- for (double t = 0; t < kFilterTime; t += kFilterStep) {
- theoryData = GetData(t);
- filterError += std::abs(m_filter->PIDGet() - theoryData);
- noiseGenError += std::abs(m_noise->Get() - theoryData);
- }
-
- RecordProperty("FilterError", filterError);
-
- // The filter should have produced values closer to the theory
- EXPECT_GT(noiseGenError, filterError)
- << "Filter should have reduced noise accumulation but failed";
-}
-
-INSTANTIATE_TEST_CASE_P(Test, FilterNoiseTest,
- testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG), );
diff --git a/wpilibc/src/test/native/cpp/FilterOutputTest.cpp b/wpilibc/src/test/native/cpp/FilterOutputTest.cpp
deleted file mode 100644
index ec71760..0000000
--- a/wpilibc/src/test/native/cpp/FilterOutputTest.cpp
+++ /dev/null
@@ -1,157 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/filters/LinearDigitalFilter.h" // NOLINT(build/include_order)
-
-#include <cmath>
-#include <functional>
-#include <memory>
-#include <random>
-#include <thread>
-
-#include "frc/Base.h"
-#include "gtest/gtest.h"
-
-/* Filter constants */
-static constexpr double kFilterStep = 0.005;
-static constexpr double kFilterTime = 2.0;
-static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
-static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
-static constexpr double kHighPassTimeConstant = 0.006631;
-static constexpr double kHighPassExpectedOutput = 10.074717;
-static constexpr int32_t kMovAvgTaps = 6;
-static constexpr double kMovAvgExpectedOutput = -10.191644;
-static constexpr double kPi = 3.14159265358979323846;
-
-using namespace frc;
-
-enum FilterOutputTestType {
- TEST_SINGLE_POLE_IIR,
- TEST_HIGH_PASS,
- TEST_MOVAVG,
- TEST_PULSE
-};
-
-std::ostream& operator<<(std::ostream& os, const FilterOutputTestType& type) {
- switch (type) {
- case TEST_SINGLE_POLE_IIR:
- os << "LinearDigitalFilter SinglePoleIIR";
- break;
- case TEST_HIGH_PASS:
- os << "LinearDigitalFilter HighPass";
- break;
- case TEST_MOVAVG:
- os << "LinearDigitalFilter MovingAverage";
- break;
- case TEST_PULSE:
- os << "LinearDigitalFilter Pulse";
- break;
- }
-
- return os;
-}
-
-class DataWrapper : public PIDSource {
- public:
- explicit DataWrapper(double (*dataFunc)(double)) { m_dataFunc = dataFunc; }
-
- virtual void SetPIDSourceType(PIDSourceType pidSource) {}
-
- virtual double PIDGet() {
- m_count += kFilterStep;
- return m_dataFunc(m_count);
- }
-
- void Reset() { m_count = -kFilterStep; }
-
- private:
- std::function<double(double)> m_dataFunc;
-
- // Make sure first call to PIDGet() uses m_count == 0
- double m_count = -kFilterStep;
-};
-
-/**
- * A fixture that includes a consistent data source wrapped in a filter
- */
-class FilterOutputTest : public testing::TestWithParam<FilterOutputTestType> {
- protected:
- std::unique_ptr<PIDSource> m_filter;
- std::shared_ptr<DataWrapper> m_data;
- double m_expectedOutput = 0.0;
-
- static double GetData(double t) {
- return 100.0 * std::sin(2.0 * kPi * t) + 20.0 * std::cos(50.0 * kPi * t);
- }
-
- static double GetPulseData(double t) {
- if (std::abs(t - 1.0) < 0.001) {
- return 1.0;
- } else {
- return 0.0;
- }
- }
-
- void SetUp() override {
- switch (GetParam()) {
- case TEST_SINGLE_POLE_IIR: {
- m_data = std::make_shared<DataWrapper>(GetData);
- m_filter = std::make_unique<LinearDigitalFilter>(
- LinearDigitalFilter::SinglePoleIIR(
- m_data, kSinglePoleIIRTimeConstant, kFilterStep));
- m_expectedOutput = kSinglePoleIIRExpectedOutput;
- break;
- }
-
- case TEST_HIGH_PASS: {
- m_data = std::make_shared<DataWrapper>(GetData);
- m_filter =
- std::make_unique<LinearDigitalFilter>(LinearDigitalFilter::HighPass(
- m_data, kHighPassTimeConstant, kFilterStep));
- m_expectedOutput = kHighPassExpectedOutput;
- break;
- }
-
- case TEST_MOVAVG: {
- m_data = std::make_shared<DataWrapper>(GetData);
- m_filter = std::make_unique<LinearDigitalFilter>(
- LinearDigitalFilter::MovingAverage(m_data, kMovAvgTaps));
- m_expectedOutput = kMovAvgExpectedOutput;
- break;
- }
-
- case TEST_PULSE: {
- m_data = std::make_shared<DataWrapper>(GetPulseData);
- m_filter = std::make_unique<LinearDigitalFilter>(
- LinearDigitalFilter::MovingAverage(m_data, kMovAvgTaps));
- m_expectedOutput = 0.0;
- break;
- }
- }
- }
-};
-
-/**
- * Test if the linear digital filters produce consistent output
- */
-TEST_P(FilterOutputTest, FilterOutput) {
- m_data->Reset();
-
- double filterOutput = 0.0;
- for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
- filterOutput = m_filter->PIDGet();
- }
-
- RecordProperty("FilterOutput", filterOutput);
-
- EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
- << "Filter output didn't match expected value";
-}
-
-INSTANTIATE_TEST_CASE_P(Test, FilterOutputTest,
- testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
- TEST_MOVAVG, TEST_PULSE), );
diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
new file mode 100644
index 0000000..888d183
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/LinearFilter.h" // NOLINT(build/include_order)
+
+#include <cmath>
+#include <memory>
+#include <random>
+
+#include <units/units.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+// Filter constants
+static constexpr units::second_t kFilterStep = 0.005_s;
+static constexpr units::second_t kFilterTime = 2.0_s;
+static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
+static constexpr int32_t kMovAvgTaps = 6;
+
+enum LinearFilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
+
+std::ostream& operator<<(std::ostream& os,
+ const LinearFilterNoiseTestType& type) {
+ switch (type) {
+ case TEST_SINGLE_POLE_IIR:
+ os << "LinearFilter SinglePoleIIR";
+ break;
+ case TEST_MOVAVG:
+ os << "LinearFilter MovingAverage";
+ break;
+ }
+
+ return os;
+}
+
+static double GetData(double t) {
+ return 100.0 * std::sin(2.0 * wpi::math::pi * t);
+}
+
+class LinearFilterNoiseTest
+ : public testing::TestWithParam<LinearFilterNoiseTestType> {
+ protected:
+ std::unique_ptr<frc::LinearFilter> m_filter;
+
+ void SetUp() override {
+ switch (GetParam()) {
+ case TEST_SINGLE_POLE_IIR: {
+ m_filter = std::make_unique<frc::LinearFilter>(
+ frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+ kFilterStep));
+ break;
+ }
+
+ case TEST_MOVAVG: {
+ m_filter = std::make_unique<frc::LinearFilter>(
+ frc::LinearFilter::MovingAverage(kMovAvgTaps));
+ break;
+ }
+ }
+ }
+};
+
+/**
+ * Test if the filter reduces the noise produced by a signal generator
+ */
+TEST_P(LinearFilterNoiseTest, NoiseReduce) {
+ double noiseGenError = 0.0;
+ double filterError = 0.0;
+
+ std::random_device rd;
+ std::mt19937 gen{rd()};
+ std::normal_distribution<double> distr{0.0, 10.0};
+
+ for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
+ double theory = GetData(t.to<double>());
+ double noise = distr(gen);
+ filterError += std::abs(m_filter->Calculate(theory + noise) - theory);
+ noiseGenError += std::abs(noise - theory);
+ }
+
+ RecordProperty("FilterError", filterError);
+
+ // The filter should have produced values closer to the theory
+ EXPECT_GT(noiseGenError, filterError)
+ << "Filter should have reduced noise accumulation but failed";
+}
+
+INSTANTIATE_TEST_SUITE_P(Test, LinearFilterNoiseTest,
+ testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));
diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
new file mode 100644
index 0000000..1f1476c
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/LinearFilter.h" // NOLINT(build/include_order)
+
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <random>
+
+#include <units/units.h>
+#include <wpi/math>
+
+#include "gtest/gtest.h"
+
+// Filter constants
+static constexpr units::second_t kFilterStep = 0.005_s;
+static constexpr units::second_t kFilterTime = 2.0_s;
+static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
+static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
+static constexpr double kHighPassTimeConstant = 0.006631;
+static constexpr double kHighPassExpectedOutput = 10.074717;
+static constexpr int32_t kMovAvgTaps = 6;
+static constexpr double kMovAvgExpectedOutput = -10.191644;
+
+enum LinearFilterOutputTestType {
+ TEST_SINGLE_POLE_IIR,
+ TEST_HIGH_PASS,
+ TEST_MOVAVG,
+ TEST_PULSE
+};
+
+std::ostream& operator<<(std::ostream& os,
+ const LinearFilterOutputTestType& type) {
+ switch (type) {
+ case TEST_SINGLE_POLE_IIR:
+ os << "LinearFilter SinglePoleIIR";
+ break;
+ case TEST_HIGH_PASS:
+ os << "LinearFilter HighPass";
+ break;
+ case TEST_MOVAVG:
+ os << "LinearFilter MovingAverage";
+ break;
+ case TEST_PULSE:
+ os << "LinearFilter Pulse";
+ break;
+ }
+
+ return os;
+}
+
+static double GetData(double t) {
+ return 100.0 * std::sin(2.0 * wpi::math::pi * t) +
+ 20.0 * std::cos(50.0 * wpi::math::pi * t);
+}
+
+static double GetPulseData(double t) {
+ if (std::abs(t - 1.0) < 0.001) {
+ return 1.0;
+ } else {
+ return 0.0;
+ }
+}
+
+/**
+ * A fixture that includes a consistent data source wrapped in a filter
+ */
+class LinearFilterOutputTest
+ : public testing::TestWithParam<LinearFilterOutputTestType> {
+ protected:
+ std::unique_ptr<frc::LinearFilter> m_filter;
+ std::function<double(double)> m_data;
+ double m_expectedOutput = 0.0;
+
+ void SetUp() override {
+ switch (GetParam()) {
+ case TEST_SINGLE_POLE_IIR: {
+ m_filter = std::make_unique<frc::LinearFilter>(
+ frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+ kFilterStep));
+ m_data = GetData;
+ m_expectedOutput = kSinglePoleIIRExpectedOutput;
+ break;
+ }
+
+ case TEST_HIGH_PASS: {
+ m_filter = std::make_unique<frc::LinearFilter>(
+ frc::LinearFilter::HighPass(kHighPassTimeConstant, kFilterStep));
+ m_data = GetData;
+ m_expectedOutput = kHighPassExpectedOutput;
+ break;
+ }
+
+ case TEST_MOVAVG: {
+ m_filter = std::make_unique<frc::LinearFilter>(
+ frc::LinearFilter::MovingAverage(kMovAvgTaps));
+ m_data = GetData;
+ m_expectedOutput = kMovAvgExpectedOutput;
+ break;
+ }
+
+ case TEST_PULSE: {
+ m_filter = std::make_unique<frc::LinearFilter>(
+ frc::LinearFilter::MovingAverage(kMovAvgTaps));
+ m_data = GetPulseData;
+ m_expectedOutput = 0.0;
+ break;
+ }
+ }
+ }
+};
+
+/**
+ * Test if the linear filters produce consistent output for a given data set.
+ */
+TEST_P(LinearFilterOutputTest, Output) {
+ double filterOutput = 0.0;
+ for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
+ filterOutput = m_filter->Calculate(m_data(t.to<double>()));
+ }
+
+ RecordProperty("LinearFilterOutput", filterOutput);
+
+ EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
+ << "Filter output didn't match expected value";
+}
+
+INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest,
+ testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
+ TEST_MOVAVG, TEST_PULSE));
diff --git a/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp b/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp
index a6f5028..cb81fc9 100644
--- a/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp
+++ b/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -132,5 +132,5 @@
}
}
-INSTANTIATE_TEST_CASE_P(Test, SpeedControllerGroupTest,
- testing::Values(TEST_ONE, TEST_TWO, TEST_THREE), );
+INSTANTIATE_TEST_SUITE_P(Test, SpeedControllerGroupTest,
+ testing::Values(TEST_ONE, TEST_TWO, TEST_THREE));
diff --git a/wpilibc/src/test/native/cpp/WatchdogTest.cpp b/wpilibc/src/test/native/cpp/WatchdogTest.cpp
index c796116..10ff996 100644
--- a/wpilibc/src/test/native/cpp/WatchdogTest.cpp
+++ b/wpilibc/src/test/native/cpp/WatchdogTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -17,10 +17,14 @@
using namespace frc;
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_EnableDisable) {
+#else
TEST(WatchdogTest, EnableDisable) {
+#endif
uint32_t watchdogCounter = 0;
- Watchdog watchdog(0.4, [&] { watchdogCounter++; });
+ Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
wpi::outs() << "Run 1\n";
watchdog.Enable();
@@ -48,10 +52,14 @@
<< "Watchdog either didn't trigger or triggered more than once";
}
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_Reset) {
+#else
TEST(WatchdogTest, Reset) {
+#endif
uint32_t watchdogCounter = 0;
- Watchdog watchdog(0.4, [&] { watchdogCounter++; });
+ Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
watchdog.Enable();
std::this_thread::sleep_for(std::chrono::milliseconds(200));
@@ -62,14 +70,18 @@
EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
}
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_SetTimeout) {
+#else
TEST(WatchdogTest, SetTimeout) {
+#endif
uint32_t watchdogCounter = 0;
- Watchdog watchdog(1.0, [&] { watchdogCounter++; });
+ Watchdog watchdog(1.0_s, [&] { watchdogCounter++; });
watchdog.Enable();
std::this_thread::sleep_for(std::chrono::milliseconds(200));
- watchdog.SetTimeout(0.2);
+ watchdog.SetTimeout(0.2_s);
EXPECT_EQ(0.2, watchdog.GetTimeout());
EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
@@ -81,8 +93,12 @@
<< "Watchdog either didn't trigger or triggered more than once";
}
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_IsExpired) {
+#else
TEST(WatchdogTest, IsExpired) {
- Watchdog watchdog(0.2, [] {});
+#endif
+ Watchdog watchdog(0.2_s, [] {});
EXPECT_FALSE(watchdog.IsExpired());
watchdog.Enable();
@@ -97,10 +113,14 @@
EXPECT_FALSE(watchdog.IsExpired());
}
+#ifdef __APPLE__
+TEST(WatchdogTest, DISABLED_Epochs) {
+#else
TEST(WatchdogTest, Epochs) {
+#endif
uint32_t watchdogCounter = 0;
- Watchdog watchdog(0.4, [&] { watchdogCounter++; });
+ Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
wpi::outs() << "Run 1\n";
watchdog.Enable();
@@ -133,8 +153,8 @@
uint32_t watchdogCounter1 = 0;
uint32_t watchdogCounter2 = 0;
- Watchdog watchdog1(0.2, [&] { watchdogCounter1++; });
- Watchdog watchdog2(0.6, [&] { watchdogCounter2++; });
+ Watchdog watchdog1(0.2_s, [&] { watchdogCounter1++; });
+ Watchdog watchdog2(0.6_s, [&] { watchdogCounter2++; });
watchdog2.Enable();
std::this_thread::sleep_for(std::chrono::milliseconds(200));
diff --git a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
new file mode 100644
index 0000000..7bdac2e
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/PIDController.h"
+#include "gtest/gtest.h"
+
+class PIDInputOutputTest : public testing::Test {
+ protected:
+ frc2::PIDController* controller;
+
+ void SetUp() override { controller = new frc2::PIDController(0, 0, 0); }
+
+ void TearDown() override { delete controller; }
+};
+
+TEST_F(PIDInputOutputTest, ContinuousInputTest) {
+ controller->SetP(1);
+ controller->EnableContinuousInput(-180, 180);
+
+ EXPECT_TRUE(controller->Calculate(-179, 179) < 0);
+}
+
+TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {
+ controller->SetP(4);
+
+ EXPECT_DOUBLE_EQ(-.1, controller->Calculate(.025, 0));
+}
+
+TEST_F(PIDInputOutputTest, IntegralGainOutputTest) {
+ controller->SetI(4);
+
+ double out = 0;
+
+ for (int i = 0; i < 5; i++) {
+ out = controller->Calculate(.025, 0);
+ }
+
+ EXPECT_DOUBLE_EQ(-.5 * controller->GetPeriod().to<double>(), out);
+}
+
+TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) {
+ controller->SetD(4);
+
+ controller->Calculate(0, 0);
+
+ EXPECT_DOUBLE_EQ(-.01_s / controller->GetPeriod(),
+ controller->Calculate(.0025, 0));
+}
diff --git a/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp
new file mode 100644
index 0000000..3251098
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/PIDController.h"
+#include "gtest/gtest.h"
+
+static constexpr double kSetpoint = 50.0;
+static constexpr double kRange = 200;
+static constexpr double kTolerance = 10.0;
+
+TEST(PIDToleranceTest, InitialTolerance) {
+ frc2::PIDController controller{0.5, 0.0, 0.0};
+ controller.EnableContinuousInput(-kRange / 2, kRange / 2);
+
+ EXPECT_TRUE(controller.AtSetpoint());
+}
+
+TEST(PIDToleranceTest, AbsoluteTolerance) {
+ frc2::PIDController controller{0.5, 0.0, 0.0};
+ controller.EnableContinuousInput(-kRange / 2, kRange / 2);
+
+ controller.SetTolerance(kTolerance);
+ controller.SetSetpoint(kSetpoint);
+
+ controller.Calculate(0.0);
+
+ EXPECT_FALSE(controller.AtSetpoint())
+ << "Error was in tolerance when it should not have been. Error was "
+ << controller.GetPositionError();
+
+ controller.Calculate(kSetpoint + kTolerance / 2);
+
+ EXPECT_TRUE(controller.AtSetpoint())
+ << "Error was not in tolerance when it should have been. Error was "
+ << controller.GetPositionError();
+
+ controller.Calculate(kSetpoint + 10 * kTolerance);
+
+ EXPECT_FALSE(controller.AtSetpoint())
+ << "Error was in tolerance when it should not have been. Error was "
+ << controller.GetPositionError();
+}
diff --git a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
new file mode 100644
index 0000000..a600054
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/math>
+
+#include "frc/controller/RamseteController.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0};
+
+units::radian_t boundRadians(units::radian_t value) {
+ while (value > units::radian_t{wpi::math::pi}) {
+ value -= units::radian_t{wpi::math::pi * 2};
+ }
+ while (value <= units::radian_t{-wpi::math::pi}) {
+ value += units::radian_t{wpi::math::pi * 2};
+ }
+ return value;
+}
+
+TEST(RamseteControllerTest, ReachesReference) {
+ frc::RamseteController controller{2.0, 0.7};
+ frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+
+ auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+ frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ waypoints, {8.8_mps, 0.1_mps_sq});
+
+ constexpr auto kDt = 0.02_s;
+ auto totalTime = trajectory.TotalTime();
+ for (size_t i = 0; i < (totalTime / kDt).to<double>(); ++i) {
+ auto state = trajectory.Sample(kDt * i);
+ auto [vx, vy, omega] = controller.Calculate(robotPose, state);
+ static_cast<void>(vy);
+
+ robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, omega * kDt});
+ }
+
+ auto& endPose = trajectory.States().back().pose;
+ EXPECT_NEAR_UNITS(endPose.Translation().X(), robotPose.Translation().X(),
+ kTolerance);
+ EXPECT_NEAR_UNITS(endPose.Translation().Y(), robotPose.Translation().Y(),
+ kTolerance);
+ EXPECT_NEAR_UNITS(boundRadians(endPose.Rotation().Radians() -
+ robotPose.Rotation().Radians()),
+ 0_rad, kAngularTolerance);
+}
diff --git a/wpilibc/src/test/native/cpp/RobotDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/DriveTest.cpp
similarity index 95%
rename from wpilibc/src/test/native/cpp/RobotDriveTest.cpp
rename to wpilibc/src/test/native/cpp/drive/DriveTest.cpp
index 464a707..1ebb6b3 100644
--- a/wpilibc/src/test/native/cpp/RobotDriveTest.cpp
+++ b/wpilibc/src/test/native/cpp/drive/DriveTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,7 +13,7 @@
using namespace frc;
-class RobotDriveTest : public testing::Test {
+class DriveTest : public testing::Test {
protected:
MockSpeedController m_rdFrontLeft;
MockSpeedController m_rdRearLeft;
@@ -36,7 +36,7 @@
-225, -270, -305, -360, -540};
};
-TEST_F(RobotDriveTest, TankDrive) {
+TEST_F(DriveTest, TankDrive) {
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
double leftJoystick, rightJoystick;
m_differentialDrive.SetDeadband(0.0);
@@ -55,7 +55,7 @@
}
}
-TEST_F(RobotDriveTest, TankDriveSquared) {
+TEST_F(DriveTest, TankDriveSquared) {
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
double leftJoystick, rightJoystick;
m_differentialDrive.SetDeadband(0.0);
@@ -74,7 +74,7 @@
}
}
-TEST_F(RobotDriveTest, ArcadeDriveSquared) {
+TEST_F(DriveTest, ArcadeDriveSquared) {
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
double moveJoystick, rotateJoystick;
m_differentialDrive.SetDeadband(0.0);
@@ -93,7 +93,7 @@
}
}
-TEST_F(RobotDriveTest, ArcadeDrive) {
+TEST_F(DriveTest, ArcadeDrive) {
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
double moveJoystick, rotateJoystick;
m_differentialDrive.SetDeadband(0.0);
@@ -112,7 +112,7 @@
}
}
-TEST_F(RobotDriveTest, MecanumCartesian) {
+TEST_F(DriveTest, MecanumCartesian) {
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
double xJoystick, yJoystick, rotateJoystick, gyroValue;
@@ -150,7 +150,7 @@
}
}
-TEST_F(RobotDriveTest, MecanumPolar) {
+TEST_F(DriveTest, MecanumPolar) {
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
double magnitudeJoystick, directionJoystick, rotateJoystick;
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp
new file mode 100644
index 0000000..829f0d2
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp
@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/Trigger.h"
+#include "gtest/gtest.h"
+
+using namespace frc2;
+class ButtonTest : public CommandTestBase {};
+
+TEST_F(ButtonTest, WhenPressedTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ Trigger([&pressed] { return pressed; }).WhenActive(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenReleasedTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = false;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ pressed = true;
+ Trigger([&pressed] { return pressed; }).WhenInactive(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = false;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhileHeldTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = false;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ pressed = false;
+ Trigger([&pressed] { return pressed; }).WhileActiveContinous(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ finished = false;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ pressed = false;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenHeldTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = false;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ pressed = false;
+ Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ finished = false;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+ pressed = false;
+ Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+ pressed = true;
+ scheduler.Run();
+ pressed = false;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, ToggleWhenPressedTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = false;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ pressed = false;
+ Trigger([&pressed] { return pressed; }).ToggleWhenActive(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ pressed = false;
+ scheduler.Run();
+ pressed = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, AndTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed1 = false;
+ bool pressed2 = false;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ (Trigger([&pressed1] { return pressed1; }) &&
+ Trigger([&pressed2] { return pressed2; }))
+ .WhenActive(&command);
+ pressed1 = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed2 = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, OrTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed1 = false;
+ bool pressed2 = false;
+ WaitUntilCommand command1([&finished] { return finished; });
+ WaitUntilCommand command2([&finished] { return finished; });
+
+ (Trigger([&pressed1] { return pressed1; }) ||
+ Trigger([&pressed2] { return pressed2; }))
+ .WhenActive(&command1);
+ pressed1 = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command1));
+
+ pressed1 = false;
+
+ (Trigger([&pressed1] { return pressed1; }) ||
+ Trigger([&pressed2] { return pressed2; }))
+ .WhenActive(&command2);
+ pressed2 = true;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(ButtonTest, NegateTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ bool finished = false;
+ bool pressed = true;
+ WaitUntilCommand command([&finished] { return finished; });
+
+ (!Trigger([&pressed] { return pressed; })).WhenActive(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ pressed = false;
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, RValueButtonTest) {
+ auto& scheduler = CommandScheduler::GetInstance();
+ int counter = 0;
+ bool pressed = false;
+
+ RunCommand command([&counter] { counter++; }, {});
+
+ Trigger([&pressed] { return pressed; }).WhenActive(std::move(command));
+ scheduler.Run();
+ EXPECT_EQ(counter, 0);
+ pressed = true;
+ scheduler.Run();
+ EXPECT_EQ(counter, 1);
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
new file mode 100644
index 0000000..0fe0b5b
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PerpetualCommand.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class CommandDecoratorTest : public CommandTestBase {};
+
+TEST_F(CommandDecoratorTest, WithTimeoutTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ auto command = RunCommand([] {}, {}).WithTimeout(.1_s);
+
+ scheduler.Schedule(&command);
+
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(150));
+
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, WithInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ auto command =
+ RunCommand([] {}, {}).WithInterrupt([&finished] { return finished; });
+
+ scheduler.Schedule(&command);
+
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+ finished = true;
+
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, BeforeStartingTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ auto command = InstantCommand([] {}, {}).BeforeStarting(
+ [&finished] { finished = true; });
+
+ scheduler.Schedule(&command);
+
+ EXPECT_TRUE(finished);
+
+ scheduler.Run();
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, AndThenTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ auto command =
+ InstantCommand([] {}, {}).AndThen([&finished] { finished = true; });
+
+ scheduler.Schedule(&command);
+
+ EXPECT_FALSE(finished);
+
+ scheduler.Run();
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ EXPECT_TRUE(finished);
+}
+
+TEST_F(CommandDecoratorTest, PerpetuallyTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ auto command = InstantCommand([] {}, {}).Perpetually();
+
+ scheduler.Schedule(&command);
+
+ scheduler.Run();
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
new file mode 100644
index 0000000..2600430
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SelectCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class CommandRequirementsTest : public CommandTestBase {};
+
+TEST_F(CommandRequirementsTest, RequirementInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement;
+
+ MockCommand command1({&requirement});
+ MockCommand command2({&requirement});
+
+ EXPECT_CALL(command1, Initialize());
+ EXPECT_CALL(command1, Execute());
+ EXPECT_CALL(command1, End(true));
+ EXPECT_CALL(command1, End(false)).Times(0);
+
+ EXPECT_CALL(command2, Initialize());
+ EXPECT_CALL(command2, Execute());
+ EXPECT_CALL(command2, End(true)).Times(0);
+ EXPECT_CALL(command2, End(false)).Times(0);
+
+ scheduler.Schedule(&command1);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command1));
+ scheduler.Schedule(&command2);
+ EXPECT_FALSE(scheduler.IsScheduled(&command1));
+ EXPECT_TRUE(scheduler.IsScheduled(&command2));
+ scheduler.Run();
+}
+
+TEST_F(CommandRequirementsTest, RequirementUninterruptibleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement;
+
+ MockCommand command1({&requirement});
+ MockCommand command2({&requirement});
+
+ EXPECT_CALL(command1, Initialize());
+ EXPECT_CALL(command1, Execute()).Times(2);
+ EXPECT_CALL(command1, End(true)).Times(0);
+ EXPECT_CALL(command1, End(false)).Times(0);
+
+ EXPECT_CALL(command2, Initialize()).Times(0);
+ EXPECT_CALL(command2, Execute()).Times(0);
+ EXPECT_CALL(command2, End(true)).Times(0);
+ EXPECT_CALL(command2, End(false)).Times(0);
+
+ scheduler.Schedule(false, &command1);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command1));
+ scheduler.Schedule(&command2);
+ EXPECT_TRUE(scheduler.IsScheduled(&command1));
+ EXPECT_FALSE(scheduler.IsScheduled(&command2));
+ scheduler.Run();
+}
+
+TEST_F(CommandRequirementsTest, DefaultCommandRequirementErrorTest) {
+ TestSubsystem requirement1;
+ ErrorConfirmer confirmer("require");
+
+ MockCommand command1;
+
+ requirement1.SetDefaultCommand(std::move(command1));
+
+ EXPECT_TRUE(requirement1.GetDefaultCommand() == NULL);
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
new file mode 100644
index 0000000..6572a98
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+
+using namespace frc2;
+class CommandScheduleTest : public CommandTestBase {};
+
+TEST_F(CommandScheduleTest, InstantScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+ MockCommand command;
+
+ EXPECT_CALL(command, Initialize());
+ EXPECT_CALL(command, Execute());
+ EXPECT_CALL(command, End(false));
+
+ command.SetFinished(true);
+ scheduler.Schedule(&command);
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, SingleIterationScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+ MockCommand command;
+
+ EXPECT_CALL(command, Initialize());
+ EXPECT_CALL(command, Execute()).Times(2);
+ EXPECT_CALL(command, End(false));
+
+ scheduler.Schedule(&command);
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ scheduler.Run();
+ command.SetFinished(true);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, MultiScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+ MockCommand command1;
+ MockCommand command2;
+ MockCommand command3;
+
+ EXPECT_CALL(command1, Initialize());
+ EXPECT_CALL(command1, Execute()).Times(2);
+ EXPECT_CALL(command1, End(false));
+
+ EXPECT_CALL(command2, Initialize());
+ EXPECT_CALL(command2, Execute()).Times(3);
+ EXPECT_CALL(command2, End(false));
+
+ EXPECT_CALL(command3, Initialize());
+ EXPECT_CALL(command3, Execute()).Times(4);
+ EXPECT_CALL(command3, End(false));
+
+ scheduler.Schedule(&command1);
+ scheduler.Schedule(&command2);
+ scheduler.Schedule(&command3);
+ EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
+ command1.SetFinished(true);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled({&command2, &command3}));
+ EXPECT_FALSE(scheduler.IsScheduled(&command1));
+ command2.SetFinished(true);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2}));
+ command3.SetFinished(true);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2, &command3}));
+}
+
+TEST_F(CommandScheduleTest, SchedulerCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+ MockCommand command;
+
+ EXPECT_CALL(command, Initialize());
+ EXPECT_CALL(command, Execute());
+ EXPECT_CALL(command, End(false)).Times(0);
+ EXPECT_CALL(command, End(true));
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ scheduler.Cancel(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, NotScheduledCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+ MockCommand command;
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp
new file mode 100644
index 0000000..0429c62
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+
+using namespace frc2;
+
+CommandTestBase::CommandTestBase() {
+ auto& scheduler = CommandScheduler::GetInstance();
+ scheduler.CancelAll();
+ scheduler.Enable();
+ scheduler.ClearButtons();
+}
+
+CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); }
+
+void CommandTestBase::SetUp() {
+ HALSIM_SetDriverStationEnabled(true);
+ while (!HALSIM_GetDriverStationEnabled()) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+}
+
+void CommandTestBase::TearDown() {
+ CommandScheduler::GetInstance().ClearButtons();
+}
+
+void CommandTestBase::SetDSEnabled(bool enabled) {
+ HALSIM_SetDriverStationEnabled(enabled);
+ while (HALSIM_GetDriverStationEnabled() != static_cast<int>(enabled)) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h
new file mode 100644
index 0000000..8b22844
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <regex>
+#include <utility>
+
+#include <mockdata/MockHooks.h>
+
+#include "ErrorConfirmer.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/SetUtilities.h"
+#include "frc2/command/SubsystemBase.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+#include "make_vector.h"
+#include "simulation/DriverStationSim.h"
+
+namespace frc2 {
+class CommandTestBase : public ::testing::Test {
+ public:
+ CommandTestBase();
+
+ class TestSubsystem : public SubsystemBase {};
+
+ protected:
+ class MockCommand : public Command {
+ public:
+ MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet<Subsystem*, 4>());
+ MOCK_METHOD0(IsFinished, bool());
+ MOCK_CONST_METHOD0(RunsWhenDisabled, bool());
+ MOCK_METHOD0(Initialize, void());
+ MOCK_METHOD0(Execute, void());
+ MOCK_METHOD1(End, void(bool interrupted));
+
+ MockCommand() {
+ m_requirements = {};
+ EXPECT_CALL(*this, GetRequirements())
+ .WillRepeatedly(::testing::Return(m_requirements));
+ EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false));
+ EXPECT_CALL(*this, RunsWhenDisabled())
+ .WillRepeatedly(::testing::Return(true));
+ }
+
+ MockCommand(std::initializer_list<Subsystem*> requirements,
+ bool finished = false, bool runWhenDisabled = true) {
+ m_requirements.insert(requirements.begin(), requirements.end());
+ EXPECT_CALL(*this, GetRequirements())
+ .WillRepeatedly(::testing::Return(m_requirements));
+ EXPECT_CALL(*this, IsFinished())
+ .WillRepeatedly(::testing::Return(finished));
+ EXPECT_CALL(*this, RunsWhenDisabled())
+ .WillRepeatedly(::testing::Return(runWhenDisabled));
+ }
+
+ MockCommand(MockCommand&& other) {
+ EXPECT_CALL(*this, IsFinished())
+ .WillRepeatedly(::testing::Return(other.IsFinished()));
+ EXPECT_CALL(*this, RunsWhenDisabled())
+ .WillRepeatedly(::testing::Return(other.RunsWhenDisabled()));
+ std::swap(m_requirements, other.m_requirements);
+ EXPECT_CALL(*this, GetRequirements())
+ .WillRepeatedly(::testing::Return(m_requirements));
+ }
+
+ MockCommand(const MockCommand& other) : Command{} {}
+
+ void SetFinished(bool finished) {
+ EXPECT_CALL(*this, IsFinished())
+ .WillRepeatedly(::testing::Return(finished));
+ }
+
+ ~MockCommand() {
+ auto& scheduler = CommandScheduler::GetInstance();
+ scheduler.Cancel(this);
+ }
+
+ protected:
+ std::unique_ptr<Command> TransferOwnership() && {
+ return std::make_unique<MockCommand>(std::move(*this));
+ }
+
+ private:
+ wpi::SmallSet<Subsystem*, 4> m_requirements;
+ };
+
+ CommandScheduler GetScheduler();
+
+ void SetUp() override;
+
+ void TearDown() override;
+
+ void SetDSEnabled(bool enabled);
+};
+} // namespace frc2
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
new file mode 100644
index 0000000..927721a
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class ConditionalCommandTest : public CommandTestBase {};
+
+TEST_F(ConditionalCommandTest, ConditionalCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
+ MockCommand* mockptr = mock.get();
+
+ EXPECT_CALL(*mock, Initialize());
+ EXPECT_CALL(*mock, Execute()).Times(2);
+ EXPECT_CALL(*mock, End(false));
+
+ ConditionalCommand conditional(
+ std::move(mock), std::make_unique<InstantCommand>(), [] { return true; });
+
+ scheduler.Schedule(&conditional);
+ scheduler.Run();
+ mockptr->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&conditional));
+}
+
+TEST_F(ConditionalCommandTest, ConditionalCommandRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ ConditionalCommand conditional(std::move(command1), std::move(command2),
+ [] { return true; });
+ scheduler.Schedule(&conditional);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&conditional));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
new file mode 100644
index 0000000..b97cbb6
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class DefaultCommandTest : public CommandTestBase {};
+
+TEST_F(DefaultCommandTest, DefaultCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem subsystem;
+
+ RunCommand command1([] {}, {&subsystem});
+
+ scheduler.SetDefaultCommand(&subsystem, std::move(command1));
+ auto handle = scheduler.GetDefaultCommand(&subsystem);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduler.IsScheduled(handle));
+}
+
+TEST_F(DefaultCommandTest, DefaultCommandInterruptResumeTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem subsystem;
+
+ RunCommand command1([] {}, {&subsystem});
+ RunCommand command2([] {}, {&subsystem});
+
+ scheduler.SetDefaultCommand(&subsystem, std::move(command1));
+ auto handle = scheduler.GetDefaultCommand(&subsystem);
+ scheduler.Run();
+ scheduler.Schedule(&command2);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command2));
+ EXPECT_FALSE(scheduler.IsScheduled(handle));
+
+ scheduler.Cancel(&command2);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduler.IsScheduled(handle));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
new file mode 100644
index 0000000..2565a94
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "ErrorConfirmer.h"
+
+ErrorConfirmer* ErrorConfirmer::instance;
+
+int32_t ErrorConfirmer::HandleError(HAL_Bool isError, int32_t errorCode,
+ HAL_Bool isLVCode, const char* details,
+ const char* location, const char* callStack,
+ HAL_Bool printMsg) {
+ if (std::regex_search(details, std::regex(instance->m_msg))) {
+ instance->ConfirmError();
+ }
+ return 1;
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h
new file mode 100644
index 0000000..011158c
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <regex>
+
+#include <mockdata/MockHooks.h>
+
+#include "gmock/gmock.h"
+#include "simulation/DriverStationSim.h"
+
+class ErrorConfirmer {
+ public:
+ explicit ErrorConfirmer(const char* msg) : m_msg(msg) {
+ if (instance != nullptr) return;
+ HALSIM_SetSendError(HandleError);
+ EXPECT_CALL(*this, ConfirmError());
+ instance = this;
+ }
+
+ ~ErrorConfirmer() {
+ HALSIM_SetSendError(nullptr);
+ instance = nullptr;
+ }
+
+ MOCK_METHOD0(ConfirmError, void());
+
+ const char* m_msg;
+
+ static int32_t HandleError(HAL_Bool isError, int32_t errorCode,
+ HAL_Bool isLVCode, const char* details,
+ const char* location, const char* callStack,
+ HAL_Bool printMsg);
+
+ private:
+ static ErrorConfirmer* instance;
+};
diff --git a/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
new file mode 100644
index 0000000..140d4fb
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+class FunctionalCommandTest : public CommandTestBase {};
+
+TEST_F(FunctionalCommandTest, FunctionalCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ int counter = 0;
+ bool finished = false;
+
+ FunctionalCommand command(
+ [&counter] { counter++; }, [&counter] { counter++; },
+ [&counter](bool) { counter++; }, [&finished] { return finished; });
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ EXPECT_EQ(4, counter);
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
new file mode 100644
index 0000000..e91f677
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+class InstantCommandTest : public CommandTestBase {};
+
+TEST_F(InstantCommandTest, InstantCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ int counter = 0;
+
+ InstantCommand command([&counter] { counter++; }, {});
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+ EXPECT_EQ(counter, 1);
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
new file mode 100644
index 0000000..695ccc3
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/NotifierCommand.h"
+
+using namespace frc2;
+class NotifierCommandTest : public CommandTestBase {};
+
+#ifdef __APPLE__
+TEST_F(NotifierCommandTest, DISABLED_NotifierCommandScheduleTest) {
+#else
+TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) {
+#endif
+ CommandScheduler scheduler = GetScheduler();
+
+ int counter = 0;
+
+ NotifierCommand command([&counter] { counter++; }, 0.01_s, {});
+
+ scheduler.Schedule(&command);
+ std::this_thread::sleep_for(std::chrono::milliseconds(250));
+ scheduler.Cancel(&command);
+
+ EXPECT_NEAR(.01 * counter, .25, .025);
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
new file mode 100644
index 0000000..55c7b98
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelCommandGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+
+ ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder)));
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(false));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(2);
+ EXPECT_CALL(*command2, End(false));
+
+ scheduler.Schedule(&group);
+
+ command1->SetFinished(true);
+ scheduler.Run();
+ command2->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+
+ ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder)));
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(false));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(2);
+ EXPECT_CALL(*command2, End(false)).Times(0);
+ EXPECT_CALL(*command2, End(true));
+
+ scheduler.Schedule(&group);
+
+ command1->SetFinished(true);
+ scheduler.Run();
+ scheduler.Run();
+ scheduler.Cancel(&group);
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelCommandGroup group((InstantCommand(), InstantCommand()));
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupCopyTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ ParallelCommandGroup group(command);
+ scheduler.Schedule(&group);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&group));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ ParallelCommandGroup group(std::move(command1), std::move(command2));
+
+ scheduler.Schedule(&group);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
new file mode 100644
index 0000000..6e3246f
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelDeadlineGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ ParallelDeadlineGroup group(
+ std::move(command1Holder),
+ tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+ std::move(command3Holder)));
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(2);
+ EXPECT_CALL(*command1, End(false));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(1);
+ EXPECT_CALL(*command2, End(false));
+
+ EXPECT_CALL(*command3, Initialize());
+ EXPECT_CALL(*command3, Execute()).Times(2);
+ EXPECT_CALL(*command3, End(true));
+
+ scheduler.Schedule(&group);
+
+ command2->SetFinished(true);
+ scheduler.Run();
+ command1->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem subsystem;
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ ParallelDeadlineGroup group(
+ std::move(command1Holder),
+ tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+ std::move(command3Holder)));
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(true));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(1);
+ EXPECT_CALL(*command2, End(true));
+
+ EXPECT_CALL(*command3, Initialize());
+ EXPECT_CALL(*command3, Execute()).Times(1);
+ EXPECT_CALL(*command3, End(true));
+
+ scheduler.Schedule(&group);
+
+ scheduler.Run();
+ scheduler.Cancel(&group);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelDeadlineGroup group{InstantCommand(), InstantCommand()};
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopyTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ ParallelDeadlineGroup group(command);
+ scheduler.Schedule(&group);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&group));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ ParallelDeadlineGroup group(std::move(command1), std::move(command2));
+
+ scheduler.Schedule(&group);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
new file mode 100644
index 0000000..54762ca
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
@@ -0,0 +1,156 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelRaceGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder),
+ std::move(command3Holder))};
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(2);
+ EXPECT_CALL(*command1, End(true));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(2);
+ EXPECT_CALL(*command2, End(false));
+
+ EXPECT_CALL(*command3, Initialize());
+ EXPECT_CALL(*command3, Execute()).Times(2);
+ EXPECT_CALL(*command3, End(true));
+
+ scheduler.Schedule(&group);
+
+ scheduler.Run();
+ command2->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder),
+ std::move(command3Holder))};
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(true));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(1);
+ EXPECT_CALL(*command2, End(true));
+
+ EXPECT_CALL(*command3, Initialize());
+ EXPECT_CALL(*command3, Execute()).Times(1);
+ EXPECT_CALL(*command3, End(true));
+
+ scheduler.Schedule(&group);
+
+ scheduler.Run();
+ scheduler.Cancel(&group);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelRaceGroup group{InstantCommand(), InstantCommand()};
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceCopyTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ ParallelRaceGroup group(command);
+ scheduler.Schedule(&group);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&group));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, RaceGroupRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ ParallelRaceGroup group(std::move(command1), std::move(command2));
+
+ scheduler.Schedule(&group);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnceTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished1 = false;
+ bool finished2 = false;
+ bool finished3 = false;
+
+ WaitUntilCommand command1([&finished1] { return finished1; });
+ WaitUntilCommand command2([&finished2] { return finished2; });
+ WaitUntilCommand command3([&finished3] { return finished3; });
+
+ SequentialCommandGroup group1(command1, command2);
+ ParallelRaceGroup group2(std::move(group1), command3);
+
+ scheduler.Schedule(&group2);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&group2));
+ finished1 = true;
+ scheduler.Run();
+ finished2 = true;
+ EXPECT_NO_FATAL_FAILURE(scheduler.Run());
+ EXPECT_FALSE(scheduler.IsScheduled(&group2));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
new file mode 100644
index 0000000..b3ef861
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/PerpetualCommand.h"
+
+using namespace frc2;
+class PerpetualCommandTest : public CommandTestBase {};
+
+TEST_F(PerpetualCommandTest, PerpetualCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool check = false;
+
+ PerpetualCommand command{InstantCommand([&check] { check = true; }, {})};
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ EXPECT_TRUE(check);
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
new file mode 100644
index 0000000..b940566
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/PrintCommand.h"
+
+using namespace frc2;
+class PrintCommandTest : public CommandTestBase {};
+
+TEST_F(PrintCommandTest, PrintCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ PrintCommand command("Test!");
+
+ testing::internal::CaptureStdout();
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_TRUE(std::regex_search(testing::internal::GetCapturedStdout(),
+ std::regex("Test!")));
+
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
new file mode 100644
index 0000000..09a569f
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ProxyScheduleCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ProxyScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandScheduleTest) {
+ CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+ bool scheduled = false;
+
+ InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+ ProxyScheduleCommand command(&toSchedule);
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduled);
+}
+
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandEndTest) {
+ CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+ bool finished = false;
+
+ WaitUntilCommand toSchedule([&finished] { return finished; });
+
+ ProxyScheduleCommand command(&toSchedule);
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
new file mode 100644
index 0000000..bac40d5
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
@@ -0,0 +1,156 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SelectCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class RobotDisabledCommandTest : public CommandTestBase {};
+
+TEST_F(RobotDisabledCommandTest, RobotDisabledCommandCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ MockCommand command({}, false, false);
+
+ EXPECT_CALL(command, End(true));
+
+ SetDSEnabled(true);
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+ SetDSEnabled(false);
+
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(RobotDisabledCommandTest, RunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ MockCommand command1;
+ MockCommand command2;
+
+ scheduler.Schedule(&command1);
+
+ SetDSEnabled(false);
+
+ scheduler.Run();
+
+ scheduler.Schedule(&command2);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command1));
+ EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(RobotDisabledCommandTest, SequentialGroupRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ SequentialCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
+ SequentialCommandGroup dontRunWhenDisabled{MockCommand(),
+ MockCommand({}, false, false)};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelGroupRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
+ ParallelCommandGroup dontRunWhenDisabled{MockCommand(),
+ MockCommand({}, false, false)};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelRaceRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelRaceGroup runWhenDisabled{MockCommand(), MockCommand()};
+ ParallelRaceGroup dontRunWhenDisabled{MockCommand(),
+ MockCommand({}, false, false)};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelDeadlineRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ParallelDeadlineGroup runWhenDisabled{MockCommand(), MockCommand()};
+ ParallelDeadlineGroup dontRunWhenDisabled{MockCommand(),
+ MockCommand({}, false, false)};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ConditionalCommandRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ ConditionalCommand runWhenDisabled{MockCommand(), MockCommand(),
+ [] { return true; }};
+ ConditionalCommand dontRunWhenDisabled{
+ MockCommand(), MockCommand({}, false, false), [] { return true; }};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, SelectCommandRunWhenDisabledTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ SelectCommand<int> runWhenDisabled{[] { return 1; },
+ std::pair(1, MockCommand()),
+ std::pair(1, MockCommand())};
+ SelectCommand<int> dontRunWhenDisabled{
+ [] { return 1; }, std::pair(1, MockCommand()),
+ std::pair(1, MockCommand({}, false, false))};
+
+ SetDSEnabled(false);
+
+ scheduler.Schedule(&runWhenDisabled);
+ scheduler.Schedule(&dontRunWhenDisabled);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+ EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp
new file mode 100644
index 0000000..07eecb3
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class RunCommandTest : public CommandTestBase {};
+
+TEST_F(RunCommandTest, RunCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ int counter = 0;
+
+ RunCommand command([&counter] { counter++; }, {});
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ scheduler.Run();
+ scheduler.Run();
+
+ EXPECT_EQ(3, counter);
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
new file mode 100644
index 0000000..29e8dc5
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ScheduleCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class ScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ScheduleCommandTest, ScheduleCommandScheduleTest) {
+ CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+ bool scheduled = false;
+
+ InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+ ScheduleCommand command(&toSchedule);
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_TRUE(scheduled);
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp
new file mode 100644
index 0000000..f0198c9
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class SchedulerTest : public CommandTestBase {};
+
+TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) {
+ CommandScheduler scheduler = GetScheduler();
+
+ InstantCommand command;
+
+ int counter = 0;
+
+ scheduler.OnCommandInitialize([&counter](const Command&) { counter++; });
+ scheduler.OnCommandExecute([&counter](const Command&) { counter++; });
+ scheduler.OnCommandFinish([&counter](const Command&) { counter++; });
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+
+ EXPECT_EQ(counter, 3);
+}
+
+TEST_F(SchedulerTest, SchedulerLambdaInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ RunCommand command([] {}, {});
+
+ int counter = 0;
+
+ scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; });
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ scheduler.Cancel(&command);
+
+ EXPECT_EQ(counter, 1);
+}
+
+TEST_F(SchedulerTest, UnregisterSubsystemTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem system;
+
+ scheduler.RegisterSubsystem(&system);
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
new file mode 100644
index 0000000..6be14ef
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class SelectCommandTest : public CommandTestBase {};
+
+TEST_F(SelectCommandTest, SelectCommandTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
+ MockCommand* mockptr = mock.get();
+
+ EXPECT_CALL(*mock, Initialize());
+ EXPECT_CALL(*mock, Execute()).Times(2);
+ EXPECT_CALL(*mock, End(false));
+
+ std::vector<std::pair<int, std::unique_ptr<Command>>> temp;
+
+ temp.emplace_back(std::pair(1, std::move(mock)));
+ temp.emplace_back(std::pair(2, std::make_unique<InstantCommand>()));
+ temp.emplace_back(std::pair(3, std::make_unique<InstantCommand>()));
+
+ SelectCommand<int> select([] { return 1; }, std::move(temp));
+
+ scheduler.Schedule(&select);
+ scheduler.Run();
+ mockptr->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&select));
+}
+
+TEST_F(SelectCommandTest, SelectCommandRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ SelectCommand<int> select([] { return 1; }, std::pair(1, std::move(command1)),
+ std::pair(2, std::move(command2)));
+
+ scheduler.Schedule(&select);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&select));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
new file mode 100644
index 0000000..3a6f8d8
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class SequentialCommandGroupTest : public CommandTestBase {};
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder),
+ std::move(command3Holder))};
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(false));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(1);
+ EXPECT_CALL(*command2, End(false));
+
+ EXPECT_CALL(*command3, Initialize());
+ EXPECT_CALL(*command3, Execute()).Times(1);
+ EXPECT_CALL(*command3, End(false));
+
+ scheduler.Schedule(&group);
+
+ command1->SetFinished(true);
+ scheduler.Run();
+ command2->SetFinished(true);
+ scheduler.Run();
+ command3->SetFinished(true);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupInterruptTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+ std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+ MockCommand* command1 = command1Holder.get();
+ MockCommand* command2 = command2Holder.get();
+ MockCommand* command3 = command3Holder.get();
+
+ SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
+ std::move(command1Holder), std::move(command2Holder),
+ std::move(command3Holder))};
+
+ EXPECT_CALL(*command1, Initialize());
+ EXPECT_CALL(*command1, Execute()).Times(1);
+ EXPECT_CALL(*command1, End(false));
+
+ EXPECT_CALL(*command2, Initialize());
+ EXPECT_CALL(*command2, Execute()).Times(0);
+ EXPECT_CALL(*command2, End(false)).Times(0);
+ EXPECT_CALL(*command2, End(true));
+
+ EXPECT_CALL(*command3, Initialize()).Times(0);
+ EXPECT_CALL(*command3, Execute()).Times(0);
+ EXPECT_CALL(*command3, End(false)).Times(0);
+ EXPECT_CALL(*command3, End(true)).Times(0);
+
+ scheduler.Schedule(&group);
+
+ command1->SetFinished(true);
+ scheduler.Run();
+ scheduler.Cancel(&group);
+ scheduler.Run();
+
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupNotScheduledCancelTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ SequentialCommandGroup group{InstantCommand(), InstantCommand()};
+
+ EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupCopyTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ SequentialCommandGroup group(command);
+ scheduler.Schedule(&group);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&group));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupRequirementTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ TestSubsystem requirement1;
+ TestSubsystem requirement2;
+ TestSubsystem requirement3;
+ TestSubsystem requirement4;
+
+ InstantCommand command1([] {}, {&requirement1, &requirement2});
+ InstantCommand command2([] {}, {&requirement3});
+ InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+ SequentialCommandGroup group(std::move(command1), std::move(command2));
+
+ scheduler.Schedule(&group);
+ scheduler.Schedule(&command3);
+
+ EXPECT_TRUE(scheduler.IsScheduled(&command3));
+ EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
new file mode 100644
index 0000000..3f25792
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/StartEndCommand.h"
+
+using namespace frc2;
+class StartEndCommandTest : public CommandTestBase {};
+
+TEST_F(StartEndCommandTest, StartEndCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ int counter = 0;
+
+ StartEndCommand command([&counter] { counter++; }, [&counter] { counter++; },
+ {});
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ scheduler.Run();
+ scheduler.Cancel(&command);
+
+ EXPECT_EQ(2, counter);
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
new file mode 100644
index 0000000..3363975
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitCommandTest : public CommandTestBase {};
+
+TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ WaitCommand command(.1_s);
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ std::this_thread::sleep_for(std::chrono::milliseconds(110));
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
new file mode 100644
index 0000000..e9728db
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitUntilCommandTest : public CommandTestBase {};
+
+TEST_F(WaitUntilCommandTest, WaitUntilCommandScheduleTest) {
+ CommandScheduler scheduler = GetScheduler();
+
+ bool finished = false;
+
+ WaitUntilCommand command([&finished] { return finished; });
+
+ scheduler.Schedule(&command);
+ scheduler.Run();
+ EXPECT_TRUE(scheduler.IsScheduled(&command));
+ finished = true;
+ scheduler.Run();
+ EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/make_vector.h b/wpilibc/src/test/native/cpp/frc2/command/make_vector.h
new file mode 100644
index 0000000..89d55b6
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/frc2/command/make_vector.h
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <type_traits>
+#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... Args,
+ typename std::enable_if_t<!std::is_trivially_copyable_v<T>, int> = 0>
+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,
+ typename std::enable_if_t<std::is_trivially_copyable_v<T>, int> = 0>
+std::vector<T> make_vector_impl(Args&&... args) {
+ return std::vector<T>{std::forward<Args>(args)...};
+}
+
+} // namespace detail
+
+template <
+ typename T = void, typename... Args,
+ typename V = detail::vec_type_helper_t<T, Args...>,
+ typename std::enable_if_t<
+ detail::all_constructible_and_convertible<V, Args...>::value, int> = 0>
+std::vector<V> make_vector(Args&&... args) {
+ return detail::make_vector_impl<V>(std::forward<Args>(args)...);
+}
+
+} // namespace tcb
diff --git a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp
new file mode 100644
index 0000000..8b5f674
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Pose2dTest, TransformBy) {
+ const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
+ const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
+
+ const auto transformed = initial + transform;
+
+ EXPECT_NEAR(transformed.Translation().X().to<double>(),
+ 1 + 5 / std::sqrt(2.0), kEpsilon);
+ EXPECT_NEAR(transformed.Translation().Y().to<double>(),
+ 2 + 5 / std::sqrt(2.0), kEpsilon);
+ EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
+}
+
+TEST(Pose2dTest, RelativeTo) {
+ const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
+ const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+
+ const auto finalRelativeToInitial = final.RelativeTo(initial);
+
+ EXPECT_NEAR(finalRelativeToInitial.Translation().X().to<double>(),
+ 5.0 * std::sqrt(2.0), kEpsilon);
+ EXPECT_NEAR(finalRelativeToInitial.Translation().Y().to<double>(), 0.0,
+ kEpsilon);
+ EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
+ kEpsilon);
+}
+
+TEST(Pose2dTest, Equality) {
+ const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
+ const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
+ EXPECT_TRUE(a == b);
+}
+
+TEST(Pose2dTest, Inequality) {
+ const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
+ const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
+ EXPECT_TRUE(a != b);
+}
+
+TEST(Pose2dTest, Minus) {
+ const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
+ const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
+
+ const auto transform = final - initial;
+
+ EXPECT_NEAR(transform.Translation().X().to<double>(), 5.0 * std::sqrt(2.0),
+ kEpsilon);
+ EXPECT_NEAR(transform.Translation().Y().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp
new file mode 100644
index 0000000..ba80787
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include <wpi/math>
+
+#include "frc/geometry/Rotation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Rotation2dTest, RadiansToDegrees) {
+ const Rotation2d one{units::radian_t(wpi::math::pi / 3)};
+ const Rotation2d two{units::radian_t(wpi::math::pi / 4)};
+
+ EXPECT_NEAR(one.Degrees().to<double>(), 60.0, kEpsilon);
+ EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, DegreesToRadians) {
+ const auto one = Rotation2d(45.0_deg);
+ const auto two = Rotation2d(30.0_deg);
+
+ EXPECT_NEAR(one.Radians().to<double>(), wpi::math::pi / 4.0, kEpsilon);
+ EXPECT_NEAR(two.Radians().to<double>(), wpi::math::pi / 6.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByFromZero) {
+ const Rotation2d zero;
+ auto sum = zero + Rotation2d(90.0_deg);
+
+ EXPECT_NEAR(sum.Radians().to<double>(), wpi::math::pi / 2.0, kEpsilon);
+ EXPECT_NEAR(sum.Degrees().to<double>(), 90.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, RotateByNonZero) {
+ auto rot = Rotation2d(90.0_deg);
+ rot += Rotation2d(30.0_deg);
+
+ EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, Minus) {
+ const auto one = Rotation2d(70.0_deg);
+ const auto two = Rotation2d(30.0_deg);
+
+ EXPECT_NEAR((one - two).Degrees().to<double>(), 40.0, kEpsilon);
+}
+
+TEST(Rotation2dTest, Equality) {
+ const auto one = Rotation2d(43_deg);
+ const auto two = Rotation2d(43_deg);
+ EXPECT_TRUE(one == two);
+}
+
+TEST(Rotation2dTest, Inequality) {
+ const auto one = Rotation2d(43_deg);
+ const auto two = Rotation2d(43.5_deg);
+ EXPECT_TRUE(one != two);
+}
diff --git a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp
new file mode 100644
index 0000000..99fced7
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Translation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Translation2dTest, Sum) {
+ const Translation2d one{1.0_m, 3.0_m};
+ const Translation2d two{2.0_m, 5.0_m};
+
+ const auto sum = one + two;
+
+ EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
+ EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Difference) {
+ const Translation2d one{1.0_m, 3.0_m};
+ const Translation2d two{2.0_m, 5.0_m};
+
+ const auto difference = one - two;
+
+ EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
+ EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
+}
+
+TEST(Translation2dTest, RotateBy) {
+ const Translation2d another{3.0_m, 0.0_m};
+ const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
+
+ EXPECT_NEAR(rotated.X().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(rotated.Y().to<double>(), 3.0, kEpsilon);
+}
+
+TEST(Translation2dTest, Multiplication) {
+ const Translation2d original{3.0_m, 5.0_m};
+ const auto mult = original * 3;
+
+ EXPECT_NEAR(mult.X().to<double>(), 9.0, kEpsilon);
+ EXPECT_NEAR(mult.Y().to<double>(), 15.0, kEpsilon);
+}
+
+TEST(Translation2d, Division) {
+ const Translation2d original{3.0_m, 5.0_m};
+ const auto div = original / 2;
+
+ EXPECT_NEAR(div.X().to<double>(), 1.5, kEpsilon);
+ EXPECT_NEAR(div.Y().to<double>(), 2.5, kEpsilon);
+}
+
+TEST(Translation2dTest, Norm) {
+ const Translation2d one{3.0_m, 5.0_m};
+ EXPECT_NEAR(one.Norm().to<double>(), std::hypot(3, 5), kEpsilon);
+}
+
+TEST(Translation2dTest, Distance) {
+ const Translation2d one{1_m, 1_m};
+ const Translation2d two{6_m, 6_m};
+ EXPECT_NEAR(one.Distance(two).to<double>(), 5 * std::sqrt(2), kEpsilon);
+}
+
+TEST(Translation2dTest, UnaryMinus) {
+ const Translation2d original{-4.5_m, 7_m};
+ const auto inverted = -original;
+
+ EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
+ EXPECT_NEAR(inverted.Y().to<double>(), -7, kEpsilon);
+}
+
+TEST(Translation2dTest, Equality) {
+ const Translation2d one{9_m, 5.5_m};
+ const Translation2d two{9_m, 5.5_m};
+ EXPECT_TRUE(one == two);
+}
+
+TEST(Translation2dTest, Inequality) {
+ const Translation2d one{9_m, 5.5_m};
+ const Translation2d two{9_m, 5.7_m};
+ EXPECT_TRUE(one != two);
+}
diff --git a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp
new file mode 100644
index 0000000..faf36d9
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include <wpi/math>
+
+#include "frc/geometry/Pose2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Twist2dTest, Straight) {
+ const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
+ const auto straightPose = Pose2d().Exp(straight);
+
+ EXPECT_NEAR(straightPose.Translation().X().to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(straightPose.Translation().Y().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
+}
+
+TEST(Twist2dTest, QuarterCircle) {
+ const Twist2d quarterCircle{5.0_m / 2.0 * wpi::math::pi, 0_m,
+ units::radian_t(wpi::math::pi / 2.0)};
+ const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
+
+ EXPECT_NEAR(quarterCirclePose.Translation().X().to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(quarterCirclePose.Translation().Y().to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
+ kEpsilon);
+}
+
+TEST(Twist2dTest, DiagonalNoDtheta) {
+ const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
+ const auto diagonalPose = Pose2d().Exp(diagonal);
+
+ EXPECT_NEAR(diagonalPose.Translation().X().to<double>(), 2.0, kEpsilon);
+ EXPECT_NEAR(diagonalPose.Translation().Y().to<double>(), 2.0, kEpsilon);
+ EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+}
+
+TEST(Twist2dTest, Equality) {
+ const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
+ const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
+ EXPECT_TRUE(one == two);
+}
+
+TEST(Twist2dTest, Inequality) {
+ const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
+ const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
+ EXPECT_TRUE(one != two);
+}
+
+TEST(Twist2dTest, Pose2dLog) {
+ const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
+ const Pose2d start{};
+
+ const auto twist = start.Log(end);
+
+ EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::math::pi, kEpsilon);
+ EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(twist.dtheta.to<double>(), wpi::math::pi / 2.0, kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
new file mode 100644
index 0000000..3fb63fb
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "gtest/gtest.h"
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(ChassisSpeeds, FieldRelativeConstruction) {
+ const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+ 1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
+
+ EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), kEpsilon);
+ EXPECT_NEAR(1.0, chassisSpeeds.vy.to<double>(), kEpsilon);
+ EXPECT_NEAR(0.5, chassisSpeeds.omega.to<double>(), kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
new file mode 100644
index 0000000..ad0d3c7
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <units/units.h>
+#include <wpi/math>
+
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(DifferentialDriveKinematics, InverseKinematicsFromZero) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const ChassisSpeeds chassisSpeeds;
+ const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, ForwardKinematicsFromZero) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const DifferentialDriveWheelSpeeds wheelSpeeds;
+ const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, InverseKinematicsForStraightLine) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
+ const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
+ const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const ChassisSpeeds chassisSpeeds{0.0_mps, 0.0_mps,
+ units::radians_per_second_t{wpi::math::pi}};
+ const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::math::pi, kEpsilon);
+ EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::math::pi, kEpsilon);
+}
+
+TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) {
+ const DifferentialDriveKinematics kinematics{0.381_m * 2};
+ const DifferentialDriveWheelSpeeds wheelSpeeds{
+ units::meters_per_second_t(+0.381 * wpi::math::pi),
+ units::meters_per_second_t(-0.381 * wpi::math::pi)};
+ const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::math::pi, kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
new file mode 100644
index 0000000..2647252
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <wpi/math>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveOdometry.h"
+#include "gtest/gtest.h"
+
+static constexpr double kEpsilon = 1E-9;
+
+using namespace frc;
+
+TEST(DifferentialDriveOdometry, OneIteration) {
+ DifferentialDriveKinematics kinematics{0.381_m * 2};
+ DifferentialDriveOdometry odometry{kinematics};
+
+ odometry.ResetPosition(Pose2d());
+ DifferentialDriveWheelSpeeds wheelSpeeds{0.02_mps, 0.02_mps};
+ odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
+ const auto& pose = odometry.UpdateWithTime(1_s, Rotation2d(), wheelSpeeds);
+
+ EXPECT_NEAR(pose.Translation().X().to<double>(), 0.02, kEpsilon);
+ EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
+}
+
+TEST(DifferentialDriveOdometry, QuarterCircle) {
+ DifferentialDriveKinematics kinematics{0.381_m * 2};
+ DifferentialDriveOdometry odometry{kinematics};
+
+ odometry.ResetPosition(Pose2d());
+ DifferentialDriveWheelSpeeds wheelSpeeds{
+ 0.0_mps, units::meters_per_second_t(5 * wpi::math::pi)};
+ odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
+ const auto& pose =
+ odometry.UpdateWithTime(1_s, Rotation2d(90_deg), wheelSpeeds);
+
+ EXPECT_NEAR(pose.Translation().X().to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
new file mode 100644
index 0000000..75f395d
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -0,0 +1,230 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <units/units.h>
+#include <wpi/math>
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class MecanumDriveKinematicsTest : public ::testing::Test {
+ protected:
+ Translation2d m_fl{12_m, 12_m};
+ Translation2d m_fr{12_m, -12_m};
+ Translation2d m_bl{-12_m, 12_m};
+ Translation2d m_br{-12_m, -12_m};
+
+ MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
+};
+
+TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
+ ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
+ */
+
+ EXPECT_NEAR(3.536, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(3.536, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(3.536, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(3.536, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
+ 3.536_mps};
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
+ [[5][0][0]]
+ */
+
+ EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
+ */
+
+ EXPECT_NEAR(-2.828427, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(2.828427, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(2.828427, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(-2.828427, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps,
+ -2.828427_mps};
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
+ [[5][0][0]]
+ */
+
+ EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(4.0, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 0_mps,
+ units::radians_per_second_t(2 * wpi::math::pi)};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
+ */
+
+ EXPECT_NEAR(-106.62919, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(106.62919, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(106.62919, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps,
+ -106.62919_mps, 106.62919_mps};
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should
+ be [[0][0][2pi]]
+ */
+
+ EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
+ ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
+ */
+
+ EXPECT_NEAR(-17.677670, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(20.506097, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(-13.435, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(16.26, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{-17.677670_mps, 20.506097_mps,
+ -13.435_mps, 16.26_mps};
+
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from wheel
+ velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be
+ [[2][3][1]]
+ */
+
+ EXPECT_NEAR(2.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(3.0, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
+ */
+
+ EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(16.971, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(-16.971, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(33.941, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{0_mps, 16.971_mps, -16.971_mps,
+ 33.941_mps};
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from the
+ wheel velocities should be [[12][-12][1]]
+ */
+
+ EXPECT_NEAR(12.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(-12, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest,
+ OffCenterTranslationRotationInverseKinematics) {
+ ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
+ auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
+
+ /*
+ By equation (13.12) of the state-space-guide, the wheel speeds should
+ be as follows:
+ velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
+ */
+ EXPECT_NEAR(2.12, moduleStates.frontLeft.to<double>(), 0.1);
+ EXPECT_NEAR(21.92, moduleStates.frontRight.to<double>(), 0.1);
+ EXPECT_NEAR(-12.02, moduleStates.rearLeft.to<double>(), 0.1);
+ EXPECT_NEAR(36.06, moduleStates.rearRight.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest,
+ OffCenterTranslationRotationForwardKinematics) {
+ MecanumDriveWheelSpeeds wheelSpeeds{2.12_mps, 21.92_mps, -12.02_mps,
+ 36.06_mps};
+ auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ /*
+ By equation (13.13) of the state-space-guide, the chassis motion from the
+ wheel velocities should be [[17][-10][1]]
+ */
+
+ EXPECT_NEAR(17.0, chassisSpeeds.vx.to<double>(), 0.1);
+ EXPECT_NEAR(-10, chassisSpeeds.vy.to<double>(), 0.1);
+ EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
+}
+
+TEST_F(MecanumDriveKinematicsTest, NormalizeTest) {
+ MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
+ wheelSpeeds.Normalize(5.5_mps);
+
+ double kFactor = 5.5 / 7.0;
+
+ EXPECT_NEAR(wheelSpeeds.frontLeft.to<double>(), 5.0 * kFactor, 1E-9);
+ EXPECT_NEAR(wheelSpeeds.frontRight.to<double>(), 6.0 * kFactor, 1E-9);
+ EXPECT_NEAR(wheelSpeeds.rearLeft.to<double>(), 4.0 * kFactor, 1E-9);
+ EXPECT_NEAR(wheelSpeeds.rearRight.to<double>(), 7.0 * kFactor, 1E-9);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
new file mode 100644
index 0000000..0a6894d
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveOdometry.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class MecanumDriveOdometryTest : public ::testing::Test {
+ protected:
+ Translation2d m_fl{12_m, 12_m};
+ Translation2d m_fr{12_m, -12_m};
+ Translation2d m_bl{-12_m, 12_m};
+ Translation2d m_br{-12_m, -12_m};
+
+ MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
+ MecanumDriveOdometry odometry{kinematics};
+};
+
+TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
+ odometry.ResetPosition(Pose2d());
+ MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
+ 3.536_mps};
+
+ odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
+ auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
+
+ EXPECT_NEAR(secondPose.Translation().X().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(secondPose.Translation().Y().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}
+
+TEST_F(MecanumDriveOdometryTest, TwoIterations) {
+ odometry.ResetPosition(Pose2d());
+ MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+
+ odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
+ auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
+
+ EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
+ EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
+ EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}
+
+TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
+ odometry.ResetPosition(Pose2d());
+ MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
+ 39.986_mps};
+ odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
+ auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
+
+ EXPECT_NEAR(pose.Translation().X().to<double>(), 12, 0.01);
+ EXPECT_NEAR(pose.Translation().Y().to<double>(), 12, 0.01);
+ EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
new file mode 100644
index 0000000..8fd67ea
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <units/units.h>
+#include <wpi/math>
+
+#include "frc/geometry/Translation2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 0.1;
+
+class SwerveDriveKinematicsTest : public ::testing::Test {
+ protected:
+ Translation2d m_fl{12_m, 12_m};
+ Translation2d m_fr{12_m, -12_m};
+ Translation2d m_bl{-12_m, 12_m};
+ Translation2d m_br{-12_m, -12_m};
+
+ SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
+};
+
+TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
+ ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s};
+
+ auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
+
+ EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Radians().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Radians().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Radians().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Radians().to<double>(), 0.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
+ SwerveModuleState state{5.0_mps, Rotation2d()};
+
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
+ auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
+
+ EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Degrees().to<double>(), 90.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().to<double>(), 90.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().to<double>(), 90.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().to<double>(), 90.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
+ SwerveModuleState state{5_mps, Rotation2d(90_deg)};
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 5.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 0_mps,
+ units::radians_per_second_t(2 * wpi::math::pi)};
+ auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
+
+ EXPECT_NEAR(fl.speed.to<double>(), 106.63, kEpsilon);
+ EXPECT_NEAR(fr.speed.to<double>(), 106.63, kEpsilon);
+ EXPECT_NEAR(bl.speed.to<double>(), 106.63, kEpsilon);
+ EXPECT_NEAR(br.speed.to<double>(), 106.63, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Degrees().to<double>(), 135.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().to<double>(), 45.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().to<double>(), -135.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
+ SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
+ SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};
+ SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)};
+ SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)};
+
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 0_mps,
+ units::radians_per_second_t(2 * wpi::math::pi)};
+ auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
+
+ EXPECT_NEAR(fl.speed.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(fr.speed.to<double>(), 150.796, kEpsilon);
+ EXPECT_NEAR(bl.speed.to<double>(), 150.796, kEpsilon);
+ EXPECT_NEAR(br.speed.to<double>(), 213.258, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Degrees().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().to<double>(), -90.0, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
+ SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)};
+ SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)};
+ SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)};
+ SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)};
+
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 75.398, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -75.398, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest,
+ OffCenterCORRotationAndTranslationInverseKinematics) {
+ ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
+ auto [fl, fr, bl, br] =
+ m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
+
+ EXPECT_NEAR(fl.speed.to<double>(), 23.43, kEpsilon);
+ EXPECT_NEAR(fr.speed.to<double>(), 23.43, kEpsilon);
+ EXPECT_NEAR(bl.speed.to<double>(), 54.08, kEpsilon);
+ EXPECT_NEAR(br.speed.to<double>(), 54.08, kEpsilon);
+
+ EXPECT_NEAR(fl.angle.Degrees().to<double>(), -140.19, kEpsilon);
+ EXPECT_NEAR(fr.angle.Degrees().to<double>(), -39.81, kEpsilon);
+ EXPECT_NEAR(bl.angle.Degrees().to<double>(), -109.44, kEpsilon);
+ EXPECT_NEAR(br.angle.Degrees().to<double>(), -70.56, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest,
+ OffCenterCORRotationAndTranslationForwardKinematics) {
+ SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)};
+ SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)};
+ SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)};
+ SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)};
+
+ auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
+
+ EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -33.0, kEpsilon);
+ EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 1.5, kEpsilon);
+}
+
+TEST_F(SwerveDriveKinematicsTest, NormalizeTest) {
+ SwerveModuleState state1{5.0_mps, Rotation2d()};
+ SwerveModuleState state2{6.0_mps, Rotation2d()};
+ SwerveModuleState state3{4.0_mps, Rotation2d()};
+ SwerveModuleState state4{7.0_mps, Rotation2d()};
+
+ std::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
+ SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
+
+ double kFactor = 5.5 / 7.0;
+
+ EXPECT_NEAR(arr[0].speed.to<double>(), 5.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[1].speed.to<double>(), 6.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[2].speed.to<double>(), 4.0 * kFactor, kEpsilon);
+ EXPECT_NEAR(arr[3].speed.to<double>(), 7.0 * kFactor, kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
new file mode 100644
index 0000000..ab81017
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveOdometry.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 0.01;
+
+class SwerveDriveOdometryTest : public ::testing::Test {
+ protected:
+ Translation2d m_fl{12_m, 12_m};
+ Translation2d m_fr{12_m, -12_m};
+ Translation2d m_bl{-12_m, 12_m};
+ Translation2d m_br{-12_m, -12_m};
+
+ SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
+ SwerveDriveOdometry<4> m_odometry{m_kinematics};
+};
+
+TEST_F(SwerveDriveOdometryTest, TwoIterations) {
+ SwerveModuleState state{5_mps, Rotation2d()};
+
+ m_odometry.ResetPosition(Pose2d());
+ m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
+ SwerveModuleState(), SwerveModuleState(),
+ SwerveModuleState());
+ auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
+ state, state);
+
+ EXPECT_NEAR(0.5, pose.Translation().X().to<double>(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Translation().Y().to<double>(), kEpsilon);
+ EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}
+
+TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
+ SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)};
+ SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)};
+ SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)};
+ SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)};
+
+ SwerveModuleState zero{0_mps, Rotation2d()};
+
+ m_odometry.ResetPosition(Pose2d());
+ m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
+ auto pose =
+ m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
+
+ EXPECT_NEAR(12.0, pose.Translation().X().to<double>(), kEpsilon);
+ EXPECT_NEAR(12.0, pose.Translation().Y().to<double>(), kEpsilon);
+ EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp b/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp
index 3c9e411..172d7c6 100644
--- a/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp
+++ b/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,11 +7,12 @@
#include "shuffleboard/MockActuatorSendable.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
using namespace frc;
-MockActuatorSendable::MockActuatorSendable(wpi::StringRef name)
- : SendableBase(false) {
- SetName(name);
+MockActuatorSendable::MockActuatorSendable(wpi::StringRef name) {
+ SendableRegistry::GetInstance().Add(this, name);
}
void MockActuatorSendable::InitSendable(SendableBuilder& builder) {
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp b/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
index 3dd9dbf..0b06d7f 100644
--- a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
+++ b/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/SuppliedValueWidgetTest.cpp b/wpilibc/src/test/native/cpp/shuffleboard/SuppliedValueWidgetTest.cpp
new file mode 100644
index 0000000..8e39915
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/shuffleboard/SuppliedValueWidgetTest.cpp
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "frc/shuffleboard/ShuffleboardInstance.h"
+#include "frc/shuffleboard/ShuffleboardTab.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class SuppliedValueWidgetTest : public testing::Test {
+ void SetUp() override {
+ m_ntInstance = nt::NetworkTableInstance::Create();
+ m_instance = std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
+ m_tab = &(m_instance->GetTab("Tab"));
+ }
+
+ protected:
+ nt::NetworkTableInstance m_ntInstance;
+ ShuffleboardTab* m_tab;
+ std::unique_ptr<detail::ShuffleboardInstance> m_instance;
+};
+
+TEST_F(SuppliedValueWidgetTest, AddString) {
+ std::string str = "foo";
+ m_tab->AddString("String", [&str]() { return str; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/String");
+
+ m_instance->Update();
+ EXPECT_EQ("foo", entry.GetValue()->GetString());
+}
+
+TEST_F(SuppliedValueWidgetTest, AddNumber) {
+ int num = 0;
+ m_tab->AddNumber("Num", [&num]() { return ++num; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Num");
+
+ m_instance->Update();
+ EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
+}
+
+TEST_F(SuppliedValueWidgetTest, AddBoolean) {
+ bool value = true;
+ m_tab->AddBoolean("Bool", [&value]() { return value; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Bool");
+
+ m_instance->Update();
+ EXPECT_EQ(true, entry.GetValue()->GetBoolean());
+}
+
+TEST_F(SuppliedValueWidgetTest, AddStringArray) {
+ std::vector<std::string> strings = {"foo", "bar"};
+ m_tab->AddStringArray("Strings", [&strings]() { return strings; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Strings");
+
+ m_instance->Update();
+ auto actual = entry.GetValue()->GetStringArray();
+
+ EXPECT_EQ(strings.size(), actual.size());
+ for (size_t i = 0; i < strings.size(); i++) {
+ EXPECT_EQ(strings[i], actual[i]);
+ }
+}
+
+TEST_F(SuppliedValueWidgetTest, AddNumberArray) {
+ std::vector<double> nums = {0, 1, 2, 3};
+ m_tab->AddNumberArray("Numbers", [&nums]() { return nums; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Numbers");
+
+ m_instance->Update();
+ auto actual = entry.GetValue()->GetDoubleArray();
+
+ EXPECT_EQ(nums.size(), actual.size());
+ for (size_t i = 0; i < nums.size(); i++) {
+ EXPECT_FLOAT_EQ(nums[i], actual[i]);
+ }
+}
+
+TEST_F(SuppliedValueWidgetTest, AddBooleanArray) {
+ std::vector<int> bools = {true, false};
+ m_tab->AddBooleanArray("Booleans", [&bools]() { return bools; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Booleans");
+
+ m_instance->Update();
+ auto actual = entry.GetValue()->GetBooleanArray();
+
+ EXPECT_EQ(bools.size(), actual.size());
+ for (size_t i = 0; i < bools.size(); i++) {
+ EXPECT_FLOAT_EQ(bools[i], actual[i]);
+ }
+}
+
+TEST_F(SuppliedValueWidgetTest, AddRaw) {
+ wpi::StringRef bytes = "\1\2\3";
+ m_tab->AddRaw("Raw", [&bytes]() { return bytes; });
+ auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Raw");
+
+ m_instance->Update();
+ auto actual = entry.GetValue()->GetRaw();
+ EXPECT_EQ(bytes, actual);
+}
diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
new file mode 100644
index 0000000..8254dd4
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <chrono>
+#include <iostream>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace frc {
+class CubicHermiteSplineTest : public ::testing::Test {
+ protected:
+ static void Run(const Pose2d& a, const std::vector<Translation2d>& waypoints,
+ const Pose2d& b) {
+ // Start the timer.
+ const auto start = std::chrono::high_resolution_clock::now();
+
+ // Generate and parameterize the spline.
+
+ const auto [startCV, endCV] =
+ SplineHelper::CubicControlVectorsFromWaypoints(a, waypoints, b);
+
+ const auto splines =
+ SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV);
+ std::vector<Spline<3>::PoseWithCurvature> poses;
+
+ poses.push_back(splines[0].GetPoint(0.0));
+
+ for (auto&& spline : splines) {
+ auto x = SplineParameterizer::Parameterize(spline);
+ poses.insert(std::end(poses), std::begin(x) + 1, std::end(x));
+ }
+
+ // End timer.
+ const auto finish = std::chrono::high_resolution_clock::now();
+
+ // Calculate the duration (used when benchmarking)
+ const auto duration =
+ std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
+
+ for (unsigned int i = 0; i < poses.size() - 1; i++) {
+ auto& p0 = poses[i];
+ auto& p1 = poses[i + 1];
+
+ // Make sure the twist is under the tolerance defined by the Spline class.
+ auto twist = p0.first.Log(p1.first);
+ EXPECT_LT(std::abs(twist.dx.to<double>()),
+ SplineParameterizer::kMaxDx.to<double>());
+ EXPECT_LT(std::abs(twist.dy.to<double>()),
+ SplineParameterizer::kMaxDy.to<double>());
+ EXPECT_LT(std::abs(twist.dtheta.to<double>()),
+ SplineParameterizer::kMaxDtheta.to<double>());
+ }
+
+ // Check first point.
+ EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
+ a.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
+ a.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
+ a.Rotation().Radians().to<double>(), 1E-9);
+
+ // Check last point.
+ EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
+ b.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
+ b.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
+ b.Rotation().Radians().to<double>(), 1E-9);
+
+ static_cast<void>(duration);
+ }
+};
+} // namespace frc
+
+TEST_F(CubicHermiteSplineTest, StraightLine) {
+ Run(Pose2d(), std::vector<Translation2d>(), Pose2d(3_m, 0_m, Rotation2d()));
+}
+
+TEST_F(CubicHermiteSplineTest, SCurve) {
+ Pose2d start{0_m, 0_m, Rotation2d(90_deg)};
+ std::vector<Translation2d> waypoints{Translation2d(1_m, 1_m),
+ Translation2d(2_m, -1_m)};
+ Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
+ Run(start, waypoints, end);
+}
diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
new file mode 100644
index 0000000..cc10c6c
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <chrono>
+#include <iostream>
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace frc {
+class QuinticHermiteSplineTest : public ::testing::Test {
+ protected:
+ static void Run(const Pose2d& a, const Pose2d& b) {
+ // Start the timer.
+ const auto start = std::chrono::high_resolution_clock::now();
+
+ // Generate and parameterize the spline.
+ const auto spline = SplineHelper::QuinticSplinesFromControlVectors(
+ SplineHelper::QuinticControlVectorsFromWaypoints({a, b}))[0];
+ const auto poses = SplineParameterizer::Parameterize(spline);
+
+ // End timer.
+ const auto finish = std::chrono::high_resolution_clock::now();
+
+ // Calculate the duration (used when benchmarking)
+ const auto duration =
+ std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
+
+ for (unsigned int i = 0; i < poses.size() - 1; i++) {
+ auto& p0 = poses[i];
+ auto& p1 = poses[i + 1];
+
+ // Make sure the twist is under the tolerance defined by the Spline class.
+ auto twist = p0.first.Log(p1.first);
+ EXPECT_LT(std::abs(twist.dx.to<double>()),
+ SplineParameterizer::kMaxDx.to<double>());
+ EXPECT_LT(std::abs(twist.dy.to<double>()),
+ SplineParameterizer::kMaxDy.to<double>());
+ EXPECT_LT(std::abs(twist.dtheta.to<double>()),
+ SplineParameterizer::kMaxDtheta.to<double>());
+ }
+
+ // Check first point.
+ EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
+ a.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
+ a.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
+ a.Rotation().Radians().to<double>(), 1E-9);
+
+ // Check last point.
+ EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
+ b.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
+ b.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
+ b.Rotation().Radians().to<double>(), 1E-9);
+
+ static_cast<void>(duration);
+ }
+};
+} // namespace frc
+
+TEST_F(QuinticHermiteSplineTest, StraightLine) {
+ Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d()));
+}
+
+TEST_F(QuinticHermiteSplineTest, SimpleSCurve) {
+ Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d()));
+}
+
+TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
+ Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
+ Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
new file mode 100644
index 0000000..37f471a
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <memory>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(CentripetalAccelerationConstraintTest, Constraint) {
+ const auto maxCentripetalAcceleration = 7_fps_sq;
+
+ auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+ config.AddConstraint(
+ CentripetalAccelerationConstraint(maxCentripetalAcceleration));
+
+ auto trajectory = TestTrajectory::GetTrajectory(config);
+
+ units::second_t time = 0_s;
+ units::second_t dt = 20_ms;
+ units::second_t duration = trajectory.TotalTime();
+
+ while (time < duration) {
+ const Trajectory::State point = trajectory.Sample(time);
+ time += dt;
+
+ auto centripetalAcceleration =
+ units::math::pow<2>(point.velocity) * point.curvature / 1_rad;
+
+ EXPECT_TRUE(centripetalAcceleration <
+ maxCentripetalAcceleration + 0.05_mps_sq);
+ }
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
new file mode 100644
index 0000000..df5ddbd
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <memory>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(DifferentialDriveKinematicsConstraintTest, Constraint) {
+ const auto maxVelocity = 12_fps;
+ const DifferentialDriveKinematics kinematics{27_in};
+
+ auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+ config.AddConstraint(
+ DifferentialDriveKinematicsConstraint(kinematics, maxVelocity));
+
+ auto trajectory = TestTrajectory::GetTrajectory(config);
+
+ units::second_t time = 0_s;
+ units::second_t dt = 20_ms;
+ units::second_t duration = trajectory.TotalTime();
+
+ while (time < duration) {
+ const Trajectory::State point = trajectory.Sample(time);
+ time += dt;
+
+ const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
+ point.velocity * point.curvature};
+
+ auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+ EXPECT_TRUE(left < maxVelocity + 0.05_mps);
+ EXPECT_TRUE(right < maxVelocity + 0.05_mps);
+ }
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
new file mode 100644
index 0000000..7e47e11
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(TrajectoryGenerationTest, ObeysConstraints) {
+ TrajectoryConfig config{12_fps, 12_fps_sq};
+ auto trajectory = TestTrajectory::GetTrajectory(config);
+
+ units::second_t time = 0_s;
+ units::second_t dt = 20_ms;
+ units::second_t duration = trajectory.TotalTime();
+
+ while (time < duration) {
+ const Trajectory::State point = trajectory.Sample(time);
+ time += dt;
+
+ EXPECT_TRUE(units::math::abs(point.velocity) <= 12_fps + 0.01_fps);
+ EXPECT_TRUE(units::math::abs(point.acceleration) <=
+ 12_fps_sq + 0.01_fps_sq);
+ }
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
new file mode 100644
index 0000000..ddd6a70
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -0,0 +1,223 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrapezoidProfile.h" // NOLINT(build/include_order)
+
+#include <chrono>
+#include <cmath>
+
+#include "gtest/gtest.h"
+
+static constexpr auto kDt = 10_ms;
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+ EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
+ if (val1 <= val2) { \
+ EXPECT_LE(val1, val2); \
+ } else { \
+ EXPECT_NEAR_UNITS(val1, val2, eps); \
+ }
+
+TEST(TrapezoidProfileTest, ReachesGoal) {
+ frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State goal{3_m, 0_mps};
+ frc::TrapezoidProfile::State state;
+
+ for (int i = 0; i < 450; ++i) {
+ frc::TrapezoidProfile profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+// Tests that decreasing the maximum velocity in the middle when it is already
+// moving faster than the new max is handled correctly
+TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
+ frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State goal{12_m, 0_mps};
+
+ frc::TrapezoidProfile profile{constraints, goal};
+ auto state = profile.Calculate(kDt);
+
+ auto lastPos = state.position;
+ for (int i = 0; i < 1600; ++i) {
+ if (i == 400) {
+ constraints.maxVelocity = 0.75_mps;
+ }
+
+ profile = frc::TrapezoidProfile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ auto estimatedVel = (state.position - lastPos) / kDt;
+
+ if (i >= 400) {
+ // Since estimatedVel can have floating point rounding errors, we check
+ // whether value is less than or within an error delta of the new
+ // constraint.
+ EXPECT_LT_OR_NEAR_UNITS(estimatedVel, constraints.maxVelocity, 1e-4_mps);
+
+ EXPECT_LE(state.velocity, constraints.maxVelocity);
+ }
+
+ lastPos = state.position;
+ }
+ EXPECT_EQ(state, goal);
+}
+
+// There is some somewhat tricky code for dealing with going backwards
+TEST(TrapezoidProfileTest, Backwards) {
+ frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+ frc::TrapezoidProfile::State state;
+
+ for (int i = 0; i < 400; ++i) {
+ frc::TrapezoidProfile profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
+ frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+ frc::TrapezoidProfile::State state;
+
+ for (int i = 0; i < 200; ++i) {
+ frc::TrapezoidProfile profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_NE(state, goal);
+
+ goal = {0.0_m, 0.0_mps};
+ for (int i = 0; i < 550; ++i) {
+ frc::TrapezoidProfile profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+// Checks to make sure that it hits top speed
+TEST(TrapezoidProfileTest, TopSpeed) {
+ frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State goal{4_m, 0_mps};
+ frc::TrapezoidProfile::State state;
+
+ for (int i = 0; i < 200; ++i) {
+ frc::TrapezoidProfile profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
+
+ for (int i = 0; i < 2000; ++i) {
+ frc::TrapezoidProfile profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ }
+ EXPECT_EQ(state, goal);
+}
+
+TEST(TrapezoidProfileTest, TimingToCurrent) {
+ frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State goal{2_m, 0_mps};
+ frc::TrapezoidProfile::State state;
+
+ for (int i = 0; i < 400; i++) {
+ frc::TrapezoidProfile profile{constraints, goal, state};
+ state = profile.Calculate(kDt);
+ EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
+ }
+}
+
+TEST(TrapezoidProfileTest, TimingToGoal) {
+ using units::unit_cast;
+
+ frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State goal{2_m, 0_mps};
+
+ frc::TrapezoidProfile profile{constraints, goal};
+ auto state = profile.Calculate(kDt);
+
+ auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
+ bool reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = frc::TrapezoidProfile(constraints, goal, state);
+ state = profile.Calculate(kDt);
+ if (!reachedGoal && state == goal) {
+ // Expected value using for loop index is just an approximation since the
+ // time left in the profile doesn't increase linearly at the endpoints
+ EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
+ reachedGoal = true;
+ }
+ }
+}
+
+TEST(TrapezoidProfileTest, TimingBeforeGoal) {
+ using units::unit_cast;
+
+ frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State goal{2_m, 0_mps};
+
+ frc::TrapezoidProfile profile{constraints, goal};
+ auto state = profile.Calculate(kDt);
+
+ auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
+ bool reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = frc::TrapezoidProfile(constraints, goal, state);
+ state = profile.Calculate(kDt);
+ if (!reachedGoal &&
+ (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
+ EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
+ reachedGoal = true;
+ }
+ }
+}
+
+TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
+ using units::unit_cast;
+
+ frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+
+ frc::TrapezoidProfile profile{constraints, goal};
+ auto state = profile.Calculate(kDt);
+
+ auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
+ bool reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = frc::TrapezoidProfile(constraints, goal, state);
+ state = profile.Calculate(kDt);
+ if (!reachedGoal && state == goal) {
+ // Expected value using for loop index is just an approximation since the
+ // time left in the profile doesn't increase linearly at the endpoints
+ EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
+ reachedGoal = true;
+ }
+ }
+}
+
+TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
+ using units::unit_cast;
+
+ frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
+ frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+
+ frc::TrapezoidProfile profile{constraints, goal};
+ auto state = profile.Calculate(kDt);
+
+ auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
+ bool reachedGoal = false;
+ for (int i = 0; i < 400; i++) {
+ profile = frc::TrapezoidProfile(constraints, goal, state);
+ state = profile.Calculate(kDt);
+ if (!reachedGoal &&
+ (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
+ EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
+ reachedGoal = true;
+ }
+ }
+}
diff --git a/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h b/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h
index f56215c..1cba8e3 100644
--- a/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h
+++ b/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,15 +9,17 @@
#include <wpi/StringRef.h>
-#include "frc/smartdashboard/SendableBase.h"
+#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
/**
* A mock sendable that marks itself as an actuator.
*/
-class MockActuatorSendable : public SendableBase {
+class MockActuatorSendable : public Sendable,
+ public SendableHelper<MockActuatorSendable> {
public:
explicit MockActuatorSendable(wpi::StringRef name);
diff --git a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h b/wpilibc/src/test/native/include/trajectory/TestTrajectory.h
new file mode 100644
index 0000000..4a496d7
--- /dev/null
+++ b/wpilibc/src/test/native/include/trajectory/TestTrajectory.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+class TestTrajectory {
+ public:
+ static Trajectory GetTrajectory(TrajectoryConfig& config) {
+ // 2018 cross scale auto waypoints
+ const Pose2d sideStart{1.54_ft, 23.23_ft, Rotation2d(180_deg)};
+ const Pose2d crossScale{23.7_ft, 6.8_ft, Rotation2d(-160_deg)};
+
+ config.SetReversed(true);
+
+ auto vector = std::vector<Translation2d>{
+ (sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()})
+ .Translation(),
+ (sideStart +
+ Transform2d{Translation2d(-19.5_ft, 5.0_ft), Rotation2d(-90_deg)})
+ .Translation()};
+
+ return TrajectoryGenerator::GenerateTrajectory(sideStart, vector,
+ crossScale, config);
+ }
+};
+
+} // namespace frc
diff --git a/wpilibc/wpilibc-config.cmake b/wpilibc/wpilibc-config.cmake
deleted file mode 100644
index 86f077c..0000000
--- a/wpilibc/wpilibc-config.cmake
+++ /dev/null
@@ -1,10 +0,0 @@
-include(CMakeFindDependencyMacro)
-find_dependency(wpiutil)
-find_dependency(ntcore)
-find_dependency(cscore)
-find_dependency(cameraserver)
-find_dependency(hal)
-find_dependency(OpenCV)
-
-get_filename_component(SELF_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
-include(${SELF_DIR}/wpilibc.cmake)
diff --git a/wpilibc/wpilibc-config.cmake.in b/wpilibc/wpilibc-config.cmake.in
new file mode 100644
index 0000000..4332c55
--- /dev/null
+++ b/wpilibc/wpilibc-config.cmake.in
@@ -0,0 +1,9 @@
+include(CMakeFindDependencyMacro)
+@FILENAME_DEP_REPLACE@
+@WPIUTIL_DEP_REPLACE@
+@NTCORE_DEP_REPLACE@
+@CSCORE_DEP_REPLACE@
+@CAMERASERVER_DEP_REPLACE@
+@HAL_DEP_REPLACE@
+
+include(${SELF_DIR}/wpilibc.cmake)