Rename our allwpilib (which is now 2020) to not have 2019 in the name
Change-Id: I3c07f85ed32ab8b97db765a9b43f2a6ce7da964a
diff --git a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
new file mode 100644
index 0000000..a760daa
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ADXL345_I2C.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress)
+ : m_i2c(port, deviceAddress),
+ m_simDevice("ADXL345_I2C", port, deviceAddress) {
+ if (m_simDevice) {
+ m_simRange =
+ m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
+ m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
+ m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+ m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
+ }
+ // Turn on the measurements
+ m_i2c.Write(kPowerCtlRegister, kPowerCtl_Measure);
+ // Specify the data format to read
+ SetRange(range);
+
+ HAL_Report(HALUsageReporting::kResourceType_ADXL345,
+ HALUsageReporting::kADXL345_I2C, 0);
+
+ SendableRegistry::GetInstance().AddLW(this, "ADXL345_I2C", port);
+}
+
+void ADXL345_I2C::SetRange(Range range) {
+ m_i2c.Write(kDataFormatRegister,
+ kDataFormat_FullRes | static_cast<uint8_t>(range));
+}
+
+double ADXL345_I2C::GetX() { return GetAcceleration(kAxis_X); }
+
+double ADXL345_I2C::GetY() { return GetAcceleration(kAxis_Y); }
+
+double ADXL345_I2C::GetZ() { return GetAcceleration(kAxis_Z); }
+
+double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis) {
+ if (axis == kAxis_X && m_simX) return m_simX.Get();
+ if (axis == kAxis_Y && m_simY) return m_simY.Get();
+ if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
+ int16_t rawAccel = 0;
+ m_i2c.Read(kDataRegister + static_cast<int>(axis), sizeof(rawAccel),
+ reinterpret_cast<uint8_t*>(&rawAccel));
+ return rawAccel * kGsPerLSB;
+}
+
+ADXL345_I2C::AllAxes ADXL345_I2C::GetAccelerations() {
+ AllAxes data;
+ if (m_simX && m_simY && m_simZ) {
+ data.XAxis = m_simX.Get();
+ data.YAxis = m_simY.Get();
+ data.ZAxis = m_simZ.Get();
+ return data;
+ }
+ int16_t rawData[3];
+ m_i2c.Read(kDataRegister, sizeof(rawData),
+ reinterpret_cast<uint8_t*>(rawData));
+
+ data.XAxis = rawData[0] * kGsPerLSB;
+ data.YAxis = rawData[1] * kGsPerLSB;
+ data.ZAxis = rawData[2] * kGsPerLSB;
+ return data;
+}
+
+void ADXL345_I2C::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("3AxisAccelerometer");
+ auto x = builder.GetEntry("X").GetHandle();
+ auto y = builder.GetEntry("Y").GetHandle();
+ auto z = builder.GetEntry("Z").GetHandle();
+ builder.SetUpdateTable([=]() {
+ auto data = GetAccelerations();
+ nt::NetworkTableEntry(x).SetDouble(data.XAxis);
+ nt::NetworkTableEntry(y).SetDouble(data.YAxis);
+ nt::NetworkTableEntry(z).SetDouble(data.ZAxis);
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
new file mode 100644
index 0000000..077a7ff
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
@@ -0,0 +1,117 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ADXL345_SPI.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range)
+ : m_spi(port), m_simDevice("ADXL345_SPI", port) {
+ if (m_simDevice) {
+ m_simRange =
+ m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
+ m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
+ m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+ m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
+ }
+ m_spi.SetClockRate(500000);
+ m_spi.SetMSBFirst();
+ m_spi.SetSampleDataOnTrailingEdge();
+ m_spi.SetClockActiveLow();
+ m_spi.SetChipSelectActiveHigh();
+
+ uint8_t commands[2];
+ // Turn on the measurements
+ commands[0] = kPowerCtlRegister;
+ commands[1] = kPowerCtl_Measure;
+ m_spi.Transaction(commands, commands, 2);
+
+ SetRange(range);
+
+ HAL_Report(HALUsageReporting::kResourceType_ADXL345,
+ HALUsageReporting::kADXL345_SPI);
+
+ SendableRegistry::GetInstance().AddLW(this, "ADXL345_SPI", port);
+}
+
+void ADXL345_SPI::SetRange(Range range) {
+ uint8_t commands[2];
+
+ // Specify the data format to read
+ commands[0] = kDataFormatRegister;
+ commands[1] = kDataFormat_FullRes | static_cast<uint8_t>(range & 0x03);
+ m_spi.Transaction(commands, commands, 2);
+
+ if (m_simRange) m_simRange.Set(range);
+}
+
+double ADXL345_SPI::GetX() { return GetAcceleration(kAxis_X); }
+
+double ADXL345_SPI::GetY() { return GetAcceleration(kAxis_Y); }
+
+double ADXL345_SPI::GetZ() { return GetAcceleration(kAxis_Z); }
+
+double ADXL345_SPI::GetAcceleration(ADXL345_SPI::Axes axis) {
+ if (axis == kAxis_X && m_simX) return m_simX.Get();
+ if (axis == kAxis_Y && m_simY) return m_simY.Get();
+ if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
+ uint8_t buffer[3];
+ uint8_t command[3] = {0, 0, 0};
+ command[0] = (kAddress_Read | kAddress_MultiByte | kDataRegister) +
+ static_cast<uint8_t>(axis);
+ m_spi.Transaction(command, buffer, 3);
+
+ // Sensor is little endian... swap bytes
+ int16_t rawAccel = buffer[2] << 8 | buffer[1];
+ return rawAccel * kGsPerLSB;
+}
+
+ADXL345_SPI::AllAxes ADXL345_SPI::GetAccelerations() {
+ AllAxes data;
+ if (m_simX && m_simY && m_simZ) {
+ data.XAxis = m_simX.Get();
+ data.YAxis = m_simY.Get();
+ data.ZAxis = m_simZ.Get();
+ return data;
+ }
+
+ uint8_t dataBuffer[7] = {0, 0, 0, 0, 0, 0, 0};
+ int16_t rawData[3];
+
+ // Select the data address.
+ dataBuffer[0] = (kAddress_Read | kAddress_MultiByte | kDataRegister);
+ m_spi.Transaction(dataBuffer, dataBuffer, 7);
+
+ for (int i = 0; i < 3; i++) {
+ // Sensor is little endian... swap bytes
+ rawData[i] = dataBuffer[i * 2 + 2] << 8 | dataBuffer[i * 2 + 1];
+ }
+
+ data.XAxis = rawData[0] * kGsPerLSB;
+ data.YAxis = rawData[1] * kGsPerLSB;
+ data.ZAxis = rawData[2] * kGsPerLSB;
+
+ return data;
+}
+
+void ADXL345_SPI::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("3AxisAccelerometer");
+ auto x = builder.GetEntry("X").GetHandle();
+ auto y = builder.GetEntry("Y").GetHandle();
+ auto z = builder.GetEntry("Z").GetHandle();
+ builder.SetUpdateTable([=]() {
+ auto data = GetAccelerations();
+ nt::NetworkTableEntry(x).SetDouble(data.XAxis);
+ nt::NetworkTableEntry(y).SetDouble(data.YAxis);
+ nt::NetworkTableEntry(z).SetDouble(data.ZAxis);
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/ADXL362.cpp b/wpilibc/src/main/native/cpp/ADXL362.cpp
new file mode 100644
index 0000000..6ed8c8c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADXL362.cpp
@@ -0,0 +1,176 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ADXL362.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/DriverStation.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+static constexpr int kRegWrite = 0x0A;
+static constexpr int kRegRead = 0x0B;
+
+static constexpr int kPartIdRegister = 0x02;
+static constexpr int kDataRegister = 0x0E;
+static constexpr int kFilterCtlRegister = 0x2C;
+static constexpr int kPowerCtlRegister = 0x2D;
+
+// static constexpr int kFilterCtl_Range2G = 0x00;
+// static constexpr int kFilterCtl_Range4G = 0x40;
+// static constexpr int kFilterCtl_Range8G = 0x80;
+static constexpr int kFilterCtl_ODR_100Hz = 0x03;
+
+static constexpr int kPowerCtl_UltraLowNoise = 0x20;
+// static constexpr int kPowerCtl_AutoSleep = 0x04;
+static constexpr int kPowerCtl_Measure = 0x02;
+
+ADXL362::ADXL362(Range range) : ADXL362(SPI::Port::kOnboardCS1, range) {}
+
+ADXL362::ADXL362(SPI::Port port, Range range)
+ : m_spi(port), m_simDevice("ADXL362", port) {
+ if (m_simDevice) {
+ m_simRange =
+ m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
+ m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
+ m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+ m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
+ }
+
+ m_spi.SetClockRate(3000000);
+ m_spi.SetMSBFirst();
+ m_spi.SetSampleDataOnTrailingEdge();
+ m_spi.SetClockActiveLow();
+ m_spi.SetChipSelectActiveLow();
+
+ uint8_t commands[3];
+ if (!m_simDevice) {
+ // Validate the part ID
+ commands[0] = kRegRead;
+ commands[1] = kPartIdRegister;
+ commands[2] = 0;
+ m_spi.Transaction(commands, commands, 3);
+ if (commands[2] != 0xF2) {
+ DriverStation::ReportError("could not find ADXL362");
+ m_gsPerLSB = 0.0;
+ return;
+ }
+ }
+
+ SetRange(range);
+
+ // Turn on the measurements
+ commands[0] = kRegWrite;
+ commands[1] = kPowerCtlRegister;
+ commands[2] = kPowerCtl_Measure | kPowerCtl_UltraLowNoise;
+ m_spi.Write(commands, 3);
+
+ HAL_Report(HALUsageReporting::kResourceType_ADXL362, port + 1);
+
+ SendableRegistry::GetInstance().AddLW(this, "ADXL362", port);
+}
+
+void ADXL362::SetRange(Range range) {
+ if (m_gsPerLSB == 0.0) return;
+
+ uint8_t commands[3];
+
+ switch (range) {
+ case kRange_2G:
+ m_gsPerLSB = 0.001;
+ break;
+ case kRange_4G:
+ m_gsPerLSB = 0.002;
+ break;
+ case kRange_8G:
+ case kRange_16G: // 16G not supported; treat as 8G
+ m_gsPerLSB = 0.004;
+ break;
+ }
+
+ // Specify the data format to read
+ commands[0] = kRegWrite;
+ commands[1] = kFilterCtlRegister;
+ commands[2] =
+ kFilterCtl_ODR_100Hz | static_cast<uint8_t>((range & 0x03) << 6);
+ m_spi.Write(commands, 3);
+
+ if (m_simRange) m_simRange.Set(range);
+}
+
+double ADXL362::GetX() { return GetAcceleration(kAxis_X); }
+
+double ADXL362::GetY() { return GetAcceleration(kAxis_Y); }
+
+double ADXL362::GetZ() { return GetAcceleration(kAxis_Z); }
+
+double ADXL362::GetAcceleration(ADXL362::Axes axis) {
+ if (m_gsPerLSB == 0.0) return 0.0;
+
+ if (axis == kAxis_X && m_simX) return m_simX.Get();
+ if (axis == kAxis_Y && m_simY) return m_simY.Get();
+ if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
+
+ uint8_t buffer[4];
+ uint8_t command[4] = {0, 0, 0, 0};
+ command[0] = kRegRead;
+ command[1] = kDataRegister + static_cast<uint8_t>(axis);
+ m_spi.Transaction(command, buffer, 4);
+
+ // Sensor is little endian... swap bytes
+ int16_t rawAccel = buffer[3] << 8 | buffer[2];
+ return rawAccel * m_gsPerLSB;
+}
+
+ADXL362::AllAxes ADXL362::GetAccelerations() {
+ AllAxes data;
+ if (m_gsPerLSB == 0.0) {
+ data.XAxis = data.YAxis = data.ZAxis = 0.0;
+ return data;
+ }
+ if (m_simX && m_simY && m_simZ) {
+ data.XAxis = m_simX.Get();
+ data.YAxis = m_simY.Get();
+ data.ZAxis = m_simZ.Get();
+ return data;
+ }
+
+ uint8_t dataBuffer[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+ int16_t rawData[3];
+
+ // Select the data address.
+ dataBuffer[0] = kRegRead;
+ dataBuffer[1] = kDataRegister;
+ m_spi.Transaction(dataBuffer, dataBuffer, 8);
+
+ for (int i = 0; i < 3; i++) {
+ // Sensor is little endian... swap bytes
+ rawData[i] = dataBuffer[i * 2 + 3] << 8 | dataBuffer[i * 2 + 2];
+ }
+
+ data.XAxis = rawData[0] * m_gsPerLSB;
+ data.YAxis = rawData[1] * m_gsPerLSB;
+ data.ZAxis = rawData[2] * m_gsPerLSB;
+
+ return data;
+}
+
+void ADXL362::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("3AxisAccelerometer");
+ auto x = builder.GetEntry("X").GetHandle();
+ auto y = builder.GetEntry("Y").GetHandle();
+ auto z = builder.GetEntry("Z").GetHandle();
+ builder.SetUpdateTable([=]() {
+ auto data = GetAccelerations();
+ nt::NetworkTableEntry(x).SetDouble(data.XAxis);
+ nt::NetworkTableEntry(y).SetDouble(data.YAxis);
+ nt::NetworkTableEntry(z).SetDouble(data.ZAxis);
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
new file mode 100644
index 0000000..2cf2f73
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* 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/ADXRS450_Gyro.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/DriverStation.h"
+#include "frc/Timer.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+static constexpr auto kSamplePeriod = 0.0005_s;
+static constexpr double kCalibrationSampleTime = 5.0;
+static constexpr double kDegreePerSecondPerLSB = 0.0125;
+
+// static constexpr int kRateRegister = 0x00;
+// static constexpr int kTemRegister = 0x02;
+// static constexpr int kLoCSTRegister = 0x04;
+// static constexpr int kHiCSTRegister = 0x06;
+// static constexpr int kQuadRegister = 0x08;
+// static constexpr int kFaultRegister = 0x0A;
+static constexpr int kPIDRegister = 0x0C;
+// static constexpr int kSNHighRegister = 0x0E;
+// static constexpr int kSNLowRegister = 0x10;
+
+ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
+
+ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port)
+ : m_spi(port), m_simDevice("ADXRS450_Gyro", port) {
+ if (m_simDevice) {
+ m_simAngle = m_simDevice.CreateDouble("Angle", false, 0.0);
+ m_simRate = m_simDevice.CreateDouble("Rate", false, 0.0);
+ }
+
+ m_spi.SetClockRate(3000000);
+ m_spi.SetMSBFirst();
+ m_spi.SetSampleDataOnLeadingEdge();
+ m_spi.SetClockActiveHigh();
+ m_spi.SetChipSelectActiveLow();
+
+ if (!m_simDevice) {
+ // Validate the part ID
+ if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
+ DriverStation::ReportError("could not find ADXRS450 gyro");
+ return;
+ }
+
+ m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c00000eu,
+ 0x04000000u, 10u, 16u, true, true);
+
+ Calibrate();
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port + 1);
+
+ SendableRegistry::GetInstance().AddLW(this, "ADXRS450_Gyro", port);
+}
+
+static bool CalcParity(int v) {
+ bool parity = false;
+ while (v != 0) {
+ parity = !parity;
+ v = v & (v - 1);
+ }
+ return parity;
+}
+
+static inline int BytesToIntBE(uint8_t* buf) {
+ int result = static_cast<int>(buf[0]) << 24;
+ result |= static_cast<int>(buf[1]) << 16;
+ result |= static_cast<int>(buf[2]) << 8;
+ result |= static_cast<int>(buf[3]);
+ return result;
+}
+
+uint16_t ADXRS450_Gyro::ReadRegister(int reg) {
+ int cmd = 0x80000000 | static_cast<int>(reg) << 17;
+ if (!CalcParity(cmd)) cmd |= 1u;
+
+ // big endian
+ uint8_t buf[4] = {static_cast<uint8_t>((cmd >> 24) & 0xff),
+ static_cast<uint8_t>((cmd >> 16) & 0xff),
+ static_cast<uint8_t>((cmd >> 8) & 0xff),
+ static_cast<uint8_t>(cmd & 0xff)};
+
+ m_spi.Write(buf, 4);
+ m_spi.Read(false, buf, 4);
+ if ((buf[0] & 0xe0) == 0) return 0; // error, return 0
+ return static_cast<uint16_t>((BytesToIntBE(buf) >> 5) & 0xffff);
+}
+
+double ADXRS450_Gyro::GetAngle() const {
+ if (m_simAngle) return m_simAngle.Get();
+ return m_spi.GetAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
+}
+
+double ADXRS450_Gyro::GetRate() const {
+ if (m_simRate) return m_simRate.Get();
+ return static_cast<double>(m_spi.GetAccumulatorLastValue()) *
+ kDegreePerSecondPerLSB;
+}
+
+void ADXRS450_Gyro::Reset() {
+ if (m_simAngle) m_simAngle.Set(0.0);
+ if (m_simRate) m_simRate.Set(0.0);
+ m_spi.ResetAccumulator();
+}
+
+void ADXRS450_Gyro::Calibrate() {
+ Wait(0.1);
+
+ m_spi.SetAccumulatorIntegratedCenter(0);
+ m_spi.ResetAccumulator();
+
+ Wait(kCalibrationSampleTime);
+
+ m_spi.SetAccumulatorIntegratedCenter(m_spi.GetAccumulatorIntegratedAverage());
+ m_spi.ResetAccumulator();
+}
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
new file mode 100644
index 0000000..be0c7d2
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AnalogAccelerometer.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+AnalogAccelerometer::AnalogAccelerometer(int channel)
+ : AnalogAccelerometer(std::make_shared<AnalogInput>(channel)) {
+ SendableRegistry::GetInstance().AddChild(this, m_analogInput.get());
+}
+
+AnalogAccelerometer::AnalogAccelerometer(AnalogInput* channel)
+ : m_analogInput(channel, NullDeleter<AnalogInput>()) {
+ if (channel == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitAccelerometer();
+ }
+}
+
+AnalogAccelerometer::AnalogAccelerometer(std::shared_ptr<AnalogInput> channel)
+ : m_analogInput(channel) {
+ if (channel == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitAccelerometer();
+ }
+}
+
+double AnalogAccelerometer::GetAcceleration() const {
+ return (m_analogInput->GetAverageVoltage() - m_zeroGVoltage) / m_voltsPerG;
+}
+
+void AnalogAccelerometer::SetSensitivity(double sensitivity) {
+ m_voltsPerG = sensitivity;
+}
+
+void AnalogAccelerometer::SetZero(double zero) { m_zeroGVoltage = zero; }
+
+double AnalogAccelerometer::PIDGet() { return GetAcceleration(); }
+
+void AnalogAccelerometer::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Accelerometer");
+ builder.AddDoubleProperty("Value", [=]() { return GetAcceleration(); },
+ nullptr);
+}
+
+void AnalogAccelerometer::InitAccelerometer() {
+ HAL_Report(HALUsageReporting::kResourceType_Accelerometer,
+ 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
new file mode 100644
index 0000000..c891506
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
@@ -0,0 +1,162 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AnalogGyro.h"
+
+#include <climits>
+#include <utility>
+
+#include <hal/AnalogGyro.h>
+#include <hal/Errors.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/AnalogInput.h"
+#include "frc/Timer.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+AnalogGyro::AnalogGyro(int channel)
+ : AnalogGyro(std::make_shared<AnalogInput>(channel)) {
+ SendableRegistry::GetInstance().AddChild(this, m_analog.get());
+}
+
+AnalogGyro::AnalogGyro(AnalogInput* channel)
+ : AnalogGyro(
+ std::shared_ptr<AnalogInput>(channel, NullDeleter<AnalogInput>())) {}
+
+AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
+ : m_analog(channel) {
+ if (channel == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitGyro();
+ Calibrate();
+ }
+}
+
+AnalogGyro::AnalogGyro(int channel, int center, double offset)
+ : AnalogGyro(std::make_shared<AnalogInput>(channel), center, offset) {
+ SendableRegistry::GetInstance().AddChild(this, m_analog.get());
+}
+
+AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel, int center,
+ double offset)
+ : m_analog(channel) {
+ if (channel == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitGyro();
+ int32_t status = 0;
+ HAL_SetAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
+ offset, center, &status);
+ if (status != 0) {
+ wpi_setHALError(status);
+ m_gyroHandle = HAL_kInvalidHandle;
+ return;
+ }
+ Reset();
+ }
+}
+
+AnalogGyro::~AnalogGyro() { HAL_FreeAnalogGyro(m_gyroHandle); }
+
+double AnalogGyro::GetAngle() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetAnalogGyroAngle(m_gyroHandle, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+double AnalogGyro::GetRate() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetAnalogGyroRate(m_gyroHandle, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+int AnalogGyro::GetCenter() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetAnalogGyroCenter(m_gyroHandle, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+double AnalogGyro::GetOffset() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetAnalogGyroOffset(m_gyroHandle, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+void AnalogGyro::SetSensitivity(double voltsPerDegreePerSecond) {
+ int32_t status = 0;
+ HAL_SetAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle,
+ voltsPerDegreePerSecond, &status);
+ wpi_setHALError(status);
+}
+
+void AnalogGyro::SetDeadband(double volts) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogGyroDeadband(m_gyroHandle, volts, &status);
+ wpi_setHALError(status);
+}
+
+void AnalogGyro::Reset() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_ResetAnalogGyro(m_gyroHandle, &status);
+ wpi_setHALError(status);
+}
+
+void AnalogGyro::InitGyro() {
+ if (StatusIsFatal()) return;
+ if (m_gyroHandle == HAL_kInvalidHandle) {
+ int32_t status = 0;
+ m_gyroHandle = HAL_InitializeAnalogGyro(m_analog->m_port, &status);
+ if (status == PARAMETER_OUT_OF_RANGE) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ " channel (must be accumulator channel)");
+ m_analog = nullptr;
+ m_gyroHandle = HAL_kInvalidHandle;
+ return;
+ }
+ if (status != 0) {
+ wpi_setHALError(status);
+ m_analog = nullptr;
+ m_gyroHandle = HAL_kInvalidHandle;
+ return;
+ }
+ }
+
+ int32_t status = 0;
+ HAL_SetupAnalogGyro(m_gyroHandle, &status);
+ if (status != 0) {
+ wpi_setHALError(status);
+ m_analog = nullptr;
+ m_gyroHandle = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel() + 1);
+
+ SendableRegistry::GetInstance().AddLW(this, "AnalogGyro",
+ m_analog->GetChannel());
+}
+
+void AnalogGyro::Calibrate() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_CalibrateAnalogGyro(m_gyroHandle, &status);
+ wpi_setHALError(status);
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogInput.cpp b/wpilibc/src/main/native/cpp/AnalogInput.cpp
new file mode 100644
index 0000000..c8197b7
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogInput.cpp
@@ -0,0 +1,235 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AnalogInput.h"
+
+#include <utility>
+
+#include <hal/AnalogAccumulator.h>
+#include <hal/AnalogInput.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Ports.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/Timer.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+AnalogInput::AnalogInput(int channel) {
+ if (!SensorUtil::CheckAnalogInputChannel(channel)) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
+ "Analog Input " + wpi::Twine(channel));
+ return;
+ }
+
+ m_channel = channel;
+
+ HAL_PortHandle port = HAL_GetPort(channel);
+ int32_t status = 0;
+ m_port = HAL_InitializeAnalogInputPort(port, &status);
+ if (status != 0) {
+ 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 + 1);
+
+ SendableRegistry::GetInstance().AddLW(this, "AnalogInput", channel);
+}
+
+AnalogInput::~AnalogInput() { HAL_FreeAnalogInputPort(m_port); }
+
+int AnalogInput::GetValue() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetAnalogValue(m_port, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+int AnalogInput::GetAverageValue() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetAnalogAverageValue(m_port, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+double AnalogInput::GetVoltage() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double voltage = HAL_GetAnalogVoltage(m_port, &status);
+ wpi_setHALError(status);
+ return voltage;
+}
+
+double AnalogInput::GetAverageVoltage() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double voltage = HAL_GetAnalogAverageVoltage(m_port, &status);
+ wpi_setHALError(status);
+ return voltage;
+}
+
+int AnalogInput::GetChannel() const {
+ if (StatusIsFatal()) return 0;
+ return m_channel;
+}
+
+void AnalogInput::SetAverageBits(int bits) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogAverageBits(m_port, bits, &status);
+ wpi_setHALError(status);
+}
+
+int AnalogInput::GetAverageBits() const {
+ int32_t status = 0;
+ int averageBits = HAL_GetAnalogAverageBits(m_port, &status);
+ wpi_setHALError(status);
+ return averageBits;
+}
+
+void AnalogInput::SetOversampleBits(int bits) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogOversampleBits(m_port, bits, &status);
+ wpi_setHALError(status);
+}
+
+int AnalogInput::GetOversampleBits() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int oversampleBits = HAL_GetAnalogOversampleBits(m_port, &status);
+ wpi_setHALError(status);
+ return oversampleBits;
+}
+
+int AnalogInput::GetLSBWeight() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int lsbWeight = HAL_GetAnalogLSBWeight(m_port, &status);
+ wpi_setHALError(status);
+ return lsbWeight;
+}
+
+int AnalogInput::GetOffset() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int offset = HAL_GetAnalogOffset(m_port, &status);
+ wpi_setHALError(status);
+ return offset;
+}
+
+bool AnalogInput::IsAccumulatorChannel() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool isAccum = HAL_IsAccumulatorChannel(m_port, &status);
+ wpi_setHALError(status);
+ return isAccum;
+}
+
+void AnalogInput::InitAccumulator() {
+ if (StatusIsFatal()) return;
+ m_accumulatorOffset = 0;
+ int32_t status = 0;
+ HAL_InitAccumulator(m_port, &status);
+ wpi_setHALError(status);
+}
+
+void AnalogInput::SetAccumulatorInitialValue(int64_t initialValue) {
+ if (StatusIsFatal()) return;
+ m_accumulatorOffset = initialValue;
+}
+
+void AnalogInput::ResetAccumulator() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_ResetAccumulator(m_port, &status);
+ wpi_setHALError(status);
+
+ if (!StatusIsFatal()) {
+ // Wait until the next sample, so the next call to GetAccumulator*()
+ // won't have old values.
+ const double sampleTime = 1.0 / GetSampleRate();
+ const double overSamples = 1 << GetOversampleBits();
+ const double averageSamples = 1 << GetAverageBits();
+ Wait(sampleTime * overSamples * averageSamples);
+ }
+}
+
+void AnalogInput::SetAccumulatorCenter(int center) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAccumulatorCenter(m_port, center, &status);
+ wpi_setHALError(status);
+}
+
+void AnalogInput::SetAccumulatorDeadband(int deadband) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAccumulatorDeadband(m_port, deadband, &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_setHALError(status);
+ return value + m_accumulatorOffset;
+}
+
+int64_t AnalogInput::GetAccumulatorCount() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int64_t count = HAL_GetAccumulatorCount(m_port, &status);
+ wpi_setHALError(status);
+ return count;
+}
+
+void AnalogInput::GetAccumulatorOutput(int64_t& value, int64_t& count) const {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_GetAccumulatorOutput(m_port, &value, &count, &status);
+ wpi_setHALError(status);
+ value += m_accumulatorOffset;
+}
+
+void AnalogInput::SetSampleRate(double samplesPerSecond) {
+ int32_t status = 0;
+ HAL_SetAnalogSampleRate(samplesPerSecond, &status);
+ wpi_setGlobalHALError(status);
+}
+
+double AnalogInput::GetSampleRate() {
+ int32_t status = 0;
+ double sampleRate = HAL_GetAnalogSampleRate(&status);
+ wpi_setGlobalHALError(status);
+ return sampleRate;
+}
+
+double AnalogInput::PIDGet() {
+ if (StatusIsFatal()) return 0.0;
+ return GetAverageVoltage();
+}
+
+void AnalogInput::SetSimDevice(HAL_SimDeviceHandle device) {
+ HAL_SetAnalogInputSimDevice(m_port, device);
+}
+
+void AnalogInput::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Analog Input");
+ builder.AddDoubleProperty("Value", [=]() { return GetAverageVoltage(); },
+ nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogOutput.cpp b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
new file mode 100644
index 0000000..041b446
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AnalogOutput.h"
+
+#include <limits>
+#include <utility>
+
+#include <hal/AnalogOutput.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Ports.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+AnalogOutput::AnalogOutput(int channel) {
+ if (!SensorUtil::CheckAnalogOutputChannel(channel)) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
+ "analog output " + wpi::Twine(channel));
+ m_channel = std::numeric_limits<int>::max();
+ m_port = HAL_kInvalidHandle;
+ return;
+ }
+
+ m_channel = channel;
+
+ HAL_PortHandle port = HAL_GetPort(m_channel);
+ int32_t status = 0;
+ m_port = HAL_InitializeAnalogOutputPort(port, &status);
+ if (status != 0) {
+ 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 + 1);
+ SendableRegistry::GetInstance().AddLW(this, "AnalogOutput", m_channel);
+}
+
+AnalogOutput::~AnalogOutput() { HAL_FreeAnalogOutputPort(m_port); }
+
+void AnalogOutput::SetVoltage(double voltage) {
+ int32_t status = 0;
+ HAL_SetAnalogOutput(m_port, voltage, &status);
+
+ wpi_setHALError(status);
+}
+
+double AnalogOutput::GetVoltage() const {
+ int32_t status = 0;
+ double voltage = HAL_GetAnalogOutput(m_port, &status);
+
+ wpi_setHALError(status);
+
+ return voltage;
+}
+
+int AnalogOutput::GetChannel() { return m_channel; }
+
+void AnalogOutput::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Analog Output");
+ builder.AddDoubleProperty("Value", [=]() { return GetVoltage(); },
+ [=](double value) { SetVoltage(value); });
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
new file mode 100644
index 0000000..b650ec7
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AnalogPotentiometer.h"
+
+#include "frc/RobotController.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+AnalogPotentiometer::AnalogPotentiometer(int channel, double fullRange,
+ double offset)
+ : AnalogPotentiometer(std::make_shared<AnalogInput>(channel), fullRange,
+ offset) {
+ SendableRegistry::GetInstance().AddChild(this, m_analog_input.get());
+}
+
+AnalogPotentiometer::AnalogPotentiometer(AnalogInput* input, double fullRange,
+ double offset)
+ : AnalogPotentiometer(
+ std::shared_ptr<AnalogInput>(input, NullDeleter<AnalogInput>()),
+ fullRange, offset) {}
+
+AnalogPotentiometer::AnalogPotentiometer(std::shared_ptr<AnalogInput> input,
+ double fullRange, double offset)
+ : m_analog_input(input), m_fullRange(fullRange), m_offset(offset) {
+ SendableRegistry::GetInstance().AddLW(this, "AnalogPotentiometer",
+ m_analog_input->GetChannel());
+}
+
+double AnalogPotentiometer::Get() const {
+ return (m_analog_input->GetVoltage() / RobotController::GetVoltage5V()) *
+ m_fullRange +
+ m_offset;
+}
+
+double AnalogPotentiometer::PIDGet() { return Get(); }
+
+void AnalogPotentiometer::InitSendable(SendableBuilder& builder) {
+ m_analog_input->InitSendable(builder);
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
new file mode 100644
index 0000000..ddbb7c8
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
@@ -0,0 +1,155 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AnalogTrigger.h"
+
+#include <utility>
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/AnalogInput.h"
+#include "frc/DutyCycle.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+AnalogTrigger::AnalogTrigger(int channel)
+ : AnalogTrigger(new AnalogInput(channel)) {
+ m_ownsAnalog = true;
+ SendableRegistry::GetInstance().AddChild(this, m_analogInput);
+}
+
+AnalogTrigger::AnalogTrigger(AnalogInput* input) {
+ m_analogInput = input;
+ int32_t status = 0;
+ m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &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(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() {
+ int32_t status = 0;
+ HAL_CleanAnalogTrigger(m_trigger, &status);
+
+ if (m_ownsAnalog) {
+ delete m_analogInput;
+ }
+}
+
+AnalogTrigger::AnalogTrigger(AnalogTrigger&& rhs)
+ : ErrorBase(std::move(rhs)),
+ SendableHelper(std::move(rhs)),
+ 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);
+}
+
+AnalogTrigger& AnalogTrigger::operator=(AnalogTrigger&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+ SendableHelper::operator=(std::move(rhs));
+
+ 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;
+}
+
+void AnalogTrigger::SetLimitsVoltage(double lower, double upper) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogTriggerLimitsVoltage(m_trigger, lower, upper, &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_setHALError(status);
+}
+
+void AnalogTrigger::SetAveraged(bool useAveragedValue) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogTriggerAveraged(m_trigger, useAveragedValue, &status);
+ wpi_setHALError(status);
+}
+
+void AnalogTrigger::SetFiltered(bool useFilteredValue) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogTriggerFiltered(m_trigger, useFilteredValue, &status);
+ wpi_setHALError(status);
+}
+
+int AnalogTrigger::GetIndex() const {
+ if (StatusIsFatal()) return -1;
+ 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_setHALError(status);
+ return result;
+}
+
+bool AnalogTrigger::GetTriggerState() {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool result = HAL_GetAnalogTriggerTriggerState(m_trigger, &status);
+ wpi_setHALError(status);
+ return result;
+}
+
+std::shared_ptr<AnalogTriggerOutput> AnalogTrigger::CreateOutput(
+ AnalogTriggerType type) const {
+ if (StatusIsFatal()) return nullptr;
+ return std::shared_ptr<AnalogTriggerOutput>(
+ new AnalogTriggerOutput(*this, type), NullDeleter<AnalogTriggerOutput>());
+}
+
+void AnalogTrigger::InitSendable(SendableBuilder& builder) {
+ if (m_ownsAnalog) m_analogInput->InitSendable(builder);
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
new file mode 100644
index 0000000..8fba479
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AnalogTriggerOutput.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/AnalogTrigger.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+bool AnalogTriggerOutput::Get() const {
+ int32_t status = 0;
+ bool result = HAL_GetAnalogTriggerOutput(
+ m_trigger->m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
+ &status);
+ wpi_setHALError(status);
+ return result;
+}
+
+HAL_Handle AnalogTriggerOutput::GetPortHandleForRouting() const {
+ return m_trigger->m_trigger;
+}
+
+AnalogTriggerType AnalogTriggerOutput::GetAnalogTriggerTypeForRouting() const {
+ return m_outputType;
+}
+
+bool AnalogTriggerOutput::IsAnalogTrigger() const { return true; }
+
+int AnalogTriggerOutput::GetChannel() const { return m_trigger->GetIndex(); }
+
+void AnalogTriggerOutput::InitSendable(SendableBuilder&) {}
+
+AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger& trigger,
+ AnalogTriggerType outputType)
+ : m_trigger(&trigger), m_outputType(outputType) {
+ HAL_Report(HALUsageReporting::kResourceType_AnalogTriggerOutput,
+ 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
new file mode 100644
index 0000000..d7cdef6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/BuiltInAccelerometer.h"
+
+#include <hal/Accelerometer.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+BuiltInAccelerometer::BuiltInAccelerometer(Range range) {
+ SetRange(range);
+
+ HAL_Report(HALUsageReporting::kResourceType_Accelerometer, 0, 0,
+ "Built-in accelerometer");
+ SendableRegistry::GetInstance().AddLW(this, "BuiltInAccel");
+}
+
+void BuiltInAccelerometer::SetRange(Range range) {
+ if (range == kRange_16G) {
+ wpi_setWPIErrorWithContext(
+ ParameterOutOfRange, "16G range not supported (use k2G, k4G, or k8G)");
+ }
+
+ HAL_SetAccelerometerActive(false);
+ HAL_SetAccelerometerRange((HAL_AccelerometerRange)range);
+ HAL_SetAccelerometerActive(true);
+}
+
+double BuiltInAccelerometer::GetX() { return HAL_GetAccelerometerX(); }
+
+double BuiltInAccelerometer::GetY() { return HAL_GetAccelerometerY(); }
+
+double BuiltInAccelerometer::GetZ() { return HAL_GetAccelerometerZ(); }
+
+void BuiltInAccelerometer::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("3AxisAccelerometer");
+ builder.AddDoubleProperty("X", [=]() { return GetX(); }, nullptr);
+ builder.AddDoubleProperty("Y", [=]() { return GetY(); }, nullptr);
+ builder.AddDoubleProperty("Z", [=]() { return GetZ(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/CAN.cpp b/wpilibc/src/main/native/cpp/CAN.cpp
new file mode 100644
index 0000000..797b9ff
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/CAN.cpp
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/CAN.h"
+
+#include <utility>
+
+#include <hal/CAN.h>
+#include <hal/CANAPI.h>
+#include <hal/Errors.h>
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+CAN::CAN(int deviceId) {
+ int32_t status = 0;
+ m_handle =
+ HAL_InitializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType, &status);
+ if (status != 0) {
+ wpi_setHALError(status);
+ m_handle = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId + 1);
+}
+
+CAN::CAN(int deviceId, int deviceManufacturer, int deviceType) {
+ int32_t status = 0;
+ m_handle = HAL_InitializeCAN(
+ static_cast<HAL_CANManufacturer>(deviceManufacturer), deviceId,
+ static_cast<HAL_CANDeviceType>(deviceType), &status);
+ if (status != 0) {
+ wpi_setHALError(status);
+ m_handle = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId + 1);
+}
+
+CAN::~CAN() {
+ if (StatusIsFatal()) return;
+ if (m_handle != HAL_kInvalidHandle) {
+ HAL_CleanCAN(m_handle);
+ m_handle = HAL_kInvalidHandle;
+ }
+}
+
+void CAN::WritePacket(const uint8_t* data, int length, int apiId) {
+ int32_t status = 0;
+ HAL_WriteCANPacket(m_handle, data, length, apiId, &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_setHALError(status);
+}
+
+void CAN::WriteRTRFrame(int length, int apiId) {
+ int32_t status = 0;
+ HAL_WriteCANRTRFrame(m_handle, length, apiId, &status);
+ wpi_setHALError(status);
+}
+
+void CAN::StopPacketRepeating(int apiId) {
+ int32_t status = 0;
+ HAL_StopCANPacketRepeating(m_handle, apiId, &status);
+ wpi_setHALError(status);
+}
+
+bool CAN::ReadPacketNew(int apiId, CANData* data) {
+ int32_t status = 0;
+ HAL_ReadCANPacketNew(m_handle, apiId, data->data, &data->length,
+ &data->timestamp, &status);
+ if (status == HAL_ERR_CANSessionMux_MessageNotFound) {
+ return false;
+ }
+ if (status != 0) {
+ wpi_setHALError(status);
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool CAN::ReadPacketLatest(int apiId, CANData* data) {
+ int32_t status = 0;
+ HAL_ReadCANPacketLatest(m_handle, apiId, data->data, &data->length,
+ &data->timestamp, &status);
+ if (status == HAL_ERR_CANSessionMux_MessageNotFound) {
+ return false;
+ }
+ if (status != 0) {
+ wpi_setHALError(status);
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool CAN::ReadPacketTimeout(int apiId, int timeoutMs, CANData* data) {
+ int32_t status = 0;
+ HAL_ReadCANPacketTimeout(m_handle, apiId, data->data, &data->length,
+ &data->timestamp, timeoutMs, &status);
+ if (status == HAL_CAN_TIMEOUT ||
+ status == HAL_ERR_CANSessionMux_MessageNotFound) {
+ return false;
+ }
+ if (status != 0) {
+ 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
new file mode 100644
index 0000000..29789be
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -0,0 +1,218 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Compressor.h"
+
+#include <hal/Compressor.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/Ports.h>
+#include <hal/Solenoid.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Compressor::Compressor(int pcmID) : m_module(pcmID) {
+ int32_t status = 0;
+ m_compressorHandle = HAL_InitializeCompressor(m_module, &status);
+ if (status != 0) {
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPCMModules(), pcmID);
+ return;
+ }
+ SetClosedLoopControl(true);
+
+ HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID + 1);
+ SendableRegistry::GetInstance().AddLW(this, "Compressor", pcmID);
+}
+
+void Compressor::Start() {
+ if (StatusIsFatal()) return;
+ SetClosedLoopControl(true);
+}
+
+void Compressor::Stop() {
+ if (StatusIsFatal()) return;
+ SetClosedLoopControl(false);
+}
+
+bool Compressor::Enabled() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value;
+
+ value = HAL_GetCompressor(m_compressorHandle, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+bool Compressor::GetPressureSwitchValue() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value;
+
+ value = HAL_GetCompressorPressureSwitch(m_compressorHandle, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+double Compressor::GetCompressorCurrent() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ double value;
+
+ value = HAL_GetCompressorCurrent(m_compressorHandle, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+void Compressor::SetClosedLoopControl(bool on) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+
+ HAL_SetCompressorClosedLoopControl(m_compressorHandle, on, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+}
+
+bool Compressor::GetClosedLoopControl() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value;
+
+ value = HAL_GetCompressorClosedLoopControl(m_compressorHandle, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+bool Compressor::GetCompressorCurrentTooHighFault() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value;
+
+ value = HAL_GetCompressorCurrentTooHighFault(m_compressorHandle, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+bool Compressor::GetCompressorCurrentTooHighStickyFault() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value;
+
+ value =
+ HAL_GetCompressorCurrentTooHighStickyFault(m_compressorHandle, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+bool Compressor::GetCompressorShortedStickyFault() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value;
+
+ value = HAL_GetCompressorShortedStickyFault(m_compressorHandle, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+bool Compressor::GetCompressorShortedFault() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value;
+
+ value = HAL_GetCompressorShortedFault(m_compressorHandle, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+bool Compressor::GetCompressorNotConnectedStickyFault() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value;
+
+ value = HAL_GetCompressorNotConnectedStickyFault(m_compressorHandle, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+bool Compressor::GetCompressorNotConnectedFault() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value;
+
+ value = HAL_GetCompressorNotConnectedFault(m_compressorHandle, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+void Compressor::ClearAllPCMStickyFaults() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+
+ HAL_ClearAllPCMStickyFaults(m_module, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+}
+
+void Compressor::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Compressor");
+ builder.AddBooleanProperty("Enabled", [=]() { return Enabled(); },
+ [=](bool value) {
+ if (value)
+ Start();
+ else
+ Stop();
+ });
+ builder.AddBooleanProperty(
+ "Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/Counter.cpp b/wpilibc/src/main/native/cpp/Counter.cpp
new file mode 100644
index 0000000..d2158ca
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Counter.cpp
@@ -0,0 +1,336 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Counter.h"
+
+#include <utility>
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/AnalogTrigger.h"
+#include "frc/DigitalInput.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Counter::Counter(Mode mode) {
+ int32_t status = 0;
+ m_counter = HAL_InitializeCounter((HAL_Counter_Mode)mode, &m_index, &status);
+ wpi_setHALError(status);
+
+ SetMaxPeriod(0.5);
+
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1);
+ SendableRegistry::GetInstance().AddLW(this, "Counter", m_index);
+}
+
+Counter::Counter(int channel) : Counter(kTwoPulse) {
+ SetUpSource(channel);
+ ClearDownSource();
+}
+
+Counter::Counter(DigitalSource* source) : Counter(kTwoPulse) {
+ SetUpSource(source);
+ ClearDownSource();
+}
+
+Counter::Counter(std::shared_ptr<DigitalSource> source) : Counter(kTwoPulse) {
+ SetUpSource(source);
+ ClearDownSource();
+}
+
+Counter::Counter(const AnalogTrigger& trigger) : Counter(kTwoPulse) {
+ SetUpSource(trigger.CreateOutput(AnalogTriggerType::kState));
+ ClearDownSource();
+}
+
+Counter::Counter(EncodingType encodingType, DigitalSource* upSource,
+ DigitalSource* downSource, bool inverted)
+ : Counter(encodingType,
+ std::shared_ptr<DigitalSource>(upSource,
+ NullDeleter<DigitalSource>()),
+ std::shared_ptr<DigitalSource>(downSource,
+ NullDeleter<DigitalSource>()),
+ inverted) {}
+
+Counter::Counter(EncodingType encodingType,
+ std::shared_ptr<DigitalSource> upSource,
+ std::shared_ptr<DigitalSource> downSource, bool inverted)
+ : Counter(kExternalDirection) {
+ if (encodingType != k1X && encodingType != k2X) {
+ wpi_setWPIErrorWithContext(
+ ParameterOutOfRange,
+ "Counter only supports 1X and 2X quadrature decoding.");
+ return;
+ }
+ SetUpSource(upSource);
+ SetDownSource(downSource);
+ int32_t status = 0;
+
+ if (encodingType == k1X) {
+ SetUpSourceEdge(true, false);
+ HAL_SetCounterAverageSize(m_counter, 1, &status);
+ } else {
+ SetUpSourceEdge(true, true);
+ HAL_SetCounterAverageSize(m_counter, 2, &status);
+ }
+
+ wpi_setHALError(status);
+ SetDownSourceEdge(inverted, true);
+}
+
+Counter::~Counter() {
+ SetUpdateWhenEmpty(true);
+
+ int32_t status = 0;
+ HAL_FreeCounter(m_counter, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::SetUpSource(int channel) {
+ if (StatusIsFatal()) return;
+ SetUpSource(std::make_shared<DigitalInput>(channel));
+ SendableRegistry::GetInstance().AddChild(this, m_upSource.get());
+}
+
+void Counter::SetUpSource(AnalogTrigger* analogTrigger,
+ AnalogTriggerType triggerType) {
+ SetUpSource(std::shared_ptr<AnalogTrigger>(analogTrigger,
+ NullDeleter<AnalogTrigger>()),
+ triggerType);
+}
+
+void Counter::SetUpSource(std::shared_ptr<AnalogTrigger> analogTrigger,
+ AnalogTriggerType triggerType) {
+ if (StatusIsFatal()) return;
+ SetUpSource(analogTrigger->CreateOutput(triggerType));
+}
+
+void Counter::SetUpSource(DigitalSource* source) {
+ SetUpSource(
+ std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
+}
+
+void Counter::SetUpSource(std::shared_ptr<DigitalSource> source) {
+ if (StatusIsFatal()) return;
+ m_upSource = source;
+ if (m_upSource->StatusIsFatal()) {
+ CloneError(*m_upSource);
+ } else {
+ int32_t status = 0;
+ HAL_SetCounterUpSource(
+ m_counter, source->GetPortHandleForRouting(),
+ (HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(),
+ &status);
+ wpi_setHALError(status);
+ }
+}
+
+void Counter::SetUpSource(DigitalSource& source) {
+ SetUpSource(
+ std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
+}
+
+void Counter::SetUpSourceEdge(bool risingEdge, bool fallingEdge) {
+ if (StatusIsFatal()) return;
+ if (m_upSource == nullptr) {
+ wpi_setWPIErrorWithContext(
+ NullParameter,
+ "Must set non-nullptr UpSource before setting UpSourceEdge");
+ }
+ int32_t status = 0;
+ HAL_SetCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::ClearUpSource() {
+ if (StatusIsFatal()) return;
+ m_upSource.reset();
+ int32_t status = 0;
+ HAL_ClearCounterUpSource(m_counter, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::SetDownSource(int channel) {
+ if (StatusIsFatal()) return;
+ SetDownSource(std::make_shared<DigitalInput>(channel));
+ SendableRegistry::GetInstance().AddChild(this, m_downSource.get());
+}
+
+void Counter::SetDownSource(AnalogTrigger* analogTrigger,
+ AnalogTriggerType triggerType) {
+ SetDownSource(std::shared_ptr<AnalogTrigger>(analogTrigger,
+ NullDeleter<AnalogTrigger>()),
+ triggerType);
+}
+
+void Counter::SetDownSource(std::shared_ptr<AnalogTrigger> analogTrigger,
+ AnalogTriggerType triggerType) {
+ if (StatusIsFatal()) return;
+ SetDownSource(analogTrigger->CreateOutput(triggerType));
+}
+
+void Counter::SetDownSource(DigitalSource* source) {
+ SetDownSource(
+ std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
+}
+
+void Counter::SetDownSource(DigitalSource& source) {
+ SetDownSource(
+ std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
+}
+
+void Counter::SetDownSource(std::shared_ptr<DigitalSource> source) {
+ if (StatusIsFatal()) return;
+ m_downSource = source;
+ if (m_downSource->StatusIsFatal()) {
+ CloneError(*m_downSource);
+ } else {
+ int32_t status = 0;
+ HAL_SetCounterDownSource(
+ m_counter, source->GetPortHandleForRouting(),
+ (HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(),
+ &status);
+ wpi_setHALError(status);
+ }
+}
+
+void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge) {
+ if (StatusIsFatal()) return;
+ if (m_downSource == nullptr) {
+ wpi_setWPIErrorWithContext(
+ NullParameter,
+ "Must set non-nullptr DownSource before setting DownSourceEdge");
+ }
+ int32_t status = 0;
+ HAL_SetCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::ClearDownSource() {
+ if (StatusIsFatal()) return;
+ m_downSource.reset();
+ int32_t status = 0;
+ HAL_ClearCounterDownSource(m_counter, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::SetUpDownCounterMode() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterUpDownMode(m_counter, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::SetExternalDirectionMode() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterExternalDirectionMode(m_counter, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::SetPulseLengthMode(double threshold) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterPulseLengthMode(m_counter, threshold, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::SetReverseDirection(bool reverseDirection) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterReverseDirection(m_counter, reverseDirection, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::SetSamplesToAverage(int samplesToAverage) {
+ if (samplesToAverage < 1 || samplesToAverage > 127) {
+ wpi_setWPIErrorWithContext(
+ ParameterOutOfRange,
+ "Average counter values must be between 1 and 127");
+ }
+ int32_t status = 0;
+ HAL_SetCounterSamplesToAverage(m_counter, samplesToAverage, &status);
+ wpi_setHALError(status);
+}
+
+int Counter::GetSamplesToAverage() const {
+ int32_t status = 0;
+ int samples = HAL_GetCounterSamplesToAverage(m_counter, &status);
+ wpi_setHALError(status);
+ return samples;
+}
+
+int Counter::GetFPGAIndex() const { return m_index; }
+
+int Counter::Get() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetCounter(m_counter, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+void Counter::Reset() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_ResetCounter(m_counter, &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_setHALError(status);
+ return value;
+}
+
+void Counter::SetMaxPeriod(double maxPeriod) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterMaxPeriod(m_counter, maxPeriod, &status);
+ wpi_setHALError(status);
+}
+
+void Counter::SetUpdateWhenEmpty(bool enabled) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterUpdateWhenEmpty(m_counter, enabled, &status);
+ wpi_setHALError(status);
+}
+
+bool Counter::GetStopped() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = HAL_GetCounterStopped(m_counter, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+bool Counter::GetDirection() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = HAL_GetCounterDirection(m_counter, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+void Counter::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Counter");
+ builder.AddDoubleProperty("Value", [=]() { return Get(); }, nullptr);
+}
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
new file mode 100644
index 0000000..e7d2c65
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DMC60.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DMC60.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+DMC60::DMC60(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ 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
new file mode 100644
index 0000000..3cec3f1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
@@ -0,0 +1,161 @@
+/*----------------------------------------------------------------------------*/
+/* 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/DigitalGlitchFilter.h"
+
+#include <algorithm>
+#include <array>
+#include <utility>
+
+#include <hal/Constants.h>
+#include <hal/DIO.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/Counter.h"
+#include "frc/Encoder.h"
+#include "frc/SensorUtil.h"
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+std::array<bool, 3> DigitalGlitchFilter::m_filterAllocated = {
+ {false, false, false}};
+wpi::mutex DigitalGlitchFilter::m_mutex;
+
+DigitalGlitchFilter::DigitalGlitchFilter() {
+ std::scoped_lock lock(m_mutex);
+ auto index =
+ std::find(m_filterAllocated.begin(), m_filterAllocated.end(), false);
+ wpi_assert(index != m_filterAllocated.end());
+
+ m_channelIndex = std::distance(m_filterAllocated.begin(), index);
+ *index = true;
+
+ HAL_Report(HALUsageReporting::kResourceType_DigitalGlitchFilter,
+ m_channelIndex + 1);
+ SendableRegistry::GetInstance().AddLW(this, "DigitalGlitchFilter",
+ m_channelIndex);
+}
+
+DigitalGlitchFilter::~DigitalGlitchFilter() {
+ if (m_channelIndex >= 0) {
+ std::scoped_lock lock(m_mutex);
+ m_filterAllocated[m_channelIndex] = false;
+ }
+}
+
+DigitalGlitchFilter::DigitalGlitchFilter(DigitalGlitchFilter&& rhs)
+ : ErrorBase(std::move(rhs)), SendableHelper(std::move(rhs)) {
+ std::swap(m_channelIndex, rhs.m_channelIndex);
+}
+
+DigitalGlitchFilter& DigitalGlitchFilter::operator=(DigitalGlitchFilter&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+ SendableHelper::operator=(std::move(rhs));
+
+ std::swap(m_channelIndex, rhs.m_channelIndex);
+
+ return *this;
+}
+
+void DigitalGlitchFilter::Add(DigitalSource* input) {
+ DoAdd(input, m_channelIndex + 1);
+}
+
+void DigitalGlitchFilter::DoAdd(DigitalSource* input, int requestedIndex) {
+ // Some sources from Counters and Encoders are null. By pushing the check
+ // here, we catch the issue more generally.
+ if (input) {
+ // We don't support GlitchFilters on AnalogTriggers.
+ if (input->IsAnalogTrigger()) {
+ wpi_setErrorWithContext(
+ -1, "Analog Triggers not supported for DigitalGlitchFilters");
+ return;
+ }
+ int32_t status = 0;
+ HAL_SetFilterSelect(input->GetPortHandleForRouting(), requestedIndex,
+ &status);
+ wpi_setHALError(status);
+
+ // Validate that we set it correctly.
+ int actualIndex =
+ HAL_GetFilterSelect(input->GetPortHandleForRouting(), &status);
+ wpi_assertEqual(actualIndex, requestedIndex);
+ }
+}
+
+void DigitalGlitchFilter::Add(Encoder* input) {
+ Add(input->m_aSource.get());
+ if (StatusIsFatal()) {
+ return;
+ }
+ Add(input->m_bSource.get());
+}
+
+void DigitalGlitchFilter::Add(Counter* input) {
+ Add(input->m_upSource.get());
+ if (StatusIsFatal()) {
+ return;
+ }
+ Add(input->m_downSource.get());
+}
+
+void DigitalGlitchFilter::Remove(DigitalSource* input) { DoAdd(input, 0); }
+
+void DigitalGlitchFilter::Remove(Encoder* input) {
+ Remove(input->m_aSource.get());
+ if (StatusIsFatal()) {
+ return;
+ }
+ Remove(input->m_bSource.get());
+}
+
+void DigitalGlitchFilter::Remove(Counter* input) {
+ Remove(input->m_upSource.get());
+ if (StatusIsFatal()) {
+ return;
+ }
+ Remove(input->m_downSource.get());
+}
+
+void DigitalGlitchFilter::SetPeriodCycles(int fpgaCycles) {
+ int32_t status = 0;
+ HAL_SetFilterPeriod(m_channelIndex, fpgaCycles, &status);
+ wpi_setHALError(status);
+}
+
+void DigitalGlitchFilter::SetPeriodNanoSeconds(uint64_t nanoseconds) {
+ int32_t status = 0;
+ int fpgaCycles =
+ nanoseconds * HAL_GetSystemClockTicksPerMicrosecond() / 4 / 1000;
+ HAL_SetFilterPeriod(m_channelIndex, fpgaCycles, &status);
+
+ wpi_setHALError(status);
+}
+
+int DigitalGlitchFilter::GetPeriodCycles() {
+ int32_t status = 0;
+ int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
+
+ wpi_setHALError(status);
+
+ return fpgaCycles;
+}
+
+uint64_t DigitalGlitchFilter::GetPeriodNanoSeconds() {
+ int32_t status = 0;
+ int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
+
+ wpi_setHALError(status);
+
+ return static_cast<uint64_t>(fpgaCycles) * 1000L /
+ static_cast<uint64_t>(HAL_GetSystemClockTicksPerMicrosecond() / 4);
+}
+
+void DigitalGlitchFilter::InitSendable(SendableBuilder&) {}
diff --git a/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp
new file mode 100644
index 0000000..7210750
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DigitalInput.h"
+
+#include <limits>
+#include <utility>
+
+#include <hal/DIO.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Ports.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+DigitalInput::DigitalInput(int channel) {
+ if (!SensorUtil::CheckDigitalChannel(channel)) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
+ "Digital Channel " + wpi::Twine(channel));
+ m_channel = std::numeric_limits<int>::max();
+ return;
+ }
+ m_channel = channel;
+
+ int32_t status = 0;
+ m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), true, &status);
+ if (status != 0) {
+ 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 + 1);
+ SendableRegistry::GetInstance().AddLW(this, "DigitalInput", channel);
+}
+
+DigitalInput::~DigitalInput() {
+ if (StatusIsFatal()) return;
+ HAL_FreeDIOPort(m_handle);
+}
+
+bool DigitalInput::Get() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = HAL_GetDIO(m_handle, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+HAL_Handle DigitalInput::GetPortHandleForRouting() const { return m_handle; }
+
+AnalogTriggerType DigitalInput::GetAnalogTriggerTypeForRouting() const {
+ return (AnalogTriggerType)0;
+}
+
+bool DigitalInput::IsAnalogTrigger() const { return false; }
+
+void DigitalInput::SetSimDevice(HAL_SimDeviceHandle device) {
+ HAL_SetDIOSimDevice(m_handle, device);
+}
+
+int DigitalInput::GetChannel() const { return m_channel; }
+
+void DigitalInput::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Digital Input");
+ builder.AddBooleanProperty("Value", [=]() { return Get(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/DigitalOutput.cpp b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
new file mode 100644
index 0000000..6e9b09b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
@@ -0,0 +1,159 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DigitalOutput.h"
+
+#include <limits>
+#include <utility>
+
+#include <hal/DIO.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Ports.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+DigitalOutput::DigitalOutput(int channel) {
+ m_pwmGenerator = HAL_kInvalidHandle;
+ if (!SensorUtil::CheckDigitalChannel(channel)) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
+ "Digital Channel " + wpi::Twine(channel));
+ m_channel = std::numeric_limits<int>::max();
+ return;
+ }
+ m_channel = channel;
+
+ int32_t status = 0;
+ m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), false, &status);
+ if (status != 0) {
+ 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 + 1);
+ SendableRegistry::GetInstance().AddLW(this, "DigitalOutput", channel);
+}
+
+DigitalOutput::~DigitalOutput() {
+ if (StatusIsFatal()) return;
+ // Disable the PWM in case it was running.
+ DisablePWM();
+
+ HAL_FreeDIOPort(m_handle);
+}
+
+void DigitalOutput::Set(bool value) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ HAL_SetDIO(m_handle, value, &status);
+ wpi_setHALError(status);
+}
+
+bool DigitalOutput::Get() const {
+ if (StatusIsFatal()) return false;
+
+ int32_t status = 0;
+ bool val = HAL_GetDIO(m_handle, &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) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ HAL_Pulse(m_handle, length, &status);
+ wpi_setHALError(status);
+}
+
+bool DigitalOutput::IsPulsing() const {
+ if (StatusIsFatal()) return false;
+
+ int32_t status = 0;
+ bool value = HAL_IsPulsing(m_handle, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+void DigitalOutput::SetPWMRate(double rate) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ HAL_SetDigitalPWMRate(rate, &status);
+ wpi_setHALError(status);
+}
+
+void DigitalOutput::EnablePWM(double initialDutyCycle) {
+ if (m_pwmGenerator != HAL_kInvalidHandle) return;
+
+ int32_t status = 0;
+
+ if (StatusIsFatal()) return;
+ m_pwmGenerator = HAL_AllocateDigitalPWM(&status);
+ wpi_setHALError(status);
+
+ if (StatusIsFatal()) return;
+ HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, initialDutyCycle, &status);
+ wpi_setHALError(status);
+
+ if (StatusIsFatal()) return;
+ HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, m_channel, &status);
+ wpi_setHALError(status);
+}
+
+void DigitalOutput::DisablePWM() {
+ if (StatusIsFatal()) return;
+ if (m_pwmGenerator == HAL_kInvalidHandle) return;
+
+ int32_t status = 0;
+
+ // Disable the output by routing to a dead bit.
+ HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, SensorUtil::kDigitalChannels,
+ &status);
+ wpi_setHALError(status);
+
+ HAL_FreeDigitalPWM(m_pwmGenerator, &status);
+ wpi_setHALError(status);
+
+ m_pwmGenerator = HAL_kInvalidHandle;
+}
+
+void DigitalOutput::UpdateDutyCycle(double dutyCycle) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle, &status);
+ wpi_setHALError(status);
+}
+
+void DigitalOutput::SetSimDevice(HAL_SimDeviceHandle device) {
+ HAL_SetDIOSimDevice(m_handle, device);
+}
+
+void DigitalOutput::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Digital Output");
+ builder.AddBooleanProperty("Value", [=]() { return Get(); },
+ [=](bool value) { Set(value); });
+}
diff --git a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
new file mode 100644
index 0000000..c896ea8
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
@@ -0,0 +1,167 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DoubleSolenoid.h"
+
+#include <utility>
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Ports.h>
+#include <hal/Solenoid.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+DoubleSolenoid::DoubleSolenoid(int forwardChannel, int reverseChannel)
+ : DoubleSolenoid(SensorUtil::GetDefaultSolenoidModule(), forwardChannel,
+ reverseChannel) {}
+
+DoubleSolenoid::DoubleSolenoid(int moduleNumber, int forwardChannel,
+ int reverseChannel)
+ : SolenoidBase(moduleNumber),
+ m_forwardChannel(forwardChannel),
+ m_reverseChannel(reverseChannel) {
+ if (!SensorUtil::CheckSolenoidModule(m_moduleNumber)) {
+ wpi_setWPIErrorWithContext(ModuleIndexOutOfRange,
+ "Solenoid Module " + wpi::Twine(m_moduleNumber));
+ return;
+ }
+ if (!SensorUtil::CheckSolenoidChannel(m_forwardChannel)) {
+ wpi_setWPIErrorWithContext(
+ ChannelIndexOutOfRange,
+ "Solenoid Channel " + wpi::Twine(m_forwardChannel));
+ return;
+ }
+ if (!SensorUtil::CheckSolenoidChannel(m_reverseChannel)) {
+ wpi_setWPIErrorWithContext(
+ ChannelIndexOutOfRange,
+ "Solenoid Channel " + wpi::Twine(m_reverseChannel));
+ return;
+ }
+ int32_t status = 0;
+ m_forwardHandle = HAL_InitializeSolenoidPort(
+ HAL_GetPortWithModule(moduleNumber, m_forwardChannel), &status);
+ if (status != 0) {
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(),
+ forwardChannel);
+ m_forwardHandle = HAL_kInvalidHandle;
+ m_reverseHandle = HAL_kInvalidHandle;
+ return;
+ }
+
+ m_reverseHandle = HAL_InitializeSolenoidPort(
+ HAL_GetPortWithModule(moduleNumber, m_reverseChannel), &status);
+ if (status != 0) {
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(),
+ reverseChannel);
+ // free forward solenoid
+ HAL_FreeSolenoidPort(m_forwardHandle);
+ m_forwardHandle = HAL_kInvalidHandle;
+ m_reverseHandle = HAL_kInvalidHandle;
+ return;
+ }
+
+ m_forwardMask = 1 << m_forwardChannel;
+ m_reverseMask = 1 << m_reverseChannel;
+
+ 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);
+}
+
+DoubleSolenoid::~DoubleSolenoid() {
+ HAL_FreeSolenoidPort(m_forwardHandle);
+ HAL_FreeSolenoidPort(m_reverseHandle);
+}
+
+void DoubleSolenoid::Set(Value value) {
+ if (StatusIsFatal()) return;
+
+ bool forward = false;
+ bool reverse = false;
+ switch (value) {
+ case kOff:
+ forward = false;
+ reverse = false;
+ break;
+ case kForward:
+ forward = true;
+ reverse = false;
+ break;
+ case kReverse:
+ forward = false;
+ reverse = true;
+ break;
+ }
+ int fstatus = 0;
+ HAL_SetSolenoid(m_forwardHandle, forward, &fstatus);
+ int rstatus = 0;
+ HAL_SetSolenoid(m_reverseHandle, reverse, &rstatus);
+
+ wpi_setHALError(fstatus);
+ wpi_setHALError(rstatus);
+}
+
+DoubleSolenoid::Value DoubleSolenoid::Get() const {
+ if (StatusIsFatal()) return kOff;
+ int fstatus = 0;
+ int rstatus = 0;
+ bool valueForward = HAL_GetSolenoid(m_forwardHandle, &fstatus);
+ bool valueReverse = HAL_GetSolenoid(m_reverseHandle, &rstatus);
+
+ wpi_setHALError(fstatus);
+ wpi_setHALError(rstatus);
+
+ if (valueForward) return kForward;
+ if (valueReverse) return kReverse;
+ return kOff;
+}
+
+bool DoubleSolenoid::IsFwdSolenoidBlackListed() const {
+ int blackList = GetPCMSolenoidBlackList(m_moduleNumber);
+ return (blackList & m_forwardMask) != 0;
+}
+
+bool DoubleSolenoid::IsRevSolenoidBlackListed() const {
+ int blackList = GetPCMSolenoidBlackList(m_moduleNumber);
+ return (blackList & m_reverseMask) != 0;
+}
+
+void DoubleSolenoid::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Double Solenoid");
+ builder.SetActuator(true);
+ builder.SetSafeState([=]() { Set(kOff); });
+ builder.AddSmallStringProperty(
+ "Value",
+ [=](wpi::SmallVectorImpl<char>& buf) -> wpi::StringRef {
+ switch (Get()) {
+ case kForward:
+ return "Forward";
+ case kReverse:
+ return "Reverse";
+ default:
+ return "Off";
+ }
+ },
+ [=](wpi::StringRef value) {
+ Value lvalue = kOff;
+ if (value == "Forward")
+ lvalue = kForward;
+ else if (value == "Reverse")
+ lvalue = kReverse;
+ Set(lvalue);
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp
new file mode 100644
index 0000000..7c64883
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DriverStation.cpp
@@ -0,0 +1,615 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DriverStation.h"
+
+#include <chrono>
+
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Power.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/SmallString.h>
+#include <wpi/StringRef.h>
+
+#include "frc/AnalogInput.h"
+#include "frc/MotorSafety.h"
+#include "frc/Timer.h"
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+
+namespace frc {
+
+class MatchDataSender {
+ public:
+ std::shared_ptr<nt::NetworkTable> table;
+ nt::NetworkTableEntry typeMetadata;
+ nt::NetworkTableEntry gameSpecificMessage;
+ nt::NetworkTableEntry eventName;
+ nt::NetworkTableEntry matchNumber;
+ nt::NetworkTableEntry replayNumber;
+ nt::NetworkTableEntry matchType;
+ nt::NetworkTableEntry alliance;
+ nt::NetworkTableEntry station;
+ nt::NetworkTableEntry controlWord;
+
+ MatchDataSender() {
+ table = nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
+ typeMetadata = table->GetEntry(".type");
+ typeMetadata.ForceSetString("FMSInfo");
+ gameSpecificMessage = table->GetEntry("GameSpecificMessage");
+ gameSpecificMessage.ForceSetString("");
+ eventName = table->GetEntry("EventName");
+ eventName.ForceSetString("");
+ matchNumber = table->GetEntry("MatchNumber");
+ matchNumber.ForceSetDouble(0);
+ replayNumber = table->GetEntry("ReplayNumber");
+ replayNumber.ForceSetDouble(0);
+ matchType = table->GetEntry("MatchType");
+ matchType.ForceSetDouble(0);
+ alliance = table->GetEntry("IsRedAlliance");
+ alliance.ForceSetBoolean(true);
+ station = table->GetEntry("StationNumber");
+ station.ForceSetDouble(1);
+ controlWord = table->GetEntry("FMSControlData");
+ controlWord.ForceSetDouble(0);
+ }
+};
+} // namespace frc
+
+using namespace frc;
+
+static constexpr double kJoystickUnpluggedMessageInterval = 1.0;
+
+DriverStation::~DriverStation() {
+ m_isRunning = false;
+ // Trigger a DS mutex release in case there is no driver station running.
+ HAL_ReleaseDSMutex();
+ m_dsThread.join();
+}
+
+DriverStation& DriverStation::GetInstance() {
+ static DriverStation instance;
+ return instance;
+}
+
+void DriverStation::ReportError(const wpi::Twine& error) {
+ wpi::SmallString<128> temp;
+ HAL_SendError(1, 1, 0, error.toNullTerminatedStringRef(temp).data(), "", "",
+ 1);
+}
+
+void DriverStation::ReportWarning(const wpi::Twine& error) {
+ wpi::SmallString<128> temp;
+ HAL_SendError(0, 1, 0, error.toNullTerminatedStringRef(temp).data(), "", "",
+ 1);
+}
+
+void DriverStation::ReportError(bool isError, int32_t code,
+ const wpi::Twine& error,
+ const wpi::Twine& location,
+ const wpi::Twine& stack) {
+ wpi::SmallString<128> errorTemp;
+ wpi::SmallString<128> locationTemp;
+ wpi::SmallString<128> stackTemp;
+ HAL_SendError(isError, code, 0,
+ error.toNullTerminatedStringRef(errorTemp).data(),
+ location.toNullTerminatedStringRef(locationTemp).data(),
+ stack.toNullTerminatedStringRef(stackTemp).data(), 1);
+}
+
+bool DriverStation::GetStickButton(int stick, int button) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return false;
+ }
+ if (button <= 0) {
+ ReportJoystickUnpluggedError(
+ "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
+ return false;
+ }
+
+ HAL_JoystickButtons buttons;
+ HAL_GetJoystickButtons(stick, &buttons);
+
+ if (button > buttons.count) {
+ ReportJoystickUnpluggedWarning(
+ "Joystick Button missing, check if all controllers are plugged in");
+ return false;
+ }
+
+ return buttons.buttons & 1 << (button - 1);
+}
+
+bool DriverStation::GetStickButtonPressed(int stick, int button) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return false;
+ }
+ if (button <= 0) {
+ ReportJoystickUnpluggedError(
+ "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
+ return false;
+ }
+
+ HAL_JoystickButtons buttons;
+ HAL_GetJoystickButtons(stick, &buttons);
+
+ if (button > buttons.count) {
+ ReportJoystickUnpluggedWarning(
+ "Joystick Button missing, check if all controllers are plugged in");
+ return false;
+ }
+ std::unique_lock lock(m_buttonEdgeMutex);
+ // If button was pressed, clear flag and return true
+ if (m_joystickButtonsPressed[stick] & 1 << (button - 1)) {
+ m_joystickButtonsPressed[stick] &= ~(1 << (button - 1));
+ return true;
+ } else {
+ return false;
+ }
+}
+
+bool DriverStation::GetStickButtonReleased(int stick, int button) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return false;
+ }
+ if (button <= 0) {
+ ReportJoystickUnpluggedError(
+ "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
+ return false;
+ }
+
+ HAL_JoystickButtons buttons;
+ HAL_GetJoystickButtons(stick, &buttons);
+
+ if (button > buttons.count) {
+ ReportJoystickUnpluggedWarning(
+ "Joystick Button missing, check if all controllers are plugged in");
+ return false;
+ }
+ std::unique_lock lock(m_buttonEdgeMutex);
+ // If button was released, clear flag and return true
+ if (m_joystickButtonsReleased[stick] & 1 << (button - 1)) {
+ m_joystickButtonsReleased[stick] &= ~(1 << (button - 1));
+ return true;
+ } else {
+ return false;
+ }
+}
+
+double DriverStation::GetStickAxis(int stick, int axis) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0.0;
+ }
+ if (axis < 0 || axis >= HAL_kMaxJoystickAxes) {
+ wpi_setWPIError(BadJoystickAxis);
+ return 0.0;
+ }
+
+ HAL_JoystickAxes axes;
+ HAL_GetJoystickAxes(stick, &axes);
+
+ if (axis >= axes.count) {
+ ReportJoystickUnpluggedWarning(
+ "Joystick Axis missing, check if all controllers are plugged in");
+ return 0.0;
+ }
+
+ return axes.axes[axis];
+}
+
+int DriverStation::GetStickPOV(int stick, int pov) {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return -1;
+ }
+ if (pov < 0 || pov >= HAL_kMaxJoystickPOVs) {
+ wpi_setWPIError(BadJoystickAxis);
+ return -1;
+ }
+
+ HAL_JoystickPOVs povs;
+ HAL_GetJoystickPOVs(stick, &povs);
+
+ if (pov >= povs.count) {
+ ReportJoystickUnpluggedWarning(
+ "Joystick POV missing, check if all controllers are plugged in");
+ return -1;
+ }
+
+ return povs.povs[pov];
+}
+
+int DriverStation::GetStickButtons(int stick) const {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+
+ HAL_JoystickButtons buttons;
+ HAL_GetJoystickButtons(stick, &buttons);
+
+ return buttons.buttons;
+}
+
+int DriverStation::GetStickAxisCount(int stick) const {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+
+ HAL_JoystickAxes axes;
+ HAL_GetJoystickAxes(stick, &axes);
+
+ return axes.count;
+}
+
+int DriverStation::GetStickPOVCount(int stick) const {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+
+ HAL_JoystickPOVs povs;
+ HAL_GetJoystickPOVs(stick, &povs);
+
+ return povs.count;
+}
+
+int DriverStation::GetStickButtonCount(int stick) const {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+
+ HAL_JoystickButtons buttons;
+ HAL_GetJoystickButtons(stick, &buttons);
+
+ return buttons.count;
+}
+
+bool DriverStation::GetJoystickIsXbox(int stick) const {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return false;
+ }
+
+ HAL_JoystickDescriptor descriptor;
+ HAL_GetJoystickDescriptor(stick, &descriptor);
+
+ return static_cast<bool>(descriptor.isXbox);
+}
+
+int DriverStation::GetJoystickType(int stick) const {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return -1;
+ }
+
+ HAL_JoystickDescriptor descriptor;
+ HAL_GetJoystickDescriptor(stick, &descriptor);
+
+ return static_cast<int>(descriptor.type);
+}
+
+std::string DriverStation::GetJoystickName(int stick) const {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ }
+
+ HAL_JoystickDescriptor descriptor;
+ HAL_GetJoystickDescriptor(stick, &descriptor);
+
+ return descriptor.name;
+}
+
+int DriverStation::GetJoystickAxisType(int stick, int axis) const {
+ if (stick < 0 || stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return -1;
+ }
+
+ HAL_JoystickDescriptor descriptor;
+ HAL_GetJoystickDescriptor(stick, &descriptor);
+
+ return static_cast<bool>(descriptor.axisTypes);
+}
+
+bool DriverStation::IsEnabled() const {
+ HAL_ControlWord controlWord;
+ HAL_GetControlWord(&controlWord);
+ return controlWord.enabled && controlWord.dsAttached;
+}
+
+bool DriverStation::IsDisabled() const {
+ HAL_ControlWord controlWord;
+ HAL_GetControlWord(&controlWord);
+ return !(controlWord.enabled && controlWord.dsAttached);
+}
+
+bool DriverStation::IsEStopped() const {
+ HAL_ControlWord controlWord;
+ HAL_GetControlWord(&controlWord);
+ return controlWord.eStop;
+}
+
+bool DriverStation::IsAutonomous() const {
+ HAL_ControlWord controlWord;
+ HAL_GetControlWord(&controlWord);
+ return controlWord.autonomous;
+}
+
+bool DriverStation::IsOperatorControl() const {
+ HAL_ControlWord controlWord;
+ HAL_GetControlWord(&controlWord);
+ return !(controlWord.autonomous || controlWord.test);
+}
+
+bool DriverStation::IsTest() const {
+ HAL_ControlWord controlWord;
+ HAL_GetControlWord(&controlWord);
+ return controlWord.test;
+}
+
+bool DriverStation::IsDSAttached() const {
+ HAL_ControlWord controlWord;
+ HAL_GetControlWord(&controlWord);
+ return controlWord.dsAttached;
+}
+
+bool DriverStation::IsNewControlData() const { return HAL_IsNewControlData(); }
+
+bool DriverStation::IsFMSAttached() const {
+ HAL_ControlWord controlWord;
+ HAL_GetControlWord(&controlWord);
+ return controlWord.fmsAttached;
+}
+
+std::string DriverStation::GetGameSpecificMessage() const {
+ HAL_MatchInfo info;
+ HAL_GetMatchInfo(&info);
+ return std::string(reinterpret_cast<char*>(info.gameSpecificMessage),
+ info.gameSpecificMessageSize);
+}
+
+std::string DriverStation::GetEventName() const {
+ HAL_MatchInfo info;
+ HAL_GetMatchInfo(&info);
+ return info.eventName;
+}
+
+DriverStation::MatchType DriverStation::GetMatchType() const {
+ HAL_MatchInfo info;
+ HAL_GetMatchInfo(&info);
+ return static_cast<DriverStation::MatchType>(info.matchType);
+}
+
+int DriverStation::GetMatchNumber() const {
+ HAL_MatchInfo info;
+ HAL_GetMatchInfo(&info);
+ return info.matchNumber;
+}
+
+int DriverStation::GetReplayNumber() const {
+ HAL_MatchInfo info;
+ HAL_GetMatchInfo(&info);
+ return info.replayNumber;
+}
+
+DriverStation::Alliance DriverStation::GetAlliance() const {
+ int32_t status = 0;
+ auto allianceStationID = HAL_GetAllianceStation(&status);
+ switch (allianceStationID) {
+ case HAL_AllianceStationID_kRed1:
+ case HAL_AllianceStationID_kRed2:
+ case HAL_AllianceStationID_kRed3:
+ return kRed;
+ case HAL_AllianceStationID_kBlue1:
+ case HAL_AllianceStationID_kBlue2:
+ case HAL_AllianceStationID_kBlue3:
+ return kBlue;
+ default:
+ return kInvalid;
+ }
+}
+
+int DriverStation::GetLocation() const {
+ int32_t status = 0;
+ auto allianceStationID = HAL_GetAllianceStation(&status);
+ switch (allianceStationID) {
+ case HAL_AllianceStationID_kRed1:
+ case HAL_AllianceStationID_kBlue1:
+ return 1;
+ case HAL_AllianceStationID_kRed2:
+ case HAL_AllianceStationID_kBlue2:
+ return 2;
+ case HAL_AllianceStationID_kRed3:
+ case HAL_AllianceStationID_kBlue3:
+ return 3;
+ default:
+ return 0;
+ }
+}
+
+void DriverStation::WaitForData() { WaitForData(0); }
+
+bool DriverStation::WaitForData(double timeout) {
+ auto timeoutTime =
+ std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+ std::unique_lock lock(m_waitForDataMutex);
+ int currentCount = m_waitForDataCounter;
+ while (m_waitForDataCounter == currentCount) {
+ if (timeout > 0) {
+ auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
+ if (timedOut == std::cv_status::timeout) {
+ return false;
+ }
+ } else {
+ m_waitForDataCond.wait(lock);
+ }
+ }
+ return true;
+}
+
+double DriverStation::GetMatchTime() const {
+ int32_t status;
+ return HAL_GetMatchTime(&status);
+}
+
+double DriverStation::GetBatteryVoltage() const {
+ int32_t status = 0;
+ double voltage = HAL_GetVinVoltage(&status);
+ wpi_setErrorWithContext(status, "getVinVoltage");
+
+ 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
+ HAL_JoystickButtons currentButtons;
+ std::unique_lock lock(m_buttonEdgeMutex);
+
+ for (int32_t i = 0; i < kJoystickPorts; i++) {
+ HAL_GetJoystickButtons(i, ¤tButtons);
+
+ // If buttons weren't pressed and are now, set flags in m_buttonsPressed
+ m_joystickButtonsPressed[i] |=
+ ~m_previousButtonStates[i].buttons & currentButtons.buttons;
+
+ // If buttons were pressed and aren't now, set flags in m_buttonsReleased
+ m_joystickButtonsReleased[i] |=
+ m_previousButtonStates[i].buttons & ~currentButtons.buttons;
+
+ m_previousButtonStates[i] = currentButtons;
+ }
+ }
+
+ WakeupWaitForData();
+ SendMatchData();
+}
+
+DriverStation::DriverStation() {
+ HAL_Initialize(500, 0);
+ m_waitForDataCounter = 0;
+
+ m_matchDataSender = std::make_unique<MatchDataSender>();
+
+ // All joysticks should default to having zero axes, povs and buttons, so
+ // uninitialized memory doesn't get sent to speed controllers.
+ for (unsigned int i = 0; i < kJoystickPorts; i++) {
+ m_joystickButtonsPressed[i] = 0;
+ m_joystickButtonsReleased[i] = 0;
+ m_previousButtonStates[i].count = 0;
+ m_previousButtonStates[i].buttons = 0;
+ }
+
+ m_dsThread = std::thread(&DriverStation::Run, this);
+}
+
+void DriverStation::ReportJoystickUnpluggedError(const wpi::Twine& message) {
+ double currentTime = Timer::GetFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ ReportError(message);
+ m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+ }
+}
+
+void DriverStation::ReportJoystickUnpluggedWarning(const wpi::Twine& message) {
+ double currentTime = Timer::GetFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ ReportWarning(message);
+ m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+ }
+}
+
+void DriverStation::Run() {
+ m_isRunning = true;
+ int safetyCounter = 0;
+ while (m_isRunning) {
+ HAL_WaitForDSData();
+ GetData();
+
+ if (IsDisabled()) safetyCounter = 0;
+
+ if (++safetyCounter >= 4) {
+ MotorSafety::CheckMotors();
+ safetyCounter = 0;
+ }
+ if (m_userInDisabled) HAL_ObserveUserProgramDisabled();
+ if (m_userInAutonomous) HAL_ObserveUserProgramAutonomous();
+ if (m_userInTeleop) HAL_ObserveUserProgramTeleop();
+ if (m_userInTest) HAL_ObserveUserProgramTest();
+ }
+}
+
+void DriverStation::SendMatchData() {
+ int32_t status = 0;
+ HAL_AllianceStationID alliance = HAL_GetAllianceStation(&status);
+ bool isRedAlliance = false;
+ int stationNumber = 1;
+ switch (alliance) {
+ case HAL_AllianceStationID::HAL_AllianceStationID_kBlue1:
+ isRedAlliance = false;
+ stationNumber = 1;
+ break;
+ case HAL_AllianceStationID::HAL_AllianceStationID_kBlue2:
+ isRedAlliance = false;
+ stationNumber = 2;
+ break;
+ case HAL_AllianceStationID::HAL_AllianceStationID_kBlue3:
+ isRedAlliance = false;
+ stationNumber = 3;
+ break;
+ case HAL_AllianceStationID::HAL_AllianceStationID_kRed1:
+ isRedAlliance = true;
+ stationNumber = 1;
+ break;
+ case HAL_AllianceStationID::HAL_AllianceStationID_kRed2:
+ isRedAlliance = true;
+ stationNumber = 2;
+ break;
+ default:
+ isRedAlliance = true;
+ stationNumber = 3;
+ break;
+ }
+
+ HAL_MatchInfo tmpDataStore;
+ HAL_GetMatchInfo(&tmpDataStore);
+
+ m_matchDataSender->alliance.SetBoolean(isRedAlliance);
+ m_matchDataSender->station.SetDouble(stationNumber);
+ m_matchDataSender->eventName.SetString(tmpDataStore.eventName);
+ m_matchDataSender->gameSpecificMessage.SetString(
+ std::string(reinterpret_cast<char*>(tmpDataStore.gameSpecificMessage),
+ tmpDataStore.gameSpecificMessageSize));
+ m_matchDataSender->matchNumber.SetDouble(tmpDataStore.matchNumber);
+ m_matchDataSender->replayNumber.SetDouble(tmpDataStore.replayNumber);
+ m_matchDataSender->matchType.SetDouble(
+ static_cast<int>(tmpDataStore.matchType));
+
+ HAL_ControlWord ctlWord;
+ HAL_GetControlWord(&ctlWord);
+ int32_t wordInt = 0;
+ std::memcpy(&wordInt, &ctlWord, sizeof(wordInt));
+ m_matchDataSender->controlWord.SetDouble(wordInt);
+}
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..ea054ce
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified 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{} {
+ Init();
+}
+
+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
new file mode 100644
index 0000000..963bcc6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Encoder.cpp
@@ -0,0 +1,268 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Encoder.h"
+
+#include <utility>
+
+#include <hal/Encoder.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Encoder::Encoder(int aChannel, int bChannel, bool reverseDirection,
+ EncodingType encodingType) {
+ m_aSource = std::make_shared<DigitalInput>(aChannel);
+ m_bSource = std::make_shared<DigitalInput>(bChannel);
+ InitEncoder(reverseDirection, encodingType);
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, m_aSource.get());
+ registry.AddChild(this, m_bSource.get());
+}
+
+Encoder::Encoder(DigitalSource* aSource, DigitalSource* bSource,
+ bool reverseDirection, EncodingType encodingType)
+ : m_aSource(aSource, NullDeleter<DigitalSource>()),
+ m_bSource(bSource, NullDeleter<DigitalSource>()) {
+ if (m_aSource == nullptr || m_bSource == nullptr)
+ wpi_setWPIError(NullParameter);
+ else
+ InitEncoder(reverseDirection, encodingType);
+}
+
+Encoder::Encoder(DigitalSource& aSource, DigitalSource& bSource,
+ bool reverseDirection, EncodingType encodingType)
+ : m_aSource(&aSource, NullDeleter<DigitalSource>()),
+ m_bSource(&bSource, NullDeleter<DigitalSource>()) {
+ InitEncoder(reverseDirection, encodingType);
+}
+
+Encoder::Encoder(std::shared_ptr<DigitalSource> aSource,
+ std::shared_ptr<DigitalSource> bSource, bool reverseDirection,
+ EncodingType encodingType)
+ : m_aSource(aSource), m_bSource(bSource) {
+ if (m_aSource == nullptr || m_bSource == nullptr)
+ wpi_setWPIError(NullParameter);
+ else
+ InitEncoder(reverseDirection, encodingType);
+}
+
+Encoder::~Encoder() {
+ int32_t status = 0;
+ HAL_FreeEncoder(m_encoder, &status);
+ wpi_setHALError(status);
+}
+
+int Encoder::Get() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetEncoder(m_encoder, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+void Encoder::Reset() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_ResetEncoder(m_encoder, &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_setHALError(status);
+ return value;
+}
+
+void Encoder::SetMaxPeriod(double maxPeriod) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod, &status);
+ wpi_setHALError(status);
+}
+
+bool Encoder::GetStopped() const {
+ if (StatusIsFatal()) return true;
+ int32_t status = 0;
+ bool value = HAL_GetEncoderStopped(m_encoder, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+bool Encoder::GetDirection() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = HAL_GetEncoderDirection(m_encoder, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+int Encoder::GetRaw() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetEncoderRaw(m_encoder, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+int Encoder::GetEncodingScale() const {
+ int32_t status = 0;
+ int val = HAL_GetEncoderEncodingScale(m_encoder, &status);
+ wpi_setHALError(status);
+ return val;
+}
+
+double Encoder::GetDistance() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetEncoderDistance(m_encoder, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+double Encoder::GetRate() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetEncoderRate(m_encoder, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+void Encoder::SetMinRate(double minRate) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetEncoderMinRate(m_encoder, minRate, &status);
+ wpi_setHALError(status);
+}
+
+void Encoder::SetDistancePerPulse(double distancePerPulse) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetEncoderDistancePerPulse(m_encoder, distancePerPulse, &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_setHALError(status);
+ return distancePerPulse;
+}
+
+void Encoder::SetReverseDirection(bool reverseDirection) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetEncoderReverseDirection(m_encoder, reverseDirection, &status);
+ wpi_setHALError(status);
+}
+
+void Encoder::SetSamplesToAverage(int samplesToAverage) {
+ if (samplesToAverage < 1 || samplesToAverage > 127) {
+ wpi_setWPIErrorWithContext(
+ ParameterOutOfRange,
+ "Average counter values must be between 1 and 127");
+ return;
+ }
+ int32_t status = 0;
+ HAL_SetEncoderSamplesToAverage(m_encoder, samplesToAverage, &status);
+ wpi_setHALError(status);
+}
+
+int Encoder::GetSamplesToAverage() const {
+ int32_t status = 0;
+ int result = HAL_GetEncoderSamplesToAverage(m_encoder, &status);
+ wpi_setHALError(status);
+ return result;
+}
+
+double Encoder::PIDGet() {
+ if (StatusIsFatal()) return 0.0;
+ switch (GetPIDSourceType()) {
+ case PIDSourceType::kDisplacement:
+ return GetDistance();
+ case PIDSourceType::kRate:
+ return GetRate();
+ default:
+ return 0.0;
+ }
+}
+
+void Encoder::SetIndexSource(int channel, Encoder::IndexingType type) {
+ // Force digital input if just given an index
+ m_indexSource = std::make_shared<DigitalInput>(channel);
+ SendableRegistry::GetInstance().AddChild(this, m_indexSource.get());
+ SetIndexSource(*m_indexSource.get(), type);
+}
+
+void Encoder::SetIndexSource(const DigitalSource& source,
+ Encoder::IndexingType type) {
+ int32_t status = 0;
+ HAL_SetEncoderIndexSource(
+ m_encoder, source.GetPortHandleForRouting(),
+ (HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(),
+ (HAL_EncoderIndexingType)type, &status);
+ wpi_setHALError(status);
+}
+
+void Encoder::SetSimDevice(HAL_SimDeviceHandle device) {
+ HAL_SetEncoderSimDevice(m_encoder, device);
+}
+
+int Encoder::GetFPGAIndex() const {
+ int32_t status = 0;
+ int val = HAL_GetEncoderFPGAIndex(m_encoder, &status);
+ wpi_setHALError(status);
+ return val;
+}
+
+void Encoder::InitSendable(SendableBuilder& builder) {
+ int32_t status = 0;
+ HAL_EncoderEncodingType type = HAL_GetEncoderEncodingType(m_encoder, &status);
+ wpi_setHALError(status);
+ if (type == HAL_EncoderEncodingType::HAL_Encoder_k4X)
+ builder.SetSmartDashboardType("Quadrature Encoder");
+ else
+ builder.SetSmartDashboardType("Encoder");
+
+ builder.AddDoubleProperty("Speed", [=]() { return GetRate(); }, nullptr);
+ builder.AddDoubleProperty("Distance", [=]() { return GetDistance(); },
+ nullptr);
+ builder.AddDoubleProperty("Distance per Tick",
+ [=]() { return GetDistancePerPulse(); }, nullptr);
+}
+
+void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) {
+ int32_t status = 0;
+ m_encoder = HAL_InitializeEncoder(
+ m_aSource->GetPortHandleForRouting(),
+ (HAL_AnalogTriggerType)m_aSource->GetAnalogTriggerTypeForRouting(),
+ m_bSource->GetPortHandleForRouting(),
+ (HAL_AnalogTriggerType)m_bSource->GetAnalogTriggerTypeForRouting(),
+ reverseDirection, (HAL_EncoderEncodingType)encodingType, &status);
+ wpi_setHALError(status);
+
+ HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex() + 1,
+ encodingType);
+ SendableRegistry::GetInstance().AddLW(this, "Encoder",
+ m_aSource->GetChannel());
+}
+
+double Encoder::DecodingScaleFactor() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double val = HAL_GetEncoderDecodingScaleFactor(m_encoder, &status);
+ wpi_setHALError(status);
+ return val;
+}
diff --git a/wpilibc/src/main/native/cpp/Error.cpp b/wpilibc/src/main/native/cpp/Error.cpp
new file mode 100644
index 0000000..ee7dcbd
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Error.cpp
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Error.h"
+
+#include <wpi/Path.h>
+#include <wpi/StackTrace.h>
+
+#include "frc/DriverStation.h"
+#include "frc/Timer.h"
+#include "frc/Utility.h"
+
+using namespace frc;
+
+Error::Error(Code code, const wpi::Twine& contextMessage,
+ wpi::StringRef filename, wpi::StringRef function, int lineNumber,
+ const ErrorBase* originatingObject) {
+ Set(code, contextMessage, filename, function, lineNumber, originatingObject);
+}
+
+bool Error::operator<(const Error& rhs) const {
+ if (m_code < rhs.m_code) {
+ return true;
+ } else if (m_message < rhs.m_message) {
+ return true;
+ } else if (m_filename < rhs.m_filename) {
+ return true;
+ } else if (m_function < rhs.m_function) {
+ return true;
+ } else if (m_lineNumber < rhs.m_lineNumber) {
+ return true;
+ } else if (m_originatingObject < rhs.m_originatingObject) {
+ return true;
+ } else if (m_timestamp < rhs.m_timestamp) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+Error::Code Error::GetCode() const { return m_code; }
+
+std::string Error::GetMessage() const { return m_message; }
+
+std::string Error::GetFilename() const { return m_filename; }
+
+std::string Error::GetFunction() const { return m_function; }
+
+int Error::GetLineNumber() const { return m_lineNumber; }
+
+const ErrorBase* Error::GetOriginatingObject() const {
+ return m_originatingObject;
+}
+
+double Error::GetTimestamp() const { return m_timestamp; }
+
+void Error::Set(Code code, const wpi::Twine& contextMessage,
+ wpi::StringRef filename, wpi::StringRef function,
+ int lineNumber, const ErrorBase* originatingObject) {
+ bool report = true;
+
+ if (code == m_code && GetTime() - m_timestamp < 1) {
+ report = false;
+ }
+
+ m_code = code;
+ m_message = contextMessage.str();
+ m_filename = filename;
+ m_function = function;
+ m_lineNumber = lineNumber;
+ m_originatingObject = originatingObject;
+
+ if (report) {
+ m_timestamp = GetTime();
+ Report();
+ }
+}
+
+void Error::Report() {
+ DriverStation::ReportError(
+ true, m_code, m_message,
+ m_function + wpi::Twine(" [") + wpi::sys::path::filename(m_filename) +
+ wpi::Twine(':') + wpi::Twine(m_lineNumber) + wpi::Twine(']'),
+ wpi::GetStackTrace(4));
+}
+
+void Error::Clear() {
+ m_code = 0;
+ m_message = "";
+ m_filename = "";
+ m_function = "";
+ m_lineNumber = 0;
+ m_originatingObject = nullptr;
+ m_timestamp = 0.0;
+}
diff --git a/wpilibc/src/main/native/cpp/ErrorBase.cpp b/wpilibc/src/main/native/cpp/ErrorBase.cpp
new file mode 100644
index 0000000..d098c07
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ErrorBase.cpp
@@ -0,0 +1,187 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ErrorBase.h"
+
+#include <cerrno>
+#include <cstdio>
+#include <cstring>
+#include <set>
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <wpi/Format.h>
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+namespace {
+struct GlobalErrors {
+ wpi::mutex mutex;
+ std::set<Error> errors;
+ const Error* lastError{nullptr};
+
+ static GlobalErrors& GetInstance();
+ static void Insert(const Error& error);
+ static void Insert(Error&& error);
+};
+} // namespace
+
+GlobalErrors& GlobalErrors::GetInstance() {
+ static GlobalErrors inst;
+ return inst;
+}
+
+void GlobalErrors::Insert(const Error& error) {
+ GlobalErrors& inst = GetInstance();
+ std::scoped_lock lock(inst.mutex);
+ inst.lastError = &(*inst.errors.insert(error).first);
+}
+
+void GlobalErrors::Insert(Error&& error) {
+ GlobalErrors& inst = GetInstance();
+ std::scoped_lock lock(inst.mutex);
+ inst.lastError = &(*inst.errors.insert(std::move(error)).first);
+}
+
+ErrorBase::ErrorBase() { HAL_Initialize(500, 0); }
+
+Error& ErrorBase::GetError() { return m_error; }
+
+const Error& ErrorBase::GetError() const { return m_error; }
+
+void ErrorBase::ClearError() const { m_error.Clear(); }
+
+void ErrorBase::SetErrnoError(const wpi::Twine& contextMessage,
+ wpi::StringRef filename, wpi::StringRef function,
+ int lineNumber) const {
+ wpi::SmallString<128> buf;
+ wpi::raw_svector_ostream err(buf);
+ int errNo = errno;
+ if (errNo == 0) {
+ err << "OK: ";
+ } else {
+ err << std::strerror(errNo) << " (" << wpi::format_hex(errNo, 10, true)
+ << "): ";
+ }
+
+ // Set the current error information for this object.
+ m_error.Set(-1, err.str() + contextMessage, filename, function, lineNumber,
+ this);
+
+ // Update the global error if there is not one already set.
+ GlobalErrors::Insert(m_error);
+}
+
+void ErrorBase::SetImaqError(int success, const wpi::Twine& contextMessage,
+ wpi::StringRef filename, wpi::StringRef function,
+ int lineNumber) const {
+ // If there was an error
+ if (success <= 0) {
+ // Set the current error information for this object.
+ m_error.Set(success, wpi::Twine(success) + ": " + contextMessage, filename,
+ function, lineNumber, this);
+
+ // Update the global error if there is not one already set.
+ GlobalErrors::Insert(m_error);
+ }
+}
+
+void ErrorBase::SetError(Error::Code code, const wpi::Twine& contextMessage,
+ wpi::StringRef filename, wpi::StringRef function,
+ int lineNumber) const {
+ // If there was an error
+ if (code != 0) {
+ // Set the current error information for this object.
+ m_error.Set(code, contextMessage, filename, function, lineNumber, this);
+
+ // Update the global error if there is not one already set.
+ GlobalErrors::Insert(m_error);
+ }
+}
+
+void ErrorBase::SetErrorRange(Error::Code code, int32_t minRange,
+ int32_t maxRange, int32_t requestedValue,
+ const wpi::Twine& contextMessage,
+ wpi::StringRef filename, wpi::StringRef function,
+ int lineNumber) const {
+ // If there was an error
+ if (code != 0) {
+ // Set the current error information for this object.
+ m_error.Set(code,
+ contextMessage + ", Minimum Value: " + wpi::Twine(minRange) +
+ ", MaximumValue: " + wpi::Twine(maxRange) +
+ ", Requested Value: " + wpi::Twine(requestedValue),
+ filename, function, lineNumber, this);
+
+ // Update the global error if there is not one already set.
+ GlobalErrors::Insert(m_error);
+ }
+}
+
+void ErrorBase::SetWPIError(const wpi::Twine& errorMessage, Error::Code code,
+ const wpi::Twine& contextMessage,
+ wpi::StringRef filename, wpi::StringRef function,
+ int lineNumber) const {
+ // Set the current error information for this object.
+ m_error.Set(code, errorMessage + ": " + contextMessage, filename, function,
+ lineNumber, this);
+
+ // Update the global error if there is not one already set.
+ GlobalErrors::Insert(m_error);
+}
+
+void ErrorBase::CloneError(const ErrorBase& rhs) const {
+ m_error = rhs.GetError();
+}
+
+bool ErrorBase::StatusIsFatal() const { return m_error.GetCode() < 0; }
+
+void ErrorBase::SetGlobalError(Error::Code code,
+ const wpi::Twine& contextMessage,
+ wpi::StringRef filename, wpi::StringRef function,
+ int lineNumber) {
+ // If there was an error
+ if (code != 0) {
+ // Set the current error information for this object.
+ GlobalErrors::Insert(
+ Error(code, contextMessage, filename, function, lineNumber, nullptr));
+ }
+}
+
+void ErrorBase::SetGlobalWPIError(const wpi::Twine& errorMessage,
+ const wpi::Twine& contextMessage,
+ wpi::StringRef filename,
+ wpi::StringRef function, int lineNumber) {
+ GlobalErrors::Insert(Error(-1, errorMessage + ": " + contextMessage, filename,
+ function, lineNumber, nullptr));
+}
+
+Error ErrorBase::GetGlobalError() {
+ auto& inst = GlobalErrors::GetInstance();
+ std::scoped_lock mutex(inst.mutex);
+ if (!inst.lastError) return Error{};
+ return *inst.lastError;
+}
+
+std::vector<Error> ErrorBase::GetGlobalErrors() {
+ auto& inst = GlobalErrors::GetInstance();
+ std::scoped_lock mutex(inst.mutex);
+ std::vector<Error> rv;
+ for (auto&& error : inst.errors) rv.push_back(error);
+ return rv;
+}
+
+void ErrorBase::ClearGlobalErrors() {
+ auto& inst = GlobalErrors::GetInstance();
+ std::scoped_lock mutex(inst.mutex);
+ inst.errors.clear();
+ inst.lastError = nullptr;
+}
diff --git a/wpilibc/src/main/native/cpp/Filesystem.cpp b/wpilibc/src/main/native/cpp/Filesystem.cpp
new file mode 100644
index 0000000..1546050
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Filesystem.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Filesystem.h"
+
+#include <wpi/FileSystem.h>
+#include <wpi/Path.h>
+
+#include "frc/RobotBase.h"
+
+void frc::filesystem::GetLaunchDirectory(wpi::SmallVectorImpl<char>& result) {
+ wpi::sys::fs::current_path(result);
+}
+
+void frc::filesystem::GetOperatingDirectory(
+ wpi::SmallVectorImpl<char>& result) {
+ if (RobotBase::IsReal()) {
+ wpi::sys::path::native("/home/lvuser", result);
+ } else {
+ frc::filesystem::GetLaunchDirectory(result);
+ }
+}
+
+void frc::filesystem::GetDeployDirectory(wpi::SmallVectorImpl<char>& result) {
+ frc::filesystem::GetOperatingDirectory(result);
+ wpi::sys::path::append(result, "deploy");
+}
diff --git a/wpilibc/src/main/native/cpp/GearTooth.cpp b/wpilibc/src/main/native/cpp/GearTooth.cpp
new file mode 100644
index 0000000..5fb5021
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/GearTooth.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/GearTooth.h"
+
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+constexpr double GearTooth::kGearToothThreshold;
+
+GearTooth::GearTooth(int channel, bool directionSensitive) : Counter(channel) {
+ EnableDirectionSensing(directionSensitive);
+ SendableRegistry::GetInstance().SetName(this, "GearTooth", channel);
+}
+
+GearTooth::GearTooth(DigitalSource* source, bool directionSensitive)
+ : Counter(source) {
+ EnableDirectionSensing(directionSensitive);
+ SendableRegistry::GetInstance().SetName(this, "GearTooth",
+ source->GetChannel());
+}
+
+GearTooth::GearTooth(std::shared_ptr<DigitalSource> source,
+ bool directionSensitive)
+ : Counter(source) {
+ EnableDirectionSensing(directionSensitive);
+ SendableRegistry::GetInstance().SetName(this, "GearTooth",
+ source->GetChannel());
+}
+
+void GearTooth::EnableDirectionSensing(bool directionSensitive) {
+ if (directionSensitive) {
+ SetPulseLengthMode(kGearToothThreshold);
+ }
+}
+
+void GearTooth::InitSendable(SendableBuilder& builder) {
+ Counter::InitSendable(builder);
+ builder.SetSmartDashboardType("Gear Tooth");
+}
diff --git a/wpilibc/src/main/native/cpp/GenericHID.cpp b/wpilibc/src/main/native/cpp/GenericHID.cpp
new file mode 100644
index 0000000..a7dcfc0
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/GenericHID.cpp
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/GenericHID.h"
+
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/DriverStation.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+GenericHID::GenericHID(int port) : m_ds(&DriverStation::GetInstance()) {
+ if (port >= DriverStation::kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ }
+ m_port = port;
+}
+
+bool GenericHID::GetRawButton(int button) const {
+ return m_ds->GetStickButton(m_port, button);
+}
+
+bool GenericHID::GetRawButtonPressed(int button) {
+ return m_ds->GetStickButtonPressed(m_port, button);
+}
+
+bool GenericHID::GetRawButtonReleased(int button) {
+ return m_ds->GetStickButtonReleased(m_port, button);
+}
+
+double GenericHID::GetRawAxis(int axis) const {
+ return m_ds->GetStickAxis(m_port, axis);
+}
+
+int GenericHID::GetPOV(int pov) const { return m_ds->GetStickPOV(m_port, pov); }
+
+int GenericHID::GetAxisCount() const { return m_ds->GetStickAxisCount(m_port); }
+
+int GenericHID::GetPOVCount() const { return m_ds->GetStickPOVCount(m_port); }
+
+int GenericHID::GetButtonCount() const {
+ return m_ds->GetStickButtonCount(m_port);
+}
+
+GenericHID::HIDType GenericHID::GetType() const {
+ return static_cast<HIDType>(m_ds->GetJoystickType(m_port));
+}
+
+std::string GenericHID::GetName() const {
+ return m_ds->GetJoystickName(m_port);
+}
+
+int GenericHID::GetAxisType(int axis) const {
+ return m_ds->GetJoystickAxisType(m_port, axis);
+}
+
+int GenericHID::GetPort() const { return m_port; }
+
+void GenericHID::SetOutput(int outputNumber, bool value) {
+ m_outputs =
+ (m_outputs & ~(1 << (outputNumber - 1))) | (value << (outputNumber - 1));
+
+ HAL_SetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
+
+void GenericHID::SetOutputs(int value) {
+ m_outputs = value;
+ HAL_SetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
+
+void GenericHID::SetRumble(RumbleType type, double value) {
+ if (value < 0)
+ value = 0;
+ else if (value > 1)
+ value = 1;
+ if (type == kLeftRumble) {
+ m_leftRumble = value * 65535;
+ } else {
+ m_rightRumble = value * 65535;
+ }
+ HAL_SetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
diff --git a/wpilibc/src/main/native/cpp/GyroBase.cpp b/wpilibc/src/main/native/cpp/GyroBase.cpp
new file mode 100644
index 0000000..ec67675
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/GyroBase.cpp
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/GyroBase.h"
+
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+double GyroBase::PIDGet() {
+ switch (GetPIDSourceType()) {
+ case PIDSourceType::kRate:
+ return GetRate();
+ case PIDSourceType::kDisplacement:
+ return GetAngle();
+ default:
+ return 0;
+ }
+}
+
+void GyroBase::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Gyro");
+ builder.AddDoubleProperty("Value", [=]() { return GetAngle(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/I2C.cpp b/wpilibc/src/main/native/cpp/I2C.cpp
new file mode 100644
index 0000000..21d92f2
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/I2C.cpp
@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/I2C.h"
+
+#include <utility>
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/I2C.h>
+
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+I2C::I2C(Port port, int deviceAddress)
+ : m_port(static_cast<HAL_I2CPort>(port)), m_deviceAddress(deviceAddress) {
+ int32_t status = 0;
+ HAL_InitializeI2C(m_port, &status);
+ // wpi_setHALError(status);
+
+ HAL_Report(HALUsageReporting::kResourceType_I2C, deviceAddress);
+}
+
+I2C::~I2C() { HAL_CloseI2C(m_port); }
+
+bool I2C::Transaction(uint8_t* dataToSend, int sendSize, uint8_t* dataReceived,
+ int receiveSize) {
+ int32_t status = 0;
+ status = HAL_TransactionI2C(m_port, m_deviceAddress, dataToSend, sendSize,
+ dataReceived, receiveSize);
+ // wpi_setHALError(status);
+ return status < 0;
+}
+
+bool I2C::AddressOnly() { return Transaction(nullptr, 0, nullptr, 0); }
+
+bool I2C::Write(int registerAddress, uint8_t data) {
+ uint8_t buffer[2];
+ buffer[0] = registerAddress;
+ buffer[1] = data;
+ int32_t status = 0;
+ status = HAL_WriteI2C(m_port, m_deviceAddress, buffer, sizeof(buffer));
+ return status < 0;
+}
+
+bool I2C::WriteBulk(uint8_t* data, int count) {
+ int32_t status = 0;
+ status = HAL_WriteI2C(m_port, m_deviceAddress, data, count);
+ return status < 0;
+}
+
+bool I2C::Read(int registerAddress, int count, uint8_t* buffer) {
+ if (count < 1) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "count");
+ return true;
+ }
+ if (buffer == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "buffer");
+ return true;
+ }
+ uint8_t regAddr = registerAddress;
+ return Transaction(®Addr, sizeof(regAddr), buffer, count);
+}
+
+bool I2C::ReadOnly(int count, uint8_t* buffer) {
+ if (count < 1) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "count");
+ return true;
+ }
+ if (buffer == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "buffer");
+ return true;
+ }
+ return HAL_ReadI2C(m_port, m_deviceAddress, buffer, count) < 0;
+}
+
+bool I2C::VerifySensor(int registerAddress, int count,
+ const uint8_t* expected) {
+ // TODO: Make use of all 7 read bytes
+ uint8_t deviceData[4];
+ for (int i = 0, curRegisterAddress = registerAddress; i < count;
+ i += 4, curRegisterAddress += 4) {
+ int toRead = count - i < 4 ? count - i : 4;
+ // Read the chunk of data. Return false if the sensor does not respond.
+ if (Read(curRegisterAddress, toRead, deviceData)) return false;
+
+ for (int j = 0; j < toRead; j++) {
+ if (deviceData[j] != expected[i + j]) return false;
+ }
+ }
+ return true;
+}
diff --git a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
new file mode 100644
index 0000000..202c61d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
@@ -0,0 +1,176 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/InterruptableSensorBase.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+InterruptableSensorBase::~InterruptableSensorBase() {
+ if (m_interrupt == HAL_kInvalidHandle) return;
+ int32_t status = 0;
+ HAL_CleanInterrupts(m_interrupt, &status);
+ // Ignore status, as an invalid handle just needs to be ignored.
+}
+
+void InterruptableSensorBase::RequestInterrupts(
+ HAL_InterruptHandlerFunction handler, void* param) {
+ if (StatusIsFatal()) return;
+
+ wpi_assert(m_interrupt == HAL_kInvalidHandle);
+ AllocateInterrupts(false);
+ if (StatusIsFatal()) return; // if allocate failed, out of interrupts
+
+ int32_t status = 0;
+ HAL_RequestInterrupts(
+ m_interrupt, GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(GetAnalogTriggerTypeForRouting()),
+ &status);
+ SetUpSourceEdge(true, false);
+ HAL_AttachInterruptHandler(m_interrupt, handler, param, &status);
+ wpi_setHALError(status);
+}
+
+void InterruptableSensorBase::RequestInterrupts(InterruptEventHandler handler) {
+ if (StatusIsFatal()) return;
+
+ wpi_assert(m_interrupt == HAL_kInvalidHandle);
+ AllocateInterrupts(false);
+ if (StatusIsFatal()) return; // if allocate failed, out of interrupts
+
+ m_interruptHandler =
+ std::make_unique<InterruptEventHandler>(std::move(handler));
+
+ int32_t status = 0;
+ HAL_RequestInterrupts(
+ m_interrupt, GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(GetAnalogTriggerTypeForRouting()),
+ &status);
+ SetUpSourceEdge(true, false);
+ HAL_AttachInterruptHandler(
+ m_interrupt,
+ [](uint32_t mask, void* param) {
+ auto self = reinterpret_cast<InterruptEventHandler*>(param);
+ // Rising edge result is the interrupt bit set in the byte 0xFF
+ // Falling edge result is the interrupt bit set in the byte 0xFF00
+ // Set any bit set to be true for that edge, and AND the 2 results
+ // together to match the existing enum for all interrupts
+ int32_t rising = (mask & 0xFF) ? 0x1 : 0x0;
+ int32_t falling = ((mask & 0xFF00) ? 0x0100 : 0x0);
+ WaitResult res = static_cast<WaitResult>(falling | rising);
+ (*self)(res);
+ },
+ m_interruptHandler.get(), &status);
+ wpi_setHALError(status);
+}
+
+void InterruptableSensorBase::RequestInterrupts() {
+ if (StatusIsFatal()) return;
+
+ wpi_assert(m_interrupt == HAL_kInvalidHandle);
+ AllocateInterrupts(true);
+ if (StatusIsFatal()) return; // if allocate failed, out of interrupts
+
+ int32_t status = 0;
+ HAL_RequestInterrupts(
+ m_interrupt, GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(GetAnalogTriggerTypeForRouting()),
+ &status);
+ wpi_setHALError(status);
+ SetUpSourceEdge(true, false);
+}
+
+void InterruptableSensorBase::CancelInterrupts() {
+ if (StatusIsFatal()) return;
+ wpi_assert(m_interrupt != HAL_kInvalidHandle);
+ int32_t status = 0;
+ HAL_CleanInterrupts(m_interrupt, &status);
+ // Ignore status, as an invalid handle just needs to be ignored.
+ m_interrupt = HAL_kInvalidHandle;
+ m_interruptHandler = nullptr;
+}
+
+InterruptableSensorBase::WaitResult InterruptableSensorBase::WaitForInterrupt(
+ double timeout, bool ignorePrevious) {
+ if (StatusIsFatal()) return InterruptableSensorBase::kTimeout;
+ wpi_assert(m_interrupt != HAL_kInvalidHandle);
+ int32_t status = 0;
+ int result;
+
+ result = HAL_WaitForInterrupt(m_interrupt, timeout, ignorePrevious, &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
+ // Set any bit set to be true for that edge, and AND the 2 results
+ // together to match the existing enum for all interrupts
+ int32_t rising = (result & 0xFF) ? 0x1 : 0x0;
+ int32_t falling = ((result & 0xFF00) ? 0x0100 : 0x0);
+ return static_cast<WaitResult>(falling | rising);
+}
+
+void InterruptableSensorBase::EnableInterrupts() {
+ if (StatusIsFatal()) return;
+ wpi_assert(m_interrupt != HAL_kInvalidHandle);
+ int32_t status = 0;
+ HAL_EnableInterrupts(m_interrupt, &status);
+ wpi_setHALError(status);
+}
+
+void InterruptableSensorBase::DisableInterrupts() {
+ if (StatusIsFatal()) return;
+ wpi_assert(m_interrupt != HAL_kInvalidHandle);
+ int32_t status = 0;
+ HAL_DisableInterrupts(m_interrupt, &status);
+ wpi_setHALError(status);
+}
+
+double InterruptableSensorBase::ReadRisingTimestamp() {
+ if (StatusIsFatal()) return 0.0;
+ wpi_assert(m_interrupt != HAL_kInvalidHandle);
+ int32_t status = 0;
+ int64_t timestamp = HAL_ReadInterruptRisingTimestamp(m_interrupt, &status);
+ wpi_setHALError(status);
+ return timestamp * 1e-6;
+}
+
+double InterruptableSensorBase::ReadFallingTimestamp() {
+ if (StatusIsFatal()) return 0.0;
+ wpi_assert(m_interrupt != HAL_kInvalidHandle);
+ int32_t status = 0;
+ int64_t timestamp = HAL_ReadInterruptFallingTimestamp(m_interrupt, &status);
+ wpi_setHALError(status);
+ return timestamp * 1e-6;
+}
+
+void InterruptableSensorBase::SetUpSourceEdge(bool risingEdge,
+ bool fallingEdge) {
+ if (StatusIsFatal()) return;
+ if (m_interrupt == HAL_kInvalidHandle) {
+ wpi_setWPIErrorWithContext(
+ NullParameter,
+ "You must call RequestInterrupts before SetUpSourceEdge");
+ return;
+ }
+ if (m_interrupt != HAL_kInvalidHandle) {
+ int32_t status = 0;
+ HAL_SetInterruptUpSourceEdge(m_interrupt, risingEdge, fallingEdge, &status);
+ wpi_setHALError(status);
+ }
+}
+
+void InterruptableSensorBase::AllocateInterrupts(bool watcher) {
+ wpi_assert(m_interrupt == HAL_kInvalidHandle);
+ // Expects the calling leaf class to allocate an interrupt index.
+ int32_t status = 0;
+ m_interrupt = HAL_InitializeInterrupts(watcher, &status);
+ wpi_setHALError(status);
+}
diff --git a/wpilibc/src/main/native/cpp/IterativeRobot.cpp b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
new file mode 100644
index 0000000..c8664a5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/IterativeRobot.h"
+
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/DriverStation.h"
+
+using namespace frc;
+
+static constexpr auto kPacketPeriod = 0.02_s;
+
+IterativeRobot::IterativeRobot() : IterativeRobotBase(kPacketPeriod) {
+ HAL_Report(HALUsageReporting::kResourceType_Framework,
+ HALUsageReporting::kFramework_Iterative);
+}
+
+void IterativeRobot::StartCompetition() {
+ RobotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL_ObserveUserProgramStarting();
+
+ // Loop forever, calling the appropriate mode-dependent function
+ 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
new file mode 100644
index 0000000..2997234
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -0,0 +1,180 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/IterativeRobotBase.h"
+
+#include <cstdio>
+
+#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/livewindow/LiveWindow.h"
+#include "frc/shuffleboard/Shuffleboard.h"
+#include "frc/smartdashboard/SmartDashboard.h"
+
+using namespace frc;
+
+IterativeRobotBase::IterativeRobotBase(double period)
+ : IterativeRobotBase(units::second_t(period)) {}
+
+IterativeRobotBase::IterativeRobotBase(units::second_t period)
+ : m_period(period),
+ m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {}
+
+void IterativeRobotBase::RobotInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+}
+
+void IterativeRobotBase::DisabledInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+}
+
+void IterativeRobotBase::AutonomousInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+}
+
+void IterativeRobotBase::TeleopInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+}
+
+void IterativeRobotBase::TestInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+}
+
+void IterativeRobotBase::RobotPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+ firstRun = false;
+ }
+}
+
+void IterativeRobotBase::DisabledPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+ firstRun = false;
+ }
+}
+
+void IterativeRobotBase::AutonomousPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+ firstRun = false;
+ }
+}
+
+void IterativeRobotBase::TeleopPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+ firstRun = false;
+ }
+}
+
+void IterativeRobotBase::TestPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+ firstRun = false;
+ }
+}
+
+void IterativeRobotBase::LoopFunc() {
+ m_watchdog.Reset();
+
+ // Call the appropriate function depending upon the current robot mode
+ if (IsDisabled()) {
+ // Call DisabledInit() if we are now just entering disabled mode from
+ // either a different mode or from power-on.
+ if (m_lastMode != Mode::kDisabled) {
+ LiveWindow::GetInstance()->SetEnabled(false);
+ Shuffleboard::DisableActuatorWidgets();
+ DisabledInit();
+ m_watchdog.AddEpoch("DisabledInit()");
+ m_lastMode = Mode::kDisabled;
+ }
+
+ HAL_ObserveUserProgramDisabled();
+ DisabledPeriodic();
+ m_watchdog.AddEpoch("DisabledPeriodic()");
+ } else if (IsAutonomous()) {
+ // Call AutonomousInit() if we are now just entering autonomous mode from
+ // either a different mode or from power-on.
+ if (m_lastMode != Mode::kAutonomous) {
+ LiveWindow::GetInstance()->SetEnabled(false);
+ Shuffleboard::DisableActuatorWidgets();
+ AutonomousInit();
+ m_watchdog.AddEpoch("AutonomousInit()");
+ m_lastMode = Mode::kAutonomous;
+ }
+
+ HAL_ObserveUserProgramAutonomous();
+ AutonomousPeriodic();
+ m_watchdog.AddEpoch("AutonomousPeriodic()");
+ } else if (IsOperatorControl()) {
+ // Call TeleopInit() if we are now just entering teleop mode from
+ // either a different mode or from power-on.
+ if (m_lastMode != Mode::kTeleop) {
+ LiveWindow::GetInstance()->SetEnabled(false);
+ Shuffleboard::DisableActuatorWidgets();
+ TeleopInit();
+ m_watchdog.AddEpoch("TeleopInit()");
+ m_lastMode = Mode::kTeleop;
+ }
+
+ HAL_ObserveUserProgramTeleop();
+ TeleopPeriodic();
+ m_watchdog.AddEpoch("TeleopPeriodic()");
+ } else {
+ // Call TestInit() if we are now just entering test mode from
+ // either a different mode or from power-on.
+ if (m_lastMode != Mode::kTest) {
+ LiveWindow::GetInstance()->SetEnabled(true);
+ Shuffleboard::EnableActuatorWidgets();
+ TestInit();
+ m_watchdog.AddEpoch("TestInit()");
+ m_lastMode = Mode::kTest;
+ }
+
+ HAL_ObserveUserProgramTest();
+ TestPeriodic();
+ m_watchdog.AddEpoch("TestPeriodic()");
+ }
+
+ RobotPeriodic();
+ m_watchdog.AddEpoch("RobotPeriodic()");
+
+ SmartDashboard::UpdateValues();
+ m_watchdog.AddEpoch("SmartDashboard::UpdateValues()");
+ LiveWindow::GetInstance()->UpdateValues();
+ m_watchdog.AddEpoch("LiveWindow::UpdateValues()");
+ Shuffleboard::Update();
+ m_watchdog.AddEpoch("Shuffleboard::Update()");
+ m_watchdog.Disable();
+
+ // Warn on loop time overruns
+ if (m_watchdog.IsExpired()) {
+ m_watchdog.PrintEpochs();
+ }
+}
+
+void IterativeRobotBase::PrintLoopOverrunMessage() {
+ wpi::SmallString<128> str;
+ wpi::raw_svector_ostream buf(str);
+
+ buf << "Loop time of " << wpi::format("%.6f", m_period.to<double>())
+ << "s overrun\n";
+
+ DriverStation::ReportWarning(str);
+}
diff --git a/wpilibc/src/main/native/cpp/Jaguar.cpp b/wpilibc/src/main/native/cpp/Jaguar.cpp
new file mode 100644
index 0000000..c87e4d1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Jaguar.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Jaguar.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Jaguar::Jaguar(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.31, 1.55, 1.507, 1.454, 0.697);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ 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
new file mode 100644
index 0000000..8d464cf
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Joystick.cpp
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Joystick.h"
+
+#include <cmath>
+
+#include <hal/FRCUsageReporting.h>
+#include <wpi/math>
+
+#include "frc/DriverStation.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+Joystick::Joystick(int port) : GenericHID(port) {
+ m_axes[Axis::kX] = kDefaultXChannel;
+ m_axes[Axis::kY] = kDefaultYChannel;
+ m_axes[Axis::kZ] = kDefaultZChannel;
+ m_axes[Axis::kTwist] = kDefaultTwistChannel;
+ m_axes[Axis::kThrottle] = kDefaultThrottleChannel;
+
+ HAL_Report(HALUsageReporting::kResourceType_Joystick, port + 1);
+}
+
+void Joystick::SetXChannel(int channel) { m_axes[Axis::kX] = channel; }
+
+void Joystick::SetYChannel(int channel) { m_axes[Axis::kY] = channel; }
+
+void Joystick::SetZChannel(int channel) { m_axes[Axis::kZ] = channel; }
+
+void Joystick::SetTwistChannel(int channel) { m_axes[Axis::kTwist] = channel; }
+
+void Joystick::SetThrottleChannel(int channel) {
+ m_axes[Axis::kThrottle] = channel;
+}
+
+int Joystick::GetXChannel() const { return m_axes[Axis::kX]; }
+
+int Joystick::GetYChannel() const { return m_axes[Axis::kY]; }
+
+int Joystick::GetZChannel() const { return m_axes[Axis::kZ]; }
+
+int Joystick::GetTwistChannel() const { return m_axes[Axis::kTwist]; }
+
+int Joystick::GetThrottleChannel() const { return m_axes[Axis::kThrottle]; }
+
+double Joystick::GetX(JoystickHand hand) const {
+ return GetRawAxis(m_axes[Axis::kX]);
+}
+
+double Joystick::GetY(JoystickHand hand) const {
+ return GetRawAxis(m_axes[Axis::kY]);
+}
+
+double Joystick::GetZ() const { return GetRawAxis(m_axes[Axis::kZ]); }
+
+double Joystick::GetTwist() const { return GetRawAxis(m_axes[Axis::kTwist]); }
+
+double Joystick::GetThrottle() const {
+ return GetRawAxis(m_axes[Axis::kThrottle]);
+}
+
+bool Joystick::GetTrigger() const { return GetRawButton(Button::kTrigger); }
+
+bool Joystick::GetTriggerPressed() {
+ return GetRawButtonPressed(Button::kTrigger);
+}
+
+bool Joystick::GetTriggerReleased() {
+ return GetRawButtonReleased(Button::kTrigger);
+}
+
+bool Joystick::GetTop() const { return GetRawButton(Button::kTop); }
+
+bool Joystick::GetTopPressed() { return GetRawButtonPressed(Button::kTop); }
+
+bool Joystick::GetTopReleased() { return GetRawButtonReleased(Button::kTop); }
+
+double Joystick::GetMagnitude() const {
+ return std::sqrt(std::pow(GetX(), 2) + std::pow(GetY(), 2));
+}
+
+double Joystick::GetDirectionRadians() const {
+ return std::atan2(GetX(), -GetY());
+}
+
+double Joystick::GetDirectionDegrees() const {
+ return (180 / wpi::math::pi) * GetDirectionRadians();
+}
diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp
new file mode 100644
index 0000000..1806e86
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp
@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/MotorSafety.h"
+
+#include <algorithm>
+#include <utility>
+
+#include <wpi/SmallPtrSet.h>
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/DriverStation.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+static wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
+static wpi::mutex listMutex;
+
+MotorSafety::MotorSafety() {
+ std::scoped_lock lock(listMutex);
+ instanceList.insert(this);
+}
+
+MotorSafety::~MotorSafety() {
+ std::scoped_lock lock(listMutex);
+ instanceList.erase(this);
+}
+
+MotorSafety::MotorSafety(MotorSafety&& rhs)
+ : ErrorBase(std::move(rhs)),
+ m_expiration(std::move(rhs.m_expiration)),
+ m_enabled(std::move(rhs.m_enabled)),
+ m_stopTime(std::move(rhs.m_stopTime)) {}
+
+MotorSafety& MotorSafety::operator=(MotorSafety&& rhs) {
+ std::scoped_lock lock(m_thisMutex, rhs.m_thisMutex);
+
+ ErrorBase::operator=(std::move(rhs));
+
+ m_expiration = std::move(rhs.m_expiration);
+ m_enabled = std::move(rhs.m_enabled);
+ m_stopTime = std::move(rhs.m_stopTime);
+
+ return *this;
+}
+
+void MotorSafety::Feed() {
+ std::scoped_lock lock(m_thisMutex);
+ m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
+}
+
+void MotorSafety::SetExpiration(double expirationTime) {
+ std::scoped_lock lock(m_thisMutex);
+ m_expiration = expirationTime;
+}
+
+double MotorSafety::GetExpiration() const {
+ std::scoped_lock lock(m_thisMutex);
+ return m_expiration;
+}
+
+bool MotorSafety::IsAlive() const {
+ std::scoped_lock lock(m_thisMutex);
+ return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
+}
+
+void MotorSafety::SetSafetyEnabled(bool enabled) {
+ std::scoped_lock lock(m_thisMutex);
+ m_enabled = enabled;
+}
+
+bool MotorSafety::IsSafetyEnabled() const {
+ std::scoped_lock lock(m_thisMutex);
+ return m_enabled;
+}
+
+void MotorSafety::Check() {
+ bool enabled;
+ double stopTime;
+
+ {
+ std::scoped_lock lock(m_thisMutex);
+ enabled = m_enabled;
+ stopTime = m_stopTime;
+ }
+
+ DriverStation& ds = DriverStation::GetInstance();
+ if (!enabled || ds.IsDisabled() || ds.IsTest()) {
+ return;
+ }
+
+ if (stopTime < Timer::GetFPGATimestamp()) {
+ wpi::SmallString<128> buf;
+ wpi::raw_svector_ostream desc(buf);
+ GetDescription(desc);
+ desc << "... Output not updated often enough.";
+ wpi_setWPIErrorWithContext(Timeout, desc.str());
+ StopMotor();
+ }
+}
+
+void MotorSafety::CheckMotors() {
+ std::scoped_lock lock(listMutex);
+ for (auto elem : instanceList) {
+ elem->Check();
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/NidecBrushless.cpp b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
new file mode 100644
index 0000000..5bce36e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/NidecBrushless.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
+ : m_dio(dioChannel), m_pwm(pwmChannel) {
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, &m_dio);
+ registry.AddChild(this, &m_pwm);
+ SetExpiration(0.0);
+ SetSafetyEnabled(false);
+
+ // the dio controls the output (in PWM mode)
+ m_dio.SetPWMRate(15625);
+ m_dio.EnablePWM(0.5);
+
+ HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel + 1);
+ registry.AddLW(this, "Nidec Brushless", pwmChannel);
+}
+
+void NidecBrushless::Set(double speed) {
+ if (!m_disabled) {
+ m_speed = speed;
+ m_dio.UpdateDutyCycle(0.5 + 0.5 * (m_isInverted ? -speed : speed));
+ m_pwm.SetRaw(0xffff);
+ }
+ Feed();
+}
+
+double NidecBrushless::Get() const { return m_speed; }
+
+void NidecBrushless::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+bool NidecBrushless::GetInverted() const { return m_isInverted; }
+
+void NidecBrushless::Disable() {
+ m_disabled = true;
+ m_dio.UpdateDutyCycle(0.5);
+ m_pwm.SetDisabled();
+}
+
+void NidecBrushless::Enable() { m_disabled = false; }
+
+void NidecBrushless::PIDWrite(double output) { Set(output); }
+
+void NidecBrushless::StopMotor() {
+ m_dio.UpdateDutyCycle(0.5);
+ m_pwm.SetDisabled();
+}
+
+void NidecBrushless::GetDescription(wpi::raw_ostream& desc) const {
+ desc << "Nidec " << GetChannel();
+}
+
+int NidecBrushless::GetChannel() const { return m_pwm.GetChannel(); }
+
+void NidecBrushless::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Nidec Brushless");
+ builder.SetActuator(true);
+ builder.SetSafeState([=]() { StopMotor(); });
+ builder.AddDoubleProperty("Value", [=]() { return Get(); },
+ [=](double value) { Set(value); });
+}
diff --git a/wpilibc/src/main/native/cpp/Notifier.cpp b/wpilibc/src/main/native/cpp/Notifier.cpp
new file mode 100644
index 0000000..a7fa038
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Notifier.cpp
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Notifier.h"
+
+#include <utility>
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/Notifier.h>
+#include <wpi/SmallString.h>
+
+#include "frc/Timer.h"
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+Notifier::Notifier(std::function<void()> handler) {
+ if (handler == nullptr)
+ wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+ m_handler = handler;
+ int32_t status = 0;
+ m_notifier = HAL_InitializeNotifier(&status);
+ wpi_setHALError(status);
+
+ m_thread = std::thread([=] {
+ for (;;) {
+ int32_t status = 0;
+ HAL_NotifierHandle notifier = m_notifier.load();
+ if (notifier == 0) break;
+ uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
+ if (curTime == 0 || status != 0) break;
+
+ std::function<void()> handler;
+ {
+ std::scoped_lock lock(m_processMutex);
+ handler = m_handler;
+ if (m_periodic) {
+ m_expirationTime += m_period;
+ UpdateAlarm();
+ } else {
+ // need to update the alarm to cause it to wait again
+ UpdateAlarm(UINT64_MAX);
+ }
+ }
+
+ // call callback
+ if (handler) handler();
+ }
+ });
+}
+
+Notifier::~Notifier() {
+ int32_t status = 0;
+ // atomically set handle to 0, then clean
+ HAL_NotifierHandle handle = m_notifier.exchange(0);
+ HAL_StopNotifier(handle, &status);
+ wpi_setHALError(status);
+
+ // Join the thread to ensure the handler has exited.
+ if (m_thread.joinable()) m_thread.join();
+
+ HAL_CleanNotifier(handle, &status);
+}
+
+Notifier::Notifier(Notifier&& rhs)
+ : ErrorBase(std::move(rhs)),
+ m_thread(std::move(rhs.m_thread)),
+ m_notifier(rhs.m_notifier.load()),
+ m_handler(std::move(rhs.m_handler)),
+ m_expirationTime(std::move(rhs.m_expirationTime)),
+ m_period(std::move(rhs.m_period)),
+ m_periodic(std::move(rhs.m_periodic)) {
+ rhs.m_notifier = HAL_kInvalidHandle;
+}
+
+Notifier& Notifier::operator=(Notifier&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+
+ m_thread = std::move(rhs.m_thread);
+ m_notifier = rhs.m_notifier.load();
+ rhs.m_notifier = HAL_kInvalidHandle;
+ m_handler = std::move(rhs.m_handler);
+ m_expirationTime = std::move(rhs.m_expirationTime);
+ m_period = std::move(rhs.m_period);
+ m_periodic = std::move(rhs.m_periodic);
+
+ 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;
+}
+
+void Notifier::StartSingle(double delay) {
+ StartSingle(units::second_t(delay));
+}
+
+void Notifier::StartSingle(units::second_t delay) {
+ std::scoped_lock lock(m_processMutex);
+ m_periodic = false;
+ m_period = delay.to<double>();
+ m_expirationTime = Timer::GetFPGATimestamp() + m_period;
+ UpdateAlarm();
+}
+
+void Notifier::StartPeriodic(double period) {
+ StartPeriodic(units::second_t(period));
+}
+
+void Notifier::StartPeriodic(units::second_t period) {
+ std::scoped_lock lock(m_processMutex);
+ m_periodic = true;
+ m_period = period.to<double>();
+ m_expirationTime = Timer::GetFPGATimestamp() + m_period;
+ UpdateAlarm();
+}
+
+void Notifier::Stop() {
+ int32_t status = 0;
+ HAL_CancelNotifierAlarm(m_notifier, &status);
+ wpi_setHALError(status);
+}
+
+void Notifier::UpdateAlarm(uint64_t triggerTime) {
+ int32_t status = 0;
+ // Return if we are being destructed, or were not created successfully
+ auto notifier = m_notifier.load();
+ if (notifier == 0) return;
+ HAL_UpdateNotifierAlarm(notifier, triggerTime, &status);
+ wpi_setHALError(status);
+}
+
+void Notifier::UpdateAlarm() {
+ UpdateAlarm(static_cast<uint64_t>(m_expirationTime * 1e6));
+}
diff --git a/wpilibc/src/main/native/cpp/PIDBase.cpp b/wpilibc/src/main/native/cpp/PIDBase.cpp
new file mode 100644
index 0000000..c8f6fed
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PIDBase.cpp
@@ -0,0 +1,353 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PIDBase.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/PIDOutput.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+template <class T>
+constexpr const T& clamp(const T& value, const T& low, const T& high) {
+ return std::max(low, std::min(value, high));
+}
+
+PIDBase::PIDBase(double Kp, double Ki, double Kd, PIDSource& source,
+ PIDOutput& output)
+ : PIDBase(Kp, Ki, Kd, 0.0, source, output) {}
+
+PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source,
+ PIDOutput& output) {
+ m_P = Kp;
+ m_I = Ki;
+ m_D = Kd;
+ m_F = Kf;
+
+ m_pidInput = &source;
+ m_filter = LinearFilter<double>::MovingAverage(1);
+
+ m_pidOutput = &output;
+
+ m_setpointTimer.Start();
+
+ static int instances = 0;
+ instances++;
+ HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
+ SendableRegistry::GetInstance().Add(this, "PIDController", instances);
+}
+
+double PIDBase::Get() const {
+ std::scoped_lock lock(m_thisMutex);
+ return m_result;
+}
+
+void PIDBase::SetContinuous(bool continuous) {
+ std::scoped_lock lock(m_thisMutex);
+ m_continuous = continuous;
+}
+
+void PIDBase::SetInputRange(double minimumInput, double maximumInput) {
+ {
+ std::scoped_lock lock(m_thisMutex);
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+ m_inputRange = maximumInput - minimumInput;
+ }
+
+ SetSetpoint(m_setpoint);
+}
+
+void PIDBase::SetOutputRange(double minimumOutput, double maximumOutput) {
+ std::scoped_lock lock(m_thisMutex);
+ m_minimumOutput = minimumOutput;
+ m_maximumOutput = maximumOutput;
+}
+
+void PIDBase::SetPID(double p, double i, double d) {
+ {
+ std::scoped_lock lock(m_thisMutex);
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ }
+}
+
+void PIDBase::SetPID(double p, double i, double d, double f) {
+ std::scoped_lock lock(m_thisMutex);
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ m_F = f;
+}
+
+void PIDBase::SetP(double p) {
+ std::scoped_lock lock(m_thisMutex);
+ m_P = p;
+}
+
+void PIDBase::SetI(double i) {
+ std::scoped_lock lock(m_thisMutex);
+ m_I = i;
+}
+
+void PIDBase::SetD(double d) {
+ std::scoped_lock lock(m_thisMutex);
+ m_D = d;
+}
+
+void PIDBase::SetF(double f) {
+ std::scoped_lock lock(m_thisMutex);
+ m_F = f;
+}
+
+double PIDBase::GetP() const {
+ std::scoped_lock lock(m_thisMutex);
+ return m_P;
+}
+
+double PIDBase::GetI() const {
+ std::scoped_lock lock(m_thisMutex);
+ return m_I;
+}
+
+double PIDBase::GetD() const {
+ std::scoped_lock lock(m_thisMutex);
+ return m_D;
+}
+
+double PIDBase::GetF() const {
+ std::scoped_lock lock(m_thisMutex);
+ return m_F;
+}
+
+void PIDBase::SetSetpoint(double setpoint) {
+ {
+ std::scoped_lock lock(m_thisMutex);
+
+ if (m_maximumInput > m_minimumInput) {
+ if (setpoint > m_maximumInput)
+ m_setpoint = m_maximumInput;
+ else if (setpoint < m_minimumInput)
+ m_setpoint = m_minimumInput;
+ else
+ m_setpoint = setpoint;
+ } else {
+ m_setpoint = setpoint;
+ }
+ }
+}
+
+double PIDBase::GetSetpoint() const {
+ std::scoped_lock lock(m_thisMutex);
+ return m_setpoint;
+}
+
+double PIDBase::GetDeltaSetpoint() const {
+ std::scoped_lock lock(m_thisMutex);
+ return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
+}
+
+double PIDBase::GetError() const {
+ double setpoint = GetSetpoint();
+ {
+ std::scoped_lock lock(m_thisMutex);
+ return GetContinuousError(setpoint - m_pidInput->PIDGet());
+ }
+}
+
+double PIDBase::GetAvgError() const { return GetError(); }
+
+void PIDBase::SetPIDSourceType(PIDSourceType pidSource) {
+ m_pidInput->SetPIDSourceType(pidSource);
+}
+
+PIDSourceType PIDBase::GetPIDSourceType() const {
+ return m_pidInput->GetPIDSourceType();
+}
+
+void PIDBase::SetTolerance(double percent) {
+ std::scoped_lock lock(m_thisMutex);
+ m_toleranceType = kPercentTolerance;
+ m_tolerance = percent;
+}
+
+void PIDBase::SetAbsoluteTolerance(double absTolerance) {
+ std::scoped_lock lock(m_thisMutex);
+ m_toleranceType = kAbsoluteTolerance;
+ m_tolerance = absTolerance;
+}
+
+void PIDBase::SetPercentTolerance(double percent) {
+ std::scoped_lock lock(m_thisMutex);
+ m_toleranceType = kPercentTolerance;
+ m_tolerance = percent;
+}
+
+void PIDBase::SetToleranceBuffer(int bufLength) {
+ std::scoped_lock lock(m_thisMutex);
+ m_filter = LinearFilter<double>::MovingAverage(bufLength);
+}
+
+bool PIDBase::OnTarget() const {
+ double error = GetError();
+
+ std::scoped_lock lock(m_thisMutex);
+ switch (m_toleranceType) {
+ case kPercentTolerance:
+ return std::fabs(error) < m_tolerance / 100 * m_inputRange;
+ break;
+ case kAbsoluteTolerance:
+ return std::fabs(error) < m_tolerance;
+ break;
+ case kNoTolerance:
+ // TODO: this case needs an error
+ return false;
+ }
+ return false;
+}
+
+void PIDBase::Reset() {
+ std::scoped_lock lock(m_thisMutex);
+ m_prevError = 0;
+ m_totalError = 0;
+ m_result = 0;
+}
+
+void PIDBase::PIDWrite(double output) { SetSetpoint(output); }
+
+void PIDBase::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("PIDController");
+ builder.SetSafeState([=]() { Reset(); });
+ builder.AddDoubleProperty("p", [=]() { return GetP(); },
+ [=](double value) { SetP(value); });
+ builder.AddDoubleProperty("i", [=]() { return GetI(); },
+ [=](double value) { SetI(value); });
+ builder.AddDoubleProperty("d", [=]() { return GetD(); },
+ [=](double value) { SetD(value); });
+ builder.AddDoubleProperty("f", [=]() { return GetF(); },
+ [=](double value) { SetF(value); });
+ builder.AddDoubleProperty("setpoint", [=]() { return GetSetpoint(); },
+ [=](double value) { SetSetpoint(value); });
+}
+
+void PIDBase::Calculate() {
+ if (m_pidInput == nullptr || m_pidOutput == nullptr) return;
+
+ bool enabled;
+ {
+ std::scoped_lock lock(m_thisMutex);
+ enabled = m_enabled;
+ }
+
+ if (enabled) {
+ double input;
+
+ // Storage for function inputs
+ PIDSourceType pidSourceType;
+ double P;
+ double I;
+ double D;
+ double feedForward = CalculateFeedForward();
+ double minimumOutput;
+ double maximumOutput;
+
+ // Storage for function input-outputs
+ double prevError;
+ double error;
+ double totalError;
+
+ {
+ std::scoped_lock lock(m_thisMutex);
+
+ input = m_filter.Calculate(m_pidInput->PIDGet());
+
+ pidSourceType = m_pidInput->GetPIDSourceType();
+ P = m_P;
+ I = m_I;
+ D = m_D;
+ minimumOutput = m_minimumOutput;
+ maximumOutput = m_maximumOutput;
+
+ prevError = m_prevError;
+ error = GetContinuousError(m_setpoint - input);
+ totalError = m_totalError;
+ }
+
+ // Storage for function outputs
+ double result;
+
+ if (pidSourceType == PIDSourceType::kRate) {
+ if (P != 0) {
+ totalError =
+ clamp(totalError + error, minimumOutput / P, maximumOutput / P);
+ }
+
+ result = D * error + P * totalError + feedForward;
+ } else {
+ if (I != 0) {
+ totalError =
+ clamp(totalError + error, minimumOutput / I, maximumOutput / I);
+ }
+
+ result =
+ P * error + I * totalError + D * (error - prevError) + feedForward;
+ }
+
+ result = clamp(result, minimumOutput, maximumOutput);
+
+ {
+ // Ensures m_enabled check and PIDWrite() call occur atomically
+ std::scoped_lock pidWriteLock(m_pidWriteMutex);
+ std::unique_lock mainLock(m_thisMutex);
+ if (m_enabled) {
+ // Don't block other PIDBase operations on PIDWrite()
+ mainLock.unlock();
+
+ m_pidOutput->PIDWrite(result);
+ }
+ }
+
+ std::scoped_lock lock(m_thisMutex);
+ m_prevError = m_error;
+ m_error = error;
+ m_totalError = totalError;
+ m_result = result;
+ }
+}
+
+double PIDBase::CalculateFeedForward() {
+ if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+ return m_F * GetSetpoint();
+ } else {
+ double temp = m_F * GetDeltaSetpoint();
+ m_prevSetpoint = m_setpoint;
+ m_setpointTimer.Reset();
+ return temp;
+ }
+}
+
+double PIDBase::GetContinuousError(double error) const {
+ if (m_continuous && m_inputRange != 0) {
+ error = std::fmod(error, m_inputRange);
+ if (std::fabs(error) > m_inputRange / 2) {
+ if (error > 0) {
+ return error - m_inputRange;
+ } else {
+ return error + m_inputRange;
+ }
+ }
+ }
+
+ return error;
+}
diff --git a/wpilibc/src/main/native/cpp/PIDController.cpp b/wpilibc/src/main/native/cpp/PIDController.cpp
new file mode 100644
index 0000000..0c86f55
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PIDController.cpp
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PIDController.h"
+
+#include "frc/Notifier.h"
+#include "frc/PIDOutput.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source,
+ PIDOutput* output, double period)
+ : PIDController(Kp, Ki, Kd, 0.0, *source, *output, period) {}
+
+PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
+ PIDSource* source, PIDOutput* output,
+ double period)
+ : PIDController(Kp, Ki, Kd, Kf, *source, *output, period) {}
+
+PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource& source,
+ PIDOutput& output, double period)
+ : PIDController(Kp, Ki, Kd, 0.0, source, output, period) {}
+
+PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
+ PIDSource& source, PIDOutput& output,
+ double period)
+ : PIDBase(Kp, Ki, Kd, Kf, source, output) {
+ m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
+ m_controlLoop->StartPeriodic(units::second_t(period));
+}
+
+PIDController::~PIDController() {
+ // Forcefully stopping the notifier so the callback can successfully run.
+ m_controlLoop->Stop();
+}
+
+void PIDController::Enable() {
+ {
+ std::scoped_lock lock(m_thisMutex);
+ m_enabled = true;
+ }
+}
+
+void PIDController::Disable() {
+ {
+ // Ensures m_enabled modification and PIDWrite() call occur atomically
+ std::scoped_lock pidWriteLock(m_pidWriteMutex);
+ {
+ std::scoped_lock mainLock(m_thisMutex);
+ m_enabled = false;
+ }
+
+ m_pidOutput->PIDWrite(0);
+ }
+}
+
+void PIDController::SetEnabled(bool enable) {
+ if (enable) {
+ Enable();
+ } else {
+ Disable();
+ }
+}
+
+bool PIDController::IsEnabled() const {
+ std::scoped_lock lock(m_thisMutex);
+ return m_enabled;
+}
+
+void PIDController::Reset() {
+ Disable();
+
+ PIDBase::Reset();
+}
+
+void PIDController::InitSendable(SendableBuilder& builder) {
+ PIDBase::InitSendable(builder);
+ builder.AddBooleanProperty("enabled", [=]() { return IsEnabled(); },
+ [=](bool value) { SetEnabled(value); });
+}
diff --git a/wpilibc/src/main/native/cpp/PIDSource.cpp b/wpilibc/src/main/native/cpp/PIDSource.cpp
new file mode 100644
index 0000000..28291dd
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PIDSource.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PIDSource.h"
+
+using namespace frc;
+
+void PIDSource::SetPIDSourceType(PIDSourceType pidSource) {
+ m_pidSource = pidSource;
+}
+
+PIDSourceType PIDSource::GetPIDSourceType() const { return m_pidSource; }
diff --git a/wpilibc/src/main/native/cpp/PWM.cpp b/wpilibc/src/main/native/cpp/PWM.cpp
new file mode 100644
index 0000000..dabcd2e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWM.cpp
@@ -0,0 +1,204 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWM.h"
+
+#include <utility>
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/PWM.h>
+#include <hal/Ports.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWM::PWM(int channel) {
+ if (!SensorUtil::CheckPWMChannel(channel)) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
+ "PWM Channel " + wpi::Twine(channel));
+ return;
+ }
+
+ int32_t status = 0;
+ m_handle = HAL_InitializePWMPort(HAL_GetPort(channel), &status);
+ if (status != 0) {
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPWMChannels(), channel);
+ m_channel = std::numeric_limits<int>::max();
+ m_handle = HAL_kInvalidHandle;
+ return;
+ }
+
+ m_channel = channel;
+
+ HAL_SetPWMDisabled(m_handle, &status);
+ wpi_setHALError(status);
+ status = 0;
+ HAL_SetPWMEliminateDeadband(m_handle, false, &status);
+ wpi_setHALError(status);
+
+ HAL_Report(HALUsageReporting::kResourceType_PWM, channel + 1);
+ SendableRegistry::GetInstance().AddLW(this, "PWM", channel);
+
+ SetSafetyEnabled(false);
+}
+
+PWM::~PWM() {
+ int32_t status = 0;
+
+ HAL_SetPWMDisabled(m_handle, &status);
+ wpi_setHALError(status);
+
+ HAL_FreePWMPort(m_handle, &status);
+ wpi_setHALError(status);
+}
+
+void PWM::StopMotor() { SetDisabled(); }
+
+void PWM::GetDescription(wpi::raw_ostream& desc) const {
+ desc << "PWM " << GetChannel();
+}
+
+void PWM::SetRaw(uint16_t value) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ HAL_SetPWMRaw(m_handle, value, &status);
+ wpi_setHALError(status);
+}
+
+uint16_t PWM::GetRaw() const {
+ if (StatusIsFatal()) return 0;
+
+ int32_t status = 0;
+ uint16_t value = HAL_GetPWMRaw(m_handle, &status);
+ wpi_setHALError(status);
+
+ return value;
+}
+
+void PWM::SetPosition(double pos) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetPWMPosition(m_handle, pos, &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_setHALError(status);
+ return position;
+}
+
+void PWM::SetSpeed(double speed) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetPWMSpeed(m_handle, speed, &status);
+ wpi_setHALError(status);
+
+ Feed();
+}
+
+double PWM::GetSpeed() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double speed = HAL_GetPWMSpeed(m_handle, &status);
+ wpi_setHALError(status);
+ return speed;
+}
+
+void PWM::SetDisabled() {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+
+ HAL_SetPWMDisabled(m_handle, &status);
+ wpi_setHALError(status);
+}
+
+void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+
+ switch (mult) {
+ case kPeriodMultiplier_4X:
+ HAL_SetPWMPeriodScale(m_handle, 3,
+ &status); // Squelch 3 out of 4 outputs
+ break;
+ case kPeriodMultiplier_2X:
+ HAL_SetPWMPeriodScale(m_handle, 1,
+ &status); // Squelch 1 out of 2 outputs
+ break;
+ case kPeriodMultiplier_1X:
+ HAL_SetPWMPeriodScale(m_handle, 0, &status); // Don't squelch any outputs
+ break;
+ default:
+ wpi_assert(false);
+ }
+
+ wpi_setHALError(status);
+}
+
+void PWM::SetZeroLatch() {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+
+ HAL_LatchPWMZero(m_handle, &status);
+ wpi_setHALError(status);
+}
+
+void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetPWMEliminateDeadband(m_handle, eliminateDeadband, &status);
+ wpi_setHALError(status);
+}
+
+void PWM::SetBounds(double max, double deadbandMax, double center,
+ double deadbandMin, double min) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetPWMConfig(m_handle, max, deadbandMax, center, deadbandMin, min,
+ &status);
+ wpi_setHALError(status);
+}
+
+void PWM::SetRawBounds(int max, int deadbandMax, int center, int deadbandMin,
+ int min) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min,
+ &status);
+ wpi_setHALError(status);
+}
+
+void PWM::GetRawBounds(int* max, int* deadbandMax, int* center,
+ int* deadbandMin, int* min) {
+ int32_t status = 0;
+ HAL_GetPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min,
+ &status);
+ wpi_setHALError(status);
+}
+
+int PWM::GetChannel() const { return m_channel; }
+
+void PWM::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("PWM");
+ builder.SetActuator(true);
+ builder.SetSafeState([=]() { SetDisabled(); });
+ builder.AddDoubleProperty("Value", [=]() { return GetRaw(); },
+ [=](double value) { SetRaw(value); });
+}
diff --git a/wpilibc/src/main/native/cpp/PWMSparkMax.cpp b/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
new file mode 100644
index 0000000..c5375f4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMSparkMax.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMSparkMax::PWMSparkMax(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_RevSparkMaxPWM, GetChannel() + 1);
+ SendableRegistry::GetInstance().SetName(this, "PWMSparkMax", GetChannel());
+}
diff --git a/wpilibc/src/main/native/cpp/PWMSpeedController.cpp b/wpilibc/src/main/native/cpp/PWMSpeedController.cpp
new file mode 100644
index 0000000..ea298de
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMSpeedController.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMSpeedController.h"
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+void PWMSpeedController::Set(double speed) {
+ SetSpeed(m_isInverted ? -speed : speed);
+}
+
+double PWMSpeedController::Get() const { return GetSpeed(); }
+
+void PWMSpeedController::SetInverted(bool isInverted) {
+ m_isInverted = isInverted;
+}
+
+bool PWMSpeedController::GetInverted() const { return m_isInverted; }
+
+void PWMSpeedController::Disable() { SetDisabled(); }
+
+void PWMSpeedController::StopMotor() { PWM::StopMotor(); }
+
+void PWMSpeedController::PIDWrite(double output) { Set(output); }
+
+PWMSpeedController::PWMSpeedController(int channel) : PWM(channel) {}
+
+void PWMSpeedController::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Speed Controller");
+ builder.SetActuator(true);
+ builder.SetSafeState([=]() { SetDisabled(); });
+ builder.AddDoubleProperty("Value", [=]() { return GetSpeed(); },
+ [=](double value) { SetSpeed(value); });
+}
diff --git a/wpilibc/src/main/native/cpp/PWMTalonFX.cpp b/wpilibc/src/main/native/cpp/PWMTalonFX.cpp
new file mode 100644
index 0000000..09b7163
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMTalonFX.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMTalonFX.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMTalonFX::PWMTalonFX(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_TalonFX, GetChannel() + 1);
+ SendableRegistry::GetInstance().SetName(this, "PWMTalonFX", GetChannel());
+}
diff --git a/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
new file mode 100644
index 0000000..1242be9
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMTalonSRX.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMTalonSRX::PWMTalonSRX(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel() + 1);
+ SendableRegistry::GetInstance().SetName(this, "PWMTalonSRX", GetChannel());
+}
diff --git a/wpilibc/src/main/native/cpp/PWMVenom.cpp b/wpilibc/src/main/native/cpp/PWMVenom.cpp
new file mode 100644
index 0000000..9fa14b7
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMVenom.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMVenom.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMVenom::PWMVenom(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_FusionVenom, GetChannel() + 1);
+ SendableRegistry::GetInstance().SetName(this, "PWMVenom", GetChannel());
+}
diff --git a/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp b/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
new file mode 100644
index 0000000..0d966ae
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMVictorSPX.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMVictorSPX::PWMVictorSPX(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ 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
new file mode 100644
index 0000000..7d7d916
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PowerDistributionPanel.h"
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/PDP.h>
+#include <hal/Ports.h>
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PowerDistributionPanel::PowerDistributionPanel() : PowerDistributionPanel(0) {}
+
+/**
+ * Initialize the PDP.
+ */
+PowerDistributionPanel::PowerDistributionPanel(int module) {
+ int32_t status = 0;
+ m_handle = HAL_InitializePDP(module, &status);
+ if (status != 0) {
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumPDPModules(), module);
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_PDP, module + 1);
+ SendableRegistry::GetInstance().AddLW(this, "PowerDistributionPanel", module);
+}
+
+double PowerDistributionPanel::GetVoltage() const {
+ int32_t status = 0;
+
+ double voltage = HAL_GetPDPVoltage(m_handle, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return voltage;
+}
+
+double PowerDistributionPanel::GetTemperature() const {
+ int32_t status = 0;
+
+ double temperature = HAL_GetPDPTemperature(m_handle, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return temperature;
+}
+
+double PowerDistributionPanel::GetCurrent(int channel) const {
+ int32_t status = 0;
+
+ if (!SensorUtil::CheckPDPChannel(channel)) {
+ wpi::SmallString<32> str;
+ wpi::raw_svector_ostream buf(str);
+ buf << "PDP Channel " << channel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ }
+
+ double current = HAL_GetPDPChannelCurrent(m_handle, channel, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return current;
+}
+
+double PowerDistributionPanel::GetTotalCurrent() const {
+ int32_t status = 0;
+
+ double current = HAL_GetPDPTotalCurrent(m_handle, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return current;
+}
+
+double PowerDistributionPanel::GetTotalPower() const {
+ int32_t status = 0;
+
+ double power = HAL_GetPDPTotalPower(m_handle, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return power;
+}
+
+double PowerDistributionPanel::GetTotalEnergy() const {
+ int32_t status = 0;
+
+ double energy = HAL_GetPDPTotalEnergy(m_handle, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return energy;
+}
+
+void PowerDistributionPanel::ResetTotalEnergy() {
+ int32_t status = 0;
+
+ HAL_ResetPDPTotalEnergy(m_handle, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+}
+
+void PowerDistributionPanel::ClearStickyFaults() {
+ int32_t status = 0;
+
+ HAL_ClearPDPStickyFaults(m_handle, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+}
+
+void PowerDistributionPanel::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("PowerDistributionPanel");
+ for (int i = 0; i < SensorUtil::kPDPChannels; ++i) {
+ builder.AddDoubleProperty("Chan" + wpi::Twine(i),
+ [=]() { return GetCurrent(i); }, nullptr);
+ }
+ builder.AddDoubleProperty("Voltage", [=]() { return GetVoltage(); }, nullptr);
+ builder.AddDoubleProperty("TotalCurrent", [=]() { return GetTotalCurrent(); },
+ nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/Preferences.cpp b/wpilibc/src/main/native/cpp/Preferences.cpp
new file mode 100644
index 0000000..6014775
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Preferences.cpp
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Preferences.h"
+
+#include <algorithm>
+
+#include <hal/FRCUsageReporting.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/StringRef.h>
+
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+// The Preferences table name
+static wpi::StringRef kTableName{"Preferences"};
+
+Preferences* Preferences::GetInstance() {
+ static Preferences instance;
+ return &instance;
+}
+
+std::vector<std::string> Preferences::GetKeys() { return m_table->GetKeys(); }
+
+std::string Preferences::GetString(wpi::StringRef key,
+ wpi::StringRef defaultValue) {
+ return m_table->GetString(key, defaultValue);
+}
+
+int Preferences::GetInt(wpi::StringRef key, int defaultValue) {
+ return static_cast<int>(m_table->GetNumber(key, defaultValue));
+}
+
+double Preferences::GetDouble(wpi::StringRef key, double defaultValue) {
+ return m_table->GetNumber(key, defaultValue);
+}
+
+float Preferences::GetFloat(wpi::StringRef key, float defaultValue) {
+ return m_table->GetNumber(key, defaultValue);
+}
+
+bool Preferences::GetBoolean(wpi::StringRef key, bool defaultValue) {
+ return m_table->GetBoolean(key, defaultValue);
+}
+
+int64_t Preferences::GetLong(wpi::StringRef key, int64_t defaultValue) {
+ return static_cast<int64_t>(m_table->GetNumber(key, defaultValue));
+}
+
+void Preferences::PutString(wpi::StringRef key, wpi::StringRef value) {
+ auto entry = m_table->GetEntry(key);
+ entry.SetString(value);
+ entry.SetPersistent();
+}
+
+void Preferences::PutInt(wpi::StringRef key, int value) {
+ auto entry = m_table->GetEntry(key);
+ entry.SetDouble(value);
+ entry.SetPersistent();
+}
+
+void Preferences::PutDouble(wpi::StringRef key, double value) {
+ auto entry = m_table->GetEntry(key);
+ entry.SetDouble(value);
+ entry.SetPersistent();
+}
+
+void Preferences::PutFloat(wpi::StringRef key, float value) {
+ auto entry = m_table->GetEntry(key);
+ entry.SetDouble(value);
+ entry.SetPersistent();
+}
+
+void Preferences::PutBoolean(wpi::StringRef key, bool value) {
+ auto entry = m_table->GetEntry(key);
+ entry.SetBoolean(value);
+ entry.SetPersistent();
+}
+
+void Preferences::PutLong(wpi::StringRef key, int64_t value) {
+ auto entry = m_table->GetEntry(key);
+ entry.SetDouble(value);
+ entry.SetPersistent();
+}
+
+bool Preferences::ContainsKey(wpi::StringRef key) {
+ return m_table->ContainsKey(key);
+}
+
+void Preferences::Remove(wpi::StringRef key) { m_table->Delete(key); }
+
+void Preferences::RemoveAll() {
+ for (auto preference : GetKeys()) {
+ if (preference != ".type") {
+ Remove(preference);
+ }
+ }
+}
+
+Preferences::Preferences()
+ : m_table(nt::NetworkTableInstance::GetDefault().GetTable(kTableName)) {
+ m_table->GetEntry(".type").SetString("RobotPreferences");
+ m_listener = m_table->AddEntryListener(
+ [=](nt::NetworkTable* table, wpi::StringRef name,
+ nt::NetworkTableEntry entry, std::shared_ptr<nt::Value> value,
+ int flags) { entry.SetPersistent(); },
+ NT_NOTIFY_NEW | NT_NOTIFY_IMMEDIATE);
+ HAL_Report(HALUsageReporting::kResourceType_Preferences, 0);
+}
diff --git a/wpilibc/src/main/native/cpp/Relay.cpp b/wpilibc/src/main/native/cpp/Relay.cpp
new file mode 100644
index 0000000..cb95223
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Relay.cpp
@@ -0,0 +1,212 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Relay.h"
+
+#include <utility>
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Ports.h>
+#include <hal/Relay.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Relay::Relay(int channel, Relay::Direction direction)
+ : m_channel(channel), m_direction(direction) {
+ if (!SensorUtil::CheckRelayChannel(m_channel)) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
+ "Relay Channel " + wpi::Twine(m_channel));
+ return;
+ }
+
+ HAL_PortHandle portHandle = HAL_GetPort(channel);
+
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ int32_t status = 0;
+ m_forwardHandle = HAL_InitializeRelayPort(portHandle, true, &status);
+ if (status != 0) {
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumRelayChannels(), channel);
+ m_forwardHandle = HAL_kInvalidHandle;
+ m_reverseHandle = HAL_kInvalidHandle;
+ return;
+ }
+ 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_setHALErrorWithRange(status, 0, HAL_GetNumRelayChannels(), channel);
+ m_forwardHandle = HAL_kInvalidHandle;
+ m_reverseHandle = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_Relay, m_channel + 128);
+ }
+
+ int32_t status = 0;
+ if (m_forwardHandle != HAL_kInvalidHandle) {
+ HAL_SetRelay(m_forwardHandle, false, &status);
+ if (status != 0) {
+ wpi_setHALError(status);
+ m_forwardHandle = HAL_kInvalidHandle;
+ m_reverseHandle = HAL_kInvalidHandle;
+ return;
+ }
+ }
+ if (m_reverseHandle != HAL_kInvalidHandle) {
+ HAL_SetRelay(m_reverseHandle, false, &status);
+ if (status != 0) {
+ wpi_setHALError(status);
+ m_forwardHandle = HAL_kInvalidHandle;
+ m_reverseHandle = HAL_kInvalidHandle;
+ return;
+ }
+ }
+
+ SendableRegistry::GetInstance().AddLW(this, "Relay", m_channel);
+}
+
+Relay::~Relay() {
+ int32_t status = 0;
+ HAL_SetRelay(m_forwardHandle, false, &status);
+ HAL_SetRelay(m_reverseHandle, false, &status);
+ // ignore errors, as we want to make sure a free happens.
+ if (m_forwardHandle != HAL_kInvalidHandle) HAL_FreeRelayPort(m_forwardHandle);
+ if (m_reverseHandle != HAL_kInvalidHandle) HAL_FreeRelayPort(m_reverseHandle);
+}
+
+void Relay::Set(Relay::Value value) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+
+ switch (value) {
+ case kOff:
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ HAL_SetRelay(m_forwardHandle, false, &status);
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ HAL_SetRelay(m_reverseHandle, false, &status);
+ }
+ break;
+ case kOn:
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ HAL_SetRelay(m_forwardHandle, true, &status);
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ HAL_SetRelay(m_reverseHandle, true, &status);
+ }
+ break;
+ case kForward:
+ if (m_direction == kReverseOnly) {
+ wpi_setWPIError(IncompatibleMode);
+ break;
+ }
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ HAL_SetRelay(m_forwardHandle, true, &status);
+ }
+ if (m_direction == kBothDirections) {
+ HAL_SetRelay(m_reverseHandle, false, &status);
+ }
+ break;
+ case kReverse:
+ if (m_direction == kForwardOnly) {
+ wpi_setWPIError(IncompatibleMode);
+ break;
+ }
+ if (m_direction == kBothDirections) {
+ HAL_SetRelay(m_forwardHandle, false, &status);
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ HAL_SetRelay(m_reverseHandle, true, &status);
+ }
+ break;
+ }
+
+ wpi_setHALError(status);
+}
+
+Relay::Value Relay::Get() const {
+ int32_t status;
+
+ if (m_direction == kForwardOnly) {
+ if (HAL_GetRelay(m_forwardHandle, &status)) {
+ return kOn;
+ } else {
+ return kOff;
+ }
+ } else if (m_direction == kReverseOnly) {
+ if (HAL_GetRelay(m_reverseHandle, &status)) {
+ return kOn;
+ } else {
+ return kOff;
+ }
+ } else {
+ if (HAL_GetRelay(m_forwardHandle, &status)) {
+ if (HAL_GetRelay(m_reverseHandle, &status)) {
+ return kOn;
+ } else {
+ return kForward;
+ }
+ } else {
+ if (HAL_GetRelay(m_reverseHandle, &status)) {
+ return kReverse;
+ } else {
+ return kOff;
+ }
+ }
+ }
+
+ wpi_setHALError(status);
+}
+
+int Relay::GetChannel() const { return m_channel; }
+
+void Relay::StopMotor() { Set(kOff); }
+
+void Relay::GetDescription(wpi::raw_ostream& desc) const {
+ desc << "Relay " << GetChannel();
+}
+
+void Relay::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Relay");
+ builder.SetActuator(true);
+ builder.SetSafeState([=]() { Set(kOff); });
+ builder.AddSmallStringProperty(
+ "Value",
+ [=](wpi::SmallVectorImpl<char>& buf) -> wpi::StringRef {
+ switch (Get()) {
+ case kOn:
+ return "On";
+ case kForward:
+ return "Forward";
+ case kReverse:
+ return "Reverse";
+ default:
+ return "Off";
+ }
+ },
+ [=](wpi::StringRef value) {
+ if (value == "Off")
+ Set(kOff);
+ else if (value == "Forward")
+ Set(kForward);
+ else if (value == "Reverse")
+ Set(kReverse);
+ else if (value == "On")
+ Set(kOn);
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/Resource.cpp b/wpilibc/src/main/native/cpp/Resource.cpp
new file mode 100644
index 0000000..d546461
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Resource.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Resource.h"
+
+#include "frc/ErrorBase.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+wpi::mutex Resource::m_createMutex;
+
+void Resource::CreateResourceObject(std::unique_ptr<Resource>& r,
+ uint32_t elements) {
+ std::scoped_lock lock(m_createMutex);
+ if (!r) {
+ r = std::make_unique<Resource>(elements);
+ }
+}
+
+Resource::Resource(uint32_t elements) {
+ m_isAllocated = std::vector<bool>(elements, false);
+}
+
+uint32_t Resource::Allocate(const std::string& resourceDesc) {
+ std::scoped_lock lock(m_allocateMutex);
+ for (uint32_t i = 0; i < m_isAllocated.size(); i++) {
+ if (!m_isAllocated[i]) {
+ m_isAllocated[i] = true;
+ return i;
+ }
+ }
+ wpi_setWPIErrorWithContext(NoAvailableResources, resourceDesc);
+ return std::numeric_limits<uint32_t>::max();
+}
+
+uint32_t Resource::Allocate(uint32_t index, const std::string& resourceDesc) {
+ std::scoped_lock lock(m_allocateMutex);
+ if (index >= m_isAllocated.size()) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc);
+ return std::numeric_limits<uint32_t>::max();
+ }
+ if (m_isAllocated[index]) {
+ wpi_setWPIErrorWithContext(ResourceAlreadyAllocated, resourceDesc);
+ return std::numeric_limits<uint32_t>::max();
+ }
+ m_isAllocated[index] = true;
+ return index;
+}
+
+void Resource::Free(uint32_t index) {
+ std::unique_lock lock(m_allocateMutex);
+ if (index == std::numeric_limits<uint32_t>::max()) return;
+ if (index >= m_isAllocated.size()) {
+ wpi_setWPIError(NotAllocated);
+ return;
+ }
+ if (!m_isAllocated[index]) {
+ wpi_setWPIError(NotAllocated);
+ return;
+ }
+ m_isAllocated[index] = false;
+}
diff --git a/wpilibc/src/main/native/cpp/RobotController.cpp b/wpilibc/src/main/native/cpp/RobotController.cpp
new file mode 100644
index 0000000..b1d7608
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/RobotController.cpp
@@ -0,0 +1,177 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/RobotController.h"
+
+#include <hal/CAN.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Power.h>
+
+#include "frc/ErrorBase.h"
+
+using namespace frc;
+
+int RobotController::GetFPGAVersion() {
+ int32_t status = 0;
+ int version = HAL_GetFPGAVersion(&status);
+ wpi_setGlobalHALError(status);
+ return version;
+}
+
+int64_t RobotController::GetFPGARevision() {
+ int32_t status = 0;
+ int64_t revision = HAL_GetFPGARevision(&status);
+ wpi_setGlobalHALError(status);
+ return revision;
+}
+
+uint64_t RobotController::GetFPGATime() {
+ int32_t status = 0;
+ uint64_t time = HAL_GetFPGATime(&status);
+ wpi_setGlobalHALError(status);
+ return time;
+}
+
+bool RobotController::GetUserButton() {
+ int32_t status = 0;
+
+ bool value = HAL_GetFPGAButton(&status);
+ wpi_setGlobalError(status);
+
+ return value;
+}
+
+bool RobotController::IsSysActive() {
+ int32_t status = 0;
+ bool retVal = HAL_GetSystemActive(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+bool RobotController::IsBrownedOut() {
+ int32_t status = 0;
+ bool retVal = HAL_GetBrownedOut(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+double RobotController::GetInputVoltage() {
+ int32_t status = 0;
+ double retVal = HAL_GetVinVoltage(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+double RobotController::GetInputCurrent() {
+ int32_t status = 0;
+ double retVal = HAL_GetVinCurrent(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+double RobotController::GetVoltage3V3() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserVoltage3V3(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+double RobotController::GetCurrent3V3() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserCurrent3V3(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+bool RobotController::GetEnabled3V3() {
+ int32_t status = 0;
+ bool retVal = HAL_GetUserActive3V3(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+int RobotController::GetFaultCount3V3() {
+ int32_t status = 0;
+ int retVal = HAL_GetUserCurrentFaults3V3(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+double RobotController::GetVoltage5V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserVoltage5V(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+double RobotController::GetCurrent5V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserCurrent5V(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+bool RobotController::GetEnabled5V() {
+ int32_t status = 0;
+ bool retVal = HAL_GetUserActive5V(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+int RobotController::GetFaultCount5V() {
+ int32_t status = 0;
+ int retVal = HAL_GetUserCurrentFaults5V(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+double RobotController::GetVoltage6V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserVoltage6V(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+double RobotController::GetCurrent6V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserCurrent6V(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+bool RobotController::GetEnabled6V() {
+ int32_t status = 0;
+ bool retVal = HAL_GetUserActive6V(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+int RobotController::GetFaultCount6V() {
+ int32_t status = 0;
+ int retVal = HAL_GetUserCurrentFaults6V(&status);
+ wpi_setGlobalHALError(status);
+ return retVal;
+}
+
+CANStatus RobotController::GetCANStatus() {
+ int32_t status = 0;
+ float percentBusUtilization = 0;
+ uint32_t busOffCount = 0;
+ uint32_t txFullCount = 0;
+ uint32_t receiveErrorCount = 0;
+ uint32_t transmitErrorCount = 0;
+ HAL_CAN_GetCANStatus(&percentBusUtilization, &busOffCount, &txFullCount,
+ &receiveErrorCount, &transmitErrorCount, &status);
+ if (status != 0) {
+ wpi_setGlobalHALError(status);
+ return {};
+ }
+ return {percentBusUtilization, static_cast<int>(busOffCount),
+ static_cast<int>(txFullCount), static_cast<int>(receiveErrorCount),
+ static_cast<int>(transmitErrorCount)};
+}
diff --git a/wpilibc/src/main/native/cpp/RobotDrive.cpp b/wpilibc/src/main/native/cpp/RobotDrive.cpp
new file mode 100644
index 0000000..5235fa2
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/RobotDrive.cpp
@@ -0,0 +1,427 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/RobotDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/GenericHID.h"
+#include "frc/Joystick.h"
+#include "frc/Talon.h"
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+static std::shared_ptr<SpeedController> make_shared_nodelete(
+ SpeedController* ptr) {
+ return std::shared_ptr<SpeedController>(ptr, NullDeleter<SpeedController>());
+}
+
+RobotDrive::RobotDrive(int leftMotorChannel, int rightMotorChannel) {
+ InitRobotDrive();
+ m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
+ m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
+ SetLeftRightMotorOutputs(0.0, 0.0);
+}
+
+RobotDrive::RobotDrive(int frontLeftMotor, int rearLeftMotor,
+ int frontRightMotor, int rearRightMotor) {
+ InitRobotDrive();
+ m_rearLeftMotor = std::make_shared<Talon>(rearLeftMotor);
+ m_rearRightMotor = std::make_shared<Talon>(rearRightMotor);
+ m_frontLeftMotor = std::make_shared<Talon>(frontLeftMotor);
+ m_frontRightMotor = std::make_shared<Talon>(frontRightMotor);
+ SetLeftRightMotorOutputs(0.0, 0.0);
+}
+
+RobotDrive::RobotDrive(SpeedController* leftMotor,
+ SpeedController* rightMotor) {
+ InitRobotDrive();
+ if (leftMotor == nullptr || rightMotor == nullptr) {
+ wpi_setWPIError(NullParameter);
+ m_rearLeftMotor = m_rearRightMotor = nullptr;
+ return;
+ }
+ m_rearLeftMotor = make_shared_nodelete(leftMotor);
+ m_rearRightMotor = make_shared_nodelete(rightMotor);
+}
+
+RobotDrive::RobotDrive(SpeedController& leftMotor,
+ SpeedController& rightMotor) {
+ InitRobotDrive();
+ m_rearLeftMotor = make_shared_nodelete(&leftMotor);
+ m_rearRightMotor = make_shared_nodelete(&rightMotor);
+}
+
+RobotDrive::RobotDrive(std::shared_ptr<SpeedController> leftMotor,
+ std::shared_ptr<SpeedController> rightMotor) {
+ InitRobotDrive();
+ if (leftMotor == nullptr || rightMotor == nullptr) {
+ wpi_setWPIError(NullParameter);
+ m_rearLeftMotor = m_rearRightMotor = nullptr;
+ return;
+ }
+ m_rearLeftMotor = leftMotor;
+ m_rearRightMotor = rightMotor;
+}
+
+RobotDrive::RobotDrive(SpeedController* frontLeftMotor,
+ SpeedController* rearLeftMotor,
+ SpeedController* frontRightMotor,
+ SpeedController* rearRightMotor) {
+ InitRobotDrive();
+ if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
+ frontRightMotor == nullptr || rearRightMotor == nullptr) {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ m_frontLeftMotor = make_shared_nodelete(frontLeftMotor);
+ m_rearLeftMotor = make_shared_nodelete(rearLeftMotor);
+ m_frontRightMotor = make_shared_nodelete(frontRightMotor);
+ m_rearRightMotor = make_shared_nodelete(rearRightMotor);
+}
+
+RobotDrive::RobotDrive(SpeedController& frontLeftMotor,
+ SpeedController& rearLeftMotor,
+ SpeedController& frontRightMotor,
+ SpeedController& rearRightMotor) {
+ InitRobotDrive();
+ m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor);
+ m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor);
+ m_frontRightMotor = make_shared_nodelete(&frontRightMotor);
+ m_rearRightMotor = make_shared_nodelete(&rearRightMotor);
+}
+
+RobotDrive::RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
+ std::shared_ptr<SpeedController> rearLeftMotor,
+ std::shared_ptr<SpeedController> frontRightMotor,
+ std::shared_ptr<SpeedController> rearRightMotor) {
+ InitRobotDrive();
+ if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
+ frontRightMotor == nullptr || rearRightMotor == nullptr) {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ m_frontLeftMotor = frontLeftMotor;
+ m_rearLeftMotor = rearLeftMotor;
+ m_frontRightMotor = frontRightMotor;
+ m_rearRightMotor = rearRightMotor;
+}
+
+void RobotDrive::Drive(double outputMagnitude, double curve) {
+ double leftOutput, rightOutput;
+ static bool reported = false;
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive_ArcadeRatioCurve, GetNumMotors());
+ reported = true;
+ }
+
+ if (curve < 0) {
+ double value = std::log(-curve);
+ double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+ 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 = 0.0000000001;
+ leftOutput = outputMagnitude;
+ rightOutput = outputMagnitude / ratio;
+ } else {
+ leftOutput = outputMagnitude;
+ rightOutput = outputMagnitude;
+ }
+ SetLeftRightMotorOutputs(leftOutput, rightOutput);
+}
+
+void RobotDrive::TankDrive(GenericHID* leftStick, GenericHID* rightStick,
+ bool squaredInputs) {
+ if (leftStick == nullptr || rightStick == nullptr) {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
+}
+
+void RobotDrive::TankDrive(GenericHID& leftStick, GenericHID& rightStick,
+ bool squaredInputs) {
+ TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs);
+}
+
+void RobotDrive::TankDrive(GenericHID* leftStick, int leftAxis,
+ GenericHID* rightStick, int rightAxis,
+ bool squaredInputs) {
+ if (leftStick == nullptr || rightStick == nullptr) {
+ wpi_setWPIError(NullParameter);
+ return;
+ }
+ TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis),
+ squaredInputs);
+}
+
+void RobotDrive::TankDrive(GenericHID& leftStick, int leftAxis,
+ GenericHID& rightStick, int rightAxis,
+ bool squaredInputs) {
+ TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis),
+ squaredInputs);
+}
+
+void RobotDrive::TankDrive(double leftValue, double rightValue,
+ bool squaredInputs) {
+ static bool reported = false;
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive_Tank, GetNumMotors());
+ reported = true;
+ }
+
+ leftValue = Limit(leftValue);
+ rightValue = Limit(rightValue);
+
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ if (squaredInputs) {
+ leftValue = std::copysign(leftValue * leftValue, leftValue);
+ rightValue = std::copysign(rightValue * rightValue, rightValue);
+ }
+
+ SetLeftRightMotorOutputs(leftValue, rightValue);
+}
+
+void RobotDrive::ArcadeDrive(GenericHID* stick, bool squaredInputs) {
+ // simply call the full-featured ArcadeDrive with the appropriate values
+ ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs);
+}
+
+void RobotDrive::ArcadeDrive(GenericHID& stick, bool squaredInputs) {
+ // simply call the full-featured ArcadeDrive with the appropriate values
+ ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
+}
+
+void RobotDrive::ArcadeDrive(GenericHID* moveStick, int moveAxis,
+ GenericHID* rotateStick, int rotateAxis,
+ bool squaredInputs) {
+ double moveValue = moveStick->GetRawAxis(moveAxis);
+ double rotateValue = rotateStick->GetRawAxis(rotateAxis);
+
+ ArcadeDrive(moveValue, rotateValue, squaredInputs);
+}
+
+void RobotDrive::ArcadeDrive(GenericHID& moveStick, int moveAxis,
+ GenericHID& rotateStick, int rotateAxis,
+ bool squaredInputs) {
+ double moveValue = moveStick.GetRawAxis(moveAxis);
+ double rotateValue = rotateStick.GetRawAxis(rotateAxis);
+
+ ArcadeDrive(moveValue, rotateValue, squaredInputs);
+}
+
+void RobotDrive::ArcadeDrive(double moveValue, double rotateValue,
+ bool squaredInputs) {
+ static bool reported = false;
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive_ArcadeStandard, GetNumMotors());
+ reported = true;
+ }
+
+ // local variables to hold the computed PWM values for the motors
+ double leftMotorOutput;
+ double rightMotorOutput;
+
+ moveValue = Limit(moveValue);
+ rotateValue = Limit(rotateValue);
+
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ if (squaredInputs) {
+ moveValue = std::copysign(moveValue * moveValue, moveValue);
+ rotateValue = std::copysign(rotateValue * rotateValue, rotateValue);
+ }
+
+ if (moveValue > 0.0) {
+ if (rotateValue > 0.0) {
+ leftMotorOutput = moveValue - rotateValue;
+ rightMotorOutput = std::max(moveValue, rotateValue);
+ } else {
+ leftMotorOutput = std::max(moveValue, -rotateValue);
+ rightMotorOutput = moveValue + rotateValue;
+ }
+ } else {
+ if (rotateValue > 0.0) {
+ leftMotorOutput = -std::max(-moveValue, rotateValue);
+ rightMotorOutput = moveValue + rotateValue;
+ } else {
+ leftMotorOutput = moveValue - rotateValue;
+ rightMotorOutput = -std::max(-moveValue, -rotateValue);
+ }
+ }
+ SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
+}
+
+void RobotDrive::MecanumDrive_Cartesian(double x, double y, double rotation,
+ double gyroAngle) {
+ static bool reported = false;
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive_MecanumCartesian, GetNumMotors());
+ reported = true;
+ }
+
+ double xIn = x;
+ double yIn = y;
+ // Negate y for the joystick.
+ yIn = -yIn;
+ // Compenstate for gyro angle.
+ RotateVector(xIn, yIn, gyroAngle);
+
+ double wheelSpeeds[kMaxNumberOfMotors];
+ wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
+ wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
+ wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
+ wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;
+
+ Normalize(wheelSpeeds);
+
+ m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_maxOutput);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput);
+
+ Feed();
+}
+
+void RobotDrive::MecanumDrive_Polar(double magnitude, double direction,
+ double rotation) {
+ static bool reported = false;
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive_MecanumPolar, GetNumMotors());
+ reported = true;
+ }
+
+ // Normalized for full power along the Cartesian axes.
+ magnitude = Limit(magnitude) * std::sqrt(2.0);
+ // The rollers are at 45 degree angles.
+ double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
+ double cosD = std::cos(dirInRad);
+ double sinD = std::sin(dirInRad);
+
+ double wheelSpeeds[kMaxNumberOfMotors];
+ wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
+ wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
+ wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
+ wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;
+
+ Normalize(wheelSpeeds);
+
+ m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_maxOutput);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput);
+
+ Feed();
+}
+
+void RobotDrive::HolonomicDrive(double magnitude, double direction,
+ double rotation) {
+ MecanumDrive_Polar(magnitude, direction, rotation);
+}
+
+void RobotDrive::SetLeftRightMotorOutputs(double leftOutput,
+ double rightOutput) {
+ wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr);
+
+ if (m_frontLeftMotor != nullptr)
+ m_frontLeftMotor->Set(Limit(leftOutput) * m_maxOutput);
+ m_rearLeftMotor->Set(Limit(leftOutput) * m_maxOutput);
+
+ if (m_frontRightMotor != nullptr)
+ m_frontRightMotor->Set(-Limit(rightOutput) * m_maxOutput);
+ m_rearRightMotor->Set(-Limit(rightOutput) * m_maxOutput);
+
+ Feed();
+}
+
+void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) {
+ if (motor < 0 || motor > 3) {
+ wpi_setWPIError(InvalidMotorIndex);
+ return;
+ }
+ switch (motor) {
+ case kFrontLeftMotor:
+ m_frontLeftMotor->SetInverted(isInverted);
+ break;
+ case kFrontRightMotor:
+ m_frontRightMotor->SetInverted(isInverted);
+ break;
+ case kRearLeftMotor:
+ m_rearLeftMotor->SetInverted(isInverted);
+ break;
+ case kRearRightMotor:
+ m_rearRightMotor->SetInverted(isInverted);
+ break;
+ }
+}
+
+void RobotDrive::SetSensitivity(double sensitivity) {
+ m_sensitivity = sensitivity;
+}
+
+void RobotDrive::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
+
+void RobotDrive::GetDescription(wpi::raw_ostream& desc) const {
+ desc << "RobotDrive";
+}
+
+void RobotDrive::StopMotor() {
+ if (m_frontLeftMotor != nullptr) m_frontLeftMotor->StopMotor();
+ if (m_frontRightMotor != nullptr) m_frontRightMotor->StopMotor();
+ if (m_rearLeftMotor != nullptr) m_rearLeftMotor->StopMotor();
+ if (m_rearRightMotor != nullptr) m_rearRightMotor->StopMotor();
+ Feed();
+}
+
+void RobotDrive::InitRobotDrive() { SetSafetyEnabled(true); }
+
+double RobotDrive::Limit(double number) {
+ if (number > 1.0) {
+ return 1.0;
+ }
+ if (number < -1.0) {
+ return -1.0;
+ }
+ return number;
+}
+
+void RobotDrive::Normalize(double* wheelSpeeds) {
+ double maxMagnitude = std::fabs(wheelSpeeds[0]);
+ for (int i = 1; i < kMaxNumberOfMotors; i++) {
+ double temp = std::fabs(wheelSpeeds[i]);
+ if (maxMagnitude < temp) maxMagnitude = temp;
+ }
+ if (maxMagnitude > 1.0) {
+ for (int i = 0; i < kMaxNumberOfMotors; i++) {
+ wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+ }
+ }
+}
+
+void RobotDrive::RotateVector(double& x, double& y, double angle) {
+ double cosA = std::cos(angle * (3.14159 / 180.0));
+ double sinA = std::sin(angle * (3.14159 / 180.0));
+ double xOut = x * cosA - y * sinA;
+ double yOut = x * sinA + y * cosA;
+ x = xOut;
+ y = yOut;
+}
diff --git a/wpilibc/src/main/native/cpp/RobotState.cpp b/wpilibc/src/main/native/cpp/RobotState.cpp
new file mode 100644
index 0000000..530cee3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/RobotState.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/RobotState.h"
+
+#include "frc/DriverStation.h"
+
+using namespace frc;
+
+bool RobotState::IsDisabled() {
+ return DriverStation::GetInstance().IsDisabled();
+}
+
+bool RobotState::IsEnabled() {
+ return DriverStation::GetInstance().IsEnabled();
+}
+
+bool RobotState::IsEStopped() {
+ return DriverStation::GetInstance().IsEStopped();
+}
+
+bool RobotState::IsOperatorControl() {
+ return DriverStation::GetInstance().IsOperatorControl();
+}
+
+bool RobotState::IsAutonomous() {
+ return DriverStation::GetInstance().IsAutonomous();
+}
+
+bool RobotState::IsTest() { return DriverStation::GetInstance().IsTest(); }
diff --git a/wpilibc/src/main/native/cpp/SD540.cpp b/wpilibc/src/main/native/cpp/SD540.cpp
new file mode 100644
index 0000000..9611f66
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SD540.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/SD540.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+SD540::SD540(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.05, 1.55, 1.50, 1.44, 0.94);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ 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
new file mode 100644
index 0000000..d51fa3b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SPI.cpp
@@ -0,0 +1,444 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/SPI.h"
+
+#include <cstring>
+#include <utility>
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/SPI.h>
+#include <wpi/SmallVector.h>
+#include <wpi/mutex.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Notifier.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+static constexpr int kAccumulateDepth = 2048;
+
+class SPI::Accumulator {
+ public:
+ Accumulator(HAL_SPIPort port, int xferSize, int validMask, int validValue,
+ int dataShift, int dataSize, bool isSigned, bool bigEndian)
+ : m_notifier([=]() {
+ std::scoped_lock lock(m_mutex);
+ Update();
+ }),
+ m_buf(new uint32_t[(xferSize + 1) * kAccumulateDepth]),
+ m_validMask(validMask),
+ m_validValue(validValue),
+ m_dataMax(1 << dataSize),
+ m_dataMsbMask(1 << (dataSize - 1)),
+ m_dataShift(dataShift),
+ m_xferSize(xferSize + 1), // +1 for timestamp
+ m_isSigned(isSigned),
+ m_bigEndian(bigEndian),
+ m_port(port) {}
+ ~Accumulator() { delete[] m_buf; }
+
+ void Update();
+
+ Notifier m_notifier;
+ uint32_t* m_buf;
+ wpi::mutex m_mutex;
+
+ int64_t m_value = 0;
+ uint32_t m_count = 0;
+ int32_t m_lastValue = 0;
+ uint32_t m_lastTimestamp = 0;
+ double m_integratedValue = 0;
+
+ int32_t m_center = 0;
+ int32_t m_deadband = 0;
+ double m_integratedCenter = 0;
+
+ int32_t m_validMask;
+ int32_t m_validValue;
+ int32_t m_dataMax; // one more than max data value
+ int32_t m_dataMsbMask; // data field MSB mask (for signed)
+ uint8_t m_dataShift; // data field shift right amount, in bits
+ int32_t m_xferSize; // SPI transfer size, in bytes
+ bool m_isSigned; // is data field signed?
+ bool m_bigEndian; // is response big endian?
+ HAL_SPIPort m_port;
+};
+
+void SPI::Accumulator::Update() {
+ bool done;
+ do {
+ done = true;
+ int32_t status = 0;
+
+ // get amount of data available
+ int32_t numToRead =
+ HAL_ReadSPIAutoReceivedData(m_port, m_buf, 0, 0, &status);
+ if (status != 0) return; // error reading
+
+ // only get whole responses; +1 is for timestamp
+ numToRead -= numToRead % m_xferSize;
+ if (numToRead > m_xferSize * kAccumulateDepth) {
+ numToRead = m_xferSize * kAccumulateDepth;
+ done = false;
+ }
+ if (numToRead == 0) return; // no samples
+
+ // read buffered data
+ HAL_ReadSPIAutoReceivedData(m_port, m_buf, numToRead, 0, &status);
+ if (status != 0) return; // error reading
+
+ // loop over all responses
+ for (int32_t off = 0; off < numToRead; off += m_xferSize) {
+ // get timestamp from first word
+ uint32_t timestamp = m_buf[off];
+
+ // convert from bytes
+ uint32_t resp = 0;
+ if (m_bigEndian) {
+ for (int32_t i = 1; i < m_xferSize; ++i) {
+ resp <<= 8;
+ resp |= m_buf[off + i] & 0xff;
+ }
+ } else {
+ for (int32_t i = m_xferSize - 1; i >= 1; --i) {
+ resp <<= 8;
+ resp |= m_buf[off + i] & 0xff;
+ }
+ }
+
+ // process response
+ if ((resp & m_validMask) == static_cast<uint32_t>(m_validValue)) {
+ // valid sensor data; extract data field
+ int32_t data = static_cast<int32_t>(resp >> m_dataShift);
+ data &= m_dataMax - 1;
+ // 2s complement conversion if signed MSB is set
+ if (m_isSigned && (data & m_dataMsbMask) != 0) data -= m_dataMax;
+ // center offset
+ int32_t dataNoCenter = data;
+ data -= m_center;
+ // only accumulate if outside deadband
+ if (data < -m_deadband || data > m_deadband) {
+ m_value += data;
+ if (m_count != 0) {
+ // timestamps use the 1us FPGA clock; also handle rollover
+ if (timestamp >= m_lastTimestamp)
+ m_integratedValue +=
+ dataNoCenter *
+ static_cast<int32_t>(timestamp - m_lastTimestamp) * 1e-6 -
+ m_integratedCenter;
+ else
+ m_integratedValue +=
+ dataNoCenter *
+ static_cast<int32_t>((1ULL << 32) - m_lastTimestamp +
+ timestamp) *
+ 1e-6 -
+ m_integratedCenter;
+ }
+ }
+ ++m_count;
+ m_lastValue = data;
+ } else {
+ // no data from the sensor; just clear the last value
+ m_lastValue = 0;
+ }
+ m_lastTimestamp = timestamp;
+ }
+ } while (!done);
+}
+
+SPI::SPI(Port port) : m_port(static_cast<HAL_SPIPort>(port)) {
+ int32_t status = 0;
+ HAL_InitializeSPI(m_port, &status);
+ wpi_setHALError(status);
+
+ HAL_Report(HALUsageReporting::kResourceType_SPI,
+ static_cast<uint8_t>(port) + 1);
+}
+
+SPI::~SPI() { HAL_CloseSPI(m_port); }
+
+void SPI::SetClockRate(int hz) { HAL_SetSPISpeed(m_port, hz); }
+
+void SPI::SetMSBFirst() {
+ m_msbFirst = true;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+}
+
+void SPI::SetLSBFirst() {
+ m_msbFirst = false;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+}
+
+void SPI::SetSampleDataOnLeadingEdge() {
+ m_sampleOnTrailing = false;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+}
+
+void SPI::SetSampleDataOnTrailingEdge() {
+ m_sampleOnTrailing = true;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+}
+
+void SPI::SetSampleDataOnFalling() {
+ m_sampleOnTrailing = true;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+}
+
+void SPI::SetSampleDataOnRising() {
+ m_sampleOnTrailing = false;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+}
+
+void SPI::SetClockActiveLow() {
+ m_clockIdleHigh = true;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+}
+
+void SPI::SetClockActiveHigh() {
+ m_clockIdleHigh = false;
+ HAL_SetSPIOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
+}
+
+void SPI::SetChipSelectActiveHigh() {
+ int32_t status = 0;
+ HAL_SetSPIChipSelectActiveHigh(m_port, &status);
+ wpi_setHALError(status);
+}
+
+void SPI::SetChipSelectActiveLow() {
+ int32_t status = 0;
+ HAL_SetSPIChipSelectActiveLow(m_port, &status);
+ wpi_setHALError(status);
+}
+
+int SPI::Write(uint8_t* data, int size) {
+ int retVal = 0;
+ retVal = HAL_WriteSPI(m_port, data, size);
+ return retVal;
+}
+
+int SPI::Read(bool initiate, uint8_t* dataReceived, int size) {
+ int retVal = 0;
+ if (initiate) {
+ wpi::SmallVector<uint8_t, 32> dataToSend;
+ dataToSend.resize(size);
+ retVal = HAL_TransactionSPI(m_port, dataToSend.data(), dataReceived, size);
+ } else {
+ retVal = HAL_ReadSPI(m_port, dataReceived, size);
+ }
+ return retVal;
+}
+
+int SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size) {
+ int retVal = 0;
+ retVal = HAL_TransactionSPI(m_port, dataToSend, dataReceived, size);
+ return retVal;
+}
+
+void SPI::InitAuto(int bufferSize) {
+ int32_t status = 0;
+ HAL_InitSPIAuto(m_port, bufferSize, &status);
+ wpi_setHALError(status);
+}
+
+void SPI::FreeAuto() {
+ int32_t status = 0;
+ HAL_FreeSPIAuto(m_port, &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_setHALError(status);
+}
+
+void SPI::StartAutoRate(units::second_t period) {
+ int32_t status = 0;
+ HAL_StartSPIAutoRate(m_port, period.to<double>(), &status);
+ wpi_setHALError(status);
+}
+
+void SPI::StartAutoRate(double period) {
+ StartAutoRate(units::second_t(period));
+}
+
+void SPI::StartAutoTrigger(DigitalSource& source, bool rising, bool falling) {
+ int32_t status = 0;
+ HAL_StartSPIAutoTrigger(
+ m_port, source.GetPortHandleForRouting(),
+ (HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(), rising,
+ falling, &status);
+ wpi_setHALError(status);
+}
+
+void SPI::StopAuto() {
+ int32_t status = 0;
+ HAL_StopSPIAuto(m_port, &status);
+ wpi_setHALError(status);
+}
+
+void SPI::ForceAutoRead() {
+ int32_t status = 0;
+ HAL_ForceSPIAutoRead(m_port, &status);
+ wpi_setHALError(status);
+}
+
+int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead,
+ units::second_t timeout) {
+ int32_t status = 0;
+ int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead,
+ timeout.to<double>(), &status);
+ wpi_setHALError(status);
+ return val;
+}
+
+int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout) {
+ return ReadAutoReceivedData(buffer, numToRead, units::second_t(timeout));
+}
+
+int SPI::GetAutoDroppedCount() {
+ int32_t status = 0;
+ int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
+ wpi_setHALError(status);
+ return val;
+}
+
+void SPI::ConfigureAutoStall(HAL_SPIPort port, int csToSclkTicks,
+ int stallTicks, int pow2BytesPerRead) {
+ int32_t status = 0;
+ HAL_ConfigureSPIAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead,
+ &status);
+ wpi_setHALError(status);
+}
+
+void SPI::InitAccumulator(units::second_t period, int cmd, int xferSize,
+ int validMask, int validValue, int dataShift,
+ int dataSize, bool isSigned, bool bigEndian) {
+ InitAuto(xferSize * kAccumulateDepth);
+ uint8_t cmdBytes[4] = {0, 0, 0, 0};
+ if (bigEndian) {
+ for (int32_t i = xferSize - 1; i >= 0; --i) {
+ cmdBytes[i] = cmd & 0xff;
+ cmd >>= 8;
+ }
+ } else {
+ cmdBytes[0] = cmd & 0xff;
+ cmd >>= 8;
+ cmdBytes[1] = cmd & 0xff;
+ cmd >>= 8;
+ cmdBytes[2] = cmd & 0xff;
+ cmd >>= 8;
+ cmdBytes[3] = cmd & 0xff;
+ }
+ SetAutoTransmitData(cmdBytes, xferSize - 4);
+ StartAutoRate(period);
+
+ m_accum.reset(new Accumulator(m_port, xferSize, validMask, validValue,
+ dataShift, dataSize, isSigned, bigEndian));
+ m_accum->m_notifier.StartPeriodic(period * kAccumulateDepth / 2);
+}
+
+void SPI::InitAccumulator(double period, int cmd, int xferSize, int validMask,
+ int validValue, int dataShift, int dataSize,
+ bool isSigned, bool bigEndian) {
+ InitAccumulator(units::second_t(period), cmd, xferSize, validMask, validValue,
+ dataShift, dataSize, isSigned, bigEndian);
+}
+
+void SPI::FreeAccumulator() {
+ m_accum.reset(nullptr);
+ FreeAuto();
+}
+
+void SPI::ResetAccumulator() {
+ if (!m_accum) return;
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->m_value = 0;
+ m_accum->m_count = 0;
+ m_accum->m_lastValue = 0;
+ m_accum->m_lastTimestamp = 0;
+ m_accum->m_integratedValue = 0;
+}
+
+void SPI::SetAccumulatorCenter(int center) {
+ if (!m_accum) return;
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->m_center = center;
+}
+
+void SPI::SetAccumulatorDeadband(int deadband) {
+ if (!m_accum) return;
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->m_deadband = deadband;
+}
+
+int SPI::GetAccumulatorLastValue() const {
+ if (!m_accum) return 0;
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->Update();
+ return m_accum->m_lastValue;
+}
+
+int64_t SPI::GetAccumulatorValue() const {
+ if (!m_accum) return 0;
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->Update();
+ return m_accum->m_value;
+}
+
+int64_t SPI::GetAccumulatorCount() const {
+ if (!m_accum) return 0;
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->Update();
+ return m_accum->m_count;
+}
+
+double SPI::GetAccumulatorAverage() const {
+ if (!m_accum) return 0;
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->Update();
+ if (m_accum->m_count == 0) return 0.0;
+ return static_cast<double>(m_accum->m_value) / m_accum->m_count;
+}
+
+void SPI::GetAccumulatorOutput(int64_t& value, int64_t& count) const {
+ if (!m_accum) {
+ value = 0;
+ count = 0;
+ return;
+ }
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->Update();
+ value = m_accum->m_value;
+ count = m_accum->m_count;
+}
+
+void SPI::SetAccumulatorIntegratedCenter(double center) {
+ if (!m_accum) return;
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->m_integratedCenter = center;
+}
+
+double SPI::GetAccumulatorIntegratedValue() const {
+ if (!m_accum) return 0;
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->Update();
+ return m_accum->m_integratedValue;
+}
+
+double SPI::GetAccumulatorIntegratedAverage() const {
+ if (!m_accum) return 0;
+ std::scoped_lock lock(m_accum->m_mutex);
+ m_accum->Update();
+ if (m_accum->m_count <= 1) return 0.0;
+ // count-1 due to not integrating the first value received
+ return m_accum->m_integratedValue / (m_accum->m_count - 1);
+}
diff --git a/wpilibc/src/main/native/cpp/SensorUtil.cpp b/wpilibc/src/main/native/cpp/SensorUtil.cpp
new file mode 100644
index 0000000..8c02f1b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SensorUtil.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/SensorUtil.h"
+
+#include <hal/AnalogInput.h>
+#include <hal/AnalogOutput.h>
+#include <hal/DIO.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/PDP.h>
+#include <hal/PWM.h>
+#include <hal/Ports.h>
+#include <hal/Relay.h>
+#include <hal/Solenoid.h>
+
+using namespace frc;
+
+const int SensorUtil::kDigitalChannels = HAL_GetNumDigitalChannels();
+const int SensorUtil::kAnalogInputs = HAL_GetNumAnalogInputs();
+const int SensorUtil::kSolenoidChannels = HAL_GetNumSolenoidChannels();
+const int SensorUtil::kSolenoidModules = HAL_GetNumPCMModules();
+const int SensorUtil::kPwmChannels = HAL_GetNumPWMChannels();
+const int SensorUtil::kRelayChannels = HAL_GetNumRelayHeaders();
+const int SensorUtil::kPDPChannels = HAL_GetNumPDPChannels();
+
+int SensorUtil::GetDefaultSolenoidModule() { return 0; }
+
+bool SensorUtil::CheckSolenoidModule(int moduleNumber) {
+ return HAL_CheckSolenoidModule(moduleNumber);
+}
+
+bool SensorUtil::CheckDigitalChannel(int channel) {
+ return HAL_CheckDIOChannel(channel);
+}
+
+bool SensorUtil::CheckRelayChannel(int channel) {
+ return HAL_CheckRelayChannel(channel);
+}
+
+bool SensorUtil::CheckPWMChannel(int channel) {
+ return HAL_CheckPWMChannel(channel);
+}
+
+bool SensorUtil::CheckAnalogInputChannel(int channel) {
+ return HAL_CheckAnalogInputChannel(channel);
+}
+
+bool SensorUtil::CheckAnalogOutputChannel(int channel) {
+ return HAL_CheckAnalogOutputChannel(channel);
+}
+
+bool SensorUtil::CheckSolenoidChannel(int channel) {
+ return HAL_CheckSolenoidChannel(channel);
+}
+
+bool SensorUtil::CheckPDPChannel(int channel) {
+ return HAL_CheckPDPChannel(channel);
+}
+
+bool SensorUtil::CheckPDPModule(int module) {
+ return HAL_CheckPDPModule(module);
+}
diff --git a/wpilibc/src/main/native/cpp/SerialPort.cpp b/wpilibc/src/main/native/cpp/SerialPort.cpp
new file mode 100644
index 0000000..e092fc2
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SerialPort.cpp
@@ -0,0 +1,166 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/SerialPort.h"
+
+#include <utility>
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/SerialPort.h>
+
+using namespace frc;
+
+SerialPort::SerialPort(int baudRate, Port port, int dataBits,
+ SerialPort::Parity parity,
+ SerialPort::StopBits stopBits) {
+ int32_t status = 0;
+
+ m_portHandle =
+ HAL_InitializeSerialPort(static_cast<HAL_SerialPort>(port), &status);
+ wpi_setHALError(status);
+ // Don't continue if initialization failed
+ if (status < 0) return;
+ HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
+ wpi_setHALError(status);
+ HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
+ wpi_setHALError(status);
+ HAL_SetSerialParity(m_portHandle, parity, &status);
+ wpi_setHALError(status);
+ HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
+ wpi_setHALError(status);
+
+ // Set the default timeout to 5 seconds.
+ SetTimeout(5.0);
+
+ // Don't wait until the buffer is full to transmit.
+ SetWriteBufferMode(kFlushOnAccess);
+
+ DisableTermination();
+
+ HAL_Report(HALUsageReporting::kResourceType_SerialPort,
+ static_cast<uint8_t>(port) + 1);
+}
+
+SerialPort::SerialPort(int baudRate, const wpi::Twine& portName, Port port,
+ int dataBits, SerialPort::Parity parity,
+ SerialPort::StopBits stopBits) {
+ int32_t status = 0;
+
+ wpi::SmallVector<char, 64> buf;
+ const char* portNameC = portName.toNullTerminatedStringRef(buf).data();
+
+ m_portHandle = HAL_InitializeSerialPortDirect(
+ static_cast<HAL_SerialPort>(port), portNameC, &status);
+ wpi_setHALError(status);
+ // Don't continue if initialization failed
+ if (status < 0) return;
+ HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
+ wpi_setHALError(status);
+ HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
+ wpi_setHALError(status);
+ HAL_SetSerialParity(m_portHandle, parity, &status);
+ wpi_setHALError(status);
+ HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
+ wpi_setHALError(status);
+
+ // Set the default timeout to 5 seconds.
+ SetTimeout(5.0);
+
+ // Don't wait until the buffer is full to transmit.
+ SetWriteBufferMode(kFlushOnAccess);
+
+ DisableTermination();
+
+ HAL_Report(HALUsageReporting::kResourceType_SerialPort,
+ static_cast<uint8_t>(port) + 1);
+}
+
+SerialPort::~SerialPort() {
+ int32_t status = 0;
+ HAL_CloseSerial(m_portHandle, &status);
+ wpi_setHALError(status);
+}
+
+void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) {
+ int32_t status = 0;
+ HAL_SetSerialFlowControl(m_portHandle, flowControl, &status);
+ wpi_setHALError(status);
+}
+
+void SerialPort::EnableTermination(char terminator) {
+ int32_t status = 0;
+ HAL_EnableSerialTermination(m_portHandle, terminator, &status);
+ wpi_setHALError(status);
+}
+
+void SerialPort::DisableTermination() {
+ int32_t status = 0;
+ HAL_DisableSerialTermination(m_portHandle, &status);
+ wpi_setHALError(status);
+}
+
+int SerialPort::GetBytesReceived() {
+ int32_t status = 0;
+ int retVal = HAL_GetSerialBytesReceived(m_portHandle, &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_setHALError(status);
+ return retVal;
+}
+
+int SerialPort::Write(const char* buffer, int count) {
+ return Write(wpi::StringRef(buffer, static_cast<size_t>(count)));
+}
+
+int SerialPort::Write(wpi::StringRef buffer) {
+ int32_t status = 0;
+ int retVal =
+ HAL_WriteSerial(m_portHandle, buffer.data(), buffer.size(), &status);
+ wpi_setHALError(status);
+ return retVal;
+}
+
+void SerialPort::SetTimeout(double timeout) {
+ int32_t status = 0;
+ HAL_SetSerialTimeout(m_portHandle, timeout, &status);
+ wpi_setHALError(status);
+}
+
+void SerialPort::SetReadBufferSize(int size) {
+ int32_t status = 0;
+ HAL_SetSerialReadBufferSize(m_portHandle, size, &status);
+ wpi_setHALError(status);
+}
+
+void SerialPort::SetWriteBufferSize(int size) {
+ int32_t status = 0;
+ HAL_SetSerialWriteBufferSize(m_portHandle, size, &status);
+ wpi_setHALError(status);
+}
+
+void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode) {
+ int32_t status = 0;
+ HAL_SetSerialWriteMode(m_portHandle, mode, &status);
+ wpi_setHALError(status);
+}
+
+void SerialPort::Flush() {
+ int32_t status = 0;
+ HAL_FlushSerial(m_portHandle, &status);
+ wpi_setHALError(status);
+}
+
+void SerialPort::Reset() {
+ int32_t status = 0;
+ HAL_ClearSerial(m_portHandle, &status);
+ wpi_setHALError(status);
+}
diff --git a/wpilibc/src/main/native/cpp/Servo.cpp b/wpilibc/src/main/native/cpp/Servo.cpp
new file mode 100644
index 0000000..4b6856a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Servo.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Servo.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+constexpr double Servo::kMaxServoAngle;
+constexpr double Servo::kMinServoAngle;
+
+constexpr double Servo::kDefaultMaxServoPWM;
+constexpr double Servo::kDefaultMinServoPWM;
+
+Servo::Servo(int channel) : PWM(channel) {
+ // Set minimum and maximum PWM values supported by the servo
+ SetBounds(kDefaultMaxServoPWM, 0.0, 0.0, 0.0, kDefaultMinServoPWM);
+
+ // Assign defaults for period multiplier for the servo PWM control signal
+ SetPeriodMultiplier(kPeriodMultiplier_4X);
+
+ HAL_Report(HALUsageReporting::kResourceType_Servo, channel + 1);
+ SendableRegistry::GetInstance().SetName(this, "Servo", channel);
+}
+
+void Servo::Set(double value) { SetPosition(value); }
+
+void Servo::SetOffline() { SetRaw(0); }
+
+double Servo::Get() const { return GetPosition(); }
+
+void Servo::SetAngle(double degrees) {
+ if (degrees < kMinServoAngle) {
+ degrees = kMinServoAngle;
+ } else if (degrees > kMaxServoAngle) {
+ degrees = kMaxServoAngle;
+ }
+
+ SetPosition((degrees - kMinServoAngle) / GetServoAngleRange());
+}
+
+double Servo::GetAngle() const {
+ return GetPosition() * GetServoAngleRange() + kMinServoAngle;
+}
+
+double Servo::GetMaxAngle() const { return kMaxServoAngle; }
+
+double Servo::GetMinAngle() const { return kMinServoAngle; }
+
+void Servo::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Servo");
+ builder.AddDoubleProperty("Value", [=]() { return Get(); },
+ [=](double value) { Set(value); });
+}
+
+double Servo::GetServoAngleRange() const {
+ return kMaxServoAngle - kMinServoAngle;
+}
diff --git a/wpilibc/src/main/native/cpp/Solenoid.cpp b/wpilibc/src/main/native/cpp/Solenoid.cpp
new file mode 100644
index 0000000..e0bde3c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Solenoid.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Solenoid.h"
+
+#include <utility>
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Ports.h>
+#include <hal/Solenoid.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Solenoid::Solenoid(int channel)
+ : Solenoid(SensorUtil::GetDefaultSolenoidModule(), channel) {}
+
+Solenoid::Solenoid(int moduleNumber, int channel)
+ : SolenoidBase(moduleNumber), m_channel(channel) {
+ if (!SensorUtil::CheckSolenoidModule(m_moduleNumber)) {
+ wpi_setWPIErrorWithContext(ModuleIndexOutOfRange,
+ "Solenoid Module " + wpi::Twine(m_moduleNumber));
+ return;
+ }
+ if (!SensorUtil::CheckSolenoidChannel(m_channel)) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
+ "Solenoid Channel " + wpi::Twine(m_channel));
+ return;
+ }
+
+ int32_t status = 0;
+ m_solenoidHandle = HAL_InitializeSolenoidPort(
+ HAL_GetPortWithModule(moduleNumber, channel), &status);
+ if (status != 0) {
+ wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(), channel);
+ m_solenoidHandle = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel + 1,
+ m_moduleNumber + 1);
+ SendableRegistry::GetInstance().AddLW(this, "Solenoid", m_moduleNumber,
+ m_channel);
+}
+
+Solenoid::~Solenoid() { HAL_FreeSolenoidPort(m_solenoidHandle); }
+
+void Solenoid::Set(bool on) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetSolenoid(m_solenoidHandle, on, &status);
+ wpi_setHALError(status);
+}
+
+bool Solenoid::Get() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = HAL_GetSolenoid(m_solenoidHandle, &status);
+ wpi_setHALError(status);
+ return value;
+}
+
+bool Solenoid::IsBlackListed() const {
+ int value = GetPCMSolenoidBlackList(m_moduleNumber) & (1 << m_channel);
+ return (value != 0);
+}
+
+void Solenoid::SetPulseDuration(double durationSeconds) {
+ int32_t durationMS = durationSeconds * 1000;
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetOneShotDuration(m_solenoidHandle, durationMS, &status);
+ wpi_setHALError(status);
+}
+
+void Solenoid::StartPulse() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_FireOneShot(m_solenoidHandle, &status);
+ wpi_setHALError(status);
+}
+
+void Solenoid::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Solenoid");
+ builder.SetActuator(true);
+ builder.SetSafeState([=]() { Set(false); });
+ builder.AddBooleanProperty("Value", [=]() { return Get(); },
+ [=](bool value) { Set(value); });
+}
diff --git a/wpilibc/src/main/native/cpp/SolenoidBase.cpp b/wpilibc/src/main/native/cpp/SolenoidBase.cpp
new file mode 100644
index 0000000..f5f8aad
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SolenoidBase.cpp
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/SolenoidBase.h"
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/Solenoid.h>
+
+using namespace frc;
+
+int SolenoidBase::GetAll(int module) {
+ int value = 0;
+ int32_t status = 0;
+ value = HAL_GetAllSolenoids(module, &status);
+ wpi_setGlobalHALError(status);
+ return value;
+}
+
+int SolenoidBase::GetAll() const {
+ return SolenoidBase::GetAll(m_moduleNumber);
+}
+
+int SolenoidBase::GetPCMSolenoidBlackList(int module) {
+ int32_t status = 0;
+ return HAL_GetPCMSolenoidBlackList(module, &status);
+}
+
+int SolenoidBase::GetPCMSolenoidBlackList() const {
+ return SolenoidBase::GetPCMSolenoidBlackList(m_moduleNumber);
+}
+
+bool SolenoidBase::GetPCMSolenoidVoltageStickyFault(int module) {
+ int32_t status = 0;
+ return HAL_GetPCMSolenoidVoltageStickyFault(module, &status);
+}
+
+bool SolenoidBase::GetPCMSolenoidVoltageStickyFault() const {
+ return SolenoidBase::GetPCMSolenoidVoltageStickyFault(m_moduleNumber);
+}
+
+bool SolenoidBase::GetPCMSolenoidVoltageFault(int module) {
+ int32_t status = 0;
+ return HAL_GetPCMSolenoidVoltageFault(module, &status);
+}
+
+bool SolenoidBase::GetPCMSolenoidVoltageFault() const {
+ return SolenoidBase::GetPCMSolenoidVoltageFault(m_moduleNumber);
+}
+
+void SolenoidBase::ClearAllPCMStickyFaults(int module) {
+ int32_t status = 0;
+ return HAL_ClearAllPCMStickyFaults(module, &status);
+}
+
+void SolenoidBase::ClearAllPCMStickyFaults() {
+ SolenoidBase::ClearAllPCMStickyFaults(m_moduleNumber);
+}
+
+SolenoidBase::SolenoidBase(int moduleNumber) : m_moduleNumber(moduleNumber) {}
diff --git a/wpilibc/src/main/native/cpp/Spark.cpp b/wpilibc/src/main/native/cpp/Spark.cpp
new file mode 100644
index 0000000..3717df4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Spark.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Spark.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Spark::Spark(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel() + 1);
+ SendableRegistry::GetInstance().SetName(this, "Spark", GetChannel());
+}
diff --git a/wpilibc/src/main/native/cpp/SpeedController.cpp b/wpilibc/src/main/native/cpp/SpeedController.cpp
new file mode 100644
index 0000000..cb3cc5b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SpeedController.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/SpeedController.h"
+
+#include <frc/RobotController.h>
+
+using namespace frc;
+
+void SpeedController::SetVoltage(units::volt_t output) {
+ Set(output / units::volt_t(RobotController::GetInputVoltage()));
+}
diff --git a/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp b/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
new file mode 100644
index 0000000..a807afc
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* 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/SpeedControllerGroup.h"
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+void SpeedControllerGroup::Set(double speed) {
+ for (auto speedController : m_speedControllers) {
+ speedController.get().Set(m_isInverted ? -speed : speed);
+ }
+}
+
+double SpeedControllerGroup::Get() const {
+ if (!m_speedControllers.empty()) {
+ return m_speedControllers.front().get().Get() * (m_isInverted ? -1 : 1);
+ }
+ return 0.0;
+}
+
+void SpeedControllerGroup::SetInverted(bool isInverted) {
+ m_isInverted = isInverted;
+}
+
+bool SpeedControllerGroup::GetInverted() const { return m_isInverted; }
+
+void SpeedControllerGroup::Disable() {
+ for (auto speedController : m_speedControllers) {
+ speedController.get().Disable();
+ }
+}
+
+void SpeedControllerGroup::StopMotor() {
+ for (auto speedController : m_speedControllers) {
+ speedController.get().StopMotor();
+ }
+}
+
+void SpeedControllerGroup::PIDWrite(double output) { Set(output); }
+
+void SpeedControllerGroup::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Speed Controller");
+ builder.SetActuator(true);
+ builder.SetSafeState([=]() { StopMotor(); });
+ builder.AddDoubleProperty("Value", [=]() { return Get(); },
+ [=](double value) { Set(value); });
+}
diff --git a/wpilibc/src/main/native/cpp/Talon.cpp b/wpilibc/src/main/native/cpp/Talon.cpp
new file mode 100644
index 0000000..4807d71
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Talon.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Talon.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Talon::Talon(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.037, 1.539, 1.513, 1.487, 0.989);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ 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
new file mode 100644
index 0000000..7b1e647
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Threads.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Threads.h"
+
+#include <hal/FRCUsageReporting.h>
+#include <hal/Threads.h>
+
+#include "frc/ErrorBase.h"
+
+namespace frc {
+
+int GetThreadPriority(std::thread& thread, bool* isRealTime) {
+ int32_t status = 0;
+ HAL_Bool rt = false;
+ auto native = thread.native_handle();
+ auto ret = HAL_GetThreadPriority(&native, &rt, &status);
+ wpi_setGlobalHALError(status);
+ *isRealTime = rt;
+ return ret;
+}
+
+int GetCurrentThreadPriority(bool* isRealTime) {
+ int32_t status = 0;
+ HAL_Bool rt = false;
+ auto ret = HAL_GetCurrentThreadPriority(&rt, &status);
+ wpi_setGlobalHALError(status);
+ *isRealTime = rt;
+ return ret;
+}
+
+bool SetThreadPriority(std::thread& thread, bool realTime, int priority) {
+ int32_t status = 0;
+ auto native = thread.native_handle();
+ auto ret = HAL_SetThreadPriority(&native, realTime, priority, &status);
+ wpi_setGlobalHALError(status);
+ return ret;
+}
+
+bool SetCurrentThreadPriority(bool realTime, int priority) {
+ int32_t status = 0;
+ auto ret = HAL_SetCurrentThreadPriority(realTime, priority, &status);
+ wpi_setGlobalHALError(status);
+ return ret;
+}
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp
new file mode 100644
index 0000000..a9358dd
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/TimedRobot.h"
+
+#include <stdint.h>
+
+#include <utility>
+
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/Notifier.h>
+
+#include "frc/Timer.h"
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+void TimedRobot::StartCompetition() {
+ RobotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL_ObserveUserProgramStarting();
+
+ m_expirationTime = units::second_t{Timer::GetFPGATimestamp()} + m_period;
+ UpdateAlarm();
+
+ // Loop forever, calling the appropriate mode-dependent function
+ while (true) {
+ int32_t status = 0;
+ uint64_t curTime = HAL_WaitForNotifierAlarm(m_notifier, &status);
+ if (curTime == 0 || status != 0) break;
+
+ m_expirationTime += m_period;
+
+ UpdateAlarm();
+
+ // Call callback
+ LoopFunc();
+ }
+}
+
+void TimedRobot::EndCompetition() {
+ int32_t status = 0;
+ HAL_StopNotifier(m_notifier, &status);
+}
+
+units::second_t TimedRobot::GetPeriod() const {
+ return units::second_t(m_period);
+}
+
+TimedRobot::TimedRobot(double period) : TimedRobot(units::second_t(period)) {}
+
+TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
+ int32_t status = 0;
+ m_notifier = HAL_InitializeNotifier(&status);
+ wpi_setHALError(status);
+ HAL_SetNotifierName(m_notifier, "TimedRobot", &status);
+
+ HAL_Report(HALUsageReporting::kResourceType_Framework,
+ HALUsageReporting::kFramework_Timed);
+}
+
+TimedRobot::~TimedRobot() {
+ int32_t status = 0;
+
+ HAL_StopNotifier(m_notifier, &status);
+ wpi_setHALError(status);
+
+ HAL_CleanNotifier(m_notifier, &status);
+}
+
+void TimedRobot::UpdateAlarm() {
+ int32_t status = 0;
+ HAL_UpdateNotifierAlarm(
+ m_notifier, static_cast<uint64_t>(m_expirationTime * 1e6), &status);
+ wpi_setHALError(status);
+}
diff --git a/wpilibc/src/main/native/cpp/Timer.cpp b/wpilibc/src/main/native/cpp/Timer.cpp
new file mode 100644
index 0000000..c91bc13
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Timer.cpp
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Timer.h"
+
+#include <chrono>
+#include <thread>
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/DriverStation.h"
+#include "frc/RobotController.h"
+
+namespace frc {
+
+void Wait(double seconds) { frc2::Wait(units::second_t(seconds)); }
+
+double GetTime() { return frc2::GetTime().to<double>(); }
+
+} // namespace frc
+
+using namespace frc;
+
+const double Timer::kRolloverTime = frc2::Timer::kRolloverTime.to<double>();
+
+Timer::Timer() { Reset(); }
+
+double Timer::Get() const { return m_timer.Get().to<double>(); }
+
+void Timer::Reset() { m_timer.Reset(); }
+
+void Timer::Start() { m_timer.Start(); }
+
+void Timer::Stop() { m_timer.Stop(); }
+
+bool Timer::HasPeriodPassed(double period) {
+ return m_timer.HasPeriodPassed(units::second_t(period));
+}
+
+double Timer::GetFPGATimestamp() {
+ return frc2::Timer::GetFPGATimestamp().to<double>();
+}
+
+double Timer::GetMatchTime() {
+ return frc2::Timer::GetMatchTime().to<double>();
+}
diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
new file mode 100644
index 0000000..fcd016e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
@@ -0,0 +1,223 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Ultrasonic.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/Counter.h"
+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+#include "frc/Timer.h"
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+// Automatic round robin mode
+std::atomic<bool> Ultrasonic::m_automaticEnabled{false};
+
+std::vector<Ultrasonic*> Ultrasonic::m_sensors;
+std::thread Ultrasonic::m_thread;
+
+Ultrasonic::Ultrasonic(int pingChannel, int echoChannel, DistanceUnit units)
+ : m_pingChannel(std::make_shared<DigitalOutput>(pingChannel)),
+ m_echoChannel(std::make_shared<DigitalInput>(echoChannel)),
+ m_counter(m_echoChannel) {
+ m_units = units;
+ Initialize();
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, m_pingChannel.get());
+ registry.AddChild(this, m_echoChannel.get());
+}
+
+Ultrasonic::Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel,
+ DistanceUnit units)
+ : m_pingChannel(pingChannel, NullDeleter<DigitalOutput>()),
+ m_echoChannel(echoChannel, NullDeleter<DigitalInput>()),
+ m_counter(m_echoChannel) {
+ if (pingChannel == nullptr || echoChannel == nullptr) {
+ wpi_setWPIError(NullParameter);
+ m_units = units;
+ return;
+ }
+ m_units = units;
+ Initialize();
+}
+
+Ultrasonic::Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel,
+ DistanceUnit units)
+ : m_pingChannel(&pingChannel, NullDeleter<DigitalOutput>()),
+ m_echoChannel(&echoChannel, NullDeleter<DigitalInput>()),
+ m_counter(m_echoChannel) {
+ m_units = units;
+ Initialize();
+}
+
+Ultrasonic::Ultrasonic(std::shared_ptr<DigitalOutput> pingChannel,
+ std::shared_ptr<DigitalInput> echoChannel,
+ DistanceUnit units)
+ : m_pingChannel(pingChannel),
+ m_echoChannel(echoChannel),
+ m_counter(m_echoChannel) {
+ m_units = units;
+ Initialize();
+}
+
+Ultrasonic::~Ultrasonic() {
+ // Delete the instance of the ultrasonic sensor by freeing the allocated
+ // digital channels. If the system was in automatic mode (round robin), then
+ // it is stopped, then started again after this sensor is removed (provided
+ // this wasn't the last sensor).
+
+ bool wasAutomaticMode = m_automaticEnabled;
+ SetAutomaticMode(false);
+
+ // No synchronization needed because the background task is stopped.
+ m_sensors.erase(std::remove(m_sensors.begin(), m_sensors.end(), this),
+ m_sensors.end());
+
+ if (!m_sensors.empty() && wasAutomaticMode) {
+ SetAutomaticMode(true);
+ }
+}
+
+void Ultrasonic::Ping() {
+ wpi_assert(!m_automaticEnabled);
+
+ // Reset the counter to zero (invalid data now)
+ m_counter.Reset();
+
+ // Do the ping to start getting a single range
+ m_pingChannel->Pulse(kPingTime);
+}
+
+bool Ultrasonic::IsRangeValid() const {
+ if (m_simRangeValid) return m_simRangeValid.Get();
+ return m_counter.Get() > 1;
+}
+
+void Ultrasonic::SetAutomaticMode(bool enabling) {
+ if (enabling == m_automaticEnabled) return; // ignore the case of no change
+
+ m_automaticEnabled = enabling;
+
+ if (enabling) {
+ /* Clear all the counters so no data is valid. No synchronization is needed
+ * because the background task is stopped.
+ */
+ for (auto& sensor : m_sensors) {
+ sensor->m_counter.Reset();
+ }
+
+ m_thread = std::thread(&Ultrasonic::UltrasonicChecker);
+
+ // TODO: Currently, lvuser does not have permissions to set task priorities.
+ // Until that is the case, uncommenting this will break user code that calls
+ // Ultrasonic::SetAutomicMode().
+ // m_task.SetPriority(kPriority);
+ } else {
+ // Wait for background task to stop running
+ if (m_thread.joinable()) {
+ m_thread.join();
+ }
+
+ // Clear all the counters (data now invalid) since automatic mode is
+ // disabled. No synchronization is needed because the background task is
+ // stopped.
+ for (auto& sensor : m_sensors) {
+ sensor->m_counter.Reset();
+ }
+ }
+}
+
+double Ultrasonic::GetRangeInches() const {
+ if (IsRangeValid()) {
+ if (m_simRange) return m_simRange.Get();
+ return m_counter.GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
+ } else {
+ return 0;
+ }
+}
+
+double Ultrasonic::GetRangeMM() const { return GetRangeInches() * 25.4; }
+
+bool Ultrasonic::IsEnabled() const { return m_enabled; }
+
+void Ultrasonic::SetEnabled(bool enable) { m_enabled = enable; }
+
+void Ultrasonic::SetDistanceUnits(DistanceUnit units) { m_units = units; }
+
+Ultrasonic::DistanceUnit Ultrasonic::GetDistanceUnits() const {
+ return m_units;
+}
+
+double Ultrasonic::PIDGet() {
+ switch (m_units) {
+ case Ultrasonic::kInches:
+ return GetRangeInches();
+ case Ultrasonic::kMilliMeters:
+ return GetRangeMM();
+ default:
+ return 0.0;
+ }
+}
+
+void Ultrasonic::SetPIDSourceType(PIDSourceType pidSource) {
+ if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
+ m_pidSource = pidSource;
+ }
+}
+
+void Ultrasonic::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Ultrasonic");
+ builder.AddDoubleProperty("Value", [=]() { return GetRangeInches(); },
+ nullptr);
+}
+
+void Ultrasonic::Initialize() {
+ m_simDevice = hal::SimDevice("Ultrasonic", m_echoChannel->GetChannel());
+ if (m_simDevice) {
+ m_simRangeValid = m_simDevice.CreateBoolean("Range Valid", false, true);
+ m_simRange = m_simDevice.CreateDouble("Range (in)", false, 0.0);
+ m_pingChannel->SetSimDevice(m_simDevice);
+ m_echoChannel->SetSimDevice(m_simDevice);
+ }
+
+ bool originalMode = m_automaticEnabled;
+ SetAutomaticMode(false); // Kill task when adding a new sensor
+ // Link this instance on the list
+ m_sensors.emplace_back(this);
+
+ m_counter.SetMaxPeriod(1.0);
+ m_counter.SetSemiPeriodMode(true);
+ m_counter.Reset();
+ m_enabled = true; // Make it available for round robin scheduling
+ SetAutomaticMode(originalMode);
+
+ static int instances = 0;
+ instances++;
+ HAL_Report(HALUsageReporting::kResourceType_Ultrasonic, instances);
+ SendableRegistry::GetInstance().AddLW(this, "Ultrasonic",
+ m_echoChannel->GetChannel());
+}
+
+void Ultrasonic::UltrasonicChecker() {
+ while (m_automaticEnabled) {
+ for (auto& sensor : m_sensors) {
+ if (!m_automaticEnabled) break;
+
+ if (sensor->IsEnabled()) {
+ sensor->m_pingChannel->Pulse(kPingTime); // do the ping
+ }
+
+ Wait(0.1); // wait for ping to return
+ }
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/Utility.cpp b/wpilibc/src/main/native/cpp/Utility.cpp
new file mode 100644
index 0000000..11f6234
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Utility.cpp
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Utility.h"
+
+#ifndef _WIN32
+#include <cxxabi.h>
+#include <execinfo.h>
+#endif
+
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/Path.h>
+#include <wpi/SmallString.h>
+#include <wpi/StackTrace.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/ErrorBase.h"
+
+using namespace frc;
+
+bool wpi_assert_impl(bool conditionValue, const wpi::Twine& conditionText,
+ const wpi::Twine& message, wpi::StringRef fileName,
+ int lineNumber, wpi::StringRef funcName) {
+ if (!conditionValue) {
+ wpi::SmallString<128> locBuf;
+ wpi::raw_svector_ostream locStream(locBuf);
+ locStream << funcName << " [" << wpi::sys::path::filename(fileName) << ":"
+ << lineNumber << "]";
+
+ wpi::SmallString<128> errorBuf;
+ wpi::raw_svector_ostream errorStream(errorBuf);
+
+ errorStream << "Assertion \"" << conditionText << "\" ";
+
+ if (message.isTriviallyEmpty() ||
+ (message.isSingleStringRef() && message.getSingleStringRef().empty())) {
+ errorStream << "failed.\n";
+ } else {
+ errorStream << "failed: " << message << "\n";
+ }
+
+ std::string stack = wpi::GetStackTrace(2);
+
+ // Print the error and send it to the DriverStation
+ HAL_SendError(1, 1, 0, errorBuf.c_str(), locBuf.c_str(), stack.c_str(), 1);
+ }
+
+ return conditionValue;
+}
+
+/**
+ * Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl.
+ *
+ * This should not be called directly; it should only be used by
+ * wpi_assertEqual_impl and wpi_assertNotEqual_impl.
+ */
+void wpi_assertEqual_common_impl(const wpi::Twine& valueA,
+ const wpi::Twine& valueB,
+ const wpi::Twine& equalityType,
+ const wpi::Twine& message,
+ wpi::StringRef fileName, int lineNumber,
+ wpi::StringRef funcName) {
+ wpi::SmallString<128> locBuf;
+ wpi::raw_svector_ostream locStream(locBuf);
+ locStream << funcName << " [" << wpi::sys::path::filename(fileName) << ":"
+ << lineNumber << "]";
+
+ wpi::SmallString<128> errorBuf;
+ wpi::raw_svector_ostream errorStream(errorBuf);
+
+ errorStream << "Assertion \"" << valueA << " " << equalityType << " "
+ << valueB << "\" ";
+
+ if (message.isTriviallyEmpty() ||
+ (message.isSingleStringRef() && message.getSingleStringRef().empty())) {
+ errorStream << "failed.\n";
+ } else {
+ errorStream << "failed: " << message << "\n";
+ }
+
+ std::string trace = wpi::GetStackTrace(3);
+
+ // Print the error and send it to the DriverStation
+ HAL_SendError(1, 1, 0, errorBuf.c_str(), locBuf.c_str(), trace.c_str(), 1);
+}
+
+bool wpi_assertEqual_impl(int valueA, int valueB,
+ const wpi::Twine& valueAString,
+ const wpi::Twine& valueBString,
+ const wpi::Twine& message, wpi::StringRef fileName,
+ int lineNumber, wpi::StringRef funcName) {
+ if (!(valueA == valueB)) {
+ wpi_assertEqual_common_impl(valueAString, valueBString, "==", message,
+ fileName, lineNumber, funcName);
+ }
+ return valueA == valueB;
+}
+
+bool wpi_assertNotEqual_impl(int valueA, int valueB,
+ const wpi::Twine& valueAString,
+ const wpi::Twine& valueBString,
+ const wpi::Twine& message, wpi::StringRef fileName,
+ int lineNumber, wpi::StringRef funcName) {
+ if (!(valueA != valueB)) {
+ wpi_assertEqual_common_impl(valueAString, valueBString, "!=", message,
+ fileName, lineNumber, funcName);
+ }
+ return valueA != valueB;
+}
diff --git a/wpilibc/src/main/native/cpp/Victor.cpp b/wpilibc/src/main/native/cpp/Victor.cpp
new file mode 100644
index 0000000..bce1913
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Victor.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Victor.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+Victor::Victor(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
+ SetPeriodMultiplier(kPeriodMultiplier_2X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ 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
new file mode 100644
index 0000000..221777d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/VictorSP.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/VictorSP.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+VictorSP::VictorSP(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel() + 1);
+ SendableRegistry::GetInstance().SetName(this, "VictorSP", GetChannel());
+}
diff --git a/wpilibc/src/main/native/cpp/Watchdog.cpp b/wpilibc/src/main/native/cpp/Watchdog.cpp
new file mode 100644
index 0000000..e5be4fa
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Watchdog.cpp
@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Watchdog.h"
+
+#include <wpi/Format.h>
+#include <wpi/PriorityQueue.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+constexpr std::chrono::milliseconds Watchdog::kMinPrintPeriod;
+
+class Watchdog::Thread : public wpi::SafeThread {
+ public:
+ template <typename T>
+ struct DerefGreater {
+ constexpr bool operator()(const T& lhs, const T& rhs) const {
+ return *lhs > *rhs;
+ }
+ };
+
+ wpi::PriorityQueue<Watchdog*, std::vector<Watchdog*>, DerefGreater<Watchdog*>>
+ m_watchdogs;
+
+ private:
+ void Main() override;
+};
+
+void Watchdog::Thread::Main() {
+ std::unique_lock lock(m_mutex);
+
+ while (m_active) {
+ if (m_watchdogs.size() > 0) {
+ if (m_cond.wait_until(lock, m_watchdogs.top()->m_expirationTime) ==
+ std::cv_status::timeout) {
+ if (m_watchdogs.size() == 0 ||
+ m_watchdogs.top()->m_expirationTime > hal::fpga_clock::now()) {
+ continue;
+ }
+
+ // If the condition variable timed out, that means a Watchdog timeout
+ // has occurred, so call its timeout function.
+ auto watchdog = m_watchdogs.top();
+ m_watchdogs.pop();
+
+ auto now = hal::fpga_clock::now();
+ if (now - watchdog->m_lastTimeoutPrintTime > kMinPrintPeriod) {
+ watchdog->m_lastTimeoutPrintTime = now;
+ if (!watchdog->m_suppressTimeoutMessage) {
+ wpi::outs() << "Watchdog not fed within "
+ << wpi::format("%.6f",
+ watchdog->m_timeout.count() / 1.0e9)
+ << "s\n";
+ }
+ }
+
+ // Set expiration flag before calling the callback so any manipulation
+ // of the flag in the callback (e.g., calling Disable()) isn't
+ // clobbered.
+ watchdog->m_isExpired = true;
+
+ lock.unlock();
+ watchdog->m_callback();
+ lock.lock();
+ }
+ // Otherwise, a Watchdog removed itself from the queue (it notifies the
+ // scheduler of this) or a spurious wakeup occurred, so just rewait with
+ // the soonest watchdog timeout.
+ } else {
+ m_cond.wait(lock, [&] { return m_watchdogs.size() > 0 || !m_active; });
+ }
+ }
+}
+
+Watchdog::Watchdog(double timeout, std::function<void()> callback)
+ : Watchdog(units::second_t{timeout}, callback) {}
+
+Watchdog::Watchdog(units::second_t timeout, std::function<void()> callback)
+ : m_timeout(timeout), m_callback(callback), m_owner(&GetThreadOwner()) {}
+
+Watchdog::~Watchdog() { Disable(); }
+
+double Watchdog::GetTime() const {
+ return (hal::fpga_clock::now() - m_startTime).count() / 1.0e6;
+}
+
+void Watchdog::SetTimeout(double timeout) {
+ SetTimeout(units::second_t{timeout});
+}
+
+void Watchdog::SetTimeout(units::second_t timeout) {
+ using std::chrono::duration_cast;
+ using std::chrono::microseconds;
+
+ m_startTime = hal::fpga_clock::now();
+ m_epochs.clear();
+
+ // Locks mutex
+ auto thr = m_owner->GetThread();
+ if (!thr) return;
+
+ m_timeout = timeout;
+ m_isExpired = false;
+
+ thr->m_watchdogs.remove(this);
+ m_expirationTime = m_startTime + duration_cast<microseconds>(m_timeout);
+ thr->m_watchdogs.emplace(this);
+ thr->m_cond.notify_all();
+}
+
+double Watchdog::GetTimeout() const {
+ // Locks mutex
+ auto thr = m_owner->GetThread();
+
+ return m_timeout.count() / 1.0e9;
+}
+
+bool Watchdog::IsExpired() const {
+ // Locks mutex
+ auto thr = m_owner->GetThread();
+
+ return m_isExpired;
+}
+
+void Watchdog::AddEpoch(wpi::StringRef epochName) {
+ auto currentTime = hal::fpga_clock::now();
+ m_epochs[epochName] = currentTime - m_startTime;
+ m_startTime = currentTime;
+}
+
+void Watchdog::PrintEpochs() {
+ auto now = hal::fpga_clock::now();
+ if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
+ m_lastEpochsPrintTime = now;
+ for (const auto& epoch : m_epochs) {
+ wpi::outs() << '\t' << epoch.getKey() << ": "
+ << wpi::format("%.6f", epoch.getValue().count() / 1.0e6)
+ << "s\n";
+ }
+ }
+}
+
+void Watchdog::Reset() { Enable(); }
+
+void Watchdog::Enable() {
+ using std::chrono::duration_cast;
+ using std::chrono::microseconds;
+
+ m_startTime = hal::fpga_clock::now();
+ m_epochs.clear();
+
+ // Locks mutex
+ auto thr = m_owner->GetThread();
+ if (!thr) return;
+
+ m_isExpired = false;
+
+ thr->m_watchdogs.remove(this);
+ m_expirationTime = m_startTime + duration_cast<microseconds>(m_timeout);
+ thr->m_watchdogs.emplace(this);
+ thr->m_cond.notify_all();
+}
+
+void Watchdog::Disable() {
+ // Locks mutex
+ auto thr = m_owner->GetThread();
+ if (!thr) return;
+
+ thr->m_watchdogs.remove(this);
+ thr->m_cond.notify_all();
+}
+
+void Watchdog::SuppressTimeoutMessage(bool suppress) {
+ m_suppressTimeoutMessage = suppress;
+}
+
+bool Watchdog::operator>(const Watchdog& rhs) {
+ return m_expirationTime > rhs.m_expirationTime;
+}
+
+wpi::SafeThreadOwner<Watchdog::Thread>& Watchdog::GetThreadOwner() {
+ static wpi::SafeThreadOwner<Thread> inst = [] {
+ wpi::SafeThreadOwner<Watchdog::Thread> inst;
+ inst.Start();
+ return inst;
+ }();
+ return inst;
+}
diff --git a/wpilibc/src/main/native/cpp/XboxController.cpp b/wpilibc/src/main/native/cpp/XboxController.cpp
new file mode 100644
index 0000000..b294257
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/XboxController.cpp
@@ -0,0 +1,160 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/XboxController.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+XboxController::XboxController(int port) : GenericHID(port) {
+ HAL_Report(HALUsageReporting::kResourceType_XboxController, port + 1);
+}
+
+double XboxController::GetX(JoystickHand hand) const {
+ if (hand == kLeftHand) {
+ return GetRawAxis(0);
+ } else {
+ return GetRawAxis(4);
+ }
+}
+
+double XboxController::GetY(JoystickHand hand) const {
+ if (hand == kLeftHand) {
+ return GetRawAxis(1);
+ } else {
+ return GetRawAxis(5);
+ }
+}
+
+double XboxController::GetTriggerAxis(JoystickHand hand) const {
+ if (hand == kLeftHand) {
+ return GetRawAxis(2);
+ } else {
+ return GetRawAxis(3);
+ }
+}
+
+bool XboxController::GetBumper(JoystickHand hand) const {
+ if (hand == kLeftHand) {
+ return GetRawButton(static_cast<int>(Button::kBumperLeft));
+ } else {
+ return GetRawButton(static_cast<int>(Button::kBumperRight));
+ }
+}
+
+bool XboxController::GetBumperPressed(JoystickHand hand) {
+ if (hand == kLeftHand) {
+ return GetRawButtonPressed(static_cast<int>(Button::kBumperLeft));
+ } else {
+ return GetRawButtonPressed(static_cast<int>(Button::kBumperRight));
+ }
+}
+
+bool XboxController::GetBumperReleased(JoystickHand hand) {
+ if (hand == kLeftHand) {
+ return GetRawButtonReleased(static_cast<int>(Button::kBumperLeft));
+ } else {
+ return GetRawButtonReleased(static_cast<int>(Button::kBumperRight));
+ }
+}
+
+bool XboxController::GetStickButton(JoystickHand hand) const {
+ if (hand == kLeftHand) {
+ return GetRawButton(static_cast<int>(Button::kStickLeft));
+ } else {
+ return GetRawButton(static_cast<int>(Button::kStickRight));
+ }
+}
+
+bool XboxController::GetStickButtonPressed(JoystickHand hand) {
+ if (hand == kLeftHand) {
+ return GetRawButtonPressed(static_cast<int>(Button::kStickLeft));
+ } else {
+ return GetRawButtonPressed(static_cast<int>(Button::kStickRight));
+ }
+}
+
+bool XboxController::GetStickButtonReleased(JoystickHand hand) {
+ if (hand == kLeftHand) {
+ return GetRawButtonReleased(static_cast<int>(Button::kStickLeft));
+ } else {
+ return GetRawButtonReleased(static_cast<int>(Button::kStickRight));
+ }
+}
+
+bool XboxController::GetAButton() const {
+ return GetRawButton(static_cast<int>(Button::kA));
+}
+
+bool XboxController::GetAButtonPressed() {
+ return GetRawButtonPressed(static_cast<int>(Button::kA));
+}
+
+bool XboxController::GetAButtonReleased() {
+ return GetRawButtonReleased(static_cast<int>(Button::kA));
+}
+
+bool XboxController::GetBButton() const {
+ return GetRawButton(static_cast<int>(Button::kB));
+}
+
+bool XboxController::GetBButtonPressed() {
+ return GetRawButtonPressed(static_cast<int>(Button::kB));
+}
+
+bool XboxController::GetBButtonReleased() {
+ return GetRawButtonReleased(static_cast<int>(Button::kB));
+}
+
+bool XboxController::GetXButton() const {
+ return GetRawButton(static_cast<int>(Button::kX));
+}
+
+bool XboxController::GetXButtonPressed() {
+ return GetRawButtonPressed(static_cast<int>(Button::kX));
+}
+
+bool XboxController::GetXButtonReleased() {
+ return GetRawButtonReleased(static_cast<int>(Button::kX));
+}
+
+bool XboxController::GetYButton() const {
+ return GetRawButton(static_cast<int>(Button::kY));
+}
+
+bool XboxController::GetYButtonPressed() {
+ return GetRawButtonPressed(static_cast<int>(Button::kY));
+}
+
+bool XboxController::GetYButtonReleased() {
+ return GetRawButtonReleased(static_cast<int>(Button::kY));
+}
+
+bool XboxController::GetBackButton() const {
+ return GetRawButton(static_cast<int>(Button::kBack));
+}
+
+bool XboxController::GetBackButtonPressed() {
+ return GetRawButtonPressed(static_cast<int>(Button::kBack));
+}
+
+bool XboxController::GetBackButtonReleased() {
+ return GetRawButtonReleased(static_cast<int>(Button::kBack));
+}
+
+bool XboxController::GetStartButton() const {
+ return GetRawButton(static_cast<int>(Button::kStart));
+}
+
+bool XboxController::GetStartButtonPressed() {
+ return GetRawButtonPressed(static_cast<int>(Button::kStart));
+}
+
+bool XboxController::GetStartButtonReleased() {
+ return GetRawButtonReleased(static_cast<int>(Button::kStart));
+}
diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
new file mode 100644
index 0000000..3abd4ba
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/PIDController.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc2;
+
+PIDController::PIDController(double Kp, double Ki, double Kd,
+ units::second_t period)
+ : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+ static int instances = 0;
+ instances++;
+ HAL_Report(HALUsageReporting::kResourceType_PIDController2, instances);
+ frc::SendableRegistry::GetInstance().Add(this, "PIDController", instances);
+}
+
+void PIDController::SetP(double Kp) { m_Kp = Kp; }
+
+void PIDController::SetI(double Ki) { m_Ki = Ki; }
+
+void PIDController::SetD(double Kd) { m_Kd = Kd; }
+
+double PIDController::GetP() const { return m_Kp; }
+
+double PIDController::GetI() const { return m_Ki; }
+
+double PIDController::GetD() const { return m_Kd; }
+
+units::second_t PIDController::GetPeriod() const {
+ return units::second_t(m_period);
+}
+
+void PIDController::SetSetpoint(double setpoint) {
+ if (m_maximumInput > m_minimumInput) {
+ m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput);
+ } else {
+ m_setpoint = setpoint;
+ }
+}
+
+double PIDController::GetSetpoint() const { return m_setpoint; }
+
+bool PIDController::AtSetpoint() const {
+ return std::abs(m_positionError) < m_positionTolerance &&
+ std::abs(m_velocityError) < m_velocityTolerance;
+}
+
+void PIDController::EnableContinuousInput(double minimumInput,
+ double maximumInput) {
+ m_continuous = true;
+ SetInputRange(minimumInput, maximumInput);
+}
+
+void PIDController::DisableContinuousInput() { m_continuous = false; }
+
+void PIDController::SetIntegratorRange(double minimumIntegral,
+ double maximumIntegral) {
+ m_minimumIntegral = minimumIntegral;
+ m_maximumIntegral = maximumIntegral;
+}
+
+void PIDController::SetTolerance(double positionTolerance,
+ double velocityTolerance) {
+ m_positionTolerance = positionTolerance;
+ m_velocityTolerance = velocityTolerance;
+}
+
+double PIDController::GetPositionError() const {
+ return GetContinuousError(m_positionError);
+}
+
+double PIDController::GetVelocityError() const { return m_velocityError; }
+
+double PIDController::Calculate(double measurement) {
+ m_prevError = m_positionError;
+ m_positionError = GetContinuousError(m_setpoint - measurement);
+ m_velocityError = (m_positionError - m_prevError) / m_period.to<double>();
+
+ if (m_Ki != 0) {
+ m_totalError =
+ std::clamp(m_totalError + m_positionError * m_period.to<double>(),
+ m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
+ }
+
+ return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError;
+}
+
+double PIDController::Calculate(double measurement, double setpoint) {
+ // Set setpoint to provided value
+ SetSetpoint(setpoint);
+ return Calculate(measurement);
+}
+
+void PIDController::Reset() {
+ m_prevError = 0;
+ m_totalError = 0;
+}
+
+void PIDController::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("PIDController");
+ builder.AddDoubleProperty("p", [this] { return GetP(); },
+ [this](double value) { SetP(value); });
+ builder.AddDoubleProperty("i", [this] { return GetI(); },
+ [this](double value) { SetI(value); });
+ builder.AddDoubleProperty("d", [this] { return GetD(); },
+ [this](double value) { SetD(value); });
+ builder.AddDoubleProperty("setpoint", [this] { return GetSetpoint(); },
+ [this](double value) { SetSetpoint(value); });
+}
+
+double PIDController::GetContinuousError(double error) const {
+ if (m_continuous && m_inputRange > 0) {
+ error = std::fmod(error, m_inputRange);
+ if (std::abs(error) > m_inputRange / 2) {
+ if (error > 0) {
+ return error - m_inputRange;
+ } else {
+ return error + m_inputRange;
+ }
+ }
+ }
+
+ return error;
+}
+
+void PIDController::SetInputRange(double minimumInput, double maximumInput) {
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+ m_inputRange = maximumInput - minimumInput;
+
+ // Clamp setpoint to new input range
+ if (m_maximumInput > m_minimumInput) {
+ m_setpoint = std::clamp(m_setpoint, m_minimumInput, m_maximumInput);
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
new file mode 100644
index 0000000..a0fffc6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/ProfiledPIDController.h"
+
+#include <hal/FRCUsageReporting.h>
+
+void frc::detail::ReportProfiledPIDController() {
+ static int instances = 0;
+ ++instances;
+ HAL_Report(HALUsageReporting::kResourceType_ProfiledPIDController, instances);
+}
diff --git a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
new file mode 100644
index 0000000..c55c2e8
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/RamseteController.h"
+
+#include <cmath>
+
+using namespace frc;
+
+/**
+ * Returns sin(x) / x.
+ *
+ * @param x Value of which to take sinc(x).
+ */
+static double Sinc(double x) {
+ if (std::abs(x) < 1e-9) {
+ return 1.0 - 1.0 / 6.0 * x * x;
+ } else {
+ return std::sin(x) / x;
+ }
+}
+
+RamseteController::RamseteController(double b, double zeta)
+ : m_b{b}, m_zeta{zeta} {}
+
+bool RamseteController::AtReference() const {
+ const auto& eTranslate = m_poseError.Translation();
+ const auto& eRotate = m_poseError.Rotation();
+ const auto& tolTranslate = m_poseTolerance.Translation();
+ const auto& tolRotate = m_poseTolerance.Rotation();
+ return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+ units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+ units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+}
+
+void RamseteController::SetTolerance(const Pose2d& poseTolerance) {
+ m_poseTolerance = poseTolerance;
+}
+
+ChassisSpeeds RamseteController::Calculate(
+ const Pose2d& currentPose, const Pose2d& poseRef,
+ units::meters_per_second_t linearVelocityRef,
+ units::radians_per_second_t angularVelocityRef) {
+ m_poseError = poseRef.RelativeTo(currentPose);
+
+ // Aliases for equation readability
+ double eX = m_poseError.Translation().X().to<double>();
+ double eY = m_poseError.Translation().Y().to<double>();
+ double eTheta = m_poseError.Rotation().Radians().to<double>();
+ double vRef = linearVelocityRef.to<double>();
+ double omegaRef = angularVelocityRef.to<double>();
+
+ double k =
+ 2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2));
+
+ units::meters_per_second_t v{vRef * m_poseError.Rotation().Cos() + k * eX};
+ units::radians_per_second_t omega{omegaRef + k * eTheta +
+ m_b * vRef * Sinc(eTheta) * eY};
+ return ChassisSpeeds{v, 0_mps, omega};
+}
+
+ChassisSpeeds RamseteController::Calculate(
+ const Pose2d& currentPose, const Trajectory::State& desiredState) {
+ return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+ desiredState.velocity * desiredState.curvature);
+}
diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
new file mode 100644
index 0000000..2d30a2b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/drive/DifferentialDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/SpeedController.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+DifferentialDrive::DifferentialDrive(SpeedController& leftMotor,
+ SpeedController& rightMotor)
+ : m_leftMotor(&leftMotor), m_rightMotor(&rightMotor) {
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, m_leftMotor);
+ registry.AddChild(this, m_rightMotor);
+ static int instances = 0;
+ ++instances;
+ registry.AddLW(this, "DifferentialDrive", instances);
+}
+
+void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
+ bool squareInputs) {
+ static bool reported = false;
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_DifferentialArcade, 2);
+ reported = true;
+ }
+
+ xSpeed = std::clamp(xSpeed, -1.0, 1.0);
+ xSpeed = ApplyDeadband(xSpeed, m_deadband);
+
+ zRotation = std::clamp(zRotation, -1.0, 1.0);
+ zRotation = ApplyDeadband(zRotation, m_deadband);
+
+ // Square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power.
+ if (squareInputs) {
+ xSpeed = std::copysign(xSpeed * xSpeed, xSpeed);
+ zRotation = std::copysign(zRotation * zRotation, zRotation);
+ }
+
+ double leftMotorOutput;
+ double rightMotorOutput;
+
+ double maxInput =
+ std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
+
+ if (xSpeed >= 0.0) {
+ // First quadrant, else second quadrant
+ if (zRotation >= 0.0) {
+ leftMotorOutput = maxInput;
+ rightMotorOutput = xSpeed - zRotation;
+ } else {
+ leftMotorOutput = xSpeed + zRotation;
+ rightMotorOutput = maxInput;
+ }
+ } else {
+ // Third quadrant, else fourth quadrant
+ if (zRotation >= 0.0) {
+ leftMotorOutput = xSpeed + zRotation;
+ rightMotorOutput = maxInput;
+ } else {
+ leftMotorOutput = maxInput;
+ rightMotorOutput = xSpeed - zRotation;
+ }
+ }
+
+ m_leftMotor->Set(std::clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
+ double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
+ m_rightMotor->Set(std::clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
+
+ Feed();
+}
+
+void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
+ bool isQuickTurn) {
+ static bool reported = false;
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_DifferentialCurvature, 2);
+ reported = true;
+ }
+
+ xSpeed = std::clamp(xSpeed, -1.0, 1.0);
+ xSpeed = ApplyDeadband(xSpeed, m_deadband);
+
+ zRotation = std::clamp(zRotation, -1.0, 1.0);
+ zRotation = ApplyDeadband(zRotation, m_deadband);
+
+ double angularPower;
+ bool overPower;
+
+ if (isQuickTurn) {
+ if (std::abs(xSpeed) < m_quickStopThreshold) {
+ m_quickStopAccumulator =
+ (1 - m_quickStopAlpha) * m_quickStopAccumulator +
+ m_quickStopAlpha * std::clamp(zRotation, -1.0, 1.0) * 2;
+ }
+ overPower = true;
+ angularPower = zRotation;
+ } else {
+ overPower = false;
+ angularPower = std::abs(xSpeed) * zRotation - m_quickStopAccumulator;
+
+ if (m_quickStopAccumulator > 1) {
+ m_quickStopAccumulator -= 1;
+ } else if (m_quickStopAccumulator < -1) {
+ m_quickStopAccumulator += 1;
+ } else {
+ m_quickStopAccumulator = 0.0;
+ }
+ }
+
+ double leftMotorOutput = xSpeed + angularPower;
+ double rightMotorOutput = xSpeed - angularPower;
+
+ // If rotation is overpowered, reduce both outputs to within acceptable range
+ if (overPower) {
+ if (leftMotorOutput > 1.0) {
+ rightMotorOutput -= leftMotorOutput - 1.0;
+ leftMotorOutput = 1.0;
+ } else if (rightMotorOutput > 1.0) {
+ leftMotorOutput -= rightMotorOutput - 1.0;
+ rightMotorOutput = 1.0;
+ } else if (leftMotorOutput < -1.0) {
+ rightMotorOutput -= leftMotorOutput + 1.0;
+ leftMotorOutput = -1.0;
+ } else if (rightMotorOutput < -1.0) {
+ leftMotorOutput -= rightMotorOutput + 1.0;
+ rightMotorOutput = -1.0;
+ }
+ }
+
+ // Normalize the wheel speeds
+ double maxMagnitude =
+ std::max(std::abs(leftMotorOutput), std::abs(rightMotorOutput));
+ if (maxMagnitude > 1.0) {
+ leftMotorOutput /= maxMagnitude;
+ rightMotorOutput /= maxMagnitude;
+ }
+
+ m_leftMotor->Set(leftMotorOutput * m_maxOutput);
+ m_rightMotor->Set(rightMotorOutput * m_maxOutput *
+ m_rightSideInvertMultiplier);
+
+ Feed();
+}
+
+void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
+ bool squareInputs) {
+ static bool reported = false;
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_DifferentialTank, 2);
+ reported = true;
+ }
+
+ leftSpeed = std::clamp(leftSpeed, -1.0, 1.0);
+ leftSpeed = ApplyDeadband(leftSpeed, m_deadband);
+
+ rightSpeed = std::clamp(rightSpeed, -1.0, 1.0);
+ rightSpeed = ApplyDeadband(rightSpeed, m_deadband);
+
+ // Square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power.
+ if (squareInputs) {
+ leftSpeed = std::copysign(leftSpeed * leftSpeed, leftSpeed);
+ rightSpeed = std::copysign(rightSpeed * rightSpeed, rightSpeed);
+ }
+
+ m_leftMotor->Set(leftSpeed * m_maxOutput);
+ m_rightMotor->Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
+
+ Feed();
+}
+
+void DifferentialDrive::SetQuickStopThreshold(double threshold) {
+ m_quickStopThreshold = threshold;
+}
+
+void DifferentialDrive::SetQuickStopAlpha(double alpha) {
+ m_quickStopAlpha = alpha;
+}
+
+bool DifferentialDrive::IsRightSideInverted() const {
+ return m_rightSideInvertMultiplier == -1.0;
+}
+
+void DifferentialDrive::SetRightSideInverted(bool rightSideInverted) {
+ m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+}
+
+void DifferentialDrive::StopMotor() {
+ m_leftMotor->StopMotor();
+ m_rightMotor->StopMotor();
+ Feed();
+}
+
+void DifferentialDrive::GetDescription(wpi::raw_ostream& desc) const {
+ desc << "DifferentialDrive";
+}
+
+void DifferentialDrive::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("DifferentialDrive");
+ builder.SetActuator(true);
+ builder.SetSafeState([=] { StopMotor(); });
+ builder.AddDoubleProperty("Left Motor Speed",
+ [=]() { return m_leftMotor->Get(); },
+ [=](double value) { m_leftMotor->Set(value); });
+ builder.AddDoubleProperty(
+ "Right Motor Speed",
+ [=]() { return m_rightMotor->Get() * m_rightSideInvertMultiplier; },
+ [=](double value) {
+ m_rightMotor->Set(value * m_rightSideInvertMultiplier);
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
new file mode 100644
index 0000000..e03951b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/drive/KilloughDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/FRCUsageReporting.h>
+#include <wpi/math>
+
+#include "frc/SpeedController.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+KilloughDrive::KilloughDrive(SpeedController& leftMotor,
+ SpeedController& rightMotor,
+ SpeedController& backMotor)
+ : KilloughDrive(leftMotor, rightMotor, backMotor, kDefaultLeftMotorAngle,
+ kDefaultRightMotorAngle, kDefaultBackMotorAngle) {}
+
+KilloughDrive::KilloughDrive(SpeedController& leftMotor,
+ SpeedController& rightMotor,
+ SpeedController& backMotor, double leftMotorAngle,
+ double rightMotorAngle, double backMotorAngle)
+ : m_leftMotor(&leftMotor),
+ m_rightMotor(&rightMotor),
+ m_backMotor(&backMotor) {
+ m_leftVec = {std::cos(leftMotorAngle * (wpi::math::pi / 180.0)),
+ std::sin(leftMotorAngle * (wpi::math::pi / 180.0))};
+ m_rightVec = {std::cos(rightMotorAngle * (wpi::math::pi / 180.0)),
+ std::sin(rightMotorAngle * (wpi::math::pi / 180.0))};
+ m_backVec = {std::cos(backMotorAngle * (wpi::math::pi / 180.0)),
+ std::sin(backMotorAngle * (wpi::math::pi / 180.0))};
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, m_leftMotor);
+ registry.AddChild(this, m_rightMotor);
+ registry.AddChild(this, m_backMotor);
+ static int instances = 0;
+ ++instances;
+ registry.AddLW(this, "KilloughDrive", instances);
+}
+
+void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
+ double zRotation, double gyroAngle) {
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_KilloughCartesian, 3);
+ reported = true;
+ }
+
+ ySpeed = std::clamp(ySpeed, -1.0, 1.0);
+ ySpeed = ApplyDeadband(ySpeed, m_deadband);
+
+ xSpeed = std::clamp(xSpeed, -1.0, 1.0);
+ xSpeed = ApplyDeadband(xSpeed, m_deadband);
+
+ // Compensate for gyro angle.
+ Vector2d input{ySpeed, xSpeed};
+ input.Rotate(-gyroAngle);
+
+ double wheelSpeeds[3];
+ wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + zRotation;
+ wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
+ wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
+
+ Normalize(wheelSpeeds);
+
+ m_leftMotor->Set(wheelSpeeds[kLeft] * m_maxOutput);
+ m_rightMotor->Set(wheelSpeeds[kRight] * m_maxOutput);
+ m_backMotor->Set(wheelSpeeds[kBack] * m_maxOutput);
+
+ Feed();
+}
+
+void KilloughDrive::DrivePolar(double magnitude, double angle,
+ double zRotation) {
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_KilloughPolar, 3);
+ reported = true;
+ }
+
+ DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)),
+ magnitude * std::cos(angle * (wpi::math::pi / 180.0)),
+ zRotation, 0.0);
+}
+
+void KilloughDrive::StopMotor() {
+ m_leftMotor->StopMotor();
+ m_rightMotor->StopMotor();
+ m_backMotor->StopMotor();
+ Feed();
+}
+
+void KilloughDrive::GetDescription(wpi::raw_ostream& desc) const {
+ desc << "KilloughDrive";
+}
+
+void KilloughDrive::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("KilloughDrive");
+ builder.SetActuator(true);
+ builder.SetSafeState([=] { StopMotor(); });
+ builder.AddDoubleProperty("Left Motor Speed",
+ [=]() { return m_leftMotor->Get(); },
+ [=](double value) { m_leftMotor->Set(value); });
+ builder.AddDoubleProperty("Right Motor Speed",
+ [=]() { return m_rightMotor->Get(); },
+ [=](double value) { m_rightMotor->Set(value); });
+ builder.AddDoubleProperty("Back Motor Speed",
+ [=]() { return m_backMotor->Get(); },
+ [=](double value) { m_backMotor->Set(value); });
+}
diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
new file mode 100644
index 0000000..0102d6a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/drive/MecanumDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/FRCUsageReporting.h>
+#include <wpi/math>
+
+#include "frc/SpeedController.h"
+#include "frc/drive/Vector2d.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor,
+ SpeedController& rearLeftMotor,
+ SpeedController& frontRightMotor,
+ SpeedController& rearRightMotor)
+ : m_frontLeftMotor(&frontLeftMotor),
+ m_rearLeftMotor(&rearLeftMotor),
+ m_frontRightMotor(&frontRightMotor),
+ m_rearRightMotor(&rearRightMotor) {
+ auto& registry = SendableRegistry::GetInstance();
+ registry.AddChild(this, m_frontLeftMotor);
+ registry.AddChild(this, m_rearLeftMotor);
+ registry.AddChild(this, m_frontRightMotor);
+ registry.AddChild(this, m_rearRightMotor);
+ static int instances = 0;
+ ++instances;
+ registry.AddLW(this, "MecanumDrive", instances);
+}
+
+void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
+ double zRotation, double gyroAngle) {
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_MecanumCartesian, 4);
+ reported = true;
+ }
+
+ ySpeed = std::clamp(ySpeed, -1.0, 1.0);
+ ySpeed = ApplyDeadband(ySpeed, m_deadband);
+
+ xSpeed = std::clamp(xSpeed, -1.0, 1.0);
+ xSpeed = ApplyDeadband(xSpeed, m_deadband);
+
+ // Compensate for gyro angle.
+ Vector2d input{ySpeed, xSpeed};
+ input.Rotate(-gyroAngle);
+
+ double wheelSpeeds[4];
+ wheelSpeeds[kFrontLeft] = input.x + input.y + zRotation;
+ wheelSpeeds[kFrontRight] = -input.x + input.y - zRotation;
+ wheelSpeeds[kRearLeft] = -input.x + input.y + zRotation;
+ wheelSpeeds[kRearRight] = input.x + input.y - zRotation;
+
+ Normalize(wheelSpeeds);
+
+ m_frontLeftMotor->Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRight] * m_maxOutput *
+ m_rightSideInvertMultiplier);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeft] * m_maxOutput);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRight] * m_maxOutput *
+ m_rightSideInvertMultiplier);
+
+ Feed();
+}
+
+void MecanumDrive::DrivePolar(double magnitude, double angle,
+ double zRotation) {
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
+ HALUsageReporting::kRobotDrive2_MecanumPolar, 4);
+ reported = true;
+ }
+
+ DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)),
+ magnitude * std::cos(angle * (wpi::math::pi / 180.0)),
+ zRotation, 0.0);
+}
+
+bool MecanumDrive::IsRightSideInverted() const {
+ return m_rightSideInvertMultiplier == -1.0;
+}
+
+void MecanumDrive::SetRightSideInverted(bool rightSideInverted) {
+ m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+}
+
+void MecanumDrive::StopMotor() {
+ m_frontLeftMotor->StopMotor();
+ m_frontRightMotor->StopMotor();
+ m_rearLeftMotor->StopMotor();
+ m_rearRightMotor->StopMotor();
+ Feed();
+}
+
+void MecanumDrive::GetDescription(wpi::raw_ostream& desc) const {
+ desc << "MecanumDrive";
+}
+
+void MecanumDrive::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("MecanumDrive");
+ builder.SetActuator(true);
+ builder.SetSafeState([=] { StopMotor(); });
+ builder.AddDoubleProperty(
+ "Front Left Motor Speed", [=]() { return m_frontLeftMotor->Get(); },
+ [=](double value) { m_frontLeftMotor->Set(value); });
+ builder.AddDoubleProperty(
+ "Front Right Motor Speed",
+ [=]() { return m_frontRightMotor->Get() * m_rightSideInvertMultiplier; },
+ [=](double value) {
+ m_frontRightMotor->Set(value * m_rightSideInvertMultiplier);
+ });
+ builder.AddDoubleProperty("Rear Left Motor Speed",
+ [=]() { return m_rearLeftMotor->Get(); },
+ [=](double value) { m_rearLeftMotor->Set(value); });
+ builder.AddDoubleProperty(
+ "Rear Right Motor Speed",
+ [=]() { return m_rearRightMotor->Get() * m_rightSideInvertMultiplier; },
+ [=](double value) {
+ m_rearRightMotor->Set(value * m_rightSideInvertMultiplier);
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
new file mode 100644
index 0000000..fbce5a1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/drive/RobotDriveBase.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/Base.h"
+#include "frc/SpeedController.h"
+
+using namespace frc;
+
+RobotDriveBase::RobotDriveBase() { SetSafetyEnabled(true); }
+
+void RobotDriveBase::SetDeadband(double deadband) { m_deadband = deadband; }
+
+void RobotDriveBase::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
+
+void RobotDriveBase::FeedWatchdog() { Feed(); }
+
+double RobotDriveBase::ApplyDeadband(double value, double deadband) {
+ if (std::abs(value) > deadband) {
+ if (value > 0.0) {
+ return (value - deadband) / (1.0 - deadband);
+ } else {
+ return (value + deadband) / (1.0 - deadband);
+ }
+ } else {
+ return 0.0;
+ }
+}
+
+void RobotDriveBase::Normalize(wpi::MutableArrayRef<double> wheelSpeeds) {
+ double maxMagnitude = std::abs(wheelSpeeds[0]);
+ for (size_t i = 1; i < wheelSpeeds.size(); i++) {
+ double temp = std::abs(wheelSpeeds[i]);
+ if (maxMagnitude < temp) {
+ maxMagnitude = temp;
+ }
+ }
+ if (maxMagnitude > 1.0) {
+ for (size_t i = 0; i < wheelSpeeds.size(); i++) {
+ wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+ }
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/drive/Vector2d.cpp b/wpilibc/src/main/native/cpp/drive/Vector2d.cpp
new file mode 100644
index 0000000..a6e68a6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/drive/Vector2d.cpp
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/drive/Vector2d.h"
+
+#include <cmath>
+
+#include <wpi/math>
+
+using namespace frc;
+
+Vector2d::Vector2d(double x, double y) {
+ this->x = x;
+ this->y = y;
+}
+
+void Vector2d::Rotate(double angle) {
+ double cosA = std::cos(angle * (wpi::math::pi / 180.0));
+ double sinA = std::sin(angle * (wpi::math::pi / 180.0));
+ double out[2];
+ out[0] = x * cosA - y * sinA;
+ out[1] = x * sinA + y * cosA;
+ x = out[0];
+ y = out[1];
+}
+
+double Vector2d::Dot(const Vector2d& vec) const {
+ return x * vec.x + y * vec.y;
+}
+
+double Vector2d::Magnitude() const { return std::sqrt(x * x + y * y); }
+
+double Vector2d::ScalarProject(const Vector2d& vec) const {
+ return Dot(vec) / vec.Magnitude();
+}
diff --git a/wpilibc/src/main/native/cpp/filters/Filter.cpp b/wpilibc/src/main/native/cpp/filters/Filter.cpp
new file mode 100644
index 0000000..c25d7af
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/filters/Filter.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/filters/Filter.h"
+
+#include "frc/Base.h"
+
+using namespace frc;
+
+Filter::Filter(PIDSource& source)
+ : m_source(std::shared_ptr<PIDSource>(&source, NullDeleter<PIDSource>())) {}
+
+Filter::Filter(std::shared_ptr<PIDSource> source)
+ : m_source(std::move(source)) {}
+
+void Filter::SetPIDSourceType(PIDSourceType pidSource) {
+ m_source->SetPIDSourceType(pidSource);
+}
+
+PIDSourceType Filter::GetPIDSourceType() const {
+ return m_source->GetPIDSourceType();
+}
+
+double Filter::PIDGetSource() { return m_source->PIDGet(); }
diff --git a/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
new file mode 100644
index 0000000..434cc27
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* 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/filters/LinearDigitalFilter.h"
+
+#include <cassert>
+#include <cmath>
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+LinearDigitalFilter::LinearDigitalFilter(PIDSource& source,
+ wpi::ArrayRef<double> ffGains,
+ wpi::ArrayRef<double> fbGains)
+ : Filter(source),
+ 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);
+}
+
+LinearDigitalFilter::LinearDigitalFilter(PIDSource& source,
+ std::initializer_list<double> ffGains,
+ std::initializer_list<double> fbGains)
+ : LinearDigitalFilter(source,
+ wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
+ wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
+
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ wpi::ArrayRef<double> ffGains,
+ wpi::ArrayRef<double> fbGains)
+ : Filter(source),
+ 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);
+}
+
+LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
+ std::initializer_list<double> ffGains,
+ std::initializer_list<double> fbGains)
+ : LinearDigitalFilter(source,
+ wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
+ wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
+
+LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(PIDSource& source,
+ double timeConstant,
+ double period) {
+ double gain = std::exp(-period / timeConstant);
+ return LinearDigitalFilter(source, {1.0 - gain}, {-gain});
+}
+
+LinearDigitalFilter LinearDigitalFilter::HighPass(PIDSource& source,
+ double timeConstant,
+ double period) {
+ double gain = std::exp(-period / timeConstant);
+ return LinearDigitalFilter(source, {gain, -gain}, {-gain});
+}
+
+LinearDigitalFilter LinearDigitalFilter::MovingAverage(PIDSource& source,
+ int taps) {
+ assert(taps > 0);
+
+ std::vector<double> gains(taps, 1.0 / taps);
+ return LinearDigitalFilter(source, gains, {});
+}
+
+LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(
+ std::shared_ptr<PIDSource> source, double timeConstant, double period) {
+ double gain = std::exp(-period / timeConstant);
+ return LinearDigitalFilter(std::move(source), {1.0 - gain}, {-gain});
+}
+
+LinearDigitalFilter LinearDigitalFilter::HighPass(
+ std::shared_ptr<PIDSource> source, double timeConstant, double period) {
+ double gain = std::exp(-period / timeConstant);
+ return LinearDigitalFilter(std::move(source), {gain, -gain}, {-gain});
+}
+
+LinearDigitalFilter LinearDigitalFilter::MovingAverage(
+ std::shared_ptr<PIDSource> source, int taps) {
+ assert(taps > 0);
+
+ std::vector<double> gains(taps, 1.0 / taps);
+ return LinearDigitalFilter(std::move(source), gains, {});
+}
+
+double LinearDigitalFilter::Get() const {
+ double retVal = 0.0;
+
+ // 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];
+ }
+
+ return retVal;
+}
+
+void LinearDigitalFilter::Reset() {
+ m_inputs.reset();
+ m_outputs.reset();
+}
+
+double LinearDigitalFilter::PIDGet() {
+ double retVal = 0.0;
+
+ // Rotate the inputs
+ m_inputs.push_front(PIDGetSource());
+
+ // 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/frc2/Timer.cpp b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
new file mode 100644
index 0000000..76721bc
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc2/Timer.h"
+
+#include <chrono>
+#include <thread>
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/DriverStation.h"
+#include "frc/RobotController.h"
+
+namespace frc2 {
+
+void Wait(units::second_t seconds) {
+ std::this_thread::sleep_for(
+ std::chrono::duration<double>(seconds.to<double>()));
+}
+
+units::second_t GetTime() {
+ using std::chrono::duration;
+ using std::chrono::duration_cast;
+ using std::chrono::system_clock;
+
+ return units::second_t(
+ duration_cast<duration<double>>(system_clock::now().time_since_epoch())
+ .count());
+}
+
+} // namespace frc2
+
+using namespace frc2;
+
+// for compatibility with msvc12--see C2864
+const units::second_t Timer::kRolloverTime = units::second_t((1ll << 32) / 1e6);
+
+Timer::Timer() { Reset(); }
+
+Timer::Timer(const Timer& rhs)
+ : m_startTime(rhs.m_startTime),
+ m_accumulatedTime(rhs.m_accumulatedTime),
+ m_running(rhs.m_running) {}
+
+Timer& Timer::operator=(const Timer& rhs) {
+ std::scoped_lock lock(m_mutex, rhs.m_mutex);
+
+ m_startTime = rhs.m_startTime;
+ m_accumulatedTime = rhs.m_accumulatedTime;
+ m_running = rhs.m_running;
+
+ return *this;
+}
+
+Timer::Timer(Timer&& rhs)
+ : m_startTime(std::move(rhs.m_startTime)),
+ m_accumulatedTime(std::move(rhs.m_accumulatedTime)),
+ m_running(std::move(rhs.m_running)) {}
+
+Timer& Timer::operator=(Timer&& rhs) {
+ std::scoped_lock lock(m_mutex, rhs.m_mutex);
+
+ m_startTime = std::move(rhs.m_startTime);
+ m_accumulatedTime = std::move(rhs.m_accumulatedTime);
+ m_running = std::move(rhs.m_running);
+
+ return *this;
+}
+
+units::second_t Timer::Get() const {
+ units::second_t result;
+ units::second_t currentTime = GetFPGATimestamp();
+
+ std::scoped_lock lock(m_mutex);
+ if (m_running) {
+ // If the current time is before the start time, then the FPGA clock rolled
+ // over. Compensate by adding the ~71 minutes that it takes to roll over to
+ // the current time.
+ if (currentTime < m_startTime) {
+ currentTime += kRolloverTime;
+ }
+
+ result = (currentTime - m_startTime) + m_accumulatedTime;
+ } else {
+ result = m_accumulatedTime;
+ }
+
+ return result;
+}
+
+void Timer::Reset() {
+ std::scoped_lock lock(m_mutex);
+ m_accumulatedTime = 0_s;
+ m_startTime = GetFPGATimestamp();
+}
+
+void Timer::Start() {
+ std::scoped_lock lock(m_mutex);
+ if (!m_running) {
+ m_startTime = GetFPGATimestamp();
+ m_running = true;
+ }
+}
+
+void Timer::Stop() {
+ units::second_t temp = Get();
+
+ std::scoped_lock lock(m_mutex);
+ if (m_running) {
+ m_accumulatedTime = temp;
+ m_running = false;
+ }
+}
+
+bool Timer::HasPeriodPassed(units::second_t period) {
+ if (Get() > period) {
+ std::scoped_lock lock(m_mutex);
+ // Advance the start time by the period.
+ m_startTime += period;
+ // Don't set it to the current time... we want to avoid drift.
+ return true;
+ }
+ return false;
+}
+
+units::second_t Timer::GetFPGATimestamp() {
+ // FPGA returns the timestamp in microseconds
+ return units::second_t(frc::RobotController::GetFPGATime() * 1.0e-6);
+}
+
+units::second_t Timer::GetMatchTime() {
+ return units::second_t(frc::DriverStation::GetInstance().GetMatchTime());
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
new file mode 100644
index 0000000..1754830
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Pose2d.h"
+
+#include <cmath>
+
+#include <wpi/json.h>
+
+using namespace frc;
+
+Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
+ : m_translation(translation), m_rotation(rotation) {}
+
+Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
+ : m_translation(x, y), m_rotation(rotation) {}
+
+Pose2d Pose2d::operator+(const Transform2d& other) const {
+ return TransformBy(other);
+}
+
+Pose2d& Pose2d::operator+=(const Transform2d& other) {
+ m_translation += other.Translation().RotateBy(m_rotation);
+ m_rotation += other.Rotation();
+ return *this;
+}
+
+Transform2d Pose2d::operator-(const Pose2d& other) const {
+ const auto pose = this->RelativeTo(other);
+ return Transform2d(pose.Translation(), pose.Rotation());
+}
+
+bool Pose2d::operator==(const Pose2d& other) const {
+ return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Pose2d::operator!=(const Pose2d& other) const {
+ return !operator==(other);
+}
+
+Pose2d Pose2d::TransformBy(const Transform2d& other) const {
+ return {m_translation + (other.Translation().RotateBy(m_rotation)),
+ m_rotation + other.Rotation()};
+}
+
+Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
+ const Transform2d transform{other, *this};
+ return {transform.Translation(), transform.Rotation()};
+}
+
+Pose2d Pose2d::Exp(const Twist2d& twist) const {
+ const auto dx = twist.dx;
+ const auto dy = twist.dy;
+ const auto dtheta = twist.dtheta.to<double>();
+
+ const auto sinTheta = std::sin(dtheta);
+ const auto cosTheta = std::cos(dtheta);
+
+ double s, c;
+ if (std::abs(dtheta) < 1E-9) {
+ s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+ c = 0.5 * dtheta;
+ } else {
+ s = sinTheta / dtheta;
+ c = (1 - cosTheta) / dtheta;
+ }
+
+ const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
+ Rotation2d{cosTheta, sinTheta}};
+
+ return *this + transform;
+}
+
+Twist2d Pose2d::Log(const Pose2d& end) const {
+ const auto transform = end.RelativeTo(*this);
+ const auto dtheta = transform.Rotation().Radians().to<double>();
+ const auto halfDtheta = dtheta / 2.0;
+
+ const auto cosMinusOne = transform.Rotation().Cos() - 1;
+
+ double halfThetaByTanOfHalfDtheta;
+
+ if (std::abs(cosMinusOne) < 1E-9) {
+ halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+ } else {
+ halfThetaByTanOfHalfDtheta =
+ -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
+ }
+
+ const Translation2d translationPart =
+ transform.Translation().RotateBy(
+ {halfThetaByTanOfHalfDtheta, -halfDtheta}) *
+ std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
+
+ return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
+}
+
+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
new file mode 100644
index 0000000..a3c4bea
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Rotation2d.h"
+
+#include <cmath>
+
+#include <wpi/json.h>
+
+using namespace frc;
+
+Rotation2d::Rotation2d(units::radian_t value)
+ : m_value(value),
+ m_cos(units::math::cos(value)),
+ m_sin(units::math::sin(value)) {}
+
+Rotation2d::Rotation2d(double x, double y) {
+ const auto magnitude = std::hypot(x, y);
+ if (magnitude > 1e-6) {
+ m_sin = y / magnitude;
+ m_cos = x / magnitude;
+ } else {
+ m_sin = 0.0;
+ m_cos = 1.0;
+ }
+ m_value = units::radian_t(std::atan2(m_sin, m_cos));
+}
+
+Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
+ return RotateBy(other);
+}
+
+Rotation2d& Rotation2d::operator+=(const Rotation2d& other) {
+ double cos = Cos() * other.Cos() - Sin() * other.Sin();
+ double sin = Cos() * other.Sin() + Sin() * other.Cos();
+ m_cos = cos;
+ m_sin = sin;
+ m_value = units::radian_t(std::atan2(m_sin, m_cos));
+ return *this;
+}
+
+Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
+ return *this + -other;
+}
+
+Rotation2d& Rotation2d::operator-=(const Rotation2d& other) {
+ *this += -other;
+ return *this;
+}
+
+Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); }
+
+Rotation2d Rotation2d::operator*(double scalar) const {
+ return Rotation2d(m_value * scalar);
+}
+
+bool Rotation2d::operator==(const Rotation2d& other) const {
+ return units::math::abs(m_value - other.m_value) < 1E-9_rad;
+}
+
+bool Rotation2d::operator!=(const Rotation2d& other) const {
+ return !operator==(other);
+}
+
+Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
+ return {Cos() * other.Cos() - Sin() * other.Sin(),
+ Cos() * other.Sin() + Sin() * other.Cos()};
+}
+
+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/Transform2d.cpp b/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp
new file mode 100644
index 0000000..04f0419
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Transform2d.h"
+
+#include "frc/geometry/Pose2d.h"
+
+using namespace frc;
+
+Transform2d::Transform2d(Pose2d initial, Pose2d final) {
+ // We are rotating the difference between the translations
+ // using a clockwise rotation matrix. This transforms the global
+ // delta into a local delta (relative to the initial pose).
+ m_translation = (final.Translation() - initial.Translation())
+ .RotateBy(-initial.Rotation());
+
+ m_rotation = final.Rotation() - initial.Rotation();
+}
+
+Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
+ : m_translation(translation), m_rotation(rotation) {}
+
+bool Transform2d::operator==(const Transform2d& other) const {
+ return m_translation == other.m_translation && m_rotation == other.m_rotation;
+}
+
+bool Transform2d::operator!=(const Transform2d& other) const {
+ return !operator==(other);
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
new file mode 100644
index 0000000..c3db7e3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/geometry/Translation2d.h"
+
+#include <wpi/json.h>
+
+using namespace frc;
+
+Translation2d::Translation2d(units::meter_t x, units::meter_t y)
+ : m_x(x), m_y(y) {}
+
+units::meter_t Translation2d::Distance(const Translation2d& other) const {
+ return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
+}
+
+units::meter_t Translation2d::Norm() const {
+ return units::math::hypot(m_x, m_y);
+}
+
+Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
+ return {m_x * other.Cos() - m_y * other.Sin(),
+ m_x * other.Sin() + m_y * other.Cos()};
+}
+
+Translation2d Translation2d::operator+(const Translation2d& other) const {
+ return {X() + other.X(), Y() + other.Y()};
+}
+
+Translation2d& Translation2d::operator+=(const Translation2d& other) {
+ m_x += other.m_x;
+ m_y += other.m_y;
+ return *this;
+}
+
+Translation2d Translation2d::operator-(const Translation2d& other) const {
+ return *this + -other;
+}
+
+Translation2d& Translation2d::operator-=(const Translation2d& other) {
+ *this += -other;
+ return *this;
+}
+
+Translation2d Translation2d::operator-() const { return {-m_x, -m_y}; }
+
+Translation2d Translation2d::operator*(double scalar) const {
+ return {scalar * m_x, scalar * m_y};
+}
+
+Translation2d& Translation2d::operator*=(double scalar) {
+ m_x *= scalar;
+ m_y *= scalar;
+ return *this;
+}
+
+Translation2d Translation2d::operator/(double scalar) const {
+ return *this * (1.0 / scalar);
+}
+
+bool Translation2d::operator==(const Translation2d& other) const {
+ return units::math::abs(m_x - other.m_x) < 1E-9_m &&
+ units::math::abs(m_y - other.m_y) < 1E-9_m;
+}
+
+bool Translation2d::operator!=(const Translation2d& other) const {
+ return !operator==(other);
+}
+
+Translation2d& Translation2d::operator/=(double scalar) {
+ *this *= (1.0 / scalar);
+ return *this;
+}
+
+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/interfaces/Potentiometer.cpp b/wpilibc/src/main/native/cpp/interfaces/Potentiometer.cpp
new file mode 100644
index 0000000..44f4aab
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/interfaces/Potentiometer.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/interfaces/Potentiometer.h"
+
+#include "frc/Utility.h"
+
+using namespace frc;
+
+void Potentiometer::SetPIDSourceType(PIDSourceType pidSource) {
+ if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
+ m_pidSource = pidSource;
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
new file mode 100644
index 0000000..07a8e3b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/DifferentialDriveOdometry.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+DifferentialDriveOdometry::DifferentialDriveOdometry(
+ const Rotation2d& gyroAngle, const Pose2d& initialPose)
+ : m_pose(initialPose) {
+ m_previousAngle = m_pose.Rotation();
+ m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ HAL_Report(HALUsageReporting::kResourceType_Odometry,
+ HALUsageReporting::kOdometry_DifferentialDrive);
+}
+
+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;
+
+ m_prevLeftDistance = leftDistance;
+ m_prevRightDistance = rightDistance;
+
+ auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+ auto angle = gyroAngle + m_gyroOffset;
+
+ auto newPose = m_pose.Exp(
+ {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
+
+ m_previousAngle = angle;
+ m_pose = {newPose.Translation(), angle};
+
+ return m_pose;
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
new file mode 100644
index 0000000..5b13f0e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+
+using namespace frc;
+
+void DifferentialDriveWheelSpeeds::Normalize(
+ units::meters_per_second_t attainableMaxSpeed) {
+ auto realMaxSpeed =
+ units::math::max(units::math::abs(left), units::math::abs(right));
+
+ if (realMaxSpeed > attainableMaxSpeed) {
+ left = left / realMaxSpeed * attainableMaxSpeed;
+ right = right / realMaxSpeed * attainableMaxSpeed;
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
new file mode 100644
index 0000000..cf6ebb5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+
+using namespace frc;
+
+MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
+ const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
+ // We have a new center of rotation. We need to compute the matrix again.
+ if (centerOfRotation != m_previousCoR) {
+ auto fl = m_frontLeftWheel - centerOfRotation;
+ auto fr = m_frontRightWheel - centerOfRotation;
+ auto rl = m_rearLeftWheel - centerOfRotation;
+ auto rr = m_rearRightWheel - centerOfRotation;
+
+ SetInverseKinematics(fl, fr, rl, rr);
+
+ m_previousCoR = centerOfRotation;
+ }
+
+ Eigen::Vector3d chassisSpeedsVector;
+ chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
+ chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+
+ Eigen::Matrix<double, 4, 1> wheelsMatrix =
+ m_inverseKinematics * chassisSpeedsVector;
+
+ MecanumDriveWheelSpeeds wheelSpeeds;
+ wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
+ wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
+ wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
+ wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
+ return wheelSpeeds;
+}
+
+ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
+ const MecanumDriveWheelSpeeds& wheelSpeeds) {
+ Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
+ // clang-format off
+ wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
+ wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
+ // clang-format on
+
+ Eigen::Vector3d chassisSpeedsVector =
+ m_forwardKinematics.solve(wheelSpeedsMatrix);
+
+ return {units::meters_per_second_t{chassisSpeedsVector(0)},
+ units::meters_per_second_t{chassisSpeedsVector(1)},
+ units::radians_per_second_t{chassisSpeedsVector(2)}};
+}
+
+void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
+ Translation2d fr,
+ Translation2d rl,
+ Translation2d rr) {
+ // clang-format off
+ m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
+ 1, 1, (fr.X() - fr.Y()).template to<double>(),
+ 1, 1, (rl.X() - rl.Y()).template to<double>(),
+ 1, -1, (-(rr.X() + rr.Y())).template to<double>();
+ // clang-format on
+ m_inverseKinematics /= std::sqrt(2);
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
new file mode 100644
index 0000000..3843483
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveOdometry.h"
+
+#include <hal/FRCUsageReporting.h>
+
+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;
+ HAL_Report(HALUsageReporting::kResourceType_Odometry,
+ HALUsageReporting::kOdometry_MecanumDrive);
+}
+
+const Pose2d& MecanumDriveOdometry::UpdateWithTime(
+ 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);
+
+ auto newPose = m_pose.Exp(
+ {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+
+ m_previousAngle = angle;
+ m_pose = {newPose.Translation(), angle};
+
+ return m_pose;
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
new file mode 100644
index 0000000..1641ba8
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+
+using namespace frc;
+
+void MecanumDriveWheelSpeeds::Normalize(
+ units::meters_per_second_t attainableMaxSpeed) {
+ std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
+ rearLeft, rearRight};
+ units::meters_per_second_t realMaxSpeed = *std::max_element(
+ wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
+ return units::math::abs(a) < units::math::abs(b);
+ });
+
+ if (realMaxSpeed > attainableMaxSpeed) {
+ for (int i = 0; i < 4; ++i) {
+ wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
+ }
+ frontLeft = wheelSpeeds[0];
+ frontRight = wheelSpeeds[1];
+ rearLeft = wheelSpeeds[2];
+ rearRight = wheelSpeeds[3];
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
new file mode 100644
index 0000000..10996e4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -0,0 +1,161 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2012-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/livewindow/LiveWindow.h"
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/mutex.h>
+
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilderImpl.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+using wpi::Twine;
+
+struct LiveWindow::Impl {
+ Impl();
+
+ struct Component {
+ bool firstTime = true;
+ bool telemetryEnabled = true;
+ };
+
+ wpi::mutex mutex;
+
+ SendableRegistry& registry;
+ int dataHandle;
+
+ std::shared_ptr<nt::NetworkTable> liveWindowTable;
+ std::shared_ptr<nt::NetworkTable> statusTable;
+ nt::NetworkTableEntry enabledEntry;
+
+ bool startLiveWindow = false;
+ bool liveWindowEnabled = false;
+ bool telemetryEnabled = true;
+
+ std::shared_ptr<Component> GetOrAdd(Sendable* sendable);
+};
+
+LiveWindow::Impl::Impl()
+ : registry(SendableRegistry::GetInstance()),
+ dataHandle(registry.GetDataHandle()),
+ liveWindowTable(
+ nt::NetworkTableInstance::GetDefault().GetTable("LiveWindow")) {
+ statusTable = liveWindowTable->GetSubTable(".status");
+ enabledEntry = statusTable->GetEntry("LW Enabled");
+}
+
+std::shared_ptr<LiveWindow::Impl::Component> LiveWindow::Impl::GetOrAdd(
+ Sendable* sendable) {
+ auto data = std::static_pointer_cast<Component>(
+ registry.GetData(sendable, dataHandle));
+ if (!data) {
+ data = std::make_shared<Component>();
+ registry.SetData(sendable, dataHandle, data);
+ }
+ return data;
+}
+
+LiveWindow* LiveWindow::GetInstance() {
+ static LiveWindow instance;
+ return &instance;
+}
+
+void LiveWindow::EnableTelemetry(Sendable* sendable) {
+ std::scoped_lock lock(m_impl->mutex);
+ // Re-enable global setting in case DisableAllTelemetry() was called.
+ m_impl->telemetryEnabled = true;
+ m_impl->GetOrAdd(sendable)->telemetryEnabled = true;
+}
+
+void LiveWindow::DisableTelemetry(Sendable* sendable) {
+ std::scoped_lock lock(m_impl->mutex);
+ m_impl->GetOrAdd(sendable)->telemetryEnabled = false;
+}
+
+void LiveWindow::DisableAllTelemetry() {
+ std::scoped_lock lock(m_impl->mutex);
+ m_impl->telemetryEnabled = false;
+ m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
+ if (!cbdata.data) cbdata.data = std::make_shared<Impl::Component>();
+ std::static_pointer_cast<Impl::Component>(cbdata.data)->telemetryEnabled =
+ false;
+ });
+}
+
+bool LiveWindow::IsEnabled() const {
+ std::scoped_lock lock(m_impl->mutex);
+ return m_impl->liveWindowEnabled;
+}
+
+void LiveWindow::SetEnabled(bool enabled) {
+ std::scoped_lock lock(m_impl->mutex);
+ if (m_impl->liveWindowEnabled == enabled) return;
+ m_impl->startLiveWindow = enabled;
+ m_impl->liveWindowEnabled = enabled;
+ // Force table generation now to make sure everything is defined
+ UpdateValuesUnsafe();
+ if (enabled) {
+ } else {
+ m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
+ cbdata.builder.StopLiveWindowMode();
+ });
+ }
+ m_impl->enabledEntry.SetBoolean(enabled);
+}
+
+void LiveWindow::UpdateValues() {
+ std::scoped_lock lock(m_impl->mutex);
+ UpdateValuesUnsafe();
+}
+
+void LiveWindow::UpdateValuesUnsafe() {
+ // Only do this if either LiveWindow mode or telemetry is enabled.
+ if (!m_impl->liveWindowEnabled && !m_impl->telemetryEnabled) return;
+
+ m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
+ if (!cbdata.sendable || cbdata.parent) return;
+
+ if (!cbdata.data) cbdata.data = std::make_shared<Impl::Component>();
+
+ auto& comp = *std::static_pointer_cast<Impl::Component>(cbdata.data);
+
+ if (!m_impl->liveWindowEnabled && !comp.telemetryEnabled) return;
+
+ if (comp.firstTime) {
+ // By holding off creating the NetworkTable entries, it allows the
+ // components to be redefined. This allows default sensor and actuator
+ // values to be created that are replaced with the custom names from
+ // users calling setName.
+ if (cbdata.name.empty()) return;
+ auto ssTable = m_impl->liveWindowTable->GetSubTable(cbdata.subsystem);
+ std::shared_ptr<NetworkTable> table;
+ // Treat name==subsystem as top level of subsystem
+ if (cbdata.name == cbdata.subsystem)
+ table = ssTable;
+ else
+ table = ssTable->GetSubTable(cbdata.name);
+ table->GetEntry(".name").SetString(cbdata.name);
+ cbdata.builder.SetTable(table);
+ cbdata.sendable->InitSendable(cbdata.builder);
+ ssTable->GetEntry(".type").SetString("LW Subsystem");
+
+ comp.firstTime = false;
+ }
+
+ if (m_impl->startLiveWindow) cbdata.builder.StartLiveWindowMode();
+ cbdata.builder.UpdateTable();
+ });
+
+ m_impl->startLiveWindow = false;
+}
+
+LiveWindow::LiveWindow() : m_impl(new Impl) {}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ComplexWidget.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ComplexWidget.cpp
new file mode 100644
index 0000000..19e17bb
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ComplexWidget.cpp
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/ComplexWidget.h"
+
+#include "frc/smartdashboard/Sendable.h"
+
+using namespace frc;
+
+ComplexWidget::ComplexWidget(ShuffleboardContainer& parent,
+ const wpi::Twine& title, Sendable& sendable)
+ : ShuffleboardValue(title),
+ ShuffleboardWidget(parent, title),
+ m_sendable(sendable) {}
+
+void ComplexWidget::EnableIfActuator() {
+ if (m_builder.IsActuator()) {
+ m_builder.StartLiveWindowMode();
+ }
+}
+
+void ComplexWidget::DisableIfActuator() {
+ if (m_builder.IsActuator()) {
+ m_builder.StopLiveWindowMode();
+ }
+}
+
+void ComplexWidget::BuildInto(std::shared_ptr<nt::NetworkTable> parentTable,
+ std::shared_ptr<nt::NetworkTable> metaTable) {
+ BuildMetadata(metaTable);
+ if (!m_builderInit) {
+ m_builder.SetTable(parentTable->GetSubTable(GetTitle()));
+ m_sendable.InitSendable(m_builder);
+ m_builder.StartListeners();
+ m_builderInit = true;
+ }
+ m_builder.UpdateTable();
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/LayoutType.cpp b/wpilibc/src/main/native/cpp/shuffleboard/LayoutType.cpp
new file mode 100644
index 0000000..a21cad9
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/LayoutType.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/LayoutType.h"
+
+using namespace frc;
+
+wpi::StringRef LayoutType::GetLayoutName() const { return m_layoutName; }
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp b/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp
new file mode 100644
index 0000000..d80001e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/RecordingController.h"
+
+#include "frc/DriverStation.h"
+
+using namespace frc;
+using namespace frc::detail;
+
+RecordingController::RecordingController(nt::NetworkTableInstance ntInstance)
+ : m_recordingControlEntry(), m_recordingFileNameFormatEntry() {
+ m_recordingControlEntry =
+ ntInstance.GetEntry("/Shuffleboard/.recording/RecordData");
+ m_recordingFileNameFormatEntry =
+ ntInstance.GetEntry("/Shuffleboard/.recording/FileNameFormat");
+ m_eventsTable = ntInstance.GetTable("/Shuffleboard/.recording/events");
+}
+
+void RecordingController::StartRecording() {
+ m_recordingControlEntry.SetBoolean(true);
+}
+
+void RecordingController::StopRecording() {
+ m_recordingControlEntry.SetBoolean(false);
+}
+
+void RecordingController::SetRecordingFileNameFormat(wpi::StringRef format) {
+ m_recordingFileNameFormatEntry.SetString(format);
+}
+
+void RecordingController::ClearRecordingFileNameFormat() {
+ m_recordingFileNameFormatEntry.Delete();
+}
+
+void RecordingController::AddEventMarker(
+ wpi::StringRef name, wpi::StringRef description,
+ ShuffleboardEventImportance importance) {
+ if (name.empty()) {
+ DriverStation::ReportError("Shuffleboard event name was not specified");
+ return;
+ }
+ m_eventsTable->GetSubTable(name)->GetEntry("Info").SetStringArray(
+ {description, ShuffleboardEventImportanceName(importance)});
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
new file mode 100644
index 0000000..b6f8ce9
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/SendableCameraWrapper.h"
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include <wpi/DenseMap.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+namespace frc {
+namespace detail {
+std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
+ CS_Source source) {
+ static wpi::DenseMap<int, std::shared_ptr<SendableCameraWrapper>> wrappers;
+ return wrappers[static_cast<int>(source)];
+}
+
+void AddToSendableRegistry(frc::Sendable* sendable, std::string name) {
+ SendableRegistry::GetInstance().Add(sendable, name);
+}
+} // namespace detail
+
+void SendableCameraWrapper::InitSendable(SendableBuilder& builder) {
+ builder.AddStringProperty(".ShuffleboardURI", [this] { return m_uri; },
+ nullptr);
+}
+} // namespace frc
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/Shuffleboard.cpp b/wpilibc/src/main/native/cpp/shuffleboard/Shuffleboard.cpp
new file mode 100644
index 0000000..2d69847
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/Shuffleboard.cpp
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/Shuffleboard.h"
+
+#include <networktables/NetworkTableInstance.h>
+
+#include "frc/shuffleboard/ShuffleboardTab.h"
+
+using namespace frc;
+
+void Shuffleboard::Update() { GetInstance().Update(); }
+
+ShuffleboardTab& Shuffleboard::GetTab(wpi::StringRef title) {
+ return GetInstance().GetTab(title);
+}
+
+void Shuffleboard::SelectTab(int index) { GetInstance().SelectTab(index); }
+
+void Shuffleboard::SelectTab(wpi::StringRef title) {
+ GetInstance().SelectTab(title);
+}
+
+void Shuffleboard::EnableActuatorWidgets() {
+ GetInstance().EnableActuatorWidgets();
+}
+
+void Shuffleboard::DisableActuatorWidgets() {
+ // Need to update to make sure the sendable builders are initialized
+ Update();
+ GetInstance().DisableActuatorWidgets();
+}
+
+void Shuffleboard::StartRecording() {
+ GetRecordingController().StartRecording();
+}
+
+void Shuffleboard::StopRecording() { GetRecordingController().StopRecording(); }
+
+void Shuffleboard::SetRecordingFileNameFormat(wpi::StringRef format) {
+ GetRecordingController().SetRecordingFileNameFormat(format);
+}
+
+void Shuffleboard::ClearRecordingFileNameFormat() {
+ GetRecordingController().ClearRecordingFileNameFormat();
+}
+
+void Shuffleboard::AddEventMarker(wpi::StringRef name,
+ wpi::StringRef description,
+ ShuffleboardEventImportance importance) {
+ GetRecordingController().AddEventMarker(name, description, importance);
+}
+
+void Shuffleboard::AddEventMarker(wpi::StringRef name,
+ ShuffleboardEventImportance importance) {
+ AddEventMarker(name, "", importance);
+}
+
+detail::ShuffleboardInstance& Shuffleboard::GetInstance() {
+ static detail::ShuffleboardInstance inst(
+ nt::NetworkTableInstance::GetDefault());
+ return inst;
+}
+
+detail::RecordingController& Shuffleboard::GetRecordingController() {
+ static detail::RecordingController inst(
+ nt::NetworkTableInstance::GetDefault());
+ return inst;
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardComponentBase.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardComponentBase.cpp
new file mode 100644
index 0000000..7617333
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardComponentBase.cpp
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/ShuffleboardComponentBase.h"
+
+#include <wpi/SmallVector.h>
+
+using namespace frc;
+
+ShuffleboardComponentBase::ShuffleboardComponentBase(
+ ShuffleboardContainer& parent, const wpi::Twine& title,
+ const wpi::Twine& type)
+ : ShuffleboardValue(title), m_parent(parent) {
+ wpi::SmallVector<char, 16> storage;
+ m_type = type.toStringRef(storage);
+}
+
+void ShuffleboardComponentBase::SetType(const wpi::Twine& type) {
+ wpi::SmallVector<char, 16> storage;
+ m_type = type.toStringRef(storage);
+ m_metadataDirty = true;
+}
+
+void ShuffleboardComponentBase::BuildMetadata(
+ std::shared_ptr<nt::NetworkTable> metaTable) {
+ if (!m_metadataDirty) {
+ return;
+ }
+ // Component type
+ if (GetType() == "") {
+ metaTable->GetEntry("PreferredComponent").Delete();
+ } else {
+ metaTable->GetEntry("PreferredComponent").ForceSetString(GetType());
+ }
+
+ // Tile size
+ if (m_width <= 0 || m_height <= 0) {
+ metaTable->GetEntry("Size").Delete();
+ } else {
+ metaTable->GetEntry("Size").SetDoubleArray(
+ {static_cast<double>(m_width), static_cast<double>(m_height)});
+ }
+
+ // Tile position
+ if (m_column < 0 || m_row < 0) {
+ metaTable->GetEntry("Position").Delete();
+ } else {
+ metaTable->GetEntry("Position")
+ .SetDoubleArray(
+ {static_cast<double>(m_column), static_cast<double>(m_row)});
+ }
+
+ // Custom properties
+ if (GetProperties().size() > 0) {
+ auto propTable = metaTable->GetSubTable("Properties");
+ for (auto& entry : GetProperties()) {
+ propTable->GetEntry(entry.first()).SetValue(entry.second);
+ }
+ }
+ m_metadataDirty = false;
+}
+
+ShuffleboardContainer& ShuffleboardComponentBase::GetParent() {
+ return m_parent;
+}
+
+const std::string& ShuffleboardComponentBase::GetType() const { return m_type; }
+
+const wpi::StringMap<std::shared_ptr<nt::Value>>&
+ShuffleboardComponentBase::GetProperties() const {
+ return m_properties;
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
new file mode 100644
index 0000000..bb9dc9e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
@@ -0,0 +1,300 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/ShuffleboardContainer.h"
+
+#include <wpi/SmallVector.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/shuffleboard/ComplexWidget.h"
+#include "frc/shuffleboard/ShuffleboardComponent.h"
+#include "frc/shuffleboard/ShuffleboardLayout.h"
+#include "frc/shuffleboard/SimpleWidget.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+static constexpr const char* layoutStrings[] = {"List Layout", "Grid Layout"};
+
+static constexpr const char* GetStringFromBuiltInLayout(BuiltInLayouts layout) {
+ return layoutStrings[static_cast<int>(layout)];
+}
+
+ShuffleboardContainer::ShuffleboardContainer(const wpi::Twine& title)
+ : ShuffleboardValue(title) {}
+
+const std::vector<std::unique_ptr<ShuffleboardComponentBase>>&
+ShuffleboardContainer::GetComponents() const {
+ return m_components;
+}
+
+ShuffleboardLayout& ShuffleboardContainer::GetLayout(const wpi::Twine& title,
+ BuiltInLayouts type) {
+ return GetLayout(title, GetStringFromBuiltInLayout(type));
+}
+
+ShuffleboardLayout& ShuffleboardContainer::GetLayout(const wpi::Twine& title,
+ const LayoutType& type) {
+ return GetLayout(title, type.GetLayoutName());
+}
+
+ShuffleboardLayout& ShuffleboardContainer::GetLayout(const wpi::Twine& title,
+ const wpi::Twine& type) {
+ wpi::SmallVector<char, 16> storage;
+ auto titleRef = title.toStringRef(storage);
+ if (m_layouts.count(titleRef) == 0) {
+ auto layout = std::make_unique<ShuffleboardLayout>(*this, type, titleRef);
+ auto ptr = layout.get();
+ m_components.emplace_back(std::move(layout));
+ m_layouts.insert(std::make_pair(titleRef, ptr));
+ }
+ return *m_layouts[titleRef];
+}
+
+ShuffleboardLayout& ShuffleboardContainer::GetLayout(const wpi::Twine& title) {
+ wpi::SmallVector<char, 16> storage;
+ auto titleRef = title.toStringRef(storage);
+ if (m_layouts.count(titleRef) == 0) {
+ wpi_setWPIErrorWithContext(
+ InvalidParameter, "No layout with the given title has been defined");
+ }
+ return *m_layouts[titleRef];
+}
+
+ComplexWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+ Sendable& sendable) {
+ CheckTitle(title);
+ auto widget = std::make_unique<ComplexWidget>(*this, title, sendable);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+ComplexWidget& ShuffleboardContainer::Add(Sendable& sendable) {
+ auto name = SendableRegistry::GetInstance().GetName(&sendable);
+ if (name.empty()) {
+ wpi::outs() << "Sendable must have a name\n";
+ }
+ return Add(name, sendable);
+}
+
+SimpleWidget& ShuffleboardContainer::Add(
+ const wpi::Twine& title, std::shared_ptr<nt::Value> defaultValue) {
+ CheckTitle(title);
+
+ auto widget = std::make_unique<SimpleWidget>(*this, title);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ ptr->GetEntry().SetDefaultValue(defaultValue);
+ return *ptr;
+}
+
+SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+ bool defaultValue) {
+ return Add(title, nt::Value::MakeBoolean(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+ double defaultValue) {
+ return Add(title, nt::Value::MakeDouble(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+ int defaultValue) {
+ return Add(title, nt::Value::MakeDouble(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+ const wpi::Twine& defaultValue) {
+ return Add(title, nt::Value::MakeString(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+ const char* defaultValue) {
+ return Add(title, nt::Value::MakeString(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+ wpi::ArrayRef<bool> defaultValue) {
+ return Add(title, nt::Value::MakeBooleanArray(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+ wpi::ArrayRef<double> defaultValue) {
+ return Add(title, nt::Value::MakeDoubleArray(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::Add(
+ const wpi::Twine& title, wpi::ArrayRef<std::string> defaultValue) {
+ return Add(title, nt::Value::MakeStringArray(defaultValue));
+}
+
+SuppliedValueWidget<std::string>& ShuffleboardContainer::AddString(
+ const wpi::Twine& title, std::function<std::string()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry, std::string value) {
+ entry.SetString(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<std::string>>(
+ *this, title, supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<double>& ShuffleboardContainer::AddNumber(
+ const wpi::Twine& title, std::function<double()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry, double value) {
+ entry.SetDouble(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<double>>(*this, title,
+ supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<bool>& ShuffleboardContainer::AddBoolean(
+ const wpi::Twine& title, std::function<bool()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry, bool value) {
+ entry.SetBoolean(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<bool>>(*this, title,
+ supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<std::vector<std::string>>&
+ShuffleboardContainer::AddStringArray(
+ const wpi::Twine& title,
+ std::function<std::vector<std::string>()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry,
+ std::vector<std::string> value) {
+ entry.SetStringArray(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<std::vector<std::string>>>(
+ *this, title, supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<std::vector<double>>& ShuffleboardContainer::AddNumberArray(
+ const wpi::Twine& title, std::function<std::vector<double>()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry,
+ std::vector<double> value) {
+ entry.SetDoubleArray(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<std::vector<double>>>(
+ *this, title, supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<std::vector<int>>& ShuffleboardContainer::AddBooleanArray(
+ const wpi::Twine& title, std::function<std::vector<int>()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry, std::vector<int> value) {
+ entry.SetBooleanArray(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<std::vector<int>>>(
+ *this, title, supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SuppliedValueWidget<wpi::StringRef>& ShuffleboardContainer::AddRaw(
+ const wpi::Twine& title, std::function<wpi::StringRef()> supplier) {
+ static auto setter = [](nt::NetworkTableEntry entry, wpi::StringRef value) {
+ entry.SetRaw(value);
+ };
+
+ CheckTitle(title);
+ auto widget = std::make_unique<SuppliedValueWidget<wpi::StringRef>>(
+ *this, title, supplier, setter);
+ auto ptr = widget.get();
+ m_components.emplace_back(std::move(widget));
+ return *ptr;
+}
+
+SimpleWidget& ShuffleboardContainer::AddPersistent(
+ const wpi::Twine& title, std::shared_ptr<nt::Value> defaultValue) {
+ auto& widget = Add(title, defaultValue);
+ widget.GetEntry().SetPersistent();
+ return widget;
+}
+
+SimpleWidget& ShuffleboardContainer::AddPersistent(const wpi::Twine& title,
+ bool defaultValue) {
+ return AddPersistent(title, nt::Value::MakeBoolean(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::AddPersistent(const wpi::Twine& title,
+ double defaultValue) {
+ return AddPersistent(title, nt::Value::MakeDouble(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::AddPersistent(const wpi::Twine& title,
+ int defaultValue) {
+ return AddPersistent(title, nt::Value::MakeDouble(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::AddPersistent(
+ const wpi::Twine& title, const wpi::Twine& defaultValue) {
+ return AddPersistent(title, nt::Value::MakeString(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::AddPersistent(
+ const wpi::Twine& title, wpi::ArrayRef<bool> defaultValue) {
+ return AddPersistent(title, nt::Value::MakeBooleanArray(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::AddPersistent(
+ const wpi::Twine& title, wpi::ArrayRef<double> defaultValue) {
+ return AddPersistent(title, nt::Value::MakeDoubleArray(defaultValue));
+}
+
+SimpleWidget& ShuffleboardContainer::AddPersistent(
+ const wpi::Twine& title, wpi::ArrayRef<std::string> defaultValue) {
+ return AddPersistent(title, nt::Value::MakeStringArray(defaultValue));
+}
+
+void ShuffleboardContainer::EnableIfActuator() {
+ for (auto& component : GetComponents()) {
+ component->EnableIfActuator();
+ }
+}
+
+void ShuffleboardContainer::DisableIfActuator() {
+ for (auto& component : GetComponents()) {
+ component->DisableIfActuator();
+ }
+}
+
+void ShuffleboardContainer::CheckTitle(const wpi::Twine& title) {
+ wpi::SmallVector<char, 16> storage;
+ auto titleRef = title.toStringRef(storage);
+ if (m_usedTitles.count(titleRef) > 0) {
+ wpi::errs() << "Title is already in use: " << title << "\n";
+ return;
+ }
+ m_usedTitles.insert(titleRef);
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
new file mode 100644
index 0000000..9717a8e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/ShuffleboardInstance.h"
+
+#include <hal/FRCUsageReporting.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/StringMap.h>
+
+#include "frc/shuffleboard/Shuffleboard.h"
+
+using namespace frc::detail;
+
+struct ShuffleboardInstance::Impl {
+ wpi::StringMap<ShuffleboardTab> tabs;
+
+ bool tabsChanged = false;
+ std::shared_ptr<nt::NetworkTable> rootTable;
+ std::shared_ptr<nt::NetworkTable> rootMetaTable;
+};
+
+ShuffleboardInstance::ShuffleboardInstance(nt::NetworkTableInstance ntInstance)
+ : m_impl(new Impl) {
+ m_impl->rootTable = ntInstance.GetTable(Shuffleboard::kBaseTableName);
+ m_impl->rootMetaTable = m_impl->rootTable->GetSubTable(".metadata");
+ HAL_Report(HALUsageReporting::kResourceType_Shuffleboard, 0);
+}
+
+ShuffleboardInstance::~ShuffleboardInstance() {}
+
+frc::ShuffleboardTab& ShuffleboardInstance::GetTab(wpi::StringRef title) {
+ if (m_impl->tabs.find(title) == m_impl->tabs.end()) {
+ m_impl->tabs.try_emplace(title, ShuffleboardTab(*this, title));
+ m_impl->tabsChanged = true;
+ }
+ return m_impl->tabs.find(title)->second;
+}
+
+void ShuffleboardInstance::Update() {
+ if (m_impl->tabsChanged) {
+ std::vector<std::string> tabTitles;
+ for (auto& entry : m_impl->tabs) {
+ tabTitles.emplace_back(entry.second.GetTitle());
+ }
+ m_impl->rootMetaTable->GetEntry("Tabs").ForceSetStringArray(tabTitles);
+ m_impl->tabsChanged = false;
+ }
+ for (auto& entry : m_impl->tabs) {
+ auto& tab = entry.second;
+ tab.BuildInto(m_impl->rootTable,
+ m_impl->rootMetaTable->GetSubTable(tab.GetTitle()));
+ }
+}
+
+void ShuffleboardInstance::EnableActuatorWidgets() {
+ for (auto& entry : m_impl->tabs) {
+ auto& tab = entry.second;
+ for (auto& component : tab.GetComponents()) {
+ component->EnableIfActuator();
+ }
+ }
+}
+
+void ShuffleboardInstance::DisableActuatorWidgets() {
+ for (auto& entry : m_impl->tabs) {
+ auto& tab = entry.second;
+ for (auto& component : tab.GetComponents()) {
+ component->DisableIfActuator();
+ }
+ }
+}
+
+void ShuffleboardInstance::SelectTab(int index) {
+ m_impl->rootMetaTable->GetEntry("Selected").ForceSetDouble(index);
+}
+
+void ShuffleboardInstance::SelectTab(wpi::StringRef title) {
+ m_impl->rootMetaTable->GetEntry("Selected").ForceSetString(title);
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
new file mode 100644
index 0000000..8418b77
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/ShuffleboardLayout.h"
+
+using namespace frc;
+
+ShuffleboardLayout::ShuffleboardLayout(ShuffleboardContainer& parent,
+ const wpi::Twine& name,
+ const wpi::Twine& type)
+ : ShuffleboardValue(type),
+ ShuffleboardComponent(parent, type, name),
+ ShuffleboardContainer(name) {
+ m_isLayout = true;
+}
+
+void ShuffleboardLayout::BuildInto(
+ std::shared_ptr<nt::NetworkTable> parentTable,
+ std::shared_ptr<nt::NetworkTable> metaTable) {
+ BuildMetadata(metaTable);
+ auto table = parentTable->GetSubTable(GetTitle());
+ table->GetEntry(".type").SetString("ShuffleboardLayout");
+ for (auto& component : GetComponents()) {
+ component->BuildInto(table, metaTable->GetSubTable(component->GetTitle()));
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardTab.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardTab.cpp
new file mode 100644
index 0000000..7a8338e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardTab.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/ShuffleboardTab.h"
+
+using namespace frc;
+
+ShuffleboardTab::ShuffleboardTab(ShuffleboardRoot& root, wpi::StringRef title)
+ : ShuffleboardValue(title), ShuffleboardContainer(title), m_root(root) {}
+
+ShuffleboardRoot& ShuffleboardTab::GetRoot() { return m_root; }
+
+void ShuffleboardTab::BuildInto(std::shared_ptr<nt::NetworkTable> parentTable,
+ std::shared_ptr<nt::NetworkTable> metaTable) {
+ auto tabTable = parentTable->GetSubTable(GetTitle());
+ tabTable->GetEntry(".type").SetString("ShuffleboardTab");
+ for (auto& component : GetComponents()) {
+ component->BuildInto(tabTable,
+ metaTable->GetSubTable(component->GetTitle()));
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
new file mode 100644
index 0000000..3b79a9c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/ShuffleboardWidget.h"
+
+using namespace frc;
+
+static constexpr const char* widgetStrings[] = {
+ "Text View",
+ "Number Slider",
+ "Number Bar",
+ "Simple Dial",
+ "Graph",
+ "Boolean Box",
+ "Toggle Button",
+ "Toggle Switch",
+ "Voltage View",
+ "PDP",
+ "ComboBox Chooser",
+ "Split Button Chooser",
+ "Encoder",
+ "Speed Controller",
+ "Command",
+ "PID Command",
+ "PID Controller",
+ "Accelerometer",
+ "3-Axis Accelerometer",
+ "Gyro",
+ "Relay",
+ "Differential Drivebase",
+ "Mecanum Drivebase",
+ "Camera Stream",
+};
+
+const char* detail::GetStringForWidgetType(BuiltInWidgets type) {
+ return widgetStrings[static_cast<int>(type)];
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/SimpleWidget.cpp b/wpilibc/src/main/native/cpp/shuffleboard/SimpleWidget.cpp
new file mode 100644
index 0000000..89af25d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/SimpleWidget.cpp
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/SimpleWidget.h"
+
+#include "frc/shuffleboard/Shuffleboard.h"
+#include "frc/shuffleboard/ShuffleboardLayout.h"
+#include "frc/shuffleboard/ShuffleboardTab.h"
+
+using namespace frc;
+
+SimpleWidget::SimpleWidget(ShuffleboardContainer& parent,
+ const wpi::Twine& title)
+ : ShuffleboardValue(title), ShuffleboardWidget(parent, title), m_entry() {}
+
+nt::NetworkTableEntry SimpleWidget::GetEntry() {
+ if (!m_entry) {
+ ForceGenerate();
+ }
+ return m_entry;
+}
+
+void SimpleWidget::BuildInto(std::shared_ptr<nt::NetworkTable> parentTable,
+ std::shared_ptr<nt::NetworkTable> metaTable) {
+ BuildMetadata(metaTable);
+ if (!m_entry) {
+ m_entry = parentTable->GetEntry(GetTitle());
+ }
+}
+
+void SimpleWidget::ForceGenerate() {
+ ShuffleboardContainer* parent = &GetParent();
+
+ while (parent->m_isLayout) {
+ parent = &(static_cast<ShuffleboardLayout*>(parent)->GetParent());
+ }
+
+ auto& tab = *static_cast<ShuffleboardTab*>(parent);
+ tab.GetRoot().Update();
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/WidgetType.cpp b/wpilibc/src/main/native/cpp/shuffleboard/WidgetType.cpp
new file mode 100644
index 0000000..cb73d30
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/WidgetType.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/shuffleboard/WidgetType.h"
+
+using namespace frc;
+
+wpi::StringRef WidgetType::GetWidgetName() const { return m_widgetName; }
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/ListenerExecutor.cpp b/wpilibc/src/main/native/cpp/smartdashboard/ListenerExecutor.cpp
new file mode 100644
index 0000000..75b373a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/ListenerExecutor.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/ListenerExecutor.h"
+
+using namespace frc::detail;
+
+void ListenerExecutor::Execute(std::function<void()> task) {
+ std::scoped_lock lock(m_lock);
+ m_tasks.emplace_back(task);
+}
+
+void ListenerExecutor::RunListenerTasks() {
+ // Locally copy tasks from internal list; minimizes blocking time
+ {
+ std::scoped_lock lock(m_lock);
+ std::swap(m_tasks, m_runningTasks);
+ }
+
+ for (auto&& task : m_runningTasks) {
+ task();
+ }
+ m_runningTasks.clear();
+}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp
new file mode 100644
index 0000000..7fc8635
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/SendableBase.h"
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+SendableBase::SendableBase(bool addLiveWindow) {
+ if (addLiveWindow)
+ SendableRegistry::GetInstance().AddLW(this, "");
+ else
+ SendableRegistry::GetInstance().Add(this, "");
+}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
new file mode 100644
index 0000000..d075deb
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
@@ -0,0 +1,402 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/SendableBuilderImpl.h"
+
+#include <ntcore_cpp.h>
+#include <wpi/SmallString.h>
+
+#include "frc/smartdashboard/SmartDashboard.h"
+
+using namespace frc;
+
+void SendableBuilderImpl::SetTable(std::shared_ptr<nt::NetworkTable> table) {
+ m_table = table;
+ m_controllableEntry = table->GetEntry(".controllable");
+}
+
+std::shared_ptr<nt::NetworkTable> SendableBuilderImpl::GetTable() {
+ return m_table;
+}
+
+bool SendableBuilderImpl::HasTable() const { return m_table != nullptr; }
+
+bool SendableBuilderImpl::IsActuator() const { return m_actuator; }
+
+void SendableBuilderImpl::UpdateTable() {
+ uint64_t time = nt::Now();
+ for (auto& property : m_properties) {
+ if (property.update) property.update(property.entry, time);
+ }
+ if (m_updateTable) m_updateTable();
+}
+
+void SendableBuilderImpl::StartListeners() {
+ for (auto& property : m_properties) property.StartListener();
+ if (m_controllableEntry) m_controllableEntry.SetBoolean(true);
+}
+
+void SendableBuilderImpl::StopListeners() {
+ for (auto& property : m_properties) property.StopListener();
+ if (m_controllableEntry) m_controllableEntry.SetBoolean(false);
+}
+
+void SendableBuilderImpl::StartLiveWindowMode() {
+ if (m_safeState) m_safeState();
+ StartListeners();
+}
+
+void SendableBuilderImpl::StopLiveWindowMode() {
+ StopListeners();
+ if (m_safeState) m_safeState();
+}
+
+void SendableBuilderImpl::ClearProperties() { m_properties.clear(); }
+
+void SendableBuilderImpl::SetSmartDashboardType(const wpi::Twine& type) {
+ m_table->GetEntry(".type").SetString(type);
+}
+
+void SendableBuilderImpl::SetActuator(bool value) {
+ m_table->GetEntry(".actuator").SetBoolean(value);
+ m_actuator = value;
+}
+
+void SendableBuilderImpl::SetSafeState(std::function<void()> func) {
+ m_safeState = func;
+}
+
+void SendableBuilderImpl::SetUpdateTable(std::function<void()> func) {
+ m_updateTable = func;
+}
+
+nt::NetworkTableEntry SendableBuilderImpl::GetEntry(const wpi::Twine& key) {
+ return m_table->GetEntry(key);
+}
+
+void SendableBuilderImpl::AddBooleanProperty(const wpi::Twine& key,
+ std::function<bool()> getter,
+ std::function<void(bool)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ entry.SetValue(nt::Value::MakeBoolean(getter(), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsBoolean()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetBoolean()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddDoubleProperty(
+ const wpi::Twine& key, std::function<double()> getter,
+ std::function<void(double)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ entry.SetValue(nt::Value::MakeDouble(getter(), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsDouble()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetDouble()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddStringProperty(
+ const wpi::Twine& key, std::function<std::string()> getter,
+ std::function<void(wpi::StringRef)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ entry.SetValue(nt::Value::MakeString(getter(), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsString()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetString()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddBooleanArrayProperty(
+ const wpi::Twine& key, std::function<std::vector<int>()> getter,
+ std::function<void(wpi::ArrayRef<int>)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ entry.SetValue(nt::Value::MakeBooleanArray(getter(), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsBooleanArray()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetBooleanArray()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddDoubleArrayProperty(
+ const wpi::Twine& key, std::function<std::vector<double>()> getter,
+ std::function<void(wpi::ArrayRef<double>)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ entry.SetValue(nt::Value::MakeDoubleArray(getter(), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsDoubleArray()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetDoubleArray()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddStringArrayProperty(
+ const wpi::Twine& key, std::function<std::vector<std::string>()> getter,
+ std::function<void(wpi::ArrayRef<std::string>)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ entry.SetValue(nt::Value::MakeStringArray(getter(), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsStringArray()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetStringArray()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddRawProperty(
+ const wpi::Twine& key, std::function<std::string()> getter,
+ std::function<void(wpi::StringRef)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ entry.SetValue(nt::Value::MakeRaw(getter(), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsRaw()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetRaw()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddValueProperty(
+ const wpi::Twine& key, std::function<std::shared_ptr<nt::Value>()> getter,
+ std::function<void(std::shared_ptr<nt::Value>)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ entry.SetValue(getter());
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ SmartDashboard::PostListenerTask([=] { setter(event.value); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddSmallStringProperty(
+ const wpi::Twine& key,
+ std::function<wpi::StringRef(wpi::SmallVectorImpl<char>& buf)> getter,
+ std::function<void(wpi::StringRef)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ wpi::SmallString<128> buf;
+ entry.SetValue(nt::Value::MakeString(getter(buf), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsString()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetString()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddSmallBooleanArrayProperty(
+ const wpi::Twine& key,
+ std::function<wpi::ArrayRef<int>(wpi::SmallVectorImpl<int>& buf)> getter,
+ std::function<void(wpi::ArrayRef<int>)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ wpi::SmallVector<int, 16> buf;
+ entry.SetValue(nt::Value::MakeBooleanArray(getter(buf), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsBooleanArray()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetBooleanArray()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddSmallDoubleArrayProperty(
+ const wpi::Twine& key,
+ std::function<wpi::ArrayRef<double>(wpi::SmallVectorImpl<double>& buf)>
+ getter,
+ std::function<void(wpi::ArrayRef<double>)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ wpi::SmallVector<double, 16> buf;
+ entry.SetValue(nt::Value::MakeDoubleArray(getter(buf), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsDoubleArray()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetDoubleArray()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddSmallStringArrayProperty(
+ const wpi::Twine& key,
+ std::function<
+ wpi::ArrayRef<std::string>(wpi::SmallVectorImpl<std::string>& buf)>
+ getter,
+ std::function<void(wpi::ArrayRef<std::string>)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ wpi::SmallVector<std::string, 16> buf;
+ entry.SetValue(nt::Value::MakeStringArray(getter(buf), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsStringArray()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetStringArray()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
+
+void SendableBuilderImpl::AddSmallRawProperty(
+ const wpi::Twine& key,
+ std::function<wpi::StringRef(wpi::SmallVectorImpl<char>& buf)> getter,
+ std::function<void(wpi::StringRef)> setter) {
+ m_properties.emplace_back(*m_table, key);
+ if (getter) {
+ m_properties.back().update = [=](nt::NetworkTableEntry entry,
+ uint64_t time) {
+ wpi::SmallVector<char, 128> buf;
+ entry.SetValue(nt::Value::MakeRaw(getter(buf), time));
+ };
+ }
+ if (setter) {
+ m_properties.back().createListener =
+ [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
+ return entry.AddListener(
+ [=](const nt::EntryNotification& event) {
+ if (!event.value->IsRaw()) return;
+ SmartDashboard::PostListenerTask(
+ [=] { setter(event.value->GetRaw()); });
+ },
+ NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
+ };
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp
new file mode 100644
index 0000000..2f7af9c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/SendableChooserBase.h"
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+std::atomic_int SendableChooserBase::s_instances{0};
+
+SendableChooserBase::SendableChooserBase() : m_instance{s_instances++} {
+ SendableRegistry::GetInstance().Add(this, "SendableChooser", m_instance);
+}
+
+SendableChooserBase::SendableChooserBase(SendableChooserBase&& oth)
+ : SendableHelper(std::move(oth)),
+ m_defaultChoice(std::move(oth.m_defaultChoice)),
+ m_selected(std::move(oth.m_selected)),
+ m_haveSelected(std::move(oth.m_haveSelected)),
+ m_activeEntries(std::move(oth.m_activeEntries)),
+ m_instance(std::move(oth.m_instance)) {}
+
+SendableChooserBase& SendableChooserBase::operator=(SendableChooserBase&& oth) {
+ SendableHelper::operator=(oth);
+ std::scoped_lock lock(m_mutex, oth.m_mutex);
+ m_defaultChoice = std::move(oth.m_defaultChoice);
+ m_selected = std::move(oth.m_selected);
+ m_haveSelected = std::move(oth.m_haveSelected);
+ m_activeEntries = std::move(oth.m_activeEntries);
+ m_instance = std::move(oth.m_instance);
+ return *this;
+}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
new file mode 100644
index 0000000..9d57ad6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
@@ -0,0 +1,341 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+#include <memory>
+
+#include <wpi/DenseMap.h>
+#include <wpi/SmallVector.h>
+#include <wpi/UidVector.h>
+#include <wpi/mutex.h>
+
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilderImpl.h"
+
+using namespace frc;
+
+struct SendableRegistry::Impl {
+ struct Component {
+ Sendable* sendable = nullptr;
+ SendableBuilderImpl builder;
+ std::string name;
+ std::string subsystem = "Ungrouped";
+ Sendable* parent = nullptr;
+ bool liveWindow = false;
+ wpi::SmallVector<std::shared_ptr<void>, 2> data;
+
+ void SetName(const wpi::Twine& moduleType, int channel) {
+ name =
+ (moduleType + wpi::Twine('[') + wpi::Twine(channel) + wpi::Twine(']'))
+ .str();
+ }
+
+ void SetName(const wpi::Twine& moduleType, int moduleNumber, int channel) {
+ name = (moduleType + wpi::Twine('[') + wpi::Twine(moduleNumber) +
+ wpi::Twine(',') + wpi::Twine(channel) + wpi::Twine(']'))
+ .str();
+ }
+ };
+
+ wpi::recursive_mutex mutex;
+
+ wpi::UidVector<std::unique_ptr<Component>, 32> components;
+ wpi::DenseMap<void*, UID> componentMap;
+ int nextDataHandle = 0;
+
+ Component& GetOrAdd(void* sendable, UID* uid = nullptr);
+};
+
+SendableRegistry::Impl::Component& SendableRegistry::Impl::GetOrAdd(
+ void* sendable, UID* uid) {
+ UID& compUid = componentMap[sendable];
+ if (compUid == 0)
+ compUid = components.emplace_back(std::make_unique<Component>()) + 1;
+ if (uid) *uid = compUid;
+
+ return *components[compUid - 1];
+}
+
+SendableRegistry& SendableRegistry::GetInstance() {
+ static SendableRegistry instance;
+ return instance;
+}
+
+void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.name = name.str();
+}
+
+void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& moduleType,
+ int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.SetName(moduleType, channel);
+}
+
+void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& moduleType,
+ int moduleNumber, int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.SetName(moduleType, moduleNumber, channel);
+}
+
+void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& subsystem,
+ const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.name = name.str();
+ comp.subsystem = subsystem.str();
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.liveWindow = true;
+ comp.name = name.str();
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& moduleType,
+ int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.liveWindow = true;
+ comp.SetName(moduleType, channel);
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& moduleType,
+ int moduleNumber, int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.liveWindow = true;
+ comp.SetName(moduleType, moduleNumber, channel);
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& subsystem,
+ const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(sendable);
+ comp.sendable = sendable;
+ comp.liveWindow = true;
+ comp.name = name.str();
+ comp.subsystem = subsystem.str();
+}
+
+void SendableRegistry::AddChild(Sendable* parent, Sendable* child) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(child);
+ comp.parent = parent;
+}
+
+void SendableRegistry::AddChild(Sendable* parent, void* child) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(child);
+ comp.parent = parent;
+}
+
+bool SendableRegistry::Remove(Sendable* sendable) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return false;
+ UID compUid = it->getSecond();
+ m_impl->components.erase(compUid - 1);
+ m_impl->componentMap.erase(it);
+ // update any parent pointers
+ for (auto&& comp : m_impl->components) {
+ if (comp->parent == sendable) comp->parent = nullptr;
+ }
+ return true;
+}
+
+void SendableRegistry::Move(Sendable* to, Sendable* from) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(from);
+ if (it == m_impl->componentMap.end()) return;
+ UID compUid = it->getSecond();
+ m_impl->componentMap.erase(it);
+ m_impl->componentMap[to] = compUid;
+ auto& comp = *m_impl->components[compUid - 1];
+ comp.sendable = to;
+ if (comp.builder.HasTable()) {
+ // rebuild builder, as lambda captures can point to "from"
+ comp.builder.ClearProperties();
+ to->InitSendable(comp.builder);
+ }
+ // update any parent pointers
+ for (auto&& comp : m_impl->components) {
+ if (comp->parent == from) comp->parent = to;
+ }
+}
+
+bool SendableRegistry::Contains(const Sendable* sendable) const {
+ std::scoped_lock lock(m_impl->mutex);
+ return m_impl->componentMap.count(sendable) != 0;
+}
+
+std::string SendableRegistry::GetName(const Sendable* sendable) const {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return std::string{};
+ return m_impl->components[it->getSecond() - 1]->name;
+}
+
+void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->name = name.str();
+}
+
+void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
+ int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->SetName(moduleType, channel);
+}
+
+void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
+ int moduleNumber, int channel) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->SetName(moduleType, moduleNumber,
+ channel);
+}
+
+void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& subsystem,
+ const wpi::Twine& name) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ auto& comp = *m_impl->components[it->getSecond() - 1];
+ comp.name = name.str();
+ comp.subsystem = subsystem.str();
+}
+
+std::string SendableRegistry::GetSubsystem(const Sendable* sendable) const {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return std::string{};
+ return m_impl->components[it->getSecond() - 1]->subsystem;
+}
+
+void SendableRegistry::SetSubsystem(Sendable* sendable,
+ const wpi::Twine& subsystem) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->subsystem = subsystem.str();
+}
+
+int SendableRegistry::GetDataHandle() {
+ std::scoped_lock lock(m_impl->mutex);
+ return m_impl->nextDataHandle++;
+}
+
+std::shared_ptr<void> SendableRegistry::SetData(Sendable* sendable, int handle,
+ std::shared_ptr<void> data) {
+ assert(handle >= 0);
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return nullptr;
+ auto& comp = *m_impl->components[it->getSecond() - 1];
+ std::shared_ptr<void> rv;
+ if (static_cast<size_t>(handle) < comp.data.size())
+ rv = std::move(comp.data[handle]);
+ else
+ comp.data.resize(handle + 1);
+ comp.data[handle] = std::move(data);
+ return rv;
+}
+
+std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
+ int handle) {
+ assert(handle >= 0);
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return nullptr;
+ auto& comp = *m_impl->components[it->getSecond() - 1];
+ if (static_cast<size_t>(handle) >= comp.data.size()) return nullptr;
+ return comp.data[handle];
+}
+
+void SendableRegistry::EnableLiveWindow(Sendable* sendable) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->liveWindow = true;
+}
+
+void SendableRegistry::DisableLiveWindow(Sendable* sendable) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto it = m_impl->componentMap.find(sendable);
+ if (it == m_impl->componentMap.end()) return;
+ m_impl->components[it->getSecond() - 1]->liveWindow = false;
+}
+
+SendableRegistry::UID SendableRegistry::GetUniqueId(Sendable* sendable) {
+ std::scoped_lock lock(m_impl->mutex);
+ UID uid;
+ auto& comp = m_impl->GetOrAdd(sendable, &uid);
+ comp.sendable = sendable;
+ return uid;
+}
+
+Sendable* SendableRegistry::GetSendable(UID uid) {
+ if (uid == 0) return nullptr;
+ std::scoped_lock lock(m_impl->mutex);
+ return m_impl->components[uid - 1]->sendable;
+}
+
+void SendableRegistry::Publish(UID sendableUid,
+ std::shared_ptr<NetworkTable> table) {
+ std::scoped_lock lock(m_impl->mutex);
+ if (sendableUid == 0) return;
+ auto& comp = *m_impl->components[sendableUid - 1];
+ comp.builder = SendableBuilderImpl{}; // clear any current builder
+ comp.builder.SetTable(table);
+ comp.sendable->InitSendable(comp.builder);
+ comp.builder.UpdateTable();
+ comp.builder.StartListeners();
+}
+
+void SendableRegistry::Update(UID sendableUid) {
+ if (sendableUid == 0) return;
+ std::scoped_lock lock(m_impl->mutex);
+ m_impl->components[sendableUid - 1]->builder.UpdateTable();
+}
+
+void SendableRegistry::ForeachLiveWindow(
+ int dataHandle,
+ wpi::function_ref<void(CallbackData& data)> callback) const {
+ assert(dataHandle >= 0);
+ std::scoped_lock lock(m_impl->mutex);
+ 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);
+ CallbackData cbdata{comp->sendable, comp->name,
+ comp->subsystem, comp->parent,
+ comp->data[dataHandle], comp->builder};
+ callback(cbdata);
+ }
+ }
+}
+
+SendableRegistry::SendableRegistry() : m_impl(new Impl) {}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
new file mode 100644
index 0000000..5e70a4c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -0,0 +1,263 @@
+/*----------------------------------------------------------------------------*/
+/* 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/smartdashboard/SmartDashboard.h"
+
+#include <hal/FRCUsageReporting.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/StringMap.h>
+#include <wpi/mutex.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+namespace {
+class Singleton {
+ public:
+ static Singleton& GetInstance();
+
+ std::shared_ptr<nt::NetworkTable> table;
+ wpi::StringMap<SendableRegistry::UID> tablesToData;
+ wpi::mutex tablesToDataMutex;
+
+ private:
+ Singleton() {
+ table = nt::NetworkTableInstance::GetDefault().GetTable("SmartDashboard");
+ HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0);
+ }
+ Singleton(const Singleton&) = delete;
+ Singleton& operator=(const Singleton&) = delete;
+};
+
+} // namespace
+
+Singleton& Singleton::GetInstance() {
+ static Singleton instance;
+ return instance;
+}
+
+void SmartDashboard::init() { Singleton::GetInstance(); }
+
+bool SmartDashboard::ContainsKey(wpi::StringRef key) {
+ return Singleton::GetInstance().table->ContainsKey(key);
+}
+
+std::vector<std::string> SmartDashboard::GetKeys(int types) {
+ return Singleton::GetInstance().table->GetKeys(types);
+}
+
+void SmartDashboard::SetPersistent(wpi::StringRef key) {
+ Singleton::GetInstance().table->GetEntry(key).SetPersistent();
+}
+
+void SmartDashboard::ClearPersistent(wpi::StringRef key) {
+ Singleton::GetInstance().table->GetEntry(key).ClearPersistent();
+}
+
+bool SmartDashboard::IsPersistent(wpi::StringRef key) {
+ return Singleton::GetInstance().table->GetEntry(key).IsPersistent();
+}
+
+void SmartDashboard::SetFlags(wpi::StringRef key, unsigned int flags) {
+ Singleton::GetInstance().table->GetEntry(key).SetFlags(flags);
+}
+
+void SmartDashboard::ClearFlags(wpi::StringRef key, unsigned int flags) {
+ Singleton::GetInstance().table->GetEntry(key).ClearFlags(flags);
+}
+
+unsigned int SmartDashboard::GetFlags(wpi::StringRef key) {
+ return Singleton::GetInstance().table->GetEntry(key).GetFlags();
+}
+
+void SmartDashboard::Delete(wpi::StringRef key) {
+ 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");
+ return;
+ }
+ auto& inst = Singleton::GetInstance();
+ std::scoped_lock lock(inst.tablesToDataMutex);
+ auto& uid = inst.tablesToData[key];
+ auto& registry = SendableRegistry::GetInstance();
+ Sendable* sddata = registry.GetSendable(uid);
+ if (sddata != data) {
+ uid = registry.GetUniqueId(data);
+ auto dataTable = inst.table->GetSubTable(key);
+ registry.Publish(uid, dataTable);
+ dataTable->GetEntry(".name").SetString(key);
+ }
+}
+
+void SmartDashboard::PutData(Sendable* value) {
+ if (value == nullptr) {
+ wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
+ return;
+ }
+ auto name = SendableRegistry::GetInstance().GetName(value);
+ if (!name.empty()) PutData(name, value);
+}
+
+Sendable* SmartDashboard::GetData(wpi::StringRef key) {
+ auto& inst = Singleton::GetInstance();
+ std::scoped_lock lock(inst.tablesToDataMutex);
+ auto it = inst.tablesToData.find(key);
+ if (it == inst.tablesToData.end()) {
+ wpi_setGlobalWPIErrorWithContext(SmartDashboardMissingKey, key);
+ return nullptr;
+ }
+ return SendableRegistry::GetInstance().GetSendable(it->getValue());
+}
+
+bool SmartDashboard::PutBoolean(wpi::StringRef keyName, bool value) {
+ return Singleton::GetInstance().table->GetEntry(keyName).SetBoolean(value);
+}
+
+bool SmartDashboard::SetDefaultBoolean(wpi::StringRef key, bool defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).SetDefaultBoolean(
+ defaultValue);
+}
+
+bool SmartDashboard::GetBoolean(wpi::StringRef keyName, bool defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(keyName).GetBoolean(
+ defaultValue);
+}
+
+bool SmartDashboard::PutNumber(wpi::StringRef keyName, double value) {
+ return Singleton::GetInstance().table->GetEntry(keyName).SetDouble(value);
+}
+
+bool SmartDashboard::SetDefaultNumber(wpi::StringRef key, double defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).SetDefaultDouble(
+ defaultValue);
+}
+
+double SmartDashboard::GetNumber(wpi::StringRef keyName, double defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(keyName).GetDouble(
+ defaultValue);
+}
+
+bool SmartDashboard::PutString(wpi::StringRef keyName, wpi::StringRef value) {
+ return Singleton::GetInstance().table->GetEntry(keyName).SetString(value);
+}
+
+bool SmartDashboard::SetDefaultString(wpi::StringRef key,
+ wpi::StringRef defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).SetDefaultString(
+ defaultValue);
+}
+
+std::string SmartDashboard::GetString(wpi::StringRef keyName,
+ wpi::StringRef defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(keyName).GetString(
+ defaultValue);
+}
+
+bool SmartDashboard::PutBooleanArray(wpi::StringRef key,
+ wpi::ArrayRef<int> value) {
+ return Singleton::GetInstance().table->GetEntry(key).SetBooleanArray(value);
+}
+
+bool SmartDashboard::SetDefaultBooleanArray(wpi::StringRef key,
+ wpi::ArrayRef<int> defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).SetDefaultBooleanArray(
+ defaultValue);
+}
+
+std::vector<int> SmartDashboard::GetBooleanArray(
+ wpi::StringRef key, wpi::ArrayRef<int> defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).GetBooleanArray(
+ defaultValue);
+}
+
+bool SmartDashboard::PutNumberArray(wpi::StringRef key,
+ wpi::ArrayRef<double> value) {
+ return Singleton::GetInstance().table->GetEntry(key).SetDoubleArray(value);
+}
+
+bool SmartDashboard::SetDefaultNumberArray(wpi::StringRef key,
+ wpi::ArrayRef<double> defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).SetDefaultDoubleArray(
+ defaultValue);
+}
+
+std::vector<double> SmartDashboard::GetNumberArray(
+ wpi::StringRef key, wpi::ArrayRef<double> defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).GetDoubleArray(
+ defaultValue);
+}
+
+bool SmartDashboard::PutStringArray(wpi::StringRef key,
+ wpi::ArrayRef<std::string> value) {
+ return Singleton::GetInstance().table->GetEntry(key).SetStringArray(value);
+}
+
+bool SmartDashboard::SetDefaultStringArray(
+ wpi::StringRef key, wpi::ArrayRef<std::string> defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).SetDefaultStringArray(
+ defaultValue);
+}
+
+std::vector<std::string> SmartDashboard::GetStringArray(
+ wpi::StringRef key, wpi::ArrayRef<std::string> defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).GetStringArray(
+ defaultValue);
+}
+
+bool SmartDashboard::PutRaw(wpi::StringRef key, wpi::StringRef value) {
+ return Singleton::GetInstance().table->GetEntry(key).SetRaw(value);
+}
+
+bool SmartDashboard::SetDefaultRaw(wpi::StringRef key,
+ wpi::StringRef defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).SetDefaultRaw(
+ defaultValue);
+}
+
+std::string SmartDashboard::GetRaw(wpi::StringRef key,
+ wpi::StringRef defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).GetRaw(defaultValue);
+}
+
+bool SmartDashboard::PutValue(wpi::StringRef keyName,
+ std::shared_ptr<nt::Value> value) {
+ return Singleton::GetInstance().table->GetEntry(keyName).SetValue(value);
+}
+
+bool SmartDashboard::SetDefaultValue(wpi::StringRef key,
+ std::shared_ptr<nt::Value> defaultValue) {
+ return Singleton::GetInstance().table->GetEntry(key).SetDefaultValue(
+ defaultValue);
+}
+
+std::shared_ptr<nt::Value> SmartDashboard::GetValue(wpi::StringRef keyName) {
+ return Singleton::GetInstance().table->GetEntry(keyName).GetValue();
+}
+
+detail::ListenerExecutor SmartDashboard::listenerExecutor;
+
+void SmartDashboard::PostListenerTask(std::function<void()> task) {
+ listenerExecutor.Execute(task);
+}
+
+void SmartDashboard::UpdateValues() {
+ auto& registry = SendableRegistry::GetInstance();
+ auto& inst = Singleton::GetInstance();
+ std::scoped_lock lock(inst.tablesToDataMutex);
+ for (auto& i : inst.tablesToData) registry.Update(i.getValue());
+ listenerExecutor.RunListenerTasks();
+}
diff --git a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
new file mode 100644
index 0000000..3991fec
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/CubicHermiteSpline.h"
+
+using namespace frc;
+
+CubicHermiteSpline::CubicHermiteSpline(
+ std::array<double, 2> xInitialControlVector,
+ std::array<double, 2> xFinalControlVector,
+ std::array<double, 2> yInitialControlVector,
+ std::array<double, 2> yFinalControlVector) {
+ const auto hermite = MakeHermiteBasis();
+ const auto x =
+ ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+ const auto y =
+ ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+ // Populate first two rows with coefficients.
+ m_coefficients.template block<1, 4>(0, 0) = hermite * x;
+ m_coefficients.template block<1, 4>(1, 0) = hermite * y;
+
+ // Populate Row 2 and Row 3 with the derivatives of the equations above.
+ // Then populate row 4 and 5 with the second derivatives.
+ for (int i = 0; i < 4; i++) {
+ // 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) * (2 - i);
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
new file mode 100644
index 0000000..bb8ad3c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/QuinticHermiteSpline.h"
+
+using namespace frc;
+
+QuinticHermiteSpline::QuinticHermiteSpline(
+ std::array<double, 3> xInitialControlVector,
+ std::array<double, 3> xFinalControlVector,
+ std::array<double, 3> yInitialControlVector,
+ std::array<double, 3> yFinalControlVector) {
+ const auto hermite = MakeHermiteBasis();
+ const auto x =
+ ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+ const auto y =
+ ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+ // Populate first two rows with coefficients.
+ m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose();
+ m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose();
+
+ // Populate Row 2 and Row 3 with the derivatives of the equations above.
+ // Then populate row 4 and 5 with the second derivatives.
+ for (int i = 0; i < 6; i++) {
+ // 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) * (4 - i);
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
new file mode 100644
index 0000000..cbfc8c1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
@@ -0,0 +1,208 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/SplineHelper.h"
+
+#include <cstddef>
+
+using namespace frc;
+
+std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
+ const Spline<3>::ControlVector& start, std::vector<Translation2d> waypoints,
+ const Spline<3>::ControlVector& end) {
+ std::vector<CubicHermiteSpline> splines;
+
+ std::array<double, 2> xInitial = start.x;
+ std::array<double, 2> yInitial = start.y;
+ std::array<double, 2> xFinal = end.x;
+ std::array<double, 2> yFinal = end.y;
+
+ if (waypoints.size() > 1) {
+ waypoints.emplace(waypoints.begin(),
+ Translation2d{units::meter_t(xInitial[0]),
+ units::meter_t(yInitial[0])});
+ waypoints.emplace_back(
+ Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
+
+ // 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);
+ c.emplace_back(1);
+ }
+ c.emplace_back(0);
+
+ // populate rhs vectors
+ dx.emplace_back(
+ 3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
+ xInitial[1]);
+ dy.emplace_back(
+ 3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
+ yInitial[1]);
+ if (waypoints.size() > 4) {
+ for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
+ dx.emplace_back(3 * (waypoints[i + 1].X().to<double>() -
+ waypoints[i - 1].X().to<double>()));
+ dy.emplace_back(3 * (waypoints[i + 1].Y().to<double>() -
+ waypoints[i - 1].Y().to<double>()));
+ }
+ }
+ dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
+ waypoints[waypoints.size() - 3].X().to<double>()) -
+ xFinal[1]);
+ dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
+ waypoints[waypoints.size() - 3].Y().to<double>()) -
+ yFinal[1]);
+
+ // Compute solution to tridiagonal system
+ ThomasAlgorithm(a, b, c, dx, &fx);
+ ThomasAlgorithm(a, b, c, dy, &fy);
+
+ fx.emplace(fx.begin(), xInitial[1]);
+ fx.emplace_back(xFinal[1]);
+ fy.emplace(fy.begin(), yInitial[1]);
+ fy.emplace_back(yFinal[1]);
+
+ for (size_t i = 0; i < fx.size() - 1; ++i) {
+ // Create the spline.
+ const CubicHermiteSpline spline{
+ {waypoints[i].X().to<double>(), fx[i]},
+ {waypoints[i + 1].X().to<double>(), fx[i + 1]},
+ {waypoints[i].Y().to<double>(), fy[i]},
+ {waypoints[i + 1].Y().to<double>(), fy[i + 1]}};
+
+ splines.push_back(spline);
+ }
+ } else if (waypoints.size() == 1) {
+ const double xDeriv =
+ (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
+ const double yDeriv =
+ (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
+
+ std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
+ xDeriv};
+ std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
+ yDeriv};
+
+ splines.emplace_back(xInitial, midXControlVector, yInitial,
+ midYControlVector);
+ splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal);
+
+ } else {
+ // Create the spline.
+ const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal};
+ splines.push_back(spline);
+ }
+
+ return splines;
+}
+
+std::vector<QuinticHermiteSpline>
+SplineHelper::QuinticSplinesFromControlVectors(
+ const std::vector<Spline<5>::ControlVector>& controlVectors) {
+ std::vector<QuinticHermiteSpline> splines;
+ for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
+ auto& xInitial = controlVectors[i].x;
+ auto& yInitial = controlVectors[i].y;
+ auto& xFinal = controlVectors[i + 1].x;
+ auto& yFinal = controlVectors[i + 1].y;
+ splines.emplace_back(xInitial, xFinal, yInitial, yFinal);
+ }
+ return splines;
+}
+
+std::array<Spline<3>::ControlVector, 2>
+SplineHelper::CubicControlVectorsFromWaypoints(
+ const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+ const Pose2d& end) {
+ double scalar;
+ if (interiorWaypoints.empty()) {
+ scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
+ } else {
+ scalar =
+ 1.2 *
+ start.Translation().Distance(interiorWaypoints.front()).to<double>();
+ }
+ const auto initialCV = CubicControlVector(scalar, start);
+ if (!interiorWaypoints.empty()) {
+ scalar =
+ 1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
+ }
+ const auto finalCV = CubicControlVector(scalar, end);
+ return {initialCV, finalCV};
+}
+
+std::vector<Spline<5>::ControlVector>
+SplineHelper::QuinticControlVectorsFromWaypoints(
+ const std::vector<Pose2d>& waypoints) {
+ std::vector<Spline<5>::ControlVector> vectors;
+ for (size_t i = 0; i < waypoints.size() - 1; ++i) {
+ auto& p0 = waypoints[i];
+ auto& p1 = waypoints[i + 1];
+
+ // This just makes the splines look better.
+ const auto scalar =
+ 1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
+
+ vectors.push_back(QuinticControlVector(scalar, p0));
+ vectors.push_back(QuinticControlVector(scalar, p1));
+ }
+ return vectors;
+}
+
+void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
+ const std::vector<double>& b,
+ const std::vector<double>& c,
+ const std::vector<double>& d,
+ std::vector<double>* solutionVector) {
+ auto& f = *solutionVector;
+ size_t N = d.size();
+
+ // Create the temporary vectors
+ // Note that this is inefficient as it is possible to call
+ // this function many times. A better implementation would
+ // pass these temporary matrices by non-const reference to
+ // save excess allocation and deallocation
+ std::vector<double> c_star(N, 0.0);
+ std::vector<double> d_star(N, 0.0);
+
+ // This updates the coefficients in the first row
+ // Note that we should be checking for division by zero here
+ c_star[0] = c[0] / b[0];
+ d_star[0] = d[0] / b[0];
+
+ // Create the c_star and d_star coefficients in the forward sweep
+ for (size_t i = 1; i < N; ++i) {
+ double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
+ c_star[i] = c[i] * m;
+ d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;
+ }
+
+ f[N - 1] = d_star[N - 1];
+ // This is the reverse sweep, used to update the solution vector f
+ for (int i = N - 2; i >= 0; i--) {
+ f[i] = d_star[i] - c_star[i] * f[i + 1];
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp b/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp
new file mode 100644
index 0000000..f3c42c6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/SplineParameterizer.h"
+
+constexpr units::meter_t frc::SplineParameterizer::kMaxDx;
+constexpr units::meter_t frc::SplineParameterizer::kMaxDy;
+constexpr units::radian_t frc::SplineParameterizer::kMaxDtheta;
diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
new file mode 100644
index 0000000..84d4f37
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -0,0 +1,153 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#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.
+ const auto newT = Lerp(t, endValue.t, i);
+
+ // Find the delta time between the current state and the interpolated state.
+ const auto deltaT = newT - t;
+
+ // If delta time is negative, flip the order of interpolation.
+ if (deltaT < 0_s) return endValue.Interpolate(*this, 1.0 - i);
+
+ // Check whether the robot is reversing at this stage.
+ const auto reversing =
+ velocity < 0_mps ||
+ (units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq);
+
+ // Calculate the new velocity.
+ // v = v_0 + at
+ const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
+
+ // Calculate the change in position.
+ // delta_s = v_0 t + 0.5 at^2
+ const units::meter_t newS =
+ (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
+ (reversing ? -1.0 : 1.0);
+
+ // Return the new state. To find the new position for the new state, we need
+ // to interpolate between the two endpoint poses. The fraction for
+ // interpolation is the change in position (delta s) divided by the total
+ // distance between the two endpoints.
+ const double interpolationFrac =
+ newS / endValue.pose.Translation().Distance(pose.Translation());
+
+ return {newT, newV, acceleration,
+ Lerp(pose, endValue.pose, interpolationFrac),
+ Lerp(curvature, endValue.curvature, interpolationFrac)};
+}
+
+Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
+ m_totalTime = states.back().t;
+}
+
+Trajectory::State Trajectory::Sample(units::second_t t) const {
+ if (t <= m_states.front().t) return m_states.front();
+ if (t >= m_totalTime) return m_states.back();
+
+ // To get the element that we want, we will use a binary search algorithm
+ // instead of iterating over a for-loop. A binary search is O(std::log(n))
+ // whereas searching using a loop is O(n).
+
+ // This starts at 1 because we use the previous state later on for
+ // interpolation.
+ int low = 1;
+ int high = m_states.size() - 1;
+
+ while (low != high) {
+ int mid = (low + high) / 2;
+ if (m_states[mid].t < t) {
+ // This index and everything under it are less than the requested
+ // timestamp. Therefore, we can discard them.
+ low = mid + 1;
+ } else {
+ // t is at least as large as the element at this index. This means that
+ // anything after it cannot be what we are looking for.
+ high = mid;
+ }
+ }
+
+ // High and Low should be the same.
+
+ // The sample's timestamp is now greater than or equal to the requested
+ // timestamp. If it is greater, we need to interpolate between the
+ // previous state and the current state to get the exact state that we
+ // want.
+ const auto sample = m_states[low];
+ const auto prevSample = m_states[low - 1];
+
+ // If the difference in states is negligible, then we are spot on!
+ if (units::math::abs(sample.t - prevSample.t) < 1E-9_s) {
+ return sample;
+ }
+ // Interpolate between the two states for the state that we want.
+ return prevSample.Interpolate(sample,
+ (t - prevSample.t) / (sample.t - prevSample.t));
+}
+
+Trajectory Trajectory::TransformBy(const Transform2d& transform) {
+ auto& firstState = m_states[0];
+ auto& firstPose = firstState.pose;
+
+ // Calculate the transformed first pose.
+ auto newFirstPose = firstPose + transform;
+ auto newStates = m_states;
+ newStates[0].pose = newFirstPose;
+
+ for (unsigned int i = 1; i < newStates.size(); i++) {
+ auto& state = newStates[i];
+ // We are transforming relative to the coordinate frame of the new initial
+ // pose.
+ state.pose = newFirstPose + (state.pose - firstPose);
+ }
+
+ return Trajectory(newStates);
+}
+
+Trajectory Trajectory::RelativeTo(const Pose2d& pose) {
+ auto newStates = m_states;
+ for (auto& state : newStates) {
+ state.pose = state.pose.RelativeTo(pose);
+ }
+ return Trajectory(newStates);
+}
+
+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/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
new file mode 100644
index 0000000..3c95472
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryGenerator.h"
+
+#include <utility>
+
+#include "frc/DriverStation.h"
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "frc/trajectory/TrajectoryParameterizer.h"
+
+using namespace frc;
+
+const Trajectory TrajectoryGenerator::kDoNothingTrajectory(
+ std::vector<Trajectory::State>{Trajectory::State()});
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+ Spline<3>::ControlVector initial,
+ const std::vector<Translation2d>& interiorWaypoints,
+ Spline<3>::ControlVector end, const TrajectoryConfig& config) {
+ const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+
+ // Make theta normal for trajectory generation if path is reversed.
+ // Flip the headings.
+ if (config.IsReversed()) {
+ initial.x[1] *= -1;
+ initial.y[1] *= -1;
+ end.x[1] *= -1;
+ end.y[1] *= -1;
+ }
+
+ std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+ try {
+ points =
+ SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
+ initial, interiorWaypoints, end));
+ } catch (SplineParameterizer::MalformedSplineException& e) {
+ DriverStation::ReportError(e.what());
+ return kDoNothingTrajectory;
+ }
+
+ // After trajectory generation, flip theta back so it's relative to the
+ // field. Also fix curvature.
+ if (config.IsReversed()) {
+ for (auto& point : points) {
+ point = {point.first + flip, -point.second};
+ }
+ }
+
+ return TrajectoryParameterizer::TimeParameterizeTrajectory(
+ points, config.Constraints(), config.StartVelocity(),
+ config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+ config.IsReversed());
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+ const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+ const Pose2d& end, const TrajectoryConfig& config) {
+ auto [startCV, endCV] = SplineHelper::CubicControlVectorsFromWaypoints(
+ start, interiorWaypoints, end);
+ return GenerateTrajectory(startCV, interiorWaypoints, endCV, config);
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+ std::vector<Spline<5>::ControlVector> controlVectors,
+ const TrajectoryConfig& config) {
+ const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+ // Make theta normal for trajectory generation if path is reversed.
+ if (config.IsReversed()) {
+ for (auto& vector : controlVectors) {
+ // Flip the headings.
+ vector.x[1] *= -1;
+ vector.y[1] *= -1;
+ }
+ }
+
+ std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+ try {
+ points = SplinePointsFromSplines(
+ SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
+ } catch (SplineParameterizer::MalformedSplineException& e) {
+ DriverStation::ReportError(e.what());
+ return kDoNothingTrajectory;
+ }
+
+ // After trajectory generation, flip theta back so it's relative to the
+ // field. Also fix curvature.
+ if (config.IsReversed()) {
+ for (auto& point : points) {
+ point = {point.first + flip, -point.second};
+ }
+ }
+
+ return TrajectoryParameterizer::TimeParameterizeTrajectory(
+ points, config.Constraints(), config.StartVelocity(),
+ config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+ config.IsReversed());
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+ const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
+ return GenerateTrajectory(
+ SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config);
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
new file mode 100644
index 0000000..131ae8a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
@@ -0,0 +1,224 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "frc/trajectory/TrajectoryParameterizer.h"
+
+using namespace frc;
+
+Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
+ const std::vector<PoseWithCurvature>& points,
+ const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+ units::meters_per_second_t startVelocity,
+ units::meters_per_second_t endVelocity,
+ units::meters_per_second_t maxVelocity,
+ units::meters_per_second_squared_t maxAcceleration, bool reversed) {
+ std::vector<ConstrainedState> constrainedStates(points.size());
+
+ ConstrainedState predecessor{points.front(), 0_m, startVelocity,
+ -maxAcceleration, maxAcceleration};
+
+ constrainedStates[0] = predecessor;
+
+ // Forward pass
+ for (unsigned int i = 0; i < points.size(); i++) {
+ auto& constrainedState = constrainedStates[i];
+ constrainedState.pose = points[i];
+
+ // Begin constraining based on predecessor
+ units::meter_t ds = constrainedState.pose.first.Translation().Distance(
+ predecessor.pose.first.Translation());
+ constrainedState.distance = ds + predecessor.distance;
+
+ // We may need to iterate to find the maximum end velocity and common
+ // acceleration, since acceleration limits may be a function of velocity.
+ while (true) {
+ // Enforce global max velocity and max reachable velocity by global
+ // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
+
+ constrainedState.maxVelocity = units::math::min(
+ maxVelocity,
+ units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity +
+ predecessor.maxAcceleration * ds * 2.0));
+
+ constrainedState.minAcceleration = -maxAcceleration;
+ constrainedState.maxAcceleration = maxAcceleration;
+
+ // At this point, the constrained state is fully constructed apart from
+ // all the custom-defined user constraints.
+ for (const auto& constraint : constraints) {
+ constrainedState.maxVelocity = units::math::min(
+ constrainedState.maxVelocity,
+ constraint->MaxVelocity(constrainedState.pose.first,
+ constrainedState.pose.second,
+ constrainedState.maxVelocity));
+ }
+
+ // Now enforce all acceleration limits.
+ EnforceAccelerationLimits(reversed, constraints, &constrainedState);
+
+ if (ds.to<double>() < kEpsilon) break;
+
+ // If the actual acceleration for this state is higher than the max
+ // acceleration that we applied, then we need to reduce the max
+ // acceleration of the predecessor and try again.
+ units::meters_per_second_squared_t actualAcceleration =
+ (constrainedState.maxVelocity * constrainedState.maxVelocity -
+ predecessor.maxVelocity * predecessor.maxVelocity) /
+ (ds * 2.0);
+
+ // If we violate the max acceleration constraint, let's modify the
+ // predecessor.
+ if (constrainedState.maxAcceleration < actualAcceleration - 1E-6_mps_sq) {
+ predecessor.maxAcceleration = constrainedState.maxAcceleration;
+ } else {
+ // Constrain the predecessor's max acceleration to the current
+ // acceleration.
+ if (actualAcceleration > predecessor.minAcceleration + 1E-6_mps_sq) {
+ predecessor.maxAcceleration = actualAcceleration;
+ }
+ // If the actual acceleration is less than the predecessor's min
+ // acceleration, it will be repaired in the backward pass.
+ break;
+ }
+ }
+ predecessor = constrainedState;
+ }
+
+ ConstrainedState successor{points.back(), constrainedStates.back().distance,
+ endVelocity, -maxAcceleration, maxAcceleration};
+
+ // Backward pass
+ for (int i = points.size() - 1; i >= 0; i--) {
+ auto& constrainedState = constrainedStates[i];
+ units::meter_t ds =
+ constrainedState.distance - successor.distance; // negative
+
+ while (true) {
+ // Enforce max velocity limit (reverse)
+ // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+ units::meters_per_second_t newMaxVelocity =
+ units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
+ successor.minAcceleration * ds * 2.0);
+
+ // No more limits to impose! This state can be finalized.
+ if (newMaxVelocity >= constrainedState.maxVelocity) break;
+
+ constrainedState.maxVelocity = newMaxVelocity;
+
+ // Check all acceleration constraints with the new max velocity.
+ EnforceAccelerationLimits(reversed, constraints, &constrainedState);
+
+ if (ds.to<double>() > -kEpsilon) break;
+
+ // If the actual acceleration for this state is lower than the min
+ // acceleration, then we need to lower the min acceleration of the
+ // successor and try again.
+ units::meters_per_second_squared_t actualAcceleration =
+ (constrainedState.maxVelocity * constrainedState.maxVelocity -
+ successor.maxVelocity * successor.maxVelocity) /
+ (ds * 2.0);
+ if (constrainedState.minAcceleration > actualAcceleration + 1E-6_mps_sq) {
+ successor.minAcceleration = constrainedState.minAcceleration;
+ } else {
+ successor.minAcceleration = actualAcceleration;
+ break;
+ }
+ }
+ successor = constrainedState;
+ }
+
+ // Now we can integrate the constrained states forward in time to obtain our
+ // trajectory states.
+
+ std::vector<Trajectory::State> states(points.size());
+ units::second_t t = 0_s;
+ units::meter_t s = 0_m;
+ units::meters_per_second_t v = 0_mps;
+
+ for (unsigned int i = 0; i < constrainedStates.size(); i++) {
+ auto state = constrainedStates[i];
+
+ // Calculate the change in position between the current state and the
+ // previous state.
+ units::meter_t ds = state.distance - s;
+
+ // Calculate the acceleration between the current state and the previous
+ // state.
+ units::meters_per_second_squared_t accel =
+ (state.maxVelocity * state.maxVelocity - v * v) / (ds * 2);
+
+ // Calculate dt.
+ units::second_t dt = 0_s;
+ if (i > 0) {
+ states.at(i - 1).acceleration = reversed ? -accel : accel;
+ if (units::math::abs(accel) > 1E-6_mps_sq) {
+ // v_f = v_0 + a * t
+ dt = (state.maxVelocity - v) / accel;
+ } else if (units::math::abs(v) > 1E-6_mps) {
+ // delta_x = v * t
+ dt = ds / v;
+ } else {
+ throw std::runtime_error(
+ "Something went wrong during trajectory generation.");
+ }
+ }
+
+ v = state.maxVelocity;
+ s = state.distance;
+
+ t += dt;
+
+ states[i] = {t, reversed ? -v : v, reversed ? -accel : accel,
+ state.pose.first, state.pose.second};
+ }
+
+ return Trajectory(states);
+}
+
+void TrajectoryParameterizer::EnforceAccelerationLimits(
+ bool reverse,
+ const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+ ConstrainedState* state) {
+ for (auto&& constraint : constraints) {
+ double factor = reverse ? -1.0 : 1.0;
+
+ auto minMaxAccel = constraint->MinMaxAcceleration(
+ state->pose.first, state->pose.second, state->maxVelocity * factor);
+
+ state->minAcceleration = units::math::max(
+ state->minAcceleration,
+ reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
+
+ state->maxAcceleration = units::math::min(
+ state->maxAcceleration,
+ reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration);
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
new file mode 100644
index 0000000..f3cf30d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryUtil.h"
+
+#include <system_error>
+
+#include <wpi/SmallString.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory,
+ const wpi::Twine& path) {
+ std::error_code error_code;
+
+ wpi::SmallString<128> buf;
+ wpi::raw_fd_ostream output{path.toStringRef(buf), error_code};
+ if (error_code) {
+ throw std::runtime_error(("Cannot open file: " + path).str());
+ }
+
+ wpi::json json = trajectory.States();
+ output << json;
+ output.flush();
+}
+
+Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) {
+ std::error_code error_code;
+
+ wpi::SmallString<128> buf;
+ wpi::raw_fd_istream input{path.toStringRef(buf), error_code};
+ if (error_code) {
+ throw std::runtime_error(("Cannot open file: " + path).str());
+ }
+
+ wpi::json json;
+ input >> json;
+
+ return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}
+
+std::string TrajectoryUtil::SerializeTrajectory(const Trajectory& trajectory) {
+ wpi::json json = trajectory.States();
+ return json.dump();
+}
+
+Trajectory TrajectoryUtil::DeserializeTrajectory(
+ const wpi::StringRef json_str) {
+ wpi::json json = wpi::json::parse(json_str);
+ return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
new file mode 100644
index 0000000..bf45c34
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+
+using namespace frc;
+
+CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
+ units::meters_per_second_squared_t maxCentripetalAcceleration)
+ : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
+
+units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) {
+ // ac = v^2 / r
+ // k (curvature) = 1 / r
+
+ // therefore, ac = v^2 * k
+ // ac / k = v^2
+ // v = std::sqrt(ac / k)
+
+ // We have to multiply by 1_rad here to get the units to cancel out nicely.
+ // The units library defines a unit for radians although it is technically
+ // unitless.
+ return units::math::sqrt(m_maxCentripetalAcceleration /
+ units::math::abs(curvature) * 1_rad);
+}
+
+TrajectoryConstraint::MinMax
+CentripetalAccelerationConstraint::MinMaxAcceleration(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) {
+ // The acceleration of the robot has no impact on the centripetal acceleration
+ // of the robot.
+ return {};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
new file mode 100644
index 0000000..8b88bf4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+
+using namespace frc;
+
+DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
+ DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+ : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t velocity) {
+ auto wheelSpeeds =
+ m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
+ wheelSpeeds.Normalize(m_maxSpeed);
+
+ return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
+ const Pose2d& pose, curvature_t curvature,
+ units::meters_per_second_t speed) {
+ return {};
+}
diff --git a/wpilibc/src/main/native/cpp/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 {};
+}