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/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/RobotBase.cpp b/wpilibc/src/main/native/cpp/RobotBase.cpp
deleted file mode 100644
index bc316f4..0000000
--- a/wpilibc/src/main/native/cpp/RobotBase.cpp
+++ /dev/null
@@ -1,125 +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/RobotBase.h"
-
-#include <cstdio>
-
-#include <cameraserver/CameraServerShared.h>
-#include <cscore.h>
-#include <hal/HAL.h>
-#include <networktables/NetworkTableInstance.h>
-
-#include "WPILibVersion.h"
-#include "frc/DriverStation.h"
-#include "frc/RobotState.h"
-#include "frc/Utility.h"
-#include "frc/WPIErrors.h"
-#include "frc/livewindow/LiveWindow.h"
-#include "frc/smartdashboard/SmartDashboard.h"
-
-using namespace frc;
-
-int frc::RunHALInitialization() {
- if (!HAL_Initialize(500, 0)) {
- wpi::errs() << "FATAL ERROR: HAL could not be initialized\n";
- return -1;
- }
- HAL_Report(HALUsageReporting::kResourceType_Language,
- HALUsageReporting::kLanguage_CPlusPlus);
- wpi::outs() << "\n********** Robot program starting **********\n";
- return 0;
-}
-
-std::thread::id RobotBase::m_threadId;
-
-namespace {
-class WPILibCameraServerShared : public frc::CameraServerShared {
- public:
- void ReportUsbCamera(int id) override {
- HAL_Report(HALUsageReporting::kResourceType_UsbCamera, id);
- }
- void ReportAxisCamera(int id) override {
- HAL_Report(HALUsageReporting::kResourceType_AxisCamera, id);
- }
- void ReportVideoServer(int id) override {
- HAL_Report(HALUsageReporting::kResourceType_PCVideoServer, id);
- }
- void SetCameraServerError(const wpi::Twine& error) override {
- wpi_setGlobalWPIErrorWithContext(CameraServerError, error);
- }
- void SetVisionRunnerError(const wpi::Twine& error) override {
- wpi_setGlobalErrorWithContext(-1, error);
- }
- void ReportDriverStationError(const wpi::Twine& error) override {
- DriverStation::ReportError(error);
- }
- std::pair<std::thread::id, bool> GetRobotMainThreadId() const override {
- return std::make_pair(RobotBase::GetThreadId(), true);
- }
-};
-} // namespace
-
-static void SetupCameraServerShared() {
- SetCameraServerShared(std::make_unique<WPILibCameraServerShared>());
-}
-
-bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
-
-bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
-
-bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
-
-bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
-
-bool RobotBase::IsTest() const { return m_ds.IsTest(); }
-
-bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); }
-
-std::thread::id RobotBase::GetThreadId() { return m_threadId; }
-
-RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
- if (!HAL_Initialize(500, 0)) {
- wpi::errs() << "FATAL ERROR: HAL could not be initialized\n";
- wpi::errs().flush();
- std::terminate();
- }
- m_threadId = std::this_thread::get_id();
-
- SetupCameraServerShared();
-
- auto inst = nt::NetworkTableInstance::GetDefault();
- inst.SetNetworkIdentity("Robot");
- inst.StartServer("/home/lvuser/networktables.ini");
-
- SmartDashboard::init();
-
- if (IsReal()) {
- std::FILE* file = nullptr;
- file = std::fopen("/tmp/frc_versions/FRC_Lib_Version.ini", "w");
-
- if (file != nullptr) {
- std::fputs("C++ ", file);
- std::fputs(GetWPILibVersion(), file);
- std::fclose(file);
- }
- }
-
- // First and one-time initialization
- inst.GetTable("LiveWindow")
- ->GetSubTable(".status")
- ->GetEntry("LW Enabled")
- .SetBoolean(false);
-
- LiveWindow::GetInstance()->SetEnabled(false);
-}
-
-RobotBase::RobotBase(RobotBase&&) : m_ds(DriverStation::GetInstance()) {}
-
-RobotBase::~RobotBase() { cs::Shutdown(); }
-
-RobotBase& RobotBase::operator=(RobotBase&&) { return *this; }
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 {};
+}