Squashed 'third_party/allwpilib_2019/' content from commit bd05dfa1c
Change-Id: I2b1c2250cdb9b055133780c33593292098c375b7
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: bd05dfa1c7cca74c4fac451e7b9d6a37e7b53447
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..71407da
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* 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/ADXL345_I2C.h"
+
+#include <hal/HAL.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress)
+ : m_i2c(port, deviceAddress) {
+ // 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);
+ SetName("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) {
+ 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 = AllAxes();
+ 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..738bc3e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* 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/ADXL345_SPI.h"
+
+#include <hal/HAL.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range)
+ : m_spi(port) {
+ 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);
+
+ SetName("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);
+}
+
+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) {
+ 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 = AllAxes();
+ 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..d709ae4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADXL362.cpp
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* 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/ADXL362.h"
+
+#include <hal/HAL.h>
+
+#include "frc/DriverStation.h"
+#include "frc/smartdashboard/SendableBuilder.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_spi.SetClockRate(3000000);
+ m_spi.SetMSBFirst();
+ m_spi.SetSampleDataOnTrailingEdge();
+ m_spi.SetClockActiveLow();
+ m_spi.SetChipSelectActiveLow();
+
+ // Validate the part ID
+ uint8_t commands[3];
+ commands[0] = kRegRead;
+ commands[1] = kPartIdRegister;
+ commands[2] = 0;
+ m_spi.Transaction(commands, commands, 3);
+ if (commands[2] != 0xF2) {
+ DriverStation::ReportError("could not find ADXL362");
+ m_gsPerLSB = 0.0;
+ return;
+ }
+
+ 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);
+
+ SetName("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);
+}
+
+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;
+
+ 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 = AllAxes();
+ if (m_gsPerLSB == 0.0) {
+ data.XAxis = data.YAxis = data.ZAxis = 0.0;
+ 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..875f403
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
@@ -0,0 +1,109 @@
+/*----------------------------------------------------------------------------*/
+/* 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/ADXRS450_Gyro.h"
+
+#include <hal/HAL.h>
+
+#include "frc/DriverStation.h"
+#include "frc/Timer.h"
+
+using namespace frc;
+
+static constexpr double kSamplePeriod = 0.0005;
+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_spi.SetClockRate(3000000);
+ m_spi.SetMSBFirst();
+ m_spi.SetSampleDataOnLeadingEdge();
+ m_spi.SetClockActiveHigh();
+ m_spi.SetChipSelectActiveLow();
+
+ // Validate the part ID
+ if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
+ DriverStation::ReportError("could not find ADXRS450 gyro");
+ return;
+ }
+
+ m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c00000eu, 0x04000000u,
+ 10u, 16u, true, true);
+
+ Calibrate();
+
+ HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port);
+ SetName("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 {
+ return m_spi.GetAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
+}
+
+double ADXRS450_Gyro::GetRate() const {
+ return static_cast<double>(m_spi.GetAccumulatorLastValue()) *
+ kDegreePerSecondPerLSB;
+}
+
+void ADXRS450_Gyro::Reset() { 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/AnalogAccelerometer.cpp b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
new file mode 100644
index 0000000..3ae8e48
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* 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/AnalogAccelerometer.h"
+
+#include <hal/HAL.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+AnalogAccelerometer::AnalogAccelerometer(int channel)
+ : AnalogAccelerometer(std::make_shared<AnalogInput>(channel)) {
+ AddChild(m_analogInput);
+}
+
+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());
+ SetName("Accelerometer", m_analogInput->GetChannel());
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogGyro.cpp b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
new file mode 100644
index 0000000..5efe7b3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
@@ -0,0 +1,173 @@
+/*----------------------------------------------------------------------------*/
+/* 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/AnalogGyro.h"
+
+#include <climits>
+#include <utility>
+
+#include <hal/AnalogGyro.h>
+#include <hal/Errors.h>
+#include <hal/HAL.h>
+
+#include "frc/AnalogInput.h"
+#include "frc/Timer.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+AnalogGyro::AnalogGyro(int channel)
+ : AnalogGyro(std::make_shared<AnalogInput>(channel)) {
+ AddChild(m_analog);
+}
+
+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) {
+ AddChild(m_analog);
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ m_gyroHandle = HAL_kInvalidHandle;
+ return;
+ }
+ Reset();
+ }
+}
+
+AnalogGyro::~AnalogGyro() { HAL_FreeAnalogGyro(m_gyroHandle); }
+
+AnalogGyro::AnalogGyro(AnalogGyro&& rhs)
+ : GyroBase(std::move(rhs)), m_analog(std::move(rhs.m_analog)) {
+ std::swap(m_gyroHandle, rhs.m_gyroHandle);
+}
+
+AnalogGyro& AnalogGyro::operator=(AnalogGyro&& rhs) {
+ GyroBase::operator=(std::move(rhs));
+
+ m_analog = std::move(rhs.m_analog);
+ std::swap(m_gyroHandle, rhs.m_gyroHandle);
+
+ return *this;
+}
+
+double AnalogGyro::GetAngle() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetAnalogGyroAngle(m_gyroHandle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+double AnalogGyro::GetRate() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetAnalogGyroRate(m_gyroHandle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+int AnalogGyro::GetCenter() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetAnalogGyroCenter(m_gyroHandle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+double AnalogGyro::GetOffset() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetAnalogGyroOffset(m_gyroHandle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+void AnalogGyro::SetSensitivity(double voltsPerDegreePerSecond) {
+ int32_t status = 0;
+ HAL_SetAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle,
+ voltsPerDegreePerSecond, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void AnalogGyro::SetDeadband(double volts) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogGyroDeadband(m_gyroHandle, volts, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void AnalogGyro::Reset() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_ResetAnalogGyro(m_gyroHandle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ m_analog = nullptr;
+ m_gyroHandle = HAL_kInvalidHandle;
+ return;
+ }
+ }
+
+ int32_t status = 0;
+ HAL_SetupAnalogGyro(m_gyroHandle, &status);
+ if (status != 0) {
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ m_analog = nullptr;
+ m_gyroHandle = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
+ SetName("AnalogGyro", m_analog->GetChannel());
+}
+
+void AnalogGyro::Calibrate() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_CalibrateAnalogGyro(m_gyroHandle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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..52a55d3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogInput.cpp
@@ -0,0 +1,250 @@
+/*----------------------------------------------------------------------------*/
+/* 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/AnalogInput.h"
+
+#include <utility>
+
+#include <hal/AnalogAccumulator.h>
+#include <hal/AnalogInput.h>
+#include <hal/HAL.h>
+#include <hal/Ports.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/Timer.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.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_setErrorWithContextRange(status, 0, HAL_GetNumAnalogInputs(), channel,
+ HAL_GetErrorMessage(status));
+ m_channel = std::numeric_limits<int>::max();
+ m_port = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel);
+ SetName("AnalogInput", channel);
+}
+
+AnalogInput::~AnalogInput() { HAL_FreeAnalogInputPort(m_port); }
+
+AnalogInput::AnalogInput(AnalogInput&& rhs)
+ : ErrorBase(std::move(rhs)),
+ SendableBase(std::move(rhs)),
+ PIDSource(std::move(rhs)),
+ m_channel(std::move(rhs.m_channel)),
+ m_accumulatorOffset(std::move(rhs.m_accumulatorOffset)) {
+ std::swap(m_port, rhs.m_port);
+}
+
+AnalogInput& AnalogInput::operator=(AnalogInput&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+ SendableBase::operator=(std::move(rhs));
+ PIDSource::operator=(std::move(rhs));
+
+ m_channel = std::move(rhs.m_channel);
+ std::swap(m_port, rhs.m_port);
+ m_accumulatorOffset = std::move(rhs.m_accumulatorOffset);
+
+ return *this;
+}
+
+int AnalogInput::GetValue() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetAnalogValue(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+int AnalogInput::GetAverageValue() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetAnalogAverageValue(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+double AnalogInput::GetVoltage() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double voltage = HAL_GetAnalogVoltage(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return voltage;
+}
+
+double AnalogInput::GetAverageVoltage() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double voltage = HAL_GetAnalogAverageVoltage(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+int AnalogInput::GetAverageBits() const {
+ int32_t status = 0;
+ int averageBits = HAL_GetAnalogAverageBits(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return averageBits;
+}
+
+void AnalogInput::SetOversampleBits(int bits) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogOversampleBits(m_port, bits, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+int AnalogInput::GetOversampleBits() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int oversampleBits = HAL_GetAnalogOversampleBits(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return oversampleBits;
+}
+
+int AnalogInput::GetLSBWeight() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int lsbWeight = HAL_GetAnalogLSBWeight(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return lsbWeight;
+}
+
+int AnalogInput::GetOffset() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int offset = HAL_GetAnalogOffset(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return offset;
+}
+
+bool AnalogInput::IsAccumulatorChannel() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool isAccum = HAL_IsAccumulatorChannel(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return isAccum;
+}
+
+void AnalogInput::InitAccumulator() {
+ if (StatusIsFatal()) return;
+ m_accumulatorOffset = 0;
+ int32_t status = 0;
+ HAL_InitAccumulator(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void AnalogInput::SetAccumulatorDeadband(int deadband) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAccumulatorDeadband(m_port, deadband, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+int64_t AnalogInput::GetAccumulatorValue() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int64_t value = HAL_GetAccumulatorValue(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ 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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ value += m_accumulatorOffset;
+}
+
+void AnalogInput::SetSampleRate(double samplesPerSecond) {
+ int32_t status = 0;
+ HAL_SetAnalogSampleRate(samplesPerSecond, &status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+double AnalogInput::GetSampleRate() {
+ int32_t status = 0;
+ double sampleRate = HAL_GetAnalogSampleRate(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return sampleRate;
+}
+
+double AnalogInput::PIDGet() {
+ if (StatusIsFatal()) return 0.0;
+ return GetAverageVoltage();
+}
+
+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..b18af5b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-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/AnalogOutput.h"
+
+#include <limits>
+#include <utility>
+
+#include <hal/HAL.h>
+#include <hal/Ports.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.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_setErrorWithContextRange(status, 0, HAL_GetNumAnalogOutputs(), channel,
+ HAL_GetErrorMessage(status));
+ m_channel = std::numeric_limits<int>::max();
+ m_port = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_AnalogOutput, m_channel);
+ SetName("AnalogOutput", m_channel);
+}
+
+AnalogOutput::~AnalogOutput() { HAL_FreeAnalogOutputPort(m_port); }
+
+AnalogOutput::AnalogOutput(AnalogOutput&& rhs)
+ : ErrorBase(std::move(rhs)),
+ SendableBase(std::move(rhs)),
+ m_channel(std::move(rhs.m_channel)) {
+ std::swap(m_port, rhs.m_port);
+}
+
+AnalogOutput& AnalogOutput::operator=(AnalogOutput&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+ SendableBase::operator=(std::move(rhs));
+
+ m_channel = std::move(rhs.m_channel);
+ std::swap(m_port, rhs.m_port);
+
+ return *this;
+}
+
+void AnalogOutput::SetVoltage(double voltage) {
+ int32_t status = 0;
+ HAL_SetAnalogOutput(m_port, voltage, &status);
+
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+double AnalogOutput::GetVoltage() const {
+ int32_t status = 0;
+ double voltage = HAL_GetAnalogOutput(m_port, &status);
+
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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..6c42ff9
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* 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/AnalogPotentiometer.h"
+
+#include "frc/RobotController.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+AnalogPotentiometer::AnalogPotentiometer(int channel, double fullRange,
+ double offset)
+ : m_analog_input(std::make_shared<AnalogInput>(channel)),
+ m_fullRange(fullRange),
+ m_offset(offset) {
+ AddChild(m_analog_input);
+}
+
+AnalogPotentiometer::AnalogPotentiometer(AnalogInput* input, double fullRange,
+ double offset)
+ : m_analog_input(input, NullDeleter<AnalogInput>()),
+ m_fullRange(fullRange),
+ m_offset(offset) {}
+
+AnalogPotentiometer::AnalogPotentiometer(std::shared_ptr<AnalogInput> input,
+ double fullRange, double offset)
+ : m_analog_input(input), m_fullRange(fullRange), m_offset(offset) {}
+
+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..aeb58df
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* 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/AnalogTrigger.h"
+
+#include <utility>
+
+#include <hal/HAL.h>
+
+#include "frc/AnalogInput.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+AnalogTrigger::AnalogTrigger(int channel)
+ : AnalogTrigger(new AnalogInput(channel)) {
+ m_ownsAnalog = true;
+ AddChild(m_analogInput);
+}
+
+AnalogTrigger::AnalogTrigger(AnalogInput* input) {
+ m_analogInput = input;
+ int32_t status = 0;
+ int index = 0;
+ m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &index, &status);
+ if (status != 0) {
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ m_index = std::numeric_limits<int>::max();
+ m_trigger = HAL_kInvalidHandle;
+ return;
+ }
+ m_index = index;
+
+ HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, input->m_channel);
+ SetName("AnalogTrigger", input->GetChannel());
+}
+
+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)),
+ SendableBase(std::move(rhs)),
+ m_index(std::move(rhs.m_index)) {
+ std::swap(m_trigger, rhs.m_trigger);
+ std::swap(m_analogInput, rhs.m_analogInput);
+ std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
+}
+
+AnalogTrigger& AnalogTrigger::operator=(AnalogTrigger&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+ SendableBase::operator=(std::move(rhs));
+
+ m_index = std::move(rhs.m_index);
+ std::swap(m_trigger, rhs.m_trigger);
+ std::swap(m_analogInput, rhs.m_analogInput);
+ 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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void AnalogTrigger::SetLimitsRaw(int lower, int upper) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogTriggerLimitsRaw(m_trigger, lower, upper, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void AnalogTrigger::SetAveraged(bool useAveragedValue) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogTriggerAveraged(m_trigger, useAveragedValue, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void AnalogTrigger::SetFiltered(bool useFilteredValue) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetAnalogTriggerFiltered(m_trigger, useFilteredValue, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+int AnalogTrigger::GetIndex() const {
+ if (StatusIsFatal()) return -1;
+ return m_index;
+}
+
+bool AnalogTrigger::GetInWindow() {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool result = HAL_GetAnalogTriggerInWindow(m_trigger, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return result;
+}
+
+bool AnalogTrigger::GetTriggerState() {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool result = HAL_GetAnalogTriggerTriggerState(m_trigger, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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..b2a8cd5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* 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/AnalogTriggerOutput.h"
+
+#include <hal/HAL.h>
+
+#include "frc/AnalogTrigger.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+AnalogTriggerOutput::~AnalogTriggerOutput() {
+ if (m_interrupt != HAL_kInvalidHandle) {
+ int32_t status = 0;
+ HAL_CleanInterrupts(m_interrupt, &status);
+ // ignore status, as an invalid handle just needs to be ignored.
+ m_interrupt = HAL_kInvalidHandle;
+ }
+}
+
+bool AnalogTriggerOutput::Get() const {
+ int32_t status = 0;
+ bool result = HAL_GetAnalogTriggerOutput(
+ m_trigger.m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
+ &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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.m_index; }
+
+void AnalogTriggerOutput::InitSendable(SendableBuilder&) {}
+
+AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger& trigger,
+ AnalogTriggerType outputType)
+ : m_trigger(trigger), m_outputType(outputType) {
+ HAL_Report(HALUsageReporting::kResourceType_AnalogTriggerOutput,
+ trigger.GetIndex(), static_cast<uint8_t>(outputType));
+}
diff --git a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
new file mode 100644
index 0000000..8452e38
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-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/BuiltInAccelerometer.h"
+
+#include <hal/Accelerometer.h>
+#include <hal/HAL.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+BuiltInAccelerometer::BuiltInAccelerometer(Range range) {
+ SetRange(range);
+
+ HAL_Report(HALUsageReporting::kResourceType_Accelerometer, 0, 0,
+ "Built-in accelerometer");
+ SetName("BuiltInAccel", 0);
+}
+
+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..f01eb3f
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/CAN.cpp
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* 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/CAN.h"
+
+#include <utility>
+
+#include <hal/CAN.h>
+#include <hal/CANAPI.h>
+#include <hal/Errors.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+
+using namespace frc;
+
+CAN::CAN(int deviceId) {
+ int32_t status = 0;
+ m_handle =
+ HAL_InitializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType, &status);
+ if (status != 0) {
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ m_handle = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId);
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ m_handle = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId);
+}
+
+CAN::~CAN() {
+ if (StatusIsFatal()) return;
+ if (m_handle != HAL_kInvalidHandle) {
+ HAL_CleanCAN(m_handle);
+ m_handle = HAL_kInvalidHandle;
+ }
+}
+
+CAN::CAN(CAN&& rhs) : ErrorBase(std::move(rhs)) {
+ std::swap(m_handle, rhs.m_handle);
+}
+
+CAN& CAN::operator=(CAN&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+
+ std::swap(m_handle, rhs.m_handle);
+
+ return *this;
+}
+
+void CAN::WritePacket(const uint8_t* data, int length, int apiId) {
+ int32_t status = 0;
+ HAL_WriteCANPacket(m_handle, data, length, apiId, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void CAN::WritePacketRepeating(const uint8_t* data, int length, int apiId,
+ int repeatMs) {
+ int32_t status = 0;
+ HAL_WriteCANPacketRepeating(m_handle, data, length, apiId, repeatMs, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void CAN::StopPacketRepeating(int apiId) {
+ int32_t status = 0;
+ HAL_StopCANPacketRepeating(m_handle, apiId, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool CAN::ReadPeriodicPacket(int apiId, int timeoutMs, int periodMs,
+ CANData* data) {
+ int32_t status = 0;
+ HAL_ReadCANPeriodicPacket(m_handle, apiId, data->data, &data->length,
+ &data->timestamp, timeoutMs, periodMs, &status);
+ if (status == HAL_CAN_TIMEOUT ||
+ status == HAL_ERR_CANSessionMux_MessageNotFound) {
+ return false;
+ }
+ if (status != 0) {
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return false;
+ } else {
+ return true;
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp
new file mode 100644
index 0000000..48e1ed6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -0,0 +1,218 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-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/Compressor.h"
+
+#include <hal/Compressor.h>
+#include <hal/HAL.h>
+#include <hal/Ports.h>
+#include <hal/Solenoid.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.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_setErrorWithContextRange(status, 0, HAL_GetNumPCMModules(), pcmID,
+ HAL_GetErrorMessage(status));
+ return;
+ }
+ SetClosedLoopControl(true);
+
+ HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID);
+ SetName("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/ControllerPower.cpp b/wpilibc/src/main/native/cpp/ControllerPower.cpp
new file mode 100644
index 0000000..d3012ea
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ControllerPower.cpp
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ControllerPower.h"
+
+#include <stdint.h>
+
+#include <hal/HAL.h>
+#include <hal/Power.h>
+
+#include "frc/ErrorBase.h"
+
+using namespace frc;
+
+double ControllerPower::GetInputVoltage() {
+ int32_t status = 0;
+ double retVal = HAL_GetVinVoltage(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double ControllerPower::GetInputCurrent() {
+ int32_t status = 0;
+ double retVal = HAL_GetVinCurrent(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double ControllerPower::GetVoltage3V3() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserVoltage3V3(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double ControllerPower::GetCurrent3V3() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserCurrent3V3(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+bool ControllerPower::GetEnabled3V3() {
+ int32_t status = 0;
+ bool retVal = HAL_GetUserActive3V3(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+int ControllerPower::GetFaultCount3V3() {
+ int32_t status = 0;
+ int retVal = HAL_GetUserCurrentFaults3V3(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double ControllerPower::GetVoltage5V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserVoltage5V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double ControllerPower::GetCurrent5V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserCurrent5V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+bool ControllerPower::GetEnabled5V() {
+ int32_t status = 0;
+ bool retVal = HAL_GetUserActive5V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+int ControllerPower::GetFaultCount5V() {
+ int32_t status = 0;
+ int retVal = HAL_GetUserCurrentFaults5V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double ControllerPower::GetVoltage6V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserVoltage6V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double ControllerPower::GetCurrent6V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserCurrent6V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+bool ControllerPower::GetEnabled6V() {
+ int32_t status = 0;
+ bool retVal = HAL_GetUserActive6V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+int ControllerPower::GetFaultCount6V() {
+ int32_t status = 0;
+ int retVal = HAL_GetUserCurrentFaults6V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
diff --git a/wpilibc/src/main/native/cpp/Counter.cpp b/wpilibc/src/main/native/cpp/Counter.cpp
new file mode 100644
index 0000000..c97bbc5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Counter.cpp
@@ -0,0 +1,358 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Counter.h"
+
+#include <utility>
+
+#include <hal/HAL.h>
+
+#include "frc/AnalogTrigger.h"
+#include "frc/DigitalInput.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+Counter::Counter(Mode mode) {
+ int32_t status = 0;
+ m_counter = HAL_InitializeCounter((HAL_Counter_Mode)mode, &m_index, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ SetMaxPeriod(.5);
+
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index, mode);
+ SetName("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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ SetDownSourceEdge(inverted, true);
+}
+
+Counter::~Counter() {
+ SetUpdateWhenEmpty(true);
+
+ int32_t status = 0;
+ HAL_FreeCounter(m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ m_counter = HAL_kInvalidHandle;
+}
+
+Counter::Counter(Counter&& rhs)
+ : ErrorBase(std::move(rhs)),
+ SendableBase(std::move(rhs)),
+ CounterBase(std::move(rhs)),
+ m_upSource(std::move(rhs.m_upSource)),
+ m_downSource(std::move(rhs.m_downSource)),
+ m_index(std::move(rhs.m_index)) {
+ std::swap(m_counter, rhs.m_counter);
+}
+
+Counter& Counter::operator=(Counter&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+ SendableBase::operator=(std::move(rhs));
+ CounterBase::operator=(std::move(rhs));
+
+ m_upSource = std::move(rhs.m_upSource);
+ m_downSource = std::move(rhs.m_downSource);
+ std::swap(m_counter, rhs.m_counter);
+ m_index = std::move(rhs.m_index);
+
+ return *this;
+}
+
+void Counter::SetUpSource(int channel) {
+ if (StatusIsFatal()) return;
+ SetUpSource(std::make_shared<DigitalInput>(channel));
+ AddChild(m_upSource);
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Counter::ClearUpSource() {
+ if (StatusIsFatal()) return;
+ m_upSource.reset();
+ int32_t status = 0;
+ HAL_ClearCounterUpSource(m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Counter::SetDownSource(int channel) {
+ if (StatusIsFatal()) return;
+ SetDownSource(std::make_shared<DigitalInput>(channel));
+ AddChild(m_downSource);
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Counter::ClearDownSource() {
+ if (StatusIsFatal()) return;
+ m_downSource.reset();
+ int32_t status = 0;
+ HAL_ClearCounterDownSource(m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Counter::SetUpDownCounterMode() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterUpDownMode(m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Counter::SetExternalDirectionMode() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterExternalDirectionMode(m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Counter::SetPulseLengthMode(double threshold) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterPulseLengthMode(m_counter, threshold, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Counter::SetReverseDirection(bool reverseDirection) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterReverseDirection(m_counter, reverseDirection, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+int Counter::GetSamplesToAverage() const {
+ int32_t status = 0;
+ int samples = HAL_GetCounterSamplesToAverage(m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+void Counter::Reset() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_ResetCounter(m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+double Counter::GetPeriod() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetCounterPeriod(m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+void Counter::SetMaxPeriod(double maxPeriod) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterMaxPeriod(m_counter, maxPeriod, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Counter::SetUpdateWhenEmpty(bool enabled) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetCounterUpdateWhenEmpty(m_counter, enabled, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+bool Counter::GetStopped() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = HAL_GetCounterStopped(m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+bool Counter::GetDirection() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = HAL_GetCounterDirection(m_counter, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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/DMC60.cpp b/wpilibc/src/main/native/cpp/DMC60.cpp
new file mode 100644
index 0000000..7dc2d8d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DMC60.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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/DMC60.h"
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+DMC60::DMC60(int channel) : PWMSpeedController(channel) {
+ /*
+ * Note that the DMC 60 uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the DMC 60 User
+ * Manual available from Digilent.
+ *
+ * 2.004ms = full "forward"
+ * 1.52ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.48ms = the "low end" of the deadband range
+ * 0.997ms = full "reverse"
+ */
+ SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_DigilentDMC60, GetChannel());
+ SetName("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..71211b3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
@@ -0,0 +1,162 @@
+/*----------------------------------------------------------------------------*/
+/* 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/DigitalGlitchFilter.h"
+
+#include <algorithm>
+#include <array>
+#include <utility>
+
+#include <hal/Constants.h>
+#include <hal/DIO.h>
+#include <hal/HAL.h>
+
+#include "frc/Counter.h"
+#include "frc/Encoder.h"
+#include "frc/SensorUtil.h"
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+std::array<bool, 3> DigitalGlitchFilter::m_filterAllocated = {
+ {false, false, false}};
+wpi::mutex DigitalGlitchFilter::m_mutex;
+
+DigitalGlitchFilter::DigitalGlitchFilter() {
+ std::lock_guard<wpi::mutex> 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);
+ SetName("DigitalGlitchFilter", m_channelIndex);
+}
+
+DigitalGlitchFilter::~DigitalGlitchFilter() {
+ if (m_channelIndex >= 0) {
+ std::lock_guard<wpi::mutex> lock(m_mutex);
+ m_filterAllocated[m_channelIndex] = false;
+ }
+}
+
+DigitalGlitchFilter::DigitalGlitchFilter(DigitalGlitchFilter&& rhs)
+ : ErrorBase(std::move(rhs)), SendableBase(std::move(rhs)) {
+ std::swap(m_channelIndex, rhs.m_channelIndex);
+}
+
+DigitalGlitchFilter& DigitalGlitchFilter::operator=(DigitalGlitchFilter&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+ SendableBase::operator=(std::move(rhs));
+
+ 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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ // Validate that we set it correctly.
+ int actualIndex =
+ HAL_GetFilterSelect(input->GetPortHandleForRouting(), &status);
+ wpi_assertEqual(actualIndex, requestedIndex);
+
+ HAL_Report(HALUsageReporting::kResourceType_DigitalInput,
+ input->GetChannel());
+ }
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+int DigitalGlitchFilter::GetPeriodCycles() {
+ int32_t status = 0;
+ int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
+
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ return fpgaCycles;
+}
+
+uint64_t DigitalGlitchFilter::GetPeriodNanoSeconds() {
+ int32_t status = 0;
+ int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
+
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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..273e9b6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* 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/DigitalInput.h"
+
+#include <limits>
+#include <utility>
+
+#include <hal/DIO.h>
+#include <hal/HAL.h>
+#include <hal/Ports.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.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_setErrorWithContextRange(status, 0, HAL_GetNumDigitalChannels(),
+ channel, HAL_GetErrorMessage(status));
+ m_handle = HAL_kInvalidHandle;
+ m_channel = std::numeric_limits<int>::max();
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel);
+ SetName("DigitalInput", channel);
+}
+
+DigitalInput::~DigitalInput() {
+ if (StatusIsFatal()) return;
+ if (m_interrupt != HAL_kInvalidHandle) {
+ int32_t status = 0;
+ HAL_CleanInterrupts(m_interrupt, &status);
+ // Ignore status, as an invalid handle just needs to be ignored.
+ m_interrupt = HAL_kInvalidHandle;
+ }
+
+ HAL_FreeDIOPort(m_handle);
+}
+
+DigitalInput::DigitalInput(DigitalInput&& rhs)
+ : DigitalSource(std::move(rhs)), m_channel(std::move(rhs.m_channel)) {
+ std::swap(m_handle, rhs.m_handle);
+}
+
+DigitalInput& DigitalInput::operator=(DigitalInput&& rhs) {
+ DigitalSource::operator=(std::move(rhs));
+
+ m_channel = std::move(rhs.m_channel);
+ std::swap(m_handle, rhs.m_handle);
+
+ return *this;
+}
+
+bool DigitalInput::Get() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = HAL_GetDIO(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+HAL_Handle DigitalInput::GetPortHandleForRouting() const { return m_handle; }
+
+AnalogTriggerType DigitalInput::GetAnalogTriggerTypeForRouting() const {
+ return (AnalogTriggerType)0;
+}
+
+bool DigitalInput::IsAnalogTrigger() const { return false; }
+
+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..b05c6b1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
@@ -0,0 +1,165 @@
+/*----------------------------------------------------------------------------*/
+/* 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/DigitalOutput.h"
+
+#include <limits>
+#include <utility>
+
+#include <hal/DIO.h>
+#include <hal/HAL.h>
+#include <hal/Ports.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.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_setErrorWithContextRange(status, 0, HAL_GetNumDigitalChannels(),
+ channel, HAL_GetErrorMessage(status));
+ m_channel = std::numeric_limits<int>::max();
+ m_handle = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel);
+ SetName("DigitalOutput", channel);
+}
+
+DigitalOutput::~DigitalOutput() {
+ if (StatusIsFatal()) return;
+ // Disable the PWM in case it was running.
+ DisablePWM();
+
+ HAL_FreeDIOPort(m_handle);
+}
+
+DigitalOutput::DigitalOutput(DigitalOutput&& rhs)
+ : ErrorBase(std::move(rhs)),
+ SendableBase(std::move(rhs)),
+ m_channel(std::move(rhs.m_channel)),
+ m_pwmGenerator(std::move(rhs.m_pwmGenerator)) {
+ std::swap(m_handle, rhs.m_handle);
+}
+
+DigitalOutput& DigitalOutput::operator=(DigitalOutput&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+ SendableBase::operator=(std::move(rhs));
+
+ m_channel = std::move(rhs.m_channel);
+ std::swap(m_handle, rhs.m_handle);
+ m_pwmGenerator = std::move(rhs.m_pwmGenerator);
+
+ return *this;
+}
+
+void DigitalOutput::Set(bool value) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ HAL_SetDIO(m_handle, value, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+bool DigitalOutput::Get() const {
+ if (StatusIsFatal()) return false;
+
+ int32_t status = 0;
+ bool val = HAL_GetDIO(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return val;
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+bool DigitalOutput::IsPulsing() const {
+ if (StatusIsFatal()) return false;
+
+ int32_t status = 0;
+ bool value = HAL_IsPulsing(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+void DigitalOutput::SetPWMRate(double rate) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ HAL_SetDigitalPWMRate(rate, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ if (StatusIsFatal()) return;
+ HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, initialDutyCycle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ if (StatusIsFatal()) return;
+ HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, m_channel, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ HAL_FreeDigitalPWM(m_pwmGenerator, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ m_pwmGenerator = HAL_kInvalidHandle;
+}
+
+void DigitalOutput::UpdateDutyCycle(double dutyCycle) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+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..86678aa
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
@@ -0,0 +1,186 @@
+/*----------------------------------------------------------------------------*/
+/* 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/DoubleSolenoid.h"
+
+#include <utility>
+
+#include <hal/HAL.h>
+#include <hal/Ports.h>
+#include <hal/Solenoid.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.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_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
+ forwardChannel, HAL_GetErrorMessage(status));
+ m_forwardHandle = HAL_kInvalidHandle;
+ m_reverseHandle = HAL_kInvalidHandle;
+ return;
+ }
+
+ m_reverseHandle = HAL_InitializeSolenoidPort(
+ HAL_GetPortWithModule(moduleNumber, m_reverseChannel), &status);
+ if (status != 0) {
+ wpi_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
+ reverseChannel, HAL_GetErrorMessage(status));
+ // 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,
+ m_moduleNumber);
+ HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel,
+ m_moduleNumber);
+ SetName("DoubleSolenoid", m_moduleNumber, m_forwardChannel);
+}
+
+DoubleSolenoid::~DoubleSolenoid() {
+ HAL_FreeSolenoidPort(m_forwardHandle);
+ HAL_FreeSolenoidPort(m_reverseHandle);
+}
+
+DoubleSolenoid::DoubleSolenoid(DoubleSolenoid&& rhs)
+ : SolenoidBase(std::move(rhs)),
+ m_forwardChannel(std::move(rhs.m_forwardChannel)),
+ m_reverseChannel(std::move(rhs.m_reverseChannel)),
+ m_forwardMask(std::move(rhs.m_forwardMask)),
+ m_reverseMask(std::move(rhs.m_reverseMask)) {
+ std::swap(m_forwardHandle, rhs.m_forwardHandle);
+ std::swap(m_reverseHandle, rhs.m_reverseHandle);
+}
+
+DoubleSolenoid& DoubleSolenoid::operator=(DoubleSolenoid&& rhs) {
+ SolenoidBase::operator=(std::move(rhs));
+
+ m_forwardChannel = std::move(rhs.m_forwardChannel);
+ m_reverseChannel = std::move(rhs.m_reverseChannel);
+ m_forwardMask = std::move(rhs.m_forwardMask);
+ m_reverseMask = std::move(rhs.m_reverseMask);
+ std::swap(m_forwardHandle, rhs.m_forwardHandle);
+ std::swap(m_reverseHandle, rhs.m_reverseHandle);
+
+ return *this;
+}
+
+void DoubleSolenoid::Set(Value value) {
+ if (StatusIsFatal()) return;
+
+ 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_setErrorWithContext(fstatus, HAL_GetErrorMessage(fstatus));
+ wpi_setErrorWithContext(rstatus, HAL_GetErrorMessage(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_setErrorWithContext(fstatus, HAL_GetErrorMessage(fstatus));
+ wpi_setErrorWithContext(rstatus, HAL_GetErrorMessage(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..3c274c6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DriverStation.cpp
@@ -0,0 +1,621 @@
+/*----------------------------------------------------------------------------*/
+/* 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/DriverStation.h"
+
+#include <chrono>
+
+#include <hal/HAL.h>
+#include <hal/Power.h>
+#include <hal/cpp/Log.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+#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<wpi::mutex> 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<wpi::mutex> 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::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;
+}
+
+bool DriverStation::IsSysActive() const {
+ int32_t status = 0;
+ bool retVal = HAL_GetSystemActive(&status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+bool DriverStation::IsBrownedOut() const {
+ int32_t status = 0;
+ bool retVal = HAL_GetBrownedOut(&status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+std::string DriverStation::GetGameSpecificMessage() const {
+ HAL_MatchInfo info;
+ HAL_GetMatchInfo(&info);
+ 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<wpi::mutex> 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::GetData() {
+ {
+ // Compute the pressed and released buttons
+ HAL_JoystickButtons currentButtons;
+ std::unique_lock<wpi::mutex> 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;
+ }
+ }
+
+ {
+ std::lock_guard<wpi::mutex> waitLock(m_waitForDataMutex);
+ // Nofify all threads
+ m_waitForDataCounter++;
+ m_waitForDataCond.notify_all();
+ }
+
+ 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/Encoder.cpp b/wpilibc/src/main/native/cpp/Encoder.cpp
new file mode 100644
index 0000000..77e7a2a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Encoder.cpp
@@ -0,0 +1,287 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Encoder.h"
+
+#include <utility>
+
+#include <hal/Encoder.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.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);
+ AddChild(m_aSource);
+ AddChild(m_bSource);
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+Encoder::Encoder(Encoder&& rhs)
+ : ErrorBase(std::move(rhs)),
+ SendableBase(std::move(rhs)),
+ CounterBase(std::move(rhs)),
+ PIDSource(std::move(rhs)),
+ m_aSource(std::move(rhs.m_aSource)),
+ m_bSource(std::move(rhs.m_bSource)),
+ m_indexSource(std::move(rhs.m_indexSource)) {
+ std::swap(m_encoder, rhs.m_encoder);
+}
+
+Encoder& Encoder::operator=(Encoder&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+ SendableBase::operator=(std::move(rhs));
+ CounterBase::operator=(std::move(rhs));
+ PIDSource::operator=(std::move(rhs));
+
+ m_aSource = std::move(rhs.m_aSource);
+ m_bSource = std::move(rhs.m_bSource);
+ m_indexSource = std::move(rhs.m_indexSource);
+ std::swap(m_encoder, rhs.m_encoder);
+
+ return *this;
+}
+
+int Encoder::Get() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetEncoder(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+void Encoder::Reset() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_ResetEncoder(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+double Encoder::GetPeriod() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetEncoderPeriod(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+void Encoder::SetMaxPeriod(double maxPeriod) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+bool Encoder::GetStopped() const {
+ if (StatusIsFatal()) return true;
+ int32_t status = 0;
+ bool value = HAL_GetEncoderStopped(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+bool Encoder::GetDirection() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = HAL_GetEncoderDirection(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+int Encoder::GetRaw() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int value = HAL_GetEncoderRaw(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+int Encoder::GetEncodingScale() const {
+ int32_t status = 0;
+ int val = HAL_GetEncoderEncodingScale(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return val;
+}
+
+double Encoder::GetDistance() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetEncoderDistance(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+double Encoder::GetRate() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = HAL_GetEncoderRate(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return value;
+}
+
+void Encoder::SetMinRate(double minRate) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetEncoderMinRate(m_encoder, minRate, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Encoder::SetDistancePerPulse(double distancePerPulse) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetEncoderDistancePerPulse(m_encoder, distancePerPulse, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+double Encoder::GetDistancePerPulse() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double distancePerPulse = HAL_GetEncoderDistancePerPulse(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return distancePerPulse;
+}
+
+void Encoder::SetReverseDirection(bool reverseDirection) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetEncoderReverseDirection(m_encoder, reverseDirection, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+int Encoder::GetSamplesToAverage() const {
+ int32_t status = 0;
+ int result = HAL_GetEncoderSamplesToAverage(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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);
+ AddChild(m_indexSource);
+ 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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+int Encoder::GetFPGAIndex() const {
+ int32_t status = 0;
+ int val = HAL_GetEncoderFPGAIndex(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return val;
+}
+
+void Encoder::InitSendable(SendableBuilder& builder) {
+ int32_t status = 0;
+ HAL_EncoderEncodingType type = HAL_GetEncoderEncodingType(m_encoder, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ 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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex(),
+ encodingType);
+ SetName("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_setErrorWithContext(status, HAL_GetErrorMessage(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..f758032
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Error.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Error.h"
+
+#include <wpi/Path.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(']'),
+ 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..947e53b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ErrorBase.cpp
@@ -0,0 +1,152 @@
+/*----------------------------------------------------------------------------*/
+/* 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/ErrorBase.h"
+
+#include <cerrno>
+#include <cstdio>
+#include <cstring>
+#include <set>
+
+#include <hal/HAL.h>
+#include <wpi/Format.h>
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#define WPI_ERRORS_DEFINE_STRINGS
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+static wpi::mutex globalErrorsMutex;
+static std::set<Error> globalErrors;
+
+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.
+ std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
+ 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.
+ std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
+ 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.
+ std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
+ 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.
+ std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
+ 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.
+ std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
+ 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) {
+ std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
+
+ // Set the current error information for this object.
+ globalErrors.emplace(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) {
+ std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
+ globalErrors.emplace(-1, errorMessage + ": " + contextMessage, filename,
+ function, lineNumber, nullptr);
+}
+
+const Error& ErrorBase::GetGlobalError() {
+ std::lock_guard<wpi::mutex> mutex(globalErrorsMutex);
+ return *globalErrors.begin();
+}
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/GamepadBase.cpp b/wpilibc/src/main/native/cpp/GamepadBase.cpp
new file mode 100644
index 0000000..4eeb744
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/GamepadBase.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/GamepadBase.h"
+
+using namespace frc;
+
+GamepadBase::GamepadBase(int port) : GenericHID(port) {}
diff --git a/wpilibc/src/main/native/cpp/GearTooth.cpp b/wpilibc/src/main/native/cpp/GearTooth.cpp
new file mode 100644
index 0000000..fba5a24
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/GearTooth.cpp
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* 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/GearTooth.h"
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+constexpr double GearTooth::kGearToothThreshold;
+
+GearTooth::GearTooth(int channel, bool directionSensitive) : Counter(channel) {
+ EnableDirectionSensing(directionSensitive);
+ SetName("GearTooth", channel);
+}
+
+GearTooth::GearTooth(DigitalSource* source, bool directionSensitive)
+ : Counter(source) {
+ EnableDirectionSensing(directionSensitive);
+ SetName("GearTooth", source->GetChannel());
+}
+
+GearTooth::GearTooth(std::shared_ptr<DigitalSource> source,
+ bool directionSensitive)
+ : Counter(source) {
+ EnableDirectionSensing(directionSensitive);
+ SetName("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..d1a3112
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/GenericHID.cpp
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* 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/GenericHID.h"
+
+#include <hal/HAL.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..4b18f73
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/I2C.cpp
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* 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/I2C.h"
+
+#include <utility>
+
+#include <hal/HAL.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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ HAL_Report(HALUsageReporting::kResourceType_I2C, deviceAddress);
+}
+
+I2C::~I2C() { HAL_CloseI2C(m_port); }
+
+I2C::I2C(I2C&& rhs)
+ : ErrorBase(std::move(rhs)),
+ m_deviceAddress(std::move(rhs.m_deviceAddress)) {
+ std::swap(m_port, rhs.m_port);
+}
+
+I2C& I2C::operator=(I2C&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+
+ std::swap(m_port, rhs.m_port);
+ m_deviceAddress = std::move(rhs.m_deviceAddress);
+
+ return *this;
+}
+
+bool I2C::Transaction(uint8_t* dataToSend, int sendSize, uint8_t* dataReceived,
+ int receiveSize) {
+ int32_t status = 0;
+ status = HAL_TransactionI2C(m_port, m_deviceAddress, dataToSend, sendSize,
+ dataReceived, receiveSize);
+ // wpi_setErrorWithContext(status, HAL_GetErrorMessage(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..220043f
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* 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/InterruptableSensorBase.h"
+
+#include <hal/HAL.h>
+
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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;
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void InterruptableSensorBase::DisableInterrupts() {
+ if (StatusIsFatal()) return;
+ wpi_assert(m_interrupt != HAL_kInvalidHandle);
+ int32_t status = 0;
+ HAL_DisableInterrupts(m_interrupt, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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..0f3add3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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/IterativeRobot.h"
+
+#include <hal/HAL.h>
+
+#include "frc/DriverStation.h"
+
+using namespace frc;
+
+static constexpr double kPacketPeriod = 0.02;
+
+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();
+
+ LoopFunc();
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
new file mode 100644
index 0000000..771d81f
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -0,0 +1,167 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/IterativeRobotBase.h"
+
+#include <cstdio>
+
+#include <hal/HAL.h>
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/DriverStation.h"
+#include "frc/Timer.h"
+#include "frc/commands/Scheduler.h"
+#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SmartDashboard.h"
+
+using namespace frc;
+
+IterativeRobotBase::IterativeRobotBase(double period)
+ : m_period(period),
+ m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {}
+
+void IterativeRobotBase::RobotInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+}
+
+void IterativeRobotBase::DisabledInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+}
+
+void IterativeRobotBase::AutonomousInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+}
+
+void IterativeRobotBase::TeleopInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+}
+
+void IterativeRobotBase::TestInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+}
+
+void IterativeRobotBase::RobotPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+ firstRun = false;
+ }
+}
+
+void IterativeRobotBase::DisabledPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+ firstRun = false;
+ }
+}
+
+void IterativeRobotBase::AutonomousPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+ firstRun = false;
+ }
+}
+
+void IterativeRobotBase::TeleopPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+ firstRun = false;
+ }
+}
+
+void IterativeRobotBase::TestPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload 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);
+ 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);
+ 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);
+ TeleopInit();
+ m_watchdog.AddEpoch("TeleopInit()");
+ m_lastMode = Mode::kTeleop;
+ Scheduler::GetInstance()->SetEnabled(true);
+ }
+
+ 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);
+ TestInit();
+ m_watchdog.AddEpoch("TestInit()");
+ m_lastMode = Mode::kTest;
+ }
+
+ HAL_ObserveUserProgramTest();
+ TestPeriodic();
+ m_watchdog.AddEpoch("TestPeriodic()");
+ }
+
+ RobotPeriodic();
+ m_watchdog.AddEpoch("RobotPeriodic()");
+ m_watchdog.Disable();
+ SmartDashboard::UpdateValues();
+
+ LiveWindow::GetInstance()->UpdateValues();
+
+ // 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 " << m_period << "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..7bde6ba
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Jaguar.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Jaguar.h"
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+Jaguar::Jaguar(int channel) : PWMSpeedController(channel) {
+ /* Input profile defined by Luminary Micro.
+ *
+ * Full reverse ranges from 0.671325ms to 0.6972211ms
+ * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
+ * Neutral ranges from 1.4482078ms to 1.5517922ms
+ * Proportional forward ranges from 1.5517922ms to 2.3027789ms
+ * Full forward ranges from 2.3027789ms to 2.328675ms
+ */
+ SetBounds(2.31, 1.55, 1.507, 1.454, .697);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel());
+ SetName("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..da77d94
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Joystick.cpp
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Joystick.h"
+
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/DriverStation.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+constexpr double kPi = 3.14159265358979323846;
+
+Joystick::Joystick(int port) : GenericHID(port) {
+ m_axes[Axis::kX] = kDefaultXChannel;
+ m_axes[Axis::kY] = kDefaultYChannel;
+ m_axes[Axis::kZ] = kDefaultZChannel;
+ m_axes[Axis::kTwist] = kDefaultTwistChannel;
+ m_axes[Axis::kThrottle] = kDefaultThrottleChannel;
+
+ HAL_Report(HALUsageReporting::kResourceType_Joystick, port);
+}
+
+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;
+}
+
+void Joystick::SetAxisChannel(AxisType axis, int channel) {
+ m_axes[axis] = channel;
+}
+
+int Joystick::GetXChannel() const { return m_axes[Axis::kX]; }
+
+int Joystick::GetYChannel() const { return m_axes[Axis::kY]; }
+
+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]);
+}
+
+double Joystick::GetAxis(AxisType axis) const {
+ switch (axis) {
+ case kXAxis:
+ return GetX();
+ case kYAxis:
+ return GetY();
+ case kZAxis:
+ return GetZ();
+ case kTwistAxis:
+ return GetTwist();
+ case kThrottleAxis:
+ return GetThrottle();
+ default:
+ wpi_setWPIError(BadJoystickAxis);
+ return 0.0;
+ }
+}
+
+bool Joystick::GetTrigger() const { return GetRawButton(Button::kTrigger); }
+
+bool Joystick::GetTriggerPressed() {
+ 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); }
+
+Joystick* Joystick::GetStickForPort(int port) {
+ static std::array<std::unique_ptr<Joystick>, DriverStation::kJoystickPorts>
+ joysticks{};
+ auto stick = joysticks[port].get();
+ if (stick == nullptr) {
+ joysticks[port] = std::make_unique<Joystick>(port);
+ stick = joysticks[port].get();
+ }
+ return stick;
+}
+
+bool Joystick::GetButton(ButtonType button) const {
+ int temp = button;
+ return GetRawButton(static_cast<Button>(temp));
+}
+
+double Joystick::GetMagnitude() const {
+ return std::sqrt(std::pow(GetX(), 2) + std::pow(GetY(), 2));
+}
+
+double Joystick::GetDirectionRadians() const {
+ return std::atan2(GetX(), -GetY());
+}
+
+double Joystick::GetDirectionDegrees() const {
+ return (180 / kPi) * GetDirectionRadians();
+}
diff --git a/wpilibc/src/main/native/cpp/JoystickBase.cpp b/wpilibc/src/main/native/cpp/JoystickBase.cpp
new file mode 100644
index 0000000..8e6858c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/JoystickBase.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/JoystickBase.h"
+
+using namespace frc;
+
+JoystickBase::JoystickBase(int port) : GenericHID(port) {}
diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp
new file mode 100644
index 0000000..aff85d8
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* 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/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::lock_guard<wpi::mutex> lock(listMutex);
+ instanceList.insert(this);
+}
+
+MotorSafety::~MotorSafety() {
+ std::lock_guard<wpi::mutex> 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) {
+ 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::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
+}
+
+void MotorSafety::SetExpiration(double expirationTime) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_expiration = expirationTime;
+}
+
+double MotorSafety::GetExpiration() const {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ return m_expiration;
+}
+
+bool MotorSafety::IsAlive() const {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
+}
+
+void MotorSafety::SetSafetyEnabled(bool enabled) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_enabled = enabled;
+}
+
+bool MotorSafety::IsSafetyEnabled() const {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ return m_enabled;
+}
+
+void MotorSafety::Check() {
+ bool enabled;
+ double stopTime;
+
+ {
+ std::lock_guard<wpi::mutex> 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::lock_guard<wpi::mutex> 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..0ccc7c7
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/NidecBrushless.h"
+
+#include <hal/HAL.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
+ : m_dio(dioChannel), m_pwm(pwmChannel) {
+ AddChild(&m_dio);
+ AddChild(&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);
+ SetName("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..69a9f4e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Notifier.cpp
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Notifier.h"
+
+#include <utility>
+
+#include <hal/HAL.h>
+
+#include "frc/Timer.h"
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+Notifier::Notifier(TimerEventHandler 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_setErrorWithContext(status, HAL_GetErrorMessage(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;
+
+ TimerEventHandler handler;
+ {
+ std::lock_guard<wpi::mutex> 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_setErrorWithContext(status, HAL_GetErrorMessage(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::SetHandler(TimerEventHandler handler) {
+ std::lock_guard<wpi::mutex> lock(m_processMutex);
+ m_handler = handler;
+}
+
+void Notifier::StartSingle(double delay) {
+ std::lock_guard<wpi::mutex> lock(m_processMutex);
+ m_periodic = false;
+ m_period = delay;
+ m_expirationTime = Timer::GetFPGATimestamp() + m_period;
+ UpdateAlarm();
+}
+
+void Notifier::StartPeriodic(double period) {
+ std::lock_guard<wpi::mutex> lock(m_processMutex);
+ m_periodic = true;
+ m_period = period;
+ m_expirationTime = Timer::GetFPGATimestamp() + m_period;
+ UpdateAlarm();
+}
+
+void Notifier::Stop() {
+ int32_t status = 0;
+ HAL_CancelNotifierAlarm(m_notifier, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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..872a45a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PIDBase.cpp
@@ -0,0 +1,360 @@
+/*----------------------------------------------------------------------------*/
+/* 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/PIDBase.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/PIDOutput.h"
+#include "frc/smartdashboard/SendableBuilder.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)
+ : SendableBase(false) {
+ m_P = Kp;
+ m_I = Ki;
+ m_D = Kd;
+ m_F = Kf;
+
+ // Save original source
+ m_origSource = std::shared_ptr<PIDSource>(&source, NullDeleter<PIDSource>());
+
+ // Create LinearDigitalFilter with original source as its source argument
+ m_filter = LinearDigitalFilter::MovingAverage(m_origSource, 1);
+ m_pidInput = &m_filter;
+
+ m_pidOutput = &output;
+
+ m_setpointTimer.Start();
+
+ static int instances = 0;
+ instances++;
+ HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
+ SetName("PIDController", instances);
+}
+
+double PIDBase::Get() const {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ return m_result;
+}
+
+void PIDBase::SetContinuous(bool continuous) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_continuous = continuous;
+}
+
+void PIDBase::SetInputRange(double minimumInput, double maximumInput) {
+ {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+ m_inputRange = maximumInput - minimumInput;
+ }
+
+ SetSetpoint(m_setpoint);
+}
+
+void PIDBase::SetOutputRange(double minimumOutput, double maximumOutput) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_minimumOutput = minimumOutput;
+ m_maximumOutput = maximumOutput;
+}
+
+void PIDBase::SetPID(double p, double i, double d) {
+ {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ }
+}
+
+void PIDBase::SetPID(double p, double i, double d, double f) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ m_F = f;
+}
+
+void PIDBase::SetP(double p) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_P = p;
+}
+
+void PIDBase::SetI(double i) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_I = i;
+}
+
+void PIDBase::SetD(double d) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_D = d;
+}
+
+void PIDBase::SetF(double f) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_F = f;
+}
+
+double PIDBase::GetP() const {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ return m_P;
+}
+
+double PIDBase::GetI() const {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ return m_I;
+}
+
+double PIDBase::GetD() const {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ return m_D;
+}
+
+double PIDBase::GetF() const {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ return m_F;
+}
+
+void PIDBase::SetSetpoint(double setpoint) {
+ {
+ std::lock_guard<wpi::mutex> 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::lock_guard<wpi::mutex> lock(m_thisMutex);
+ return m_setpoint;
+}
+
+double PIDBase::GetDeltaSetpoint() const {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
+}
+
+double PIDBase::GetError() const {
+ double setpoint = GetSetpoint();
+ {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ 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::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_toleranceType = kPercentTolerance;
+ m_tolerance = percent;
+}
+
+void PIDBase::SetAbsoluteTolerance(double absTolerance) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_toleranceType = kAbsoluteTolerance;
+ m_tolerance = absTolerance;
+}
+
+void PIDBase::SetPercentTolerance(double percent) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_toleranceType = kPercentTolerance;
+ m_tolerance = percent;
+}
+
+void PIDBase::SetToleranceBuffer(int bufLength) {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+
+ // Create LinearDigitalFilter with original source as its source argument
+ m_filter = LinearDigitalFilter::MovingAverage(m_origSource, bufLength);
+ m_pidInput = &m_filter;
+}
+
+bool PIDBase::OnTarget() const {
+ double error = GetError();
+
+ std::lock_guard<wpi::mutex> 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::lock_guard<wpi::mutex> 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_origSource == nullptr || m_pidOutput == nullptr) return;
+
+ bool enabled;
+ {
+ std::lock_guard<wpi::mutex> 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::lock_guard<wpi::mutex> lock(m_thisMutex);
+
+ input = 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::lock_guard<wpi::mutex> pidWriteLock(m_pidWriteMutex);
+ std::unique_lock<wpi::mutex> mainLock(m_thisMutex);
+ if (m_enabled) {
+ // Don't block other PIDBase operations on PIDWrite()
+ mainLock.unlock();
+
+ m_pidOutput->PIDWrite(result);
+ }
+ }
+
+ std::lock_guard<wpi::mutex> 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..b1c51c6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PIDController.cpp
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* 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/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(period);
+}
+
+PIDController::~PIDController() {
+ // Forcefully stopping the notifier so the callback can successfully run.
+ m_controlLoop->Stop();
+}
+
+void PIDController::Enable() {
+ {
+ std::lock_guard<wpi::mutex> lock(m_thisMutex);
+ m_enabled = true;
+ }
+}
+
+void PIDController::Disable() {
+ {
+ // Ensures m_enabled modification and PIDWrite() call occur atomically
+ std::lock_guard<wpi::mutex> pidWriteLock(m_pidWriteMutex);
+ {
+ std::lock_guard<wpi::mutex> 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::lock_guard<wpi::mutex> 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..603c94b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWM.cpp
@@ -0,0 +1,220 @@
+/*----------------------------------------------------------------------------*/
+/* 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/PWM.h"
+
+#include <utility>
+
+#include <hal/HAL.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"
+
+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_setErrorWithContextRange(status, 0, HAL_GetNumPWMChannels(), channel,
+ HAL_GetErrorMessage(status));
+ m_channel = std::numeric_limits<int>::max();
+ m_handle = HAL_kInvalidHandle;
+ return;
+ }
+
+ m_channel = channel;
+
+ HAL_SetPWMDisabled(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ status = 0;
+ HAL_SetPWMEliminateDeadband(m_handle, false, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ HAL_Report(HALUsageReporting::kResourceType_PWM, channel);
+ SetName("PWM", channel);
+
+ SetSafetyEnabled(false);
+}
+
+PWM::~PWM() {
+ int32_t status = 0;
+
+ HAL_SetPWMDisabled(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ HAL_FreePWMPort(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+PWM::PWM(PWM&& rhs)
+ : MotorSafety(std::move(rhs)),
+ SendableBase(std::move(rhs)),
+ m_channel(std::move(rhs.m_channel)) {
+ std::swap(m_handle, rhs.m_handle);
+}
+
+PWM& PWM::operator=(PWM&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+ SendableBase::operator=(std::move(rhs));
+
+ m_channel = std::move(rhs.m_channel);
+ std::swap(m_handle, rhs.m_handle);
+
+ return *this;
+}
+
+void PWM::StopMotor() { SetDisabled(); }
+
+void PWM::GetDescription(wpi::raw_ostream& desc) const {
+ desc << "PWM " << GetChannel();
+}
+
+void PWM::SetRaw(uint16_t value) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ HAL_SetPWMRaw(m_handle, value, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+uint16_t PWM::GetRaw() const {
+ if (StatusIsFatal()) return 0;
+
+ int32_t status = 0;
+ uint16_t value = HAL_GetPWMRaw(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ return value;
+}
+
+void PWM::SetPosition(double pos) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetPWMPosition(m_handle, pos, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+double PWM::GetPosition() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double position = HAL_GetPWMPosition(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return position;
+}
+
+void PWM::SetSpeed(double speed) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetPWMSpeed(m_handle, speed, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ Feed();
+}
+
+double PWM::GetSpeed() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double speed = HAL_GetPWMSpeed(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return speed;
+}
+
+void PWM::SetDisabled() {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+
+ HAL_SetPWMDisabled(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void PWM::SetZeroLatch() {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+
+ HAL_LatchPWMZero(m_handle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetPWMEliminateDeadband(m_handle, eliminateDeadband, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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/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/PWMTalonSRX.cpp b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
new file mode 100644
index 0000000..ccb50b6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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/PWMTalonSRX.h"
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+PWMTalonSRX::PWMTalonSRX(int channel) : PWMSpeedController(channel) {
+ /* Note that the PWMTalonSRX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the TalonSRX User
+ * Manual available from Cross The Road Electronics.
+ * 2.004ms = full "forward"
+ * 1.52ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.48ms = the "low end" of the deadband range
+ * 0.997ms = full "reverse"
+ */
+ SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel());
+ SetName("PWMTalonSRX", 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..b4f1fd2
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* 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/PWMVictorSPX.h"
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+PWMVictorSPX::PWMVictorSPX(int channel) : PWMSpeedController(channel) {
+ /* Note that the PWMVictorSPX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the VictorSPX User
+ * Manual available from Cross The Road Electronics.
+ * 2.004ms = full "forward"
+ * 1.52ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.48ms = the "low end" of the deadband range
+ * 0.997ms = full "reverse"
+ */
+ SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_PWMVictorSPX, GetChannel());
+ SetName("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..8872028
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-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/PowerDistributionPanel.h"
+
+#include <hal/HAL.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"
+
+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_setErrorWithContextRange(status, 0, HAL_GetNumPDPModules(), module,
+ HAL_GetErrorMessage(status));
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_PDP, module);
+ SetName("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..7a896d1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Preferences.cpp
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/Preferences.h"
+
+#include <algorithm>
+
+#include <hal/HAL.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..9128d20
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Relay.cpp
@@ -0,0 +1,233 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Relay.h"
+
+#include <utility>
+
+#include <hal/HAL.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"
+
+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_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
+ channel, HAL_GetErrorMessage(status));
+ m_forwardHandle = HAL_kInvalidHandle;
+ m_reverseHandle = HAL_kInvalidHandle;
+ return;
+ }
+ HAL_Report(HALUsageReporting::kResourceType_Relay, m_channel);
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ int32_t status = 0;
+ m_reverseHandle = HAL_InitializeRelayPort(portHandle, false, &status);
+ if (status != 0) {
+ wpi_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
+ channel, HAL_GetErrorMessage(status));
+ 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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ m_forwardHandle = HAL_kInvalidHandle;
+ m_reverseHandle = HAL_kInvalidHandle;
+ return;
+ }
+ }
+
+ SetName("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);
+}
+
+Relay::Relay(Relay&& rhs)
+ : MotorSafety(std::move(rhs)),
+ SendableBase(std::move(rhs)),
+ m_channel(std::move(rhs.m_channel)),
+ m_direction(std::move(rhs.m_direction)) {
+ std::swap(m_forwardHandle, rhs.m_forwardHandle);
+ std::swap(m_reverseHandle, rhs.m_reverseHandle);
+}
+
+Relay& Relay::operator=(Relay&& rhs) {
+ MotorSafety::operator=(std::move(rhs));
+ SendableBase::operator=(std::move(rhs));
+
+ m_channel = std::move(rhs.m_channel);
+ m_direction = std::move(rhs.m_direction);
+ std::swap(m_forwardHandle, rhs.m_forwardHandle);
+ std::swap(m_reverseHandle, rhs.m_reverseHandle);
+
+ return *this;
+}
+
+void Relay::Set(Relay::Value value) {
+ if (StatusIsFatal()) return;
+
+ 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_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(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..e2c53d4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Resource.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* 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/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::lock_guard<wpi::mutex> 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::lock_guard<wpi::mutex> 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::lock_guard<wpi::mutex> 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<wpi::mutex> 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/RobotBase.cpp b/wpilibc/src/main/native/cpp/RobotBase.cpp
new file mode 100644
index 0000000..bc316f4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/RobotBase.cpp
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/RobotBase.h"
+
+#include <cstdio>
+
+#include <cameraserver/CameraServerShared.h>
+#include <cscore.h>
+#include <hal/HAL.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "WPILibVersion.h"
+#include "frc/DriverStation.h"
+#include "frc/RobotState.h"
+#include "frc/Utility.h"
+#include "frc/WPIErrors.h"
+#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SmartDashboard.h"
+
+using namespace frc;
+
+int frc::RunHALInitialization() {
+ if (!HAL_Initialize(500, 0)) {
+ wpi::errs() << "FATAL ERROR: HAL could not be initialized\n";
+ return -1;
+ }
+ HAL_Report(HALUsageReporting::kResourceType_Language,
+ HALUsageReporting::kLanguage_CPlusPlus);
+ wpi::outs() << "\n********** Robot program starting **********\n";
+ return 0;
+}
+
+std::thread::id RobotBase::m_threadId;
+
+namespace {
+class WPILibCameraServerShared : public frc::CameraServerShared {
+ public:
+ void ReportUsbCamera(int id) override {
+ HAL_Report(HALUsageReporting::kResourceType_UsbCamera, id);
+ }
+ void ReportAxisCamera(int id) override {
+ HAL_Report(HALUsageReporting::kResourceType_AxisCamera, id);
+ }
+ void ReportVideoServer(int id) override {
+ HAL_Report(HALUsageReporting::kResourceType_PCVideoServer, id);
+ }
+ void SetCameraServerError(const wpi::Twine& error) override {
+ wpi_setGlobalWPIErrorWithContext(CameraServerError, error);
+ }
+ void SetVisionRunnerError(const wpi::Twine& error) override {
+ wpi_setGlobalErrorWithContext(-1, error);
+ }
+ void ReportDriverStationError(const wpi::Twine& error) override {
+ DriverStation::ReportError(error);
+ }
+ std::pair<std::thread::id, bool> GetRobotMainThreadId() const override {
+ return std::make_pair(RobotBase::GetThreadId(), true);
+ }
+};
+} // namespace
+
+static void SetupCameraServerShared() {
+ SetCameraServerShared(std::make_unique<WPILibCameraServerShared>());
+}
+
+bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
+
+bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
+
+bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
+
+bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
+
+bool RobotBase::IsTest() const { return m_ds.IsTest(); }
+
+bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); }
+
+std::thread::id RobotBase::GetThreadId() { return m_threadId; }
+
+RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
+ if (!HAL_Initialize(500, 0)) {
+ wpi::errs() << "FATAL ERROR: HAL could not be initialized\n";
+ wpi::errs().flush();
+ std::terminate();
+ }
+ m_threadId = std::this_thread::get_id();
+
+ SetupCameraServerShared();
+
+ auto inst = nt::NetworkTableInstance::GetDefault();
+ inst.SetNetworkIdentity("Robot");
+ inst.StartServer("/home/lvuser/networktables.ini");
+
+ SmartDashboard::init();
+
+ if (IsReal()) {
+ std::FILE* file = nullptr;
+ file = std::fopen("/tmp/frc_versions/FRC_Lib_Version.ini", "w");
+
+ if (file != nullptr) {
+ std::fputs("C++ ", file);
+ std::fputs(GetWPILibVersion(), file);
+ std::fclose(file);
+ }
+ }
+
+ // First and one-time initialization
+ inst.GetTable("LiveWindow")
+ ->GetSubTable(".status")
+ ->GetEntry("LW Enabled")
+ .SetBoolean(false);
+
+ LiveWindow::GetInstance()->SetEnabled(false);
+}
+
+RobotBase::RobotBase(RobotBase&&) : m_ds(DriverStation::GetInstance()) {}
+
+RobotBase::~RobotBase() { cs::Shutdown(); }
+
+RobotBase& RobotBase::operator=(RobotBase&&) { return *this; }
diff --git a/wpilibc/src/main/native/cpp/RobotController.cpp b/wpilibc/src/main/native/cpp/RobotController.cpp
new file mode 100644
index 0000000..347440d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/RobotController.cpp
@@ -0,0 +1,174 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/RobotController.h"
+
+#include <hal/HAL.h>
+
+#include "frc/ErrorBase.h"
+
+using namespace frc;
+
+int RobotController::GetFPGAVersion() {
+ int32_t status = 0;
+ int version = HAL_GetFPGAVersion(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return version;
+}
+
+int64_t RobotController::GetFPGARevision() {
+ int32_t status = 0;
+ int64_t revision = HAL_GetFPGARevision(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return revision;
+}
+
+uint64_t RobotController::GetFPGATime() {
+ int32_t status = 0;
+ uint64_t time = HAL_GetFPGATime(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(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_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+bool RobotController::IsBrownedOut() {
+ int32_t status = 0;
+ bool retVal = HAL_GetBrownedOut(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double RobotController::GetInputVoltage() {
+ int32_t status = 0;
+ double retVal = HAL_GetVinVoltage(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double RobotController::GetInputCurrent() {
+ int32_t status = 0;
+ double retVal = HAL_GetVinCurrent(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double RobotController::GetVoltage3V3() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserVoltage3V3(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double RobotController::GetCurrent3V3() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserCurrent3V3(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+bool RobotController::GetEnabled3V3() {
+ int32_t status = 0;
+ bool retVal = HAL_GetUserActive3V3(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+int RobotController::GetFaultCount3V3() {
+ int32_t status = 0;
+ int retVal = HAL_GetUserCurrentFaults3V3(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double RobotController::GetVoltage5V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserVoltage5V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double RobotController::GetCurrent5V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserCurrent5V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+bool RobotController::GetEnabled5V() {
+ int32_t status = 0;
+ bool retVal = HAL_GetUserActive5V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+int RobotController::GetFaultCount5V() {
+ int32_t status = 0;
+ int retVal = HAL_GetUserCurrentFaults5V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double RobotController::GetVoltage6V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserVoltage6V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+double RobotController::GetCurrent6V() {
+ int32_t status = 0;
+ double retVal = HAL_GetUserCurrent6V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+bool RobotController::GetEnabled6V() {
+ int32_t status = 0;
+ bool retVal = HAL_GetUserActive6V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+int RobotController::GetFaultCount6V() {
+ int32_t status = 0;
+ int retVal = HAL_GetUserCurrentFaults6V(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(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_setGlobalErrorWithContext(status, HAL_GetErrorMessage(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..a20eb65
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/RobotDrive.cpp
@@ -0,0 +1,427 @@
+/*----------------------------------------------------------------------------*/
+/* 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/RobotDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.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, GetNumMotors(),
+ HALUsageReporting::kRobotDrive_ArcadeRatioCurve);
+ reported = true;
+ }
+
+ if (curve < 0) {
+ double value = std::log(-curve);
+ double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+ if (ratio == 0) ratio = .0000000001;
+ leftOutput = outputMagnitude / ratio;
+ rightOutput = outputMagnitude;
+ } else if (curve > 0) {
+ double value = std::log(curve);
+ double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+ if (ratio == 0) ratio = .0000000001;
+ 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, GetNumMotors(),
+ HALUsageReporting::kRobotDrive_Tank);
+ 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, GetNumMotors(),
+ HALUsageReporting::kRobotDrive_ArcadeStandard);
+ 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, GetNumMotors(),
+ HALUsageReporting::kRobotDrive_MecanumCartesian);
+ 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, GetNumMotors(),
+ HALUsageReporting::kRobotDrive_MecanumPolar);
+ 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..f5da5c1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/RobotState.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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/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::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..977ae7b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SD540.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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/SD540.h"
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+SD540::SD540(int channel) : PWMSpeedController(channel) {
+ /* Note that the SD540 uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the SD540 User Manual available
+ * from Mindsensors.
+ *
+ * 2.05ms = full "forward"
+ * 1.55ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.44ms = the "low end" of the deadband range
+ * 0.94ms = full "reverse"
+ */
+ SetBounds(2.05, 1.55, 1.50, 1.44, .94);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel());
+ SetName("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..6b41adb
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SPI.cpp
@@ -0,0 +1,442 @@
+/*----------------------------------------------------------------------------*/
+/* 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/SPI.h"
+
+#include <cstring>
+#include <utility>
+
+#include <hal/HAL.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::lock_guard<wpi::mutex> 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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ static int instances = 0;
+ instances++;
+ HAL_Report(HALUsageReporting::kResourceType_SPI, instances);
+}
+
+SPI::~SPI() { HAL_CloseSPI(m_port); }
+
+SPI::SPI(SPI&& rhs)
+ : ErrorBase(std::move(rhs)),
+ m_msbFirst(std::move(rhs.m_msbFirst)),
+ m_sampleOnTrailing(std::move(rhs.m_sampleOnTrailing)),
+ m_clockIdleHigh(std::move(rhs.m_clockIdleHigh)),
+ m_accum(std::move(rhs.m_accum)) {
+ std::swap(m_port, rhs.m_port);
+}
+
+SPI& SPI::operator=(SPI&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+
+ std::swap(m_port, rhs.m_port);
+ m_msbFirst = std::move(rhs.m_msbFirst);
+ m_sampleOnTrailing = std::move(rhs.m_sampleOnTrailing);
+ m_clockIdleHigh = std::move(rhs.m_clockIdleHigh);
+ m_accum = std::move(rhs.m_accum);
+
+ return *this;
+}
+
+void SPI::SetClockRate(double hz) { HAL_SetSPISpeed(m_port, hz); }
+
+void SPI::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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SPI::SetChipSelectActiveLow() {
+ int32_t status = 0;
+ HAL_SetSPIChipSelectActiveLow(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SPI::FreeAuto() {
+ int32_t status = 0;
+ HAL_FreeSPIAuto(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SPI::SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize) {
+ int32_t status = 0;
+ HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(),
+ zeroSize, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SPI::StartAutoRate(double period) {
+ int32_t status = 0;
+ HAL_StartSPIAutoRate(m_port, period, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SPI::StopAuto() {
+ int32_t status = 0;
+ HAL_StopSPIAuto(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SPI::ForceAutoRead() {
+ int32_t status = 0;
+ HAL_ForceSPIAutoRead(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout) {
+ int32_t status = 0;
+ int32_t val =
+ HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead, timeout, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return val;
+}
+
+int SPI::GetAutoDroppedCount() {
+ int32_t status = 0;
+ int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return val;
+}
+
+void SPI::InitAccumulator(double 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::FreeAccumulator() {
+ m_accum.reset(nullptr);
+ FreeAuto();
+}
+
+void SPI::ResetAccumulator() {
+ if (!m_accum) return;
+ std::lock_guard<wpi::mutex> 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::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ m_accum->m_center = center;
+}
+
+void SPI::SetAccumulatorDeadband(int deadband) {
+ if (!m_accum) return;
+ std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ m_accum->m_deadband = deadband;
+}
+
+int SPI::GetAccumulatorLastValue() const {
+ if (!m_accum) return 0;
+ std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ m_accum->Update();
+ return m_accum->m_lastValue;
+}
+
+int64_t SPI::GetAccumulatorValue() const {
+ if (!m_accum) return 0;
+ std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ m_accum->Update();
+ return m_accum->m_value;
+}
+
+int64_t SPI::GetAccumulatorCount() const {
+ if (!m_accum) return 0;
+ std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ m_accum->Update();
+ return m_accum->m_count;
+}
+
+double SPI::GetAccumulatorAverage() const {
+ if (!m_accum) return 0;
+ std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ 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::lock_guard<wpi::mutex> 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::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ m_accum->m_integratedCenter = center;
+}
+
+double SPI::GetAccumulatorIntegratedValue() const {
+ if (!m_accum) return 0;
+ std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ m_accum->Update();
+ return m_accum->m_integratedValue;
+}
+
+double SPI::GetAccumulatorIntegratedAverage() const {
+ if (!m_accum) return 0;
+ std::lock_guard<wpi::mutex> lock(m_accum->m_mutex);
+ 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/SampleRobot.cpp b/wpilibc/src/main/native/cpp/SampleRobot.cpp
new file mode 100644
index 0000000..338f7be
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SampleRobot.cpp
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/SampleRobot.h"
+
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <networktables/NetworkTable.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/DriverStation.h"
+#include "frc/Timer.h"
+#include "frc/livewindow/LiveWindow.h"
+
+using namespace frc;
+
+void SampleRobot::StartCompetition() {
+ LiveWindow* lw = LiveWindow::GetInstance();
+
+ RobotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL_ObserveUserProgramStarting();
+
+ RobotMain();
+
+ if (!m_robotMainOverridden) {
+ while (true) {
+ if (IsDisabled()) {
+ m_ds.InDisabled(true);
+ Disabled();
+ m_ds.InDisabled(false);
+ while (IsDisabled()) m_ds.WaitForData();
+ } else if (IsAutonomous()) {
+ m_ds.InAutonomous(true);
+ Autonomous();
+ m_ds.InAutonomous(false);
+ while (IsAutonomous() && IsEnabled()) m_ds.WaitForData();
+ } else if (IsTest()) {
+ lw->SetEnabled(true);
+ m_ds.InTest(true);
+ Test();
+ m_ds.InTest(false);
+ while (IsTest() && IsEnabled()) m_ds.WaitForData();
+ lw->SetEnabled(false);
+ } else {
+ m_ds.InOperatorControl(true);
+ OperatorControl();
+ m_ds.InOperatorControl(false);
+ while (IsOperatorControl() && IsEnabled()) m_ds.WaitForData();
+ }
+ }
+ }
+}
+
+void SampleRobot::RobotInit() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+}
+
+void SampleRobot::Disabled() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+}
+
+void SampleRobot::Autonomous() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+}
+
+void SampleRobot::OperatorControl() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+}
+
+void SampleRobot::Test() {
+ wpi::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n";
+}
+
+void SampleRobot::RobotMain() { m_robotMainOverridden = false; }
+
+SampleRobot::SampleRobot() {
+ HAL_Report(HALUsageReporting::kResourceType_Framework,
+ HALUsageReporting::kFramework_Simple);
+}
diff --git a/wpilibc/src/main/native/cpp/SensorUtil.cpp b/wpilibc/src/main/native/cpp/SensorUtil.cpp
new file mode 100644
index 0000000..2012da4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SensorUtil.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* 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/SensorUtil.h"
+
+#include <hal/AnalogInput.h>
+#include <hal/AnalogOutput.h>
+#include <hal/DIO.h>
+#include <hal/HAL.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..a399f4d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SerialPort.cpp
@@ -0,0 +1,203 @@
+/*----------------------------------------------------------------------------*/
+/* 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/SerialPort.h"
+
+#include <utility>
+
+#include <hal/HAL.h>
+#include <hal/SerialPort.h>
+
+// static ViStatus _VI_FUNCH ioCompleteHandler (ViSession vi, ViEventType
+// eventType, ViEvent event, ViAddr userHandle);
+
+using namespace frc;
+
+SerialPort::SerialPort(int baudRate, Port port, int dataBits,
+ SerialPort::Parity parity,
+ SerialPort::StopBits stopBits) {
+ int32_t status = 0;
+
+ m_port = port;
+
+ HAL_InitializeSerialPort(static_cast<HAL_SerialPort>(port), &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ // Don't continue if initialization failed
+ if (status < 0) return;
+ HAL_SetSerialBaudRate(static_cast<HAL_SerialPort>(port), baudRate, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ HAL_SetSerialDataBits(static_cast<HAL_SerialPort>(port), dataBits, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ HAL_SetSerialParity(static_cast<HAL_SerialPort>(port), parity, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ HAL_SetSerialStopBits(static_cast<HAL_SerialPort>(port), stopBits, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ // Set the default timeout to 5 seconds.
+ SetTimeout(5.0);
+
+ // Don't wait until the buffer is full to transmit.
+ SetWriteBufferMode(kFlushOnAccess);
+
+ EnableTermination();
+
+ // viInstallHandler(m_portHandle, VI_EVENT_IO_COMPLETION, ioCompleteHandler,
+ // this);
+ // viEnableEvent(m_portHandle, VI_EVENT_IO_COMPLETION, VI_HNDLR, VI_NULL);
+
+ HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
+}
+
+SerialPort::SerialPort(int baudRate, const wpi::Twine& portName, Port port,
+ int dataBits, SerialPort::Parity parity,
+ SerialPort::StopBits stopBits) {
+ int32_t status = 0;
+
+ m_port = port;
+
+ wpi::SmallVector<char, 64> buf;
+ const char* portNameC = portName.toNullTerminatedStringRef(buf).data();
+
+ HAL_InitializeSerialPortDirect(static_cast<HAL_SerialPort>(port), portNameC,
+ &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ // Don't continue if initialization failed
+ if (status < 0) return;
+ HAL_SetSerialBaudRate(static_cast<HAL_SerialPort>(port), baudRate, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ HAL_SetSerialDataBits(static_cast<HAL_SerialPort>(port), dataBits, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ HAL_SetSerialParity(static_cast<HAL_SerialPort>(port), parity, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ HAL_SetSerialStopBits(static_cast<HAL_SerialPort>(port), stopBits, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ // Set the default timeout to 5 seconds.
+ SetTimeout(5.0);
+
+ // Don't wait until the buffer is full to transmit.
+ SetWriteBufferMode(kFlushOnAccess);
+
+ EnableTermination();
+
+ // viInstallHandler(m_portHandle, VI_EVENT_IO_COMPLETION, ioCompleteHandler,
+ // this);
+ // viEnableEvent(m_portHandle, VI_EVENT_IO_COMPLETION, VI_HNDLR, VI_NULL);
+
+ HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
+}
+
+SerialPort::~SerialPort() {
+ int32_t status = 0;
+ HAL_CloseSerial(static_cast<HAL_SerialPort>(m_port), &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+SerialPort::SerialPort(SerialPort&& rhs)
+ : ErrorBase(std::move(rhs)),
+ m_resourceManagerHandle(std::move(rhs.m_resourceManagerHandle)),
+ m_portHandle(std::move(rhs.m_portHandle)),
+ m_consoleModeEnabled(std::move(rhs.m_consoleModeEnabled)) {
+ std::swap(m_port, rhs.m_port);
+}
+
+SerialPort& SerialPort::operator=(SerialPort&& rhs) {
+ ErrorBase::operator=(std::move(rhs));
+
+ m_resourceManagerHandle = std::move(rhs.m_resourceManagerHandle);
+ m_portHandle = std::move(rhs.m_portHandle);
+ m_consoleModeEnabled = std::move(rhs.m_consoleModeEnabled);
+ std::swap(m_port, rhs.m_port);
+
+ return *this;
+}
+
+void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) {
+ int32_t status = 0;
+ HAL_SetSerialFlowControl(static_cast<HAL_SerialPort>(m_port), flowControl,
+ &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SerialPort::EnableTermination(char terminator) {
+ int32_t status = 0;
+ HAL_EnableSerialTermination(static_cast<HAL_SerialPort>(m_port), terminator,
+ &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SerialPort::DisableTermination() {
+ int32_t status = 0;
+ HAL_DisableSerialTermination(static_cast<HAL_SerialPort>(m_port), &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+int SerialPort::GetBytesReceived() {
+ int32_t status = 0;
+ int retVal =
+ HAL_GetSerialBytesReceived(static_cast<HAL_SerialPort>(m_port), &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+int SerialPort::Read(char* buffer, int count) {
+ int32_t status = 0;
+ int retVal = HAL_ReadSerial(static_cast<HAL_SerialPort>(m_port), buffer,
+ count, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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(static_cast<HAL_SerialPort>(m_port),
+ buffer.data(), buffer.size(), &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ return retVal;
+}
+
+void SerialPort::SetTimeout(double timeout) {
+ int32_t status = 0;
+ HAL_SetSerialTimeout(static_cast<HAL_SerialPort>(m_port), timeout, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SerialPort::SetReadBufferSize(int size) {
+ int32_t status = 0;
+ HAL_SetSerialReadBufferSize(static_cast<HAL_SerialPort>(m_port), size,
+ &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SerialPort::SetWriteBufferSize(int size) {
+ int32_t status = 0;
+ HAL_SetSerialWriteBufferSize(static_cast<HAL_SerialPort>(m_port), size,
+ &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode) {
+ int32_t status = 0;
+ HAL_SetSerialWriteMode(static_cast<HAL_SerialPort>(m_port), mode, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SerialPort::Flush() {
+ int32_t status = 0;
+ HAL_FlushSerial(static_cast<HAL_SerialPort>(m_port), &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void SerialPort::Reset() {
+ int32_t status = 0;
+ HAL_ClearSerial(static_cast<HAL_SerialPort>(m_port), &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
diff --git a/wpilibc/src/main/native/cpp/Servo.cpp b/wpilibc/src/main/native/cpp/Servo.cpp
new file mode 100644
index 0000000..b4c9eb5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Servo.cpp
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Servo.h"
+
+#include <hal/HAL.h>
+
+#include "frc/smartdashboard/SendableBuilder.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);
+ SetName("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..3445f5d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Solenoid.cpp
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Solenoid.h"
+
+#include <utility>
+
+#include <hal/HAL.h>
+#include <hal/Ports.h>
+#include <hal/Solenoid.h>
+
+#include "frc/SensorUtil.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.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_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
+ channel, HAL_GetErrorMessage(status));
+ m_solenoidHandle = HAL_kInvalidHandle;
+ return;
+ }
+
+ HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel,
+ m_moduleNumber);
+ SetName("Solenoid", m_moduleNumber, m_channel);
+}
+
+Solenoid::~Solenoid() { HAL_FreeSolenoidPort(m_solenoidHandle); }
+
+Solenoid::Solenoid(Solenoid&& rhs)
+ : SolenoidBase(std::move(rhs)), m_channel(std::move(rhs.m_channel)) {
+ std::swap(m_solenoidHandle, rhs.m_solenoidHandle);
+}
+
+Solenoid& Solenoid::operator=(Solenoid&& rhs) {
+ SolenoidBase::operator=(std::move(rhs));
+
+ std::swap(m_solenoidHandle, rhs.m_solenoidHandle);
+ m_channel = std::move(rhs.m_channel);
+
+ return *this;
+}
+
+void Solenoid::Set(bool on) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_SetSolenoid(m_solenoidHandle, on, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+bool Solenoid::Get() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = HAL_GetSolenoid(m_solenoidHandle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+ 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_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void Solenoid::StartPulse() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ HAL_FireOneShot(m_solenoidHandle, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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..c0b79a5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/SolenoidBase.cpp
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* 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/SolenoidBase.h"
+
+#include <hal/HAL.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_setGlobalErrorWithContext(status, HAL_GetErrorMessage(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..fcfcb96
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Spark.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Spark.h"
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+Spark::Spark(int channel) : PWMSpeedController(channel) {
+ /* Note that the Spark uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the Spark User Manual available
+ * from REV Robotics.
+ *
+ * 2.003ms = full "forward"
+ * 1.55ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.46ms = the "low end" of the deadband range
+ * 0.999ms = full "reverse"
+ */
+ SetBounds(2.003, 1.55, 1.50, 1.46, .999);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel());
+ SetName("Spark", GetChannel());
+}
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..34f659a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Talon.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Talon.h"
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+Talon::Talon(int channel) : PWMSpeedController(channel) {
+ /* Note that the Talon uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the Talon User Manual available
+ * from CTRE.
+ *
+ * 2.037ms = full "forward"
+ * 1.539ms = the "high end" of the deadband range
+ * 1.513ms = center of the deadband range (off)
+ * 1.487ms = the "low end" of the deadband range
+ * 0.989ms = full "reverse"
+ */
+ SetBounds(2.037, 1.539, 1.513, 1.487, .989);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel());
+ SetName("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..7713f66
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Threads.cpp
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Threads.h"
+
+#include <hal/HAL.h>
+#include <hal/Threads.h>
+
+#include "frc/ErrorBase.h"
+
+using 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_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ *isRealTime = rt;
+ return ret;
+}
+
+int GetCurrentThreadPriority(bool* isRealTime) {
+ int32_t status = 0;
+ HAL_Bool rt = false;
+ auto ret = HAL_GetCurrentThreadPriority(&rt, &status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(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_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return ret;
+}
+
+bool SetCurrentThreadPriority(bool realTime, int priority) {
+ int32_t status = 0;
+ auto ret = HAL_SetCurrentThreadPriority(realTime, priority, &status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return ret;
+}
diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp
new file mode 100644
index 0000000..24ab668
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/TimedRobot.h"
+
+#include <stdint.h>
+
+#include <utility>
+
+#include <hal/HAL.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 = 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();
+ }
+}
+
+double TimedRobot::GetPeriod() const { return m_period; }
+
+TimedRobot::TimedRobot(double period) : IterativeRobotBase(period) {
+ int32_t status = 0;
+ m_notifier = HAL_InitializeNotifier(&status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ HAL_Report(HALUsageReporting::kResourceType_Framework,
+ HALUsageReporting::kFramework_Timed);
+}
+
+TimedRobot::~TimedRobot() {
+ int32_t status = 0;
+
+ HAL_StopNotifier(m_notifier, &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+ HAL_CleanNotifier(m_notifier, &status);
+}
+
+TimedRobot::TimedRobot(TimedRobot&& rhs)
+ : IterativeRobotBase(std::move(rhs)),
+ m_expirationTime(std::move(rhs.m_expirationTime)) {
+ std::swap(m_notifier, rhs.m_notifier);
+}
+
+TimedRobot& TimedRobot::operator=(TimedRobot&& rhs) {
+ IterativeRobotBase::operator=(std::move(rhs));
+ ErrorBase::operator=(std::move(rhs));
+
+ std::swap(m_notifier, rhs.m_notifier);
+ m_expirationTime = std::move(rhs.m_expirationTime);
+
+ return *this;
+}
+
+void TimedRobot::UpdateAlarm() {
+ int32_t status = 0;
+ HAL_UpdateNotifierAlarm(
+ m_notifier, static_cast<uint64_t>(m_expirationTime * 1e6), &status);
+ wpi_setErrorWithContext(status, HAL_GetErrorMessage(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..ae4a66e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Timer.cpp
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Timer.h"
+
+#include <chrono>
+#include <thread>
+
+#include <hal/HAL.h>
+
+#include "frc/DriverStation.h"
+#include "frc/RobotController.h"
+
+namespace frc {
+
+void Wait(double seconds) {
+ std::this_thread::sleep_for(std::chrono::duration<double>(seconds));
+}
+
+double GetClock() { return Timer::GetFPGATimestamp(); }
+
+double GetTime() {
+ using std::chrono::duration;
+ using std::chrono::duration_cast;
+ using std::chrono::system_clock;
+
+ return duration_cast<duration<double>>(system_clock::now().time_since_epoch())
+ .count();
+}
+
+} // namespace frc
+
+using namespace frc;
+
+// for compatibility with msvc12--see C2864
+const double Timer::kRolloverTime = (1ll << 32) / 1e6;
+
+Timer::Timer() { Reset(); }
+
+double Timer::Get() const {
+ double result;
+ double currentTime = GetFPGATimestamp();
+
+ std::lock_guard<wpi::mutex> lock(m_mutex);
+ if (m_running) {
+ // If the current time is before the start time, then the FPGA clock rolled
+ // over. Compensate by adding the ~71 minutes that it takes to roll over to
+ // the current time.
+ if (currentTime < m_startTime) {
+ currentTime += kRolloverTime;
+ }
+
+ result = (currentTime - m_startTime) + m_accumulatedTime;
+ } else {
+ result = m_accumulatedTime;
+ }
+
+ return result;
+}
+
+void Timer::Reset() {
+ std::lock_guard<wpi::mutex> lock(m_mutex);
+ m_accumulatedTime = 0;
+ m_startTime = GetFPGATimestamp();
+}
+
+void Timer::Start() {
+ std::lock_guard<wpi::mutex> lock(m_mutex);
+ if (!m_running) {
+ m_startTime = GetFPGATimestamp();
+ m_running = true;
+ }
+}
+
+void Timer::Stop() {
+ double temp = Get();
+
+ std::lock_guard<wpi::mutex> lock(m_mutex);
+ if (m_running) {
+ m_accumulatedTime = temp;
+ m_running = false;
+ }
+}
+
+bool Timer::HasPeriodPassed(double period) {
+ if (Get() > period) {
+ std::lock_guard<wpi::mutex> lock(m_mutex);
+ // Advance the start time by the period.
+ m_startTime += period;
+ // Don't set it to the current time... we want to avoid drift.
+ return true;
+ }
+ return false;
+}
+
+double Timer::GetFPGATimestamp() {
+ // FPGA returns the timestamp in microseconds
+ return RobotController::GetFPGATime() * 1.0e-6;
+}
+
+double Timer::GetMatchTime() {
+ return DriverStation::GetInstance().GetMatchTime();
+}
diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
new file mode 100644
index 0000000..ee4f5cc
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
@@ -0,0 +1,207 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Ultrasonic.h"
+
+#include <hal/HAL.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"
+
+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();
+ AddChild(m_pingChannel);
+ AddChild(m_echoChannel);
+}
+
+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 { 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())
+ 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() {
+ 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);
+ SetName("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..503b6d0
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Utility.cpp
@@ -0,0 +1,203 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Utility.h"
+
+#ifndef _WIN32
+#include <cxxabi.h>
+#include <execinfo.h>
+#endif
+
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+
+#include <hal/DriverStation.h>
+#include <hal/HAL.h>
+#include <wpi/Path.h>
+#include <wpi/SmallString.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 = 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 = 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;
+}
+
+namespace frc {
+
+int GetFPGAVersion() {
+ int32_t status = 0;
+ int version = HAL_GetFPGAVersion(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return version;
+}
+
+int64_t GetFPGARevision() {
+ int32_t status = 0;
+ int64_t revision = HAL_GetFPGARevision(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return revision;
+}
+
+uint64_t GetFPGATime() {
+ int32_t status = 0;
+ uint64_t time = HAL_GetFPGATime(&status);
+ wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+ return time;
+}
+
+bool GetUserButton() {
+ int32_t status = 0;
+
+ bool value = HAL_GetFPGAButton(&status);
+ wpi_setGlobalError(status);
+
+ return value;
+}
+
+#ifndef _WIN32
+
+/**
+ * Demangle a C++ symbol, used for printing stack traces.
+ */
+static std::string demangle(char const* mangledSymbol) {
+ char buffer[256];
+ size_t length;
+ int32_t status;
+
+ if (std::sscanf(mangledSymbol, "%*[^(]%*[(]%255[^)+]", buffer)) {
+ char* symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status);
+ if (status == 0) {
+ return symbol;
+ } else {
+ // If the symbol couldn't be demangled, it's probably a C function,
+ // so just return it as-is.
+ return buffer;
+ }
+ }
+
+ // If everything else failed, just return the mangled symbol
+ return mangledSymbol;
+}
+
+std::string GetStackTrace(int offset) {
+ void* stackTrace[128];
+ int stackSize = backtrace(stackTrace, 128);
+ char** mangledSymbols = backtrace_symbols(stackTrace, stackSize);
+ wpi::SmallString<1024> buf;
+ wpi::raw_svector_ostream trace(buf);
+
+ for (int i = offset; i < stackSize; i++) {
+ // Only print recursive functions once in a row.
+ if (i == 0 || stackTrace[i] != stackTrace[i - 1]) {
+ trace << "\tat " << demangle(mangledSymbols[i]) << "\n";
+ }
+ }
+
+ std::free(mangledSymbols);
+
+ return trace.str();
+}
+
+#else
+static std::string demangle(char const* mangledSymbol) {
+ return "no demangling on windows";
+}
+
+std::string GetStackTrace(int offset) { return "no stack trace on windows"; }
+#endif
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/cpp/Victor.cpp b/wpilibc/src/main/native/cpp/Victor.cpp
new file mode 100644
index 0000000..2c29ece
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Victor.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Victor.h"
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+Victor::Victor(int channel) : PWMSpeedController(channel) {
+ /* Note that the Victor uses the following bounds for PWM values. These
+ * values were determined empirically and optimized for the Victor 888. These
+ * values should work reasonably well for Victor 884 controllers as well but
+ * if users experience issues such as asymmetric behaviour around the deadband
+ * or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor 884 User
+ * Manual available from IFI.
+ *
+ * 2.027ms = full "forward"
+ * 1.525ms = the "high end" of the deadband range
+ * 1.507ms = center of the deadband range (off)
+ * 1.49ms = the "low end" of the deadband range
+ * 1.026ms = full "reverse"
+ */
+ SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
+ SetPeriodMultiplier(kPeriodMultiplier_2X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel());
+ SetName("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..5e2b6b9
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/VictorSP.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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/VictorSP.h"
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+VictorSP::VictorSP(int channel) : PWMSpeedController(channel) {
+ /* Note that the VictorSP uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the VictorSP User
+ * Manual available from Vex.
+ *
+ * 2.004ms = full "forward"
+ * 1.52ms = the "high end" of the deadband range
+ * 1.50ms = center of the deadband range (off)
+ * 1.48ms = the "low end" of the deadband range
+ * 0.997ms = full "reverse"
+ */
+ SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel());
+ SetName("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..b67f94d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Watchdog.cpp
@@ -0,0 +1,184 @@
+/*----------------------------------------------------------------------------*/
+/* 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/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 : public std::binary_function<T, T, bool> {
+ 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<wpi::mutex> 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.0e6)
+ << "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)
+ : m_timeout(static_cast<int64_t>(timeout * 1.0e6)),
+ 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) {
+ m_startTime = hal::fpga_clock::now();
+ m_epochs.clear();
+
+ // Locks mutex
+ auto thr = m_owner->GetThread();
+ if (!thr) return;
+
+ m_timeout = std::chrono::microseconds(static_cast<int64_t>(timeout * 1.0e6));
+ m_isExpired = false;
+
+ thr->m_watchdogs.remove(this);
+ m_expirationTime = m_startTime + 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.0e6;
+}
+
+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() {
+ 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 + 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;
+
+ m_isExpired = false;
+
+ 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..fa54980
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/XboxController.cpp
@@ -0,0 +1,160 @@
+/*----------------------------------------------------------------------------*/
+/* 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/XboxController.h"
+
+#include <hal/HAL.h>
+
+using namespace frc;
+
+XboxController::XboxController(int port) : GenericHID(port) {
+ HAL_Report(HALUsageReporting::kResourceType_XboxController, port);
+}
+
+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/buttons/Button.cpp b/wpilibc/src/main/native/cpp/buttons/Button.cpp
new file mode 100644
index 0000000..45ede65
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/Button.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/Button.h"
+
+using namespace frc;
+
+void Button::WhenPressed(Command* command) { WhenActive(command); }
+
+void Button::WhileHeld(Command* command) { WhileActive(command); }
+
+void Button::WhenReleased(Command* command) { WhenInactive(command); }
+
+void Button::CancelWhenPressed(Command* command) { CancelWhenActive(command); }
+
+void Button::ToggleWhenPressed(Command* command) { ToggleWhenActive(command); }
diff --git a/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp
new file mode 100644
index 0000000..1e10255
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/ButtonScheduler.h"
+
+#include "frc/commands/Scheduler.h"
+
+using namespace frc;
+
+ButtonScheduler::ButtonScheduler(bool last, Trigger* button, Command* orders)
+ : m_pressedLast(last), m_button(button), m_command(orders) {}
+
+void ButtonScheduler::Start() { Scheduler::GetInstance()->AddButton(this); }
diff --git a/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
new file mode 100644
index 0000000..39d1d25
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/CancelButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger* button,
+ Command* orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void CancelButtonScheduler::Execute() {
+ bool pressed = m_button->Grab();
+
+ if (!m_pressedLast && pressed) {
+ m_command->Cancel();
+ }
+
+ m_pressedLast = pressed;
+}
diff --git a/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
new file mode 100644
index 0000000..feaa3c6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/HeldButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger* button,
+ Command* orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void HeldButtonScheduler::Execute() {
+ bool pressed = m_button->Grab();
+
+ if (pressed) {
+ m_command->Start();
+ } else if (m_pressedLast && !pressed) {
+ m_command->Cancel();
+ }
+
+ m_pressedLast = pressed;
+}
diff --git a/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp b/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp
new file mode 100644
index 0000000..cac67c4
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/InternalButton.h"
+
+using namespace frc;
+
+InternalButton::InternalButton(bool inverted)
+ : m_pressed(inverted), m_inverted(inverted) {}
+
+void InternalButton::SetInverted(bool inverted) { m_inverted = inverted; }
+
+void InternalButton::SetPressed(bool pressed) { m_pressed = pressed; }
+
+bool InternalButton::Get() { return m_pressed ^ m_inverted; }
diff --git a/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp b/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp
new file mode 100644
index 0000000..2c93be2
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/JoystickButton.h"
+
+using namespace frc;
+
+JoystickButton::JoystickButton(GenericHID* joystick, int buttonNumber)
+ : m_joystick(joystick), m_buttonNumber(buttonNumber) {}
+
+bool JoystickButton::Get() { return m_joystick->GetRawButton(m_buttonNumber); }
diff --git a/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp b/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp
new file mode 100644
index 0000000..5e8b1e0
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/NetworkButton.h"
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+
+using namespace frc;
+
+NetworkButton::NetworkButton(const wpi::Twine& tableName,
+ const wpi::Twine& field)
+ : NetworkButton(nt::NetworkTableInstance::GetDefault().GetTable(tableName),
+ field) {}
+
+NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
+ const wpi::Twine& field)
+ : m_entry(table->GetEntry(field)) {}
+
+bool NetworkButton::Get() {
+ return m_entry.GetInstance().IsConnected() && m_entry.GetBoolean(false);
+}
diff --git a/wpilibc/src/main/native/cpp/buttons/POVButton.cpp b/wpilibc/src/main/native/cpp/buttons/POVButton.cpp
new file mode 100644
index 0000000..6729923
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/POVButton.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/POVButton.h"
+
+using namespace frc;
+
+POVButton::POVButton(GenericHID& joystick, int angle, int povNumber)
+ : m_joystick(&joystick), m_angle(angle), m_povNumber(povNumber) {}
+
+bool POVButton::Get() { return m_joystick->GetPOV(m_povNumber) == m_angle; }
diff --git a/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
new file mode 100644
index 0000000..a964117
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/PressedButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger* button,
+ Command* orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void PressedButtonScheduler::Execute() {
+ bool pressed = m_button->Grab();
+
+ if (!m_pressedLast && pressed) {
+ m_command->Start();
+ }
+
+ m_pressedLast = pressed;
+}
diff --git a/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
new file mode 100644
index 0000000..6d67004
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/ReleasedButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger* button,
+ Command* orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void ReleasedButtonScheduler::Execute() {
+ bool pressed = m_button->Grab();
+
+ if (m_pressedLast && !pressed) {
+ m_command->Start();
+ }
+
+ m_pressedLast = pressed;
+}
diff --git a/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
new file mode 100644
index 0000000..b722d45
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/ToggleButtonScheduler.h"
+
+#include "frc/buttons/Button.h"
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger* button,
+ Command* orders)
+ : ButtonScheduler(last, button, orders) {}
+
+void ToggleButtonScheduler::Execute() {
+ bool pressed = m_button->Grab();
+
+ if (!m_pressedLast && pressed) {
+ if (m_command->IsRunning()) {
+ m_command->Cancel();
+ } else {
+ m_command->Start();
+ }
+ }
+
+ m_pressedLast = pressed;
+}
diff --git a/wpilibc/src/main/native/cpp/buttons/Trigger.cpp b/wpilibc/src/main/native/cpp/buttons/Trigger.cpp
new file mode 100644
index 0000000..2ccaaf2
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/buttons/Trigger.cpp
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/buttons/Button.h"
+#include "frc/buttons/CancelButtonScheduler.h"
+#include "frc/buttons/HeldButtonScheduler.h"
+#include "frc/buttons/PressedButtonScheduler.h"
+#include "frc/buttons/ReleasedButtonScheduler.h"
+#include "frc/buttons/ToggleButtonScheduler.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+bool Trigger::Grab() { return Get() || m_sendablePressed; }
+
+void Trigger::WhenActive(Command* command) {
+ auto pbs = new PressedButtonScheduler(Grab(), this, command);
+ pbs->Start();
+}
+
+void Trigger::WhileActive(Command* command) {
+ auto hbs = new HeldButtonScheduler(Grab(), this, command);
+ hbs->Start();
+}
+
+void Trigger::WhenInactive(Command* command) {
+ auto rbs = new ReleasedButtonScheduler(Grab(), this, command);
+ rbs->Start();
+}
+
+void Trigger::CancelWhenActive(Command* command) {
+ auto cbs = new CancelButtonScheduler(Grab(), this, command);
+ cbs->Start();
+}
+
+void Trigger::ToggleWhenActive(Command* command) {
+ auto tbs = new ToggleButtonScheduler(Grab(), this, command);
+ tbs->Start();
+}
+
+void Trigger::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Button");
+ builder.SetSafeState([=]() { m_sendablePressed = false; });
+ builder.AddBooleanProperty("pressed", [=]() { return Grab(); },
+ [=](bool value) { m_sendablePressed = value; });
+}
diff --git a/wpilibc/src/main/native/cpp/commands/Command.cpp b/wpilibc/src/main/native/cpp/commands/Command.cpp
new file mode 100644
index 0000000..ad7824a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/Command.cpp
@@ -0,0 +1,244 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/Command.h"
+
+#include <typeinfo>
+
+#include "frc/RobotState.h"
+#include "frc/Timer.h"
+#include "frc/WPIErrors.h"
+#include "frc/commands/CommandGroup.h"
+#include "frc/commands/Scheduler.h"
+#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+int Command::m_commandCounter = 0;
+
+Command::Command() : Command("", -1.0) {}
+
+Command::Command(const wpi::Twine& name) : Command(name, -1.0) {}
+
+Command::Command(double timeout) : Command("", timeout) {}
+
+Command::Command(Subsystem& subsystem) : Command("", -1.0) {
+ Requires(&subsystem);
+}
+
+Command::Command(const wpi::Twine& name, double timeout) : SendableBase(false) {
+ // We use -1.0 to indicate no timeout.
+ if (timeout < 0.0 && timeout != -1.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+
+ m_timeout = timeout;
+
+ // If name contains an empty string
+ if (name.isTriviallyEmpty() ||
+ (name.isSingleStringRef() && name.getSingleStringRef().empty())) {
+ SetName("Command_" + wpi::Twine(typeid(*this).name()));
+ } else {
+ SetName(name);
+ }
+}
+
+Command::Command(const wpi::Twine& name, Subsystem& subsystem)
+ : Command(name, -1.0) {
+ Requires(&subsystem);
+}
+
+Command::Command(double timeout, Subsystem& subsystem) : Command("", timeout) {
+ Requires(&subsystem);
+}
+
+Command::Command(const wpi::Twine& name, double timeout, Subsystem& subsystem)
+ : Command(name, timeout) {
+ Requires(&subsystem);
+}
+
+double Command::TimeSinceInitialized() const {
+ if (m_startTime < 0.0)
+ return 0.0;
+ else
+ return Timer::GetFPGATimestamp() - m_startTime;
+}
+
+void Command::Requires(Subsystem* subsystem) {
+ if (!AssertUnlocked("Can not add new requirement to command")) return;
+
+ if (subsystem != nullptr)
+ m_requirements.insert(subsystem);
+ else
+ wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+}
+
+void Command::Start() {
+ LockChanges();
+ if (m_parent != nullptr)
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse,
+ "Can not start a command that is part of a command group");
+
+ m_completed = false;
+ Scheduler::GetInstance()->AddCommand(this);
+}
+
+bool Command::Run() {
+ if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled())
+ Cancel();
+
+ if (IsCanceled()) return false;
+
+ if (!m_initialized) {
+ m_initialized = true;
+ StartTiming();
+ _Initialize();
+ Initialize();
+ }
+ _Execute();
+ Execute();
+ return !IsFinished();
+}
+
+void Command::Cancel() {
+ if (m_parent != nullptr)
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse,
+ "Can not cancel a command that is part of a command group");
+
+ _Cancel();
+}
+
+bool Command::IsRunning() const { return m_running; }
+
+bool Command::IsInitialized() const { return m_initialized; }
+
+bool Command::IsCompleted() const { return m_completed; }
+
+bool Command::IsCanceled() const { return m_canceled; }
+
+bool Command::IsInterruptible() const { return m_interruptible; }
+
+void Command::SetInterruptible(bool interruptible) {
+ m_interruptible = interruptible;
+}
+
+bool Command::DoesRequire(Subsystem* system) const {
+ return m_requirements.count(system) > 0;
+}
+
+const Command::SubsystemSet& Command::GetRequirements() const {
+ return m_requirements;
+}
+
+CommandGroup* Command::GetGroup() const { return m_parent; }
+
+void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
+
+bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
+
+int Command::GetID() const { return m_commandID; }
+
+void Command::SetTimeout(double timeout) {
+ if (timeout < 0.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ else
+ m_timeout = timeout;
+}
+
+bool Command::IsTimedOut() const {
+ return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
+}
+
+bool Command::AssertUnlocked(const std::string& message) {
+ if (m_locked) {
+ std::string buf =
+ message + " after being started or being added to a command group";
+ wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
+ return false;
+ }
+ return true;
+}
+
+void Command::SetParent(CommandGroup* parent) {
+ if (parent == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "parent");
+ } else if (m_parent != nullptr) {
+ wpi_setWPIErrorWithContext(CommandIllegalUse,
+ "Can not give command to a command group after "
+ "already being put in a command group");
+ } else {
+ LockChanges();
+ m_parent = parent;
+ }
+}
+
+bool Command::IsParented() const { return m_parent != nullptr; }
+
+void Command::ClearRequirements() { m_requirements.clear(); }
+
+void Command::Initialize() {}
+
+void Command::Execute() {}
+
+void Command::End() {}
+
+void Command::Interrupted() { End(); }
+
+void Command::_Initialize() { m_completed = false; }
+
+void Command::_Interrupted() { m_completed = true; }
+
+void Command::_Execute() {}
+
+void Command::_End() { m_completed = true; }
+
+void Command::_Cancel() {
+ if (IsRunning()) m_canceled = true;
+}
+
+void Command::LockChanges() { m_locked = true; }
+
+void Command::Removed() {
+ if (m_initialized) {
+ if (IsCanceled()) {
+ Interrupted();
+ _Interrupted();
+ } else {
+ End();
+ _End();
+ }
+ }
+ m_initialized = false;
+ m_canceled = false;
+ m_running = false;
+ m_completed = true;
+}
+
+void Command::StartRunning() {
+ m_running = true;
+ m_startTime = -1;
+ m_completed = false;
+}
+
+void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
+
+void Command::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Command");
+ builder.AddStringProperty(".name", [=]() { return GetName(); }, nullptr);
+ builder.AddBooleanProperty("running", [=]() { return IsRunning(); },
+ [=](bool value) {
+ if (value) {
+ if (!IsRunning()) Start();
+ } else {
+ if (IsRunning()) Cancel();
+ }
+ });
+ builder.AddBooleanProperty(".isParented", [=]() { return IsParented(); },
+ nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp b/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp
new file mode 100644
index 0000000..eac1746
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp
@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/CommandGroup.h"
+
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+CommandGroup::CommandGroup(const wpi::Twine& name) : Command(name) {}
+
+void CommandGroup::AddSequential(Command* command) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+ m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
+
+ command->SetParent(this);
+
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+void CommandGroup::AddSequential(Command* command, double timeout) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+ if (timeout < 0.0) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ return;
+ }
+
+ m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
+ timeout);
+
+ command->SetParent(this);
+
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+void CommandGroup::AddParallel(Command* command) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+
+ m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
+
+ command->SetParent(this);
+
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+void CommandGroup::AddParallel(Command* command, double timeout) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+ if (!AssertUnlocked("Cannot add new command to command group")) return;
+ if (timeout < 0.0) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ return;
+ }
+
+ m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
+ timeout);
+
+ command->SetParent(this);
+
+ // Iterate through command->GetRequirements() and call Requires() on each
+ // required subsystem
+ for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+}
+
+bool CommandGroup::IsInterruptible() const {
+ if (!Command::IsInterruptible()) return false;
+
+ if (m_currentCommandIndex != -1 &&
+ static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+ Command* cmd = m_commands[m_currentCommandIndex].m_command;
+ if (!cmd->IsInterruptible()) return false;
+ }
+
+ for (const auto& child : m_children) {
+ if (!child->m_command->IsInterruptible()) return false;
+ }
+
+ return true;
+}
+
+int CommandGroup::GetSize() const { return m_children.size(); }
+
+void CommandGroup::Initialize() {}
+
+void CommandGroup::Execute() {}
+
+bool CommandGroup::IsFinished() {
+ return static_cast<size_t>(m_currentCommandIndex) >= m_commands.size() &&
+ m_children.empty();
+}
+
+void CommandGroup::End() {}
+
+void CommandGroup::Interrupted() {}
+
+void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
+
+void CommandGroup::_Execute() {
+ CommandGroupEntry* entry;
+ Command* cmd = nullptr;
+ bool firstRun = false;
+
+ if (m_currentCommandIndex == -1) {
+ firstRun = true;
+ m_currentCommandIndex = 0;
+ }
+
+ // While there are still commands in this group to run
+ while (static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+ // If a command is prepared to run
+ if (cmd != nullptr) {
+ // If command timed out, cancel it so it's removed from the Scheduler
+ if (entry->IsTimedOut()) cmd->_Cancel();
+
+ // If command finished or was cancelled, remove it from Scheduler
+ if (cmd->Run()) {
+ break;
+ } else {
+ cmd->Removed();
+
+ // Advance to next command in group
+ m_currentCommandIndex++;
+ firstRun = true;
+ cmd = nullptr;
+ continue;
+ }
+ }
+
+ entry = &m_commands[m_currentCommandIndex];
+ cmd = nullptr;
+
+ switch (entry->m_state) {
+ case CommandGroupEntry::kSequence_InSequence:
+ cmd = entry->m_command;
+ if (firstRun) {
+ cmd->StartRunning();
+ CancelConflicts(cmd);
+ firstRun = false;
+ }
+ break;
+
+ case CommandGroupEntry::kSequence_BranchPeer:
+ // Start executing a parallel command and advance to next entry in group
+ m_currentCommandIndex++;
+ entry->m_command->Start();
+ break;
+
+ case CommandGroupEntry::kSequence_BranchChild:
+ m_currentCommandIndex++;
+
+ /* Causes scheduler to skip children of current command which require
+ * the same subsystems as it
+ */
+ CancelConflicts(entry->m_command);
+ entry->m_command->StartRunning();
+
+ // Add current command entry to list of children of this group
+ m_children.push_back(entry);
+ break;
+ }
+ }
+
+ // Run Children
+ for (auto& entry : m_children) {
+ auto child = entry->m_command;
+ if (entry->IsTimedOut()) {
+ child->_Cancel();
+ }
+
+ // If child finished or was cancelled, set it to nullptr. nullptr entries
+ // are removed later.
+ if (!child->Run()) {
+ child->Removed();
+ entry = nullptr;
+ }
+ }
+
+ m_children.erase(std::remove(m_children.begin(), m_children.end(), nullptr),
+ m_children.end());
+}
+
+void CommandGroup::_End() {
+ // Theoretically, we don't have to check this, but we do if teams override the
+ // IsFinished method
+ if (m_currentCommandIndex != -1 &&
+ static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
+ Command* cmd = m_commands[m_currentCommandIndex].m_command;
+ cmd->_Cancel();
+ cmd->Removed();
+ }
+
+ for (auto& child : m_children) {
+ Command* cmd = child->m_command;
+ cmd->_Cancel();
+ cmd->Removed();
+ }
+ m_children.clear();
+}
+
+void CommandGroup::_Interrupted() { _End(); }
+
+void CommandGroup::CancelConflicts(Command* command) {
+ for (auto childIter = m_children.begin(); childIter != m_children.end();) {
+ Command* child = (*childIter)->m_command;
+ bool erased = false;
+
+ for (auto&& requirement : command->GetRequirements()) {
+ if (child->DoesRequire(requirement)) {
+ child->_Cancel();
+ child->Removed();
+ childIter = m_children.erase(childIter);
+ erased = true;
+ break;
+ }
+ }
+ if (!erased) childIter++;
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp b/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp
new file mode 100644
index 0000000..7a75f00
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/CommandGroupEntry.h"
+
+#include "frc/commands/Command.h"
+
+using namespace frc;
+
+CommandGroupEntry::CommandGroupEntry(Command* command, Sequence state,
+ double timeout)
+ : m_timeout(timeout), m_command(command), m_state(state) {}
+
+bool CommandGroupEntry::IsTimedOut() const {
+ if (m_timeout < 0.0) return false;
+ double time = m_command->TimeSinceInitialized();
+ if (time == 0.0) return false;
+ return time >= m_timeout;
+}
diff --git a/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp b/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp
new file mode 100644
index 0000000..c44f7ba
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/ConditionalCommand.h"
+
+#include "frc/commands/Scheduler.h"
+
+using namespace frc;
+
+static void RequireAll(Command& command, Command* onTrue, Command* onFalse) {
+ if (onTrue != nullptr) {
+ for (auto requirement : onTrue->GetRequirements())
+ command.Requires(requirement);
+ }
+ if (onFalse != nullptr) {
+ for (auto requirement : onFalse->GetRequirements())
+ command.Requires(requirement);
+ }
+}
+
+ConditionalCommand::ConditionalCommand(Command* onTrue, Command* onFalse) {
+ m_onTrue = onTrue;
+ m_onFalse = onFalse;
+
+ RequireAll(*this, onTrue, onFalse);
+}
+
+ConditionalCommand::ConditionalCommand(const wpi::Twine& name, Command* onTrue,
+ Command* onFalse)
+ : Command(name) {
+ m_onTrue = onTrue;
+ m_onFalse = onFalse;
+
+ RequireAll(*this, onTrue, onFalse);
+}
+
+void ConditionalCommand::_Initialize() {
+ if (Condition()) {
+ m_chosenCommand = m_onTrue;
+ } else {
+ m_chosenCommand = m_onFalse;
+ }
+
+ if (m_chosenCommand != nullptr) {
+ // This is a hack to make cancelling the chosen command inside a
+ // CommandGroup work properly
+ m_chosenCommand->ClearRequirements();
+
+ m_chosenCommand->Start();
+ }
+ Command::_Initialize();
+}
+
+void ConditionalCommand::_Cancel() {
+ if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
+ m_chosenCommand->Cancel();
+ }
+
+ Command::_Cancel();
+}
+
+bool ConditionalCommand::IsFinished() {
+ if (m_chosenCommand != nullptr) {
+ return m_chosenCommand->IsCompleted();
+ } else {
+ return true;
+ }
+}
+
+void ConditionalCommand::_Interrupted() {
+ if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
+ m_chosenCommand->Cancel();
+ }
+
+ Command::_Interrupted();
+}
diff --git a/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp b/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp
new file mode 100644
index 0000000..88ebfde
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/InstantCommand.h"
+
+using namespace frc;
+
+InstantCommand::InstantCommand(const wpi::Twine& name) : Command(name) {}
+
+InstantCommand::InstantCommand(Subsystem& subsystem) : Command(subsystem) {}
+
+InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem)
+ : Command(name, subsystem) {}
+
+InstantCommand::InstantCommand(std::function<void()> func) : m_func(func) {}
+
+InstantCommand::InstantCommand(Subsystem& subsystem, std::function<void()> func)
+ : InstantCommand(subsystem) {
+ m_func = func;
+}
+
+InstantCommand::InstantCommand(const wpi::Twine& name,
+ std::function<void()> func)
+ : InstantCommand(name) {
+ m_func = func;
+}
+
+InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
+ std::function<void()> func)
+ : InstantCommand(name, subsystem) {
+ m_func = func;
+}
+
+void InstantCommand::_Initialize() {
+ Command::_Initialize();
+ if (m_func) {
+ m_func();
+ }
+}
+
+bool InstantCommand::IsFinished() { return true; }
diff --git a/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp b/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp
new file mode 100644
index 0000000..875d8fe
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/PIDCommand.h"
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ double f, double period)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double f, double period) {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ double period)
+ : Command(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double period) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ double f, double period, Subsystem& subsystem)
+ : Command(name, subsystem) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double f, double period,
+ Subsystem& subsystem)
+ : Command(subsystem) {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ Subsystem& subsystem)
+ : Command(name, subsystem) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+ double period, Subsystem& subsystem)
+ : Command(name, subsystem) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, Subsystem& subsystem) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+PIDCommand::PIDCommand(double p, double i, double d, double period,
+ Subsystem& subsystem) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
+}
+
+void PIDCommand::_Initialize() { m_controller->Enable(); }
+
+void PIDCommand::_End() { m_controller->Disable(); }
+
+void PIDCommand::_Interrupted() { _End(); }
+
+void PIDCommand::SetSetpointRelative(double deltaSetpoint) {
+ SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+void PIDCommand::PIDWrite(double output) { UsePIDOutput(output); }
+
+double PIDCommand::PIDGet() { return ReturnPIDInput(); }
+
+std::shared_ptr<PIDController> PIDCommand::GetPIDController() const {
+ return m_controller;
+}
+
+void PIDCommand::SetSetpoint(double setpoint) {
+ m_controller->SetSetpoint(setpoint);
+}
+
+double PIDCommand::GetSetpoint() const { return m_controller->GetSetpoint(); }
+
+double PIDCommand::GetPosition() { return ReturnPIDInput(); }
+
+void PIDCommand::InitSendable(SendableBuilder& builder) {
+ m_controller->InitSendable(builder);
+ Command::InitSendable(builder);
+ builder.SetSmartDashboardType("PIDCommand");
+}
diff --git a/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp b/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp
new file mode 100644
index 0000000..0893da3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp
@@ -0,0 +1,97 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/PIDSubsystem.h"
+
+#include "frc/PIDController.h"
+
+using namespace frc;
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+ AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
+ double f)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+ AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
+ double f, double period)
+ : Subsystem(name) {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+ AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+ AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+ AddChild("PIDController", m_controller);
+}
+
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f,
+ double period)
+ : Subsystem("PIDSubsystem") {
+ m_controller =
+ std::make_shared<PIDController>(p, i, d, f, this, this, period);
+ AddChild("PIDController", m_controller);
+}
+
+void PIDSubsystem::Enable() { m_controller->Enable(); }
+
+void PIDSubsystem::Disable() { m_controller->Disable(); }
+
+void PIDSubsystem::PIDWrite(double output) { UsePIDOutput(output); }
+
+double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
+
+void PIDSubsystem::SetSetpoint(double setpoint) {
+ m_controller->SetSetpoint(setpoint);
+}
+
+void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
+ SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+void PIDSubsystem::SetInputRange(double minimumInput, double maximumInput) {
+ m_controller->SetInputRange(minimumInput, maximumInput);
+}
+
+void PIDSubsystem::SetOutputRange(double minimumOutput, double maximumOutput) {
+ m_controller->SetOutputRange(minimumOutput, maximumOutput);
+}
+
+double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
+
+double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
+
+double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
+
+void PIDSubsystem::SetAbsoluteTolerance(double absValue) {
+ m_controller->SetAbsoluteTolerance(absValue);
+}
+
+void PIDSubsystem::SetPercentTolerance(double percent) {
+ m_controller->SetPercentTolerance(percent);
+}
+
+bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
+
+std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
+ return m_controller;
+}
diff --git a/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp b/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp
new file mode 100644
index 0000000..b4bea24
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/PrintCommand.h"
+
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+PrintCommand::PrintCommand(const wpi::Twine& message)
+ : InstantCommand("Print \"" + message + wpi::Twine('"')) {
+ m_message = message.str();
+}
+
+void PrintCommand::Initialize() { wpi::outs() << m_message << '\n'; }
diff --git a/wpilibc/src/main/native/cpp/commands/Scheduler.cpp b/wpilibc/src/main/native/cpp/commands/Scheduler.cpp
new file mode 100644
index 0000000..8a7af76
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/Scheduler.cpp
@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/Scheduler.h"
+
+#include <algorithm>
+#include <set>
+#include <string>
+#include <vector>
+
+#include <hal/HAL.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/mutex.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/buttons/ButtonScheduler.h"
+#include "frc/commands/Command.h"
+#include "frc/commands/Subsystem.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+struct Scheduler::Impl {
+ void Remove(Command* command);
+ void ProcessCommandAddition(Command* command);
+
+ using SubsystemSet = std::set<Subsystem*>;
+ SubsystemSet subsystems;
+ wpi::mutex buttonsMutex;
+ using ButtonVector = std::vector<std::unique_ptr<ButtonScheduler>>;
+ ButtonVector buttons;
+ using CommandVector = std::vector<Command*>;
+ wpi::mutex additionsMutex;
+ CommandVector additions;
+ using CommandSet = std::set<Command*>;
+ CommandSet commands;
+ bool adding = false;
+ bool enabled = true;
+ std::vector<std::string> commandsBuf;
+ std::vector<double> idsBuf;
+ bool runningCommandsChanged = false;
+};
+
+Scheduler* Scheduler::GetInstance() {
+ static Scheduler instance;
+ return &instance;
+}
+
+void Scheduler::AddCommand(Command* command) {
+ std::lock_guard<wpi::mutex> lock(m_impl->additionsMutex);
+ if (std::find(m_impl->additions.begin(), m_impl->additions.end(), command) !=
+ m_impl->additions.end())
+ return;
+ m_impl->additions.push_back(command);
+}
+
+void Scheduler::AddButton(ButtonScheduler* button) {
+ std::lock_guard<wpi::mutex> lock(m_impl->buttonsMutex);
+ m_impl->buttons.emplace_back(button);
+}
+
+void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
+ if (subsystem == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+ return;
+ }
+ m_impl->subsystems.insert(subsystem);
+}
+
+void Scheduler::Run() {
+ // Get button input (going backwards preserves button priority)
+ {
+ if (!m_impl->enabled) return;
+
+ std::lock_guard<wpi::mutex> lock(m_impl->buttonsMutex);
+ for (auto& button : m_impl->buttons) {
+ button->Execute();
+ }
+ }
+
+ // Call every subsystem's periodic method
+ for (auto& subsystem : m_impl->subsystems) {
+ subsystem->Periodic();
+ }
+
+ m_impl->runningCommandsChanged = false;
+
+ // Loop through the commands
+ for (auto cmdIter = m_impl->commands.begin();
+ cmdIter != m_impl->commands.end();) {
+ Command* command = *cmdIter;
+ // Increment before potentially removing to keep the iterator valid
+ ++cmdIter;
+ if (!command->Run()) {
+ Remove(command);
+ m_impl->runningCommandsChanged = true;
+ }
+ }
+
+ // Add the new things
+ {
+ std::lock_guard<wpi::mutex> lock(m_impl->additionsMutex);
+ for (auto& addition : m_impl->additions) {
+ // Check to make sure no adding during adding
+ if (m_impl->adding) {
+ wpi_setWPIErrorWithContext(IncompatibleState,
+ "Can not start command from cancel method");
+ } else {
+ m_impl->ProcessCommandAddition(addition);
+ }
+ }
+ m_impl->additions.clear();
+ }
+
+ // Add in the defaults
+ for (auto& subsystem : m_impl->subsystems) {
+ if (subsystem->GetCurrentCommand() == nullptr) {
+ if (m_impl->adding) {
+ wpi_setWPIErrorWithContext(IncompatibleState,
+ "Can not start command from cancel method");
+ } else {
+ m_impl->ProcessCommandAddition(subsystem->GetDefaultCommand());
+ }
+ }
+ subsystem->ConfirmCommand();
+ }
+}
+
+void Scheduler::Remove(Command* command) {
+ if (command == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "command");
+ return;
+ }
+
+ m_impl->Remove(command);
+}
+
+void Scheduler::RemoveAll() {
+ while (m_impl->commands.size() > 0) {
+ Remove(*m_impl->commands.begin());
+ }
+}
+
+void Scheduler::ResetAll() {
+ RemoveAll();
+ m_impl->subsystems.clear();
+ m_impl->buttons.clear();
+ m_impl->additions.clear();
+ m_impl->commands.clear();
+}
+
+void Scheduler::SetEnabled(bool enabled) { m_impl->enabled = enabled; }
+
+void Scheduler::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Scheduler");
+ auto namesEntry = builder.GetEntry("Names");
+ auto idsEntry = builder.GetEntry("Ids");
+ auto cancelEntry = builder.GetEntry("Cancel");
+ builder.SetUpdateTable([=]() {
+ // Get the list of possible commands to cancel
+ auto new_toCancel = cancelEntry.GetValue();
+ wpi::ArrayRef<double> toCancel;
+ if (new_toCancel) toCancel = new_toCancel->GetDoubleArray();
+
+ // Cancel commands whose cancel buttons were pressed on the SmartDashboard
+ if (!toCancel.empty()) {
+ for (auto& command : m_impl->commands) {
+ for (const auto& cancelled : toCancel) {
+ if (command->GetID() == cancelled) {
+ command->Cancel();
+ }
+ }
+ }
+ nt::NetworkTableEntry(cancelEntry).SetDoubleArray({});
+ }
+
+ // Set the running commands
+ if (m_impl->runningCommandsChanged) {
+ m_impl->commandsBuf.resize(0);
+ m_impl->idsBuf.resize(0);
+ for (const auto& command : m_impl->commands) {
+ m_impl->commandsBuf.emplace_back(command->GetName());
+ m_impl->idsBuf.emplace_back(command->GetID());
+ }
+ nt::NetworkTableEntry(namesEntry).SetStringArray(m_impl->commandsBuf);
+ nt::NetworkTableEntry(idsEntry).SetDoubleArray(m_impl->idsBuf);
+ }
+ });
+}
+
+Scheduler::Scheduler() : m_impl(new Impl) {
+ HAL_Report(HALUsageReporting::kResourceType_Command,
+ HALUsageReporting::kCommand_Scheduler);
+ SetName("Scheduler");
+}
+
+Scheduler::~Scheduler() {}
+
+void Scheduler::Impl::Remove(Command* command) {
+ if (!commands.erase(command)) return;
+
+ for (auto&& requirement : command->GetRequirements()) {
+ requirement->SetCurrentCommand(nullptr);
+ }
+
+ command->Removed();
+}
+
+void Scheduler::Impl::ProcessCommandAddition(Command* command) {
+ if (command == nullptr) return;
+
+ // Only add if not already in
+ auto found = commands.find(command);
+ if (found == commands.end()) {
+ // Check that the requirements can be had
+ const auto& requirements = command->GetRequirements();
+ for (const auto& requirement : requirements) {
+ if (requirement->GetCurrentCommand() != nullptr &&
+ !requirement->GetCurrentCommand()->IsInterruptible())
+ return;
+ }
+
+ // Give it the requirements
+ adding = true;
+ for (auto&& requirement : requirements) {
+ if (requirement->GetCurrentCommand() != nullptr) {
+ requirement->GetCurrentCommand()->Cancel();
+ Remove(requirement->GetCurrentCommand());
+ }
+ requirement->SetCurrentCommand(command);
+ }
+ adding = false;
+
+ commands.insert(command);
+
+ command->StartRunning();
+ runningCommandsChanged = true;
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/commands/StartCommand.cpp b/wpilibc/src/main/native/cpp/commands/StartCommand.cpp
new file mode 100644
index 0000000..8884124
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/StartCommand.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/StartCommand.h"
+
+using namespace frc;
+
+StartCommand::StartCommand(Command* commandToStart)
+ : InstantCommand("StartCommand") {
+ m_commandToFork = commandToStart;
+}
+
+void StartCommand::Initialize() { m_commandToFork->Start(); }
diff --git a/wpilibc/src/main/native/cpp/commands/Subsystem.cpp b/wpilibc/src/main/native/cpp/commands/Subsystem.cpp
new file mode 100644
index 0000000..308642f
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/Subsystem.cpp
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/Subsystem.h"
+
+#include "frc/WPIErrors.h"
+#include "frc/commands/Command.h"
+#include "frc/commands/Scheduler.h"
+#include "frc/livewindow/LiveWindow.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+Subsystem::Subsystem(const wpi::Twine& name) {
+ SetName(name, name);
+ Scheduler::GetInstance()->RegisterSubsystem(this);
+}
+
+void Subsystem::SetDefaultCommand(Command* command) {
+ if (command == nullptr) {
+ m_defaultCommand = nullptr;
+ } else {
+ const auto& reqs = command->GetRequirements();
+ if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
+ wpi_setWPIErrorWithContext(
+ CommandIllegalUse, "A default command must require the subsystem");
+ return;
+ }
+
+ m_defaultCommand = command;
+ }
+}
+
+Command* Subsystem::GetDefaultCommand() {
+ if (!m_initializedDefaultCommand) {
+ m_initializedDefaultCommand = true;
+ InitDefaultCommand();
+ }
+ return m_defaultCommand;
+}
+
+wpi::StringRef Subsystem::GetDefaultCommandName() {
+ Command* defaultCommand = GetDefaultCommand();
+ if (defaultCommand) {
+ return defaultCommand->GetName();
+ } else {
+ return wpi::StringRef();
+ }
+}
+
+void Subsystem::SetCurrentCommand(Command* command) {
+ m_currentCommand = command;
+ m_currentCommandChanged = true;
+}
+
+Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; }
+
+wpi::StringRef Subsystem::GetCurrentCommandName() const {
+ Command* currentCommand = GetCurrentCommand();
+ if (currentCommand) {
+ return currentCommand->GetName();
+ } else {
+ return wpi::StringRef();
+ }
+}
+
+void Subsystem::Periodic() {}
+
+void Subsystem::InitDefaultCommand() {}
+
+void Subsystem::AddChild(const wpi::Twine& name,
+ std::shared_ptr<Sendable> child) {
+ AddChild(name, *child);
+}
+
+void Subsystem::AddChild(const wpi::Twine& name, Sendable* child) {
+ AddChild(name, *child);
+}
+
+void Subsystem::AddChild(const wpi::Twine& name, Sendable& child) {
+ child.SetName(GetSubsystem(), name);
+ LiveWindow::GetInstance()->Add(&child);
+}
+
+void Subsystem::AddChild(std::shared_ptr<Sendable> child) { AddChild(*child); }
+
+void Subsystem::AddChild(Sendable* child) { AddChild(*child); }
+
+void Subsystem::AddChild(Sendable& child) {
+ child.SetSubsystem(GetSubsystem());
+ LiveWindow::GetInstance()->Add(&child);
+}
+
+void Subsystem::ConfirmCommand() {
+ if (m_currentCommandChanged) m_currentCommandChanged = false;
+}
+
+void Subsystem::InitSendable(SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Subsystem");
+
+ builder.AddBooleanProperty(
+ ".hasDefault", [=]() { return m_defaultCommand != nullptr; }, nullptr);
+ builder.AddStringProperty(".default",
+ [=]() { return GetDefaultCommandName(); }, nullptr);
+
+ builder.AddBooleanProperty(
+ ".hasCommand", [=]() { return m_currentCommand != nullptr; }, nullptr);
+ builder.AddStringProperty(".command",
+ [=]() { return GetCurrentCommandName(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp b/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp
new file mode 100644
index 0000000..122751a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/TimedCommand.h"
+
+using namespace frc;
+
+TimedCommand::TimedCommand(const wpi::Twine& name, double timeout)
+ : Command(name, timeout) {}
+
+TimedCommand::TimedCommand(double timeout) : Command(timeout) {}
+
+TimedCommand::TimedCommand(const wpi::Twine& name, double timeout,
+ Subsystem& subsystem)
+ : Command(name, timeout, subsystem) {}
+
+TimedCommand::TimedCommand(double timeout, Subsystem& subsystem)
+ : Command(timeout, subsystem) {}
+
+bool TimedCommand::IsFinished() { return IsTimedOut(); }
diff --git a/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp b/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp
new file mode 100644
index 0000000..225d95b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/WaitCommand.h"
+
+using namespace frc;
+
+WaitCommand::WaitCommand(double timeout)
+ : TimedCommand("Wait(" + std::to_string(timeout) + ")", timeout) {}
+
+WaitCommand::WaitCommand(const wpi::Twine& name, double timeout)
+ : TimedCommand(name, timeout) {}
diff --git a/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp b/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp
new file mode 100644
index 0000000..5c99c1b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/WaitForChildren.h"
+
+#include "frc/commands/CommandGroup.h"
+
+using namespace frc;
+
+WaitForChildren::WaitForChildren(double timeout)
+ : Command("WaitForChildren", timeout) {}
+
+WaitForChildren::WaitForChildren(const wpi::Twine& name, double timeout)
+ : Command(name, timeout) {}
+
+bool WaitForChildren::IsFinished() {
+ return GetGroup() == nullptr || GetGroup()->GetSize() == 0;
+}
diff --git a/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp b/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp
new file mode 100644
index 0000000..6e24b6b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/commands/WaitUntilCommand.h"
+
+#include "frc/Timer.h"
+
+using namespace frc;
+
+WaitUntilCommand::WaitUntilCommand(double time)
+ : Command("WaitUntilCommand", time) {
+ m_time = time;
+}
+
+WaitUntilCommand::WaitUntilCommand(const wpi::Twine& name, double time)
+ : Command(name, time) {
+ m_time = time;
+}
+
+bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }
diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
new file mode 100644
index 0000000..5ad65b8
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -0,0 +1,225 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/drive/DifferentialDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/SpeedController.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+DifferentialDrive::DifferentialDrive(SpeedController& leftMotor,
+ SpeedController& rightMotor)
+ : m_leftMotor(leftMotor), m_rightMotor(rightMotor) {
+ AddChild(&m_leftMotor);
+ AddChild(&m_rightMotor);
+ static int instances = 0;
+ ++instances;
+ SetName("DifferentialDrive", instances);
+}
+
+void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
+ bool squareInputs) {
+ static bool reported = false;
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+ HALUsageReporting::kRobotDrive2_DifferentialArcade);
+ reported = true;
+ }
+
+ xSpeed = Limit(xSpeed);
+ xSpeed = ApplyDeadband(xSpeed, m_deadband);
+
+ zRotation = Limit(zRotation);
+ 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(Limit(leftMotorOutput) * m_maxOutput);
+ m_rightMotor.Set(Limit(rightMotorOutput) * m_maxOutput *
+ m_rightSideInvertMultiplier);
+
+ Feed();
+}
+
+void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
+ bool isQuickTurn) {
+ static bool reported = false;
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+ HALUsageReporting::kRobotDrive2_DifferentialCurvature);
+ reported = true;
+ }
+
+ xSpeed = Limit(xSpeed);
+ xSpeed = ApplyDeadband(xSpeed, m_deadband);
+
+ zRotation = Limit(zRotation);
+ 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 * Limit(zRotation) * 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, 2,
+ HALUsageReporting::kRobotDrive2_DifferentialTank);
+ reported = true;
+ }
+
+ leftSpeed = Limit(leftSpeed);
+ leftSpeed = ApplyDeadband(leftSpeed, m_deadband);
+
+ rightSpeed = Limit(rightSpeed);
+ 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..b1af5de
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/drive/KilloughDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/SpeedController.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+constexpr double kPi = 3.14159265358979323846;
+
+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 * (kPi / 180.0)),
+ std::sin(leftMotorAngle * (kPi / 180.0))};
+ m_rightVec = {std::cos(rightMotorAngle * (kPi / 180.0)),
+ std::sin(rightMotorAngle * (kPi / 180.0))};
+ m_backVec = {std::cos(backMotorAngle * (kPi / 180.0)),
+ std::sin(backMotorAngle * (kPi / 180.0))};
+ AddChild(&m_leftMotor);
+ AddChild(&m_rightMotor);
+ AddChild(&m_backMotor);
+ static int instances = 0;
+ ++instances;
+ SetName("KilloughDrive", instances);
+}
+
+void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
+ double zRotation, double gyroAngle) {
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
+ HALUsageReporting::kRobotDrive2_KilloughCartesian);
+ reported = true;
+ }
+
+ ySpeed = Limit(ySpeed);
+ ySpeed = ApplyDeadband(ySpeed, m_deadband);
+
+ xSpeed = Limit(xSpeed);
+ 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, 3,
+ HALUsageReporting::kRobotDrive2_KilloughPolar);
+ reported = true;
+ }
+
+ DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)),
+ magnitude * std::cos(angle * (kPi / 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..c74ba19
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* 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/drive/MecanumDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/SpeedController.h"
+#include "frc/drive/Vector2d.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+constexpr double kPi = 3.14159265358979323846;
+
+MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor,
+ SpeedController& rearLeftMotor,
+ SpeedController& frontRightMotor,
+ SpeedController& rearRightMotor)
+ : m_frontLeftMotor(frontLeftMotor),
+ m_rearLeftMotor(rearLeftMotor),
+ m_frontRightMotor(frontRightMotor),
+ m_rearRightMotor(rearRightMotor) {
+ AddChild(&m_frontLeftMotor);
+ AddChild(&m_rearLeftMotor);
+ AddChild(&m_frontRightMotor);
+ AddChild(&m_rearRightMotor);
+ static int instances = 0;
+ ++instances;
+ SetName("MecanumDrive", instances);
+}
+
+void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
+ double zRotation, double gyroAngle) {
+ if (!reported) {
+ HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
+ HALUsageReporting::kRobotDrive2_MecanumCartesian);
+ reported = true;
+ }
+
+ ySpeed = Limit(ySpeed);
+ ySpeed = ApplyDeadband(ySpeed, m_deadband);
+
+ xSpeed = Limit(xSpeed);
+ 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, 4,
+ HALUsageReporting::kRobotDrive2_MecanumPolar);
+ reported = true;
+ }
+
+ DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)),
+ magnitude * std::cos(angle * (kPi / 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..c22cb2a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/drive/RobotDriveBase.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+
+#include <hal/HAL.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::Limit(double value) {
+ if (value > 1.0) {
+ return 1.0;
+ }
+ if (value < -1.0) {
+ return -1.0;
+ }
+ return value;
+}
+
+double RobotDriveBase::ApplyDeadband(double value, double deadband) {
+ if (std::abs(value) > deadband) {
+ if (value > 0.0) {
+ 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..3d4b689
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/drive/Vector2d.cpp
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/drive/Vector2d.h"
+
+#include <cmath>
+
+using namespace frc;
+
+constexpr double kPi = 3.14159265358979323846;
+
+Vector2d::Vector2d(double x, double y) {
+ this->x = x;
+ this->y = y;
+}
+
+void Vector2d::Rotate(double angle) {
+ double cosA = std::cos(angle * (kPi / 180.0));
+ double sinA = std::sin(angle * (kPi / 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..db4cebe
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
@@ -0,0 +1,122 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/filters/LinearDigitalFilter.h"
+
+#include <cassert>
+#include <cmath>
+
+#include <hal/HAL.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(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::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/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/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
new file mode 100644
index 0000000..135d044
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -0,0 +1,238 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2012-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/livewindow/LiveWindow.h"
+
+#include <algorithm>
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/DenseMap.h>
+#include <wpi/SmallString.h>
+#include <wpi/mutex.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/commands/Scheduler.h"
+#include "frc/smartdashboard/SendableBuilderImpl.h"
+
+using namespace frc;
+
+using wpi::Twine;
+
+struct LiveWindow::Impl {
+ Impl();
+
+ struct Component {
+ std::shared_ptr<Sendable> sendable;
+ Sendable* parent = nullptr;
+ SendableBuilderImpl builder;
+ bool firstTime = true;
+ bool telemetryEnabled = true;
+ };
+
+ wpi::mutex mutex;
+
+ wpi::DenseMap<void*, Component> components;
+
+ std::shared_ptr<nt::NetworkTable> liveWindowTable;
+ std::shared_ptr<nt::NetworkTable> statusTable;
+ nt::NetworkTableEntry enabledEntry;
+
+ bool startLiveWindow = false;
+ bool liveWindowEnabled = false;
+ bool telemetryEnabled = true;
+};
+
+LiveWindow::Impl::Impl()
+ : liveWindowTable(
+ nt::NetworkTableInstance::GetDefault().GetTable("LiveWindow")) {
+ statusTable = liveWindowTable->GetSubTable(".status");
+ enabledEntry = statusTable->GetEntry("LW Enabled");
+}
+
+LiveWindow* LiveWindow::GetInstance() {
+ static LiveWindow instance;
+ return &instance;
+}
+
+void LiveWindow::Run() { UpdateValues(); }
+
+void LiveWindow::AddSensor(const wpi::Twine& subsystem, const wpi::Twine& name,
+ Sendable* component) {
+ Add(component);
+ component->SetName(subsystem, name);
+}
+
+void LiveWindow::AddSensor(const wpi::Twine& subsystem, const wpi::Twine& name,
+ Sendable& component) {
+ Add(&component);
+ component.SetName(subsystem, name);
+}
+
+void LiveWindow::AddSensor(const wpi::Twine& subsystem, const wpi::Twine& name,
+ std::shared_ptr<Sendable> component) {
+ Add(component);
+ component->SetName(subsystem, name);
+}
+
+void LiveWindow::AddActuator(const wpi::Twine& subsystem,
+ const wpi::Twine& name, Sendable* component) {
+ Add(component);
+ component->SetName(subsystem, name);
+}
+
+void LiveWindow::AddActuator(const wpi::Twine& subsystem,
+ const wpi::Twine& name, Sendable& component) {
+ Add(&component);
+ component.SetName(subsystem, name);
+}
+
+void LiveWindow::AddActuator(const wpi::Twine& subsystem,
+ const wpi::Twine& name,
+ std::shared_ptr<Sendable> component) {
+ Add(component);
+ component->SetName(subsystem, name);
+}
+
+void LiveWindow::AddSensor(const wpi::Twine& type, int channel,
+ Sendable* component) {
+ Add(component);
+ component->SetName("Ungrouped",
+ type + Twine('[') + Twine(channel) + Twine(']'));
+}
+
+void LiveWindow::AddActuator(const wpi::Twine& type, int channel,
+ Sendable* component) {
+ Add(component);
+ component->SetName("Ungrouped",
+ type + Twine('[') + Twine(channel) + Twine(']'));
+}
+
+void LiveWindow::AddActuator(const wpi::Twine& type, int module, int channel,
+ Sendable* component) {
+ Add(component);
+ component->SetName("Ungrouped", type + Twine('[') + Twine(module) +
+ Twine(',') + Twine(channel) + Twine(']'));
+}
+
+void LiveWindow::Add(std::shared_ptr<Sendable> sendable) {
+ std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ auto& comp = m_impl->components[sendable.get()];
+ comp.sendable = sendable;
+}
+
+void LiveWindow::Add(Sendable* sendable) {
+ Add(std::shared_ptr<Sendable>(sendable, NullDeleter<Sendable>()));
+}
+
+void LiveWindow::AddChild(Sendable* parent, std::shared_ptr<Sendable> child) {
+ AddChild(parent, child.get());
+}
+
+void LiveWindow::AddChild(Sendable* parent, void* child) {
+ std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ auto& comp = m_impl->components[child];
+ comp.parent = parent;
+ comp.telemetryEnabled = false;
+}
+
+void LiveWindow::Remove(Sendable* sendable) {
+ std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ m_impl->components.erase(sendable);
+}
+
+void LiveWindow::EnableTelemetry(Sendable* sendable) {
+ std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ // Re-enable global setting in case DisableAllTelemetry() was called.
+ m_impl->telemetryEnabled = true;
+ auto i = m_impl->components.find(sendable);
+ if (i != m_impl->components.end()) i->getSecond().telemetryEnabled = true;
+}
+
+void LiveWindow::DisableTelemetry(Sendable* sendable) {
+ std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ auto i = m_impl->components.find(sendable);
+ if (i != m_impl->components.end()) i->getSecond().telemetryEnabled = false;
+}
+
+void LiveWindow::DisableAllTelemetry() {
+ std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ m_impl->telemetryEnabled = false;
+ for (auto& i : m_impl->components) i.getSecond().telemetryEnabled = false;
+}
+
+bool LiveWindow::IsEnabled() const {
+ std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ return m_impl->liveWindowEnabled;
+}
+
+void LiveWindow::SetEnabled(bool enabled) {
+ std::lock_guard<wpi::mutex> lock(m_impl->mutex);
+ if (m_impl->liveWindowEnabled == enabled) return;
+ Scheduler* scheduler = Scheduler::GetInstance();
+ m_impl->startLiveWindow = enabled;
+ m_impl->liveWindowEnabled = enabled;
+ // Force table generation now to make sure everything is defined
+ UpdateValuesUnsafe();
+ if (enabled) {
+ scheduler->SetEnabled(false);
+ scheduler->RemoveAll();
+ } else {
+ for (auto& i : m_impl->components) {
+ i.getSecond().builder.StopLiveWindowMode();
+ }
+ scheduler->SetEnabled(true);
+ }
+ m_impl->enabledEntry.SetBoolean(enabled);
+}
+
+void LiveWindow::UpdateValues() {
+ std::lock_guard<wpi::mutex> 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;
+
+ for (auto& i : m_impl->components) {
+ auto& comp = i.getSecond();
+ if (comp.sendable && !comp.parent &&
+ (m_impl->liveWindowEnabled || comp.telemetryEnabled)) {
+ if (comp.firstTime) {
+ // By holding off creating the NetworkTable entries, it allows the
+ // components to be redefined. This allows default sensor and actuator
+ // values to be created that are replaced with the custom names from
+ // users calling setName.
+ auto name = comp.sendable->GetName();
+ if (name.empty()) continue;
+ auto subsystem = comp.sendable->GetSubsystem();
+ auto ssTable = m_impl->liveWindowTable->GetSubTable(subsystem);
+ std::shared_ptr<NetworkTable> table;
+ // Treat name==subsystem as top level of subsystem
+ if (name == subsystem)
+ table = ssTable;
+ else
+ table = ssTable->GetSubTable(name);
+ table->GetEntry(".name").SetString(name);
+ comp.builder.SetTable(table);
+ comp.sendable->InitSendable(comp.builder);
+ ssTable->GetEntry(".type").SetString("LW Subsystem");
+
+ comp.firstTime = false;
+ }
+
+ if (m_impl->startLiveWindow) comp.builder.StartLiveWindowMode();
+ comp.builder.UpdateTable();
+ }
+ }
+
+ m_impl->startLiveWindow = false;
+}
+
+LiveWindow::LiveWindow() : m_impl(new Impl) {}
diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindowSendable.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindowSendable.cpp
new file mode 100644
index 0000000..7f03f52
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindowSendable.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/livewindow/LiveWindowSendable.h"
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+std::string LiveWindowSendable::GetName() const { return std::string(); }
+
+void LiveWindowSendable::SetName(const wpi::Twine&) {}
+
+std::string LiveWindowSendable::GetSubsystem() const { return std::string(); }
+
+void LiveWindowSendable::SetSubsystem(const wpi::Twine&) {}
+
+void LiveWindowSendable::InitSendable(SendableBuilder& builder) {
+ builder.SetUpdateTable([=]() { UpdateTable(); });
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/BuiltInLayouts.cpp b/wpilibc/src/main/native/cpp/shuffleboard/BuiltInLayouts.cpp
new file mode 100644
index 0000000..5d09310
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/BuiltInLayouts.cpp
@@ -0,0 +1,13 @@
+/*----------------------------------------------------------------------------*/
+/* 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/BuiltInLayouts.h"
+
+using namespace frc;
+
+const LayoutType BuiltInLayouts::kList{"List Layout"};
+const LayoutType BuiltInLayouts::kGrid{"Grid Layout"};
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/BuiltInWidgets.cpp b/wpilibc/src/main/native/cpp/shuffleboard/BuiltInWidgets.cpp
new file mode 100644
index 0000000..df6a1f3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/BuiltInWidgets.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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/BuiltInWidgets.h"
+
+using namespace frc;
+
+const WidgetType BuiltInWidgets::kTextView{"Text View"};
+const WidgetType BuiltInWidgets::kNumberSlider{"Number Slider"};
+const WidgetType BuiltInWidgets::kNumberBar{"Number Bar"};
+const WidgetType BuiltInWidgets::kDial{"Simple Dial"};
+const WidgetType BuiltInWidgets::kGraph{"Graph"};
+const WidgetType BuiltInWidgets::kBooleanBox{"Boolean Box"};
+const WidgetType BuiltInWidgets::kToggleButton{"Toggle Button"};
+const WidgetType BuiltInWidgets::kToggleSwitch{"Toggle Switch"};
+const WidgetType BuiltInWidgets::kVoltageView{"Voltage View"};
+const WidgetType BuiltInWidgets::kPowerDistributionPanel{"PDP"};
+const WidgetType BuiltInWidgets::kComboBoxChooser{"ComboBox Chooser"};
+const WidgetType BuiltInWidgets::kSplitButtonChooser{"Split Button Chooser"};
+const WidgetType BuiltInWidgets::kEncoder{"Encoder"};
+const WidgetType BuiltInWidgets::kSpeedController{"Speed Controller"};
+const WidgetType BuiltInWidgets::kCommand{"Command"};
+const WidgetType BuiltInWidgets::kPIDCommand{"PID Command"};
+const WidgetType BuiltInWidgets::kPIDController{"PID Controller"};
+const WidgetType BuiltInWidgets::kAccelerometer{"Accelerometer"};
+const WidgetType BuiltInWidgets::k3AxisAccelerometer{"3-Axis Accelerometer"};
+const WidgetType BuiltInWidgets::kGyro{"Gyro"};
+const WidgetType BuiltInWidgets::kRelay{"Relay"};
+const WidgetType BuiltInWidgets::kDifferentialDrive{"Differential Drivebase"};
+const WidgetType BuiltInWidgets::kMecanumDrive{"Mecanum Drivebase"};
+const WidgetType BuiltInWidgets::kCameraStream{"Camera Stream"};
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..294be79
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp
@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* 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/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;
+ }
+ auto arr = wpi::ArrayRef<std::string>{
+ description, ShuffleboardEventImportanceName(importance)};
+ m_eventsTable->GetSubTable(name)->GetEntry("Info").SetStringArray(arr);
+}
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..87d7e9d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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/SendableCameraWrapper.h"
+
+#include <cscore_oo.h>
+#include <wpi/DenseMap.h>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+namespace {
+constexpr const char* kProtocol = "camera_server://";
+wpi::DenseMap<int, std::unique_ptr<SendableCameraWrapper>> wrappers;
+} // namespace
+
+SendableCameraWrapper& SendableCameraWrapper::Wrap(
+ const cs::VideoSource& source) {
+ return Wrap(source.GetHandle());
+}
+
+SendableCameraWrapper& SendableCameraWrapper::Wrap(CS_Source source) {
+ auto& wrapper = wrappers[static_cast<int>(source)];
+ if (!wrapper)
+ wrapper = std::make_unique<SendableCameraWrapper>(source, private_init{});
+ return *wrapper;
+}
+
+SendableCameraWrapper::SendableCameraWrapper(CS_Source source,
+ const private_init&)
+ : SendableBase(false), m_uri(kProtocol) {
+ CS_Status status = 0;
+ auto name = cs::GetSourceName(source, &status);
+ SetName(name);
+ m_uri += name;
+}
+
+void SendableCameraWrapper::InitSendable(SendableBuilder& builder) {
+ builder.AddStringProperty(".ShuffleboardURI", [this] { return m_uri; },
+ nullptr);
+}
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..4954cff
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* 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/ShuffleboardContainer.h"
+
+#include <wpi/SmallVector.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/shuffleboard/ComplexWidget.h"
+#include "frc/shuffleboard/SendableCameraWrapper.h"
+#include "frc/shuffleboard/ShuffleboardComponent.h"
+#include "frc/shuffleboard/ShuffleboardLayout.h"
+#include "frc/shuffleboard/SimpleWidget.h"
+
+using namespace frc;
+
+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,
+ 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(const wpi::Twine& title,
+ const cs::VideoSource& video) {
+ return Add(title, SendableCameraWrapper::Wrap(video));
+}
+
+ComplexWidget& ShuffleboardContainer::Add(Sendable& sendable) {
+ if (sendable.GetName().empty()) {
+ wpi::outs() << "Sendable must have a name\n";
+ }
+ return Add(sendable.GetName(), sendable);
+}
+
+ComplexWidget& ShuffleboardContainer::Add(const cs::VideoSource& video) {
+ return Add(SendableCameraWrapper::Wrap(video));
+}
+
+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));
+}
+
+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..39e5778
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* 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/ShuffleboardInstance.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");
+}
+
+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/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/NamedSendable.cpp b/wpilibc/src/main/native/cpp/smartdashboard/NamedSendable.cpp
new file mode 100644
index 0000000..61028b3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/NamedSendable.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/NamedSendable.h"
+
+using namespace frc;
+
+void NamedSendable::SetName(const wpi::Twine&) {}
+
+std::string NamedSendable::GetSubsystem() const { return std::string(); }
+
+void NamedSendable::SetSubsystem(const wpi::Twine&) {}
+
+void NamedSendable::InitSendable(SendableBuilder&) {}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp
new file mode 100644
index 0000000..28bd7fd
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/SendableBase.h"
+
+#include <utility>
+
+#include "frc/livewindow/LiveWindow.h"
+
+using namespace frc;
+
+SendableBase::SendableBase(bool addLiveWindow) {
+ if (addLiveWindow) LiveWindow::GetInstance()->Add(this);
+}
+
+SendableBase::~SendableBase() { LiveWindow::GetInstance()->Remove(this); }
+
+SendableBase::SendableBase(SendableBase&& rhs) {
+ m_name = std::move(rhs.m_name);
+ m_subsystem = std::move(rhs.m_subsystem);
+}
+
+SendableBase& SendableBase::operator=(SendableBase&& rhs) {
+ Sendable::operator=(std::move(rhs));
+
+ m_name = std::move(rhs.m_name);
+ m_subsystem = std::move(rhs.m_subsystem);
+
+ return *this;
+}
+
+std::string SendableBase::GetName() const {
+ std::lock_guard<wpi::mutex> lock(m_mutex);
+ return m_name;
+}
+
+void SendableBase::SetName(const wpi::Twine& name) {
+ std::lock_guard<wpi::mutex> lock(m_mutex);
+ m_name = name.str();
+}
+
+std::string SendableBase::GetSubsystem() const {
+ std::lock_guard<wpi::mutex> lock(m_mutex);
+ return m_subsystem;
+}
+
+void SendableBase::SetSubsystem(const wpi::Twine& subsystem) {
+ std::lock_guard<wpi::mutex> lock(m_mutex);
+ m_subsystem = subsystem.str();
+}
+
+void SendableBase::AddChild(std::shared_ptr<Sendable> child) {
+ LiveWindow::GetInstance()->AddChild(this, child);
+}
+
+void SendableBase::AddChild(void* child) {
+ LiveWindow::GetInstance()->AddChild(this, child);
+}
+
+void SendableBase::SetName(const wpi::Twine& moduleType, int channel) {
+ SetName(moduleType + wpi::Twine('[') + wpi::Twine(channel) + wpi::Twine(']'));
+}
+
+void SendableBase::SetName(const wpi::Twine& moduleType, int moduleNumber,
+ int channel) {
+ SetName(moduleType + wpi::Twine('[') + wpi::Twine(moduleNumber) +
+ wpi::Twine(',') + wpi::Twine(channel) + wpi::Twine(']'));
+}
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..bce03b5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
@@ -0,0 +1,382 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/SendableBuilderImpl.h"
+
+#include <ntcore_cpp.h>
+#include <wpi/SmallString.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::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::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;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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) { 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;
+ 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;
+ 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;
+ 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;
+ 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;
+ 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..2cdf92c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/SendableChooserBase.h"
+
+using namespace frc;
+
+std::atomic_int SendableChooserBase::s_instances{0};
+
+SendableChooserBase::SendableChooserBase()
+ : SendableBase(false), m_instance{s_instances++} {}
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..98e5568
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -0,0 +1,263 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2011-2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/SmartDashboard.h"
+
+#include <hal/HAL.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/StringMap.h>
+#include <wpi/mutex.h>
+
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilderImpl.h"
+
+using namespace frc;
+
+namespace {
+class SmartDashboardData {
+ public:
+ SmartDashboardData() = default;
+ explicit SmartDashboardData(Sendable* sendable_) : sendable(sendable_) {}
+
+ Sendable* sendable = nullptr;
+ SendableBuilderImpl builder;
+};
+
+class Singleton {
+ public:
+ static Singleton& GetInstance();
+
+ std::shared_ptr<nt::NetworkTable> table;
+ wpi::StringMap<SmartDashboardData> tablesToData;
+ wpi::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);
+}
+
+void SmartDashboard::PutData(wpi::StringRef key, Sendable* data) {
+ if (data == nullptr) {
+ wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
+ return;
+ }
+ auto& inst = Singleton::GetInstance();
+ std::lock_guard<wpi::mutex> lock(inst.tablesToDataMutex);
+ auto& sddata = inst.tablesToData[key];
+ if (!sddata.sendable || sddata.sendable != data) {
+ sddata = SmartDashboardData(data);
+ auto dataTable = inst.table->GetSubTable(key);
+ sddata.builder.SetTable(dataTable);
+ data->InitSendable(sddata.builder);
+ sddata.builder.UpdateTable();
+ sddata.builder.StartListeners();
+ dataTable->GetEntry(".name").SetString(key);
+ }
+}
+
+void SmartDashboard::PutData(Sendable* value) {
+ if (value == nullptr) {
+ wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
+ return;
+ }
+ PutData(value->GetName(), value);
+}
+
+Sendable* SmartDashboard::GetData(wpi::StringRef key) {
+ auto& inst = Singleton::GetInstance();
+ std::lock_guard<wpi::mutex> lock(inst.tablesToDataMutex);
+ auto data = inst.tablesToData.find(key);
+ if (data == inst.tablesToData.end()) {
+ wpi_setGlobalWPIErrorWithContext(SmartDashboardMissingKey, key);
+ return nullptr;
+ }
+ return data->getValue().sendable;
+}
+
+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();
+}
+
+void SmartDashboard::UpdateValues() {
+ auto& inst = Singleton::GetInstance();
+ std::lock_guard<wpi::mutex> lock(inst.tablesToDataMutex);
+ for (auto& i : inst.tablesToData) {
+ i.getValue().builder.UpdateTable();
+ }
+}