Squashed 'third_party/allwpilib_2019/' changes from 936627bd9..a3f7420da
e20d96ea4 Use __has_include for WPILib.h (#2164)
a76d006a0 Update native-utils to 2020.7.2 (#2161)
24c031d69 Increase SPI auto byte count to allow 32 bytes to be sent (#2163)
6b4eecf5f Add hidden functions to get the SPI system and SPI DMA (#2162)
ccdd0fbdb Add TrapezoidProfile external PID examples (#2131)
5c6b8a0f4 Replace std::is_pod_v with std::is_standard_layout_v (#2159)
67d2fed68 Add DutyCycleEncoder channel constructor (#2158)
d8f11eb14 Hardcode channels for LSB weight (#2153)
b2ae75acd Add way to disable "no extensions found" message (#2134)
4f951789f Build testbench tests online inorder to improve speed (#2144)
005c4c5be Try catch around task dependencies to fix loading in editor (#2156)
34f6b3f4c Fix C++ RamseteCommand param doxygen (#2157)
f7a93713f Fix up templated TrapezoidProfile classes (#2151)
8c2ff94d7 Rename MathUtils to MathUtil for consistency with other util classes (#2155)
d003ec2dc Update to 2020v9 image (#2154)
8e7cc3fe7 Add user-friendly SimDeviceSim wrappers (#2150)
6c8f6cf47 Fix bug in cubic and quintic hermetic spline generation (#2139)
e37ecd33a Sim GUI: Add support for LED displays (#2138)
57c5523d6 Fix documentation in RamseteCommand (#2149)
7b9c6ebc2 Fix wpiutilJNIShared linkage typo in wpilibj (#2143)
9a515c80f Template C++ LinearFilter to work with unit types (#2142)
5b73c17f2 Remove encoder velocities methods in DifferentialDriveOdometry (#2147)
b8c102426 Fix PS3Eye VID and PID (#2146)
2622c6c29 Add default values for b and zeta in RamseteController (#2145)
f66ae5999 Add HSV helpers to AddressableLED (#2135)
5e97c81d8 Add MedianFilter class for moving-window median (#2136)
f79b7a058 Remove unnecessary constructor arg for LinearFilter's circular buffers (#2140)
e49494830 Replace Jenkins with Azure agent (#1914)
b67d049ac Check status of PDP CAN reads (#2126)
70102a60b SendableRegistry.foreachLiveWindow: Prevent concurrent modification (#2129)
6dcd2b0e2 Improve various subsystem APIs (#2130)
ce3973435 HAL_CAN_ReceiveMessage: return MessageNotFound if no callbacks registered (#2133)
3fcfc8ea7 Fix double disable segfaulting interrupts (#2132)
6ceafe3cd Fix class reference for logger (#2123)
b058dcf64 Catch exceptions generated by OpenCV in cscore JNI (#2118)
0b9307fdf Remove unused parts of .styleguide files (#2119)
39be812b2 Fix C++ ArmFeedforward (#2120)
21e957ee4 Add DifferentialDrive voltage constraint (#2075)
e0bc97f66 Add ProfiledPIDSubsystem example (#2076)
3df44c874 Remove Rotation2d.h wpi/math include (#2117)
a58dbec8a Add holonomic follower examples (#2052)
9a8067465 Fix incomplete .styleguide (#2113)
ffa4b907c Fix C++ floating point literal formatting (#2114)
3d1ca856b Fix missing typename and return type (#2115)
5f85457a9 Add usage reporting for AddressableLED (#2108)
4ebae1712 Enforce leading/trailing zeros in Java numeric constants (#2105)
fa85fbfc1 Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)
f62e23f1a Add Doxygen for new HAL interfaces (#2110)
5443fdabc Directly construct PWM port from HAL, avoid wpilib PWM object (#2106)
c0e36df9d Standardize on PWMVictorSPX in examples (#2104)
8c4d9f541 Add TrapezoidProfileSubsystem (#2077)
45201d15f Add encoder distance overload to DifferentialDriveOdometry (#2096)
845aba33f Make feedforward classes constexpr (#2103)
500c43fb8 Add examples for DMA, DutyCycle, DutyCycleEncoder and AddressableLED (#2100)
589162811 Use DifferentialDriveWheelSpeeds in RamseteCommand ctor (#2091)
b37b68daa Add JRE deployment to MyRobot Deploy (#2099)
0e83c65d2 Fix small logic error in ParallelDeadlineGroup (#2095)
6f6c6da9f Updates to addressable LED (#2098)
1894219ef Fix devmain package in commands (#2097)
77a9949bb Add AddressableLED simulation GUI support
a4c9e4ec2 Add AddressableLED simulation support
8ed205907 Add AddressableLED (#2092)
59507b12d Bump to 2020 v7 image (#2086)
5d39bf806 Make halsimgui::DrawLEDs() values parameter const (#2088)
841ef91c0 Use gyro angle instead of robot angle for odometry (#2081)
1b66ead49 Use standard constant for pi instead of 3.14 (#2084)
db2c3dddd Use DMA Global Number for DMA Index (#2085)
82b2170fe Add DMA support to HAL and WPILibC (#2080)
8280b7e3a Add DutyCycleEncoder and AnalogEncoder (#2040)
551096006 Use kNumSystems for DutyCycle count in Ports (#2083)
df1065218 Remove release configs of deploy in MyRobot (#2082)
bf5388393 Add deploy options to myRobot (#2079)
b7bc1ea74 Update to 2020v6 image (#2078)
708009cd2 Update to gradle 6.0 (#2074)
3cce61b89 Add SmartDashboard::GetEntry function in C++ (#2064)
565e1f3e7 Fix spelling in MecanumDriveOdometry docs (#2072)
1853f7b6b Add timing window to simulation GUI
c5a049712 Add simulation pause/resume/step support
f5446c740 Add Notifier HALSIM access
3e049e02f Add name to HAL Notifier
2da64d15f Make usage reporting enums type match (#2069)
f04d95e50 Make FRCUsageReporting.h C-compatible (#2070)
d748c67a5 Generate docs for command libraries and fix doclint enable (#2071)
55a7f2b4a Add template for old command-based style (#2031)
486fa9c69 Add XboxController examples for arcade and tank drive (#2058)
e3dd1c5d7 Fix small bug in SplineHelper (#2061)
7dc7c71b5 Add feedforward components (#2045)
5f33d6af1 Fix ProfiledPIDSubsystem parameter name (#2017)
94843adb8 Standardize documentation of Speed Controllers bounds (#2043)
9bcff37b9 Add HAL specific version of wpi_setError (#2055)
326aecc9a Add error message for CAN Write Overrun (#2062)
00228678d Add requirements param to more Command APIs (#2059)
ff39a96ce Make DigitalOutput a DigitalSource (#2054)
5ccad2e8a Fix frc2::Timer returning incorrect timestamp values (#2057)
629e95776 Add VendorDeps JSON files for command libraries (#2048)
6858a57f7 Make notifier command tests a lot more lenient (#2056)
0ebe32823 Fix MyRobotStatic accidentally linking to shared command libs (#2046)
384d00f9e Fix various duty cycle bugs (#2047)
1f6850adf Add CAN Manufacturer for Studica (#2050)
7508aada9 Add ability to end startCompetition() main loop (#2032)
f5b4be16d Old C++ Command: Make GetName et al public (#2042)
e6f5c93ab Clean up new C++ commands (#2027)
39f46ceab Don't allow rising and falling values to be read from AnalogTrigger (#2039)
d93aa2b6b Add missing lock in FRCDriverStation (#2034)
114ddaf81 Fix duplicate encoders in examples (#2033)
f22d0961e Sim GUI: Add duty cycle support
3262c2bad Sim GUI: Use new multi-channel PDP getter function
96d40192a Revert accidental change to MyRobot.cpp (#2029)
ed30d5d40 Add JSON support for Trajectories (#2025)
2b6811edd Fix null pointer dereference in C++ CommandScheduler (#2023)
1d695a166 Add FPGA Duty Cycle support (#1987)
509819d83 Split the two command implementations into separate libraries (#2012)
2ad15cae1 Add multi PDP getter and sim PCM/PDP multi arg functions (#2014)
931b8ceef Add new usage reporting types from 2020v5 (#2026)
0b3821eba Change files with CRLF line endings to LF (#2022)
6f159d142 Add way to atomically check for new data, and wait otherwise (#2015)
a769f1f22 Fix bug in RamseteCommand (using degrees instead of radians) (#2020)
c5186d815 Clean up PIDCommand (#2010)
9ebd23d61 Add setVoltage method to SpeedController (#1997)
f6e311ef8 Fix SplineHelper bug (#2018)
f33bd9f05 Fix NPE in RamseteCommand (#2019)
1c1e0c9a6 Add HAL_SetAllSolenoids to sim (#2004)
ea9bb651a Remove accidental OpenCV link from wpilibc shared library (#2013)
cc0742518 Change command decorators to return implementation (#2007)
16b34cce2 Remove IterativeRobot templates (#2011)
669127e49 Update intellisense to work with Beta 2020 code (#2008)
9dc30797e Fix usage reporting indices (#2009)
f6b844ea3 Move HAL Interrupt struct to anonymous namespace (#2003)
a72f80991 Add extern C to DigitalGlitchFilterJNI (#2002)
916596cb0 Fix invalid examples json, add validator (#2001)
5509a8e96 Use constexpr for all example constants
0be6b6475 Use constexpr for DifferentialDriveKinematics
Change-Id: I1416646cbab487ad8021830215766d8ec7f24ddc
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: a3f7420dab7a104c27a0c3bf0872c999c98fd9a9
diff --git a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
index 5b5762a..a760daa 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
@@ -7,7 +7,7 @@
#include "frc/ADXL345_I2C.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
diff --git a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
index bb9275a..077a7ff 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
@@ -7,7 +7,7 @@
#include "frc/ADXL345_SPI.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
diff --git a/wpilibc/src/main/native/cpp/ADXL362.cpp b/wpilibc/src/main/native/cpp/ADXL362.cpp
index 2f97f1b..6ed8c8c 100644
--- a/wpilibc/src/main/native/cpp/ADXL362.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL362.cpp
@@ -7,7 +7,7 @@
#include "frc/ADXL362.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
#include "frc/smartdashboard/SendableBuilder.h"
@@ -72,7 +72,7 @@
commands[2] = kPowerCtl_Measure | kPowerCtl_UltraLowNoise;
m_spi.Write(commands, 3);
- HAL_Report(HALUsageReporting::kResourceType_ADXL362, port);
+ HAL_Report(HALUsageReporting::kResourceType_ADXL362, port + 1);
SendableRegistry::GetInstance().AddLW(this, "ADXL362", port);
}
diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
index 3dde234..2cf2f73 100644
--- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
+++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
@@ -7,7 +7,7 @@
#include "frc/ADXRS450_Gyro.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
#include "frc/Timer.h"
@@ -57,7 +57,7 @@
Calibrate();
}
- HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port);
+ HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port + 1);
SendableRegistry::GetInstance().AddLW(this, "ADXRS450_Gyro", port);
}
diff --git a/wpilibc/src/main/native/cpp/AddressableLED.cpp b/wpilibc/src/main/native/cpp/AddressableLED.cpp
new file mode 100644
index 0000000..b0c3a45
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AddressableLED.cpp
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* 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/AddressableLED.h"
+
+#include <hal/AddressableLED.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/PWM.h>
+#include <hal/Ports.h>
+
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+AddressableLED::AddressableLED(int port) {
+ int32_t status = 0;
+
+ m_pwmHandle = HAL_InitializePWMPort(HAL_GetPort(port), &status);
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPWMChannels(), port);
+ if (m_pwmHandle == HAL_kInvalidHandle) {
+ return;
+ }
+
+ m_handle = HAL_InitializeAddressableLED(m_pwmHandle, &status);
+ wpi_setHALError(status);
+ if (m_handle == HAL_kInvalidHandle) {
+ HAL_FreePWMPort(m_pwmHandle, &status);
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_AddressableLEDs, port + 1);
+}
+
+AddressableLED::~AddressableLED() {
+ HAL_FreeAddressableLED(m_handle);
+ int32_t status = 0;
+ HAL_FreePWMPort(m_pwmHandle, &status);
+}
+
+void AddressableLED::SetLength(int length) {
+ int32_t status = 0;
+ HAL_SetAddressableLEDLength(m_handle, length, &status);
+ wpi_setHALError(status);
+}
+
+static_assert(sizeof(AddressableLED::LEDData) == sizeof(HAL_AddressableLEDData),
+ "LED Structs MUST be the same size");
+
+void AddressableLED::SetData(wpi::ArrayRef<LEDData> ledData) {
+ int32_t status = 0;
+ HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
+ &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::SetData(std::initializer_list<LEDData> ledData) {
+ int32_t status = 0;
+ HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
+ &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::SetBitTiming(units::nanosecond_t lowTime0,
+ units::nanosecond_t highTime0,
+ units::nanosecond_t lowTime1,
+ units::nanosecond_t highTime1) {
+ int32_t status = 0;
+ HAL_SetAddressableLEDBitTiming(
+ m_handle, lowTime0.to<int32_t>(), highTime0.to<int32_t>(),
+ lowTime1.to<int32_t>(), highTime1.to<int32_t>(), &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::SetSyncTime(units::microsecond_t syncTime) {
+ int32_t status = 0;
+ HAL_SetAddressableLEDSyncTime(m_handle, syncTime.to<int32_t>(), &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::Start() {
+ int32_t status = 0;
+ HAL_StartAddressableLEDOutput(m_handle, &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::Stop() {
+ int32_t status = 0;
+ HAL_StopAddressableLEDOutput(m_handle, &status);
+ wpi_setHALError(status);
+}
+
+void AddressableLED::LEDData::SetHSV(int h, int s, int v) {
+ if (s == 0) {
+ SetRGB(v, v, v);
+ return;
+ }
+
+ int region = h / 30;
+ int remainder = (h - (region * 30)) * 6;
+
+ int p = (v * (255 - s)) >> 8;
+ int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
+ int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
+
+ switch (region) {
+ case 0:
+ SetRGB(v, t, p);
+ break;
+ case 1:
+ SetRGB(q, v, p);
+ break;
+ case 2:
+ SetRGB(p, v, t);
+ break;
+ case 3:
+ SetRGB(p, q, v);
+ break;
+ case 4:
+ SetRGB(t, p, v);
+ break;
+ default:
+ SetRGB(v, p, q);
+ break;
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
index 4436bcf..be0c7d2 100644
--- a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
@@ -7,7 +7,7 @@
#include "frc/AnalogAccelerometer.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
@@ -58,7 +58,7 @@
void AnalogAccelerometer::InitAccelerometer() {
HAL_Report(HALUsageReporting::kResourceType_Accelerometer,
- m_analogInput->GetChannel());
+ m_analogInput->GetChannel() + 1);
SendableRegistry::GetInstance().AddLW(this, "Accelerometer",
m_analogInput->GetChannel());
diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
new file mode 100644
index 0000000..a194961
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AnalogEncoder.h"
+
+#include "frc/AnalogInput.h"
+#include "frc/Counter.h"
+#include "frc/DriverStation.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+AnalogEncoder::AnalogEncoder(AnalogInput& analogInput)
+ : m_analogInput{&analogInput, NullDeleter<AnalogInput>{}},
+ m_analogTrigger{m_analogInput.get()},
+ m_counter{} {
+ Init();
+}
+
+AnalogEncoder::AnalogEncoder(AnalogInput* analogInput)
+ : m_analogInput{analogInput, NullDeleter<AnalogInput>{}},
+ m_analogTrigger{m_analogInput.get()},
+ m_counter{} {
+ Init();
+}
+
+AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> analogInput)
+ : m_analogInput{std::move(analogInput)},
+ m_analogTrigger{m_analogInput.get()},
+ m_counter{} {
+ Init();
+}
+
+void AnalogEncoder::Init() {
+ m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()};
+
+ if (m_simDevice) {
+ m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
+ }
+
+ m_analogTrigger.SetLimitsVoltage(1.25, 3.75);
+ m_counter.SetUpSource(
+ m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse));
+ m_counter.SetDownSource(
+ m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse));
+
+ SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder",
+ m_analogInput->GetChannel());
+}
+
+units::turn_t AnalogEncoder::Get() const {
+ if (m_simPosition) return units::turn_t{m_simPosition.Get()};
+
+ // As the values are not atomic, keep trying until we get 2 reads of the same
+ // value If we don't within 10 attempts, error
+ for (int i = 0; i < 10; i++) {
+ auto counter = m_counter.Get();
+ auto pos = m_analogInput->GetVoltage();
+ auto counter2 = m_counter.Get();
+ auto pos2 = m_analogInput->GetVoltage();
+ if (counter == counter2 && pos == pos2) {
+ units::turn_t turns{counter + pos - m_positionOffset};
+ m_lastPosition = turns;
+ return turns;
+ }
+ }
+
+ frc::DriverStation::GetInstance().ReportWarning(
+ "Failed to read Analog Encoder. Potential Speed Overrun. Returning last "
+ "value");
+ return m_lastPosition;
+}
+
+double AnalogEncoder::GetPositionOffset() const { return m_positionOffset; }
+
+void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) {
+ m_distancePerRotation = distancePerRotation;
+}
+
+double AnalogEncoder::GetDistancePerRotation() const {
+ return m_distancePerRotation;
+}
+
+double AnalogEncoder::GetDistance() const {
+ return Get().to<double>() * GetDistancePerRotation();
+}
+
+void AnalogEncoder::Reset() {
+ m_counter.Reset();
+ m_positionOffset = m_analogInput->GetVoltage();
+}
+
+void AnalogEncoder::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("AbsoluteEncoder");
+ builder.AddDoubleProperty("Distance", [this] { return this->GetDistance(); },
+ nullptr);
+ builder.AddDoubleProperty("Distance Per Rotation",
+ [this] { return this->GetDistancePerRotation(); },
+ nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogGyro.cpp b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
index 36cde8a..c891506 100644
--- a/wpilibc/src/main/native/cpp/AnalogGyro.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
@@ -12,7 +12,7 @@
#include <hal/AnalogGyro.h>
#include <hal/Errors.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/AnalogInput.h"
#include "frc/Timer.h"
@@ -56,7 +56,7 @@
HAL_SetAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
offset, center, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_gyroHandle = HAL_kInvalidHandle;
return;
}
@@ -70,7 +70,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetAnalogGyroAngle(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -78,7 +78,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetAnalogGyroRate(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -86,7 +86,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetAnalogGyroCenter(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -94,7 +94,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetAnalogGyroOffset(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -102,21 +102,21 @@
int32_t status = 0;
HAL_SetAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle,
voltsPerDegreePerSecond, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogGyro::SetDeadband(double volts) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogGyroDeadband(m_gyroHandle, volts, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogGyro::Reset() {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_ResetAnalogGyro(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogGyro::InitGyro() {
@@ -132,7 +132,7 @@
return;
}
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_analog = nullptr;
m_gyroHandle = HAL_kInvalidHandle;
return;
@@ -142,13 +142,13 @@
int32_t status = 0;
HAL_SetupAnalogGyro(m_gyroHandle, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_analog = nullptr;
m_gyroHandle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel() + 1);
SendableRegistry::GetInstance().AddLW(this, "AnalogGyro",
m_analog->GetChannel());
@@ -158,5 +158,5 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_CalibrateAnalogGyro(m_gyroHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
diff --git a/wpilibc/src/main/native/cpp/AnalogInput.cpp b/wpilibc/src/main/native/cpp/AnalogInput.cpp
index a200f4d..c8197b7 100644
--- a/wpilibc/src/main/native/cpp/AnalogInput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogInput.cpp
@@ -11,7 +11,8 @@
#include <hal/AnalogAccumulator.h>
#include <hal/AnalogInput.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include "frc/SensorUtil.h"
@@ -35,14 +36,13 @@
int32_t status = 0;
m_port = HAL_InitializeAnalogInputPort(port, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumAnalogInputs(), channel,
- HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumAnalogInputs(), channel);
m_channel = std::numeric_limits<int>::max();
m_port = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel);
+ HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel + 1);
SendableRegistry::GetInstance().AddLW(this, "AnalogInput", channel);
}
@@ -53,7 +53,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetAnalogValue(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -61,7 +61,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetAnalogAverageValue(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -69,7 +69,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double voltage = HAL_GetAnalogVoltage(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return voltage;
}
@@ -77,7 +77,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double voltage = HAL_GetAnalogAverageVoltage(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return voltage;
}
@@ -90,13 +90,13 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogAverageBits(m_port, bits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int AnalogInput::GetAverageBits() const {
int32_t status = 0;
int averageBits = HAL_GetAnalogAverageBits(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return averageBits;
}
@@ -104,14 +104,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogOversampleBits(m_port, bits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int AnalogInput::GetOversampleBits() const {
if (StatusIsFatal()) return 0;
int32_t status = 0;
int oversampleBits = HAL_GetAnalogOversampleBits(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return oversampleBits;
}
@@ -119,7 +119,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int lsbWeight = HAL_GetAnalogLSBWeight(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return lsbWeight;
}
@@ -127,7 +127,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int offset = HAL_GetAnalogOffset(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return offset;
}
@@ -135,7 +135,7 @@
if (StatusIsFatal()) return false;
int32_t status = 0;
bool isAccum = HAL_IsAccumulatorChannel(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return isAccum;
}
@@ -144,7 +144,7 @@
m_accumulatorOffset = 0;
int32_t status = 0;
HAL_InitAccumulator(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogInput::SetAccumulatorInitialValue(int64_t initialValue) {
@@ -156,7 +156,7 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_ResetAccumulator(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
if (!StatusIsFatal()) {
// Wait until the next sample, so the next call to GetAccumulator*()
@@ -172,21 +172,21 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAccumulatorCenter(m_port, center, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogInput::SetAccumulatorDeadband(int deadband) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAccumulatorDeadband(m_port, deadband, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int64_t AnalogInput::GetAccumulatorValue() const {
if (StatusIsFatal()) return 0;
int32_t status = 0;
int64_t value = HAL_GetAccumulatorValue(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value + m_accumulatorOffset;
}
@@ -194,7 +194,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int64_t count = HAL_GetAccumulatorCount(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return count;
}
@@ -202,20 +202,20 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_GetAccumulatorOutput(m_port, &value, &count, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
value += m_accumulatorOffset;
}
void AnalogInput::SetSampleRate(double samplesPerSecond) {
int32_t status = 0;
HAL_SetAnalogSampleRate(samplesPerSecond, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
}
double AnalogInput::GetSampleRate() {
int32_t status = 0;
double sampleRate = HAL_GetAnalogSampleRate(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return sampleRate;
}
diff --git a/wpilibc/src/main/native/cpp/AnalogOutput.cpp b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
index 5e56efc..041b446 100644
--- a/wpilibc/src/main/native/cpp/AnalogOutput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
@@ -10,7 +10,9 @@
#include <limits>
#include <utility>
-#include <hal/HAL.h>
+#include <hal/AnalogOutput.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include "frc/SensorUtil.h"
@@ -35,14 +37,13 @@
int32_t status = 0;
m_port = HAL_InitializeAnalogOutputPort(port, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumAnalogOutputs(), channel,
- HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumAnalogOutputs(), channel);
m_channel = std::numeric_limits<int>::max();
m_port = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_AnalogOutput, m_channel);
+ HAL_Report(HALUsageReporting::kResourceType_AnalogOutput, m_channel + 1);
SendableRegistry::GetInstance().AddLW(this, "AnalogOutput", m_channel);
}
@@ -52,14 +53,14 @@
int32_t status = 0;
HAL_SetAnalogOutput(m_port, voltage, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double AnalogOutput::GetVoltage() const {
int32_t status = 0;
double voltage = HAL_GetAnalogOutput(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return voltage;
}
diff --git a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
index 60ce04a..ddbb7c8 100644
--- a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
@@ -9,9 +9,10 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/AnalogInput.h"
+#include "frc/DutyCycle.h"
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableRegistry.h"
@@ -26,19 +27,31 @@
AnalogTrigger::AnalogTrigger(AnalogInput* input) {
m_analogInput = input;
int32_t status = 0;
- int index = 0;
- m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &index, &status);
+ m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
- m_index = std::numeric_limits<int>::max();
+ wpi_setHALError(status);
m_trigger = HAL_kInvalidHandle;
return;
}
- m_index = index;
+ int index = GetIndex();
- HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, input->m_channel);
- SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger",
- input->GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
+ SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger", index);
+}
+
+AnalogTrigger::AnalogTrigger(DutyCycle* input) {
+ m_dutyCycle = input;
+ int32_t status = 0;
+ m_trigger = HAL_InitializeAnalogTriggerDutyCycle(input->m_handle, &status);
+ if (status != 0) {
+ wpi_setHALError(status);
+ m_trigger = HAL_kInvalidHandle;
+ return;
+ }
+ int index = GetIndex();
+
+ HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
+ SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger", index);
}
AnalogTrigger::~AnalogTrigger() {
@@ -53,9 +66,9 @@
AnalogTrigger::AnalogTrigger(AnalogTrigger&& rhs)
: ErrorBase(std::move(rhs)),
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_dutyCycle, rhs.m_dutyCycle);
std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
}
@@ -63,9 +76,9 @@
ErrorBase::operator=(std::move(rhs));
SendableHelper::operator=(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_dutyCycle, rhs.m_dutyCycle);
std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
return *this;
@@ -75,40 +88,50 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerLimitsVoltage(m_trigger, lower, upper, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
+}
+
+void AnalogTrigger::SetLimitsDutyCycle(double lower, double upper) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogTriggerLimitsDutyCycle(m_trigger, lower, upper, &status);
+ wpi_setHALError(status);
}
void AnalogTrigger::SetLimitsRaw(int lower, int upper) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerLimitsRaw(m_trigger, lower, upper, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogTrigger::SetAveraged(bool useAveragedValue) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerAveraged(m_trigger, useAveragedValue, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void AnalogTrigger::SetFiltered(bool useFilteredValue) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetAnalogTriggerFiltered(m_trigger, useFilteredValue, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int AnalogTrigger::GetIndex() const {
if (StatusIsFatal()) return -1;
- return m_index;
+ int32_t status = 0;
+ auto ret = HAL_GetAnalogTriggerFPGAIndex(m_trigger, &status);
+ wpi_setHALError(status);
+ return ret;
}
bool AnalogTrigger::GetInWindow() {
if (StatusIsFatal()) return false;
int32_t status = 0;
bool result = HAL_GetAnalogTriggerInWindow(m_trigger, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
@@ -116,7 +139,7 @@
if (StatusIsFatal()) return false;
int32_t status = 0;
bool result = HAL_GetAnalogTriggerTriggerState(m_trigger, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
diff --git a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
index c2c7265..8fba479 100644
--- a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
@@ -7,7 +7,7 @@
#include "frc/AnalogTriggerOutput.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/AnalogTrigger.h"
#include "frc/WPIErrors.h"
@@ -19,7 +19,7 @@
bool result = HAL_GetAnalogTriggerOutput(
m_trigger->m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
@@ -33,7 +33,7 @@
bool AnalogTriggerOutput::IsAnalogTrigger() const { return true; }
-int AnalogTriggerOutput::GetChannel() const { return m_trigger->m_index; }
+int AnalogTriggerOutput::GetChannel() const { return m_trigger->GetIndex(); }
void AnalogTriggerOutput::InitSendable(SendableBuilder&) {}
@@ -41,5 +41,5 @@
AnalogTriggerType outputType)
: m_trigger(&trigger), m_outputType(outputType) {
HAL_Report(HALUsageReporting::kResourceType_AnalogTriggerOutput,
- trigger.GetIndex(), static_cast<uint8_t>(outputType));
+ trigger.GetIndex() + 1, static_cast<uint8_t>(outputType) + 1);
}
diff --git a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
index 71c8e89..d7cdef6 100644
--- a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
@@ -8,7 +8,7 @@
#include "frc/BuiltInAccelerometer.h"
#include <hal/Accelerometer.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/WPIErrors.h"
#include "frc/smartdashboard/SendableBuilder.h"
diff --git a/wpilibc/src/main/native/cpp/CAN.cpp b/wpilibc/src/main/native/cpp/CAN.cpp
index 35a926a..797b9ff 100644
--- a/wpilibc/src/main/native/cpp/CAN.cpp
+++ b/wpilibc/src/main/native/cpp/CAN.cpp
@@ -13,7 +13,6 @@
#include <hal/CANAPI.h>
#include <hal/Errors.h>
#include <hal/FRCUsageReporting.h>
-#include <hal/HALBase.h>
using namespace frc;
@@ -22,12 +21,12 @@
m_handle =
HAL_InitializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_handle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId);
+ HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId + 1);
}
CAN::CAN(int deviceId, int deviceManufacturer, int deviceType) {
@@ -36,12 +35,12 @@
static_cast<HAL_CANManufacturer>(deviceManufacturer), deviceId,
static_cast<HAL_CANDeviceType>(deviceType), &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_handle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId);
+ HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId + 1);
}
CAN::~CAN() {
@@ -55,26 +54,26 @@
void CAN::WritePacket(const uint8_t* data, int length, int apiId) {
int32_t status = 0;
HAL_WriteCANPacket(m_handle, data, length, apiId, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void CAN::WritePacketRepeating(const uint8_t* data, int length, int apiId,
int repeatMs) {
int32_t status = 0;
HAL_WriteCANPacketRepeating(m_handle, data, length, apiId, repeatMs, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(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));
+ wpi_setHALError(status);
}
void CAN::StopPacketRepeating(int apiId) {
int32_t status = 0;
HAL_StopCANPacketRepeating(m_handle, apiId, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool CAN::ReadPacketNew(int apiId, CANData* data) {
@@ -85,7 +84,7 @@
return false;
}
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return false;
} else {
return true;
@@ -100,7 +99,7 @@
return false;
}
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return false;
} else {
return true;
@@ -116,7 +115,7 @@
return false;
}
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(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 10d75f1..29789be 100644
--- a/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -8,7 +8,7 @@
#include "frc/Compressor.h"
#include <hal/Compressor.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/Ports.h>
#include <hal/Solenoid.h>
@@ -22,13 +22,12 @@
int32_t status = 0;
m_compressorHandle = HAL_InitializeCompressor(m_module, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumPCMModules(), pcmID,
- HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPCMModules(), pcmID);
return;
}
SetClosedLoopControl(true);
- HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID);
+ HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID + 1);
SendableRegistry::GetInstance().AddLW(this, "Compressor", pcmID);
}
diff --git a/wpilibc/src/main/native/cpp/Counter.cpp b/wpilibc/src/main/native/cpp/Counter.cpp
index 1ef40fd..d2158ca 100644
--- a/wpilibc/src/main/native/cpp/Counter.cpp
+++ b/wpilibc/src/main/native/cpp/Counter.cpp
@@ -9,7 +9,8 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/AnalogTrigger.h"
#include "frc/DigitalInput.h"
@@ -22,11 +23,11 @@
Counter::Counter(Mode mode) {
int32_t status = 0;
m_counter = HAL_InitializeCounter((HAL_Counter_Mode)mode, &m_index, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
- SetMaxPeriod(.5);
+ SetMaxPeriod(0.5);
- HAL_Report(HALUsageReporting::kResourceType_Counter, m_index, mode);
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1);
SendableRegistry::GetInstance().AddLW(this, "Counter", m_index);
}
@@ -81,7 +82,7 @@
HAL_SetCounterAverageSize(m_counter, 2, &status);
}
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
SetDownSourceEdge(inverted, true);
}
@@ -90,7 +91,7 @@
int32_t status = 0;
HAL_FreeCounter(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetUpSource(int channel) {
@@ -128,7 +129,7 @@
m_counter, source->GetPortHandleForRouting(),
(HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(),
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
}
@@ -146,7 +147,7 @@
}
int32_t status = 0;
HAL_SetCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::ClearUpSource() {
@@ -154,7 +155,7 @@
m_upSource.reset();
int32_t status = 0;
HAL_ClearCounterUpSource(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetDownSource(int channel) {
@@ -197,7 +198,7 @@
m_counter, source->GetPortHandleForRouting(),
(HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(),
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
}
@@ -210,7 +211,7 @@
}
int32_t status = 0;
HAL_SetCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::ClearDownSource() {
@@ -218,42 +219,42 @@
m_downSource.reset();
int32_t status = 0;
HAL_ClearCounterDownSource(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetUpDownCounterMode() {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterUpDownMode(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetExternalDirectionMode() {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterExternalDirectionMode(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetPulseLengthMode(double threshold) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterPulseLengthMode(m_counter, threshold, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetReverseDirection(bool reverseDirection) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterReverseDirection(m_counter, reverseDirection, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetSamplesToAverage(int samplesToAverage) {
@@ -264,13 +265,13 @@
}
int32_t status = 0;
HAL_SetCounterSamplesToAverage(m_counter, samplesToAverage, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int Counter::GetSamplesToAverage() const {
int32_t status = 0;
int samples = HAL_GetCounterSamplesToAverage(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return samples;
}
@@ -280,7 +281,7 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetCounter(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -288,14 +289,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_ResetCounter(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double Counter::GetPeriod() const {
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetCounterPeriod(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -303,21 +304,21 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterMaxPeriod(m_counter, maxPeriod, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Counter::SetUpdateWhenEmpty(bool enabled) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetCounterUpdateWhenEmpty(m_counter, enabled, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool Counter::GetStopped() const {
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetCounterStopped(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -325,7 +326,7 @@
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetCounterDirection(m_counter, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
diff --git a/wpilibc/src/main/native/cpp/DMA.cpp b/wpilibc/src/main/native/cpp/DMA.cpp
new file mode 100644
index 0000000..1861555
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DMA.cpp
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* 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/DMA.h"
+
+#include <frc/AnalogInput.h>
+#include <frc/Counter.h>
+#include <frc/DigitalSource.h>
+#include <frc/DutyCycle.h>
+#include <frc/Encoder.h>
+
+#include <hal/DMA.h>
+#include <hal/HALBase.h>
+
+using namespace frc;
+
+DMA::DMA() {
+ int32_t status = 0;
+ dmaHandle = HAL_InitializeDMA(&status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+DMA::~DMA() { HAL_FreeDMA(dmaHandle); }
+
+void DMA::SetPause(bool pause) {
+ int32_t status = 0;
+ HAL_SetDMAPause(dmaHandle, pause, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::SetRate(int cycles) {
+ int32_t status = 0;
+ HAL_SetDMARate(dmaHandle, cycles, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddEncoder(const Encoder* encoder) {
+ int32_t status = 0;
+ HAL_AddDMAEncoder(dmaHandle, encoder->m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddEncoderPeriod(const Encoder* encoder) {
+ int32_t status = 0;
+ HAL_AddDMAEncoderPeriod(dmaHandle, encoder->m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddCounter(const Counter* counter) {
+ int32_t status = 0;
+ HAL_AddDMACounter(dmaHandle, counter->m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddCounterPeriod(const Counter* counter) {
+ int32_t status = 0;
+ HAL_AddDMACounterPeriod(dmaHandle, counter->m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddDigitalSource(const DigitalSource* digitalSource) {
+ int32_t status = 0;
+ HAL_AddDMADigitalSource(dmaHandle, digitalSource->GetPortHandleForRouting(),
+ &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddDutyCycle(const DutyCycle* dutyCycle) {
+ int32_t status = 0;
+ HAL_AddDMADutyCycle(dmaHandle, dutyCycle->m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddAnalogInput(const AnalogInput* analogInput) {
+ int32_t status = 0;
+ HAL_AddDMAAnalogInput(dmaHandle, analogInput->m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddAveragedAnalogInput(const AnalogInput* analogInput) {
+ int32_t status = 0;
+ HAL_AddDMAAveragedAnalogInput(dmaHandle, analogInput->m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddAnalogAccumulator(const AnalogInput* analogInput) {
+ int32_t status = 0;
+ HAL_AddDMAAnalogAccumulator(dmaHandle, analogInput->m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::SetExternalTrigger(DigitalSource* source, bool rising, bool falling) {
+ int32_t status = 0;
+ HAL_SetDMAExternalTrigger(dmaHandle, source->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ source->GetAnalogTriggerTypeForRouting()),
+ rising, falling, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::StartDMA(int queueDepth) {
+ int32_t status = 0;
+ HAL_StartDMA(dmaHandle, queueDepth, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::StopDMA() {
+ int32_t status = 0;
+ HAL_StopDMA(dmaHandle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
diff --git a/wpilibc/src/main/native/cpp/DMC60.cpp b/wpilibc/src/main/native/cpp/DMC60.cpp
index d61bd40..e7d2c65 100644
--- a/wpilibc/src/main/native/cpp/DMC60.cpp
+++ b/wpilibc/src/main/native/cpp/DMC60.cpp
@@ -7,32 +7,18 @@
#include "frc/DMC60.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
DMC60::DMC60(int channel) : PWMSpeedController(channel) {
- /*
- * Note that the DMC 60 uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the DMC 60 User
- * Manual available from Digilent.
- *
- * 2.004ms = full "forward"
- * 1.52ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.48ms = the "low end" of the deadband range
- * 0.997ms = full "reverse"
- */
- SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_DigilentDMC60, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_DigilentDMC60, GetChannel() + 1);
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 9e92da8..3cec3f1 100644
--- a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
@@ -13,7 +13,7 @@
#include <hal/Constants.h>
#include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/Counter.h"
#include "frc/Encoder.h"
@@ -38,7 +38,7 @@
*index = true;
HAL_Report(HALUsageReporting::kResourceType_DigitalGlitchFilter,
- m_channelIndex);
+ m_channelIndex + 1);
SendableRegistry::GetInstance().AddLW(this, "DigitalGlitchFilter",
m_channelIndex);
}
@@ -81,15 +81,12 @@
int32_t status = 0;
HAL_SetFilterSelect(input->GetPortHandleForRouting(), requestedIndex,
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Validate that we set it correctly.
int actualIndex =
HAL_GetFilterSelect(input->GetPortHandleForRouting(), &status);
wpi_assertEqual(actualIndex, requestedIndex);
-
- HAL_Report(HALUsageReporting::kResourceType_DigitalInput,
- input->GetChannel());
}
}
@@ -130,7 +127,7 @@
void DigitalGlitchFilter::SetPeriodCycles(int fpgaCycles) {
int32_t status = 0;
HAL_SetFilterPeriod(m_channelIndex, fpgaCycles, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void DigitalGlitchFilter::SetPeriodNanoSeconds(uint64_t nanoseconds) {
@@ -139,14 +136,14 @@
nanoseconds * HAL_GetSystemClockTicksPerMicrosecond() / 4 / 1000;
HAL_SetFilterPeriod(m_channelIndex, fpgaCycles, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int DigitalGlitchFilter::GetPeriodCycles() {
int32_t status = 0;
int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return fpgaCycles;
}
@@ -155,7 +152,7 @@
int32_t status = 0;
int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return static_cast<uint64_t>(fpgaCycles) * 1000L /
static_cast<uint64_t>(HAL_GetSystemClockTicksPerMicrosecond() / 4);
diff --git a/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp
index d0fa41b..7210750 100644
--- a/wpilibc/src/main/native/cpp/DigitalInput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -11,7 +11,8 @@
#include <utility>
#include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include "frc/SensorUtil.h"
@@ -33,14 +34,13 @@
int32_t status = 0;
m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), true, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumDigitalChannels(),
- channel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumDigitalChannels(), channel);
m_handle = HAL_kInvalidHandle;
m_channel = std::numeric_limits<int>::max();
return;
}
- HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel);
+ HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel + 1);
SendableRegistry::GetInstance().AddLW(this, "DigitalInput", channel);
}
@@ -53,7 +53,7 @@
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetDIO(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
diff --git a/wpilibc/src/main/native/cpp/DigitalOutput.cpp b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
index 2068b5d..6e9b09b 100644
--- a/wpilibc/src/main/native/cpp/DigitalOutput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
@@ -11,7 +11,8 @@
#include <utility>
#include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include "frc/SensorUtil.h"
@@ -34,14 +35,13 @@
int32_t status = 0;
m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), false, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumDigitalChannels(),
- channel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumDigitalChannels(), channel);
m_channel = std::numeric_limits<int>::max();
m_handle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel);
+ HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel + 1);
SendableRegistry::GetInstance().AddLW(this, "DigitalOutput", channel);
}
@@ -58,7 +58,7 @@
int32_t status = 0;
HAL_SetDIO(m_handle, value, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool DigitalOutput::Get() const {
@@ -66,10 +66,18 @@
int32_t status = 0;
bool val = HAL_GetDIO(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
+HAL_Handle DigitalOutput::GetPortHandleForRouting() const { return m_handle; }
+
+AnalogTriggerType DigitalOutput::GetAnalogTriggerTypeForRouting() const {
+ return (AnalogTriggerType)0;
+}
+
+bool DigitalOutput::IsAnalogTrigger() const { return false; }
+
int DigitalOutput::GetChannel() const { return m_channel; }
void DigitalOutput::Pulse(double length) {
@@ -77,7 +85,7 @@
int32_t status = 0;
HAL_Pulse(m_handle, length, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool DigitalOutput::IsPulsing() const {
@@ -85,7 +93,7 @@
int32_t status = 0;
bool value = HAL_IsPulsing(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -94,7 +102,7 @@
int32_t status = 0;
HAL_SetDigitalPWMRate(rate, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void DigitalOutput::EnablePWM(double initialDutyCycle) {
@@ -104,15 +112,15 @@
if (StatusIsFatal()) return;
m_pwmGenerator = HAL_AllocateDigitalPWM(&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
if (StatusIsFatal()) return;
HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, initialDutyCycle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
if (StatusIsFatal()) return;
HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, m_channel, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void DigitalOutput::DisablePWM() {
@@ -124,10 +132,10 @@
// Disable the output by routing to a dead bit.
HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, SensorUtil::kDigitalChannels,
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_FreeDigitalPWM(m_pwmGenerator, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_pwmGenerator = HAL_kInvalidHandle;
}
@@ -137,7 +145,7 @@
int32_t status = 0;
HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void DigitalOutput::SetSimDevice(HAL_SimDeviceHandle device) {
diff --git a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
index 0b35ff5..c896ea8 100644
--- a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
+++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
@@ -9,7 +9,8 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include <hal/Solenoid.h>
@@ -50,8 +51,8 @@
m_forwardHandle = HAL_InitializeSolenoidPort(
HAL_GetPortWithModule(moduleNumber, m_forwardChannel), &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
- forwardChannel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(),
+ forwardChannel);
m_forwardHandle = HAL_kInvalidHandle;
m_reverseHandle = HAL_kInvalidHandle;
return;
@@ -60,8 +61,8 @@
m_reverseHandle = HAL_InitializeSolenoidPort(
HAL_GetPortWithModule(moduleNumber, m_reverseChannel), &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
- reverseChannel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(),
+ reverseChannel);
// free forward solenoid
HAL_FreeSolenoidPort(m_forwardHandle);
m_forwardHandle = HAL_kInvalidHandle;
@@ -72,10 +73,10 @@
m_forwardMask = 1 << m_forwardChannel;
m_reverseMask = 1 << m_reverseChannel;
- HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_forwardChannel,
- m_moduleNumber);
- HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel,
- m_moduleNumber);
+ HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_forwardChannel + 1,
+ m_moduleNumber + 1);
+ HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel + 1,
+ m_moduleNumber + 1);
SendableRegistry::GetInstance().AddLW(this, "DoubleSolenoid", m_moduleNumber,
m_forwardChannel);
@@ -110,8 +111,8 @@
int rstatus = 0;
HAL_SetSolenoid(m_reverseHandle, reverse, &rstatus);
- wpi_setErrorWithContext(fstatus, HAL_GetErrorMessage(fstatus));
- wpi_setErrorWithContext(rstatus, HAL_GetErrorMessage(rstatus));
+ wpi_setHALError(fstatus);
+ wpi_setHALError(rstatus);
}
DoubleSolenoid::Value DoubleSolenoid::Get() const {
@@ -121,8 +122,8 @@
bool valueForward = HAL_GetSolenoid(m_forwardHandle, &fstatus);
bool valueReverse = HAL_GetSolenoid(m_reverseHandle, &rstatus);
- wpi_setErrorWithContext(fstatus, HAL_GetErrorMessage(fstatus));
- wpi_setErrorWithContext(rstatus, HAL_GetErrorMessage(rstatus));
+ wpi_setHALError(fstatus);
+ wpi_setHALError(rstatus);
if (valueForward) return kForward;
if (valueReverse) return kReverse;
diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp
index d268de7..7c64883 100644
--- a/wpilibc/src/main/native/cpp/DriverStation.cpp
+++ b/wpilibc/src/main/native/cpp/DriverStation.cpp
@@ -9,7 +9,9 @@
#include <chrono>
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Power.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
@@ -473,6 +475,13 @@
return voltage;
}
+void DriverStation::WakeupWaitForData() {
+ std::scoped_lock waitLock(m_waitForDataMutex);
+ // Nofify all threads
+ m_waitForDataCounter++;
+ m_waitForDataCond.notify_all();
+}
+
void DriverStation::GetData() {
{
// Compute the pressed and released buttons
@@ -494,13 +503,7 @@
}
}
- {
- std::scoped_lock waitLock(m_waitForDataMutex);
- // Nofify all threads
- m_waitForDataCounter++;
- m_waitForDataCond.notify_all();
- }
-
+ WakeupWaitForData();
SendMatchData();
}
diff --git a/wpilibc/src/main/native/cpp/DutyCycle.cpp b/wpilibc/src/main/native/cpp/DutyCycle.cpp
new file mode 100644
index 0000000..12f390e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DutyCycle.cpp
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DutyCycle.h"
+
+#include <hal/DutyCycle.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+DutyCycle::DutyCycle(DigitalSource* source)
+ : m_source{source, NullDeleter<DigitalSource>()} {
+ if (m_source == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitDutyCycle();
+ }
+}
+
+DutyCycle::DutyCycle(DigitalSource& source)
+ : m_source{&source, NullDeleter<DigitalSource>()} {
+ InitDutyCycle();
+}
+
+DutyCycle::DutyCycle(std::shared_ptr<DigitalSource> source)
+ : m_source{std::move(source)} {
+ if (m_source == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitDutyCycle();
+ }
+}
+
+DutyCycle::~DutyCycle() { HAL_FreeDutyCycle(m_handle); }
+
+void DutyCycle::InitDutyCycle() {
+ int32_t status = 0;
+ m_handle =
+ HAL_InitializeDutyCycle(m_source->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_source->GetAnalogTriggerTypeForRouting()),
+ &status);
+ wpi_setHALError(status);
+ int index = GetFPGAIndex();
+ HAL_Report(HALUsageReporting::kResourceType_DutyCycle, index + 1);
+ SendableRegistry::GetInstance().AddLW(this, "Duty Cycle", index);
+}
+
+int DutyCycle::GetFPGAIndex() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleFPGAIndex(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+int DutyCycle::GetFrequency() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleFrequency(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+double DutyCycle::GetOutput() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutput(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+unsigned int DutyCycle::GetOutputRaw() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutputRaw(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+unsigned int DutyCycle::GetOutputScaleFactor() const {
+ int32_t status = 0;
+ auto retVal = HAL_GetDutyCycleOutputScaleFactor(m_handle, &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+int DutyCycle::GetSourceChannel() const { return m_source->GetChannel(); }
+
+void DutyCycle::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Duty Cycle");
+ builder.AddDoubleProperty("Frequency",
+ [this] { return this->GetFrequency(); }, nullptr);
+ builder.AddDoubleProperty("Output", [this] { return this->GetOutput(); },
+ nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
new file mode 100644
index 0000000..728b1de
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -0,0 +1,150 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DutyCycleEncoder.h"
+
+#include "frc/Counter.h"
+#include "frc/DigitalInput.h"
+#include "frc/DigitalSource.h"
+#include "frc/DriverStation.h"
+#include "frc/DutyCycle.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+DutyCycleEncoder::DutyCycleEncoder(int channel)
+ : m_dutyCycle{std::make_shared<DutyCycle>(
+ std::make_shared<DigitalInput>(channel))},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
+ : m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle)
+ : m_dutyCycle{dutyCycle, NullDeleter<DutyCycle>{}},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle)
+ : m_dutyCycle{std::move(dutyCycle)},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource)
+ : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource)
+ : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource)
+ : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
+ m_analogTrigger{m_dutyCycle.get()},
+ m_counter{} {
+ Init();
+}
+
+void DutyCycleEncoder::Init() {
+ m_simDevice = hal::SimDevice{"DutyCycleEncoder", m_dutyCycle->GetFPGAIndex()};
+
+ if (m_simDevice) {
+ m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
+ m_simIsConnected = m_simDevice.CreateBoolean("Connected", false, true);
+ }
+
+ m_analogTrigger.SetLimitsDutyCycle(0.25, 0.75);
+ m_counter.SetUpSource(
+ m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse));
+ m_counter.SetDownSource(
+ m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse));
+
+ SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder",
+ m_dutyCycle->GetSourceChannel());
+}
+
+units::turn_t DutyCycleEncoder::Get() const {
+ if (m_simPosition) return units::turn_t{m_simPosition.Get()};
+
+ // As the values are not atomic, keep trying until we get 2 reads of the same
+ // value If we don't within 10 attempts, error
+ for (int i = 0; i < 10; i++) {
+ auto counter = m_counter.Get();
+ auto pos = m_dutyCycle->GetOutput();
+ auto counter2 = m_counter.Get();
+ auto pos2 = m_dutyCycle->GetOutput();
+ if (counter == counter2 && pos == pos2) {
+ units::turn_t turns{counter + pos - m_positionOffset};
+ m_lastPosition = turns;
+ return turns;
+ }
+ }
+
+ frc::DriverStation::GetInstance().ReportWarning(
+ "Failed to read DutyCycle Encoder. Potential Speed Overrun. Returning "
+ "last value");
+ return m_lastPosition;
+}
+
+void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) {
+ m_distancePerRotation = distancePerRotation;
+}
+
+double DutyCycleEncoder::GetDistancePerRotation() const {
+ return m_distancePerRotation;
+}
+
+double DutyCycleEncoder::GetDistance() const {
+ return Get().to<double>() * GetDistancePerRotation();
+}
+
+int DutyCycleEncoder::GetFrequency() const {
+ return m_dutyCycle->GetFrequency();
+}
+
+void DutyCycleEncoder::Reset() {
+ m_counter.Reset();
+ m_positionOffset = m_dutyCycle->GetOutput();
+}
+
+bool DutyCycleEncoder::IsConnected() const {
+ if (m_simIsConnected) return m_simIsConnected.Get();
+ return GetFrequency() > m_frequencyThreshold;
+}
+
+void DutyCycleEncoder::SetConnectedFrequencyThreshold(int frequency) {
+ if (frequency < 0) {
+ frequency = 0;
+ }
+ m_frequencyThreshold = frequency;
+}
+
+void DutyCycleEncoder::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("AbsoluteEncoder");
+ builder.AddDoubleProperty("Distance", [this] { return this->GetDistance(); },
+ nullptr);
+ builder.AddDoubleProperty("Distance Per Rotation",
+ [this] { return this->GetDistancePerRotation(); },
+ nullptr);
+ builder.AddDoubleProperty("Is Connected",
+ [this] { return this->IsConnected(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/Encoder.cpp b/wpilibc/src/main/native/cpp/Encoder.cpp
index fb043cc..963bcc6 100644
--- a/wpilibc/src/main/native/cpp/Encoder.cpp
+++ b/wpilibc/src/main/native/cpp/Encoder.cpp
@@ -11,7 +11,6 @@
#include <hal/Encoder.h>
#include <hal/FRCUsageReporting.h>
-#include <hal/HALBase.h>
#include "frc/DigitalInput.h"
#include "frc/WPIErrors.h"
@@ -60,14 +59,14 @@
Encoder::~Encoder() {
int32_t status = 0;
HAL_FreeEncoder(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int Encoder::Get() const {
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetEncoder(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -75,14 +74,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_ResetEncoder(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double Encoder::GetPeriod() const {
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetEncoderPeriod(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -90,14 +89,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool Encoder::GetStopped() const {
if (StatusIsFatal()) return true;
int32_t status = 0;
bool value = HAL_GetEncoderStopped(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -105,7 +104,7 @@
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetEncoderDirection(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -113,14 +112,14 @@
if (StatusIsFatal()) return 0;
int32_t status = 0;
int value = HAL_GetEncoderRaw(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
int Encoder::GetEncodingScale() const {
int32_t status = 0;
int val = HAL_GetEncoderEncodingScale(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
@@ -128,7 +127,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetEncoderDistance(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -136,7 +135,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double value = HAL_GetEncoderRate(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -144,21 +143,21 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetEncoderMinRate(m_encoder, minRate, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Encoder::SetDistancePerPulse(double distancePerPulse) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetEncoderDistancePerPulse(m_encoder, distancePerPulse, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double Encoder::GetDistancePerPulse() const {
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double distancePerPulse = HAL_GetEncoderDistancePerPulse(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return distancePerPulse;
}
@@ -166,7 +165,7 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetEncoderReverseDirection(m_encoder, reverseDirection, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Encoder::SetSamplesToAverage(int samplesToAverage) {
@@ -178,13 +177,13 @@
}
int32_t status = 0;
HAL_SetEncoderSamplesToAverage(m_encoder, samplesToAverage, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int Encoder::GetSamplesToAverage() const {
int32_t status = 0;
int result = HAL_GetEncoderSamplesToAverage(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return result;
}
@@ -214,7 +213,7 @@
m_encoder, source.GetPortHandleForRouting(),
(HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(),
(HAL_EncoderIndexingType)type, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Encoder::SetSimDevice(HAL_SimDeviceHandle device) {
@@ -224,14 +223,14 @@
int Encoder::GetFPGAIndex() const {
int32_t status = 0;
int val = HAL_GetEncoderFPGAIndex(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
void Encoder::InitSendable(SendableBuilder& builder) {
int32_t status = 0;
HAL_EncoderEncodingType type = HAL_GetEncoderEncodingType(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
if (type == HAL_EncoderEncodingType::HAL_Encoder_k4X)
builder.SetSmartDashboardType("Quadrature Encoder");
else
@@ -252,9 +251,9 @@
m_bSource->GetPortHandleForRouting(),
(HAL_AnalogTriggerType)m_bSource->GetAnalogTriggerTypeForRouting(),
reverseDirection, (HAL_EncoderEncodingType)encodingType, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
- HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex(),
+ HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex() + 1,
encodingType);
SendableRegistry::GetInstance().AddLW(this, "Encoder",
m_aSource->GetChannel());
@@ -264,6 +263,6 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double val = HAL_GetEncoderDecodingScaleFactor(m_encoder, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
diff --git a/wpilibc/src/main/native/cpp/ErrorBase.cpp b/wpilibc/src/main/native/cpp/ErrorBase.cpp
index 8e70e61..d098c07 100644
--- a/wpilibc/src/main/native/cpp/ErrorBase.cpp
+++ b/wpilibc/src/main/native/cpp/ErrorBase.cpp
@@ -12,7 +12,8 @@
#include <cstring>
#include <set>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <wpi/Format.h>
#include <wpi/SmallString.h>
#include <wpi/raw_ostream.h>
diff --git a/wpilibc/src/main/native/cpp/GenericHID.cpp b/wpilibc/src/main/native/cpp/GenericHID.cpp
index 504d669..a7dcfc0 100644
--- a/wpilibc/src/main/native/cpp/GenericHID.cpp
+++ b/wpilibc/src/main/native/cpp/GenericHID.cpp
@@ -7,7 +7,8 @@
#include "frc/GenericHID.h"
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
#include "frc/WPIErrors.h"
diff --git a/wpilibc/src/main/native/cpp/I2C.cpp b/wpilibc/src/main/native/cpp/I2C.cpp
index 44b71af..21d92f2 100644
--- a/wpilibc/src/main/native/cpp/I2C.cpp
+++ b/wpilibc/src/main/native/cpp/I2C.cpp
@@ -9,7 +9,7 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/I2C.h>
#include "frc/WPIErrors.h"
@@ -20,7 +20,7 @@
: m_port(static_cast<HAL_I2CPort>(port)), m_deviceAddress(deviceAddress) {
int32_t status = 0;
HAL_InitializeI2C(m_port, &status);
- // wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ // wpi_setHALError(status);
HAL_Report(HALUsageReporting::kResourceType_I2C, deviceAddress);
}
@@ -32,7 +32,7 @@
int32_t status = 0;
status = HAL_TransactionI2C(m_port, m_deviceAddress, dataToSend, sendSize,
dataReceived, receiveSize);
- // wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ // wpi_setHALError(status);
return status < 0;
}
diff --git a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
index 36150d4..202c61d 100644
--- a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
+++ b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
@@ -7,7 +7,7 @@
#include "frc/InterruptableSensorBase.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/Utility.h"
#include "frc/WPIErrors.h"
@@ -36,7 +36,7 @@
&status);
SetUpSourceEdge(true, false);
HAL_AttachInterruptHandler(m_interrupt, handler, param, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void InterruptableSensorBase::RequestInterrupts(InterruptEventHandler handler) {
@@ -69,7 +69,7 @@
(*self)(res);
},
m_interruptHandler.get(), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void InterruptableSensorBase::RequestInterrupts() {
@@ -84,7 +84,7 @@
m_interrupt, GetPortHandleForRouting(),
static_cast<HAL_AnalogTriggerType>(GetAnalogTriggerTypeForRouting()),
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
SetUpSourceEdge(true, false);
}
@@ -106,7 +106,7 @@
int result;
result = HAL_WaitForInterrupt(m_interrupt, timeout, ignorePrevious, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Rising edge result is the interrupt bit set in the byte 0xFF
// Falling edge result is the interrupt bit set in the byte 0xFF00
@@ -122,7 +122,7 @@
wpi_assert(m_interrupt != HAL_kInvalidHandle);
int32_t status = 0;
HAL_EnableInterrupts(m_interrupt, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void InterruptableSensorBase::DisableInterrupts() {
@@ -130,7 +130,7 @@
wpi_assert(m_interrupt != HAL_kInvalidHandle);
int32_t status = 0;
HAL_DisableInterrupts(m_interrupt, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double InterruptableSensorBase::ReadRisingTimestamp() {
@@ -138,7 +138,7 @@
wpi_assert(m_interrupt != HAL_kInvalidHandle);
int32_t status = 0;
int64_t timestamp = HAL_ReadInterruptRisingTimestamp(m_interrupt, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return timestamp * 1e-6;
}
@@ -147,7 +147,7 @@
wpi_assert(m_interrupt != HAL_kInvalidHandle);
int32_t status = 0;
int64_t timestamp = HAL_ReadInterruptFallingTimestamp(m_interrupt, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return timestamp * 1e-6;
}
@@ -163,7 +163,7 @@
if (m_interrupt != HAL_kInvalidHandle) {
int32_t status = 0;
HAL_SetInterruptUpSourceEdge(m_interrupt, risingEdge, fallingEdge, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
}
@@ -172,5 +172,5 @@
// Expects the calling leaf class to allocate an interrupt index.
int32_t status = 0;
m_interrupt = HAL_InitializeInterrupts(watcher, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
diff --git a/wpilibc/src/main/native/cpp/IterativeRobot.cpp b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
index 950b239..c8664a5 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobot.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
@@ -7,7 +7,8 @@
#include "frc/IterativeRobot.h"
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
@@ -30,7 +31,13 @@
while (true) {
// Wait for driver station data so the loop doesn't hog the CPU
DriverStation::GetInstance().WaitForData();
+ if (m_exit) break;
LoopFunc();
}
}
+
+void IterativeRobot::EndCompetition() {
+ m_exit = true;
+ DriverStation::GetInstance().WakeupWaitForData();
+}
diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
index 90dc54b..2997234 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -9,14 +9,14 @@
#include <cstdio>
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
#include <wpi/Format.h>
#include <wpi/SmallString.h>
#include <wpi/raw_ostream.h>
#include "frc/DriverStation.h"
#include "frc/Timer.h"
-#include "frc/commands/Scheduler.h"
#include "frc/livewindow/LiveWindow.h"
#include "frc/shuffleboard/Shuffleboard.h"
#include "frc/smartdashboard/SmartDashboard.h"
@@ -131,7 +131,6 @@
TeleopInit();
m_watchdog.AddEpoch("TeleopInit()");
m_lastMode = Mode::kTeleop;
- Scheduler::GetInstance()->SetEnabled(true);
}
HAL_ObserveUserProgramTeleop();
diff --git a/wpilibc/src/main/native/cpp/Jaguar.cpp b/wpilibc/src/main/native/cpp/Jaguar.cpp
index 487d489..c87e4d1 100644
--- a/wpilibc/src/main/native/cpp/Jaguar.cpp
+++ b/wpilibc/src/main/native/cpp/Jaguar.cpp
@@ -7,26 +7,18 @@
#include "frc/Jaguar.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
Jaguar::Jaguar(int channel) : PWMSpeedController(channel) {
- /* Input profile defined by Luminary Micro.
- *
- * Full reverse ranges from 0.671325ms to 0.6972211ms
- * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
- * Neutral ranges from 1.4482078ms to 1.5517922ms
- * Proportional forward ranges from 1.5517922ms to 2.3027789ms
- * Full forward ranges from 2.3027789ms to 2.328675ms
- */
- SetBounds(2.31, 1.55, 1.507, 1.454, .697);
+ SetBounds(2.31, 1.55, 1.507, 1.454, 0.697);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel() + 1);
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 34afd0a..8d464cf 100644
--- a/wpilibc/src/main/native/cpp/Joystick.cpp
+++ b/wpilibc/src/main/native/cpp/Joystick.cpp
@@ -9,7 +9,7 @@
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <wpi/math>
#include "frc/DriverStation.h"
@@ -24,7 +24,7 @@
m_axes[Axis::kTwist] = kDefaultTwistChannel;
m_axes[Axis::kThrottle] = kDefaultThrottleChannel;
- HAL_Report(HALUsageReporting::kResourceType_Joystick, port);
+ HAL_Report(HALUsageReporting::kResourceType_Joystick, port + 1);
}
void Joystick::SetXChannel(int channel) { m_axes[Axis::kX] = channel; }
diff --git a/wpilibc/src/main/native/cpp/LinearFilter.cpp b/wpilibc/src/main/native/cpp/LinearFilter.cpp
deleted file mode 100644
index 95df127..0000000
--- a/wpilibc/src/main/native/cpp/LinearFilter.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/NidecBrushless.cpp b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
index 90ce43e..5bce36e 100644
--- a/wpilibc/src/main/native/cpp/NidecBrushless.cpp
+++ b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
@@ -7,7 +7,7 @@
#include "frc/NidecBrushless.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
@@ -26,7 +26,7 @@
m_dio.SetPWMRate(15625);
m_dio.EnablePWM(0.5);
- HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel);
+ HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel + 1);
registry.AddLW(this, "Nidec Brushless", pwmChannel);
}
diff --git a/wpilibc/src/main/native/cpp/Notifier.cpp b/wpilibc/src/main/native/cpp/Notifier.cpp
index a2f93f9..a7fa038 100644
--- a/wpilibc/src/main/native/cpp/Notifier.cpp
+++ b/wpilibc/src/main/native/cpp/Notifier.cpp
@@ -9,7 +9,9 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/Notifier.h>
+#include <wpi/SmallString.h>
#include "frc/Timer.h"
#include "frc/Utility.h"
@@ -23,7 +25,7 @@
m_handler = handler;
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_thread = std::thread([=] {
for (;;) {
@@ -57,7 +59,7 @@
// atomically set handle to 0, then clean
HAL_NotifierHandle handle = m_notifier.exchange(0);
HAL_StopNotifier(handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Join the thread to ensure the handler has exited.
if (m_thread.joinable()) m_thread.join();
@@ -90,6 +92,13 @@
return *this;
}
+void Notifier::SetName(const wpi::Twine& name) {
+ wpi::SmallString<64> nameBuf;
+ int32_t status = 0;
+ HAL_SetNotifierName(m_notifier,
+ name.toNullTerminatedStringRef(nameBuf).data(), &status);
+}
+
void Notifier::SetHandler(std::function<void()> handler) {
std::scoped_lock lock(m_processMutex);
m_handler = handler;
@@ -122,7 +131,7 @@
void Notifier::Stop() {
int32_t status = 0;
HAL_CancelNotifierAlarm(m_notifier, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Notifier::UpdateAlarm(uint64_t triggerTime) {
@@ -131,7 +140,7 @@
auto notifier = m_notifier.load();
if (notifier == 0) return;
HAL_UpdateNotifierAlarm(notifier, triggerTime, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Notifier::UpdateAlarm() {
diff --git a/wpilibc/src/main/native/cpp/PIDBase.cpp b/wpilibc/src/main/native/cpp/PIDBase.cpp
index a2efe0f..c8f6fed 100644
--- a/wpilibc/src/main/native/cpp/PIDBase.cpp
+++ b/wpilibc/src/main/native/cpp/PIDBase.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/PIDOutput.h"
#include "frc/smartdashboard/SendableBuilder.h"
@@ -35,7 +35,7 @@
m_F = Kf;
m_pidInput = &source;
- m_filter = LinearFilter::MovingAverage(1);
+ m_filter = LinearFilter<double>::MovingAverage(1);
m_pidOutput = &output;
@@ -196,7 +196,7 @@
void PIDBase::SetToleranceBuffer(int bufLength) {
std::scoped_lock lock(m_thisMutex);
- m_filter = LinearFilter::MovingAverage(bufLength);
+ m_filter = LinearFilter<double>::MovingAverage(bufLength);
}
bool PIDBase::OnTarget() const {
diff --git a/wpilibc/src/main/native/cpp/PWM.cpp b/wpilibc/src/main/native/cpp/PWM.cpp
index c6b398f..dabcd2e 100644
--- a/wpilibc/src/main/native/cpp/PWM.cpp
+++ b/wpilibc/src/main/native/cpp/PWM.cpp
@@ -9,7 +9,8 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/PWM.h>
#include <hal/Ports.h>
@@ -31,8 +32,7 @@
int32_t status = 0;
m_handle = HAL_InitializePWMPort(HAL_GetPort(channel), &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumPWMChannels(), channel,
- HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPWMChannels(), channel);
m_channel = std::numeric_limits<int>::max();
m_handle = HAL_kInvalidHandle;
return;
@@ -41,12 +41,12 @@
m_channel = channel;
HAL_SetPWMDisabled(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
status = 0;
HAL_SetPWMEliminateDeadband(m_handle, false, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
- HAL_Report(HALUsageReporting::kResourceType_PWM, channel);
+ HAL_Report(HALUsageReporting::kResourceType_PWM, channel + 1);
SendableRegistry::GetInstance().AddLW(this, "PWM", channel);
SetSafetyEnabled(false);
@@ -56,10 +56,10 @@
int32_t status = 0;
HAL_SetPWMDisabled(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_FreePWMPort(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::StopMotor() { SetDisabled(); }
@@ -73,7 +73,7 @@
int32_t status = 0;
HAL_SetPWMRaw(m_handle, value, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
uint16_t PWM::GetRaw() const {
@@ -81,7 +81,7 @@
int32_t status = 0;
uint16_t value = HAL_GetPWMRaw(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -90,14 +90,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetPWMPosition(m_handle, pos, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
double PWM::GetPosition() const {
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double position = HAL_GetPWMPosition(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return position;
}
@@ -105,7 +105,7 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetPWMSpeed(m_handle, speed, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
Feed();
}
@@ -114,7 +114,7 @@
if (StatusIsFatal()) return 0.0;
int32_t status = 0;
double speed = HAL_GetPWMSpeed(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return speed;
}
@@ -124,7 +124,7 @@
int32_t status = 0;
HAL_SetPWMDisabled(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
@@ -148,7 +148,7 @@
wpi_assert(false);
}
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::SetZeroLatch() {
@@ -157,14 +157,14 @@
int32_t status = 0;
HAL_LatchPWMZero(m_handle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetPWMEliminateDeadband(m_handle, eliminateDeadband, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::SetBounds(double max, double deadbandMax, double center,
@@ -173,7 +173,7 @@
int32_t status = 0;
HAL_SetPWMConfig(m_handle, max, deadbandMax, center, deadbandMin, min,
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::SetRawBounds(int max, int deadbandMax, int center, int deadbandMin,
@@ -182,7 +182,7 @@
int32_t status = 0;
HAL_SetPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min,
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void PWM::GetRawBounds(int* max, int* deadbandMax, int* center,
@@ -190,7 +190,7 @@
int32_t status = 0;
HAL_GetPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min,
&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int PWM::GetChannel() const { return m_channel; }
diff --git a/wpilibc/src/main/native/cpp/PWMSparkMax.cpp b/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
index d8d3b2a..c5375f4 100644
--- a/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
+++ b/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
@@ -7,26 +7,18 @@
#include "frc/PWMSparkMax.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.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);
+ SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_RevSparkMaxPWM, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_RevSparkMaxPWM, GetChannel() + 1);
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 0bbc5b9..1242be9 100644
--- a/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
+++ b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
@@ -7,30 +7,18 @@
#include "frc/PWMTalonSRX.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
PWMTalonSRX::PWMTalonSRX(int channel) : PWMSpeedController(channel) {
- /* Note that the PWMTalonSRX uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the TalonSRX User
- * Manual available from Cross The Road Electronics.
- * 2.004ms = full "forward"
- * 1.52ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.48ms = the "low end" of the deadband range
- * 0.997ms = full "reverse"
- */
- SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel() + 1);
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 122d219..0d966ae 100644
--- a/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
+++ b/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
@@ -7,30 +7,18 @@
#include "frc/PWMVictorSPX.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
PWMVictorSPX::PWMVictorSPX(int channel) : PWMSpeedController(channel) {
- /* Note that the PWMVictorSPX uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the VictorSPX User
- * Manual available from Cross The Road Electronics.
- * 2.004ms = full "forward"
- * 1.52ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.48ms = the "low end" of the deadband range
- * 0.997ms = full "reverse"
- */
- SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_PWMVictorSPX, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_PWMVictorSPX, GetChannel() + 1);
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 b3297ea..7d7d916 100644
--- a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
+++ b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
@@ -7,7 +7,7 @@
#include "frc/PowerDistributionPanel.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/PDP.h>
#include <hal/Ports.h>
#include <wpi/SmallString.h>
@@ -29,12 +29,11 @@
int32_t status = 0;
m_handle = HAL_InitializePDP(module, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumPDPModules(), module,
- HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPDPModules(), module);
return;
}
- HAL_Report(HALUsageReporting::kResourceType_PDP, module);
+ HAL_Report(HALUsageReporting::kResourceType_PDP, module + 1);
SendableRegistry::GetInstance().AddLW(this, "PowerDistributionPanel", module);
}
diff --git a/wpilibc/src/main/native/cpp/Preferences.cpp b/wpilibc/src/main/native/cpp/Preferences.cpp
index 7a896d1..6014775 100644
--- a/wpilibc/src/main/native/cpp/Preferences.cpp
+++ b/wpilibc/src/main/native/cpp/Preferences.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. */
@@ -9,7 +9,7 @@
#include <algorithm>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/StringRef.h>
diff --git a/wpilibc/src/main/native/cpp/Relay.cpp b/wpilibc/src/main/native/cpp/Relay.cpp
index 3a3e772..cb95223 100644
--- a/wpilibc/src/main/native/cpp/Relay.cpp
+++ b/wpilibc/src/main/native/cpp/Relay.cpp
@@ -9,7 +9,8 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include <hal/Relay.h>
#include <wpi/raw_ostream.h>
@@ -35,20 +36,18 @@
int32_t status = 0;
m_forwardHandle = HAL_InitializeRelayPort(portHandle, true, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
- channel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumRelayChannels(), channel);
m_forwardHandle = HAL_kInvalidHandle;
m_reverseHandle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_Relay, m_channel);
+ HAL_Report(HALUsageReporting::kResourceType_Relay, m_channel + 1);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
int32_t status = 0;
m_reverseHandle = HAL_InitializeRelayPort(portHandle, false, &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
- channel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumRelayChannels(), channel);
m_forwardHandle = HAL_kInvalidHandle;
m_reverseHandle = HAL_kInvalidHandle;
return;
@@ -61,7 +60,7 @@
if (m_forwardHandle != HAL_kInvalidHandle) {
HAL_SetRelay(m_forwardHandle, false, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_forwardHandle = HAL_kInvalidHandle;
m_reverseHandle = HAL_kInvalidHandle;
return;
@@ -70,7 +69,7 @@
if (m_reverseHandle != HAL_kInvalidHandle) {
HAL_SetRelay(m_reverseHandle, false, &status);
if (status != 0) {
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
m_forwardHandle = HAL_kInvalidHandle;
m_reverseHandle = HAL_kInvalidHandle;
return;
@@ -137,7 +136,7 @@
break;
}
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
Relay::Value Relay::Get() const {
@@ -171,7 +170,7 @@
}
}
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int Relay::GetChannel() const { return m_channel; }
diff --git a/wpilibc/src/main/native/cpp/RobotController.cpp b/wpilibc/src/main/native/cpp/RobotController.cpp
index 347440d..b1d7608 100644
--- a/wpilibc/src/main/native/cpp/RobotController.cpp
+++ b/wpilibc/src/main/native/cpp/RobotController.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,7 +7,10 @@
#include "frc/RobotController.h"
-#include <hal/HAL.h>
+#include <hal/CAN.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Power.h>
#include "frc/ErrorBase.h"
@@ -16,21 +19,21 @@
int RobotController::GetFPGAVersion() {
int32_t status = 0;
int version = HAL_GetFPGAVersion(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return version;
}
int64_t RobotController::GetFPGARevision() {
int32_t status = 0;
int64_t revision = HAL_GetFPGARevision(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return revision;
}
uint64_t RobotController::GetFPGATime() {
int32_t status = 0;
uint64_t time = HAL_GetFPGATime(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return time;
}
@@ -46,112 +49,112 @@
bool RobotController::IsSysActive() {
int32_t status = 0;
bool retVal = HAL_GetSystemActive(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
bool RobotController::IsBrownedOut() {
int32_t status = 0;
bool retVal = HAL_GetBrownedOut(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetInputVoltage() {
int32_t status = 0;
double retVal = HAL_GetVinVoltage(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetInputCurrent() {
int32_t status = 0;
double retVal = HAL_GetVinCurrent(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetVoltage3V3() {
int32_t status = 0;
double retVal = HAL_GetUserVoltage3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetCurrent3V3() {
int32_t status = 0;
double retVal = HAL_GetUserCurrent3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
bool RobotController::GetEnabled3V3() {
int32_t status = 0;
bool retVal = HAL_GetUserActive3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
int RobotController::GetFaultCount3V3() {
int32_t status = 0;
int retVal = HAL_GetUserCurrentFaults3V3(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetVoltage5V() {
int32_t status = 0;
double retVal = HAL_GetUserVoltage5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetCurrent5V() {
int32_t status = 0;
double retVal = HAL_GetUserCurrent5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
bool RobotController::GetEnabled5V() {
int32_t status = 0;
bool retVal = HAL_GetUserActive5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
int RobotController::GetFaultCount5V() {
int32_t status = 0;
int retVal = HAL_GetUserCurrentFaults5V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetVoltage6V() {
int32_t status = 0;
double retVal = HAL_GetUserVoltage6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
double RobotController::GetCurrent6V() {
int32_t status = 0;
double retVal = HAL_GetUserCurrent6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
bool RobotController::GetEnabled6V() {
int32_t status = 0;
bool retVal = HAL_GetUserActive6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
int RobotController::GetFaultCount6V() {
int32_t status = 0;
int retVal = HAL_GetUserCurrentFaults6V(&status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return retVal;
}
@@ -165,7 +168,7 @@
HAL_CAN_GetCANStatus(&percentBusUtilization, &busOffCount, &txFullCount,
&receiveErrorCount, &transmitErrorCount, &status);
if (status != 0) {
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return {};
}
return {percentBusUtilization, static_cast<int>(busOffCount),
diff --git a/wpilibc/src/main/native/cpp/RobotDrive.cpp b/wpilibc/src/main/native/cpp/RobotDrive.cpp
index fd43c96..5235fa2 100644
--- a/wpilibc/src/main/native/cpp/RobotDrive.cpp
+++ b/wpilibc/src/main/native/cpp/RobotDrive.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/GenericHID.h"
#include "frc/Joystick.h"
@@ -128,13 +128,13 @@
if (curve < 0) {
double value = std::log(-curve);
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
- if (ratio == 0) ratio = .0000000001;
+ if (ratio == 0) ratio = 0.0000000001;
leftOutput = outputMagnitude / ratio;
rightOutput = outputMagnitude;
} else if (curve > 0) {
double value = std::log(curve);
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
- if (ratio == 0) ratio = .0000000001;
+ if (ratio == 0) ratio = 0.0000000001;
leftOutput = outputMagnitude;
rightOutput = outputMagnitude / ratio;
} else {
diff --git a/wpilibc/src/main/native/cpp/SD540.cpp b/wpilibc/src/main/native/cpp/SD540.cpp
index 93733b2..9611f66 100644
--- a/wpilibc/src/main/native/cpp/SD540.cpp
+++ b/wpilibc/src/main/native/cpp/SD540.cpp
@@ -7,31 +7,19 @@
#include "frc/SD540.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
SD540::SD540(int channel) : PWMSpeedController(channel) {
- /* Note that the SD540 uses the following bounds for PWM values. These values
- * should work reasonably well for most controllers, but if users experience
- * issues such as asymmetric behavior around the deadband or inability to
- * saturate the controller in either direction, calibration is recommended.
- * The calibration procedure can be found in the SD540 User Manual available
- * from Mindsensors.
- *
- * 2.05ms = full "forward"
- * 1.55ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.44ms = the "low end" of the deadband range
- * 0.94ms = full "reverse"
- */
- SetBounds(2.05, 1.55, 1.50, 1.44, .94);
+ SetBounds(2.05, 1.55, 1.50, 1.44, 0.94);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540,
+ GetChannel() + 1);
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 54cf82b..074cc79 100644
--- a/wpilibc/src/main/native/cpp/SPI.cpp
+++ b/wpilibc/src/main/native/cpp/SPI.cpp
@@ -10,7 +10,7 @@
#include <cstring>
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/SPI.h>
#include <wpi/SmallVector.h>
#include <wpi/mutex.h>
@@ -155,9 +155,10 @@
SPI::SPI(Port port) : m_port(static_cast<HAL_SPIPort>(port)) {
int32_t status = 0;
HAL_InitializeSPI(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
- HAL_Report(HALUsageReporting::kResourceType_SPI, port);
+ HAL_Report(HALUsageReporting::kResourceType_SPI,
+ static_cast<uint8_t>(port) + 1);
}
SPI::~SPI() { HAL_CloseSPI(m_port); }
@@ -207,13 +208,13 @@
void SPI::SetChipSelectActiveHigh() {
int32_t status = 0;
HAL_SetSPIChipSelectActiveHigh(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::SetChipSelectActiveLow() {
int32_t status = 0;
HAL_SetSPIChipSelectActiveLow(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int SPI::Write(uint8_t* data, int size) {
@@ -243,26 +244,26 @@
void SPI::InitAuto(int bufferSize) {
int32_t status = 0;
HAL_InitSPIAuto(m_port, bufferSize, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::FreeAuto() {
int32_t status = 0;
HAL_FreeSPIAuto(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize) {
int32_t status = 0;
HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(),
zeroSize, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::StartAutoRate(units::second_t period) {
int32_t status = 0;
HAL_StartSPIAutoRate(m_port, period.to<double>(), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::StartAutoRate(double period) {
@@ -275,19 +276,19 @@
m_port, source.GetPortHandleForRouting(),
(HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(), rising,
falling, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::StopAuto() {
int32_t status = 0;
HAL_StopSPIAuto(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SPI::ForceAutoRead() {
int32_t status = 0;
HAL_ForceSPIAutoRead(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead,
@@ -295,7 +296,7 @@
int32_t status = 0;
int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead,
timeout.to<double>(), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
@@ -306,7 +307,7 @@
int SPI::GetAutoDroppedCount() {
int32_t status = 0;
int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return val;
}
diff --git a/wpilibc/src/main/native/cpp/SensorUtil.cpp b/wpilibc/src/main/native/cpp/SensorUtil.cpp
index 2012da4..8c02f1b 100644
--- a/wpilibc/src/main/native/cpp/SensorUtil.cpp
+++ b/wpilibc/src/main/native/cpp/SensorUtil.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,7 +10,7 @@
#include <hal/AnalogInput.h>
#include <hal/AnalogOutput.h>
#include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/PDP.h>
#include <hal/PWM.h>
#include <hal/Ports.h>
diff --git a/wpilibc/src/main/native/cpp/SerialPort.cpp b/wpilibc/src/main/native/cpp/SerialPort.cpp
index 46e02c8..e092fc2 100644
--- a/wpilibc/src/main/native/cpp/SerialPort.cpp
+++ b/wpilibc/src/main/native/cpp/SerialPort.cpp
@@ -9,7 +9,7 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/SerialPort.h>
using namespace frc;
@@ -21,17 +21,17 @@
m_portHandle =
HAL_InitializeSerialPort(static_cast<HAL_SerialPort>(port), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Don't continue if initialization failed
if (status < 0) return;
HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialParity(m_portHandle, parity, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Set the default timeout to 5 seconds.
SetTimeout(5.0);
@@ -41,7 +41,8 @@
DisableTermination();
- HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
+ HAL_Report(HALUsageReporting::kResourceType_SerialPort,
+ static_cast<uint8_t>(port) + 1);
}
SerialPort::SerialPort(int baudRate, const wpi::Twine& portName, Port port,
@@ -54,17 +55,17 @@
m_portHandle = HAL_InitializeSerialPortDirect(
static_cast<HAL_SerialPort>(port), portNameC, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Don't continue if initialization failed
if (status < 0) return;
HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialParity(m_portHandle, parity, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
// Set the default timeout to 5 seconds.
SetTimeout(5.0);
@@ -74,44 +75,45 @@
DisableTermination();
- HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
+ HAL_Report(HALUsageReporting::kResourceType_SerialPort,
+ static_cast<uint8_t>(port) + 1);
}
SerialPort::~SerialPort() {
int32_t status = 0;
HAL_CloseSerial(m_portHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) {
int32_t status = 0;
HAL_SetSerialFlowControl(m_portHandle, flowControl, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::EnableTermination(char terminator) {
int32_t status = 0;
HAL_EnableSerialTermination(m_portHandle, terminator, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::DisableTermination() {
int32_t status = 0;
HAL_DisableSerialTermination(m_portHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
int SerialPort::GetBytesReceived() {
int32_t status = 0;
int retVal = HAL_GetSerialBytesReceived(m_portHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return retVal;
}
int SerialPort::Read(char* buffer, int count) {
int32_t status = 0;
int retVal = HAL_ReadSerial(m_portHandle, buffer, count, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return retVal;
}
@@ -123,42 +125,42 @@
int32_t status = 0;
int retVal =
HAL_WriteSerial(m_portHandle, buffer.data(), buffer.size(), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return retVal;
}
void SerialPort::SetTimeout(double timeout) {
int32_t status = 0;
HAL_SetSerialTimeout(m_portHandle, timeout, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::SetReadBufferSize(int size) {
int32_t status = 0;
HAL_SetSerialReadBufferSize(m_portHandle, size, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::SetWriteBufferSize(int size) {
int32_t status = 0;
HAL_SetSerialWriteBufferSize(m_portHandle, size, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode) {
int32_t status = 0;
HAL_SetSerialWriteMode(m_portHandle, mode, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::Flush() {
int32_t status = 0;
HAL_FlushSerial(m_portHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void SerialPort::Reset() {
int32_t status = 0;
HAL_ClearSerial(m_portHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
diff --git a/wpilibc/src/main/native/cpp/Servo.cpp b/wpilibc/src/main/native/cpp/Servo.cpp
index 09e482b..4b6856a 100644
--- a/wpilibc/src/main/native/cpp/Servo.cpp
+++ b/wpilibc/src/main/native/cpp/Servo.cpp
@@ -7,7 +7,7 @@
#include "frc/Servo.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
@@ -27,7 +27,7 @@
// Assign defaults for period multiplier for the servo PWM control signal
SetPeriodMultiplier(kPeriodMultiplier_4X);
- HAL_Report(HALUsageReporting::kResourceType_Servo, channel);
+ HAL_Report(HALUsageReporting::kResourceType_Servo, channel + 1);
SendableRegistry::GetInstance().SetName(this, "Servo", channel);
}
diff --git a/wpilibc/src/main/native/cpp/Solenoid.cpp b/wpilibc/src/main/native/cpp/Solenoid.cpp
index c860c44..e0bde3c 100644
--- a/wpilibc/src/main/native/cpp/Solenoid.cpp
+++ b/wpilibc/src/main/native/cpp/Solenoid.cpp
@@ -9,7 +9,8 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <hal/Ports.h>
#include <hal/Solenoid.h>
@@ -40,14 +41,13 @@
m_solenoidHandle = HAL_InitializeSolenoidPort(
HAL_GetPortWithModule(moduleNumber, channel), &status);
if (status != 0) {
- wpi_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
- channel, HAL_GetErrorMessage(status));
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(), channel);
m_solenoidHandle = HAL_kInvalidHandle;
return;
}
- HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel,
- m_moduleNumber);
+ HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel + 1,
+ m_moduleNumber + 1);
SendableRegistry::GetInstance().AddLW(this, "Solenoid", m_moduleNumber,
m_channel);
}
@@ -58,14 +58,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetSolenoid(m_solenoidHandle, on, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
bool Solenoid::Get() const {
if (StatusIsFatal()) return false;
int32_t status = 0;
bool value = HAL_GetSolenoid(m_solenoidHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
return value;
}
@@ -79,14 +79,14 @@
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_SetOneShotDuration(m_solenoidHandle, durationMS, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Solenoid::StartPulse() {
if (StatusIsFatal()) return;
int32_t status = 0;
HAL_FireOneShot(m_solenoidHandle, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
void Solenoid::InitSendable(SendableBuilder& builder) {
diff --git a/wpilibc/src/main/native/cpp/SolenoidBase.cpp b/wpilibc/src/main/native/cpp/SolenoidBase.cpp
index c0b79a5..f5f8aad 100644
--- a/wpilibc/src/main/native/cpp/SolenoidBase.cpp
+++ b/wpilibc/src/main/native/cpp/SolenoidBase.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,7 +7,7 @@
#include "frc/SolenoidBase.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/Solenoid.h>
using namespace frc;
@@ -16,7 +16,7 @@
int value = 0;
int32_t status = 0;
value = HAL_GetAllSolenoids(module, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return value;
}
diff --git a/wpilibc/src/main/native/cpp/Spark.cpp b/wpilibc/src/main/native/cpp/Spark.cpp
index 18f8ee6..3717df4 100644
--- a/wpilibc/src/main/native/cpp/Spark.cpp
+++ b/wpilibc/src/main/native/cpp/Spark.cpp
@@ -7,31 +7,18 @@
#include "frc/Spark.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
Spark::Spark(int channel) : PWMSpeedController(channel) {
- /* Note that the Spark uses the following bounds for PWM values. These values
- * should work reasonably well for most controllers, but if users experience
- * issues such as asymmetric behavior around the deadband or inability to
- * saturate the controller in either direction, calibration is recommended.
- * The calibration procedure can be found in the Spark User Manual available
- * from REV Robotics.
- *
- * 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);
+ SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "Spark", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibc/src/main/native/cpp/SpeedController.cpp
similarity index 65%
rename from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
rename to wpilibc/src/main/native/cpp/SpeedController.cpp
index 8bd62ea..cb3cc5b 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/wpilibc/src/main/native/cpp/SpeedController.cpp
@@ -5,12 +5,12 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc2/command/PrintCommand.h"
+#include "frc/SpeedController.h"
-using namespace frc2;
+#include <frc/RobotController.h>
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
+using namespace frc;
+
+void SpeedController::SetVoltage(units::volt_t output) {
+ Set(output / units::volt_t(RobotController::GetInputVoltage()));
}
-
-bool PrintCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibc/src/main/native/cpp/Talon.cpp b/wpilibc/src/main/native/cpp/Talon.cpp
index 8dc1928..4807d71 100644
--- a/wpilibc/src/main/native/cpp/Talon.cpp
+++ b/wpilibc/src/main/native/cpp/Talon.cpp
@@ -7,31 +7,18 @@
#include "frc/Talon.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
Talon::Talon(int channel) : PWMSpeedController(channel) {
- /* Note that the Talon uses the following bounds for PWM values. These values
- * should work reasonably well for most controllers, but if users experience
- * issues such as asymmetric behavior around the deadband or inability to
- * saturate the controller in either direction, calibration is recommended.
- * The calibration procedure can be found in the Talon User Manual available
- * from CTRE.
- *
- * 2.037ms = full "forward"
- * 1.539ms = the "high end" of the deadband range
- * 1.513ms = center of the deadband range (off)
- * 1.487ms = the "low end" of the deadband range
- * 0.989ms = full "reverse"
- */
- SetBounds(2.037, 1.539, 1.513, 1.487, .989);
+ SetBounds(2.037, 1.539, 1.513, 1.487, 0.989);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel() + 1);
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 798e86a..7b1e647 100644
--- a/wpilibc/src/main/native/cpp/Threads.cpp
+++ b/wpilibc/src/main/native/cpp/Threads.cpp
@@ -7,7 +7,7 @@
#include "frc/Threads.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <hal/Threads.h>
#include "frc/ErrorBase.h"
@@ -19,7 +19,7 @@
HAL_Bool rt = false;
auto native = thread.native_handle();
auto ret = HAL_GetThreadPriority(&native, &rt, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
*isRealTime = rt;
return ret;
}
@@ -28,7 +28,7 @@
int32_t status = 0;
HAL_Bool rt = false;
auto ret = HAL_GetCurrentThreadPriority(&rt, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
*isRealTime = rt;
return ret;
}
@@ -37,14 +37,14 @@
int32_t status = 0;
auto native = thread.native_handle();
auto ret = HAL_SetThreadPriority(&native, realTime, priority, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return ret;
}
bool SetCurrentThreadPriority(bool realTime, int priority) {
int32_t status = 0;
auto ret = HAL_SetCurrentThreadPriority(realTime, priority, &status);
- wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setGlobalHALError(status);
return ret;
}
diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp
index ffff2dd..a9358dd 100644
--- a/wpilibc/src/main/native/cpp/TimedRobot.cpp
+++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp
@@ -11,7 +11,9 @@
#include <utility>
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/Notifier.h>
#include "frc/Timer.h"
#include "frc/Utility.h"
@@ -43,6 +45,11 @@
}
}
+void TimedRobot::EndCompetition() {
+ int32_t status = 0;
+ HAL_StopNotifier(m_notifier, &status);
+}
+
units::second_t TimedRobot::GetPeriod() const {
return units::second_t(m_period);
}
@@ -52,7 +59,8 @@
TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
+ HAL_SetNotifierName(m_notifier, "TimedRobot", &status);
HAL_Report(HALUsageReporting::kResourceType_Framework,
HALUsageReporting::kFramework_Timed);
@@ -62,7 +70,7 @@
int32_t status = 0;
HAL_StopNotifier(m_notifier, &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
HAL_CleanNotifier(m_notifier, &status);
}
@@ -71,5 +79,5 @@
int32_t status = 0;
HAL_UpdateNotifierAlarm(
m_notifier, static_cast<uint64_t>(m_expirationTime * 1e6), &status);
- wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ wpi_setHALError(status);
}
diff --git a/wpilibc/src/main/native/cpp/Timer.cpp b/wpilibc/src/main/native/cpp/Timer.cpp
index cdfa9ab..c91bc13 100644
--- a/wpilibc/src/main/native/cpp/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/Timer.cpp
@@ -10,7 +10,7 @@
#include <chrono>
#include <thread>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
#include "frc/RobotController.h"
diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
index 35320de..fcd016e 100644
--- a/wpilibc/src/main/native/cpp/Ultrasonic.cpp
+++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
@@ -7,7 +7,7 @@
#include "frc/Ultrasonic.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/Counter.h"
#include "frc/DigitalInput.h"
diff --git a/wpilibc/src/main/native/cpp/Utility.cpp b/wpilibc/src/main/native/cpp/Utility.cpp
index 499f9b4..11f6234 100644
--- a/wpilibc/src/main/native/cpp/Utility.cpp
+++ b/wpilibc/src/main/native/cpp/Utility.cpp
@@ -17,7 +17,7 @@
#include <cstring>
#include <hal/DriverStation.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <wpi/Path.h>
#include <wpi/SmallString.h>
#include <wpi/StackTrace.h>
diff --git a/wpilibc/src/main/native/cpp/Victor.cpp b/wpilibc/src/main/native/cpp/Victor.cpp
index 49dcd57..bce1913 100644
--- a/wpilibc/src/main/native/cpp/Victor.cpp
+++ b/wpilibc/src/main/native/cpp/Victor.cpp
@@ -7,32 +7,18 @@
#include "frc/Victor.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
Victor::Victor(int channel) : PWMSpeedController(channel) {
- /* Note that the Victor uses the following bounds for PWM values. These
- * values were determined empirically and optimized for the Victor 888. These
- * values should work reasonably well for Victor 884 controllers as well but
- * if users experience issues such as asymmetric behaviour around the deadband
- * or inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the Victor 884 User
- * Manual available from IFI.
- *
- * 2.027ms = full "forward"
- * 1.525ms = the "high end" of the deadband range
- * 1.507ms = center of the deadband range (off)
- * 1.49ms = the "low end" of the deadband range
- * 1.026ms = full "reverse"
- */
SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
SetPeriodMultiplier(kPeriodMultiplier_2X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel() + 1);
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 82e966f..221777d 100644
--- a/wpilibc/src/main/native/cpp/VictorSP.cpp
+++ b/wpilibc/src/main/native/cpp/VictorSP.cpp
@@ -7,31 +7,18 @@
#include "frc/VictorSP.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableRegistry.h"
using namespace frc;
VictorSP::VictorSP(int channel) : PWMSpeedController(channel) {
- /* Note that the VictorSP uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the VictorSP User
- * Manual available from Vex.
- *
- * 2.004ms = full "forward"
- * 1.52ms = the "high end" of the deadband range
- * 1.50ms = center of the deadband range (off)
- * 1.48ms = the "low end" of the deadband range
- * 0.997ms = full "reverse"
- */
- SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetSpeed(0.0);
SetZeroLatch();
- HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel());
+ HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel() + 1);
SendableRegistry::GetInstance().SetName(this, "VictorSP", GetChannel());
}
diff --git a/wpilibc/src/main/native/cpp/XboxController.cpp b/wpilibc/src/main/native/cpp/XboxController.cpp
index fa54980..b294257 100644
--- a/wpilibc/src/main/native/cpp/XboxController.cpp
+++ b/wpilibc/src/main/native/cpp/XboxController.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. */
@@ -7,12 +7,12 @@
#include "frc/XboxController.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
using namespace frc;
XboxController::XboxController(int port) : GenericHID(port) {
- HAL_Report(HALUsageReporting::kResourceType_XboxController, port);
+ HAL_Report(HALUsageReporting::kResourceType_XboxController, port + 1);
}
double XboxController::GetX(JoystickHand hand) const {
diff --git a/wpilibc/src/main/native/cpp/buttons/Button.cpp b/wpilibc/src/main/native/cpp/buttons/Button.cpp
deleted file mode 100644
index 45ede65..0000000
--- a/wpilibc/src/main/native/cpp/buttons/Button.cpp
+++ /dev/null
@@ -1,20 +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/buttons/Button.h"
-
-using namespace frc;
-
-void Button::WhenPressed(Command* command) { WhenActive(command); }
-
-void Button::WhileHeld(Command* command) { WhileActive(command); }
-
-void Button::WhenReleased(Command* command) { WhenInactive(command); }
-
-void Button::CancelWhenPressed(Command* command) { CancelWhenActive(command); }
-
-void Button::ToggleWhenPressed(Command* command) { ToggleWhenActive(command); }
diff --git a/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp
deleted file mode 100644
index 1e10255..0000000
--- a/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp
+++ /dev/null
@@ -1,17 +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/buttons/ButtonScheduler.h"
-
-#include "frc/commands/Scheduler.h"
-
-using namespace frc;
-
-ButtonScheduler::ButtonScheduler(bool last, Trigger* button, Command* orders)
- : m_pressedLast(last), m_button(button), m_command(orders) {}
-
-void ButtonScheduler::Start() { Scheduler::GetInstance()->AddButton(this); }
diff --git a/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
deleted file mode 100644
index 39d1d25..0000000
--- a/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
+++ /dev/null
@@ -1,27 +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/buttons/CancelButtonScheduler.h"
-
-#include "frc/buttons/Button.h"
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger* button,
- Command* orders)
- : ButtonScheduler(last, button, orders) {}
-
-void CancelButtonScheduler::Execute() {
- bool pressed = m_button->Grab();
-
- if (!m_pressedLast && pressed) {
- m_command->Cancel();
- }
-
- m_pressedLast = pressed;
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
deleted file mode 100644
index feaa3c6..0000000
--- a/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
+++ /dev/null
@@ -1,29 +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/buttons/HeldButtonScheduler.h"
-
-#include "frc/buttons/Button.h"
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger* button,
- Command* orders)
- : ButtonScheduler(last, button, orders) {}
-
-void HeldButtonScheduler::Execute() {
- bool pressed = m_button->Grab();
-
- if (pressed) {
- m_command->Start();
- } else if (m_pressedLast && !pressed) {
- m_command->Cancel();
- }
-
- m_pressedLast = pressed;
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp b/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp
deleted file mode 100644
index cac67c4..0000000
--- a/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp
+++ /dev/null
@@ -1,19 +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/buttons/InternalButton.h"
-
-using namespace frc;
-
-InternalButton::InternalButton(bool inverted)
- : m_pressed(inverted), m_inverted(inverted) {}
-
-void InternalButton::SetInverted(bool inverted) { m_inverted = inverted; }
-
-void InternalButton::SetPressed(bool pressed) { m_pressed = pressed; }
-
-bool InternalButton::Get() { return m_pressed ^ m_inverted; }
diff --git a/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp b/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp
deleted file mode 100644
index 2c93be2..0000000
--- a/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp
+++ /dev/null
@@ -1,15 +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/buttons/JoystickButton.h"
-
-using namespace frc;
-
-JoystickButton::JoystickButton(GenericHID* joystick, int buttonNumber)
- : m_joystick(joystick), m_buttonNumber(buttonNumber) {}
-
-bool JoystickButton::Get() { return m_joystick->GetRawButton(m_buttonNumber); }
diff --git a/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp b/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp
deleted file mode 100644
index 5e8b1e0..0000000
--- a/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp
+++ /dev/null
@@ -1,26 +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/buttons/NetworkButton.h"
-
-#include <networktables/NetworkTable.h>
-#include <networktables/NetworkTableInstance.h>
-
-using namespace frc;
-
-NetworkButton::NetworkButton(const wpi::Twine& tableName,
- const wpi::Twine& field)
- : NetworkButton(nt::NetworkTableInstance::GetDefault().GetTable(tableName),
- field) {}
-
-NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
- const wpi::Twine& field)
- : m_entry(table->GetEntry(field)) {}
-
-bool NetworkButton::Get() {
- return m_entry.GetInstance().IsConnected() && m_entry.GetBoolean(false);
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/POVButton.cpp b/wpilibc/src/main/native/cpp/buttons/POVButton.cpp
deleted file mode 100644
index 6729923..0000000
--- a/wpilibc/src/main/native/cpp/buttons/POVButton.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/buttons/POVButton.h"
-
-using namespace frc;
-
-POVButton::POVButton(GenericHID& joystick, int angle, int povNumber)
- : m_joystick(&joystick), m_angle(angle), m_povNumber(povNumber) {}
-
-bool POVButton::Get() { return m_joystick->GetPOV(m_povNumber) == m_angle; }
diff --git a/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
deleted file mode 100644
index a964117..0000000
--- a/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
+++ /dev/null
@@ -1,27 +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/buttons/PressedButtonScheduler.h"
-
-#include "frc/buttons/Button.h"
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger* button,
- Command* orders)
- : ButtonScheduler(last, button, orders) {}
-
-void PressedButtonScheduler::Execute() {
- bool pressed = m_button->Grab();
-
- if (!m_pressedLast && pressed) {
- m_command->Start();
- }
-
- m_pressedLast = pressed;
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
deleted file mode 100644
index 6d67004..0000000
--- a/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
+++ /dev/null
@@ -1,27 +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/buttons/ReleasedButtonScheduler.h"
-
-#include "frc/buttons/Button.h"
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger* button,
- Command* orders)
- : ButtonScheduler(last, button, orders) {}
-
-void ReleasedButtonScheduler::Execute() {
- bool pressed = m_button->Grab();
-
- if (m_pressedLast && !pressed) {
- m_command->Start();
- }
-
- m_pressedLast = pressed;
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
deleted file mode 100644
index b722d45..0000000
--- a/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
+++ /dev/null
@@ -1,31 +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/buttons/ToggleButtonScheduler.h"
-
-#include "frc/buttons/Button.h"
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger* button,
- Command* orders)
- : ButtonScheduler(last, button, orders) {}
-
-void ToggleButtonScheduler::Execute() {
- bool pressed = m_button->Grab();
-
- if (!m_pressedLast && pressed) {
- if (m_command->IsRunning()) {
- m_command->Cancel();
- } else {
- m_command->Start();
- }
- }
-
- m_pressedLast = pressed;
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/Trigger.cpp b/wpilibc/src/main/native/cpp/buttons/Trigger.cpp
deleted file mode 100644
index f215083..0000000
--- a/wpilibc/src/main/native/cpp/buttons/Trigger.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/buttons/Button.h"
-#include "frc/buttons/CancelButtonScheduler.h"
-#include "frc/buttons/HeldButtonScheduler.h"
-#include "frc/buttons/PressedButtonScheduler.h"
-#include "frc/buttons/ReleasedButtonScheduler.h"
-#include "frc/buttons/ToggleButtonScheduler.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-
-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) {
- auto pbs = new PressedButtonScheduler(Grab(), this, command);
- pbs->Start();
-}
-
-void Trigger::WhileActive(Command* command) {
- auto hbs = new HeldButtonScheduler(Grab(), this, command);
- hbs->Start();
-}
-
-void Trigger::WhenInactive(Command* command) {
- auto rbs = new ReleasedButtonScheduler(Grab(), this, command);
- rbs->Start();
-}
-
-void Trigger::CancelWhenActive(Command* command) {
- auto cbs = new CancelButtonScheduler(Grab(), this, command);
- cbs->Start();
-}
-
-void Trigger::ToggleWhenActive(Command* command) {
- auto tbs = new ToggleButtonScheduler(Grab(), this, command);
- tbs->Start();
-}
-
-void Trigger::InitSendable(SendableBuilder& builder) {
- builder.SetSmartDashboardType("Button");
- builder.SetSafeState([=]() { m_sendablePressed = false; });
- builder.AddBooleanProperty("pressed", [=]() { return Grab(); },
- [=](bool value) { m_sendablePressed = value; });
-}
diff --git a/wpilibc/src/main/native/cpp/commands/Command.cpp b/wpilibc/src/main/native/cpp/commands/Command.cpp
deleted file mode 100644
index a7154c0..0000000
--- a/wpilibc/src/main/native/cpp/commands/Command.cpp
+++ /dev/null
@@ -1,264 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/commands/Command.h"
-
-#include <typeinfo>
-
-#include "frc/RobotState.h"
-#include "frc/Timer.h"
-#include "frc/WPIErrors.h"
-#include "frc/commands/CommandGroup.h"
-#include "frc/commands/Scheduler.h"
-#include "frc/livewindow/LiveWindow.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-int Command::m_commandCounter = 0;
-
-Command::Command() : Command("", -1.0) {}
-
-Command::Command(const wpi::Twine& name) : Command(name, -1.0) {}
-
-Command::Command(double timeout) : Command("", timeout) {}
-
-Command::Command(Subsystem& subsystem) : Command("", -1.0) {
- Requires(&subsystem);
-}
-
-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");
-
- m_timeout = timeout;
-
- // If name contains an empty string
- if (name.isTriviallyEmpty() ||
- (name.isSingleStringRef() && name.getSingleStringRef().empty())) {
- SendableRegistry::GetInstance().Add(
- this, "Command_" + wpi::Twine(typeid(*this).name()));
- } else {
- SendableRegistry::GetInstance().Add(this, name);
- }
-}
-
-Command::Command(const wpi::Twine& name, Subsystem& subsystem)
- : Command(name, -1.0) {
- Requires(&subsystem);
-}
-
-Command::Command(double timeout, Subsystem& subsystem) : Command("", timeout) {
- Requires(&subsystem);
-}
-
-Command::Command(const wpi::Twine& name, double timeout, Subsystem& subsystem)
- : Command(name, timeout) {
- Requires(&subsystem);
-}
-
-double Command::TimeSinceInitialized() const {
- if (m_startTime < 0.0)
- return 0.0;
- else
- return Timer::GetFPGATimestamp() - m_startTime;
-}
-
-void Command::Requires(Subsystem* subsystem) {
- if (!AssertUnlocked("Can not add new requirement to command")) return;
-
- if (subsystem != nullptr)
- m_requirements.insert(subsystem);
- else
- wpi_setWPIErrorWithContext(NullParameter, "subsystem");
-}
-
-void Command::Start() {
- LockChanges();
- if (m_parent != nullptr)
- wpi_setWPIErrorWithContext(
- CommandIllegalUse,
- "Can not start a command that is part of a command group");
-
- m_completed = false;
- Scheduler::GetInstance()->AddCommand(this);
-}
-
-bool Command::Run() {
- if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled())
- Cancel();
-
- if (IsCanceled()) return false;
-
- if (!m_initialized) {
- m_initialized = true;
- StartTiming();
- _Initialize();
- Initialize();
- }
- _Execute();
- Execute();
- return !IsFinished();
-}
-
-void Command::Cancel() {
- if (m_parent != nullptr)
- wpi_setWPIErrorWithContext(
- CommandIllegalUse,
- "Can not cancel a command that is part of a command group");
-
- _Cancel();
-}
-
-bool Command::IsRunning() const { return m_running; }
-
-bool Command::IsInitialized() const { return m_initialized; }
-
-bool Command::IsCompleted() const { return m_completed; }
-
-bool Command::IsCanceled() const { return m_canceled; }
-
-bool Command::IsInterruptible() const { return m_interruptible; }
-
-void Command::SetInterruptible(bool interruptible) {
- m_interruptible = interruptible;
-}
-
-bool Command::DoesRequire(Subsystem* system) const {
- return m_requirements.count(system) > 0;
-}
-
-const Command::SubsystemSet& Command::GetRequirements() const {
- return m_requirements;
-}
-
-CommandGroup* Command::GetGroup() const { return m_parent; }
-
-void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
-
-bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
-
-int Command::GetID() const { return m_commandID; }
-
-void Command::SetTimeout(double timeout) {
- if (timeout < 0.0)
- wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
- else
- m_timeout = timeout;
-}
-
-bool Command::IsTimedOut() const {
- return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
-}
-
-bool Command::AssertUnlocked(const std::string& message) {
- if (m_locked) {
- std::string buf =
- message + " after being started or being added to a command group";
- wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
- return false;
- }
- return true;
-}
-
-void Command::SetParent(CommandGroup* parent) {
- if (parent == nullptr) {
- wpi_setWPIErrorWithContext(NullParameter, "parent");
- } else if (m_parent != nullptr) {
- wpi_setWPIErrorWithContext(CommandIllegalUse,
- "Can not give command to a command group after "
- "already being put in a command group");
- } else {
- LockChanges();
- m_parent = parent;
- }
-}
-
-bool Command::IsParented() const { return m_parent != nullptr; }
-
-void Command::ClearRequirements() { m_requirements.clear(); }
-
-void Command::Initialize() {}
-
-void Command::Execute() {}
-
-void Command::End() {}
-
-void Command::Interrupted() { End(); }
-
-void Command::_Initialize() { m_completed = false; }
-
-void Command::_Interrupted() { m_completed = true; }
-
-void Command::_Execute() {}
-
-void Command::_End() { m_completed = true; }
-
-void Command::_Cancel() {
- if (IsRunning()) m_canceled = true;
-}
-
-void Command::LockChanges() { m_locked = true; }
-
-void Command::Removed() {
- if (m_initialized) {
- if (IsCanceled()) {
- Interrupted();
- _Interrupted();
- } else {
- End();
- _End();
- }
- }
- m_initialized = false;
- m_canceled = false;
- m_running = false;
- m_completed = true;
-}
-
-void Command::StartRunning() {
- m_running = true;
- m_startTime = -1;
- m_completed = false;
-}
-
-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 SendableRegistry::GetInstance().GetName(this); },
- nullptr);
- builder.AddBooleanProperty("running", [=]() { return IsRunning(); },
- [=](bool value) {
- if (value) {
- if (!IsRunning()) Start();
- } else {
- if (IsRunning()) Cancel();
- }
- });
- builder.AddBooleanProperty(".isParented", [=]() { return IsParented(); },
- nullptr);
-}
diff --git a/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp b/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp
deleted file mode 100644
index eac1746..0000000
--- a/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp
+++ /dev/null
@@ -1,243 +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/commands/CommandGroup.h"
-
-#include "frc/WPIErrors.h"
-
-using namespace frc;
-
-CommandGroup::CommandGroup(const wpi::Twine& name) : Command(name) {}
-
-void CommandGroup::AddSequential(Command* command) {
- if (command == nullptr) {
- wpi_setWPIErrorWithContext(NullParameter, "command");
- return;
- }
- if (!AssertUnlocked("Cannot add new command to command group")) return;
-
- m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
-
- command->SetParent(this);
-
- // Iterate through command->GetRequirements() and call Requires() on each
- // required subsystem
- for (auto&& requirement : command->GetRequirements()) Requires(requirement);
-}
-
-void CommandGroup::AddSequential(Command* command, double timeout) {
- if (command == nullptr) {
- wpi_setWPIErrorWithContext(NullParameter, "command");
- return;
- }
- if (!AssertUnlocked("Cannot add new command to command group")) return;
- if (timeout < 0.0) {
- wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
- return;
- }
-
- m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
- timeout);
-
- command->SetParent(this);
-
- // Iterate through command->GetRequirements() and call Requires() on each
- // required subsystem
- for (auto&& requirement : command->GetRequirements()) Requires(requirement);
-}
-
-void CommandGroup::AddParallel(Command* command) {
- if (command == nullptr) {
- wpi_setWPIErrorWithContext(NullParameter, "command");
- return;
- }
- if (!AssertUnlocked("Cannot add new command to command group")) return;
-
- m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
-
- command->SetParent(this);
-
- // Iterate through command->GetRequirements() and call Requires() on each
- // required subsystem
- for (auto&& requirement : command->GetRequirements()) Requires(requirement);
-}
-
-void CommandGroup::AddParallel(Command* command, double timeout) {
- if (command == nullptr) {
- wpi_setWPIErrorWithContext(NullParameter, "command");
- return;
- }
- if (!AssertUnlocked("Cannot add new command to command group")) return;
- if (timeout < 0.0) {
- wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
- return;
- }
-
- m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
- timeout);
-
- command->SetParent(this);
-
- // Iterate through command->GetRequirements() and call Requires() on each
- // required subsystem
- for (auto&& requirement : command->GetRequirements()) Requires(requirement);
-}
-
-bool CommandGroup::IsInterruptible() const {
- if (!Command::IsInterruptible()) return false;
-
- if (m_currentCommandIndex != -1 &&
- static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
- Command* cmd = m_commands[m_currentCommandIndex].m_command;
- if (!cmd->IsInterruptible()) return false;
- }
-
- for (const auto& child : m_children) {
- if (!child->m_command->IsInterruptible()) return false;
- }
-
- return true;
-}
-
-int CommandGroup::GetSize() const { return m_children.size(); }
-
-void CommandGroup::Initialize() {}
-
-void CommandGroup::Execute() {}
-
-bool CommandGroup::IsFinished() {
- return static_cast<size_t>(m_currentCommandIndex) >= m_commands.size() &&
- m_children.empty();
-}
-
-void CommandGroup::End() {}
-
-void CommandGroup::Interrupted() {}
-
-void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
-
-void CommandGroup::_Execute() {
- CommandGroupEntry* entry;
- Command* cmd = nullptr;
- bool firstRun = false;
-
- if (m_currentCommandIndex == -1) {
- firstRun = true;
- m_currentCommandIndex = 0;
- }
-
- // While there are still commands in this group to run
- while (static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
- // If a command is prepared to run
- if (cmd != nullptr) {
- // If command timed out, cancel it so it's removed from the Scheduler
- if (entry->IsTimedOut()) cmd->_Cancel();
-
- // If command finished or was cancelled, remove it from Scheduler
- if (cmd->Run()) {
- break;
- } else {
- cmd->Removed();
-
- // Advance to next command in group
- m_currentCommandIndex++;
- firstRun = true;
- cmd = nullptr;
- continue;
- }
- }
-
- entry = &m_commands[m_currentCommandIndex];
- cmd = nullptr;
-
- switch (entry->m_state) {
- case CommandGroupEntry::kSequence_InSequence:
- cmd = entry->m_command;
- if (firstRun) {
- cmd->StartRunning();
- CancelConflicts(cmd);
- firstRun = false;
- }
- break;
-
- case CommandGroupEntry::kSequence_BranchPeer:
- // Start executing a parallel command and advance to next entry in group
- m_currentCommandIndex++;
- entry->m_command->Start();
- break;
-
- case CommandGroupEntry::kSequence_BranchChild:
- m_currentCommandIndex++;
-
- /* Causes scheduler to skip children of current command which require
- * the same subsystems as it
- */
- CancelConflicts(entry->m_command);
- entry->m_command->StartRunning();
-
- // Add current command entry to list of children of this group
- m_children.push_back(entry);
- break;
- }
- }
-
- // Run Children
- for (auto& entry : m_children) {
- auto child = entry->m_command;
- if (entry->IsTimedOut()) {
- child->_Cancel();
- }
-
- // If child finished or was cancelled, set it to nullptr. nullptr entries
- // are removed later.
- if (!child->Run()) {
- child->Removed();
- entry = nullptr;
- }
- }
-
- m_children.erase(std::remove(m_children.begin(), m_children.end(), nullptr),
- m_children.end());
-}
-
-void CommandGroup::_End() {
- // Theoretically, we don't have to check this, but we do if teams override the
- // IsFinished method
- if (m_currentCommandIndex != -1 &&
- static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
- Command* cmd = m_commands[m_currentCommandIndex].m_command;
- cmd->_Cancel();
- cmd->Removed();
- }
-
- for (auto& child : m_children) {
- Command* cmd = child->m_command;
- cmd->_Cancel();
- cmd->Removed();
- }
- m_children.clear();
-}
-
-void CommandGroup::_Interrupted() { _End(); }
-
-void CommandGroup::CancelConflicts(Command* command) {
- for (auto childIter = m_children.begin(); childIter != m_children.end();) {
- Command* child = (*childIter)->m_command;
- bool erased = false;
-
- for (auto&& requirement : command->GetRequirements()) {
- if (child->DoesRequire(requirement)) {
- child->_Cancel();
- child->Removed();
- childIter = m_children.erase(childIter);
- erased = true;
- break;
- }
- }
- if (!erased) childIter++;
- }
-}
diff --git a/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp b/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp
deleted file mode 100644
index 7a75f00..0000000
--- a/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp
+++ /dev/null
@@ -1,23 +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/commands/CommandGroupEntry.h"
-
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-CommandGroupEntry::CommandGroupEntry(Command* command, Sequence state,
- double timeout)
- : m_timeout(timeout), m_command(command), m_state(state) {}
-
-bool CommandGroupEntry::IsTimedOut() const {
- if (m_timeout < 0.0) return false;
- double time = m_command->TimeSinceInitialized();
- if (time == 0.0) return false;
- return time >= m_timeout;
-}
diff --git a/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp b/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp
deleted file mode 100644
index c44f7ba..0000000
--- a/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp
+++ /dev/null
@@ -1,80 +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/commands/ConditionalCommand.h"
-
-#include "frc/commands/Scheduler.h"
-
-using namespace frc;
-
-static void RequireAll(Command& command, Command* onTrue, Command* onFalse) {
- if (onTrue != nullptr) {
- for (auto requirement : onTrue->GetRequirements())
- command.Requires(requirement);
- }
- if (onFalse != nullptr) {
- for (auto requirement : onFalse->GetRequirements())
- command.Requires(requirement);
- }
-}
-
-ConditionalCommand::ConditionalCommand(Command* onTrue, Command* onFalse) {
- m_onTrue = onTrue;
- m_onFalse = onFalse;
-
- RequireAll(*this, onTrue, onFalse);
-}
-
-ConditionalCommand::ConditionalCommand(const wpi::Twine& name, Command* onTrue,
- Command* onFalse)
- : Command(name) {
- m_onTrue = onTrue;
- m_onFalse = onFalse;
-
- RequireAll(*this, onTrue, onFalse);
-}
-
-void ConditionalCommand::_Initialize() {
- if (Condition()) {
- m_chosenCommand = m_onTrue;
- } else {
- m_chosenCommand = m_onFalse;
- }
-
- if (m_chosenCommand != nullptr) {
- // This is a hack to make cancelling the chosen command inside a
- // CommandGroup work properly
- m_chosenCommand->ClearRequirements();
-
- m_chosenCommand->Start();
- }
- Command::_Initialize();
-}
-
-void ConditionalCommand::_Cancel() {
- if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
- m_chosenCommand->Cancel();
- }
-
- Command::_Cancel();
-}
-
-bool ConditionalCommand::IsFinished() {
- if (m_chosenCommand != nullptr) {
- return m_chosenCommand->IsCompleted();
- } else {
- return true;
- }
-}
-
-void ConditionalCommand::_Interrupted() {
- if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
- m_chosenCommand->Cancel();
- }
-
- Command::_Interrupted();
-}
diff --git a/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp b/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp
deleted file mode 100644
index 88ebfde..0000000
--- a/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp
+++ /dev/null
@@ -1,45 +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/commands/InstantCommand.h"
-
-using namespace frc;
-
-InstantCommand::InstantCommand(const wpi::Twine& name) : Command(name) {}
-
-InstantCommand::InstantCommand(Subsystem& subsystem) : Command(subsystem) {}
-
-InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem)
- : Command(name, subsystem) {}
-
-InstantCommand::InstantCommand(std::function<void()> func) : m_func(func) {}
-
-InstantCommand::InstantCommand(Subsystem& subsystem, std::function<void()> func)
- : InstantCommand(subsystem) {
- m_func = func;
-}
-
-InstantCommand::InstantCommand(const wpi::Twine& name,
- std::function<void()> func)
- : InstantCommand(name) {
- m_func = func;
-}
-
-InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
- std::function<void()> func)
- : InstantCommand(name, subsystem) {
- m_func = func;
-}
-
-void InstantCommand::_Initialize() {
- Command::_Initialize();
- if (m_func) {
- m_func();
- }
-}
-
-bool InstantCommand::IsFinished() { return true; }
diff --git a/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp b/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp
deleted file mode 100644
index 875d8fe..0000000
--- a/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp
+++ /dev/null
@@ -1,108 +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/commands/PIDCommand.h"
-
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
- double f, double period)
- : Command(name) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, double f, double period) {
- m_controller =
- std::make_shared<PIDController>(p, i, d, f, this, this, period);
-}
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d)
- : Command(name) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this);
-}
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
- double period)
- : Command(name) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, double period) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
- double f, double period, Subsystem& subsystem)
- : Command(name, subsystem) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, double f, double period,
- Subsystem& subsystem)
- : Command(subsystem) {
- m_controller =
- std::make_shared<PIDController>(p, i, d, f, this, this, period);
-}
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
- Subsystem& subsystem)
- : Command(name, subsystem) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this);
-}
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
- double period, Subsystem& subsystem)
- : Command(name, subsystem) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, Subsystem& subsystem) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, double period,
- Subsystem& subsystem) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-void PIDCommand::_Initialize() { m_controller->Enable(); }
-
-void PIDCommand::_End() { m_controller->Disable(); }
-
-void PIDCommand::_Interrupted() { _End(); }
-
-void PIDCommand::SetSetpointRelative(double deltaSetpoint) {
- SetSetpoint(GetSetpoint() + deltaSetpoint);
-}
-
-void PIDCommand::PIDWrite(double output) { UsePIDOutput(output); }
-
-double PIDCommand::PIDGet() { return ReturnPIDInput(); }
-
-std::shared_ptr<PIDController> PIDCommand::GetPIDController() const {
- return m_controller;
-}
-
-void PIDCommand::SetSetpoint(double setpoint) {
- m_controller->SetSetpoint(setpoint);
-}
-
-double PIDCommand::GetSetpoint() const { return m_controller->GetSetpoint(); }
-
-double PIDCommand::GetPosition() { return ReturnPIDInput(); }
-
-void PIDCommand::InitSendable(SendableBuilder& builder) {
- m_controller->InitSendable(builder);
- Command::InitSendable(builder);
- builder.SetSmartDashboardType("PIDCommand");
-}
diff --git a/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp b/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp
deleted file mode 100644
index 0893da3..0000000
--- a/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp
+++ /dev/null
@@ -1,97 +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/commands/PIDSubsystem.h"
-
-#include "frc/PIDController.h"
-
-using namespace frc;
-
-PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d)
- : Subsystem(name) {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this);
- AddChild("PIDController", m_controller);
-}
-
-PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
- double f)
- : Subsystem(name) {
- m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
- AddChild("PIDController", m_controller);
-}
-
-PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
- double f, double period)
- : Subsystem(name) {
- m_controller =
- std::make_shared<PIDController>(p, i, d, f, this, this, period);
- AddChild("PIDController", m_controller);
-}
-
-PIDSubsystem::PIDSubsystem(double p, double i, double d)
- : Subsystem("PIDSubsystem") {
- m_controller = std::make_shared<PIDController>(p, i, d, this, this);
- AddChild("PIDController", m_controller);
-}
-
-PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
- : Subsystem("PIDSubsystem") {
- m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
- AddChild("PIDController", m_controller);
-}
-
-PIDSubsystem::PIDSubsystem(double p, double i, double d, double f,
- double period)
- : Subsystem("PIDSubsystem") {
- m_controller =
- std::make_shared<PIDController>(p, i, d, f, this, this, period);
- AddChild("PIDController", m_controller);
-}
-
-void PIDSubsystem::Enable() { m_controller->Enable(); }
-
-void PIDSubsystem::Disable() { m_controller->Disable(); }
-
-void PIDSubsystem::PIDWrite(double output) { UsePIDOutput(output); }
-
-double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
-
-void PIDSubsystem::SetSetpoint(double setpoint) {
- m_controller->SetSetpoint(setpoint);
-}
-
-void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
- SetSetpoint(GetSetpoint() + deltaSetpoint);
-}
-
-void PIDSubsystem::SetInputRange(double minimumInput, double maximumInput) {
- m_controller->SetInputRange(minimumInput, maximumInput);
-}
-
-void PIDSubsystem::SetOutputRange(double minimumOutput, double maximumOutput) {
- m_controller->SetOutputRange(minimumOutput, maximumOutput);
-}
-
-double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
-
-double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
-
-double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
-
-void PIDSubsystem::SetAbsoluteTolerance(double absValue) {
- m_controller->SetAbsoluteTolerance(absValue);
-}
-
-void PIDSubsystem::SetPercentTolerance(double percent) {
- m_controller->SetPercentTolerance(percent);
-}
-
-bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
-
-std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
- return m_controller;
-}
diff --git a/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp b/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp
deleted file mode 100644
index b4bea24..0000000
--- a/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp
+++ /dev/null
@@ -1,19 +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/commands/PrintCommand.h"
-
-#include <wpi/raw_ostream.h>
-
-using namespace frc;
-
-PrintCommand::PrintCommand(const wpi::Twine& message)
- : InstantCommand("Print \"" + message + wpi::Twine('"')) {
- m_message = message.str();
-}
-
-void PrintCommand::Initialize() { wpi::outs() << m_message << '\n'; }
diff --git a/wpilibc/src/main/native/cpp/commands/Scheduler.cpp b/wpilibc/src/main/native/cpp/commands/Scheduler.cpp
deleted file mode 100644
index f406d74..0000000
--- a/wpilibc/src/main/native/cpp/commands/Scheduler.cpp
+++ /dev/null
@@ -1,245 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/commands/Scheduler.h"
-
-#include <algorithm>
-#include <set>
-#include <string>
-#include <vector>
-
-#include <hal/HAL.h>
-#include <networktables/NetworkTableEntry.h>
-#include <wpi/mutex.h>
-
-#include "frc/WPIErrors.h"
-#include "frc/buttons/ButtonScheduler.h"
-#include "frc/commands/Command.h"
-#include "frc/commands/Subsystem.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-struct Scheduler::Impl {
- void Remove(Command* command);
- void ProcessCommandAddition(Command* command);
-
- using SubsystemSet = std::set<Subsystem*>;
- SubsystemSet subsystems;
- wpi::mutex buttonsMutex;
- using ButtonVector = std::vector<std::unique_ptr<ButtonScheduler>>;
- ButtonVector buttons;
- using CommandVector = std::vector<Command*>;
- wpi::mutex additionsMutex;
- CommandVector additions;
- using CommandSet = std::set<Command*>;
- CommandSet commands;
- bool adding = false;
- bool enabled = true;
- std::vector<std::string> commandsBuf;
- std::vector<double> idsBuf;
- bool runningCommandsChanged = false;
-};
-
-Scheduler* Scheduler::GetInstance() {
- static Scheduler instance;
- return &instance;
-}
-
-void Scheduler::AddCommand(Command* command) {
- std::scoped_lock lock(m_impl->additionsMutex);
- if (std::find(m_impl->additions.begin(), m_impl->additions.end(), command) !=
- m_impl->additions.end())
- return;
- m_impl->additions.push_back(command);
-}
-
-void Scheduler::AddButton(ButtonScheduler* button) {
- std::scoped_lock lock(m_impl->buttonsMutex);
- m_impl->buttons.emplace_back(button);
-}
-
-void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
- if (subsystem == nullptr) {
- wpi_setWPIErrorWithContext(NullParameter, "subsystem");
- return;
- }
- m_impl->subsystems.insert(subsystem);
-}
-
-void Scheduler::Run() {
- // Get button input (going backwards preserves button priority)
- {
- if (!m_impl->enabled) return;
-
- std::scoped_lock lock(m_impl->buttonsMutex);
- for (auto& button : m_impl->buttons) {
- button->Execute();
- }
- }
-
- // Call every subsystem's periodic method
- for (auto& subsystem : m_impl->subsystems) {
- subsystem->Periodic();
- }
-
- m_impl->runningCommandsChanged = false;
-
- // Loop through the commands
- for (auto cmdIter = m_impl->commands.begin();
- cmdIter != m_impl->commands.end();) {
- Command* command = *cmdIter;
- // Increment before potentially removing to keep the iterator valid
- ++cmdIter;
- if (!command->Run()) {
- Remove(command);
- m_impl->runningCommandsChanged = true;
- }
- }
-
- // Add the new things
- {
- 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) {
- wpi_setWPIErrorWithContext(IncompatibleState,
- "Can not start command from cancel method");
- } else {
- m_impl->ProcessCommandAddition(addition);
- }
- }
- m_impl->additions.clear();
- }
-
- // Add in the defaults
- for (auto& subsystem : m_impl->subsystems) {
- if (subsystem->GetCurrentCommand() == nullptr) {
- if (m_impl->adding) {
- wpi_setWPIErrorWithContext(IncompatibleState,
- "Can not start command from cancel method");
- } else {
- m_impl->ProcessCommandAddition(subsystem->GetDefaultCommand());
- }
- }
- subsystem->ConfirmCommand();
- }
-}
-
-void Scheduler::Remove(Command* command) {
- if (command == nullptr) {
- wpi_setWPIErrorWithContext(NullParameter, "command");
- return;
- }
-
- m_impl->Remove(command);
-}
-
-void Scheduler::RemoveAll() {
- while (m_impl->commands.size() > 0) {
- Remove(*m_impl->commands.begin());
- }
-}
-
-void Scheduler::ResetAll() {
- RemoveAll();
- m_impl->subsystems.clear();
- m_impl->buttons.clear();
- m_impl->additions.clear();
- m_impl->commands.clear();
-}
-
-void Scheduler::SetEnabled(bool enabled) { m_impl->enabled = enabled; }
-
-void Scheduler::InitSendable(SendableBuilder& builder) {
- builder.SetSmartDashboardType("Scheduler");
- auto namesEntry = builder.GetEntry("Names");
- auto idsEntry = builder.GetEntry("Ids");
- auto cancelEntry = builder.GetEntry("Cancel");
- builder.SetUpdateTable([=]() {
- // Get the list of possible commands to cancel
- auto new_toCancel = cancelEntry.GetValue();
- wpi::ArrayRef<double> toCancel;
- if (new_toCancel) toCancel = new_toCancel->GetDoubleArray();
-
- // Cancel commands whose cancel buttons were pressed on the SmartDashboard
- if (!toCancel.empty()) {
- for (auto& command : m_impl->commands) {
- for (const auto& cancelled : toCancel) {
- if (command->GetID() == cancelled) {
- command->Cancel();
- }
- }
- }
- nt::NetworkTableEntry(cancelEntry).SetDoubleArray({});
- }
-
- // Set the running commands
- 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(registry.GetName(command));
- m_impl->idsBuf.emplace_back(command->GetID());
- }
- nt::NetworkTableEntry(namesEntry).SetStringArray(m_impl->commandsBuf);
- nt::NetworkTableEntry(idsEntry).SetDoubleArray(m_impl->idsBuf);
- }
- });
-}
-
-Scheduler::Scheduler() : m_impl(new Impl) {
- HAL_Report(HALUsageReporting::kResourceType_Command,
- HALUsageReporting::kCommand_Scheduler);
- SendableRegistry::GetInstance().AddLW(this, "Scheduler");
-}
-
-Scheduler::~Scheduler() {}
-
-void Scheduler::Impl::Remove(Command* command) {
- if (!commands.erase(command)) return;
-
- for (auto&& requirement : command->GetRequirements()) {
- requirement->SetCurrentCommand(nullptr);
- }
-
- command->Removed();
-}
-
-void Scheduler::Impl::ProcessCommandAddition(Command* command) {
- if (command == nullptr) return;
-
- // Only add if not already in
- auto found = commands.find(command);
- if (found == commands.end()) {
- // Check that the requirements can be had
- const auto& requirements = command->GetRequirements();
- for (const auto& requirement : requirements) {
- if (requirement->GetCurrentCommand() != nullptr &&
- !requirement->GetCurrentCommand()->IsInterruptible())
- return;
- }
-
- // Give it the requirements
- adding = true;
- for (auto&& requirement : requirements) {
- if (requirement->GetCurrentCommand() != nullptr) {
- requirement->GetCurrentCommand()->Cancel();
- Remove(requirement->GetCurrentCommand());
- }
- requirement->SetCurrentCommand(command);
- }
- adding = false;
-
- commands.insert(command);
-
- command->StartRunning();
- runningCommandsChanged = true;
- }
-}
diff --git a/wpilibc/src/main/native/cpp/commands/StartCommand.cpp b/wpilibc/src/main/native/cpp/commands/StartCommand.cpp
deleted file mode 100644
index 8884124..0000000
--- a/wpilibc/src/main/native/cpp/commands/StartCommand.cpp
+++ /dev/null
@@ -1,17 +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/commands/StartCommand.h"
-
-using namespace frc;
-
-StartCommand::StartCommand(Command* commandToStart)
- : InstantCommand("StartCommand") {
- m_commandToFork = commandToStart;
-}
-
-void StartCommand::Initialize() { m_commandToFork->Start(); }
diff --git a/wpilibc/src/main/native/cpp/commands/Subsystem.cpp b/wpilibc/src/main/native/cpp/commands/Subsystem.cpp
deleted file mode 100644
index 6e665ea..0000000
--- a/wpilibc/src/main/native/cpp/commands/Subsystem.cpp
+++ /dev/null
@@ -1,134 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/commands/Subsystem.h"
-
-#include "frc/WPIErrors.h"
-#include "frc/commands/Command.h"
-#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) {
- SendableRegistry::GetInstance().AddLW(this, name, name);
- Scheduler::GetInstance()->RegisterSubsystem(this);
-}
-
-void Subsystem::SetDefaultCommand(Command* command) {
- if (command == nullptr) {
- m_defaultCommand = nullptr;
- } else {
- const auto& reqs = command->GetRequirements();
- if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
- wpi_setWPIErrorWithContext(
- CommandIllegalUse, "A default command must require the subsystem");
- return;
- }
-
- m_defaultCommand = command;
- }
-}
-
-Command* Subsystem::GetDefaultCommand() {
- if (!m_initializedDefaultCommand) {
- m_initializedDefaultCommand = true;
- InitDefaultCommand();
- }
- return m_defaultCommand;
-}
-
-wpi::StringRef Subsystem::GetDefaultCommandName() {
- Command* defaultCommand = GetDefaultCommand();
- if (defaultCommand) {
- return SendableRegistry::GetInstance().GetName(defaultCommand);
- } else {
- return wpi::StringRef();
- }
-}
-
-void Subsystem::SetCurrentCommand(Command* command) {
- m_currentCommand = command;
- m_currentCommandChanged = true;
-}
-
-Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; }
-
-wpi::StringRef Subsystem::GetCurrentCommandName() const {
- Command* currentCommand = GetCurrentCommand();
- if (currentCommand) {
- return SendableRegistry::GetInstance().GetName(currentCommand);
- } else {
- return wpi::StringRef();
- }
-}
-
-void Subsystem::Periodic() {}
-
-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);
-}
-
-void Subsystem::AddChild(const wpi::Twine& name, Sendable* child) {
- AddChild(name, *child);
-}
-
-void Subsystem::AddChild(const wpi::Twine& name, Sendable& 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); }
-
-void Subsystem::AddChild(Sendable* child) { AddChild(*child); }
-
-void Subsystem::AddChild(Sendable& child) {
- auto& registry = SendableRegistry::GetInstance();
- registry.SetSubsystem(&child, registry.GetSubsystem(this));
- registry.EnableLiveWindow(&child);
- registry.AddChild(this, &child);
-}
-
-void Subsystem::ConfirmCommand() {
- if (m_currentCommandChanged) m_currentCommandChanged = false;
-}
-
-void Subsystem::InitSendable(SendableBuilder& builder) {
- builder.SetSmartDashboardType("Subsystem");
-
- builder.AddBooleanProperty(
- ".hasDefault", [=]() { return m_defaultCommand != nullptr; }, nullptr);
- builder.AddStringProperty(".default",
- [=]() { return GetDefaultCommandName(); }, nullptr);
-
- builder.AddBooleanProperty(
- ".hasCommand", [=]() { return m_currentCommand != nullptr; }, nullptr);
- builder.AddStringProperty(".command",
- [=]() { return GetCurrentCommandName(); }, nullptr);
-}
diff --git a/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp b/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp
deleted file mode 100644
index 122751a..0000000
--- a/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp
+++ /dev/null
@@ -1,24 +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/commands/TimedCommand.h"
-
-using namespace frc;
-
-TimedCommand::TimedCommand(const wpi::Twine& name, double timeout)
- : Command(name, timeout) {}
-
-TimedCommand::TimedCommand(double timeout) : Command(timeout) {}
-
-TimedCommand::TimedCommand(const wpi::Twine& name, double timeout,
- Subsystem& subsystem)
- : Command(name, timeout, subsystem) {}
-
-TimedCommand::TimedCommand(double timeout, Subsystem& subsystem)
- : Command(timeout, subsystem) {}
-
-bool TimedCommand::IsFinished() { return IsTimedOut(); }
diff --git a/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp b/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp
deleted file mode 100644
index 225d95b..0000000
--- a/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp
+++ /dev/null
@@ -1,16 +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/commands/WaitCommand.h"
-
-using namespace frc;
-
-WaitCommand::WaitCommand(double timeout)
- : TimedCommand("Wait(" + std::to_string(timeout) + ")", timeout) {}
-
-WaitCommand::WaitCommand(const wpi::Twine& name, double timeout)
- : TimedCommand(name, timeout) {}
diff --git a/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp b/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp
deleted file mode 100644
index 5c99c1b..0000000
--- a/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp
+++ /dev/null
@@ -1,22 +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/commands/WaitForChildren.h"
-
-#include "frc/commands/CommandGroup.h"
-
-using namespace frc;
-
-WaitForChildren::WaitForChildren(double timeout)
- : Command("WaitForChildren", timeout) {}
-
-WaitForChildren::WaitForChildren(const wpi::Twine& name, double timeout)
- : Command(name, timeout) {}
-
-bool WaitForChildren::IsFinished() {
- return GetGroup() == nullptr || GetGroup()->GetSize() == 0;
-}
diff --git a/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp b/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp
deleted file mode 100644
index 6e24b6b..0000000
--- a/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp
+++ /dev/null
@@ -1,24 +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/commands/WaitUntilCommand.h"
-
-#include "frc/Timer.h"
-
-using namespace frc;
-
-WaitUntilCommand::WaitUntilCommand(double time)
- : Command("WaitUntilCommand", time) {
- m_time = time;
-}
-
-WaitUntilCommand::WaitUntilCommand(const wpi::Twine& name, double time)
- : Command(name, time) {
- m_time = time;
-}
-
-bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }
diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
index 2202d93..264859f 100644
--- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableRegistry.h"
diff --git a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
deleted file mode 100644
index 4ef3bf9..0000000
--- a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index ee23a5e..2d30a2b 100644
--- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/SpeedController.h"
#include "frc/smartdashboard/SendableBuilder.h"
diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
index 4c15de8..e03951b 100644
--- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <wpi/math>
#include "frc/SpeedController.h"
diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index 473e033..0102d6a 100644
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -10,7 +10,7 @@
#include <algorithm>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <wpi/math>
#include "frc/SpeedController.h"
diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
index bae8868..fbce5a1 100644
--- a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
+++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
@@ -11,7 +11,7 @@
#include <cmath>
#include <cstddef>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/Base.h"
#include "frc/SpeedController.h"
diff --git a/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
index 7dfc8f0..434cc27 100644
--- a/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
+++ b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
@@ -10,7 +10,7 @@
#include <cassert>
#include <cmath>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
using namespace frc;
diff --git a/wpilibc/src/main/native/cpp/frc2/Timer.cpp b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
index 126cfdd..76721bc 100644
--- a/wpilibc/src/main/native/cpp/frc2/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
@@ -10,7 +10,7 @@
#include <chrono>
#include <thread>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include "frc/DriverStation.h"
#include "frc/RobotController.h"
@@ -129,7 +129,7 @@
units::second_t Timer::GetFPGATimestamp() {
// FPGA returns the timestamp in microseconds
- return units::second_t(frc::RobotController::GetFPGATime()) * 1.0e-6;
+ return units::second_t(frc::RobotController::GetFPGATime() * 1.0e-6);
}
units::second_t Timer::GetMatchTime() {
diff --git a/wpilibc/src/main/native/cpp/frc2/command/Command.cpp b/wpilibc/src/main/native/cpp/frc2/command/Command.cpp
deleted file mode 100644
index 692879c..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/Command.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index aeba26b..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index ba53397..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 464a4a6..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ /dev/null
@@ -1,346 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 78ae006..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 2344513..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 63c3179..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index b199074..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index c4eac81..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 4391e60..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp
+++ /dev/null
@@ -1,59 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index b81f6f6..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index d8a4159..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
+++ /dev/null
@@ -1,83 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index d967e67..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 8a02717..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index f29850b..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/ProfiledPIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
deleted file mode 100644
index c0cc19b..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 469e153..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 6f96315..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 0de739e..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp
+++ /dev/null
@@ -1,113 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 5c2c755..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index ea1ea8d..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 1aa19e4..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 33407bf..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index cccb55b..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 226f080..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index bb17edc..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 1279d66..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index d7a0daf..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index e519a9f..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
deleted file mode 100644
index 304bf98..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp
+++ /dev/null
@@ -1,119 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
index 42d20f7..1754830 100644
--- a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
@@ -9,6 +9,8 @@
#include <cmath>
+#include <wpi/json.h>
+
using namespace frc;
Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
@@ -96,3 +98,13 @@
return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
}
+
+void frc::to_json(wpi::json& json, const Pose2d& pose) {
+ json = wpi::json{{"translation", pose.Translation()},
+ {"rotation", pose.Rotation()}};
+}
+
+void frc::from_json(const wpi::json& json, Pose2d& pose) {
+ pose = Pose2d{json.at("translation").get<Translation2d>(),
+ json.at("rotation").get<Rotation2d>()};
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
index 2723442..a3c4bea 100644
--- a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -9,6 +9,8 @@
#include <cmath>
+#include <wpi/json.h>
+
using namespace frc;
Rotation2d::Rotation2d(units::radian_t value)
@@ -68,3 +70,11 @@
return {Cos() * other.Cos() - Sin() * other.Sin(),
Cos() * other.Sin() + Sin() * other.Cos()};
}
+
+void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
+ json = wpi::json{{"radians", rotation.Radians().to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
+ rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
index ca287d1..c3db7e3 100644
--- a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
@@ -7,6 +7,8 @@
#include "frc/geometry/Translation2d.h"
+#include <wpi/json.h>
+
using namespace frc;
Translation2d::Translation2d(units::meter_t x, units::meter_t y)
@@ -73,3 +75,13 @@
*this *= (1.0 / scalar);
return *this;
}
+
+void frc::to_json(wpi::json& json, const Translation2d& translation) {
+ json = wpi::json{{"x", translation.X().to<double>()},
+ {"y", translation.Y().to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Translation2d& translation) {
+ translation = Translation2d{units::meter_t{json.at("x").get<double>()},
+ units::meter_t{json.at("y").get<double>()}};
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp
deleted file mode 100644
index 8301481..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
index 418dd0f..8591da4 100644
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -10,23 +10,26 @@
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
- DifferentialDriveKinematics kinematics, const Pose2d& initialPose)
- : m_kinematics(kinematics), m_pose(initialPose) {
+ const Rotation2d& gyroAngle, const Pose2d& initialPose)
+ : m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
-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;
+const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
+ units::meter_t leftDistance,
+ units::meter_t rightDistance) {
+ auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
+ auto deltaRightDistance = rightDistance - m_prevRightDistance;
- auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
- static_cast<void>(dtheta);
+ m_prevLeftDistance = leftDistance;
+ m_prevRightDistance = rightDistance;
+
+ auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+ auto angle = gyroAngle + m_gyroOffset;
auto newPose = m_pose.Exp(
- {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+ {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 18e5635..615635a 100644
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -10,18 +10,22 @@
using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+ const Rotation2d& gyroAngle,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
const Pose2d& MecanumDriveOdometry::UpdateWithTime(
- units::second_t currentTime, const Rotation2d& angle,
+ units::second_t currentTime, const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
+ auto angle = gyroAngle + m_gyroOffset;
+
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
static_cast<void>(dtheta);
diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
index 1f011f5..10996e4 100644
--- a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -12,7 +12,7 @@
#include <networktables/NetworkTableInstance.h>
#include <wpi/mutex.h>
-#include "frc/commands/Scheduler.h"
+#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableBuilderImpl.h"
#include "frc/smartdashboard/SendableRegistry.h"
@@ -99,19 +99,15 @@
void LiveWindow::SetEnabled(bool enabled) {
std::scoped_lock lock(m_impl->mutex);
if (m_impl->liveWindowEnabled == enabled) return;
- Scheduler* scheduler = Scheduler::GetInstance();
m_impl->startLiveWindow = enabled;
m_impl->liveWindowEnabled = enabled;
// Force table generation now to make sure everything is defined
UpdateValuesUnsafe();
if (enabled) {
- scheduler->SetEnabled(false);
- scheduler->RemoveAll();
} else {
m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
cbdata.builder.StopLiveWindowMode();
});
- scheduler->SetEnabled(true);
}
m_impl->enabledEntry.SetBoolean(enabled);
}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
index a50b77f..9717a8e 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -7,7 +7,7 @@
#include "frc/shuffleboard/ShuffleboardInstance.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/StringMap.h>
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
index 56a27dc..bce6196 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
@@ -42,7 +42,7 @@
}
};
- wpi::mutex mutex;
+ wpi::recursive_mutex mutex;
wpi::UidVector<std::unique_ptr<Component>, 32> components;
wpi::DenseMap<void*, UID> componentMap;
@@ -310,7 +310,9 @@
wpi::function_ref<void(CallbackData& data)> callback) const {
assert(dataHandle >= 0);
std::scoped_lock lock(m_impl->mutex);
- for (auto&& comp : m_impl->components) {
+ wpi::SmallVector<Impl::Component*, 128> components;
+ for (auto&& comp : m_impl->components) components.emplace_back(comp.get());
+ for (auto comp : components) {
if (comp->sendable && comp->liveWindow) {
if (static_cast<size_t>(dataHandle) >= comp->data.size())
comp->data.resize(dataHandle + 1);
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
index e8c9700..5e70a4c 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -7,7 +7,7 @@
#include "frc/smartdashboard/SmartDashboard.h"
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/StringMap.h>
@@ -81,6 +81,10 @@
Singleton::GetInstance().table->Delete(key);
}
+nt::NetworkTableEntry SmartDashboard::GetEntry(wpi::StringRef key) {
+ return Singleton::GetInstance().table->GetEntry(key);
+}
+
void SmartDashboard::PutData(wpi::StringRef key, Sendable* data) {
if (data == nullptr) {
wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
diff --git a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
index 3578b1d..3991fec 100644
--- a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
+++ b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
@@ -27,10 +27,20 @@
// 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++) {
+ // Here, we are multiplying by (3 - i) to manually take the derivative. The
+ // power of the term in index 0 is 3, index 1 is 2 and so on. To find the
+ // coefficient of the derivative, we can use the power rule and multiply
+ // the existing coefficient by its power.
m_coefficients.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, i) * (3 - i);
+ }
+ for (int i = 0; i < 3; i++) {
+ // Here, we are multiplying by (2 - i) to manually take the derivative. The
+ // power of the term in index 0 is 2, index 1 is 1 and so on. To find the
+ // coefficient of the derivative, we can use the power rule and multiply
+ // the existing coefficient by its power.
m_coefficients.template block<2, 1>(4, i) =
- m_coefficients.template block<2, 1>(2, i) * (3 - i);
+ m_coefficients.template block<2, 1>(2, i) * (2 - i);
}
}
diff --git a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
index f714c6f..bb8ad3c 100644
--- a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
+++ b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
@@ -27,10 +27,19 @@
// 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++) {
+ // Here, we are multiplying by (5 - i) to manually take the derivative. The
+ // power of the term in index 0 is 5, index 1 is 4 and so on. To find the
+ // coefficient of the derivative, we can use the power rule and multiply
+ // the existing coefficient by its power.
m_coefficients.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, i) * (5 - i);
-
+ }
+ for (int i = 0; i < 5; i++) {
+ // Here, we are multiplying by (4 - i) to manually take the derivative. The
+ // power of the term in index 0 is 4, index 1 is 3 and so on. To find the
+ // coefficient of the derivative, we can use the power rule and multiply
+ // the existing coefficient by its power.
m_coefficients.template block<2, 1>(4, i) =
- m_coefficients.template block<2, 1>(2, i) * (5 - i);
+ m_coefficients.template block<2, 1>(2, i) * (4 - i);
}
}
diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
index 7c3fdc1..cbfc8c1 100644
--- a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
@@ -28,13 +28,25 @@
waypoints.emplace_back(
Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
+ // Populate tridiagonal system for clamped cubic
+ /* See:
+ https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
+ /undervisningsmateriale/chap7alecture.pdf
+ */
+
+ // Above-diagonal of tridiagonal matrix, zero-padded
std::vector<double> a;
+ // Diagonal of tridiagonal matrix
std::vector<double> b(waypoints.size() - 2, 4.0);
+ // Below-diagonal of tridiagonal matrix, zero-padded
std::vector<double> c;
+ // rhs vectors
std::vector<double> dx, dy;
+ // solution vectors
std::vector<double> fx(waypoints.size() - 2, 0.0),
fy(waypoints.size() - 2, 0.0);
+ // populate above-diagonal and below-diagonal vectors
a.emplace_back(0);
for (size_t i = 0; i < waypoints.size() - 3; ++i) {
a.emplace_back(1);
@@ -42,6 +54,7 @@
}
c.emplace_back(0);
+ // populate rhs vectors
dx.emplace_back(
3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
xInitial[1]);
@@ -63,6 +76,7 @@
waypoints[waypoints.size() - 3].Y().to<double>()) -
yFinal[1]);
+ // Compute solution to tridiagonal system
ThomasAlgorithm(a, b, c, dx, &fx);
ThomasAlgorithm(a, b, c, dy, &fy);
diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
index 9b4b4f5..7b2b2b6 100644
--- a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -7,8 +7,20 @@
#include "frc/trajectory/Trajectory.h"
+#include <wpi/json.h>
+
using namespace frc;
+bool Trajectory::State::operator==(const Trajectory::State& other) const {
+ return t == other.t && velocity == other.velocity &&
+ acceleration == other.acceleration && pose == other.pose &&
+ curvature == other.curvature;
+}
+
+bool Trajectory::State::operator!=(const Trajectory::State& other) const {
+ return !operator==(other);
+}
+
Trajectory::State Trajectory::State::Interpolate(State endValue,
double i) const {
// Find the new [t] value.
@@ -94,3 +106,21 @@
return prevSample.Interpolate(sample,
(t - prevSample.t) / (sample.t - prevSample.t));
}
+
+void frc::to_json(wpi::json& json, const Trajectory::State& state) {
+ json = wpi::json{{"time", state.t.to<double>()},
+ {"velocity", state.velocity.to<double>()},
+ {"acceleration", state.acceleration.to<double>()},
+ {"pose", state.pose},
+ {"curvature", state.curvature.to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Trajectory::State& state) {
+ state.pose = json.at("pose").get<Pose2d>();
+ state.t = units::second_t{json.at("time").get<double>()};
+ state.velocity =
+ units::meters_per_second_t{json.at("velocity").get<double>()};
+ state.acceleration =
+ units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
+ state.curvature = frc::curvature_t{json.at("curvature").get<double>()};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
new file mode 100644
index 0000000..f3cf30d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryUtil.h"
+
+#include <system_error>
+
+#include <wpi/SmallString.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory,
+ const wpi::Twine& path) {
+ std::error_code error_code;
+
+ wpi::SmallString<128> buf;
+ wpi::raw_fd_ostream output{path.toStringRef(buf), error_code};
+ if (error_code) {
+ throw std::runtime_error(("Cannot open file: " + path).str());
+ }
+
+ wpi::json json = trajectory.States();
+ output << json;
+ output.flush();
+}
+
+Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) {
+ std::error_code error_code;
+
+ wpi::SmallString<128> buf;
+ wpi::raw_fd_istream input{path.toStringRef(buf), error_code};
+ if (error_code) {
+ throw std::runtime_error(("Cannot open file: " + path).str());
+ }
+
+ wpi::json json;
+ input >> json;
+
+ return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}
+
+std::string TrajectoryUtil::SerializeTrajectory(const Trajectory& trajectory) {
+ wpi::json json = trajectory.States();
+ return json.dump();
+}
+
+Trajectory TrajectoryUtil::DeserializeTrajectory(
+ const wpi::StringRef json_str) {
+ wpi::json json = wpi::json::parse(json_str);
+ return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
new file mode 100644
index 0000000..3d6b61c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* 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/DifferentialDriveVoltageConstraint.h"
+
+#include <algorithm>
+#include <limits>
+
+#include <wpi/MathExtras.h>
+
+using namespace frc;
+
+DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
+ SimpleMotorFeedforward<units::meter> feedforward,
+ DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
+ : m_feedforward(feedforward),
+ m_kinematics(kinematics),
+ m_maxVoltage(maxVoltage) {}
+
+units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) {
+ return units::meters_per_second_t(std::numeric_limits<double>::max());
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveVoltageConstraint::MinMaxAcceleration(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) {
+ auto wheelSpeeds =
+ m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
+
+ auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right);
+ auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right);
+
+ // Calculate maximum/minimum possible accelerations from motor dynamics
+ // and max/min wheel speeds
+ auto maxWheelAcceleration =
+ m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+ auto minWheelAcceleration =
+ m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed);
+
+ // Robot chassis turning on radius = 1/|curvature|. Outer wheel has radius
+ // increased by half of the trackwidth T. Inner wheel has radius decreased
+ // by half of the trackwidth. Achassis / radius = Aouter / (radius + T/2), so
+ // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2)
+ // Inner wheel is similar.
+
+ // sgn(speed) term added to correctly account for which wheel is on
+ // outside of turn:
+ // If moving forward, max acceleration constraint corresponds to wheel on
+ // outside of turn
+ // If moving backward, max acceleration constraint corresponds to wheel on
+ // inside of turn
+ auto maxChassisAcceleration =
+ maxWheelAcceleration /
+ (1 + m_kinematics.trackWidth * units::math::abs(curvature) *
+ wpi::sgn(speed) / (2_rad));
+ auto minChassisAcceleration =
+ minWheelAcceleration /
+ (1 - m_kinematics.trackWidth * units::math::abs(curvature) *
+ wpi::sgn(speed) / (2_rad));
+
+ // Negate acceleration corresponding to wheel on inside of turn
+ // if center of turn is inside of wheelbase
+ if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
+ if (speed > 0_mps) {
+ minChassisAcceleration = -minChassisAcceleration;
+ } else {
+ maxChassisAcceleration = -maxChassisAcceleration;
+ }
+ }
+
+ return {minChassisAcceleration, maxChassisAcceleration};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
new file mode 100644
index 0000000..2fd8151
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.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/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
+
+using namespace frc;
+
+MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
+ MecanumDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+ : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) {
+ auto xVelocity = velocity * pose.Rotation().Cos();
+ auto yVelocity = velocity * pose.Rotation().Sin();
+ auto wheelSpeeds =
+ m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
+ wheelSpeeds.Normalize(m_maxSpeed);
+
+ auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+TrajectoryConstraint::MinMax
+MecanumDriveKinematicsConstraint::MinMaxAcceleration(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) {
+ return {};
+}
diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp
index 65c072d..5fd824d 100644
--- a/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -14,7 +14,8 @@
#include <cstdio>
#include <cameraserver/CameraServerShared.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
#include <networktables/NetworkTableInstance.h>
#include "WPILibVersion.h"
@@ -117,11 +118,6 @@
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();
diff --git a/wpilibc/src/main/native/include/frc/AddressableLED.h b/wpilibc/src/main/native/include/frc/AddressableLED.h
new file mode 100644
index 0000000..2ffe0ea
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/AddressableLED.h
@@ -0,0 +1,139 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/AddressableLEDTypes.h>
+#include <hal/Types.h>
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+
+#include "frc/ErrorBase.h"
+
+namespace frc {
+
+/**
+ * A class for driving addressable LEDs, such as WS2812s and NeoPixels.
+ */
+class AddressableLED : public ErrorBase {
+ public:
+ class LEDData : public HAL_AddressableLEDData {
+ public:
+ LEDData() : LEDData(0, 0, 0) {}
+ LEDData(int _r, int _g, int _b) {
+ r = _r;
+ g = _g;
+ b = _b;
+ padding = 0;
+ }
+
+ /**
+ * A helper method to set all values of the LED.
+ *
+ * @param r the r value [0-255]
+ * @param g the g value [0-255]
+ * @param b the b value [0-255]
+ */
+ void SetRGB(int r, int g, int b) {
+ this->r = r;
+ this->g = g;
+ this->b = b;
+ }
+
+ /**
+ * A helper method to set all values of the LED.
+ *
+ * @param h the h value [0-180]
+ * @param s the s value [0-255]
+ * @param v the v value [0-255]
+ */
+ void SetHSV(int h, int s, int v);
+ };
+
+ /**
+ * Constructs a new driver for a specific port.
+ *
+ * @param port the output port to use (Must be a PWM port)
+ */
+ explicit AddressableLED(int port);
+
+ ~AddressableLED() override;
+
+ /**
+ * Sets the length of the LED strip.
+ *
+ * <p>Calling this is an expensive call, so its best to call it once, then
+ * just update data.
+ *
+ * @param length the strip length
+ */
+ void SetLength(int length);
+
+ /**
+ * Sets the led output data.
+ *
+ * <p>If the output is enabled, this will start writing the next data cycle.
+ * It is safe to call, even while output is enabled.
+ *
+ * @param ledData the buffer to write
+ */
+ void SetData(wpi::ArrayRef<LEDData> ledData);
+
+ /**
+ * Sets the led output data.
+ *
+ * <p>If the output is enabled, this will start writing the next data cycle.
+ * It is safe to call, even while output is enabled.
+ *
+ * @param ledData the buffer to write
+ */
+ void SetData(std::initializer_list<LEDData> ledData);
+
+ /**
+ * Sets the bit timing.
+ *
+ * <p>By default, the driver is set up to drive WS2812s, so nothing needs to
+ * be set for those.
+ *
+ * @param lowTime0 low time for 0 bit
+ * @param highTime0 high time for 0 bit
+ * @param lowTime1 low time for 1 bit
+ * @param highTime1 high time for 1 bit
+ */
+ void SetBitTiming(units::nanosecond_t lowTime0, units::nanosecond_t highTime0,
+ units::nanosecond_t lowTime1,
+ units::nanosecond_t highTime1);
+
+ /**
+ * Sets the sync time.
+ *
+ * <p>The sync time is the time to hold output so LEDs enable. Default set for
+ * WS2812.
+ *
+ * @param syncTimeMicroSeconds the sync time
+ */
+ void SetSyncTime(units::microsecond_t syncTime);
+
+ /**
+ * Starts the output.
+ *
+ * <p>The output writes continously.
+ */
+ void Start();
+
+ /**
+ * Stops the output.
+ */
+ void Stop();
+
+ private:
+ hal::Handle<HAL_DigitalHandle> m_pwmHandle;
+ hal::Handle<HAL_AddressableLEDHandle> m_handle;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
new file mode 100644
index 0000000..872db87
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/SimDevice.h>
+#include <hal/Types.h>
+#include <units/units.h>
+
+#include "frc/AnalogTrigger.h"
+#include "frc/Counter.h"
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+class AnalogInput;
+
+/**
+ * Class for supporting continuous analog encoders, such as the US Digital MA3.
+ */
+class AnalogEncoder : public ErrorBase,
+ public Sendable,
+ public SendableHelper<AnalogEncoder> {
+ public:
+ /**
+ * Construct a new AnalogEncoder attached to a specific AnalogInput.
+ *
+ * @param analogInput the analog input to attach to
+ */
+ explicit AnalogEncoder(AnalogInput& analogInput);
+
+ /**
+ * Construct a new AnalogEncoder attached to a specific AnalogInput.
+ *
+ * @param analogInput the analog input to attach to
+ */
+ explicit AnalogEncoder(AnalogInput* analogInput);
+
+ /**
+ * Construct a new AnalogEncoder attached to a specific AnalogInput.
+ *
+ * @param analogInput the analog input to attach to
+ */
+ explicit AnalogEncoder(std::shared_ptr<AnalogInput> analogInput);
+
+ ~AnalogEncoder() override = default;
+
+ AnalogEncoder(AnalogEncoder&&) = default;
+ AnalogEncoder& operator=(AnalogEncoder&&) = default;
+
+ /**
+ * Reset the Encoder distance to zero.
+ */
+ void Reset();
+
+ /**
+ * Get the encoder value since the last reset.
+ *
+ * This is reported in rotations since the last reset.
+ *
+ * @return the encoder value in rotations
+ */
+ units::turn_t Get() const;
+
+ /**
+ * Get the offset of position relative to the last reset.
+ *
+ * GetPositionInRotation() - GetPositionOffset() will give an encoder absolute
+ * position relative to the last reset. This could potentially be negative,
+ * which needs to be accounted for.
+ *
+ * @return the position offset
+ */
+ double GetPositionOffset() const;
+
+ /**
+ * Set the distance per rotation of the encoder. This sets the multiplier used
+ * to determine the distance driven based on the rotation value from the
+ * encoder. Set this value based on the how far the mechanism travels in 1
+ * rotation of the encoder, and factor in gearing reductions following the
+ * encoder shaft. This distance can be in any units you like, linear or
+ * angular.
+ *
+ * @param distancePerRotation the distance per rotation of the encoder
+ */
+ void SetDistancePerRotation(double distancePerRotation);
+
+ /**
+ * Get the distance per rotation for this encoder.
+ *
+ * @return The scale factor that will be used to convert rotation to useful
+ * units.
+ */
+ double GetDistancePerRotation() const;
+
+ /**
+ * Get the distance the sensor has driven since the last reset as scaled by
+ * the value from SetDistancePerRotation.
+ *
+ * @return The distance driven since the last reset
+ */
+ double GetDistance() const;
+
+ void InitSendable(SendableBuilder& builder) override;
+
+ private:
+ void Init();
+
+ std::shared_ptr<AnalogInput> m_analogInput;
+ AnalogTrigger m_analogTrigger;
+ Counter m_counter;
+ double m_positionOffset = 0;
+ double m_distancePerRotation = 1.0;
+ mutable units::turn_t m_lastPosition{0.0};
+
+ hal::SimDevice m_simDevice;
+ hal::SimDouble m_simPosition;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/AnalogInput.h b/wpilibc/src/main/native/include/frc/AnalogInput.h
index 6567542..3d14c4c 100644
--- a/wpilibc/src/main/native/include/frc/AnalogInput.h
+++ b/wpilibc/src/main/native/include/frc/AnalogInput.h
@@ -19,6 +19,8 @@
namespace frc {
class SendableBuilder;
+class DMA;
+class DMASample;
/**
* Analog input class.
@@ -38,6 +40,8 @@
public SendableHelper<AnalogInput> {
friend class AnalogTrigger;
friend class AnalogGyro;
+ friend class DMA;
+ friend class DMASample;
public:
static constexpr int kAccumulatorModuleNumber = 1;
diff --git a/wpilibc/src/main/native/include/frc/AnalogTrigger.h b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
index 6a57f8a..7554bd9 100644
--- a/wpilibc/src/main/native/include/frc/AnalogTrigger.h
+++ b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
@@ -19,6 +19,7 @@
namespace frc {
class AnalogInput;
+class DutyCycle;
class SendableBuilder;
class AnalogTrigger : public ErrorBase,
@@ -45,6 +46,13 @@
*/
explicit AnalogTrigger(AnalogInput* channel);
+ /**
+ * Construct an analog trigger given a duty cycle input.
+ *
+ * @param channel The pointer to the existing DutyCycle object
+ */
+ explicit AnalogTrigger(DutyCycle* dutyCycle);
+
~AnalogTrigger() override;
AnalogTrigger(AnalogTrigger&& rhs);
@@ -61,6 +69,16 @@
void SetLimitsVoltage(double lower, double upper);
/**
+ * Set the upper and lower duty cycle limits of the analog trigger.
+ *
+ * The limits are given as floating point values between 0 and 1.
+ *
+ * @param lower The lower limit of the trigger in percentage.
+ * @param upper The upper limit of the trigger in percentage.
+ */
+ void SetLimitsDutyCycle(double lower, double upper);
+
+ /**
* Set the upper and lower limits of the analog trigger.
*
* The limits are given in ADC codes. If oversampling is used, the units must
@@ -83,6 +101,17 @@
void SetAveraged(bool useAveragedValue);
/**
+ * Configure the analog trigger to use the duty cycle vs. raw values.
+ *
+ * If the value is true, then the duty cycle value is selected for the analog
+ * trigger, otherwise the immediate value is used.
+ *
+ * @param useDutyCycle If true, use the duty cycle value, otherwise use the
+ * instantaneous reading
+ */
+ void SetDutyCycle(bool useDutyCycle);
+
+ /**
* Configure the analog trigger to use a filtered value.
*
* The analog trigger will operate with a 3 point average rejection filter.
@@ -139,9 +168,9 @@
void InitSendable(SendableBuilder& builder) override;
private:
- int m_index;
hal::Handle<HAL_AnalogTriggerHandle> m_trigger;
AnalogInput* m_analogInput = nullptr;
+ DutyCycle* m_dutyCycle = nullptr;
bool m_ownsAnalog = false;
};
diff --git a/wpilibc/src/main/native/include/frc/Counter.h b/wpilibc/src/main/native/include/frc/Counter.h
index 50e3948..2770a02 100644
--- a/wpilibc/src/main/native/include/frc/Counter.h
+++ b/wpilibc/src/main/native/include/frc/Counter.h
@@ -21,6 +21,8 @@
class DigitalGlitchFilter;
class SendableBuilder;
+class DMA;
+class DMASample;
/**
* Class for counting the number of ticks on a digital input channel.
@@ -36,6 +38,9 @@
public CounterBase,
public Sendable,
public SendableHelper<Counter> {
+ friend class DMA;
+ friend class DMASample;
+
public:
enum Mode {
kTwoPulse = 0,
diff --git a/wpilibc/src/main/native/include/frc/DMA.h b/wpilibc/src/main/native/include/frc/DMA.h
new file mode 100644
index 0000000..57cdd27
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DMA.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <hal/Types.h>
+
+#include "frc/ErrorBase.h"
+
+namespace frc {
+class Encoder;
+class Counter;
+class DigitalSource;
+class DutyCycle;
+class AnalogInput;
+class DMASample;
+
+class DMA : public ErrorBase {
+ friend class DMASample;
+
+ public:
+ DMA();
+ ~DMA() override;
+
+ DMA& operator=(DMA&& other) = default;
+ DMA(DMA&& other) = default;
+
+ void SetPause(bool pause);
+ void SetRate(int cycles);
+
+ void AddEncoder(const Encoder* encoder);
+ void AddEncoderPeriod(const Encoder* encoder);
+
+ void AddCounter(const Counter* counter);
+ void AddCounterPeriod(const Counter* counter);
+
+ void AddDigitalSource(const DigitalSource* digitalSource);
+
+ void AddDutyCycle(const DutyCycle* digitalSource);
+
+ void AddAnalogInput(const AnalogInput* analogInput);
+ void AddAveragedAnalogInput(const AnalogInput* analogInput);
+ void AddAnalogAccumulator(const AnalogInput* analogInput);
+
+ void SetExternalTrigger(DigitalSource* source, bool rising, bool falling);
+
+ void StartDMA(int queueDepth);
+ void StopDMA();
+
+ private:
+ hal::Handle<HAL_DMAHandle> dmaHandle;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DMASample.h b/wpilibc/src/main/native/include/frc/DMASample.h
new file mode 100644
index 0000000..4592930
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DMASample.h
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <type_traits>
+
+#include <hal/AnalogInput.h>
+#include <hal/DMA.h>
+#include <units/units.h>
+
+#include "frc/AnalogInput.h"
+#include "frc/Counter.h"
+#include "frc/DMA.h"
+#include "frc/DutyCycle.h"
+#include "frc/Encoder.h"
+
+namespace frc {
+class DMASample : public HAL_DMASample {
+ public:
+ HAL_DMAReadStatus Update(const DMA* dma, units::second_t timeout,
+ int32_t* remaining, int32_t* status) {
+ units::millisecond_t ms = timeout;
+ auto timeoutMs = ms.to<int32_t>();
+ return HAL_ReadDMA(dma->dmaHandle, this, timeoutMs, remaining, status);
+ }
+
+ uint64_t GetTime() const { return timeStamp; }
+
+ units::second_t GetTimeStamp() const {
+ return units::second_t{static_cast<double>(GetTime()) * 1.0e-6};
+ }
+
+ int32_t GetEncoderRaw(const Encoder* encoder, int32_t* status) const {
+ return HAL_GetDMASampleEncoderRaw(this, encoder->m_encoder, status);
+ }
+
+ double GetEncoderDistance(const Encoder* encoder, int32_t* status) const {
+ double val = GetEncoderRaw(encoder, status);
+ val *= encoder->DecodingScaleFactor();
+ val *= encoder->GetDistancePerPulse();
+ return val;
+ }
+
+ int32_t GetEncoderPeriodRaw(const Encoder* encoder, int32_t* status) const {
+ return HAL_GetDMASampleEncoderPeriodRaw(this, encoder->m_encoder, status);
+ }
+
+ int32_t GetCounter(const Counter* counter, int32_t* status) const {
+ return HAL_GetDMASampleCounter(this, counter->m_counter, status);
+ }
+
+ int32_t GetCounterPeriod(const Counter* counter, int32_t* status) const {
+ return HAL_GetDMASampleCounterPeriod(this, counter->m_counter, status);
+ }
+
+ bool GetDigitalSource(const DigitalSource* digitalSource,
+ int32_t* status) const {
+ return HAL_GetDMASampleDigitalSource(
+ this, digitalSource->GetPortHandleForRouting(), status);
+ }
+
+ int32_t GetAnalogInputRaw(const AnalogInput* analogInput,
+ int32_t* status) const {
+ return HAL_GetDMASampleAnalogInputRaw(this, analogInput->m_port, status);
+ }
+
+ double GetAnalogInputVoltage(const AnalogInput* analogInput,
+ int32_t* status) {
+ return HAL_GetAnalogValueToVolts(
+ analogInput->m_port, GetAnalogInputRaw(analogInput, status), status);
+ }
+
+ int32_t GetAveragedAnalogInputRaw(const AnalogInput* analogInput,
+ int32_t* status) const {
+ return HAL_GetDMASampleAveragedAnalogInputRaw(this, analogInput->m_port,
+ status);
+ }
+
+ double GetAveragedAnalogInputVoltage(const AnalogInput* analogInput,
+ int32_t* status) {
+ return HAL_GetAnalogValueToVolts(
+ analogInput->m_port, GetAveragedAnalogInputRaw(analogInput, status),
+ status);
+ }
+
+ void GetAnalogAccumulator(const AnalogInput* analogInput, int64_t* count,
+ int64_t* value, int32_t* status) const {
+ return HAL_GetDMASampleAnalogAccumulator(this, analogInput->m_port, count,
+ value, status);
+ }
+
+ int32_t GetDutyCycleOutputRaw(const DutyCycle* dutyCycle,
+ int32_t* status) const {
+ return HAL_GetDMASampleDutyCycleOutputRaw(this, dutyCycle->m_handle,
+ status);
+ }
+
+ double GetDutyCycleOutput(const DutyCycle* dutyCycle, int32_t* status) {
+ return GetDutyCycleOutputRaw(dutyCycle, status) /
+ static_cast<double>(dutyCycle->GetOutputScaleFactor());
+ }
+};
+
+static_assert(std::is_standard_layout_v<frc::DMASample>,
+ "frc::DMASample must have standard layout");
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DMC60.h b/wpilibc/src/main/native/include/frc/DMC60.h
index 5a33b5a..ecf01e1 100644
--- a/wpilibc/src/main/native/include/frc/DMC60.h
+++ b/wpilibc/src/main/native/include/frc/DMC60.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,6 +13,19 @@
/**
* Digilent DMC 60 Speed Controller.
+ *
+ * Note that the DMC 60 uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the DMC 60 User
+ * Manual available from Digilent.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
*/
class DMC60 : public PWMSpeedController {
public:
diff --git a/wpilibc/src/main/native/include/frc/DigitalOutput.h b/wpilibc/src/main/native/include/frc/DigitalOutput.h
index 45727a4..1d1d152 100644
--- a/wpilibc/src/main/native/include/frc/DigitalOutput.h
+++ b/wpilibc/src/main/native/include/frc/DigitalOutput.h
@@ -9,7 +9,7 @@
#include <hal/Types.h>
-#include "frc/ErrorBase.h"
+#include "frc/DigitalSource.h"
#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableHelper.h"
@@ -24,7 +24,7 @@
* elsewhere will allocate channels automatically so for those devices it
* shouldn't be done here.
*/
-class DigitalOutput : public ErrorBase,
+class DigitalOutput : public DigitalSource,
public Sendable,
public SendableHelper<DigitalOutput> {
public:
@@ -59,10 +59,26 @@
*/
bool Get() const;
+ // Digital Source Interface
+ /**
+ * @return The HAL Handle to the specified source.
+ */
+ HAL_Handle GetPortHandleForRouting() const override;
+
+ /**
+ * @return The type of analog trigger output to be used. 0 for Digitals
+ */
+ AnalogTriggerType GetAnalogTriggerTypeForRouting() const override;
+
+ /**
+ * Is source an AnalogTrigger
+ */
+ bool IsAnalogTrigger() const override;
+
/**
* @return The GPIO channel number that this object represents.
*/
- int GetChannel() const;
+ int GetChannel() const override;
/**
* Output a single pulse on the digital output line.
diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h
index 8abffde..4508701 100644
--- a/wpilibc/src/main/native/include/frc/DriverStation.h
+++ b/wpilibc/src/main/native/include/frc/DriverStation.h
@@ -394,6 +394,11 @@
*/
void InTest(bool entering) { m_userInTest = entering; }
+ /**
+ * Forces WaitForData() to return immediately.
+ */
+ void WakeupWaitForData();
+
protected:
/**
* Copy data from the DS task for the user.
diff --git a/wpilibc/src/main/native/include/frc/DutyCycle.h b/wpilibc/src/main/native/include/frc/DutyCycle.h
new file mode 100644
index 0000000..f0fc2d8
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DutyCycle.h
@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+class DigitalSource;
+class AnalogTrigger;
+class DMA;
+class DMASample;
+
+/**
+ * Class to read a duty cycle PWM input.
+ *
+ * <p>PWM input signals are specified with a frequency and a ratio of high to
+ * low in that frequency. There are 8 of these in the roboRIO, and they can be
+ * attached to any DigitalSource.
+ *
+ * <p>These can be combined as the input of an AnalogTrigger to a Counter in
+ * order to implement rollover checking.
+ *
+ */
+class DutyCycle : public ErrorBase,
+ public Sendable,
+ public SendableHelper<DutyCycle> {
+ friend class AnalogTrigger;
+ friend class DMA;
+ friend class DMASample;
+
+ public:
+ /**
+ * Constructs a DutyCycle input from a DigitalSource input.
+ *
+ * <p> This class does not own the inputted source.
+ *
+ * @param source The DigitalSource to use.
+ */
+ explicit DutyCycle(DigitalSource& source);
+ /**
+ * Constructs a DutyCycle input from a DigitalSource input.
+ *
+ * <p> This class does not own the inputted source.
+ *
+ * @param source The DigitalSource to use.
+ */
+ explicit DutyCycle(DigitalSource* source);
+ /**
+ * Constructs a DutyCycle input from a DigitalSource input.
+ *
+ * <p> This class does not own the inputted source.
+ *
+ * @param source The DigitalSource to use.
+ */
+ explicit DutyCycle(std::shared_ptr<DigitalSource> source);
+
+ /**
+ * Close the DutyCycle and free all resources.
+ */
+ ~DutyCycle() override;
+
+ DutyCycle(DutyCycle&&) = default;
+ DutyCycle& operator=(DutyCycle&&) = default;
+
+ /**
+ * Get the frequency of the duty cycle signal.
+ *
+ * @return frequency in Hertz
+ */
+ int GetFrequency() const;
+
+ /**
+ * Get the output ratio of the duty cycle signal.
+ *
+ * <p> 0 means always low, 1 means always high.
+ *
+ * @return output ratio between 0 and 1
+ */
+ double GetOutput() const;
+
+ /**
+ * Get the raw output ratio of the duty cycle signal.
+ *
+ * <p> 0 means always low, an output equal to
+ * GetOutputScaleFactor() means always high.
+ *
+ * @return output ratio in raw units
+ */
+ unsigned int GetOutputRaw() const;
+
+ /**
+ * Get the scale factor of the output.
+ *
+ * <p> An output equal to this value is always high, and then linearly scales
+ * down to 0. Divide the result of getOutputRaw by this in order to get the
+ * percentage between 0 and 1.
+ *
+ * @return the output scale factor
+ */
+ unsigned int GetOutputScaleFactor() const;
+
+ /**
+ * Get the FPGA index for the DutyCycle.
+ *
+ * @return the FPGA index
+ */
+ int GetFPGAIndex() const;
+
+ /**
+ * Get the channel of the source.
+ *
+ * @return the source channel
+ */
+ int GetSourceChannel() const;
+
+ protected:
+ void InitSendable(SendableBuilder& builder) override;
+
+ private:
+ void InitDutyCycle();
+ std::shared_ptr<DigitalSource> m_source;
+ hal::Handle<HAL_DutyCycleHandle> m_handle;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
new file mode 100644
index 0000000..92864a8
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
@@ -0,0 +1,174 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/SimDevice.h>
+#include <hal/Types.h>
+#include <units/units.h>
+
+#include "frc/AnalogTrigger.h"
+#include "frc/Counter.h"
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+class DutyCycle;
+class DigitalSource;
+
+/**
+ * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with
+ * PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
+ * Encoder.
+ */
+class DutyCycleEncoder : public ErrorBase,
+ public Sendable,
+ public SendableHelper<DutyCycleEncoder> {
+ public:
+ /**
+ * Construct a new DutyCycleEncoder on a specific channel.
+ *
+ * @param channel the channel to attach to
+ */
+ explicit DutyCycleEncoder(int channel);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+ *
+ * @param dutyCycle the duty cycle to attach to
+ */
+ explicit DutyCycleEncoder(DutyCycle& dutyCycle);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+ *
+ * @param dutyCycle the duty cycle to attach to
+ */
+ explicit DutyCycleEncoder(DutyCycle* dutyCycle);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+ *
+ * @param dutyCycle the duty cycle to attach to
+ */
+ explicit DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+ *
+ * @param source the digital source to attach to
+ */
+ explicit DutyCycleEncoder(DigitalSource& digitalSource);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+ *
+ * @param source the digital source to attach to
+ */
+ explicit DutyCycleEncoder(DigitalSource* digitalSource);
+
+ /**
+ * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+ *
+ * @param source the digital source to attach to
+ */
+ explicit DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource);
+
+ ~DutyCycleEncoder() override = default;
+
+ DutyCycleEncoder(DutyCycleEncoder&&) = default;
+ DutyCycleEncoder& operator=(DutyCycleEncoder&&) = default;
+
+ /**
+ * Get the frequency in Hz of the duty cycle signal from the encoder.
+ *
+ * @return duty cycle frequency in Hz
+ */
+ int GetFrequency() const;
+
+ /**
+ * Get if the sensor is connected
+ *
+ * This uses the duty cycle frequency to determine if the sensor is connected.
+ * By default, a value of 100 Hz is used as the threshold, and this value can
+ * be changed with SetConnectedFrequencyThreshold.
+ *
+ * @return true if the sensor is connected
+ */
+ bool IsConnected() const;
+
+ /**
+ * Change the frequency threshold for detecting connection used by
+ * IsConnected.
+ *
+ * @param frequency the minimum frequency in Hz.
+ */
+ void SetConnectedFrequencyThreshold(int frequency);
+
+ /**
+ * Reset the Encoder distance to zero.
+ */
+ void Reset();
+
+ /**
+ * Get the encoder value since the last reset.
+ *
+ * This is reported in rotations since the last reset.
+ *
+ * @return the encoder value in rotations
+ */
+ units::turn_t Get() const;
+
+ /**
+ * Set the distance per rotation of the encoder. This sets the multiplier used
+ * to determine the distance driven based on the rotation value from the
+ * encoder. Set this value based on the how far the mechanism travels in 1
+ * rotation of the encoder, and factor in gearing reductions following the
+ * encoder shaft. This distance can be in any units you like, linear or
+ * angular.
+ *
+ * @param distancePerRotation the distance per rotation of the encoder
+ */
+ void SetDistancePerRotation(double distancePerRotation);
+
+ /**
+ * Get the distance per rotation for this encoder.
+ *
+ * @return The scale factor that will be used to convert rotation to useful
+ * units.
+ */
+ double GetDistancePerRotation() const;
+
+ /**
+ * Get the distance the sensor has driven since the last reset as scaled by
+ * the value from SetDistancePerRotation.
+ *
+ * @return The distance driven since the last reset
+ */
+ double GetDistance() const;
+
+ void InitSendable(SendableBuilder& builder) override;
+
+ private:
+ void Init();
+
+ std::shared_ptr<DutyCycle> m_dutyCycle;
+ AnalogTrigger m_analogTrigger;
+ Counter m_counter;
+ int m_frequencyThreshold = 100;
+ double m_positionOffset = 0;
+ double m_distancePerRotation = 1.0;
+ mutable units::turn_t m_lastPosition{0.0};
+
+ hal::SimDevice m_simDevice;
+ hal::SimDouble m_simPosition;
+ hal::SimBoolean m_simIsConnected;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Encoder.h b/wpilibc/src/main/native/include/frc/Encoder.h
index 074cc5e..60552f6 100644
--- a/wpilibc/src/main/native/include/frc/Encoder.h
+++ b/wpilibc/src/main/native/include/frc/Encoder.h
@@ -23,6 +23,8 @@
class DigitalSource;
class DigitalGlitchFilter;
class SendableBuilder;
+class DMA;
+class DMASample;
/**
* Class to read quad encoders.
@@ -44,6 +46,9 @@
public PIDSource,
public Sendable,
public SendableHelper<Encoder> {
+ friend class DMA;
+ friend class DMASample;
+
public:
enum IndexingType {
kResetWhileHigh,
diff --git a/wpilibc/src/main/native/include/frc/ErrorBase.h b/wpilibc/src/main/native/include/frc/ErrorBase.h
index 7235352..e9168aa 100644
--- a/wpilibc/src/main/native/include/frc/ErrorBase.h
+++ b/wpilibc/src/main/native/include/frc/ErrorBase.h
@@ -16,6 +16,9 @@
#include "frc/Base.h"
#include "frc/Error.h"
+// Forward declared manually to avoid needing to pull in entire HAL header.
+extern "C" const char* HAL_GetErrorMessage(int32_t code);
+
#define wpi_setErrnoErrorWithContext(context) \
this->SetErrnoError((context), __FILE__, __FUNCTION__, __LINE__)
#define wpi_setErrnoError() wpi_setErrnoErrorWithContext("")
@@ -35,6 +38,22 @@
this->SetErrorRange((code), (min), (max), (req), (context), __FILE__, \
__FUNCTION__, __LINE__); \
} while (0)
+
+#define wpi_setHALError(code) \
+ do { \
+ if ((code) != 0) \
+ this->SetError((code), HAL_GetErrorMessage(code), __FILE__, \
+ __FUNCTION__, __LINE__); \
+ } while (0)
+
+#define wpi_setHALErrorWithRange(code, min, max, req) \
+ do { \
+ if ((code) != 0) \
+ this->SetErrorRange((code), (min), (max), (req), \
+ HAL_GetErrorMessage(code), __FILE__, __FUNCTION__, \
+ __LINE__); \
+ } while (0)
+
#define wpi_setError(code) wpi_setErrorWithContext(code, "")
#define wpi_setStaticErrorWithContext(object, code, context) \
do { \
@@ -43,12 +62,21 @@
} while (0)
#define wpi_setStaticError(object, code) \
wpi_setStaticErrorWithContext(object, code, "")
+
#define wpi_setGlobalErrorWithContext(code, context) \
do { \
if ((code) != 0) \
::frc::ErrorBase::SetGlobalError((code), (context), __FILE__, \
__FUNCTION__, __LINE__); \
} while (0)
+
+#define wpi_setGlobalHALError(code) \
+ do { \
+ if ((code) != 0) \
+ ::frc::ErrorBase::SetGlobalError((code), HAL_GetErrorMessage(code), \
+ __FILE__, __FUNCTION__, __LINE__); \
+ } while (0)
+
#define wpi_setGlobalError(code) wpi_setGlobalErrorWithContext(code, "")
#define wpi_setWPIErrorWithContext(error, context) \
this->SetWPIError(wpi_error_s_##error(), wpi_error_value_##error(), \
diff --git a/wpilibc/src/main/native/include/frc/IterativeRobot.h b/wpilibc/src/main/native/include/frc/IterativeRobot.h
index 8ce6356..447c477 100644
--- a/wpilibc/src/main/native/include/frc/IterativeRobot.h
+++ b/wpilibc/src/main/native/include/frc/IterativeRobot.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,8 @@
#pragma once
+#include <atomic>
+
#include "frc/IterativeRobotBase.h"
namespace frc {
@@ -28,9 +30,6 @@
IterativeRobot();
virtual ~IterativeRobot() = default;
- IterativeRobot(IterativeRobot&&) = default;
- IterativeRobot& operator=(IterativeRobot&&) = default;
-
/**
* Provide an alternate "main loop" via StartCompetition().
*
@@ -38,6 +37,14 @@
* with the DS packets.
*/
void StartCompetition() override;
+
+ /**
+ * Ends the main loop in StartCompetition().
+ */
+ void EndCompetition() override;
+
+ private:
+ std::atomic<bool> m_exit{false};
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Jaguar.h b/wpilibc/src/main/native/include/frc/Jaguar.h
index f22eb42..7a8503e 100644
--- a/wpilibc/src/main/native/include/frc/Jaguar.h
+++ b/wpilibc/src/main/native/include/frc/Jaguar.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,6 +13,19 @@
/**
* Luminary Micro / Vex Robotics Jaguar Speed Controller with PWM control.
+ *
+ * Note that the Jaguar uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended. The
+ * calibration procedure can be found in the Jaguar User Manual available from
+ * Vex.
+ *
+ * \li 2.310ms = full "forward"
+ * \li 1.550ms = the "high end" of the deadband range
+ * \li 1.507ms = center of the deadband range (off)
+ * \li 1.454ms = the "low end" of the deadband range
+ * \li 0.697ms = full "reverse"
*/
class Jaguar : public PWMSpeedController {
public:
diff --git a/wpilibc/src/main/native/include/frc/LinearFilter.h b/wpilibc/src/main/native/include/frc/LinearFilter.h
index de52003..2b83a32 100644
--- a/wpilibc/src/main/native/include/frc/LinearFilter.h
+++ b/wpilibc/src/main/native/include/frc/LinearFilter.h
@@ -7,9 +7,12 @@
#pragma once
+#include <cassert>
+#include <cmath>
#include <initializer_list>
#include <vector>
+#include <hal/FRCUsageReporting.h>
#include <units/units.h>
#include <wpi/ArrayRef.h>
#include <wpi/circular_buffer.h>
@@ -66,6 +69,7 @@
* Combining this with Note 1 - the impetus is on YOU as a developer to make
* sure Calculate() gets called at the desired, constant frequency!
*/
+template <class T>
class LinearFilter {
public:
/**
@@ -74,7 +78,15 @@
* @param ffGains The "feed forward" or FIR gains.
* @param fbGains The "feed back" or IIR gains.
*/
- LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains);
+ 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);
+ }
/**
* Create a linear FIR or IIR filter.
@@ -99,8 +111,11 @@
* @param period The period in seconds between samples taken by the
* user.
*/
- static LinearFilter SinglePoleIIR(double timeConstant,
- units::second_t period);
+ static LinearFilter<T> SinglePoleIIR(double timeConstant,
+ units::second_t period) {
+ double gain = std::exp(-period.to<double>() / timeConstant);
+ return LinearFilter(1.0 - gain, -gain);
+ }
/**
* Creates a first-order high-pass filter of the form:<br>
@@ -113,7 +128,10 @@
* @param period The period in seconds between samples taken by the
* user.
*/
- static LinearFilter HighPass(double timeConstant, units::second_t period);
+ static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
+ double gain = std::exp(-period.to<double>() / timeConstant);
+ return LinearFilter({gain, -gain}, {-gain});
+ }
/**
* Creates a K-tap FIR moving average filter of the form:<br>
@@ -124,12 +142,20 @@
* @param taps The number of samples to average over. Higher = smoother but
* slower
*/
- static LinearFilter MovingAverage(int taps);
+ static LinearFilter<T> MovingAverage(int taps) {
+ assert(taps > 0);
+
+ std::vector<double> gains(taps, 1.0 / taps);
+ return LinearFilter(gains, {});
+ }
/**
* Reset the filter state.
*/
- void Reset();
+ void Reset() {
+ m_inputs.reset();
+ m_outputs.reset();
+ }
/**
* Calculates the next value of the filter.
@@ -138,11 +164,29 @@
*
* @return The filtered value at this step
*/
- double Calculate(double input);
+ T Calculate(T input) {
+ T retVal = T(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;
+ }
private:
- wpi::circular_buffer<double> m_inputs{0};
- wpi::circular_buffer<double> m_outputs{0};
+ wpi::circular_buffer<T> m_inputs;
+ wpi::circular_buffer<T> m_outputs;
std::vector<double> m_inputGains;
std::vector<double> m_outputGains;
};
diff --git a/wpilibc/src/main/native/include/frc/MedianFilter.h b/wpilibc/src/main/native/include/frc/MedianFilter.h
new file mode 100644
index 0000000..b5d499b
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/MedianFilter.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+#include <vector>
+
+#include <wpi/Algorithm.h>
+#include <wpi/circular_buffer.h>
+
+namespace frc {
+/**
+ * A class that implements a moving-window median filter. Useful for reducing
+ * measurement noise, especially with processes that generate occasional,
+ * extreme outliers (such as values from vision processing, LIDAR, or ultrasonic
+ * sensors).
+ */
+template <class T>
+class MedianFilter {
+ public:
+ /**
+ * Creates a new MedianFilter.
+ *
+ * @param size The number of samples in the moving window.
+ */
+ explicit MedianFilter(size_t size) : m_valueBuffer(size), m_size{size} {}
+
+ /**
+ * Calculates the moving-window median for the next value of the input stream.
+ *
+ * @param next The next input value.
+ * @return The median of the moving window, updated to include the next value.
+ */
+ T Calculate(T next) {
+ // Insert next value at proper point in sorted array
+ wpi::insert_sorted(m_orderedValues, next);
+
+ size_t curSize = m_orderedValues.size();
+
+ // If buffer is at max size, pop element off of end of circular buffer
+ // and remove from ordered list
+ if (curSize > m_size) {
+ m_orderedValues.erase(std::find(m_orderedValues.begin(),
+ m_orderedValues.end(),
+ m_valueBuffer.pop_back()));
+ curSize = curSize - 1;
+ }
+
+ // Add next value to circular buffer
+ m_valueBuffer.push_front(next);
+
+ if (curSize % 2 == 1) {
+ // If size is odd, return middle element of sorted list
+ return m_orderedValues[curSize / 2];
+ } else {
+ // If size is even, return average of middle elements
+ return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
+ 2.0;
+ }
+ }
+
+ /**
+ * Resets the filter, clearing the window of all elements.
+ */
+ void Reset() {
+ m_orderedValues.clear();
+ m_valueBuffer.reset();
+ }
+
+ private:
+ wpi::circular_buffer<T> m_valueBuffer;
+ std::vector<T> m_orderedValues;
+ size_t m_size;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Notifier.h b/wpilibc/src/main/native/include/frc/Notifier.h
index f2f37f1..24ffba3 100644
--- a/wpilibc/src/main/native/include/frc/Notifier.h
+++ b/wpilibc/src/main/native/include/frc/Notifier.h
@@ -16,6 +16,7 @@
#include <hal/Types.h>
#include <units/units.h>
+#include <wpi/Twine.h>
#include <wpi/deprecated.h>
#include <wpi/mutex.h>
@@ -47,6 +48,13 @@
Notifier& operator=(Notifier&& rhs);
/**
+ * Sets the name of the notifier. Used for debugging purposes only.
+ *
+ * @param name Name
+ */
+ void SetName(const wpi::Twine& name);
+
+ /**
* Change the handler function.
*
* @param handler Handler
diff --git a/wpilibc/src/main/native/include/frc/PIDBase.h b/wpilibc/src/main/native/include/frc/PIDBase.h
index 098718f..513d46b 100644
--- a/wpilibc/src/main/native/include/frc/PIDBase.h
+++ b/wpilibc/src/main/native/include/frc/PIDBase.h
@@ -402,7 +402,7 @@
double m_error = 0;
double m_result = 0;
- LinearFilter m_filter{{}, {}};
+ LinearFilter<double> m_filter{{}, {}};
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/PWM.h b/wpilibc/src/main/native/include/frc/PWM.h
index 58e18d2..406a93e 100644
--- a/wpilibc/src/main/native/include/frc/PWM.h
+++ b/wpilibc/src/main/native/include/frc/PWM.h
@@ -17,7 +17,7 @@
#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
-
+class AddressableLED;
class SendableBuilder;
/**
@@ -34,11 +34,12 @@
* - 1999 to 1001 = linear scaling from "full forward" to "center"
* - 1000 = center value
* - 999 to 2 = linear scaling from "center" to "full reverse"
- * - 1 = minimum pulse width (currently .5ms)
+ * - 1 = minimum pulse width (currently 0.5ms)
* - 0 = disabled (i.e. PWM output is held low)
*/
class PWM : public MotorSafety, public Sendable, public SendableHelper<PWM> {
public:
+ friend class AddressableLED;
/**
* Represents the amount to multiply the minimum servo-pulse pwm period by.
*/
diff --git a/wpilibc/src/main/native/include/frc/PWMSparkMax.h b/wpilibc/src/main/native/include/frc/PWMSparkMax.h
index c8b22d7..3ce6466 100644
--- a/wpilibc/src/main/native/include/frc/PWMSparkMax.h
+++ b/wpilibc/src/main/native/include/frc/PWMSparkMax.h
@@ -12,14 +12,27 @@
namespace frc {
/**
- * REV Robotics SparkMax Speed Controller.
+ * REV Robotics SPARK MAX Speed Controller.
+ *
+ * Note that the SPARK MAX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the SPARK MAX User
+ * Manual available from REV Robotics.
+ *
+ * \li 2.003ms = full "forward"
+ * \li 1.550ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.460ms = the "low end" of the deadband range
+ * \li 0.999ms = full "reverse"
*/
class PWMSparkMax : public PWMSpeedController {
public:
/**
- * Constructor for a SparkMax.
+ * Constructor for a SPARK MAX.
*
- * @param channel The PWM channel that the Spark is attached to. 0-9 are
+ * @param channel The PWM channel that the SPARK MAX is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
explicit PWMSparkMax(int channel);
diff --git a/wpilibc/src/main/native/include/frc/PWMTalonSRX.h b/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
index 9260e1e..b9c8369 100644
--- a/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
+++ b/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,13 +14,26 @@
/**
* Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM
* control.
+ *
+ * Note that the Talon SRX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Talon SRX User
+ * Manual available from Cross The Road Electronics.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
*/
class PWMTalonSRX : public PWMSpeedController {
public:
/**
- * Construct a PWMTalonSRX connected via PWM.
+ * Construct a Talon SRX connected via PWM.
*
- * @param channel The PWM channel that the PWMTalonSRX is attached to. 0-9 are
+ * @param channel The PWM channel that the Talon SRX is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
explicit PWMTalonSRX(int channel);
diff --git a/wpilibc/src/main/native/include/frc/PWMVictorSPX.h b/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
index d7112c9..a19e704 100644
--- a/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
+++ b/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -14,13 +14,26 @@
/**
* Cross the Road Electronics (CTRE) Victor SPX Speed Controller with PWM
* control.
+ *
+ * Note that the Victor SPX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor SPX User
+ * Manual available from Cross The Road Electronics.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
*/
class PWMVictorSPX : public PWMSpeedController {
public:
/**
- * Construct a PWMVictorSPX connected via PWM.
+ * Construct a Victor SPX connected via PWM.
*
- * @param channel The PWM channel that the PWMVictorSPX is attached to. 0-9
+ * @param channel The PWM channel that the Victor SPX is attached to. 0-9
* are on-board, 10-19 are on the MXP port
*/
explicit PWMVictorSPX(int channel);
diff --git a/wpilibc/src/main/native/include/frc/RobotBase.h b/wpilibc/src/main/native/include/frc/RobotBase.h
index 85a9d12..725aa97 100644
--- a/wpilibc/src/main/native/include/frc/RobotBase.h
+++ b/wpilibc/src/main/native/include/frc/RobotBase.h
@@ -7,9 +7,12 @@
#pragma once
+#include <chrono>
#include <thread>
#include <hal/Main.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
#include <wpi/raw_ostream.h>
#include "frc/Base.h"
@@ -23,9 +26,13 @@
namespace impl {
template <class Robot>
-void RunRobot() {
- static Robot robot;
- robot.StartCompetition();
+void RunRobot(wpi::mutex& m, Robot** robot) {
+ static Robot theRobot;
+ {
+ std::scoped_lock lock{m};
+ *robot = &theRobot;
+ }
+ theRobot.StartCompetition();
}
} // namespace impl
@@ -36,20 +43,50 @@
if (halInit != 0) {
return halInit;
}
+
+ static wpi::mutex m;
+ static wpi::condition_variable cv;
+ static Robot* robot = nullptr;
+ static bool exited = false;
+
if (HAL_HasMain()) {
- std::thread([] {
+ std::thread thr([] {
try {
- impl::RunRobot<Robot>();
+ impl::RunRobot<Robot>(m, &robot);
} catch (...) {
HAL_ExitMain();
+ {
+ std::scoped_lock lock{m};
+ robot = nullptr;
+ exited = true;
+ }
+ cv.notify_all();
throw;
}
+
HAL_ExitMain();
- })
- .detach();
+ {
+ std::scoped_lock lock{m};
+ robot = nullptr;
+ exited = true;
+ }
+ cv.notify_all();
+ });
+
HAL_RunMain();
+
+ // signal loop to exit
+ if (robot) robot->EndCompetition();
+
+ // prefer to join, but detach to exit if it doesn't exit in a timely manner
+ using namespace std::chrono_literals;
+ std::unique_lock lock{m};
+ if (cv.wait_for(lock, 1s, [] { return exited; }))
+ thr.join();
+ else
+ thr.detach();
} else {
- impl::RunRobot<Robot>();
+ impl::RunRobot<Robot>(m, &robot);
}
return 0;
@@ -127,6 +164,8 @@
virtual void StartCompetition() = 0;
+ virtual void EndCompetition() = 0;
+
static constexpr bool IsReal() {
#ifdef __FRC_ROBORIO__
return true;
diff --git a/wpilibc/src/main/native/include/frc/SD540.h b/wpilibc/src/main/native/include/frc/SD540.h
index 91666b4..07f7f18 100644
--- a/wpilibc/src/main/native/include/frc/SD540.h
+++ b/wpilibc/src/main/native/include/frc/SD540.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,6 +13,19 @@
/**
* Mindsensors SD540 Speed Controller.
+ *
+ * Note that the SD540 uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the SD540 User Manual available
+ * from Mindsensors.
+ *
+ * \li 2.05ms = full "forward"
+ * \li 1.55ms = the "high end" of the deadband range
+ * \li 1.50ms = center of the deadband range (off)
+ * \li 1.44ms = the "low end" of the deadband range
+ * \li 0.94ms = full "reverse"
*/
class SD540 : public PWMSpeedController {
public:
diff --git a/wpilibc/src/main/native/include/frc/Servo.h b/wpilibc/src/main/native/include/frc/Servo.h
index ae5c599..16b4281 100644
--- a/wpilibc/src/main/native/include/frc/Servo.h
+++ b/wpilibc/src/main/native/include/frc/Servo.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -105,7 +105,7 @@
static constexpr double kMinServoAngle = 0.0;
static constexpr double kDefaultMaxServoPWM = 2.4;
- static constexpr double kDefaultMinServoPWM = .6;
+ static constexpr double kDefaultMinServoPWM = 0.6;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Spark.h b/wpilibc/src/main/native/include/frc/Spark.h
index 640696f..24ed8f5 100644
--- a/wpilibc/src/main/native/include/frc/Spark.h
+++ b/wpilibc/src/main/native/include/frc/Spark.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -12,14 +12,27 @@
namespace frc {
/**
- * REV Robotics Speed Controller.
+ * REV Robotics SPARK Speed Controller.
+ *
+ * Note that the SPARK uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the SPARK User Manual available
+ * from REV Robotics.
+ *
+ * \li 2.003ms = full "forward"
+ * \li 1.550ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.460ms = the "low end" of the deadband range
+ * \li 0.999ms = full "reverse"
*/
class Spark : public PWMSpeedController {
public:
/**
- * Constructor for a Spark.
+ * Constructor for a SPARK.
*
- * @param channel The PWM channel that the Spark is attached to. 0-9 are
+ * @param channel The PWM channel that the SPARK is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
explicit Spark(int channel);
diff --git a/wpilibc/src/main/native/include/frc/SpeedController.h b/wpilibc/src/main/native/include/frc/SpeedController.h
index 49a828b..2b11aee 100644
--- a/wpilibc/src/main/native/include/frc/SpeedController.h
+++ b/wpilibc/src/main/native/include/frc/SpeedController.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,8 @@
#pragma once
+#include <units/units.h>
+
#include "frc/PIDOutput.h"
namespace frc {
@@ -26,6 +28,20 @@
virtual void Set(double speed) = 0;
/**
+ * Sets the voltage output of the SpeedController. Compensates for
+ * the current bus voltage to ensure that the desired voltage is output even
+ * if the battery voltage is below 12V - highly useful when the voltage
+ * outputs are "meaningful" (e.g. they come from a feedforward calculation).
+ *
+ * <p>NOTE: This function *must* be called regularly in order for voltage
+ * compensation to work properly - unlike the ordinary set function, it is not
+ * "set it and forget it."
+ *
+ * @param output The voltage to output.
+ */
+ virtual void SetVoltage(units::volt_t output);
+
+ /**
* Common interface for getting the current set speed of a speed controller.
*
* @return The current set speed. Value is between -1.0 and 1.0.
diff --git a/wpilibc/src/main/native/include/frc/Talon.h b/wpilibc/src/main/native/include/frc/Talon.h
index 9a5400f..6e92dfc 100644
--- a/wpilibc/src/main/native/include/frc/Talon.h
+++ b/wpilibc/src/main/native/include/frc/Talon.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,6 +13,19 @@
/**
* Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller.
+ *
+ * Note that the Talon uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the Talon User Manual available
+ * from CTRE.
+ *
+ * \li 2.037ms = full "forward"
+ * \li 1.539ms = the "high end" of the deadband range
+ * \li 1.513ms = center of the deadband range (off)
+ * \li 1.487ms = the "low end" of the deadband range
+ * \li 0.989ms = full "reverse"
*/
class Talon : public PWMSpeedController {
public:
diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h
index 1c8ff80..112e2c9 100644
--- a/wpilibc/src/main/native/include/frc/TimedRobot.h
+++ b/wpilibc/src/main/native/include/frc/TimedRobot.h
@@ -35,6 +35,11 @@
void StartCompetition() override;
/**
+ * Ends the main loop in StartCompetition().
+ */
+ void EndCompetition() override;
+
+ /**
* Get the time period between calls to Periodic() functions.
*/
units::second_t GetPeriod() const;
diff --git a/wpilibc/src/main/native/include/frc/Victor.h b/wpilibc/src/main/native/include/frc/Victor.h
index 5b6cba4..89c4a89 100644
--- a/wpilibc/src/main/native/include/frc/Victor.h
+++ b/wpilibc/src/main/native/include/frc/Victor.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -16,6 +16,20 @@
*
* The Vex Robotics Victor 884 Speed Controller can also be used with this
* class but may need to be calibrated per the Victor 884 user manual.
+ *
+ * Note that the Victor uses the following bounds for PWM values. These
+ * values were determined empirically and optimized for the Victor 888. These
+ * values should work reasonably well for Victor 884 controllers as well but
+ * if users experience issues such as asymmetric behavior around the deadband
+ * or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor 884 User
+ * Manual available from Vex.
+ *
+ * \li 2.027ms = full "forward"
+ * \li 1.525ms = the "high end" of the deadband range
+ * \li 1.507ms = center of the deadband range (off)
+ * \li 1.490ms = the "low end" of the deadband range
+ * \li 1.026ms = full "reverse"
*/
class Victor : public PWMSpeedController {
public:
diff --git a/wpilibc/src/main/native/include/frc/VictorSP.h b/wpilibc/src/main/native/include/frc/VictorSP.h
index b23fba1..d8aa8e8 100644
--- a/wpilibc/src/main/native/include/frc/VictorSP.h
+++ b/wpilibc/src/main/native/include/frc/VictorSP.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -13,11 +13,24 @@
/**
* Vex Robotics Victor SP Speed Controller.
+ *
+ * Note that the Victor SP uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor SP User
+ * Manual available from Vex.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
*/
class VictorSP : public PWMSpeedController {
public:
/**
- * Constructor for a VictorSP.
+ * Constructor for a Victor SP.
*
* @param channel The PWM channel that the VictorSP is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
diff --git a/wpilibc/src/main/native/include/frc/WPILib.h b/wpilibc/src/main/native/include/frc/WPILib.h
index 69fa061..9d62514 100644
--- a/wpilibc/src/main/native/include/frc/WPILib.h
+++ b/wpilibc/src/main/native/include/frc/WPILib.h
@@ -16,8 +16,10 @@
// clang-format on
+#if __has_include(<cameraserver/CameraServer.h>)
#include <cameraserver/CameraServer.h>
#include <vision/VisionRunner.h>
+#endif
#include "frc/ADXL345_I2C.h"
#include "frc/ADXL345_SPI.h"
@@ -82,6 +84,7 @@
#include "frc/VictorSP.h"
#include "frc/WPIErrors.h"
#include "frc/XboxController.h"
+#if __has_include("frc/buttons/InternalButton.h")
#include "frc/buttons/InternalButton.h"
#include "frc/buttons/JoystickButton.h"
#include "frc/buttons/NetworkButton.h"
@@ -90,12 +93,12 @@
#include "frc/commands/PIDCommand.h"
#include "frc/commands/PIDSubsystem.h"
#include "frc/commands/PrintCommand.h"
-#include "frc/commands/Scheduler.h"
#include "frc/commands/StartCommand.h"
#include "frc/commands/Subsystem.h"
#include "frc/commands/WaitCommand.h"
#include "frc/commands/WaitForChildren.h"
#include "frc/commands/WaitUntilCommand.h"
+#endif
#include "frc/drive/DifferentialDrive.h"
#include "frc/drive/KilloughDrive.h"
#include "frc/drive/MecanumDrive.h"
diff --git a/wpilibc/src/main/native/include/frc/buttons/Button.h b/wpilibc/src/main/native/include/frc/buttons/Button.h
deleted file mode 100644
index 71641e9..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/Button.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/Trigger.h"
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-/**
- * This class provides an easy way to link commands to OI inputs.
- *
- * It is very easy to link a button to a command. For instance, you could link
- * the trigger button of a joystick to a "score" command.
- *
- * This class represents a subclass of Trigger that is specifically aimed at
- * buttons on an operator interface as a common use case of the more generalized
- * Trigger objects. This is a simple wrapper around Trigger with the method
- * names renamed to fit the Button object use.
- */
-class Button : public Trigger {
- public:
- Button() = default;
- Button(Button&&) = default;
- Button& operator=(Button&&) = default;
-
- /**
- * Specifies the command to run when a button is first pressed.
- *
- * @param command The pointer to the command to run
- */
- virtual void WhenPressed(Command* command);
-
- /**
- * Specifies the command to be scheduled while the button is pressed.
- *
- * The command will be scheduled repeatedly while the button is pressed and
- * will be canceled when the button is released.
- *
- * @param command The pointer to the command to run
- */
- virtual void WhileHeld(Command* command);
-
- /**
- * Specifies the command to run when the button is released.
- *
- * The command will be scheduled a single time.
- *
- * @param command The pointer to the command to run
- */
- virtual void WhenReleased(Command* command);
-
- /**
- * Cancels the specificed command when the button is pressed.
- *
- * @param command The command to be canceled
- */
- virtual void CancelWhenPressed(Command* command);
-
- /**
- * Toggle the specified command when the button is pressed.
- *
- * @param command The command to be toggled
- */
- virtual void ToggleWhenPressed(Command* command);
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/ButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/ButtonScheduler.h
deleted file mode 100644
index ec2e35b..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/ButtonScheduler.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class ButtonScheduler {
- public:
- ButtonScheduler(bool last, Trigger* button, Command* orders);
- virtual ~ButtonScheduler() = default;
-
- ButtonScheduler(ButtonScheduler&&) = default;
- ButtonScheduler& operator=(ButtonScheduler&&) = default;
-
- virtual void Execute() = 0;
- void Start();
-
- protected:
- bool m_pressedLast;
- Trigger* m_button;
- Command* m_command;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/CancelButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/CancelButtonScheduler.h
deleted file mode 100644
index 69b8a11..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/CancelButtonScheduler.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/ButtonScheduler.h"
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class CancelButtonScheduler : public ButtonScheduler {
- public:
- CancelButtonScheduler(bool last, Trigger* button, Command* orders);
- virtual ~CancelButtonScheduler() = default;
-
- CancelButtonScheduler(CancelButtonScheduler&&) = default;
- CancelButtonScheduler& operator=(CancelButtonScheduler&&) = default;
-
- virtual void Execute();
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/HeldButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/HeldButtonScheduler.h
deleted file mode 100644
index ad4a160..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/HeldButtonScheduler.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/ButtonScheduler.h"
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class HeldButtonScheduler : public ButtonScheduler {
- public:
- HeldButtonScheduler(bool last, Trigger* button, Command* orders);
- virtual ~HeldButtonScheduler() = default;
-
- HeldButtonScheduler(HeldButtonScheduler&&) = default;
- HeldButtonScheduler& operator=(HeldButtonScheduler&&) = default;
-
- virtual void Execute();
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/InternalButton.h b/wpilibc/src/main/native/include/frc/buttons/InternalButton.h
deleted file mode 100644
index 430a21e..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/InternalButton.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/Button.h"
-
-namespace frc {
-
-class InternalButton : public Button {
- public:
- InternalButton() = default;
- explicit InternalButton(bool inverted);
- virtual ~InternalButton() = default;
-
- InternalButton(InternalButton&&) = default;
- InternalButton& operator=(InternalButton&&) = default;
-
- void SetInverted(bool inverted);
- void SetPressed(bool pressed);
-
- virtual bool Get();
-
- private:
- bool m_pressed = false;
- bool m_inverted = false;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/JoystickButton.h b/wpilibc/src/main/native/include/frc/buttons/JoystickButton.h
deleted file mode 100644
index 1b4264f..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/JoystickButton.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/GenericHID.h"
-#include "frc/buttons/Button.h"
-
-namespace frc {
-
-class JoystickButton : public Button {
- public:
- JoystickButton(GenericHID* joystick, int buttonNumber);
- virtual ~JoystickButton() = default;
-
- JoystickButton(JoystickButton&&) = default;
- JoystickButton& operator=(JoystickButton&&) = default;
-
- virtual bool Get();
-
- private:
- GenericHID* m_joystick;
- int m_buttonNumber;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/NetworkButton.h b/wpilibc/src/main/native/include/frc/buttons/NetworkButton.h
deleted file mode 100644
index fef8065..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/NetworkButton.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <networktables/NetworkTable.h>
-#include <networktables/NetworkTableEntry.h>
-#include <wpi/Twine.h>
-
-#include "frc/buttons/Button.h"
-
-namespace frc {
-
-class NetworkButton : public Button {
- public:
- NetworkButton(const wpi::Twine& tableName, const wpi::Twine& field);
- NetworkButton(std::shared_ptr<nt::NetworkTable> table,
- const wpi::Twine& field);
- virtual ~NetworkButton() = default;
-
- NetworkButton(NetworkButton&&) = default;
- NetworkButton& operator=(NetworkButton&&) = default;
-
- virtual bool Get();
-
- private:
- nt::NetworkTableEntry m_entry;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/POVButton.h b/wpilibc/src/main/native/include/frc/buttons/POVButton.h
deleted file mode 100644
index 15c4bf8..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/POVButton.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/GenericHID.h"
-#include "frc/buttons/Button.h"
-
-namespace frc {
-class POVButton : public Button {
- public:
- /**
- * Creates a POV button for triggering commands.
- *
- * @param joystick The GenericHID object that has the POV
- * @param angle The desired angle in degrees (e.g. 90, 270)
- * @param povNumber The POV number (@see GenericHID#GetPOV)
- */
- POVButton(GenericHID& joystick, int angle, int povNumber = 0);
- virtual ~POVButton() = default;
-
- POVButton(POVButton&&) = default;
- POVButton& operator=(POVButton&&) = default;
-
- bool Get() override;
-
- private:
- GenericHID* m_joystick;
- int m_angle;
- int m_povNumber;
-};
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/PressedButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/PressedButtonScheduler.h
deleted file mode 100644
index 0c1fb03..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/PressedButtonScheduler.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/ButtonScheduler.h"
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class PressedButtonScheduler : public ButtonScheduler {
- public:
- PressedButtonScheduler(bool last, Trigger* button, Command* orders);
- virtual ~PressedButtonScheduler() = default;
-
- PressedButtonScheduler(PressedButtonScheduler&&) = default;
- PressedButtonScheduler& operator=(PressedButtonScheduler&&) = default;
-
- virtual void Execute();
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
deleted file mode 100644
index 899a483..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/ButtonScheduler.h"
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class ReleasedButtonScheduler : public ButtonScheduler {
- public:
- ReleasedButtonScheduler(bool last, Trigger* button, Command* orders);
- virtual ~ReleasedButtonScheduler() = default;
-
- ReleasedButtonScheduler(ReleasedButtonScheduler&&) = default;
- ReleasedButtonScheduler& operator=(ReleasedButtonScheduler&&) = default;
-
- virtual void Execute();
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/ToggleButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
deleted file mode 100644
index aeb93b3..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/ButtonScheduler.h"
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class ToggleButtonScheduler : public ButtonScheduler {
- public:
- ToggleButtonScheduler(bool last, Trigger* button, Command* orders);
- virtual ~ToggleButtonScheduler() = default;
-
- ToggleButtonScheduler(ToggleButtonScheduler&&) = default;
- ToggleButtonScheduler& operator=(ToggleButtonScheduler&&) = default;
-
- virtual void Execute();
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/Trigger.h b/wpilibc/src/main/native/include/frc/buttons/Trigger.h
deleted file mode 100644
index 56700e9..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/Trigger.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <atomic>
-
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class Command;
-
-/**
- * This class provides an easy way to link commands to inputs.
- *
- * It is very easy to link a polled input to a command. For instance, you could
- * link the trigger button of a joystick to a "score" command or an encoder
- * reaching a particular value.
- *
- * It is encouraged that teams write a subclass of Trigger if they want to have
- * something unusual (for instance, if they want to react to the user holding
- * a button while the robot is reading a certain sensor input). For this, they
- * only have to write the {@link Trigger#Get()} method to get the full
- * functionality of the Trigger class.
- */
-class Trigger : public Sendable, public SendableHelper<Trigger> {
- public:
- Trigger() = default;
- ~Trigger() override = default;
-
- Trigger(const Trigger& rhs);
- Trigger& operator=(const Trigger& rhs);
- Trigger(Trigger&& rhs);
- Trigger& operator=(Trigger&& rhs);
-
- bool Grab();
- virtual bool Get() = 0;
- void WhenActive(Command* command);
- void WhileActive(Command* command);
- void WhenInactive(Command* command);
- void CancelWhenActive(Command* command);
- void ToggleWhenActive(Command* command);
-
- void InitSendable(SendableBuilder& builder) override;
-
- private:
- std::atomic_bool m_sendablePressed{false};
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/Command.h b/wpilibc/src/main/native/include/frc/commands/Command.h
deleted file mode 100644
index d40bb00..0000000
--- a/wpilibc/src/main/native/include/frc/commands/Command.h
+++ /dev/null
@@ -1,492 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <string>
-
-#include <wpi/SmallPtrSet.h>
-#include <wpi/Twine.h>
-
-#include "frc/ErrorBase.h"
-#include "frc/commands/Subsystem.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class CommandGroup;
-
-/**
- * The Command class is at the very core of the entire command framework.
- *
- * Every command can be started with a call to Start(). Once a command is
- * started it will call Initialize(), and then will repeatedly call Execute()
- * until the IsFinished() returns true. Once it does,End() will be called.
- *
- * However, if at any point while it is running Cancel() is called, then the
- * command will be stopped and Interrupted() will be called.
- *
- * If a command uses a Subsystem, then it should specify that it does so by
- * calling the Requires() method in its constructor. Note that a Command may
- * have multiple requirements, and Requires() should be called for each one.
- *
- * If a command is running and a new command with shared requirements is
- * started, then one of two things will happen. If the active command is
- * interruptible, then Cancel() will be called and the command will be removed
- * to make way for the new one. If the active command is not interruptible, the
- * other one will not even be started, and the active one will continue
- * functioning.
- *
- * @see CommandGroup
- * @see Subsystem
- */
-class Command : public ErrorBase,
- public Sendable,
- public SendableHelper<Command> {
- friend class CommandGroup;
- friend class Scheduler;
-
- public:
- /**
- * Creates a new command.
- *
- * The name of this command will be default.
- */
- Command();
-
- /**
- * Creates a new command with the given name and no timeout.
- *
- * @param name the name for this command
- */
- explicit Command(const wpi::Twine& name);
-
- /**
- * Creates a new command with the given timeout and a default name.
- *
- * @param timeout the time (in seconds) before this command "times out"
- * @see IsTimedOut()
- */
- explicit Command(double timeout);
-
- /**
- * Creates a new command with the given timeout and a default name.
- *
- * @param subsystem the subsystem that the command requires
- */
- explicit Command(Subsystem& subsystem);
-
- /**
- * Creates a new command with the given name and timeout.
- *
- * @param name the name of the command
- * @param timeout the time (in seconds) before this command "times out"
- * @see IsTimedOut()
- */
- Command(const wpi::Twine& name, double timeout);
-
- /**
- * Creates a new command with the given name and timeout.
- *
- * @param name the name of the command
- * @param subsystem the subsystem that the command requires
- */
- Command(const wpi::Twine& name, Subsystem& subsystem);
-
- /**
- * Creates a new command with the given name and timeout.
- *
- * @param timeout the time (in seconds) before this command "times out"
- * @param subsystem the subsystem that the command requires @see IsTimedOut()
- */
- Command(double timeout, Subsystem& subsystem);
-
- /**
- * Creates a new command with the given name and timeout.
- *
- * @param name the name of the command
- * @param timeout the time (in seconds) before this command "times out"
- * @param subsystem the subsystem that the command requires @see IsTimedOut()
- */
- Command(const wpi::Twine& name, double timeout, Subsystem& subsystem);
-
- ~Command() override = default;
-
- Command(Command&&) = default;
- Command& operator=(Command&&) = default;
-
- /**
- * Returns the time since this command was initialized (in seconds).
- *
- * This function will work even if there is no specified timeout.
- *
- * @return the time since this command was initialized (in seconds).
- */
- double TimeSinceInitialized() const;
-
- /**
- * This method specifies that the given Subsystem is used by this command.
- *
- * This method is crucial to the functioning of the Command System in general.
- *
- * Note that the recommended way to call this method is in the constructor.
- *
- * @param subsystem The Subsystem required
- * @see Subsystem
- */
- void Requires(Subsystem* s);
-
- /**
- * Starts up the command. Gets the command ready to start.
- *
- * Note that the command will eventually start, however it will not
- * necessarily do so immediately, and may in fact be canceled before
- * initialize is even called.
- */
- void Start();
-
- /**
- * The run method is used internally to actually run the commands.
- *
- * @return Whether or not the command should stay within the Scheduler.
- */
- bool Run();
-
- /**
- * This will cancel the current command.
- *
- * This will cancel the current command eventually. It can be called multiple
- * times. And it can be called when the command is not running. If the command
- * is running though, then the command will be marked as canceled and
- * eventually removed.
- *
- * A command can not be canceled if it is a part of a command group, you must
- * cancel the command group instead.
- */
- void Cancel();
-
- /**
- * Returns whether or not the command is running.
- *
- * This may return true even if the command has just been canceled, as it may
- * not have yet called Interrupted().
- *
- * @return whether or not the command is running
- */
- bool IsRunning() const;
-
- /**
- * Returns whether or not the command has been initialized.
- *
- * @return whether or not the command has been initialized.
- */
- bool IsInitialized() const;
-
- /**
- * Returns whether or not the command has completed running.
- *
- * @return whether or not the command has completed running.
- */
- bool IsCompleted() const;
-
- /**
- * Returns whether or not this has been canceled.
- *
- * @return whether or not this has been canceled
- */
- bool IsCanceled() const;
-
- /**
- * Returns whether or not this command can be interrupted.
- *
- * @return whether or not this command can be interrupted
- */
- bool IsInterruptible() const;
-
- /**
- * Sets whether or not this command can be interrupted.
- *
- * @param interruptible whether or not this command can be interrupted
- */
- void SetInterruptible(bool interruptible);
-
- /**
- * Checks if the command requires the given Subsystem.
- *
- * @param system the system
- * @return whether or not the subsystem is required (false if given nullptr)
- */
- bool DoesRequire(Subsystem* subsystem) const;
-
- using SubsystemSet = wpi::SmallPtrSetImpl<Subsystem*>;
-
- /**
- * Returns the requirements (as an std::set of Subsystem pointers) of this
- * command.
- *
- * @return The requirements (as an std::set of Subsystem pointers) of this
- * command
- */
- const SubsystemSet& GetRequirements() const;
-
- /**
- * Returns the CommandGroup that this command is a part of.
- *
- * Will return null if this Command is not in a group.
- *
- * @return The CommandGroup that this command is a part of (or null if not in
- * group)
- */
- CommandGroup* GetGroup() const;
-
- /**
- * Sets whether or not this Command should run when the robot is disabled.
- *
- * By default a command will not run when the robot is disabled, and will in
- * fact be canceled.
- *
- * @param run Whether this command should run when the robot is disabled.
- */
- void SetRunWhenDisabled(bool run);
-
- /**
- * Returns whether or not this Command will run when the robot is disabled, or
- * if it will cancel itself.
- *
- * @return Whether this Command will run when the robot is disabled, or if it
- * will cancel itself.
- */
- bool WillRunWhenDisabled() const;
-
- /**
- * Get the ID (sequence number) for this command.
- *
- * The ID is a unique sequence number that is incremented for each command.
- *
- * @return The ID of this command
- */
- int GetID() const;
-
- protected:
- /**
- * Sets the timeout of this command.
- *
- * @param timeout the timeout (in seconds)
- * @see IsTimedOut()
- */
- void SetTimeout(double timeout);
-
- /**
- * Returns whether or not the TimeSinceInitialized() method returns a number
- * which is greater than or equal to the timeout for the command.
- *
- * If there is no timeout, this will always return false.
- *
- * @return whether the time has expired
- */
- bool IsTimedOut() const;
-
- /**
- * If changes are locked, then this will generate a CommandIllegalUse error.
- *
- * @param message The message to report on error (it is appended by a default
- * message)
- * @return True if assert passed, false if assert failed.
- */
- bool AssertUnlocked(const std::string& message);
-
- /**
- * Sets the parent of this command. No actual change is made to the group.
- *
- * @param parent the parent
- */
- void SetParent(CommandGroup* parent);
-
- /**
- * Returns whether the command has a parent.
- *
- * @param True if the command has a parent.
- */
- bool IsParented() const;
-
- /**
- * Clears list of subsystem requirements.
- *
- * This is only used by ConditionalCommand so cancelling the chosen command
- * works properly in CommandGroup.
- */
- void ClearRequirements();
-
- /**
- * The initialize method is called the first time this Command is run after
- * being started.
- */
- virtual void Initialize();
-
- /**
- * The execute method is called repeatedly until this Command either finishes
- * or is canceled.
- */
- virtual void Execute();
-
- /**
- * Returns whether this command is finished.
- *
- * If it is, then the command will be removed and End() will be called.
- *
- * It may be useful for a team to reference the IsTimedOut() method for
- * time-sensitive commands.
- *
- * Returning false will result in the command never ending automatically.
- * It may still be cancelled manually or interrupted by another command.
- * Returning true will result in the command executing once and finishing
- * immediately. We recommend using InstantCommand for this.
- *
- * @return Whether this command is finished.
- * @see IsTimedOut()
- */
- virtual bool IsFinished() = 0;
-
- /**
- * Called when the command ended peacefully.
- *
- * This is where you may want to wrap up loose ends, like shutting off a motor
- * that was being used in the command.
- */
- virtual void End();
-
- /**
- * Called when the command ends because somebody called Cancel() or another
- * command shared the same requirements as this one, and booted it out.
- *
- * This is where you may want to wrap up loose ends, like shutting off a motor
- * that was being used in the command.
- *
- * Generally, it is useful to simply call the End() method within this method,
- * as done here.
- */
- virtual void Interrupted();
-
- virtual void _Initialize();
- virtual void _Interrupted();
- virtual void _Execute();
- virtual void _End();
-
- /**
- * This works like Cancel(), except that it doesn't throw an exception if it
- * is a part of a command group.
- *
- * Should only be called by the parent command group.
- */
- virtual void _Cancel();
-
- friend class ConditionalCommand;
-
- /**
- * Gets the name of this Command.
- *
- * @return Name
- */
- std::string GetName() const;
-
- /**
- * Sets the name of this Command.
- *
- * @param name name
- */
- void SetName(const wpi::Twine& name);
-
- /**
- * Gets the subsystem name of this Command.
- *
- * @return Subsystem name
- */
- std::string GetSubsystem() const;
-
- /**
- * Sets the subsystem name of this Command.
- *
- * @param subsystem subsystem name
- */
- void SetSubsystem(const wpi::Twine& subsystem);
-
- private:
- /**
- * Prevents further changes from being made.
- */
- void LockChanges();
-
- /**
- * Called when the command has been removed.
- *
- * This will call Interrupted() or End().
- */
- void Removed();
-
- /**
- * This is used internally to mark that the command has been started.
- *
- * The lifecycle of a command is:
- *
- * StartRunning() is called. Run() is called (multiple times potentially).
- * Removed() is called.
- *
- * It is very important that StartRunning() and Removed() be called in order
- * or some assumptions of the code will be broken.
- */
- void StartRunning();
-
- /**
- * Called to indicate that the timer should start.
- *
- * This is called right before Initialize() is, inside the Run() method.
- */
- void StartTiming();
-
- // The time since this command was initialized
- double m_startTime = -1;
-
- // The time (in seconds) before this command "times out" (-1 if no timeout)
- double m_timeout;
-
- // Whether or not this command has been initialized
- bool m_initialized = false;
-
- // The requirements (or null if no requirements)
- wpi::SmallPtrSet<Subsystem*, 4> m_requirements;
-
- // Whether or not it is running
- bool m_running = false;
-
- // Whether or not it is interruptible
- bool m_interruptible = true;
-
- // Whether or not it has been canceled
- bool m_canceled = false;
-
- // Whether or not it has been locked
- bool m_locked = false;
-
- // Whether this command should run when the robot is disabled
- bool m_runWhenDisabled = false;
-
- // The CommandGroup this is in
- CommandGroup* m_parent = nullptr;
-
- // Whether or not this command has completed running
- bool m_completed = false;
-
- int m_commandID = m_commandCounter++;
- static int m_commandCounter;
-
- public:
- void InitSendable(SendableBuilder& builder) override;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/CommandGroup.h b/wpilibc/src/main/native/include/frc/commands/CommandGroup.h
deleted file mode 100644
index 690ae6d..0000000
--- a/wpilibc/src/main/native/include/frc/commands/CommandGroup.h
+++ /dev/null
@@ -1,180 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <vector>
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-#include "frc/commands/CommandGroupEntry.h"
-
-namespace frc {
-
-/**
- * A CommandGroup is a list of commands which are executed in sequence.
- *
- * Commands in a CommandGroup are added using the AddSequential() method and are
- * called sequentially. CommandGroups are themselves Commands and can be given
- * to other CommandGroups.
- *
- * CommandGroups will carry all of the requirements of their Command
- * subcommands. Additional requirements can be specified by calling Requires()
- * normally in the constructor.
- *
- * CommandGroups can also execute commands in parallel, simply by adding them
- * using AddParallel().
- *
- * @see Command
- * @see Subsystem
- */
-class CommandGroup : public Command {
- public:
- CommandGroup() = default;
-
- /**
- * Creates a new CommandGroup with the given name.
- *
- * @param name The name for this command group
- */
- explicit CommandGroup(const wpi::Twine& name);
-
- virtual ~CommandGroup() = default;
-
- CommandGroup(CommandGroup&&) = default;
- CommandGroup& operator=(CommandGroup&&) = default;
-
- /**
- * Adds a new Command to the group. The Command will be started after all the
- * previously added Commands.
- *
- * Note that any requirements the given Command has will be added to the
- * group. For this reason, a Command's requirements can not be changed after
- * being added to a group.
- *
- * It is recommended that this method be called in the constructor.
- *
- * @param command The Command to be added
- */
- void AddSequential(Command* command);
-
- /**
- * Adds a new Command to the group with a given timeout. The Command will be
- * started after all the previously added commands.
- *
- * Once the Command is started, it will be run until it finishes or the time
- * expires, whichever is sooner. Note that the given Command will have no
- * knowledge that it is on a timer.
- *
- * Note that any requirements the given Command has will be added to the
- * group. For this reason, a Command's requirements can not be changed after
- * being added to a group.
- *
- * It is recommended that this method be called in the constructor.
- *
- * @param command The Command to be added
- * @param timeout The timeout (in seconds)
- */
- void AddSequential(Command* command, double timeout);
-
- /**
- * Adds a new child Command to the group. The Command will be started after
- * all the previously added Commands.
- *
- * Instead of waiting for the child to finish, a CommandGroup will have it run
- * at the same time as the subsequent Commands. The child will run until
- * either it finishes, a new child with conflicting requirements is started,
- * or the main sequence runs a Command with conflicting requirements. In the
- * latter two cases, the child will be canceled even if it says it can't be
- * interrupted.
- *
- * Note that any requirements the given Command has will be added to the
- * group. For this reason, a Command's requirements can not be changed after
- * being added to a group.
- *
- * It is recommended that this method be called in the constructor.
- *
- * @param command The command to be added
- */
- void AddParallel(Command* command);
-
- /**
- * Adds a new child Command to the group with the given timeout. The Command
- * will be started after all the previously added Commands.
- *
- * Once the Command is started, it will run until it finishes, is interrupted,
- * or the time expires, whichever is sooner. Note that the given Command will
- * have no knowledge that it is on a timer.
- *
- * Instead of waiting for the child to finish, a CommandGroup will have it run
- * at the same time as the subsequent Commands. The child will run until
- * either it finishes, the timeout expires, a new child with conflicting
- * requirements is started, or the main sequence runs a Command with
- * conflicting requirements. In the latter two cases, the child will be
- * canceled even if it says it can't be interrupted.
- *
- * Note that any requirements the given Command has will be added to the
- * group. For this reason, a Command's requirements can not be changed after
- * being added to a group.
- *
- * It is recommended that this method be called in the constructor.
- *
- * @param command The command to be added
- * @param timeout The timeout (in seconds)
- */
- void AddParallel(Command* command, double timeout);
-
- bool IsInterruptible() const;
-
- int GetSize() const;
-
- protected:
- /**
- * Can be overridden by teams.
- */
- virtual void Initialize();
-
- /**
- * Can be overridden by teams.
- */
- virtual void Execute();
-
- /**
- * Can be overridden by teams.
- */
- virtual bool IsFinished();
-
- /**
- * Can be overridden by teams.
- */
- virtual void End();
-
- /**
- * Can be overridden by teams.
- */
- virtual void Interrupted();
-
- virtual void _Initialize();
- virtual void _Execute();
- virtual void _End();
- virtual void _Interrupted();
-
- private:
- void CancelConflicts(Command* command);
-
- // The commands in this group (stored in entries)
- std::vector<CommandGroupEntry> m_commands;
-
- // The active children in this group (stored in entries)
- std::vector<CommandGroupEntry*> m_children;
-
- // The current command, -1 signifies that none have been run
- int m_currentCommandIndex = -1;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/CommandGroupEntry.h b/wpilibc/src/main/native/include/frc/commands/CommandGroupEntry.h
deleted file mode 100644
index 2de1e43..0000000
--- a/wpilibc/src/main/native/include/frc/commands/CommandGroupEntry.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace frc {
-
-class Command;
-
-class CommandGroupEntry {
- public:
- enum Sequence {
- kSequence_InSequence,
- kSequence_BranchPeer,
- kSequence_BranchChild
- };
-
- CommandGroupEntry() = default;
- CommandGroupEntry(Command* command, Sequence state, double timeout = -1.0);
-
- CommandGroupEntry(CommandGroupEntry&&) = default;
- CommandGroupEntry& operator=(CommandGroupEntry&&) = default;
-
- bool IsTimedOut() const;
-
- double m_timeout = -1.0;
- Command* m_command = nullptr;
- Sequence m_state = kSequence_InSequence;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h b/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h
deleted file mode 100644
index f5cd738..0000000
--- a/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h
+++ /dev/null
@@ -1,84 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-/**
- * A ConditionalCommand is a Command that starts one of two commands.
- *
- * A ConditionalCommand uses the Condition method to determine whether it should
- * run onTrue or onFalse.
- *
- * A ConditionalCommand adds the proper Command to the Scheduler during
- * Initialize() and then IsFinished() will return true once that Command has
- * finished executing.
- *
- * If no Command is specified for onFalse, the occurrence of that condition
- * will be a no-op.
- *
- * A ConditionalCommand will require the superset of subsystems of the onTrue
- * and onFalse commands.
- *
- * @see Command
- * @see Scheduler
- */
-class ConditionalCommand : public Command {
- public:
- /**
- * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
- *
- * @param onTrue The Command to execute if Condition() returns true
- * @param onFalse The Command to execute if Condition() returns false
- */
- explicit ConditionalCommand(Command* onTrue, Command* onFalse = nullptr);
-
- /**
- * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
- *
- * @param name The name for this command group
- * @param onTrue The Command to execute if Condition() returns true
- * @param onFalse The Command to execute if Condition() returns false
- */
- ConditionalCommand(const wpi::Twine& name, Command* onTrue,
- Command* onFalse = nullptr);
-
- virtual ~ConditionalCommand() = default;
-
- ConditionalCommand(ConditionalCommand&&) = default;
- ConditionalCommand& operator=(ConditionalCommand&&) = default;
-
- protected:
- /**
- * The Condition to test to determine which Command to run.
- *
- * @return true if m_onTrue should be run or false if m_onFalse should be run.
- */
- virtual bool Condition() = 0;
-
- void _Initialize() override;
- void _Cancel() override;
- bool IsFinished() override;
- void _Interrupted() override;
-
- private:
- // The Command to execute if Condition() returns true
- Command* m_onTrue;
-
- // The Command to execute if Condition() returns false
- Command* m_onFalse;
-
- // Stores command chosen by condition
- Command* m_chosenCommand = nullptr;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/InstantCommand.h b/wpilibc/src/main/native/include/frc/commands/InstantCommand.h
deleted file mode 100644
index 987dc18..0000000
--- a/wpilibc/src/main/native/include/frc/commands/InstantCommand.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <functional>
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-#include "frc/commands/Subsystem.h"
-
-namespace frc {
-
-/**
- * This command will execute once, then finish immediately afterward.
- *
- * Subclassing InstantCommand is shorthand for returning true from IsFinished().
- */
-class InstantCommand : public Command {
- public:
- /**
- * Creates a new InstantCommand with the given name.
- *
- * @param name The name for this command
- */
- explicit InstantCommand(const wpi::Twine& name);
-
- /**
- * Creates a new InstantCommand with the given requirement.
- *
- * @param subsystem The subsystem that the command requires
- */
- explicit InstantCommand(Subsystem& subsystem);
-
- /**
- * Creates a new InstantCommand with the given name.
- *
- * @param name The name for this command
- * @param subsystem The subsystem that the command requires
- */
- InstantCommand(const wpi::Twine& name, Subsystem& subsystem);
-
- /**
- * Create a command that calls the given function when run.
- *
- * @param func The function to run when Initialize() is run.
- */
- explicit InstantCommand(std::function<void()> func);
-
- /**
- * Create a command that calls the given function when run.
- *
- * @param subsystem The subsystems that this command runs on.
- * @param func The function to run when Initialize() is run.
- */
- InstantCommand(Subsystem& subsystem, std::function<void()> func);
-
- /**
- * Create a command that calls the given function when run.
- *
- * @param name The name of the command.
- * @param func The function to run when Initialize() is run.
- */
- InstantCommand(const wpi::Twine& name, std::function<void()> func);
-
- /**
- * Create a command that calls the given function when run.
- *
- * @param name The name of the command.
- * @param subsystem The subsystems that this command runs on.
- * @param func The function to run when Initialize() is run.
- */
- InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
- std::function<void()> func);
-
- InstantCommand() = default;
- virtual ~InstantCommand() = default;
-
- InstantCommand(InstantCommand&&) = default;
- InstantCommand& operator=(InstantCommand&&) = default;
-
- protected:
- std::function<void()> m_func = nullptr;
- void _Initialize() override;
- bool IsFinished() override;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/PIDCommand.h b/wpilibc/src/main/native/include/frc/commands/PIDCommand.h
deleted file mode 100644
index 02f1710..0000000
--- a/wpilibc/src/main/native/include/frc/commands/PIDCommand.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <wpi/Twine.h>
-
-#include "frc/PIDController.h"
-#include "frc/PIDOutput.h"
-#include "frc/PIDSource.h"
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-class PIDCommand : public Command, public PIDOutput, public PIDSource {
- public:
- PIDCommand(const wpi::Twine& name, double p, double i, double d);
- PIDCommand(const wpi::Twine& name, double p, double i, double d,
- double period);
- PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
- double period);
- PIDCommand(double p, double i, double d);
- PIDCommand(double p, double i, double d, double period);
- PIDCommand(double p, double i, double d, double f, double period);
- PIDCommand(const wpi::Twine& name, double p, double i, double d,
- Subsystem& subsystem);
- PIDCommand(const wpi::Twine& name, double p, double i, double d,
- double period, Subsystem& subsystem);
- PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
- double period, Subsystem& subsystem);
- PIDCommand(double p, double i, double d, Subsystem& subsystem);
- PIDCommand(double p, double i, double d, double period, Subsystem& subsystem);
- PIDCommand(double p, double i, double d, double f, double period,
- Subsystem& subsystem);
- virtual ~PIDCommand() = default;
-
- PIDCommand(PIDCommand&&) = default;
- PIDCommand& operator=(PIDCommand&&) = default;
-
- void SetSetpointRelative(double deltaSetpoint);
-
- // PIDOutput interface
- void PIDWrite(double output) override;
-
- // PIDSource interface
- double PIDGet() override;
-
- protected:
- std::shared_ptr<PIDController> GetPIDController() const;
- void _Initialize() override;
- void _Interrupted() override;
- void _End() override;
- void SetSetpoint(double setpoint);
- double GetSetpoint() const;
- double GetPosition();
-
- virtual double ReturnPIDInput() = 0;
- virtual void UsePIDOutput(double output) = 0;
-
- private:
- // The internal PIDController
- std::shared_ptr<PIDController> m_controller;
-
- public:
- void InitSendable(SendableBuilder& builder) override;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/PIDSubsystem.h b/wpilibc/src/main/native/include/frc/commands/PIDSubsystem.h
deleted file mode 100644
index 1965492..0000000
--- a/wpilibc/src/main/native/include/frc/commands/PIDSubsystem.h
+++ /dev/null
@@ -1,236 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <wpi/Twine.h>
-
-#include "frc/PIDController.h"
-#include "frc/PIDOutput.h"
-#include "frc/PIDSource.h"
-#include "frc/commands/Subsystem.h"
-
-namespace frc {
-
-/**
- * This class is designed to handle the case where there is a Subsystem which
- * uses a single PIDController almost constantly (for instance, an elevator
- * which attempts to stay at a constant height).
- *
- * It provides some convenience methods to run an internal PIDController. It
- * also allows access to the internal PIDController in order to give total
- * control to the programmer.
- */
-class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
- public:
- /**
- * Instantiates a PIDSubsystem that will use the given P, I, and D values.
- *
- * @param name the name
- * @param p the proportional value
- * @param i the integral value
- * @param d the derivative value
- */
- PIDSubsystem(const wpi::Twine& name, double p, double i, double d);
-
- /**
- * Instantiates a PIDSubsystem that will use the given P, I, and D values.
- *
- * @param name the name
- * @param p the proportional value
- * @param i the integral value
- * @param d the derivative value
- * @param f the feedforward value
- */
- PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f);
-
- /**
- * Instantiates a PIDSubsystem that will use the given P, I, and D values.
- *
- * It will also space the time between PID loop calculations to be equal to
- * the given period.
- *
- * @param name the name
- * @param p the proportional value
- * @param i the integral value
- * @param d the derivative value
- * @param f the feedfoward value
- * @param period the time (in seconds) between calculations
- */
- PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f,
- double period);
-
- /**
- * Instantiates a PIDSubsystem that will use the given P, I, and D values.
- *
- * It will use the class name as its name.
- *
- * @param p the proportional value
- * @param i the integral value
- * @param d the derivative value
- */
- PIDSubsystem(double p, double i, double d);
-
- /**
- * Instantiates a PIDSubsystem that will use the given P, I, and D values.
- *
- * It will use the class name as its name.
- *
- * @param p the proportional value
- * @param i the integral value
- * @param d the derivative value
- * @param f the feedforward value
- */
- PIDSubsystem(double p, double i, double d, double f);
-
- /**
- * Instantiates a PIDSubsystem that will use the given P, I, and D values.
- *
- * It will use the class name as its name. It will also space the time
- * between PID loop calculations to be equal to the given period.
- *
- * @param p the proportional value
- * @param i the integral value
- * @param d the derivative value
- * @param f the feedforward value
- * @param period the time (in seconds) between calculations
- */
- PIDSubsystem(double p, double i, double d, double f, double period);
-
- ~PIDSubsystem() override = default;
-
- PIDSubsystem(PIDSubsystem&&) = default;
- PIDSubsystem& operator=(PIDSubsystem&&) = default;
-
- /**
- * Enables the internal PIDController.
- */
- void Enable();
-
- /**
- * Disables the internal PIDController.
- */
- void Disable();
-
- // PIDOutput interface
- void PIDWrite(double output) override;
-
- // PIDSource interface
-
- double PIDGet() override;
-
- /**
- * Sets the setpoint to the given value.
- *
- * If SetRange() was called, then the given setpoint will be trimmed to fit
- * within the range.
- *
- * @param setpoint the new setpoint
- */
- void SetSetpoint(double setpoint);
-
- /**
- * Adds the given value to the setpoint.
- *
- * If SetRange() was used, then the bounds will still be honored by this
- * method.
- *
- * @param deltaSetpoint the change in the setpoint
- */
- void SetSetpointRelative(double deltaSetpoint);
-
- /**
- * Sets the maximum and minimum values expected from the input.
- *
- * @param minimumInput the minimum value expected from the input
- * @param maximumInput the maximum value expected from the output
- */
- void SetInputRange(double minimumInput, double maximumInput);
-
- /**
- * Sets the maximum and minimum values to write.
- *
- * @param minimumOutput the minimum value to write to the output
- * @param maximumOutput the maximum value to write to the output
- */
- void SetOutputRange(double minimumOutput, double maximumOutput);
-
- /**
- * Return the current setpoint.
- *
- * @return The current setpoint
- */
- double GetSetpoint();
-
- /**
- * Returns the current position.
- *
- * @return the current position
- */
- double GetPosition();
-
- /**
- * Returns the current rate.
- *
- * @return the current rate
- */
- double GetRate();
-
- /**
- * Set the absolute error which is considered tolerable for use with
- * OnTarget.
- *
- * @param absValue absolute error which is tolerable
- */
- virtual void SetAbsoluteTolerance(double absValue);
-
- /**
- * Set the percentage error which is considered tolerable for use with
- * OnTarget().
- *
- * @param percent percentage error which is tolerable
- */
- virtual void SetPercentTolerance(double percent);
-
- /**
- * Return true if the error is within the percentage of the total input range,
- * determined by SetTolerance().
- *
- * This asssumes that the maximum and minimum input were set using SetInput().
- * Use OnTarget() in the IsFinished() method of commands that use this
- * subsystem.
- *
- * Currently this just reports on target as the actual value passes through
- * the setpoint. Ideally it should be based on being within the tolerance for
- * some period of time.
- *
- * @return True if the error is within the percentage tolerance of the input
- * range
- */
- virtual bool OnTarget() const;
-
- protected:
- /**
- * Returns the PIDController used by this PIDSubsystem.
- *
- * Use this if you would like to fine tune the PID loop.
- *
- * @return The PIDController used by this PIDSubsystem
- */
- std::shared_ptr<PIDController> GetPIDController();
-
- virtual double ReturnPIDInput() = 0;
- virtual void UsePIDOutput(double output) = 0;
-
- private:
- // The internal PIDController
- std::shared_ptr<PIDController> m_controller;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/PrintCommand.h b/wpilibc/src/main/native/include/frc/commands/PrintCommand.h
deleted file mode 100644
index 13156c9..0000000
--- a/wpilibc/src/main/native/include/frc/commands/PrintCommand.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <string>
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/InstantCommand.h"
-
-namespace frc {
-
-class PrintCommand : public InstantCommand {
- public:
- explicit PrintCommand(const wpi::Twine& message);
- virtual ~PrintCommand() = default;
-
- PrintCommand(PrintCommand&&) = default;
- PrintCommand& operator=(PrintCommand&&) = default;
-
- protected:
- virtual void Initialize();
-
- private:
- std::string m_message;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/Scheduler.h b/wpilibc/src/main/native/include/frc/commands/Scheduler.h
deleted file mode 100644
index d15ea39..0000000
--- a/wpilibc/src/main/native/include/frc/commands/Scheduler.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class ButtonScheduler;
-class Command;
-class Subsystem;
-
-class Scheduler : public ErrorBase,
- public Sendable,
- public SendableHelper<Scheduler> {
- public:
- /**
- * Returns the Scheduler, creating it if one does not exist.
- *
- * @return the Scheduler
- */
- static Scheduler* GetInstance();
-
- /**
- * Add a command to be scheduled later.
- *
- * In any pass through the scheduler, all commands are added to the additions
- * list, then at the end of the pass, they are all scheduled.
- *
- * @param command The command to be scheduled
- */
- void AddCommand(Command* command);
-
- void AddButton(ButtonScheduler* button);
-
- /**
- * Registers a Subsystem to this Scheduler, so that the Scheduler might know
- * if a default Command needs to be run.
- *
- * All Subsystems should call this.
- *
- * @param system the system
- */
- void RegisterSubsystem(Subsystem* subsystem);
-
- /**
- * Runs a single iteration of the loop.
- *
- * This method should be called often in order to have a functioning
- * Command system. The loop has five stages:
- *
- * <ol>
- * <li>Poll the Buttons</li>
- * <li>Execute/Remove the Commands</li>
- * <li>Send values to SmartDashboard</li>
- * <li>Add Commands</li>
- * <li>Add Defaults</li>
- * </ol>
- */
- void Run();
-
- /**
- * Removes the Command from the Scheduler.
- *
- * @param command the command to remove
- */
- void Remove(Command* command);
-
- void RemoveAll();
-
- /**
- * Completely resets the scheduler. Undefined behavior if running.
- */
- void ResetAll();
-
- void SetEnabled(bool enabled);
-
- void InitSendable(SendableBuilder& builder) override;
-
- private:
- Scheduler();
- ~Scheduler() override;
-
- Scheduler(Scheduler&&) = default;
- Scheduler& operator=(Scheduler&&) = default;
-
- struct Impl;
- std::unique_ptr<Impl> m_impl;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/StartCommand.h b/wpilibc/src/main/native/include/frc/commands/StartCommand.h
deleted file mode 100644
index 4f0b676..0000000
--- a/wpilibc/src/main/native/include/frc/commands/StartCommand.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/commands/InstantCommand.h"
-
-namespace frc {
-
-class StartCommand : public InstantCommand {
- public:
- explicit StartCommand(Command* commandToStart);
- virtual ~StartCommand() = default;
-
- StartCommand(StartCommand&&) = default;
- StartCommand& operator=(StartCommand&&) = default;
-
- protected:
- virtual void Initialize();
-
- private:
- Command* m_commandToFork;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/Subsystem.h b/wpilibc/src/main/native/include/frc/commands/Subsystem.h
deleted file mode 100644
index 2637dd0..0000000
--- a/wpilibc/src/main/native/include/frc/commands/Subsystem.h
+++ /dev/null
@@ -1,199 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <string>
-
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class Command;
-
-class Subsystem : public ErrorBase,
- public Sendable,
- public SendableHelper<Subsystem> {
- friend class Scheduler;
-
- public:
- /**
- * Creates a subsystem with the given name.
- *
- * @param name the name of the subsystem
- */
- explicit Subsystem(const wpi::Twine& name);
-
- Subsystem(Subsystem&&) = default;
- Subsystem& operator=(Subsystem&&) = default;
-
- /**
- * Sets the default command. If this is not called or is called with null,
- * then there will be no default command for the subsystem.
- *
- * <b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the
- * subsystem is a singleton.
- *
- * @param command the default command (or null if there should be none)
- */
- void SetDefaultCommand(Command* command);
-
- /**
- * Returns the default command (or null if there is none).
- *
- * @return the default command
- */
- Command* GetDefaultCommand();
-
- /**
- * Returns the default command name, or empty string is there is none.
- *
- * @return the default command name
- */
- wpi::StringRef GetDefaultCommandName();
-
- /**
- * Sets the current command.
- *
- * @param command the new current command
- */
- void SetCurrentCommand(Command* command);
-
- /**
- * Returns the command which currently claims this subsystem.
- *
- * @return the command which currently claims this subsystem
- */
- Command* GetCurrentCommand() const;
-
- /**
- * Returns the current command name, or empty string if no current command.
- *
- * @return the current command name
- */
- wpi::StringRef GetCurrentCommandName() const;
-
- /**
- * When the run method of the scheduler is called this method will be called.
- */
- virtual void Periodic();
-
- /**
- * Initialize the default command for this subsystem.
- *
- * This is meant to be the place to call SetDefaultCommand in a subsystem and
- * will be called on all the subsystems by the CommandBase method before the
- * program starts running by using the list of all registered Subsystems
- * inside the Scheduler.
- *
- * This should be overridden by a Subsystem that has a default Command
- */
- virtual void InitDefaultCommand();
-
- /**
- * Gets the name of this Subsystem.
- *
- * @return Name
- */
- std::string GetName() const;
-
- /**
- * Sets the name of this Subsystem.
- *
- * @param name name
- */
- void SetName(const wpi::Twine& name);
-
- /**
- * Gets the subsystem name of this Subsystem.
- *
- * @return Subsystem name
- */
- std::string GetSubsystem() const;
-
- /**
- * Sets the subsystem name of this Subsystem.
- *
- * @param subsystem subsystem name
- */
- void SetSubsystem(const wpi::Twine& subsystem);
-
- /**
- * Associate a Sendable with this Subsystem.
- * Also update the child's name.
- *
- * @param name name to give child
- * @param child sendable
- */
- void AddChild(const wpi::Twine& name, std::shared_ptr<Sendable> child);
-
- /**
- * Associate a Sendable with this Subsystem.
- * Also update the child's name.
- *
- * @param name name to give child
- * @param child sendable
- */
- void AddChild(const wpi::Twine& name, Sendable* child);
-
- /**
- * Associate a Sendable with this Subsystem.
- * Also update the child's name.
- *
- * @param name name to give child
- * @param child sendable
- */
- void AddChild(const wpi::Twine& name, Sendable& child);
-
- /**
- * Associate a {@link Sendable} with this Subsystem.
- *
- * @param child sendable
- */
- void AddChild(std::shared_ptr<Sendable> child);
-
- /**
- * Associate a {@link Sendable} with this Subsystem.
- *
- * @param child sendable
- */
- void AddChild(Sendable* child);
-
- /**
- * Associate a {@link Sendable} with this Subsystem.
- *
- * @param child sendable
- */
- void AddChild(Sendable& child);
-
- private:
- /**
- * Call this to alert Subsystem that the current command is actually the
- * command.
- *
- * Sometimes, the Subsystem is told that it has no command while the Scheduler
- * is going through the loop, only to be soon after given a new one. This will
- * avoid that situation.
- */
- void ConfirmCommand();
-
- Command* m_currentCommand = nullptr;
- bool m_currentCommandChanged = true;
- Command* m_defaultCommand = nullptr;
- bool m_initializedDefaultCommand = false;
-
- public:
- void InitSendable(SendableBuilder& builder) override;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/TimedCommand.h b/wpilibc/src/main/native/include/frc/commands/TimedCommand.h
deleted file mode 100644
index d2be010..0000000
--- a/wpilibc/src/main/native/include/frc/commands/TimedCommand.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-/**
- * A TimedCommand will wait for a timeout before finishing.
- *
- * TimedCommand is used to execute a command for a given amount of time.
- */
-class TimedCommand : public Command {
- public:
- /**
- * Creates a new TimedCommand with the given name and timeout.
- *
- * @param name the name of the command
- * @param timeout the time (in seconds) before this command "times out"
- */
- TimedCommand(const wpi::Twine& name, double timeout);
-
- /**
- * Creates a new WaitCommand with the given timeout.
- *
- * @param timeout the time (in seconds) before this command "times out"
- */
- explicit TimedCommand(double timeout);
-
- /**
- * Creates a new TimedCommand with the given name and timeout.
- *
- * @param name the name of the command
- * @param timeout the time (in seconds) before this command "times out"
- * @param subsystem the subsystem that the command requires
- */
- TimedCommand(const wpi::Twine& name, double timeout, Subsystem& subsystem);
-
- /**
- * Creates a new WaitCommand with the given timeout.
- *
- * @param timeout the time (in seconds) before this command "times out"
- * @param subsystem the subsystem that the command requires
- */
- TimedCommand(double timeout, Subsystem& subsystem);
-
- virtual ~TimedCommand() = default;
-
- TimedCommand(TimedCommand&&) = default;
- TimedCommand& operator=(TimedCommand&&) = default;
-
- protected:
- /**
- * Ends command when timed out.
- */
- bool IsFinished() override;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/WaitCommand.h b/wpilibc/src/main/native/include/frc/commands/WaitCommand.h
deleted file mode 100644
index 481b1c6..0000000
--- a/wpilibc/src/main/native/include/frc/commands/WaitCommand.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/TimedCommand.h"
-
-namespace frc {
-
-class WaitCommand : public TimedCommand {
- public:
- /**
- * Creates a new WaitCommand with the given name and timeout.
- *
- * @param name the name of the command
- * @param timeout the time (in seconds) before this command "times out"
- */
- explicit WaitCommand(double timeout);
-
- /**
- * Creates a new WaitCommand with the given timeout.
- *
- * @param timeout the time (in seconds) before this command "times out"
- */
- WaitCommand(const wpi::Twine& name, double timeout);
-
- virtual ~WaitCommand() = default;
-
- WaitCommand(WaitCommand&&) = default;
- WaitCommand& operator=(WaitCommand&&) = default;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/WaitForChildren.h b/wpilibc/src/main/native/include/frc/commands/WaitForChildren.h
deleted file mode 100644
index 6f2e285..0000000
--- a/wpilibc/src/main/native/include/frc/commands/WaitForChildren.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-class WaitForChildren : public Command {
- public:
- explicit WaitForChildren(double timeout);
- WaitForChildren(const wpi::Twine& name, double timeout);
- virtual ~WaitForChildren() = default;
-
- WaitForChildren(WaitForChildren&&) = default;
- WaitForChildren& operator=(WaitForChildren&&) = default;
-
- protected:
- virtual bool IsFinished();
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/WaitUntilCommand.h b/wpilibc/src/main/native/include/frc/commands/WaitUntilCommand.h
deleted file mode 100644
index 1231750..0000000
--- a/wpilibc/src/main/native/include/frc/commands/WaitUntilCommand.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-class WaitUntilCommand : public Command {
- public:
- /**
- * A WaitCommand will wait until a certain match time before finishing.
- *
- * This will wait until the game clock reaches some value, then continue to
- * the next command.
- *
- * @see CommandGroup
- */
- explicit WaitUntilCommand(double time);
-
- WaitUntilCommand(const wpi::Twine& name, double time);
-
- virtual ~WaitUntilCommand() = default;
-
- WaitUntilCommand(WaitUntilCommand&&) = default;
- WaitUntilCommand& operator=(WaitUntilCommand&&) = default;
-
- protected:
- /**
- * Check if we've reached the actual finish time.
- */
- virtual bool IsFinished();
-
- private:
- double m_time;
-};
-
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
new file mode 100644
index 0000000..6f6e086
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -0,0 +1,149 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/MathExtras.h>
+
+namespace frc {
+/**
+ * A helper class that computes feedforward outputs for a simple arm (modeled as
+ * a motor acting against the force of gravity on a beam suspended at an angle).
+ */
+class ArmFeedforward {
+ using Angle = units::radians;
+ using Velocity = units::radians_per_second;
+ using Acceleration = units::compound_unit<units::radians_per_second,
+ units::inverse<units::second>>;
+ using kv_unit =
+ units::compound_unit<units::volts,
+ units::inverse<units::radians_per_second>>;
+ using ka_unit =
+ units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+ public:
+ constexpr ArmFeedforward() = default;
+
+ /**
+ * Creates a new ArmFeedforward with the specified gains.
+ *
+ * @param kS The static gain, in volts.
+ * @param kCos The gravity gain, in volts.
+ * @param kV The velocity gain, in volt seconds per radian.
+ * @param kA The acceleration gain, in volt seconds^2 per radian.
+ */
+ constexpr ArmFeedforward(
+ units::volt_t kS, units::volt_t kCos, units::unit_t<kv_unit> kV,
+ units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+ : kS(kS), kCos(kCos), kV(kV), kA(kA) {}
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param angle The angle setpoint, in radians.
+ * @param velocity The velocity setpoint, in radians per second.
+ * @param acceleration The acceleration setpoint, in radians per second^2.
+ * @return The computed feedforward, in volts.
+ */
+ units::volt_t Calculate(units::unit_t<Angle> angle,
+ units::unit_t<Velocity> velocity,
+ units::unit_t<Acceleration> acceleration =
+ units::unit_t<Acceleration>(0)) const {
+ return kS * wpi::sgn(velocity) + kCos * units::math::cos(angle) +
+ kV * velocity + kA * acceleration;
+ }
+
+ // Rearranging the main equation from the calculate() method yields the
+ // formulas for the methods below:
+
+ /**
+ * Calculates the maximum achievable velocity given a maximum voltage supply,
+ * a position, and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the arm.
+ * @param angle The angle of the arm
+ * @param acceleration The acceleration of the arm.
+ * @return The maximum possible velocity at the given acceleration and angle.
+ */
+ units::unit_t<Velocity> MaxAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Angle> angle,
+ units::unit_t<Acceleration> acceleration) {
+ // Assume max velocity is positive
+ return (maxVoltage - kS - kCos * units::math::cos(angle) -
+ kA * acceleration) /
+ kV;
+ }
+
+ /**
+ * Calculates the minimum achievable velocity given a maximum voltage supply,
+ * a position, and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the arm.
+ * @param angle The angle of the arm
+ * @param acceleration The acceleration of the arm.
+ * @return The minimum possible velocity at the given acceleration and angle.
+ */
+ units::unit_t<Velocity> MinAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Angle> angle,
+ units::unit_t<Acceleration> acceleration) {
+ // Assume min velocity is negative, ks flips sign
+ return (-maxVoltage + kS - kCos * units::math::cos(angle) -
+ kA * acceleration) /
+ kV;
+ }
+
+ /**
+ * Calculates the maximum achievable acceleration given a maximum voltage
+ * supply, a position, and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the arm.
+ * @param angle The angle of the arm
+ * @param velocity The velocity of the arm.
+ * @return The maximum possible acceleration at the given velocity and angle.
+ */
+ units::unit_t<Acceleration> MaxAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Angle> angle,
+ units::unit_t<Velocity> velocity) {
+ return (maxVoltage - kS * wpi::sgn(velocity) -
+ kCos * units::math::cos(angle) - kV * velocity) /
+ kA;
+ }
+
+ /**
+ * Calculates the minimum achievable acceleration given a maximum voltage
+ * supply, a position, and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the arm.
+ * @param angle The angle of the arm
+ * @param velocity The velocity of the arm.
+ * @return The minimum possible acceleration at the given velocity and angle.
+ */
+ units::unit_t<Acceleration> MinAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Angle> angle,
+ units::unit_t<Velocity> velocity) {
+ return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
+ }
+
+ units::volt_t kS{0};
+ units::volt_t kCos{0};
+ units::unit_t<kv_unit> kV{0};
+ units::unit_t<ka_unit> kA{0};
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
new file mode 100644
index 0000000..c664fc4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/MathExtras.h>
+
+namespace frc {
+/**
+ * A helper class that computes feedforward outputs for a simple elevator
+ * (modeled as a motor acting against the force of gravity).
+ */
+template <class Distance>
+class ElevatorFeedforward {
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Acceleration =
+ units::compound_unit<Velocity, units::inverse<units::seconds>>;
+ using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
+ using ka_unit =
+ units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+ public:
+ ElevatorFeedforward() = default;
+
+ /**
+ * Creates a new ElevatorFeedforward with the specified gains.
+ *
+ * @param kS The static gain, in volts.
+ * @param kG The gravity gain, in volts.
+ * @param kV The velocity gain, in volt seconds per distance.
+ * @param kA The acceleration gain, in volt seconds^2 per distance.
+ */
+ constexpr ElevatorFeedforward(
+ units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
+ units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+ : kS(kS), kG(kG), kV(kV), kA(kA) {}
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param velocity The velocity setpoint, in distance per second.
+ * @param acceleration The acceleration setpoint, in distance per second^2.
+ * @return The computed feedforward, in volts.
+ */
+ constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
+ units::unit_t<Acceleration> acceleration =
+ units::unit_t<Acceleration>(0)) {
+ return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
+ }
+
+ // Rearranging the main equation from the calculate() method yields the
+ // formulas for the methods below:
+
+ /**
+ * Calculates the maximum achievable velocity given a maximum voltage supply
+ * and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+ * @param acceleration The acceleration of the elevator.
+ * @return The maximum possible velocity at the given acceleration.
+ */
+ constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+ // Assume max velocity is positive
+ return (maxVoltage - kS - kG - kA * acceleration) / kV;
+ }
+
+ /**
+ * Calculates the minimum achievable velocity given a maximum voltage supply
+ * and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+ * @param acceleration The acceleration of the elevator.
+ * @return The minimum possible velocity at the given acceleration.
+ */
+ constexpr units::unit_t<Velocity> MinAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+ // Assume min velocity is negative, ks flips sign
+ return (-maxVoltage + kS - kG - kA * acceleration) / kV;
+ }
+
+ /**
+ * Calculates the maximum achievable acceleration given a maximum voltage
+ * supply and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+ * @param velocity The velocity of the elevator.
+ * @return The maximum possible acceleration at the given velocity.
+ */
+ constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+ return (maxVoltage - kS * wpi::sgn(velocity) - kG - kV * velocity) / kA;
+ }
+
+ /**
+ * Calculates the minimum achievable acceleration given a maximum voltage
+ * supply and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+ * @param velocity The velocity of the elevator.
+ * @return The minimum possible acceleration at the given velocity.
+ */
+ constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+ return MaxAchievableAcceleration(-maxVoltage, velocity);
+ }
+
+ units::volt_t kS{0};
+ units::volt_t kG{0};
+ units::unit_t<kv_unit> kV{0};
+ units::unit_t<ka_unit> kA{0};
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
index 64da9eb..96fd331 100644
--- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -7,6 +7,8 @@
#pragma once
+#include <algorithm>
+#include <cmath>
#include <functional>
#include <limits>
@@ -14,6 +16,7 @@
#include "frc/controller/PIDController.h"
#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilder.h"
#include "frc/smartdashboard/SendableHelper.h"
#include "frc/trajectory/TrapezoidProfile.h"
@@ -23,8 +26,20 @@
* Implements a PID control loop whose setpoint is constrained by a trapezoid
* profile.
*/
-class ProfiledPIDController : public Sendable,
- public SendableHelper<ProfiledPIDController> {
+template <class Distance>
+class ProfiledPIDController
+ : public Sendable,
+ public SendableHelper<ProfiledPIDController<Distance>> {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using Acceleration =
+ units::compound_unit<Velocity, units::inverse<units::seconds>>;
+ using Acceleration_t = units::unit_t<Acceleration>;
+ using State = typename TrapezoidProfile<Distance>::State;
+ using Constraints = typename TrapezoidProfile<Distance>::Constraints;
+
public:
/**
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
@@ -38,8 +53,8 @@
* default is 20 milliseconds.
*/
ProfiledPIDController(double Kp, double Ki, double Kd,
- frc::TrapezoidProfile::Constraints constraints,
- units::second_t period = 20_ms);
+ Constraints constraints, units::second_t period = 20_ms)
+ : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
~ProfiledPIDController() override = default;
@@ -57,96 +72,98 @@
* @param Ki Integral coefficient
* @param Kd Differential coefficient
*/
- void SetPID(double Kp, double Ki, double Kd);
+ void SetPID(double Kp, double Ki, double Kd) {
+ m_controller.SetPID(Kp, Ki, Kd);
+ }
/**
* Sets the proportional coefficient of the PID controller gain.
*
* @param Kp proportional coefficient
*/
- void SetP(double Kp);
+ void SetP(double Kp) { m_controller.SetP(Kp); }
/**
* Sets the integral coefficient of the PID controller gain.
*
* @param Ki integral coefficient
*/
- void SetI(double Ki);
+ void SetI(double Ki) { m_controller.SetI(Ki); }
/**
* Sets the differential coefficient of the PID controller gain.
*
* @param Kd differential coefficient
*/
- void SetD(double Kd);
+ void SetD(double Kd) { m_controller.SetD(Kd); }
/**
* Gets the proportional coefficient.
*
* @return proportional coefficient
*/
- double GetP() const;
+ double GetP() const { return m_controller.GetP(); }
/**
* Gets the integral coefficient.
*
* @return integral coefficient
*/
- double GetI() const;
+ double GetI() const { return m_controller.GetI(); }
/**
* Gets the differential coefficient.
*
* @return differential coefficient
*/
- double GetD() const;
+ double GetD() const { return m_controller.GetD(); }
/**
* Gets the period of this controller.
*
* @return The period of the controller.
*/
- units::second_t GetPeriod() const;
+ units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
/**
* Sets the goal for the ProfiledPIDController.
*
* @param goal The desired unprofiled setpoint.
*/
- void SetGoal(TrapezoidProfile::State goal);
+ void SetGoal(State goal) { m_goal = goal; }
/**
* Sets the goal for the ProfiledPIDController.
*
* @param goal The desired unprofiled setpoint.
*/
- void SetGoal(units::meter_t goal);
+ void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
/**
* Gets the goal for the ProfiledPIDController.
*/
- TrapezoidProfile::State GetGoal() const;
+ State GetGoal() const { return m_goal; }
/**
* Returns true if the error is within the tolerance of the error.
*
* This will return false until at least one input value has been computed.
*/
- bool AtGoal() const;
+ bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
/**
* Set velocity and acceleration constraints for goal.
*
* @param constraints Velocity and acceleration constraints for goal.
*/
- void SetConstraints(frc::TrapezoidProfile::Constraints constraints);
+ void SetConstraints(Constraints constraints) { m_constraints = constraints; }
/**
* Returns the current setpoint of the ProfiledPIDController.
*
* @return The current setpoint.
*/
- TrapezoidProfile::State GetSetpoint() const;
+ State GetSetpoint() const { return m_setpoint; }
/**
* Returns true if the error is within the tolerance of the error.
@@ -157,7 +174,7 @@
*
* This will return false until at least one input value has been computed.
*/
- bool AtSetpoint() const;
+ bool AtSetpoint() const { return m_controller.AtSetpoint(); }
/**
* Enables continuous input.
@@ -169,12 +186,15 @@
* @param minimumInput The minimum value expected from the input.
* @param maximumInput The maximum value expected from the input.
*/
- void EnableContinuousInput(double minimumInput, double maximumInput);
+ void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
+ m_controller.EnableContinuousInput(minimumInput.template to<double>(),
+ maximumInput.template to<double>());
+ }
/**
* Disables continuous input.
*/
- void DisableContinuousInput();
+ void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
/**
* Sets the minimum and maximum values for the integrator.
@@ -185,7 +205,9 @@
* @param minimumIntegral The minimum value of the integrator.
* @param maximumIntegral The maximum value of the integrator.
*/
- void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
+ void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
+ m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
+ }
/**
* Sets the error which is considered tolerable for use with
@@ -195,27 +217,39 @@
* @param velocityTolerance Velocity error which is tolerable.
*/
void SetTolerance(
- double positionTolerance,
- double velocityTolerance = std::numeric_limits<double>::infinity());
+ Distance_t positionTolerance,
+ Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
+ m_controller.SetTolerance(positionTolerance.template to<double>(),
+ velocityTolerance.template to<double>());
+ }
/**
* Returns the difference between the setpoint and the measurement.
*
* @return The error.
*/
- double GetPositionError() const;
+ Distance_t GetPositionError() const {
+ return Distance_t(m_controller.GetPositionError());
+ }
/**
* Returns the change in error per second.
*/
- double GetVelocityError() const;
+ Velocity_t GetVelocityError() const {
+ return Velocity_t(m_controller.GetVelocityError());
+ }
/**
* Returns the next output of the PID controller.
*
* @param measurement The current measurement of the process variable.
*/
- double Calculate(units::meter_t measurement);
+ double Calculate(Distance_t measurement) {
+ frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
+ m_setpoint = profile.Calculate(GetPeriod());
+ return m_controller.Calculate(measurement.template to<double>(),
+ m_setpoint.position.template to<double>());
+ }
/**
* Returns the next output of the PID controller.
@@ -223,15 +257,20 @@
* @param measurement The current measurement of the process variable.
* @param goal The new goal of the controller.
*/
- double Calculate(units::meter_t measurement, TrapezoidProfile::State goal);
-
+ double Calculate(Distance_t measurement, State goal) {
+ SetGoal(goal);
+ return Calculate(measurement);
+ }
/**
* Returns the next output of the PID controller.
*
* @param measurement The current measurement of the process variable.
* @param goal The new goal of the controller.
*/
- double Calculate(units::meter_t measurement, units::meter_t goal);
+ double Calculate(Distance_t measurement, Distance_t goal) {
+ SetGoal(goal);
+ return Calculate(measurement);
+ }
/**
* Returns the next output of the PID controller.
@@ -240,21 +279,36 @@
* @param goal The new goal of the controller.
* @param constraints Velocity and acceleration constraints for goal.
*/
- double Calculate(units::meter_t measurement, units::meter_t goal,
- frc::TrapezoidProfile::Constraints constraints);
+ double Calculate(
+ Distance_t measurement, Distance_t goal,
+ typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
+ SetConstraints(constraints);
+ return Calculate(measurement, goal);
+ }
/**
* Reset the previous error, the integral term, and disable the controller.
*/
- void Reset();
+ void Reset() { m_controller.Reset(); }
- void InitSendable(frc::SendableBuilder& builder) override;
+ void InitSendable(frc::SendableBuilder& builder) override {
+ 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.template to<double>(); },
+ [this](double value) { SetGoal(Distance_t{value}); });
+ }
private:
frc2::PIDController m_controller;
- frc::TrapezoidProfile::State m_goal;
- frc::TrapezoidProfile::State m_setpoint;
- frc::TrapezoidProfile::Constraints m_constraints;
+ typename frc::TrapezoidProfile<Distance>::State m_goal;
+ typename frc::TrapezoidProfile<Distance>::State m_setpoint;
+ typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/RamseteController.h b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
index 3a564aa..6ec6edc 100644
--- a/wpilibc/src/main/native/include/frc/controller/RamseteController.h
+++ b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
@@ -55,6 +55,13 @@
RamseteController(double b, double zeta);
/**
+ * Construct a Ramsete unicycle controller. The default arguments for
+ * b and zeta of 2.0 and 0.7 have been well-tested to produce desireable
+ * results.
+ */
+ RamseteController() : RamseteController(2.0, 0.7) {}
+
+ /**
* Returns true if the pose error is within tolerance of the reference.
*/
bool AtReference() const;
diff --git a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
new file mode 100644
index 0000000..8f82cb9
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/MathExtras.h>
+
+namespace frc {
+/**
+ * A helper class that computes feedforward voltages for a simple
+ * permanent-magnet DC motor.
+ */
+template <class Distance>
+class SimpleMotorFeedforward {
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Acceleration =
+ units::compound_unit<Velocity, units::inverse<units::seconds>>;
+ using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
+ using ka_unit =
+ units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+ public:
+ constexpr SimpleMotorFeedforward() = default;
+
+ /**
+ * Creates a new SimpleMotorFeedforward with the specified gains.
+ *
+ * @param kS The static gain, in volts.
+ * @param kV The velocity gain, in volt seconds per distance.
+ * @param kA The acceleration gain, in volt seconds^2 per distance.
+ */
+ constexpr SimpleMotorFeedforward(
+ units::volt_t kS, units::unit_t<kv_unit> kV,
+ units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+ : kS(kS), kV(kV), kA(kA) {}
+
+ /**
+ * Calculates the feedforward from the gains and setpoints.
+ *
+ * @param velocity The velocity setpoint, in distance per second.
+ * @param acceleration The acceleration setpoint, in distance per second^2.
+ * @return The computed feedforward, in volts.
+ */
+ constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
+ units::unit_t<Acceleration> acceleration =
+ units::unit_t<Acceleration>(0)) const {
+ return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
+ }
+
+ // Rearranging the main equation from the calculate() method yields the
+ // formulas for the methods below:
+
+ /**
+ * Calculates the maximum achievable velocity given a maximum voltage supply
+ * and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param acceleration The acceleration of the motor.
+ * @return The maximum possible velocity at the given acceleration.
+ */
+ constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+ // Assume max velocity is positive
+ return (maxVoltage - kS - kA * acceleration) / kV;
+ }
+
+ /**
+ * Calculates the minimum achievable velocity given a maximum voltage supply
+ * and an acceleration. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the acceleration constraint, and this will give you
+ * a simultaneously-achievable velocity constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param acceleration The acceleration of the motor.
+ * @return The minimum possible velocity at the given acceleration.
+ */
+ constexpr units::unit_t<Velocity> MinAchievableVelocity(
+ units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+ // Assume min velocity is positive, ks flips sign
+ return (-maxVoltage + kS - kA * acceleration) / kV;
+ }
+
+ /**
+ * Calculates the maximum achievable acceleration given a maximum voltage
+ * supply and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param velocity The velocity of the motor.
+ * @return The maximum possible acceleration at the given velocity.
+ */
+ constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+ return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
+ }
+
+ /**
+ * Calculates the minimum achievable acceleration given a maximum voltage
+ * supply and a velocity. Useful for ensuring that velocity and
+ * acceleration constraints for a trapezoidal profile are simultaneously
+ * achievable - enter the velocity constraint, and this will give you
+ * a simultaneously-achievable acceleration constraint.
+ *
+ * @param maxVoltage The maximum voltage that can be supplied to the motor.
+ * @param velocity The velocity of the motor.
+ * @return The minimum possible acceleration at the given velocity.
+ */
+ constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+ units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+ return MaxAchievableAcceleration(-maxVoltage, velocity);
+ }
+
+ units::volt_t kS{0};
+ units::unit_t<kv_unit> kV{0};
+ units::unit_t<ka_unit> kA{0};
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
index 192d21b..c6b705a 100644
--- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
@@ -30,12 +30,12 @@
* @code{.cpp}
* class Robot {
* public:
- * frc::Spark m_frontLeft{1};
- * frc::Spark m_rearLeft{2};
+ * frc::PWMVictorSPX m_frontLeft{1};
+ * frc::PWMVictorSPX m_rearLeft{2};
* frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
*
- * frc::Spark m_frontRight{3};
- * frc::Spark m_rearRight{4};
+ * frc::PWMVictorSPX m_frontRight{3};
+ * frc::PWMVictorSPX m_rearRight{4};
* frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
*
* frc::DifferentialDrive m_drive{m_left, m_right};
@@ -46,14 +46,14 @@
* @code{.cpp}
* class Robot {
* public:
- * frc::Spark m_frontLeft{1};
- * frc::Spark m_midLeft{2};
- * frc::Spark m_rearLeft{3};
+ * frc::PWMVictorSPX m_frontLeft{1};
+ * frc::PWMVictorSPX m_midLeft{2};
+ * frc::PWMVictorSPX m_rearLeft{3};
* frc::SpeedControllerGroup m_left{m_frontLeft, m_midLeft, m_rearLeft};
*
- * frc::Spark m_frontRight{4};
- * frc::Spark m_midRight{5};
- * frc::Spark m_rearRight{6};
+ * frc::PWMVictorSPX m_frontRight{4};
+ * frc::PWMVictorSPX m_midRight{5};
+ * frc::PWMVictorSPX m_rearRight{6};
* frc::SpeedControllerGroup m_right{m_frontRight, m_midRight, m_rearRight};
*
* frc::DifferentialDrive m_drive{m_left, m_right};
diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
index 2161e82..46328a0 100644
--- a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
+++ b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
@@ -11,6 +11,10 @@
#include "Translation2d.h"
#include "Twist2d.h"
+namespace wpi {
+class json;
+} // namespace wpi
+
namespace frc {
/**
@@ -146,8 +150,8 @@
*
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
- * 0.01 meters and changes angle by .5 degrees since the previous pose update,
- * the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+ * 0.01 meters and changes angle by 0.5 degrees since the previous pose
+ * update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
*
* @return The new pose of the robot.
*/
@@ -167,4 +171,9 @@
Translation2d m_translation;
Rotation2d m_rotation;
};
+
+void to_json(wpi::json& json, const Pose2d& pose);
+
+void from_json(const wpi::json& json, Pose2d& pose);
+
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
index 9453e0c..b636513 100644
--- a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
@@ -8,7 +8,10 @@
#pragma once
#include <units/units.h>
-#include <wpi/math>
+
+namespace wpi {
+class json;
+} // namespace wpi
namespace frc {
@@ -175,4 +178,9 @@
double m_cos = 1;
double m_sin = 0;
};
+
+void to_json(wpi::json& json, const Rotation2d& rotation);
+
+void from_json(const wpi::json& json, Rotation2d& rotation);
+
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
index a53abf1..90c61e2 100644
--- a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
@@ -11,6 +11,10 @@
#include "Rotation2d.h"
+namespace wpi {
+class json;
+} // namespace wpi
+
namespace frc {
/**
@@ -211,4 +215,9 @@
units::meter_t m_x = 0_m;
units::meter_t m_y = 0_m;
};
+
+void to_json(wpi::json& json, const Translation2d& state);
+
+void from_json(const wpi::json& json, Translation2d& state);
+
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index 34407b9..cb2121f 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -31,7 +31,8 @@
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
- explicit DifferentialDriveKinematics(units::meter_t trackWidth);
+ constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth)
+ : trackWidth(trackWidth) {}
/**
* Returns a chassis speed from left and right component velocities using
@@ -40,8 +41,11 @@
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
*/
- ChassisSpeeds ToChassisSpeeds(
- const DifferentialDriveWheelSpeeds& wheelSpeeds) const;
+ constexpr ChassisSpeeds ToChassisSpeeds(
+ const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
+ return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
+ (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
+ }
/**
* Returns left and right component velocities from a chassis speed using
@@ -51,10 +55,12 @@
* represent the chassis' speed.
* @return The left and right velocities.
*/
- DifferentialDriveWheelSpeeds ToWheelSpeeds(
- const ChassisSpeeds& chassisSpeeds) const;
+ constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds) const {
+ return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
+ chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
+ }
- private:
- units::meter_t m_trackWidth;
+ units::meter_t trackWidth;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index fd41d15..379839d 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -23,29 +23,38 @@
* path following. Furthermore, odometry can be used for latency compensation
* when using computer-vision systems.
*
- * Note: It is important to reset both your encoders to zero before you start
- * using this class. Only reset your encoders ONCE. You should not reset your
- * encoders even if you want to reset your robot's pose.
+ * It is important that you reset your encoders to zero before using this class.
+ * Any subsequent pose resets also require the encoders to be reset to zero.
*/
class DifferentialDriveOdometry {
public:
/**
* Constructs a DifferentialDriveOdometry object.
*
- * @param kinematics The differential drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
- explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
+ explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
+ * You NEED to reset your encoders (to zero) when calling this method.
+ *
+ * The gyroscope angle does not need to be reset here on the user's robot
+ * code. The library automatically takes care of offsetting the gyro angle.
+ *
* @param pose The position on the field that your robot is at.
+ * @param gyroAngle The angle reported by the gyroscope.
*/
- void ResetPosition(const Pose2d& pose) {
+ void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
+
+ m_prevLeftDistance = 0_m;
+ m_prevRightDistance = 0_m;
}
/**
@@ -55,46 +64,26 @@
const Pose2d& GetPose() const { return m_pose; }
/**
- * Updates the robot's position on the field using forward kinematics and
- * integration of the pose over time. This method takes in the current time as
- * a parameter to calculate period (difference between two timestamps). The
- * period is used to calculate the change in distance from a velocity. This
- * also takes in an angle parameter which is used instead of the
- * angular rate that is calculated from forward kinematics.
+ * Updates the robot position on the field using distance measurements from
+ * encoders. This method is more numerically accurate than using velocities to
+ * integrate the pose and is also advantageous for teams that are using lower
+ * CPR encoders.
*
- * @param currentTime The current time.
- * @param angle The angle of the robot.
- * @param wheelSpeeds The current wheel speeds.
- *
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param leftDistance The distance traveled by the left encoder.
+ * @param rightDistance The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
- const Pose2d& UpdateWithTime(units::second_t currentTime,
- const Rotation2d& angle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds);
-
- /**
- * Updates the robot's position on the field using forward kinematics and
- * integration of the pose over time. This method automatically calculates
- * the current time to calculate period (difference between two timestamps).
- * The period is used to calculate the change in distance from a velocity.
- * This also takes in an angle parameter which is used instead of the
- * angular rate that is calculated from forward kinematics.
- *
- * @param angle The angle of the robot.
- * @param wheelSpeeds The current wheel speeds.
- *
- * @return The new pose of the robot.
- */
- const Pose2d& Update(const Rotation2d& angle,
- const DifferentialDriveWheelSpeeds& wheelSpeeds) {
- return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
- }
+ const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+ units::meter_t rightDistance);
private:
- DifferentialDriveKinematics m_kinematics;
Pose2d m_pose;
- units::second_t m_previousTime = -1_s;
+ Rotation2d m_gyroOffset;
Rotation2d m_previousAngle;
+
+ units::meter_t m_prevLeftDistance = 0_m;
+ units::meter_t m_prevRightDistance = 0_m;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
index e8816e1..697a6b0 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -31,19 +31,26 @@
* Constructs a MecanumDriveOdometry object.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+ const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
+ * The gyroscope angle does not need to be reset here on the user's robot
+ * code. The library automatically takes care of offsetting the gyro angle.
+ *
* @param pose The position on the field that your robot is at.
+ * @param gyroAngle The angle reported by the gyroscope.
*/
- void ResetPosition(const Pose2d& pose) {
+ void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
/**
@@ -61,13 +68,13 @@
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
- * @param angle The angle of the robot.
+ * @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
const Pose2d& UpdateWithTime(units::second_t currentTime,
- const Rotation2d& angle,
+ const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds);
/**
@@ -78,14 +85,15 @@
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
- * @param angle The angle of the robot.
- * @param wheelSpeeds The current wheel speeds.
+ * @param gyroAngle The angle reported by the gyroscope.
+ * @param wheelSpeeds The current wheel speeds.
*
* @return The new pose of the robot.
*/
- const Pose2d& Update(const Rotation2d& angle,
+ const Pose2d& Update(const Rotation2d& gyroAngle,
MecanumDriveWheelSpeeds wheelSpeeds) {
- return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
+ return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
+ wheelSpeeds);
}
private:
@@ -94,6 +102,7 @@
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
+ Rotation2d m_gyroOffset;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index fcd8087..1bdbcea 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -122,6 +122,22 @@
ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates);
/**
+ * Performs forward kinematics to return the resulting chassis state from the
+ * given module states. This method is often used for odometry -- determining
+ * the robot's position on the field using data from the real-world speed and
+ * angle of each module on the robot.
+ *
+ * @param moduleStates The state of the modules as an std::array of type
+ * SwerveModuleState, NumModules long as measured from respective encoders
+ * and gyros. The order of the swerve module states should be same as passed
+ * into the constructor of this class.
+ *
+ * @return The resulting chassis speed.
+ */
+ ChassisSpeeds ToChassisSpeeds(
+ std::array<SwerveModuleState, NumModules> moduleStates);
+
+ /**
* Normalizes the wheel speeds using some max attainable speed. Sometimes,
* after inverse kinematics, the requested speed from a/several modules may be
* above the max attainable speed for the driving motor on that module. To fix
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index 138954d..b362aa0 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -63,6 +63,13 @@
"locations provided in constructor.");
std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
+
+ return this->ToChassisSpeeds(moduleStates);
+}
+
+template <size_t NumModules>
+ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
+ std::array<SwerveModuleState, NumModules> moduleStates) {
Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
for (size_t i = 0; i < NumModules; i++) {
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index 1dbe178..8093ca5 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -36,19 +36,26 @@
* Constructs a SwerveDriveOdometry object.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
+ * @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
+ const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d());
/**
* Resets the robot's position on the field.
*
+ * The gyroscope angle does not need to be reset here on the user's robot
+ * code. The library automatically takes care of offsetting the gyro angle.
+ *
* @param pose The position on the field that your robot is at.
+ * @param gyroAngle The angle reported by the gyroscope.
*/
- void ResetPosition(const Pose2d& pose) {
+ void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
/**
@@ -66,7 +73,7 @@
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
- * @param angle The angle of the robot.
+ * @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
@@ -75,7 +82,7 @@
*/
template <typename... ModuleStates>
const Pose2d& UpdateWithTime(units::second_t currentTime,
- const Rotation2d& angle,
+ const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates);
/**
@@ -86,7 +93,7 @@
* This also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
- * @param angle The angle of the robot.
+ * @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
@@ -94,9 +101,9 @@
* @return The new pose of the robot.
*/
template <typename... ModuleStates>
- const Pose2d& Update(const Rotation2d& angle,
+ const Pose2d& Update(const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
- return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle,
+ return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
moduleStates...);
}
@@ -106,6 +113,7 @@
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
+ Rotation2d m_gyroOffset;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 1ff75ac..23852d0 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -10,20 +10,24 @@
namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
- SwerveDriveKinematics<NumModules> kinematics, const Pose2d& initialPose)
+ SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
+ const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
template <size_t NumModules>
template <typename... ModuleStates>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
- units::second_t currentTime, const Rotation2d& angle,
+ units::second_t currentTime, const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
+ auto angle = gyroAngle + m_gyroOffset;
+
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
static_cast<void>(dtheta);
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h b/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
index 97805ba..9fdc049 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
@@ -11,6 +11,7 @@
#include <string>
#include <vector>
+#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableValue.h>
#include "frc/ErrorBase.h"
@@ -97,6 +98,16 @@
static void Delete(wpi::StringRef key);
/**
+ * Returns an NT Entry mapping to the specified key
+ *
+ * This is useful if an entry is used often, or is read and then modified.
+ *
+ * @param key the key
+ * @return the entry for the key
+ */
+ static nt::NetworkTableEntry GetEntry(wpi::StringRef key);
+
+ /**
* Maps the specified key to the specified value in this table.
*
* The value can be retrieved by calling the get method with a key that is
diff --git a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
index a8979c1..1e08a94 100644
--- a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -48,7 +48,8 @@
}
private:
- Eigen::Matrix<double, 6, 4> m_coefficients;
+ Eigen::Matrix<double, 6, 4> m_coefficients =
+ Eigen::Matrix<double, 6, 4>::Zero();
/**
* Returns the hermite basis matrix for cubic hermite spline interpolation.
diff --git a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
index 730eab4..632d3ad 100644
--- a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -48,7 +48,8 @@
}
private:
- Eigen::Matrix<double, 6, 6> m_coefficients;
+ Eigen::Matrix<double, 6, 6> m_coefficients =
+ Eigen::Matrix<double, 6, 6>::Zero();
/**
* Returns the hermite basis matrix for quintic hermite spline interpolation.
diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
index 0a384fb..96c2c6e 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
@@ -13,6 +13,10 @@
#include "frc/geometry/Pose2d.h"
+namespace wpi {
+class json;
+} // namespace wpi
+
namespace frc {
/**
@@ -48,6 +52,22 @@
curvature_t curvature{0.0};
/**
+ * Checks equality between this State and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are equal.
+ */
+ bool operator==(const State& other) const;
+
+ /**
+ * Checks inequality between this State and another object.
+ *
+ * @param other The other object.
+ * @return Whether the two objects are not equal.
+ */
+ bool operator!=(const State& other) const;
+
+ /**
* Interpolates between two States.
*
* @param endValue The end value for the interpolation.
@@ -103,4 +123,9 @@
return startValue + (endValue - startValue) * t;
}
};
+
+void to_json(wpi::json& json, const Trajectory::State& state);
+
+void from_json(const wpi::json& json, Trajectory::State& state);
+
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
index a739070..36118a0 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
@@ -14,7 +14,11 @@
#include <units/units.h>
#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
namespace frc {
@@ -89,6 +93,27 @@
}
/**
+ * Adds a mecanum drive kinematics constraint to ensure that
+ * no wheel velocity of a mecanum drive goes above the max velocity.
+ *
+ * @param kinematics The mecanum drive kinematics.
+ */
+ void SetKinematics(MecanumDriveKinematics kinematics) {
+ AddConstraint(MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
+ }
+
+ /**
+ * Adds a swerve drive kinematics constraint to ensure that
+ * no wheel velocity of a swerve drive goes above the max velocity.
+ *
+ * @param kinematics The swerve drive kinematics.
+ */
+ template <size_t NumModules>
+ void SetKinematics(SwerveDriveKinematics<NumModules>& kinematics) {
+ AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
+ }
+
+ /**
* Returns the starting velocity of the trajectory.
* @return The starting velocity of the trajectory.
*/
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h
new file mode 100644
index 0000000..700ed9c
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <wpi/StringRef.h>
+#include <wpi/Twine.h>
+
+#include "frc/trajectory/Trajectory.h"
+
+namespace frc {
+class TrajectoryUtil {
+ public:
+ TrajectoryUtil() = delete;
+
+ /**
+ * Exports a Trajectory to a PathWeaver-style JSON file.
+ *
+ * @param trajectory the trajectory to export
+ * @param path the path of the file to export to
+ *
+ * @return The interpolated state.
+ */
+ static void ToPathweaverJson(const Trajectory& trajectory,
+ const wpi::Twine& path);
+ /**
+ * Imports a Trajectory from a PathWeaver-style JSON file.
+ *
+ * @param path The path of the json file to import from.
+ *
+ * @return The trajectory represented by the file.
+ */
+ static Trajectory FromPathweaverJson(const wpi::Twine& path);
+
+ /**
+ * Deserializes a Trajectory from PathWeaver-style JSON.
+
+ * @param json the string containing the serialized JSON
+
+ * @return the trajectory represented by the JSON
+ */
+ static std::string SerializeTrajectory(const Trajectory& trajectory);
+
+ /**
+ * Serializes a Trajectory to PathWeaver-style JSON.
+
+ * @param trajectory the trajectory to export
+
+ * @return the string containing the serialized JSON
+ */
+ static Trajectory DeserializeTrajectory(wpi::StringRef json_str);
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index c528c9e..0479cfe 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -40,18 +40,27 @@
* `Calculate()` and to determine when the profile has completed via
* `IsFinished()`.
*/
+template <class Distance>
class TrapezoidProfile {
+ using Distance_t = units::unit_t<Distance>;
+ using Velocity =
+ units::compound_unit<Distance, units::inverse<units::seconds>>;
+ using Velocity_t = units::unit_t<Velocity>;
+ using Acceleration =
+ units::compound_unit<Velocity, units::inverse<units::seconds>>;
+ using Acceleration_t = units::unit_t<Acceleration>;
+
public:
class Constraints {
public:
- units::meters_per_second_t maxVelocity = 0_mps;
- units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
+ Velocity_t maxVelocity{0};
+ Acceleration_t maxAcceleration{0};
};
class State {
public:
- units::meter_t position = 0_m;
- units::meters_per_second_t velocity = 0_mps;
+ Distance_t position{0};
+ Velocity_t velocity{0};
bool operator==(const State& rhs) const {
return position == rhs.position && velocity == rhs.velocity;
}
@@ -66,7 +75,7 @@
* @param initial The initial state (usually the current state).
*/
TrapezoidProfile(Constraints constraints, State goal,
- State initial = State{0_m, 0_mps});
+ State initial = State{Distance_t(0), Velocity_t(0)});
TrapezoidProfile(const TrapezoidProfile&) = default;
TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
@@ -86,7 +95,7 @@
*
* @param target The target distance.
*/
- units::second_t TimeLeftUntil(units::meter_t target) const;
+ units::second_t TimeLeftUntil(Distance_t target) const;
/**
* Returns the total time the profile takes to reach the goal.
@@ -135,5 +144,6 @@
units::second_t m_endFullSpeed;
units::second_t m_endDeccel;
};
-
} // namespace frc
+
+#include "TrapezoidProfile.inc"
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
similarity index 76%
rename from wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
rename to wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 6026aa5..50f232d 100644
--- a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -5,12 +5,14 @@
/* the project. */
/*----------------------------------------------------------------------------*/
-#include "frc/trajectory/TrapezoidProfile.h"
+#pragma once
-using namespace frc;
+#include <algorithm>
-TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
- State initial)
+namespace frc {
+template <class Distance>
+TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
+ State goal, State initial)
: m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
m_constraints(constraints),
m_initial(Direct(initial)),
@@ -24,30 +26,30 @@
// ended at zero velocity
units::second_t cutoffBegin =
m_initial.velocity / m_constraints.maxAcceleration;
- units::meter_t cutoffDistBegin =
+ Distance_t cutoffDistBegin =
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
- units::meter_t cutoffDistEnd =
+ Distance_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 =
+ Distance_t fullTrapezoidDist =
cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
units::second_t accelerationTime =
m_constraints.maxVelocity / m_constraints.maxAcceleration;
- units::meter_t fullSpeedDist =
+ Distance_t fullSpeedDist =
fullTrapezoidDist -
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
- if (fullSpeedDist < 0_m) {
+ if (fullSpeedDist < Distance_t(0)) {
accelerationTime =
units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
- fullSpeedDist = 0_m;
+ fullSpeedDist = Distance_t(0);
}
m_endAccel = accelerationTime - cutoffBegin;
@@ -55,7 +57,9 @@
m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
}
-TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
+template <class Distance>
+typename TrapezoidProfile<Distance>::State
+TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
State result = m_initial;
if (t < m_endAccel) {
@@ -83,9 +87,11 @@
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;
+template <class Distance>
+units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
+ Distance_t target) const {
+ Distance_t position = m_initial.position * m_direction;
+ Velocity_t velocity = m_initial.velocity * m_direction;
units::second_t endAccel = m_endAccel * m_direction;
units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
@@ -101,21 +107,19 @@
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;
+ const Acceleration_t acceleration = m_constraints.maxAcceleration;
+ const Acceleration_t decceleration = -m_constraints.maxAcceleration;
- units::meter_t distToTarget = units::math::abs(target - position);
+ Distance_t distToTarget = units::math::abs(target - position);
- if (distToTarget < 1e-6_m) {
+ if (distToTarget < Distance_t(1e-6)) {
return 0_s;
}
- units::meter_t accelDist =
+ Distance_t accelDist =
velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
- units::meters_per_second_t deccelVelocity;
+ Velocity_t deccelVelocity;
if (endAccel > 0_s) {
deccelVelocity = units::math::sqrt(
units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
@@ -123,20 +127,20 @@
deccelVelocity = velocity;
}
- units::meter_t deccelDist =
+ Distance_t deccelDist =
deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
- deccelDist = units::math::max(deccelDist, 0_m);
+ deccelDist = units::math::max(deccelDist, Distance_t(0));
- units::meter_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+ Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
if (accelDist > distToTarget) {
accelDist = distToTarget;
- fullSpeedDist = 0_m;
- deccelDist = 0_m;
+ fullSpeedDist = Distance_t(0);
+ deccelDist = Distance_t(0);
} else if (accelDist + fullSpeedDist > distToTarget) {
fullSpeedDist = distToTarget - accelDist;
- deccelDist = 0_m;
+ deccelDist = Distance_t(0);
} else {
deccelDist = distToTarget - fullSpeedDist - accelDist;
}
@@ -156,3 +160,4 @@
return accelTime + fullSpeedTime + deccelTime;
}
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
new file mode 100644
index 0000000..50ace85
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * A class that enforces constraints on differential drive voltage expenditure
+ * based on the motor dynamics and the drive kinematics. Ensures that the
+ * acceleration of any wheel of the robot while following the trajectory is
+ * never higher than what can be achieved with the given maximum voltage.
+ */
+class DifferentialDriveVoltageConstraint : public TrajectoryConstraint {
+ public:
+ /**
+ * Creates a new DifferentialDriveVoltageConstraint.
+ *
+ * @param feedforward A feedforward component describing the behavior of the
+ * drive.
+ * @param kinematics A kinematics component describing the drive geometry.
+ * @param maxVoltage The maximum voltage available to the motors while
+ * following the path. Should be somewhat less than the nominal battery
+ * voltage (12V) to account for "voltage sag" due to current draw.
+ */
+ DifferentialDriveVoltageConstraint(
+ SimpleMotorFeedforward<units::meter> feedforward,
+ DifferentialDriveKinematics kinematics, units::volt_t maxVoltage);
+
+ units::meters_per_second_t MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) override;
+
+ MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) override;
+
+ private:
+ SimpleMotorFeedforward<units::meter> m_feedforward;
+ DifferentialDriveKinematics m_kinematics;
+ units::volt_t m_maxVoltage;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
new file mode 100644
index 0000000..53e3953
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include <units/units.h>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+namespace frc {
+class MecanumDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+ MecanumDriveKinematicsConstraint(MecanumDriveKinematics kinematics,
+ units::meters_per_second_t maxSpeed);
+
+ units::meters_per_second_t MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) override;
+
+ MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) override;
+
+ private:
+ MecanumDriveKinematics m_kinematics;
+ units::meters_per_second_t m_maxSpeed;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
new file mode 100644
index 0000000..a8ad870
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include <units/units.h>
+
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+namespace frc {
+
+template <size_t NumModules>
+class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+ SwerveDriveKinematicsConstraint(
+ frc::SwerveDriveKinematics<NumModules> kinematics,
+ units::meters_per_second_t maxSpeed);
+
+ units::meters_per_second_t MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) override;
+
+ MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) override;
+
+ private:
+ frc::SwerveDriveKinematics<NumModules> m_kinematics;
+ units::meters_per_second_t m_maxSpeed;
+};
+} // namespace frc
+
+#include "SwerveDriveKinematicsConstraint.inc"
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
new file mode 100644
index 0000000..c3437d5
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+
+namespace frc {
+
+template <size_t NumModules>
+SwerveDriveKinematicsConstraint<NumModules>::SwerveDriveKinematicsConstraint(
+ frc::SwerveDriveKinematics<NumModules> kinematics,
+ units::meters_per_second_t maxSpeed)
+ : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+template <size_t NumModules>
+units::meters_per_second_t
+SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) {
+ auto xVelocity = velocity * pose.Rotation().Cos();
+ auto yVelocity = velocity * pose.Rotation().Sin();
+ auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
+ {xVelocity, yVelocity, velocity * curvature});
+ m_kinematics.NormalizeWheelSpeeds(&wheelSpeeds, m_maxSpeed);
+
+ auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+ return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+template <size_t NumModules>
+TrajectoryConstraint::MinMax
+SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) {
+ return {};
+}
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc2/command/Command.h b/wpilibc/src/main/native/include/frc2/command/Command.h
deleted file mode 100644
index 49e904e..0000000
--- a/wpilibc/src/main/native/include/frc2/command/Command.h
+++ /dev/null
@@ -1,242 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/ErrorBase.h>
-#include <frc/WPIErrors.h>
-#include <frc2/command/Subsystem.h>
-
-#include <memory>
-#include <string>
-
-#include <units/units.h>
-#include <wpi/ArrayRef.h>
-#include <wpi/Demangle.h>
-#include <wpi/SmallSet.h>
-#include <wpi/Twine.h>
-
-namespace frc2 {
-
-template <typename T>
-std::string GetTypeName(const T& type) {
- return wpi::Demangle(typeid(type).name());
-}
-
-class ParallelCommandGroup;
-class ParallelRaceGroup;
-class ParallelDeadlineGroup;
-class SequentialCommandGroup;
-class PerpetualCommand;
-class ProxyScheduleCommand;
-
-/**
- * A state machine representing a complete action to be performed by the robot.
- * Commands are run by the CommandScheduler, and can be composed into
- * CommandGroups to allow users to build complicated multi-step actions without
- * the need to roll the state machine logic themselves.
- *
- * <p>Commands are run synchronously from the main robot loop; no multithreading
- * is used, unless specified explicitly from the command implementation.
- *
- * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
- * or decorators will not function!
- *
- * @see CommandScheduler
- * @see CommandHelper
- */
-class Command : public frc::ErrorBase {
- public:
- Command() = default;
- virtual ~Command();
-
- Command(const Command&);
- Command& operator=(const Command&);
- Command(Command&&) = default;
- Command& operator=(Command&&) = default;
-
- /**
- * The initial subroutine of a command. Called once when the command is
- * initially scheduled.
- */
- virtual void Initialize();
-
- /**
- * The main body of a command. Called repeatedly while the command is
- * scheduled.
- */
- virtual void Execute();
-
- /**
- * The action to take when the command ends. Called when either the command
- * finishes normally, or when it interrupted/canceled.
- *
- * @param interrupted whether the command was interrupted/canceled
- */
- virtual void End(bool interrupted);
-
- /**
- * Whether the command has finished. Once a command finishes, the scheduler
- * will call its end() method and un-schedule it.
- *
- * @return whether the command has finished.
- */
- virtual bool IsFinished() { return false; }
-
- /**
- * Specifies the set of subsystems used by this command. Two commands cannot
- * use the same subsystem at the same time. If the command is scheduled as
- * interruptible and another command is scheduled that shares a requirement,
- * the command will be interrupted. Else, the command will not be scheduled.
- * If no subsystems are required, return an empty set.
- *
- * <p>Note: it is recommended that user implementations contain the
- * requirements as a field, and return that field here, rather than allocating
- * a new set every time this is called.
- *
- * @return the set of subsystems that are required
- */
- virtual wpi::SmallSet<Subsystem*, 4> GetRequirements() const = 0;
-
- /**
- * Decorates this command with a timeout. If the specified timeout is
- * exceeded before the command finishes normally, the command will be
- * interrupted and un-scheduled. Note that the timeout only applies to the
- * command returned by this method; the calling command is not itself changed.
- *
- * @param duration the timeout duration
- * @return the command with the timeout added
- */
- ParallelRaceGroup WithTimeout(units::second_t duration) &&;
-
- /**
- * Decorates this command with an interrupt condition. If the specified
- * condition becomes true before the command finishes normally, the command
- * will be interrupted and un-scheduled. Note that this only applies to the
- * command returned by this method; the calling command is not itself changed.
- *
- * @param condition the interrupt condition
- * @return the command with the interrupt condition added
- */
- ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
-
- /**
- * Decorates this command with a runnable to run before this command starts.
- *
- * @param toRun the Runnable to run
- * @return the decorated command
- */
- SequentialCommandGroup BeforeStarting(std::function<void()> toRun) &&;
-
- /**
- * Decorates this command with a runnable to run after the command finishes.
- *
- * @param toRun the Runnable to run
- * @return the decorated command
- */
- SequentialCommandGroup AndThen(std::function<void()> toRun) &&;
-
- /**
- * Decorates this command to run perpetually, ignoring its ordinary end
- * conditions. The decorated command can still be interrupted or canceled.
- *
- * @return the decorated command
- */
- PerpetualCommand Perpetually() &&;
-
- /**
- * Decorates this command to run "by proxy" by wrapping it in a {@link
- * ProxyScheduleCommand}. This is useful for "forking off" from command groups
- * when the user does not wish to extend the command's requirements to the
- * entire command group.
- *
- * @return the decorated command
- */
- ProxyScheduleCommand AsProxy();
-
- /**
- * Schedules this command.
- *
- * @param interruptible whether this command can be interrupted by another
- * command that shares one of its requirements
- */
- void Schedule(bool interruptible);
-
- /**
- * Schedules this command, defaulting to interruptible.
- */
- void Schedule() { Schedule(true); }
-
- /**
- * Cancels this command. Will call the command's interrupted() method.
- * Commands will be canceled even if they are not marked as interruptible.
- */
- void Cancel();
-
- /**
- * Whether or not the command is currently scheduled. Note that this does not
- * detect whether the command is being run by a CommandGroup, only whether it
- * is directly being run by the scheduler.
- *
- * @return Whether the command is scheduled.
- */
- bool IsScheduled() const;
-
- /**
- * Whether the command requires a given subsystem. Named "hasRequirement"
- * rather than "requires" to avoid confusion with
- * {@link
- * edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
- * - this may be able to be changed in a few years.
- *
- * @param requirement the subsystem to inquire about
- * @return whether the subsystem is required
- */
- bool HasRequirement(Subsystem* requirement) const;
-
- /**
- * Whether the command is currently grouped in a command group. Used as extra
- * insurance to prevent accidental independent use of grouped commands.
- */
- bool IsGrouped() const;
-
- /**
- * Sets whether the command is currently grouped in a command group. Can be
- * used to "reclaim" a command if a group is no longer going to use it. NOT
- * ADVISED!
- */
- void SetGrouped(bool grouped);
-
- /**
- * Whether the given command should run when the robot is disabled. Override
- * to return true if the command should run when disabled.
- *
- * @return whether the command should run when the robot is disabled
- */
- virtual bool RunsWhenDisabled() const { return false; }
-
- virtual std::string GetName() const;
-
- protected:
- /**
- * Transfers ownership of this command to a unique pointer. Used for
- * decorator methods.
- */
- virtual std::unique_ptr<Command> TransferOwnership() && = 0;
-
- bool m_isGrouped = false;
-};
-
-/**
- * Checks if two commands have disjoint requirement sets.
- *
- * @param first The first command to check.
- * @param second The second command to check.
- * @return False if first and second share a requirement.
- */
-bool RequirementsDisjoint(Command* first, Command* second);
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandBase.h b/wpilibc/src/main/native/include/frc2/command/CommandBase.h
deleted file mode 100644
index 3b1698f..0000000
--- a/wpilibc/src/main/native/include/frc2/command/CommandBase.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
-
-#include <string>
-
-#include <wpi/SmallSet.h>
-#include <wpi/Twine.h>
-
-#include "Command.h"
-
-namespace frc2 {
-/**
- * A Sendable base class for Commands.
- */
-class CommandBase : public Command,
- public frc::Sendable,
- public frc::SendableHelper<CommandBase> {
- public:
- /**
- * Adds the specified requirements to the command.
- *
- * @param requirements the requirements to add
- */
- void AddRequirements(std::initializer_list<Subsystem*> requirements);
-
- void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
-
- wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
-
- /**
- * Sets the name of this Command.
- *
- * @param name name
- */
- void SetName(const wpi::Twine& name);
-
- /**
- * Gets the name of this Command.
- *
- * @return Name
- */
- std::string GetName() const override;
-
- /**
- * Gets the subsystem name of this Command.
- *
- * @return Subsystem name
- */
- std::string GetSubsystem() const;
-
- /**
- * Sets the subsystem name of this Command.
- *
- * @param subsystem subsystem name
- */
- void SetSubsystem(const wpi::Twine& subsystem);
-
- void InitSendable(frc::SendableBuilder& builder) override;
-
- protected:
- CommandBase();
- wpi::SmallSet<Subsystem*, 4> m_requirements;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h b/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h
deleted file mode 100644
index 9d265b4..0000000
--- a/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/ErrorBase.h>
-
-#include <memory>
-#include <set>
-#include <vector>
-
-#include "CommandBase.h"
-
-namespace frc2 {
-
-/**
- * A base for CommandGroups. Statically tracks commands that have been
- * allocated to groups to ensure those commands are not also used independently,
- * which can result in inconsistent command state and unpredictable execution.
- */
-class CommandGroupBase : public CommandBase {
- public:
- /**
- * Requires that the specified command not have been already allocated to a
- * CommandGroup. Reports an error if the command is already grouped.
- *
- * @param commands The command to check
- * @return True if all the command is ungrouped.
- */
- static bool RequireUngrouped(Command& command);
-
- /**
- * Requires that the specified commands not have been already allocated to a
- * CommandGroup. Reports an error if any of the commands are already grouped.
- *
- * @param commands The commands to check
- * @return True if all the commands are ungrouped.
- */
- static bool RequireUngrouped(wpi::ArrayRef<std::unique_ptr<Command>>);
-
- /**
- * Requires that the specified commands not have been already allocated to a
- * CommandGroup. Reports an error if any of the commands are already grouped.
- *
- * @param commands The commands to check
- * @return True if all the commands are ungrouped.
- */
- static bool RequireUngrouped(std::initializer_list<Command*>);
-
- /**
- * Adds the given commands to the command group.
- *
- * @param commands The commands to add.
- */
- virtual void AddCommands(
- std::vector<std::unique_ptr<Command>>&& commands) = 0;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandHelper.h b/wpilibc/src/main/native/include/frc2/command/CommandHelper.h
deleted file mode 100644
index ec15f88..0000000
--- a/wpilibc/src/main/native/include/frc2/command/CommandHelper.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <type_traits>
-#include <utility>
-
-#include "Command.h"
-
-namespace frc2 {
-
-/**
- * CRTP implementation to allow polymorphic decorator functions in Command.
- *
- * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
- * or decorators will not function!
- */
-template <typename Base, typename CRTP,
- typename = std::enable_if_t<std::is_base_of_v<Command, Base>>>
-class CommandHelper : public Base {
- using Base::Base;
-
- public:
- CommandHelper() = default;
-
- protected:
- std::unique_ptr<Command> TransferOwnership() && override {
- return std::make_unique<CRTP>(std::move(*static_cast<CRTP*>(this)));
- }
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h
deleted file mode 100644
index 2e9cdd5..0000000
--- a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h
+++ /dev/null
@@ -1,372 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/ErrorBase.h>
-#include <frc/RobotState.h>
-#include <frc/WPIErrors.h>
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-
-#include <networktables/NetworkTableEntry.h>
-#include <wpi/DenseMap.h>
-#include <wpi/FunctionExtras.h>
-#include <wpi/SmallSet.h>
-
-#include "CommandState.h"
-
-namespace frc2 {
-class Command;
-class Subsystem;
-
-/**
- * The scheduler responsible for running Commands. A Command-based robot should
- * call Run() on the singleton instance in its periodic block in order to run
- * commands synchronously from the main loop. Subsystems should be registered
- * with the scheduler using RegisterSubsystem() in order for their Periodic()
- * methods to be called and for their default commands to be scheduled.
- */
-class CommandScheduler final : public frc::Sendable,
- public frc::ErrorBase,
- public frc::SendableHelper<CommandScheduler> {
- public:
- /**
- * Returns the Scheduler instance.
- *
- * @return the instance
- */
- static CommandScheduler& GetInstance();
-
- using Action = std::function<void(const Command&)>;
-
- /**
- * Adds a button binding to the scheduler, which will be polled to schedule
- * commands.
- *
- * @param button The button to add
- */
- void AddButton(wpi::unique_function<void()> button);
-
- /**
- * Removes all button bindings from the scheduler.
- */
- void ClearButtons();
-
- /**
- * Schedules a command for execution. Does nothing if the command is already
- * scheduled. If a command's requirements are not available, it will only be
- * started if all the commands currently using those requirements have been
- * scheduled as interruptible. If this is the case, they will be interrupted
- * and the command will be scheduled.
- *
- * @param interruptible whether this command can be interrupted
- * @param command the command to schedule
- */
- void Schedule(bool interruptible, Command* command);
-
- /**
- * Schedules a command for execution, with interruptible defaulted to true.
- * Does nothing if the command is already scheduled.
- *
- * @param command the command to schedule
- */
- void Schedule(Command* command);
-
- /**
- * Schedules multiple commands for execution. Does nothing if the command is
- * already scheduled. If a command's requirements are not available, it will
- * only be started if all the commands currently using those requirements have
- * been scheduled as interruptible. If this is the case, they will be
- * interrupted and the command will be scheduled.
- *
- * @param interruptible whether the commands should be interruptible
- * @param commands the commands to schedule
- */
- void Schedule(bool interruptible, wpi::ArrayRef<Command*> commands);
-
- /**
- * Schedules multiple commands for execution. Does nothing if the command is
- * already scheduled. If a command's requirements are not available, it will
- * only be started if all the commands currently using those requirements have
- * been scheduled as interruptible. If this is the case, they will be
- * interrupted and the command will be scheduled.
- *
- * @param interruptible whether the commands should be interruptible
- * @param commands the commands to schedule
- */
- void Schedule(bool interruptible, std::initializer_list<Command*> commands);
-
- /**
- * Schedules multiple commands for execution, with interruptible defaulted to
- * true. Does nothing if the command is already scheduled.
- *
- * @param commands the commands to schedule
- */
- void Schedule(wpi::ArrayRef<Command*> commands);
-
- /**
- * Schedules multiple commands for execution, with interruptible defaulted to
- * true. Does nothing if the command is already scheduled.
- *
- * @param commands the commands to schedule
- */
- void Schedule(std::initializer_list<Command*> commands);
-
- /**
- * Runs a single iteration of the scheduler. The execution occurs in the
- * following order:
- *
- * <p>Subsystem periodic methods are called.
- *
- * <p>Button bindings are polled, and new commands are scheduled from them.
- *
- * <p>Currently-scheduled commands are executed.
- *
- * <p>End conditions are checked on currently-scheduled commands, and commands
- * that are finished have their end methods called and are removed.
- *
- * <p>Any subsystems not being used as requirements have their default methods
- * started.
- */
- void Run();
-
- /**
- * Registers subsystems with the scheduler. This must be called for the
- * subsystem's periodic block to run when the scheduler is run, and for the
- * subsystem's default command to be scheduled. It is recommended to call
- * this from the constructor of your subsystem implementations.
- *
- * @param subsystem the subsystem to register
- */
- void RegisterSubsystem(Subsystem* subsystem);
-
- /**
- * Un-registers subsystems with the scheduler. The subsystem will no longer
- * have its periodic block called, and will not have its default command
- * scheduled.
- *
- * @param subsystem the subsystem to un-register
- */
- void UnregisterSubsystem(Subsystem* subsystem);
-
- void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
-
- void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
-
- /**
- * Sets the default command for a subsystem. Registers that subsystem if it
- * is not already registered. Default commands will run whenever there is no
- * other command currently scheduled that requires the subsystem. Default
- * commands should be written to never end (i.e. their IsFinished() method
- * should return false), as they would simply be re-scheduled if they do.
- * Default commands must also require their subsystem.
- *
- * @param subsystem the subsystem whose default command will be set
- * @param defaultCommand the default command to associate with the subsystem
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) {
- if (!defaultCommand.HasRequirement(subsystem)) {
- wpi_setWPIErrorWithContext(
- CommandIllegalUse, "Default commands must require their subsystem!");
- return;
- }
- if (defaultCommand.IsFinished()) {
- wpi_setWPIErrorWithContext(CommandIllegalUse,
- "Default commands should not end!");
- return;
- }
- m_subsystems[subsystem] = std::make_unique<std::remove_reference_t<T>>(
- std::forward<T>(defaultCommand));
- }
-
- /**
- * Gets the default command associated with this subsystem. Null if this
- * subsystem has no default command associated with it.
- *
- * @param subsystem the subsystem to inquire about
- * @return the default command associated with the subsystem
- */
- Command* GetDefaultCommand(const Subsystem* subsystem) const;
-
- /**
- * Cancels a command. The scheduler will only call the interrupted method of
- * a canceled command, not the end method (though the interrupted method may
- * itself call the end method). Commands will be canceled even if they are
- * not scheduled as interruptible.
- *
- * @param command the command to cancel
- */
- void Cancel(Command* command);
-
- /**
- * Cancels commands. The scheduler will only call the interrupted method of a
- * canceled command, not the end method (though the interrupted method may
- * itself call the end method). Commands will be canceled even if they are
- * not scheduled as interruptible.
- *
- * @param commands the commands to cancel
- */
- void Cancel(wpi::ArrayRef<Command*> commands);
-
- /**
- * Cancels commands. The scheduler will only call the interrupted method of a
- * canceled command, not the end method (though the interrupted method may
- * itself call the end method). Commands will be canceled even if they are
- * not scheduled as interruptible.
- *
- * @param commands the commands to cancel
- */
- void Cancel(std::initializer_list<Command*> commands);
-
- /**
- * Cancels all commands that are currently scheduled.
- */
- void CancelAll();
-
- /**
- * Returns the time since a given command was scheduled. Note that this only
- * works on commands that are directly scheduled by the scheduler; it will not
- * work on commands inside of commandgroups, as the scheduler does not see
- * them.
- *
- * @param command the command to query
- * @return the time since the command was scheduled, in seconds
- */
- double TimeSinceScheduled(const Command* command) const;
-
- /**
- * Whether the given commands are running. Note that this only works on
- * commands that are directly scheduled by the scheduler; it will not work on
- * commands inside of CommandGroups, as the scheduler does not see them.
- *
- * @param commands the command to query
- * @return whether the command is currently scheduled
- */
- bool IsScheduled(wpi::ArrayRef<const Command*> commands) const;
-
- /**
- * Whether the given commands are running. Note that this only works on
- * commands that are directly scheduled by the scheduler; it will not work on
- * commands inside of CommandGroups, as the scheduler does not see them.
- *
- * @param commands the command to query
- * @return whether the command is currently scheduled
- */
- bool IsScheduled(std::initializer_list<const Command*> commands) const;
-
- /**
- * Whether a given command is running. Note that this only works on commands
- * that are directly scheduled by the scheduler; it will not work on commands
- * inside of CommandGroups, as the scheduler does not see them.
- *
- * @param commands the command to query
- * @return whether the command is currently scheduled
- */
- bool IsScheduled(const Command* command) const;
-
- /**
- * Returns the command currently requiring a given subsystem. Null if no
- * command is currently requiring the subsystem
- *
- * @param subsystem the subsystem to be inquired about
- * @return the command currently requiring the subsystem
- */
- Command* Requiring(const Subsystem* subsystem) const;
-
- /**
- * Disables the command scheduler.
- */
- void Disable();
-
- /**
- * Enables the command scheduler.
- */
- void Enable();
-
- /**
- * Adds an action to perform on the initialization of any command by the
- * scheduler.
- *
- * @param action the action to perform
- */
- void OnCommandInitialize(Action action);
-
- /**
- * Adds an action to perform on the execution of any command by the scheduler.
- *
- * @param action the action to perform
- */
- void OnCommandExecute(Action action);
-
- /**
- * Adds an action to perform on the interruption of any command by the
- * scheduler.
- *
- * @param action the action to perform
- */
- void OnCommandInterrupt(Action action);
-
- /**
- * Adds an action to perform on the finishing of any command by the scheduler.
- *
- * @param action the action to perform
- */
- void OnCommandFinish(Action action);
-
- void InitSendable(frc::SendableBuilder& builder) override;
-
- private:
- // Constructor; private as this is a singleton
- CommandScheduler();
-
- // A map from commands to their scheduling state. Also used as a set of the
- // currently-running commands.
- wpi::DenseMap<Command*, CommandState> m_scheduledCommands;
-
- // A map from required subsystems to their requiring commands. Also used as a
- // set of the currently-required subsystems.
- wpi::DenseMap<Subsystem*, Command*> m_requirements;
-
- // A map from subsystems registered with the scheduler to their default
- // commands. Also used as a list of currently-registered subsystems.
- wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> m_subsystems;
-
- // The set of currently-registered buttons that will be polled every
- // iteration.
- wpi::SmallVector<wpi::unique_function<void()>, 4> m_buttons;
-
- bool m_disabled{false};
-
- // NetworkTable entries for use in Sendable impl
- nt::NetworkTableEntry m_namesEntry;
- nt::NetworkTableEntry m_idsEntry;
- nt::NetworkTableEntry m_cancelEntry;
-
- // Lists of user-supplied actions to be executed on scheduling events for
- // every command.
- wpi::SmallVector<Action, 4> m_initActions;
- wpi::SmallVector<Action, 4> m_executeActions;
- wpi::SmallVector<Action, 4> m_interruptActions;
- wpi::SmallVector<Action, 4> m_finishActions;
-
- // Flag and queues for avoiding concurrent modification if commands are
- // scheduled/canceled during run
-
- bool m_inRunLoop = false;
- wpi::DenseMap<Command*, bool> m_toSchedule;
- wpi::SmallVector<Command*, 4> m_toCancel;
-
- friend class CommandTestBase;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandState.h b/wpilibc/src/main/native/include/frc2/command/CommandState.h
deleted file mode 100644
index 41fc5d0..0000000
--- a/wpilibc/src/main/native/include/frc2/command/CommandState.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace frc2 {
-/**
- * Class that holds scheduling state for a command. Used internally by the
- * CommandScheduler
- */
-class CommandState final {
- public:
- CommandState() = default;
-
- explicit CommandState(bool interruptible);
-
- bool IsInterruptible() const { return m_interruptible; }
-
- // The time since this command was initialized.
- double TimeSinceInitialized() const;
-
- private:
- double m_startTime = -1;
- bool m_interruptible;
-
- void StartTiming();
- void StartRunning();
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h b/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h
deleted file mode 100644
index 0419cf9..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <iostream>
-#include <memory>
-#include <utility>
-
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * Runs one of two commands, depending on the value of the given condition when
- * this command is initialized. Does not actually schedule the selected command
- * - rather, the command is run through this command; this ensures that the
- * command will behave as expected if used as part of a CommandGroup. Requires
- * the requirements of both commands, again to ensure proper functioning when
- * used in a CommandGroup. If this is undesired, consider using
- * ScheduleCommand.
- *
- * <p>As this command contains multiple component commands within it, it is
- * technically a command group; the command instances that are passed to it
- * cannot be added to any other groups, or scheduled individually.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- *
- * @see ScheduleCommand
- */
-class ConditionalCommand
- : public CommandHelper<CommandBase, ConditionalCommand> {
- public:
- /**
- * Creates a new ConditionalCommand.
- *
- * @param onTrue the command to run if the condition is true
- * @param onFalse the command to run if the condition is false
- * @param condition the condition to determine which command to run
- */
- template <class T1, class T2,
- typename = std::enable_if_t<
- std::is_base_of_v<Command, std::remove_reference_t<T1>>>,
- typename = std::enable_if_t<
- std::is_base_of_v<Command, std::remove_reference_t<T2>>>>
- ConditionalCommand(T1&& onTrue, T2&& onFalse, std::function<bool()> condition)
- : ConditionalCommand(std::make_unique<std::remove_reference_t<T1>>(
- std::forward<T1>(onTrue)),
- std::make_unique<std::remove_reference_t<T2>>(
- std::forward<T2>(onFalse)),
- condition) {}
-
- /**
- * Creates a new ConditionalCommand.
- *
- * @param onTrue the command to run if the condition is true
- * @param onFalse the command to run if the condition is false
- * @param condition the condition to determine which command to run
- */
- ConditionalCommand(std::unique_ptr<Command>&& onTrue,
- std::unique_ptr<Command>&& onFalse,
- std::function<bool()> condition);
-
- ConditionalCommand(ConditionalCommand&& other) = default;
-
- // No copy constructors for command groups
- ConditionalCommand(const ConditionalCommand& other) = delete;
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- bool IsFinished() override;
-
- bool RunsWhenDisabled() const override;
-
- private:
- std::unique_ptr<Command> m_onTrue;
- std::unique_ptr<Command> m_onFalse;
- std::function<bool()> m_condition;
- Command* m_selectedCommand{nullptr};
- bool m_runsWhenDisabled = true;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h
deleted file mode 100644
index b47135c..0000000
--- a/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that allows the user to pass in functions for each of the basic
- * command methods through the constructor. Useful for inline definitions of
- * complex commands - note, however, that if a command is beyond a certain
- * complexity it is usually better practice to write a proper class for it than
- * to inline it.
- */
-class FunctionalCommand : public CommandHelper<CommandBase, FunctionalCommand> {
- public:
- /**
- * Creates a new FunctionalCommand.
- *
- * @param onInit the function to run on command initialization
- * @param onExecute the function to run on command execution
- * @param onEnd the function to run on command end
- * @param isFinished the function that determines whether the command has
- * finished
- * @param requirements the subsystems required by this command
- */
- FunctionalCommand(std::function<void()> onInit,
- std::function<void()> onExecute,
- std::function<void(bool)> onEnd,
- std::function<bool()> isFinished);
-
- FunctionalCommand(FunctionalCommand&& other) = default;
-
- FunctionalCommand(const FunctionalCommand& other) = default;
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- bool IsFinished() override;
-
- private:
- std::function<void()> m_onInit;
- std::function<void()> m_onExecute;
- std::function<void(bool)> m_onEnd;
- std::function<bool()> m_isFinished;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/InstantCommand.h b/wpilibc/src/main/native/include/frc2/command/InstantCommand.h
deleted file mode 100644
index ec28a11..0000000
--- a/wpilibc/src/main/native/include/frc2/command/InstantCommand.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A Command that runs instantly; it will initialize, execute once, and end on
- * the same iteration of the scheduler. Users can either pass in a Runnable and
- * a set of requirements, or else subclass this command if desired.
- */
-class InstantCommand : public CommandHelper<CommandBase, InstantCommand> {
- public:
- /**
- * Creates a new InstantCommand that runs the given Runnable with the given
- * requirements.
- *
- * @param toRun the Runnable to run
- * @param requirements the subsystems required by this command
- */
- InstantCommand(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements);
-
- InstantCommand(InstantCommand&& other) = default;
-
- InstantCommand(const InstantCommand& other) = default;
-
- /**
- * Creates a new InstantCommand with a Runnable that does nothing. Useful
- * only as a no-arg constructor to call implicitly from subclass constructors.
- */
- InstantCommand();
-
- void Initialize() override;
-
- bool IsFinished() final;
-
- private:
- std::function<void()> m_toRun;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h
deleted file mode 100644
index 3365c04..0000000
--- a/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/Notifier.h>
-
-#include <units/units.h>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that starts a notifier to run the given runnable periodically in a
- * separate thread. Has no end condition as-is; either subclass it or use {@link
- * Command#withTimeout(double)} or
- * {@link Command#withInterrupt(BooleanSupplier)} to give it one.
- *
- * <p>WARNING: Do not use this class unless you are confident in your ability to
- * make the executed code thread-safe. If you do not know what "thread-safe"
- * means, that is a good sign that you should not use this class.
- */
-class NotifierCommand : public CommandHelper<CommandBase, NotifierCommand> {
- public:
- /**
- * Creates a new NotifierCommand.
- *
- * @param toRun the runnable for the notifier to run
- * @param period the period at which the notifier should run
- * @param requirements the subsystems required by this command
- */
- NotifierCommand(std::function<void()> toRun, units::second_t period,
- std::initializer_list<Subsystem*> requirements);
-
- NotifierCommand(NotifierCommand&& other);
-
- NotifierCommand(const NotifierCommand& other);
-
- void Initialize() override;
-
- void End(bool interrupted) override;
-
- private:
- std::function<void()> m_toRun;
- frc::Notifier m_notifier;
- units::second_t m_period;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h b/wpilibc/src/main/native/include/frc2/command/PIDCommand.h
deleted file mode 100644
index 6e553ff..0000000
--- a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h
+++ /dev/null
@@ -1,124 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/controller/PIDController.h"
-#include "frc2/command/CommandBase.h"
-#include "frc2/command/CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that controls an output with a PIDController. Runs forever by
- * default - to add exit conditions and/or other behavior, subclass this class.
- * The controller calculation and output are performed synchronously in the
- * command's execute() method.
- *
- * @see PIDController
- */
-class PIDCommand : public CommandHelper<CommandBase, PIDCommand> {
- public:
- /**
- * Creates a new PIDCommand, which controls the given output with a
- * PIDController.
- *
- * @param controller the controller that controls the output.
- * @param measurementSource the measurement of the process variable
- * @param setpointSource the controller's reference (aka setpoint)
- * @param useOutput the controller's output
- * @param requirements the subsystems required by this command
- */
- PIDCommand(PIDController controller,
- std::function<double()> measurementSource,
- std::function<double()> setpointSource,
- std::function<void(double)> useOutput,
- std::initializer_list<Subsystem*> requirements);
-
- /**
- * Creates a new PIDCommand, which controls the given output with a
- * PIDController with a constant setpoint.
- *
- * @param controller the controller that controls the output.
- * @param measurementSource the measurement of the process variable
- * @param setpoint the controller's setpoint (aka setpoint)
- * @param useOutput the controller's output
- * @param requirements the subsystems required by this command
- */
- PIDCommand(PIDController controller,
- std::function<double()> measurementSource, double setpoint,
- std::function<void(double)> useOutput,
- std::initializer_list<Subsystem*> requirements);
-
- PIDCommand(PIDCommand&& other) = default;
-
- PIDCommand(const PIDCommand& other) = default;
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- /**
- * Sets the function that uses the output of the PIDController.
- *
- * @param useOutput The function that uses the output.
- */
- void SetOutput(std::function<void(double)> useOutput);
-
- /**
- * Sets the setpoint for the controller to track the given source.
- *
- * @param setpointSource The setpoint source
- */
- void SetSetpoint(std::function<double()> setpointSource);
-
- /**
- * Sets the setpoint for the controller to a constant value.
- *
- * @param setpoint The setpoint
- */
- void SetSetpoint(double setpoint);
-
- /**
- * Sets the setpoint for the controller to a constant value relative (i.e.
- * added to) the current setpoint.
- *
- * @param relativeReference The change in setpoint
- */
- void SetSetpointRelative(double relativeSetpoint);
-
- /**
- * Gets the measurement of the process variable. Wraps the passed-in function
- * for readability.
- *
- * @return The measurement of the process variable
- */
- virtual double GetMeasurement();
-
- /**
- * Gets the measurement of the process variable. Wraps the passed-in function
- * for readability.
- *
- * @return The measurement of the process variable
- */
- virtual void UseOutput(double output);
-
- /**
- * Returns the PIDController used by the command.
- *
- * @return The PIDController
- */
- PIDController& getController();
-
- protected:
- PIDController m_controller;
- std::function<double()> m_measurement;
- std::function<double()> m_setpoint;
- std::function<void(double)> m_useOutput;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h
deleted file mode 100644
index 7aa21c0..0000000
--- a/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/controller/PIDController.h"
-#include "frc2/command/SubsystemBase.h"
-
-namespace frc2 {
-/**
- * A subsystem that uses a PIDController to control an output. The controller
- * is run synchronously from the subsystem's periodic() method.
- *
- * @see PIDController
- */
-class PIDSubsystem : public SubsystemBase {
- public:
- /**
- * Creates a new PIDSubsystem.
- *
- * @param controller the PIDController to use
- */
- explicit PIDSubsystem(PIDController controller);
-
- void Periodic() override;
-
- /**
- * Uses the output from the PIDController.
- *
- * @param output the output of the PIDController
- */
- virtual void UseOutput(double output) = 0;
-
- /**
- * Returns the reference (setpoint) used by the PIDController.
- *
- * @return the reference (setpoint) to be used by the controller
- */
- virtual double GetSetpoint() = 0;
-
- /**
- * Returns the measurement of the process variable used by the PIDController.
- *
- * @return the measurement of the process variable
- */
- virtual double GetMeasurement() = 0;
-
- /**
- * Enables the PID control. Resets the controller.
- */
- virtual void Enable();
-
- /**
- * Disables the PID control. Sets output to zero.
- */
- virtual void Disable();
-
- /**
- * Returns the PIDController.
- *
- * @return The controller.
- */
- PIDController& GetController();
-
- protected:
- PIDController m_controller;
- bool m_enabled;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h
deleted file mode 100644
index 322e7b1..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-#include <vector>
-
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A CommandGroup that runs a set of commands in parallel, ending when the last
- * command ends.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class ParallelCommandGroup
- : public CommandHelper<CommandGroupBase, ParallelCommandGroup> {
- public:
- /**
- * Creates a new ParallelCommandGroup. The given commands will be executed
- * simultaneously. The command group will finish when the last command
- * finishes. If the CommandGroup is interrupted, only the commands that are
- * still running will be interrupted.
- *
- * @param commands the commands to include in this group.
- */
- explicit ParallelCommandGroup(
- std::vector<std::unique_ptr<Command>>&& commands);
-
- /**
- * Creates a new ParallelCommandGroup. The given commands will be executed
- * simultaneously. The command group will finish when the last command
- * finishes. If the CommandGroup is interrupted, only the commands that are
- * still running will be interrupted.
- *
- * @param commands the commands to include in this group.
- */
- template <class... Types,
- typename = std::enable_if_t<std::conjunction_v<
- std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
- explicit ParallelCommandGroup(Types&&... commands) {
- AddCommands(std::forward<Types>(commands)...);
- }
-
- ParallelCommandGroup(ParallelCommandGroup&& other) = default;
-
- // No copy constructors for commandgroups
- ParallelCommandGroup(const ParallelCommandGroup&) = delete;
-
- // Prevent template expansion from emulating copy ctor
- ParallelCommandGroup(ParallelCommandGroup&) = delete;
-
- template <class... Types,
- typename = std::enable_if_t<std::conjunction_v<
- std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
- void AddCommands(Types&&... commands) {
- std::vector<std::unique_ptr<Command>> foo;
- ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
- std::forward<Types>(commands))),
- ...);
- AddCommands(std::move(foo));
- }
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- bool IsFinished() override;
-
- bool RunsWhenDisabled() const override;
-
- private:
- void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
-
- std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
- bool m_runWhenDisabled{true};
- bool isRunning = false;
-};
-} // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
deleted file mode 100644
index 168d0f8..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-#include <vector>
-
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A CommandGroup that runs a set of commands in parallel, ending only when a
- * specific command (the "deadline") ends, interrupting all other commands that
- * are still running at that point.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class ParallelDeadlineGroup
- : public CommandHelper<CommandGroupBase, ParallelDeadlineGroup> {
- public:
- /**
- * Creates a new ParallelDeadlineGroup. The given commands (including the
- * deadline) will be executed simultaneously. The CommandGroup will finish
- * when the deadline finishes, interrupting all other still-running commands.
- * If the CommandGroup is interrupted, only the commands still running will be
- * interrupted.
- *
- * @param deadline the command that determines when the group ends
- * @param commands the commands to be executed
- */
- ParallelDeadlineGroup(std::unique_ptr<Command>&& deadline,
- std::vector<std::unique_ptr<Command>>&& commands);
- /**
- * Creates a new ParallelDeadlineGroup. The given commands (including the
- * deadline) will be executed simultaneously. The CommandGroup will finish
- * when the deadline finishes, interrupting all other still-running commands.
- * If the CommandGroup is interrupted, only the commands still running will be
- * interrupted.
- *
- * @param deadline the command that determines when the group ends
- * @param commands the commands to be executed
- */
- template <class T, class... Types,
- typename = std::enable_if_t<
- std::is_base_of_v<Command, std::remove_reference_t<T>>>,
- typename = std::enable_if_t<std::conjunction_v<
- std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
- explicit ParallelDeadlineGroup(T&& deadline, Types&&... commands) {
- SetDeadline(std::make_unique<std::remove_reference_t<T>>(
- std::forward<T>(deadline)));
- AddCommands(std::forward<Types>(commands)...);
- }
-
- ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default;
-
- // No copy constructors for command groups
- ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete;
-
- // Prevent template expansion from emulating copy ctor
- ParallelDeadlineGroup(ParallelDeadlineGroup&) = delete;
-
- template <class... Types,
- typename = std::enable_if_t<std::conjunction_v<
- std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
- void AddCommands(Types&&... commands) {
- std::vector<std::unique_ptr<Command>> foo;
- ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
- std::forward<Types>(commands))),
- ...);
- AddCommands(std::move(foo));
- }
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- bool IsFinished() override;
-
- bool RunsWhenDisabled() const override;
-
- private:
- void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
-
- void SetDeadline(std::unique_ptr<Command>&& deadline);
-
- std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
- Command* m_deadline;
- bool m_runWhenDisabled{true};
- bool isRunning = false;
-};
-} // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h
deleted file mode 100644
index d7411e9..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h
+++ /dev/null
@@ -1,90 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <memory>
-#include <set>
-#include <unordered_map>
-#include <utility>
-#include <vector>
-
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A CommandGroup that runs a set of commands in parallel, ending when any one
- * of the commands ends and interrupting all the others.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class ParallelRaceGroup
- : public CommandHelper<CommandGroupBase, ParallelRaceGroup> {
- public:
- /**
- * Creates a new ParallelCommandRace. The given commands will be executed
- * simultaneously, and will "race to the finish" - the first command to finish
- * ends the entire command, with all other commands being interrupted.
- *
- * @param commands the commands to include in this group.
- */
- explicit ParallelRaceGroup(std::vector<std::unique_ptr<Command>>&& commands);
-
- template <class... Types,
- typename = std::enable_if_t<std::conjunction_v<
- std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
- explicit ParallelRaceGroup(Types&&... commands) {
- AddCommands(std::forward<Types>(commands)...);
- }
-
- ParallelRaceGroup(ParallelRaceGroup&& other) = default;
-
- // No copy constructors for command groups
- ParallelRaceGroup(const ParallelRaceGroup&) = delete;
-
- // Prevent template expansion from emulating copy ctor
- ParallelRaceGroup(ParallelRaceGroup&) = delete;
-
- template <class... Types>
- void AddCommands(Types&&... commands) {
- std::vector<std::unique_ptr<Command>> foo;
- ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
- std::forward<Types>(commands))),
- ...);
- AddCommands(std::move(foo));
- }
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- bool IsFinished() override;
-
- bool RunsWhenDisabled() const override;
-
- private:
- void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
-
- std::set<std::unique_ptr<Command>> m_commands;
- bool m_runWhenDisabled{true};
- bool m_finished{false};
- bool isRunning = false;
-};
-} // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h b/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h
deleted file mode 100644
index 3f3c9e7..0000000
--- a/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <memory>
-#include <utility>
-
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that runs another command in perpetuity, ignoring that command's
- * end conditions. While this class does not extend {@link CommandGroupBase},
- * it is still considered a CommandGroup, as it allows one to compose another
- * command within it; the command instances that are passed to it cannot be
- * added to any other groups, or scheduled individually.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class PerpetualCommand : public CommandHelper<CommandBase, PerpetualCommand> {
- public:
- /**
- * Creates a new PerpetualCommand. Will run another command in perpetuity,
- * ignoring that command's end conditions, unless this command itself is
- * interrupted.
- *
- * @param command the command to run perpetually
- */
- explicit PerpetualCommand(std::unique_ptr<Command>&& command);
-
- /**
- * Creates a new PerpetualCommand. Will run another command in perpetuity,
- * ignoring that command's end conditions, unless this command itself is
- * interrupted.
- *
- * @param command the command to run perpetually
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- explicit PerpetualCommand(T&& command)
- : PerpetualCommand(std::make_unique<std::remove_reference_t<T>>(
- std::forward<T>(command))) {}
-
- PerpetualCommand(PerpetualCommand&& other) = default;
-
- // No copy constructors for command groups
- PerpetualCommand(const PerpetualCommand& other) = delete;
-
- // Prevent template expansion from emulating copy ctor
- PerpetualCommand(PerpetualCommand&) = delete;
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- private:
- std::unique_ptr<Command> m_command;
-};
-} // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/PrintCommand.h b/wpilibc/src/main/native/include/frc2/command/PrintCommand.h
deleted file mode 100644
index fb420cb..0000000
--- a/wpilibc/src/main/native/include/frc2/command/PrintCommand.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-#include <wpi/raw_ostream.h>
-
-#include "CommandHelper.h"
-#include "InstantCommand.h"
-
-namespace frc2 {
-/**
- * A command that prints a string when initialized.
- */
-class PrintCommand : public CommandHelper<InstantCommand, PrintCommand> {
- public:
- /**
- * Creates a new a PrintCommand.
- *
- * @param message the message to print
- */
- explicit PrintCommand(const wpi::Twine& message);
-
- PrintCommand(PrintCommand&& other) = default;
-
- PrintCommand(const PrintCommand& other) = default;
-
- bool RunsWhenDisabled() const override;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h
deleted file mode 100644
index b1ff9e6..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ /dev/null
@@ -1,132 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/controller/ProfiledPIDController.h"
-#include "frc2/command/CommandBase.h"
-#include "frc2/command/CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that controls an output with a ProfiledPIDController. Runs forever
- * by default - to add exit conditions and/or other behavior, subclass this
- * class. The controller calculation and output are performed synchronously in
- * the command's execute() method.
- *
- * @see ProfiledPIDController
- */
-class ProfiledPIDCommand
- : public CommandHelper<CommandBase, ProfiledPIDCommand> {
- using State = frc::TrapezoidProfile::State;
-
- public:
- /**
- * Creates a new PIDCommand, which controls the given output with a
- * ProfiledPIDController.
- *
- * @param controller the controller that controls the output.
- * @param measurementSource the measurement of the process variable
- * @param goalSource the controller's goal
- * @param useOutput the controller's output
- * @param requirements the subsystems required by this command
- */
- ProfiledPIDCommand(frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource,
- std::function<State()> goalSource,
- std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements);
-
- /**
- * Creates a new PIDCommand, which controls the given output with a
- * ProfiledPIDController.
- *
- * @param controller the controller that controls the output.
- * @param measurementSource the measurement of the process variable
- * @param goalSource the controller's goal
- * @param useOutput the controller's output
- * @param requirements the subsystems required by this command
- */
- ProfiledPIDCommand(frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource,
- std::function<units::meter_t()> goalSource,
- std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements);
-
- /**
- * Creates a new PIDCommand, which controls the given output with a
- * ProfiledPIDController with a constant goal.
- *
- * @param controller the controller that controls the output.
- * @param measurementSource the measurement of the process variable
- * @param goal the controller's goal
- * @param useOutput the controller's output
- * @param requirements the subsystems required by this command
- */
- ProfiledPIDCommand(frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource,
- State goal, std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements);
-
- /**
- * Creates a new PIDCommand, which controls the given output with a
- * ProfiledPIDController with a constant goal.
- *
- * @param controller the controller that controls the output.
- * @param measurementSource the measurement of the process variable
- * @param goal the controller's goal
- * @param useOutput the controller's output
- * @param requirements the subsystems required by this command
- */
- ProfiledPIDCommand(frc::ProfiledPIDController controller,
- std::function<units::meter_t()> measurementSource,
- units::meter_t goal,
- std::function<void(double, State)> useOutput,
- std::initializer_list<Subsystem*> requirements);
-
- ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
-
- ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- /**
- * Gets the measurement of the process variable. Wraps the passed-in function
- * for readability.
- *
- * @return The measurement of the process variable
- */
- virtual units::meter_t GetMeasurement();
-
- /**
- * Gets the measurement of the process variable. Wraps the passed-in function
- * for readability.
- *
- * @return The measurement of the process variable
- */
- virtual void UseOutput(double output, State state);
-
- /**
- * Returns the ProfiledPIDController used by the command.
- *
- * @return The ProfiledPIDController
- */
- frc::ProfiledPIDController& GetController();
-
- protected:
- frc::ProfiledPIDController m_controller;
- std::function<units::meter_t()> m_measurement;
- std::function<State()> m_goal;
- std::function<void(double, State)> m_useOutput;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
deleted file mode 100644
index 4fa47b2..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/controller/ProfiledPIDController.h"
-#include "frc2/command/SubsystemBase.h"
-
-namespace frc2 {
-/**
- * A subsystem that uses a ProfiledPIDController to control an output. The
- * controller is run synchronously from the subsystem's periodic() method.
- *
- * @see ProfiledPIDController
- */
-class ProfiledPIDSubsystem : public SubsystemBase {
- using State = frc::TrapezoidProfile::State;
-
- public:
- /**
- * Creates a new ProfiledPIDSubsystem.
- *
- * @param controller the ProfiledPIDController to use
- */
- explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller);
-
- void Periodic() override;
-
- /**
- * Uses the output from the ProfiledPIDController.
- *
- * @param output the output of the ProfiledPIDController
- */
- virtual void UseOutput(double output, State state) = 0;
-
- /**
- * Returns the goal used by the ProfiledPIDController.
- *
- * @return the goal to be used by the controller
- */
- virtual State GetGoal() = 0;
-
- /**
- * Returns the measurement of the process variable used by the
- * ProfiledPIDController.
- *
- * @return the measurement of the process variable
- */
- virtual units::meter_t GetMeasurement() = 0;
-
- /**
- * Enables the PID control. Resets the controller.
- */
- virtual void Enable();
-
- /**
- * Disables the PID control. Sets output to zero.
- */
- virtual void Disable();
-
- /**
- * Returns the ProfiledPIDController.
- *
- * @return The controller.
- */
- frc::ProfiledPIDController& GetController();
-
- protected:
- frc::ProfiledPIDController m_controller;
- bool m_enabled;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h b/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h
deleted file mode 100644
index 8906424..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/SmallVector.h>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "SetUtilities.h"
-
-namespace frc2 {
-/**
- * Schedules the given commands when this command is initialized, and ends when
- * all the commands are no longer scheduled. Useful for forking off from
- * CommandGroups. If this command is interrupted, it will cancel all of the
- * commands.
- */
-class ProxyScheduleCommand
- : public CommandHelper<CommandBase, ProxyScheduleCommand> {
- public:
- /**
- * Creates a new ProxyScheduleCommand that schedules the given commands when
- * initialized, and ends when they are all no longer scheduled.
- *
- * @param toSchedule the commands to schedule
- */
- explicit ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
-
- ProxyScheduleCommand(ProxyScheduleCommand&& other) = default;
-
- ProxyScheduleCommand(const ProxyScheduleCommand& other) = default;
-
- void Initialize() override;
-
- void End(bool interrupted) override;
-
- void Execute() override;
-
- bool IsFinished() override;
-
- private:
- wpi::SmallVector<Command*, 4> m_toSchedule;
- bool m_finished{false};
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h
deleted file mode 100644
index 592f194..0000000
--- a/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h
+++ /dev/null
@@ -1,147 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include <functional>
-#include <memory>
-
-#include <units/units.h>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "frc/controller/PIDController.h"
-#include "frc/controller/RamseteController.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/trajectory/Trajectory.h"
-#include "frc2/Timer.h"
-
-#pragma once
-
-namespace frc2 {
-/**
- * A command that uses a RAMSETE controller to follow a trajectory
- * with a differential drive.
- *
- * <p>The command handles trajectory-following, PID calculations, and
- * feedforwards internally. This is intended to be a more-or-less "complete
- * solution" that can be used by teams without a great deal of controls
- * expertise.
- *
- * <p>Advanced teams seeking more flexibility (for example, those who wish to
- * use the onboard PID functionality of a "smart" motor controller) may use the
- * secondary constructor that omits the PID and feedforward functionality,
- * returning only the raw wheel speeds from the RAMSETE controller.
- *
- * @see RamseteController
- * @see Trajectory
- */
-class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
- using voltsecondspermeter =
- units::compound_unit<units::volt, units::second,
- units::inverse<units::meter>>;
- using voltsecondssquaredpermeter =
- units::compound_unit<units::volt, units::squared<units::second>,
- units::inverse<units::meter>>;
-
- public:
- /**
- * Constructs a new RamseteCommand that, when executed, will follow the
- * provided trajectory. PID control and feedforward are handled internally,
- * and outputs are scaled -1 to 1 for easy consumption by speed controllers.
- *
- * <p>Note: The controller will *not* set the outputVolts to zero upon
- * completion of the path - this is left to the user, since it is not
- * appropriate for paths with nonstationary endstates.
- *
- * @param trajectory The trajectory to follow.
- * @param pose A function that supplies the robot pose - use one of
- * the odometry classes to provide this.
- * @param controller The RAMSETE controller used to follow the
- * trajectory.
- * @param ks Constant feedforward term for the robot drive.
- * @param kv Velocity-proportional feedforward term for the robot
- * drive.
- * @param ka Acceleration-proportional feedforward term for the
- * robot drive.
- * @param kinematics The kinematics for the robot drivetrain.
- * @param leftSpeed A function that supplies the speed of the left side
- * of the robot drive.
- * @param rightSpeed A function that supplies the speed of the right side
- * of the robot drive.
- * @param leftController The PIDController for the left side of the robot
- * drive.
- * @param rightController The PIDController for the right side of the robot
- * drive.
- * @param output A function that consumes the computed left and right
- * outputs (in volts) for the robot drive.
- * @param requirements The subsystems to require.
- */
- RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
- frc::RamseteController controller, units::volt_t ks,
- units::unit_t<voltsecondspermeter> kv,
- units::unit_t<voltsecondssquaredpermeter> ka,
- frc::DifferentialDriveKinematics kinematics,
- std::function<units::meters_per_second_t()> leftSpeed,
- std::function<units::meters_per_second_t()> rightSpeed,
- frc2::PIDController leftController,
- frc2::PIDController rightController,
- std::function<void(units::volt_t, units::volt_t)> output,
- std::initializer_list<Subsystem*> requirements);
-
- /**
- * Constructs a new RamseteCommand that, when executed, will follow the
- * provided trajectory. Performs no PID control and calculates no
- * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller,
- * and will need to be converted into a usable form by the user.
- *
- * @param trajectory The trajectory to follow.
- * @param pose A function that supplies the robot pose - use one of
- * the odometry classes to provide this.
- * @param controller The RAMSETE controller used to follow the
- * trajectory.
- * @param kinematics The kinematics for the robot drivetrain.
- * @param output A function that consumes the computed left and right
- * outputs (in volts) for the robot drive.
- * @param requirements The subsystems to require.
- */
- RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
- frc::RamseteController controller,
- frc::DifferentialDriveKinematics kinematics,
- std::function<void(units::meters_per_second_t,
- units::meters_per_second_t)>
- output,
- std::initializer_list<Subsystem*> requirements);
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- bool IsFinished() override;
-
- private:
- frc::Trajectory m_trajectory;
- std::function<frc::Pose2d()> m_pose;
- frc::RamseteController m_controller;
- const units::volt_t m_ks;
- const units::unit_t<voltsecondspermeter> m_kv;
- const units::unit_t<voltsecondssquaredpermeter> m_ka;
- frc::DifferentialDriveKinematics m_kinematics;
- std::function<units::meters_per_second_t()> m_leftSpeed;
- std::function<units::meters_per_second_t()> m_rightSpeed;
- std::unique_ptr<frc2::PIDController> m_leftController;
- std::unique_ptr<frc2::PIDController> m_rightController;
- std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
- std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
- m_outputVel;
-
- Timer m_timer;
- units::second_t m_prevTime;
- frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/RunCommand.h b/wpilibc/src/main/native/include/frc2/command/RunCommand.h
deleted file mode 100644
index 5a0524a..0000000
--- a/wpilibc/src/main/native/include/frc2/command/RunCommand.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that runs a Runnable continuously. Has no end condition as-is;
- * either subclass it or use Command.WithTimeout() or
- * Command.WithInterrupt() to give it one. If you only wish
- * to execute a Runnable once, use InstantCommand.
- */
-class RunCommand : public CommandHelper<CommandBase, RunCommand> {
- public:
- /**
- * Creates a new RunCommand. The Runnable will be run continuously until the
- * command ends. Does not run when disabled.
- *
- * @param toRun the Runnable to run
- * @param requirements the subsystems to require
- */
- RunCommand(std::function<void()> toRun,
- std::initializer_list<Subsystem*> requirements);
-
- RunCommand(RunCommand&& other) = default;
-
- RunCommand(const RunCommand& other) = default;
-
- void Execute();
-
- protected:
- std::function<void()> m_toRun;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h b/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h
deleted file mode 100644
index 422823b..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/SmallVector.h>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "SetUtilities.h"
-
-namespace frc2 {
-/**
- * Schedules the given commands when this command is initialized. Useful for
- * forking off from CommandGroups. Note that if run from a CommandGroup, the
- * group will not know about the status of the scheduled commands, and will
- * treat this command as finishing instantly.
- */
-class ScheduleCommand : public CommandHelper<CommandBase, ScheduleCommand> {
- public:
- /**
- * Creates a new ScheduleCommand that schedules the given commands when
- * initialized.
- *
- * @param toSchedule the commands to schedule
- */
- explicit ScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
-
- ScheduleCommand(ScheduleCommand&& other) = default;
-
- ScheduleCommand(const ScheduleCommand& other) = default;
-
- void Initialize() override;
-
- bool IsFinished() override;
-
- bool RunsWhenDisabled() const override;
-
- private:
- wpi::SmallVector<Command*, 4> m_toSchedule;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/SelectCommand.h b/wpilibc/src/main/native/include/frc2/command/SelectCommand.h
deleted file mode 100644
index 8dba378..0000000
--- a/wpilibc/src/main/native/include/frc2/command/SelectCommand.h
+++ /dev/null
@@ -1,153 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-#include <vector>
-
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "PrintCommand.h"
-
-namespace frc2 {
-template <typename Key>
-/**
- * Runs one of a selection of commands, either using a selector and a key to
- * command mapping, or a supplier that returns the command directly at runtime.
- * Does not actually schedule the selected command - rather, the command is run
- * through this command; this ensures that the command will behave as expected
- * if used as part of a CommandGroup. Requires the requirements of all included
- * commands, again to ensure proper functioning when used in a CommandGroup. If
- * this is undesired, consider using ScheduleCommand.
- *
- * <p>As this command contains multiple component commands within it, it is
- * technically a command group; the command instances that are passed to it
- * cannot be added to any other groups, or scheduled individually.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
- public:
- /**
- * Creates a new selectcommand.
- *
- * @param commands the map of commands to choose from
- * @param selector the selector to determine which command to run
- */
- template <class... Types,
- typename = std::enable_if_t<std::conjunction_v<
- std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
- SelectCommand(std::function<Key()> selector,
- std::pair<Key, Types>... commands)
- : m_selector{std::move(selector)} {
- std::vector<std::pair<Key, std::unique_ptr<Command>>> foo;
-
- ((void)foo.emplace_back(commands.first,
- std::make_unique<std::remove_reference_t<Types>>(
- std::move(commands.second))),
- ...);
-
- for (auto&& command : foo) {
- if (!CommandGroupBase::RequireUngrouped(command.second)) {
- return;
- }
- }
-
- for (auto&& command : foo) {
- this->AddRequirements(command.second->GetRequirements());
- m_runsWhenDisabled &= command.second->RunsWhenDisabled();
- m_commands.emplace(std::move(command.first), std::move(command.second));
- }
- }
-
- SelectCommand(
- std::function<Key()> selector,
- std::vector<std::pair<Key, std::unique_ptr<Command>>>&& commands)
- : m_selector{std::move(selector)} {
- for (auto&& command : commands) {
- if (!CommandGroupBase::RequireUngrouped(command.second)) {
- return;
- }
- }
-
- for (auto&& command : commands) {
- this->AddRequirements(command.second->GetRequirements());
- m_runsWhenDisabled &= command.second->RunsWhenDisabled();
- m_commands.emplace(std::move(command.first), std::move(command.second));
- }
- }
-
- // No copy constructors for command groups
- SelectCommand(const SelectCommand& other) = delete;
-
- // Prevent template expansion from emulating copy ctor
- SelectCommand(SelectCommand&) = delete;
-
- /**
- * Creates a new selectcommand.
- *
- * @param toRun a supplier providing the command to run
- */
- explicit SelectCommand(std::function<Command*()> toRun) : m_toRun{toRun} {}
-
- SelectCommand(SelectCommand&& other) = default;
-
- void Initialize() override;
-
- void Execute() override { m_selectedCommand->Execute(); }
-
- void End(bool interrupted) override {
- return m_selectedCommand->End(interrupted);
- }
-
- bool IsFinished() override { return m_selectedCommand->IsFinished(); }
-
- bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
-
- protected:
- std::unique_ptr<Command> TransferOwnership() && override {
- return std::make_unique<SelectCommand>(std::move(*this));
- }
-
- private:
- std::unordered_map<Key, std::unique_ptr<Command>> m_commands;
- std::function<Key()> m_selector;
- std::function<Command*()> m_toRun;
- Command* m_selectedCommand;
- bool m_runsWhenDisabled = true;
-};
-
-template <typename T>
-void SelectCommand<T>::Initialize() {
- if (m_selector) {
- auto find = m_commands.find(m_selector());
- if (find == m_commands.end()) {
- m_selectedCommand = new PrintCommand(
- "SelectCommand selector value does not correspond to any command!");
- return;
- }
- m_selectedCommand = find->second.get();
- } else {
- m_selectedCommand = m_toRun();
- }
- m_selectedCommand->Initialize();
-}
-
-} // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h
deleted file mode 100644
index dd1f2ce..0000000
--- a/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h
+++ /dev/null
@@ -1,104 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <limits>
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include <wpi/ArrayRef.h>
-
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-#include "frc/ErrorBase.h"
-#include "frc/WPIErrors.h"
-
-namespace frc2 {
-
-const size_t invalid_index = std::numeric_limits<size_t>::max();
-
-/**
- * A CommandGroups that runs a list of commands in sequence.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class SequentialCommandGroup
- : public CommandHelper<CommandGroupBase, SequentialCommandGroup> {
- public:
- /**
- * Creates a new SequentialCommandGroup. The given commands will be run
- * sequentially, with the CommandGroup finishing when the last command
- * finishes.
- *
- * @param commands the commands to include in this group.
- */
- explicit SequentialCommandGroup(
- std::vector<std::unique_ptr<Command>>&& commands);
-
- /**
- * Creates a new SequentialCommandGroup. The given commands will be run
- * sequentially, with the CommandGroup finishing when the last command
- * finishes.
- *
- * @param commands the commands to include in this group.
- */
- template <class... Types,
- typename = std::enable_if_t<std::conjunction_v<
- std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
- explicit SequentialCommandGroup(Types&&... commands) {
- AddCommands(std::forward<Types>(commands)...);
- }
-
- SequentialCommandGroup(SequentialCommandGroup&& other) = default;
-
- // No copy constructors for command groups
- SequentialCommandGroup(const SequentialCommandGroup&) = delete;
-
- // Prevent template expansion from emulating copy ctor
- SequentialCommandGroup(SequentialCommandGroup&) = delete;
-
- template <class... Types,
- typename = std::enable_if_t<std::conjunction_v<
- std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
- void AddCommands(Types&&... commands) {
- std::vector<std::unique_ptr<Command>> foo;
- ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
- std::forward<Types>(commands))),
- ...);
- AddCommands(std::move(foo));
- }
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- bool IsFinished() override;
-
- bool RunsWhenDisabled() const override;
-
- private:
- void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
-
- wpi::SmallVector<std::unique_ptr<Command>, 4> m_commands;
- size_t m_currentCommandIndex{invalid_index};
- bool m_runWhenDisabled{true};
-};
-} // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/SetUtilities.h b/wpilibc/src/main/native/include/frc2/command/SetUtilities.h
deleted file mode 100644
index d21f572..0000000
--- a/wpilibc/src/main/native/include/frc2/command/SetUtilities.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/ArrayRef.h>
-#include <wpi/SmallVector.h>
-
-namespace frc2 {
-template <typename T>
-void SetInsert(wpi::SmallVectorImpl<T*>& vector, wpi::ArrayRef<T*> toAdd) {
- for (auto addCommand : toAdd) {
- bool exists = false;
- for (auto existingCommand : vector) {
- if (addCommand == existingCommand) {
- exists = true;
- break;
- }
- }
- if (!exists) {
- vector.emplace_back(addCommand);
- }
- }
-}
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h
deleted file mode 100644
index 987b9db..0000000
--- a/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that runs a given runnable when it is initalized, and another
- * runnable when it ends. Useful for running and then stopping a motor, or
- * extending and then retracting a solenoid. Has no end condition as-is; either
- * subclass it or use Command.WithTimeout() or Command.WithInterrupt() to give
- * it one.
- */
-class StartEndCommand : public CommandHelper<CommandBase, StartEndCommand> {
- public:
- /**
- * Creates a new StartEndCommand. Will run the given runnables when the
- * command starts and when it ends.
- *
- * @param onInit the Runnable to run on command init
- * @param onEnd the Runnable to run on command end
- * @param requirements the subsystems required by this command
- */
- StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
- std::initializer_list<Subsystem*> requirements);
-
- StartEndCommand(StartEndCommand&& other) = default;
-
- StartEndCommand(const StartEndCommand& other);
-
- void Initialize() override;
-
- void End(bool interrupted) override;
-
- protected:
- std::function<void()> m_onInit;
- std::function<void()> m_onEnd;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/Subsystem.h b/wpilibc/src/main/native/include/frc2/command/Subsystem.h
deleted file mode 100644
index 5eb10bc..0000000
--- a/wpilibc/src/main/native/include/frc2/command/Subsystem.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc2/command/CommandScheduler.h>
-
-#include <utility>
-
-namespace frc2 {
-class Command;
-/**
- * A robot subsystem. Subsystems are the basic unit of robot organization in
- * the Command-based framework; they encapsulate low-level hardware objects
- * (motor controllers, sensors, etc) and provide methods through which they can
- * be used by Commands. Subsystems are used by the CommandScheduler's resource
- * management system to ensure multiple robot actions are not "fighting" over
- * the same hardware; Commands that use a subsystem should include that
- * subsystem in their GetRequirements() method, and resources used within a
- * subsystem should generally remain encapsulated and not be shared by other
- * parts of the robot.
- *
- * <p>Subsystems must be registered with the scheduler with the
- * CommandScheduler.RegisterSubsystem() method in order for the
- * Periodic() method to be called. It is recommended that this method be called
- * from the constructor of users' Subsystem implementations. The
- * SubsystemBase class offers a simple base for user implementations
- * that handles this.
- *
- * @see Command
- * @see CommandScheduler
- * @see SubsystemBase
- */
-class Subsystem {
- public:
- ~Subsystem();
- /**
- * This method is called periodically by the CommandScheduler. Useful for
- * updating subsystem-specific state that you don't want to offload to a
- * Command. Teams should try to be consistent within their own codebases
- * about which responsibilities will be handled by Commands, and which will be
- * handled here.
- */
- virtual void Periodic();
-
- /**
- * Sets the default Command of the subsystem. The default command will be
- * automatically scheduled when no other commands are scheduled that require
- * the subsystem. Default commands should generally not end on their own, i.e.
- * their IsFinished() method should always return false. Will automatically
- * register this subsystem with the CommandScheduler.
- *
- * @param defaultCommand the default command to associate with this subsystem
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- void SetDefaultCommand(T&& defaultCommand) {
- CommandScheduler::GetInstance().SetDefaultCommand(
- this, std::forward<T>(defaultCommand));
- }
-
- /**
- * Gets the default command for this subsystem. Returns null if no default
- * command is currently associated with the subsystem.
- *
- * @return the default command associated with this subsystem
- */
- Command* GetDefaultCommand() const;
-
- /**
- * Returns the command currently running on this subsystem. Returns null if
- * no command is currently scheduled that requires this subsystem.
- *
- * @return the scheduled command currently requiring this subsystem
- */
- Command* GetCurrentCommand() const;
-
- /**
- * Registers this subsystem with the CommandScheduler, allowing its
- * Periodic() method to be called when the scheduler runs.
- */
- void Register();
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h b/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h
deleted file mode 100644
index e7dffc7..0000000
--- a/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
-
-#include <string>
-
-#include "Subsystem.h"
-
-namespace frc2 {
-/**
- * A base for subsystems that handles registration in the constructor, and
- * provides a more intuitive method for setting the default command.
- */
-class SubsystemBase : public Subsystem,
- public frc::Sendable,
- public frc::SendableHelper<SubsystemBase> {
- public:
- void InitSendable(frc::SendableBuilder& builder) override;
-
- /**
- * Gets the name of this Subsystem.
- *
- * @return Name
- */
- std::string GetName() const;
-
- /**
- * Sets the name of this Subsystem.
- *
- * @param name name
- */
- void SetName(const wpi::Twine& name);
-
- /**
- * Gets the subsystem name of this Subsystem.
- *
- * @return Subsystem name
- */
- std::string GetSubsystem() const;
-
- /**
- * Sets the subsystem name of this Subsystem.
- *
- * @param subsystem subsystem name
- */
- void SetSubsystem(const wpi::Twine& name);
-
- /**
- * Associate a Sendable with this Subsystem.
- * Also update the child's name.
- *
- * @param name name to give child
- * @param child sendable
- */
- void AddChild(std::string name, frc::Sendable* child);
-
- protected:
- SubsystemBase();
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
deleted file mode 100644
index 6864f53..0000000
--- a/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#include <frc/trajectory/TrapezoidProfile.h>
-#include <frc2/Timer.h>
-
-#include <functional>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-#pragma once
-
-namespace frc2 {
-/**
- * A command that runs a TrapezoidProfile. Useful for smoothly controlling
- * mechanism motion.
- *
- * @see TrapezoidProfile
- */
-class TrapezoidProfileCommand
- : public CommandHelper<CommandBase, TrapezoidProfileCommand> {
- public:
- /**
- * Creates a new TrapezoidProfileCommand that will execute the given
- * TrapezoidalProfile. Output will be piped to the provided consumer function.
- *
- * @param profile The motion profile to execute.
- * @param output The consumer for the profile output.
- */
- TrapezoidProfileCommand(
- frc::TrapezoidProfile profile,
- std::function<void(frc::TrapezoidProfile::State)> output,
- std::initializer_list<Subsystem*> requirements);
-
- void Initialize() override;
-
- void Execute() override;
-
- void End(bool interrupted) override;
-
- bool IsFinished() override;
-
- private:
- frc::TrapezoidProfile m_profile;
- std::function<void(frc::TrapezoidProfile::State)> m_output;
-
- Timer m_timer;
-};
-
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/WaitCommand.h b/wpilibc/src/main/native/include/frc2/command/WaitCommand.h
deleted file mode 100644
index 1544592..0000000
--- a/wpilibc/src/main/native/include/frc2/command/WaitCommand.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-#include <wpi/Twine.h>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "frc2/Timer.h"
-
-namespace frc2 {
-/**
- * A command that does nothing but takes a specified amount of time to finish.
- * Useful for CommandGroups. Can also be subclassed to make a command with an
- * internal timer.
- */
-class WaitCommand : public CommandHelper<CommandBase, WaitCommand> {
- public:
- /**
- * Creates a new WaitCommand. This command will do nothing, and end after the
- * specified duration.
- *
- * @param duration the time to wait
- */
- explicit WaitCommand(units::second_t duration);
-
- WaitCommand(WaitCommand&& other) = default;
-
- WaitCommand(const WaitCommand& other) = default;
-
- void Initialize() override;
-
- void End(bool interrupted) override;
-
- bool IsFinished() override;
-
- bool RunsWhenDisabled() const override;
-
- protected:
- Timer m_timer;
-
- private:
- units::second_t m_duration;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h b/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h
deleted file mode 100644
index 8da18e8..0000000
--- a/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "CommandBase.h"
-#include "frc/Timer.h"
-#include "frc2/command/CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that does nothing but ends after a specified match time or
- * condition. Useful for CommandGroups.
- */
-class WaitUntilCommand : public CommandHelper<CommandBase, WaitUntilCommand> {
- public:
- /**
- * Creates a new WaitUntilCommand that ends after a given condition becomes
- * true.
- *
- * @param condition the condition to determine when to end
- */
- explicit WaitUntilCommand(std::function<bool()> condition);
-
- /**
- * Creates a new WaitUntilCommand that ends after a given match time.
- *
- * <p>NOTE: The match timer used for this command is UNOFFICIAL. Using this
- * command does NOT guarantee that the time at which the action is performed
- * will be judged to be legal by the referees. When in doubt, add a safety
- * factor or time the action manually.
- *
- * @param time the match time after which to end, in seconds
- */
- explicit WaitUntilCommand(double time);
-
- WaitUntilCommand(WaitUntilCommand&& other) = default;
-
- WaitUntilCommand(const WaitUntilCommand& other) = default;
-
- bool IsFinished() override;
-
- bool RunsWhenDisabled() const override;
-
- private:
- std::function<bool()> m_condition;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/Button.h b/wpilibc/src/main/native/include/frc2/command/button/Button.h
deleted file mode 100644
index 3e0f7fe..0000000
--- a/wpilibc/src/main/native/include/frc2/command/button/Button.h
+++ /dev/null
@@ -1,207 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-#include <utility>
-
-#include "Trigger.h"
-
-namespace frc2 {
-class Command;
-/**
- * A class used to bind command scheduling to button presses. Can be composed
- * with other buttons with the operators in Trigger.
- *
- * @see Trigger
- */
-class Button : public Trigger {
- public:
- /**
- * Create a new button that is pressed when the given condition is true.
- *
- * @param isActive Whether the button is pressed.
- */
- explicit Button(std::function<bool()> isPressed);
-
- /**
- * Create a new button that is pressed active (default constructor) - activity
- * can be further determined by subclass code.
- */
- Button() = default;
-
- /**
- * Binds a command to start when the button is pressed. Takes a
- * raw pointer, and so is non-owning; users are responsible for the lifespan
- * of the command.
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- Button WhenPressed(Command* command, bool interruptible = true);
-
- /**
- * Binds a command to start when the button is pressed. Transfers
- * command ownership to the button scheduler, so the user does not have to
- * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
- * *copied.*
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- Button WhenPressed(T&& command, bool interruptible = true) {
- WhenActive(std::forward<T>(command), interruptible);
- return *this;
- }
-
- /**
- * Binds a runnable to execute when the button is pressed.
- *
- * @param toRun the runnable to execute.
- */
- Button WhenPressed(std::function<void()> toRun);
-
- /**
- * Binds a command to be started repeatedly while the button is pressed, and
- * cancelled when it is released. Takes a raw pointer, and so is non-owning;
- * users are responsible for the lifespan of the command.
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The button, for chained calls.
- */
- Button WhileHeld(Command* command, bool interruptible = true);
-
- /**
- * Binds a command to be started repeatedly while the button is pressed, and
- * cancelled when it is released. Transfers command ownership to the button
- * scheduler, so the user does not have to worry about lifespan - rvalue refs
- * will be *moved*, lvalue refs will be *copied.*
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The button, for chained calls.
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- Button WhileHeld(T&& command, bool interruptible = true) {
- WhileActiveContinous(std::forward<T>(command), interruptible);
- return *this;
- }
-
- /**
- * Binds a runnable to execute repeatedly while the button is pressed.
- *
- * @param toRun the runnable to execute.
- */
- Button WhileHeld(std::function<void()> toRun);
-
- /**
- * Binds a command to be started when the button is pressed, and cancelled
- * when it is released. Takes a raw pointer, and so is non-owning; users are
- * responsible for the lifespan of the command.
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The button, for chained calls.
- */
- Button WhenHeld(Command* command, bool interruptible = true);
-
- /**
- * Binds a command to be started when the button is pressed, and cancelled
- * when it is released. Transfers command ownership to the button scheduler,
- * so the user does not have to worry about lifespan - rvalue refs will be
- * *moved*, lvalue refs will be *copied.*
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The button, for chained calls.
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- Button WhenHeld(T&& command, bool interruptible = true) {
- WhileActiveOnce(std::forward<T>(command), interruptible);
- return *this;
- }
-
- /**
- * Binds a command to start when the button is released. Takes a
- * raw pointer, and so is non-owning; users are responsible for the lifespan
- * of the command.
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The button, for chained calls.
- */
- Button WhenReleased(Command* command, bool interruptible = true);
-
- /**
- * Binds a command to start when the button is pressed. Transfers
- * command ownership to the button scheduler, so the user does not have to
- * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
- * *copied.*
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The button, for chained calls.
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- Button WhenReleased(T&& command, bool interruptible = true) {
- WhenInactive(std::forward<T>(command), interruptible);
- return *this;
- }
-
- /**
- * Binds a runnable to execute when the button is released.
- *
- * @param toRun the runnable to execute.
- */
- Button WhenReleased(std::function<void()> toRun);
-
- /**
- * Binds a command to start when the button is pressed, and be cancelled when
- * it is pressed again. Takes a raw pointer, and so is non-owning; users are
- * responsible for the lifespan of the command.
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The button, for chained calls.
- */
- Button ToggleWhenPressed(Command* command, bool interruptible = true);
-
- /**
- * Binds a command to start when the button is pressed, and be cancelled when
- * it is pessed again. Transfers command ownership to the button scheduler,
- * so the user does not have to worry about lifespan - rvalue refs will be
- * *moved*, lvalue refs will be *copied.*
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The button, for chained calls.
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- Button ToggleWhenPressed(T&& command, bool interruptible = true) {
- ToggleWhenActive(std::forward<T>(command), interruptible);
- return *this;
- }
-
- /**
- * Binds a command to be cancelled when the button is pressed. Takes a
- * raw pointer, and so is non-owning; users are responsible for the lifespan
- * and scheduling of the command.
- *
- * @param command The command to bind.
- * @return The button, for chained calls.
- */
- Button CancelWhenPressed(Command* command);
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h b/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h
deleted file mode 100644
index a23738b..0000000
--- a/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-#include <frc/GenericHID.h>
-
-#include "Button.h"
-
-namespace frc2 {
-/**
- * A class used to bind command scheduling to joystick button presses. Can be
- * composed with other buttons with the operators in Trigger.
- *
- * @see Trigger
- */
-class JoystickButton : public Button {
- public:
- /**
- * Creates a JoystickButton that commands can be bound to.
- *
- * @param joystick The joystick on which the button is located.
- * @param buttonNumber The number of the button on the joystic.
- */
- explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber)
- : m_joystick{joystick}, m_buttonNumber{buttonNumber} {}
-
- bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); }
-
- private:
- frc::GenericHID* m_joystick;
- int m_buttonNumber;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/POVButton.h b/wpilibc/src/main/native/include/frc2/command/button/POVButton.h
deleted file mode 100644
index 758cab2..0000000
--- a/wpilibc/src/main/native/include/frc2/command/button/POVButton.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-#include <frc/GenericHID.h>
-
-#include "Button.h"
-
-namespace frc2 {
-/**
- * A class used to bind command scheduling to joystick POV presses. Can be
- * composed with other buttons with the operators in Trigger.
- *
- * @see Trigger
- */
-class POVButton : public Button {
- public:
- /**
- * Creates a POVButton that commands can be bound to.
- *
- * @param joystick The joystick on which the button is located.
- * @param angle The angle of the POV corresponding to a button press.
- * @param povNumber The number of the POV on the joystick.
- */
- POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0)
- : m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {}
-
- bool Get() const override {
- return m_joystick->GetPOV(m_povNumber) == m_angle;
- }
-
- private:
- frc::GenericHID* m_joystick;
- int m_angle;
- int m_povNumber;
-};
-} // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/Trigger.h b/wpilibc/src/main/native/include/frc2/command/button/Trigger.h
deleted file mode 100644
index 1a65ff6..0000000
--- a/wpilibc/src/main/native/include/frc2/command/button/Trigger.h
+++ /dev/null
@@ -1,325 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc2/command/Command.h>
-#include <frc2/command/CommandScheduler.h>
-
-#include <atomic>
-#include <memory>
-#include <utility>
-
-namespace frc2 {
-class Command;
-/**
- * A class used to bind command scheduling to events. The
- * Trigger class is a base for all command-event-binding classes, and so the
- * methods are named fairly abstractly; for purpose-specific wrappers, see
- * Button.
- *
- * @see Button
- */
-class Trigger {
- public:
- /**
- * Create a new trigger that is active when the given condition is true.
- *
- * @param isActive Whether the trigger is active.
- */
- explicit Trigger(std::function<bool()> isActive)
- : m_isActive{std::move(isActive)} {}
-
- /**
- * Create a new trigger that is never active (default constructor) - activity
- * can be further determined by subclass code.
- */
- Trigger() {
- m_isActive = [] { return false; };
- }
-
- Trigger(const Trigger& other);
-
- /**
- * Returns whether the trigger is active. Can be overridden by a subclass.
- *
- * @return Whether the trigger is active.
- */
- virtual bool Get() const { return m_isActive(); }
-
- /**
- * Binds a command to start when the trigger becomes active. Takes a
- * raw pointer, and so is non-owning; users are responsible for the lifespan
- * of the command.
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- Trigger WhenActive(Command* command, bool interruptible = true);
-
- /**
- * Binds a command to start when the trigger becomes active. Transfers
- * command ownership to the button scheduler, so the user does not have to
- * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
- * *copied.*
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- Trigger WhenActive(T&& command, bool interruptible = true) {
- CommandScheduler::GetInstance().AddButton(
- [pressedLast = Get(), *this,
- command = std::make_unique<std::remove_reference_t<T>>(
- std::forward<T>(command)),
- interruptible]() mutable {
- bool pressed = Get();
-
- if (!pressedLast && pressed) {
- command->Schedule(interruptible);
- }
-
- pressedLast = pressed;
- });
-
- return *this;
- }
-
- /**
- * Binds a runnable to execute when the trigger becomes active.
- *
- * @param toRun the runnable to execute.
- */
- Trigger WhenActive(std::function<void()> toRun);
-
- /**
- * Binds a command to be started repeatedly while the trigger is active, and
- * cancelled when it becomes inactive. Takes a raw pointer, and so is
- * non-owning; users are responsible for the lifespan of the command.
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- Trigger WhileActiveContinous(Command* command, bool interruptible = true);
-
- /**
- * Binds a command to be started repeatedly while the trigger is active, and
- * cancelled when it becomes inactive. Transfers command ownership to the
- * button scheduler, so the user does not have to worry about lifespan -
- * rvalue refs will be *moved*, lvalue refs will be *copied.*
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
- CommandScheduler::GetInstance().AddButton(
- [pressedLast = Get(), *this,
- command = std::make_unique<std::remove_reference_t<T>>(
- std::forward<T>(command)),
- interruptible]() mutable {
- bool pressed = Get();
-
- if (pressed) {
- command->Schedule(interruptible);
- } else if (pressedLast && !pressed) {
- command->Cancel();
- }
-
- pressedLast = pressed;
- });
- return *this;
- }
-
- /**
- * Binds a runnable to execute repeatedly while the trigger is active.
- *
- * @param toRun the runnable to execute.
- */
- Trigger WhileActiveContinous(std::function<void()> toRun);
-
- /**
- * Binds a command to be started when the trigger becomes active, and
- * cancelled when it becomes inactive. Takes a raw pointer, and so is
- * non-owning; users are responsible for the lifespan of the command.
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- Trigger WhileActiveOnce(Command* command, bool interruptible = true);
-
- /**
- * Binds a command to be started when the trigger becomes active, and
- * cancelled when it becomes inactive. Transfers command ownership to the
- * button scheduler, so the user does not have to worry about lifespan -
- * rvalue refs will be *moved*, lvalue refs will be *copied.*
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
- CommandScheduler::GetInstance().AddButton(
- [pressedLast = Get(), *this,
- command = std::make_unique<std::remove_reference_t<T>>(
- std::forward<T>(command)),
- interruptible]() mutable {
- bool pressed = Get();
-
- if (!pressedLast && pressed) {
- command->Schedule(interruptible);
- } else if (pressedLast && !pressed) {
- command->Cancel();
- }
-
- pressedLast = pressed;
- });
- return *this;
- }
-
- /**
- * Binds a command to start when the trigger becomes inactive. Takes a
- * raw pointer, and so is non-owning; users are responsible for the lifespan
- * of the command.
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- Trigger WhenInactive(Command* command, bool interruptible = true);
-
- /**
- * Binds a command to start when the trigger becomes inactive. Transfers
- * command ownership to the button scheduler, so the user does not have to
- * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
- * *copied.*
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- Trigger WhenInactive(T&& command, bool interruptible = true) {
- CommandScheduler::GetInstance().AddButton(
- [pressedLast = Get(), *this,
- command = std::make_unique<std::remove_reference_t<T>>(
- std::forward<T>(command)),
- interruptible]() mutable {
- bool pressed = Get();
-
- if (pressedLast && !pressed) {
- command->Schedule(interruptible);
- }
-
- pressedLast = pressed;
- });
- return *this;
- }
-
- /**
- * Binds a runnable to execute when the trigger becomes inactive.
- *
- * @param toRun the runnable to execute.
- */
- Trigger WhenInactive(std::function<void()> toRun);
-
- /**
- * Binds a command to start when the trigger becomes active, and be cancelled
- * when it again becomes active. Takes a raw pointer, and so is non-owning;
- * users are responsible for the lifespan of the command.
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- Trigger ToggleWhenActive(Command* command, bool interruptible = true);
-
- /**
- * Binds a command to start when the trigger becomes active, and be cancelled
- * when it again becomes active. Transfers command ownership to the button
- * scheduler, so the user does not have to worry about lifespan - rvalue refs
- * will be *moved*, lvalue refs will be *copied.*
- *
- * @param command The command to bind.
- * @param interruptible Whether the command should be interruptible.
- * @return The trigger, for chained calls.
- */
- template <class T, typename = std::enable_if_t<std::is_base_of_v<
- Command, std::remove_reference_t<T>>>>
- Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
- CommandScheduler::GetInstance().AddButton(
- [pressedLast = Get(), *this,
- command = std::make_unique<std::remove_reference_t<T>>(
- std::forward<T>(command)),
- interruptible]() mutable {
- bool pressed = Get();
-
- if (!pressedLast && pressed) {
- if (command->IsScheduled()) {
- command->Cancel();
- } else {
- command->Schedule(interruptible);
- }
- }
-
- pressedLast = pressed;
- });
- return *this;
- }
-
- /**
- * Binds a command to be cancelled when the trigger becomes active. Takes a
- * raw pointer, and so is non-owning; users are responsible for the lifespan
- * and scheduling of the command.
- *
- * @param command The command to bind.
- * @return The trigger, for chained calls.
- */
- Trigger CancelWhenActive(Command* command);
-
- /**
- * Composes two triggers with logical AND.
- *
- * @return A trigger which is active when both component triggers are active.
- */
- Trigger operator&&(Trigger rhs) {
- return Trigger([*this, rhs] { return Get() && rhs.Get(); });
- }
-
- /**
- * Composes two triggers with logical OR.
- *
- * @return A trigger which is active when either component trigger is active.
- */
- Trigger operator||(Trigger rhs) {
- return Trigger([*this, rhs] { return Get() || rhs.Get(); });
- }
-
- /**
- * Composes a trigger with logical NOT.
- *
- * @return A trigger which is active when the component trigger is inactive,
- * and vice-versa.
- */
- Trigger operator!() {
- return Trigger([*this] { return !Get(); });
- }
-
- private:
- std::function<bool()> m_isActive;
-};
-} // namespace frc2