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/TrapezoidProfile.cpp b/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
deleted file mode 100644
index 6026aa5..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
+++ /dev/null
@@ -1,158 +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"
-
-using namespace frc;
-
-TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
- State initial)
- : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
- m_constraints(constraints),
- m_initial(Direct(initial)),
- m_goal(Direct(goal)) {
- if (m_initial.velocity > m_constraints.maxVelocity) {
- m_initial.velocity = m_constraints.maxVelocity;
- }
-
- // Deal with a possibly truncated motion profile (with nonzero initial or
- // final velocity) by calculating the parameters as if the profile began and
- // ended at zero velocity
- units::second_t cutoffBegin =
- m_initial.velocity / m_constraints.maxAcceleration;
- units::meter_t cutoffDistBegin =
- cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
-
- units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
- units::meter_t cutoffDistEnd =
- cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
-
- // Now we can calculate the parameters as if it was a full trapezoid instead
- // of a truncated one
-
- units::meter_t fullTrapezoidDist =
- cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
- units::second_t accelerationTime =
- m_constraints.maxVelocity / m_constraints.maxAcceleration;
-
- units::meter_t fullSpeedDist =
- fullTrapezoidDist -
- accelerationTime * accelerationTime * m_constraints.maxAcceleration;
-
- // Handle the case where the profile never reaches full speed
- if (fullSpeedDist < 0_m) {
- accelerationTime =
- units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
- fullSpeedDist = 0_m;
- }
-
- m_endAccel = accelerationTime - cutoffBegin;
- m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
- m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
-}
-
-TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
- State result = m_initial;
-
- if (t < m_endAccel) {
- result.velocity += t * m_constraints.maxAcceleration;
- result.position +=
- (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
- } else if (t < m_endFullSpeed) {
- result.velocity = m_constraints.maxVelocity;
- result.position += (m_initial.velocity +
- m_endAccel * m_constraints.maxAcceleration / 2.0) *
- m_endAccel +
- m_constraints.maxVelocity * (t - m_endAccel);
- } else if (t <= m_endDeccel) {
- result.velocity =
- m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
- units::second_t timeLeft = m_endDeccel - t;
- result.position =
- m_goal.position -
- (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
- timeLeft;
- } else {
- result = m_goal;
- }
-
- return Direct(result);
-}
-
-units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
- units::meter_t position = m_initial.position * m_direction;
- units::meters_per_second_t velocity = m_initial.velocity * m_direction;
-
- units::second_t endAccel = m_endAccel * m_direction;
- units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
-
- if (target < position) {
- endAccel *= -1.0;
- endFullSpeed *= -1.0;
- velocity *= -1.0;
- }
-
- endAccel = units::math::max(endAccel, 0_s);
- endFullSpeed = units::math::max(endFullSpeed, 0_s);
- units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
- endDeccel = units::math::max(endDeccel, 0_s);
-
- const units::meters_per_second_squared_t acceleration =
- m_constraints.maxAcceleration;
- const units::meters_per_second_squared_t decceleration =
- -m_constraints.maxAcceleration;
-
- units::meter_t distToTarget = units::math::abs(target - position);
-
- if (distToTarget < 1e-6_m) {
- return 0_s;
- }
-
- units::meter_t accelDist =
- velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
-
- units::meters_per_second_t deccelVelocity;
- if (endAccel > 0_s) {
- deccelVelocity = units::math::sqrt(
- units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
- } else {
- deccelVelocity = velocity;
- }
-
- units::meter_t deccelDist =
- deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
-
- deccelDist = units::math::max(deccelDist, 0_m);
-
- units::meter_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
-
- if (accelDist > distToTarget) {
- accelDist = distToTarget;
- fullSpeedDist = 0_m;
- deccelDist = 0_m;
- } else if (accelDist + fullSpeedDist > distToTarget) {
- fullSpeedDist = distToTarget - accelDist;
- deccelDist = 0_m;
- } else {
- deccelDist = distToTarget - fullSpeedDist - accelDist;
- }
-
- units::second_t accelTime =
- (-velocity + units::math::sqrt(units::math::abs(
- velocity * velocity + 2 * acceleration * accelDist))) /
- acceleration;
-
- units::second_t deccelTime =
- (-deccelVelocity +
- units::math::sqrt(units::math::abs(deccelVelocity * deccelVelocity +
- 2 * decceleration * deccelDist))) /
- decceleration;
-
- units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
-
- return accelTime + fullSpeedTime + deccelTime;
-}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/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 {};
+}