Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/wpilibc/Athena/src/ADXL345_I2C.cpp b/wpilibc/Athena/src/ADXL345_I2C.cpp
new file mode 100644
index 0000000..000d4b8
--- /dev/null
+++ b/wpilibc/Athena/src/ADXL345_I2C.cpp
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "ADXL345_I2C.h"
+#include "I2C.h"
+#include "HAL/HAL.hpp"
+#include "LiveWindow/LiveWindow.h"
+
+const uint8_t ADXL345_I2C::kAddress;
+const uint8_t ADXL345_I2C::kPowerCtlRegister;
+const uint8_t ADXL345_I2C::kDataFormatRegister;
+const uint8_t ADXL345_I2C::kDataRegister;
+constexpr double ADXL345_I2C::kGsPerLSB;
+
+/**
+ * Constructor.
+ *
+ * @param port The I2C port the accelerometer is attached to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ADXL345_I2C::ADXL345_I2C(Port port, Range range) : I2C(port, kAddress) {
+ // Turn on the measurements
+ Write(kPowerCtlRegister, kPowerCtl_Measure);
+ // Specify the data format to read
+ SetRange(range);
+
+ HALReport(HALUsageReporting::kResourceType_ADXL345,
+ HALUsageReporting::kADXL345_I2C, 0);
+ LiveWindow::GetInstance()->AddSensor("ADXL345_I2C", port, this);
+}
+
+/** {@inheritdoc} */
+void ADXL345_I2C::SetRange(Range range) {
+ Write(kDataFormatRegister, kDataFormat_FullRes | (uint8_t)range);
+}
+
+/** {@inheritdoc} */
+double ADXL345_I2C::GetX() { return GetAcceleration(kAxis_X); }
+
+/** {@inheritdoc} */
+double ADXL345_I2C::GetY() { return GetAcceleration(kAxis_Y); }
+
+/** {@inheritdoc} */
+double ADXL345_I2C::GetZ() { return GetAcceleration(kAxis_Z); }
+
+/**
+ * Get the acceleration of one axis in Gs.
+ *
+ * @param axis The axis to read from.
+ * @return Acceleration of the ADXL345 in Gs.
+ */
+double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis) {
+ int16_t rawAccel = 0;
+ Read(kDataRegister + (uint8_t)axis, sizeof(rawAccel), (uint8_t *)&rawAccel);
+ return rawAccel * kGsPerLSB;
+}
+
+/**
+ * Get the acceleration of all axes in Gs.
+ *
+ * @return An object containing the acceleration measured on each axis of the
+ * ADXL345 in Gs.
+ */
+ADXL345_I2C::AllAxes ADXL345_I2C::GetAccelerations() {
+ AllAxes data = AllAxes();
+ int16_t rawData[3];
+ Read(kDataRegister, sizeof(rawData), (uint8_t *)rawData);
+
+ data.XAxis = rawData[0] * kGsPerLSB;
+ data.YAxis = rawData[1] * kGsPerLSB;
+ data.ZAxis = rawData[2] * kGsPerLSB;
+ return data;
+}
+
+std::string ADXL345_I2C::GetSmartDashboardType() const {
+ return "3AxisAccelerometer";
+}
+
+void ADXL345_I2C::InitTable(std::shared_ptr<ITable> subtable) {
+ m_table = subtable;
+ UpdateTable();
+}
+
+void ADXL345_I2C::UpdateTable() {
+ m_table->PutNumber("X", GetX());
+ m_table->PutNumber("Y", GetY());
+ m_table->PutNumber("Z", GetZ());
+}
+
+std::shared_ptr<ITable> ADXL345_I2C::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/ADXL345_SPI.cpp b/wpilibc/Athena/src/ADXL345_SPI.cpp
new file mode 100644
index 0000000..7fb3b71
--- /dev/null
+++ b/wpilibc/Athena/src/ADXL345_SPI.cpp
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "ADXL345_SPI.h"
+#include "DigitalInput.h"
+#include "DigitalOutput.h"
+#include "LiveWindow/LiveWindow.h"
+
+const uint8_t ADXL345_SPI::kPowerCtlRegister;
+const uint8_t ADXL345_SPI::kDataFormatRegister;
+const uint8_t ADXL345_SPI::kDataRegister;
+constexpr double ADXL345_SPI::kGsPerLSB;
+
+/**
+ * Constructor.
+ *
+ * @param port The SPI port the accelerometer is attached to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range) : SPI(port) {
+ SetClockRate(500000);
+ SetMSBFirst();
+ SetSampleDataOnFalling();
+ SetClockActiveLow();
+ SetChipSelectActiveHigh();
+
+ uint8_t commands[2];
+ // Turn on the measurements
+ commands[0] = kPowerCtlRegister;
+ commands[1] = kPowerCtl_Measure;
+ Transaction(commands, commands, 2);
+
+ SetRange(range);
+
+ HALReport(HALUsageReporting::kResourceType_ADXL345,
+ HALUsageReporting::kADXL345_SPI);
+
+ LiveWindow::GetInstance()->AddSensor("ADXL345_SPI", port, this);
+}
+
+/** {@inheritdoc} */
+void ADXL345_SPI::SetRange(Range range) {
+ uint8_t commands[2];
+
+ // Specify the data format to read
+ commands[0] = kDataFormatRegister;
+ commands[1] = kDataFormat_FullRes | (uint8_t)(range & 0x03);
+ Transaction(commands, commands, 2);
+}
+
+/** {@inheritdoc} */
+double ADXL345_SPI::GetX() { return GetAcceleration(kAxis_X); }
+
+/** {@inheritdoc} */
+double ADXL345_SPI::GetY() { return GetAcceleration(kAxis_Y); }
+
+/** {@inheritdoc} */
+double ADXL345_SPI::GetZ() { return GetAcceleration(kAxis_Z); }
+
+/**
+ * Get the acceleration of one axis in Gs.
+ *
+ * @param axis The axis to read from.
+ * @return Acceleration of the ADXL345 in Gs.
+ */
+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) + (uint8_t)axis;
+ Transaction(command, buffer, 3);
+
+ // Sensor is little endian... swap bytes
+ int16_t rawAccel = buffer[2] << 8 | buffer[1];
+ return rawAccel * kGsPerLSB;
+}
+
+/**
+ * Get the acceleration of all axes in Gs.
+ *
+ * @return An object containing the acceleration measured on each axis of the
+ * ADXL345 in Gs.
+ */
+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);
+ Transaction(dataBuffer, dataBuffer, 7);
+
+ for (int32_t 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;
+}
+
+std::string ADXL345_SPI::GetSmartDashboardType() const {
+ return "3AxisAccelerometer";
+}
+
+void ADXL345_SPI::InitTable(std::shared_ptr<ITable> subtable) {
+ m_table = subtable;
+ UpdateTable();
+}
+
+void ADXL345_SPI::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("X", GetX());
+ m_table->PutNumber("Y", GetY());
+ m_table->PutNumber("Z", GetZ());
+ }
+}
+
+std::shared_ptr<ITable> ADXL345_SPI::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/ADXL362.cpp b/wpilibc/Athena/src/ADXL362.cpp
new file mode 100644
index 0000000..3c70b9b
--- /dev/null
+++ b/wpilibc/Athena/src/ADXL362.cpp
@@ -0,0 +1,181 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "ADXL362.h"
+#include "DigitalInput.h"
+#include "DigitalOutput.h"
+#include "DriverStation.h"
+#include "LiveWindow/LiveWindow.h"
+
+static uint8_t kRegWrite = 0x0A;
+static uint8_t kRegRead = 0x0B;
+
+static uint8_t kPartIdRegister = 0x02;
+static uint8_t kDataRegister = 0x0E;
+static uint8_t kFilterCtlRegister = 0x2C;
+static uint8_t kPowerCtlRegister = 0x2D;
+
+//static uint8_t kFilterCtl_Range2G = 0x00;
+//static uint8_t kFilterCtl_Range4G = 0x40;
+//static uint8_t kFilterCtl_Range8G = 0x80;
+static uint8_t kFilterCtl_ODR_100Hz = 0x03;
+
+static uint8_t kPowerCtl_UltraLowNoise = 0x20;
+//static uint8_t kPowerCtl_AutoSleep = 0x04;
+static uint8_t kPowerCtl_Measure = 0x02;
+
+/**
+ * Constructor. Uses the onboard CS1.
+ *
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ADXL362::ADXL362(Range range) : ADXL362(SPI::Port::kOnboardCS1, range) {}
+
+/**
+ * Constructor.
+ *
+ * @param port The SPI port the accelerometer is attached to
+ * @param range The range (+ or -) that the accelerometer will measure.
+ */
+ADXL362::ADXL362(SPI::Port port, Range range) : m_spi(port) {
+ m_spi.SetClockRate(3000000);
+ m_spi.SetMSBFirst();
+ m_spi.SetSampleDataOnFalling();
+ 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);
+
+ HALReport(HALUsageReporting::kResourceType_ADXL362, port);
+
+ LiveWindow::GetInstance()->AddSensor("ADXL362", port, this);
+}
+
+/** {@inheritdoc} */
+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 | (uint8_t)((range & 0x03) << 6);
+ m_spi.Write(commands, 3);
+}
+
+/** {@inheritdoc} */
+double ADXL362::GetX() { return GetAcceleration(kAxis_X); }
+
+/** {@inheritdoc} */
+double ADXL362::GetY() { return GetAcceleration(kAxis_Y); }
+
+/** {@inheritdoc} */
+double ADXL362::GetZ() { return GetAcceleration(kAxis_Z); }
+
+/**
+ * Get the acceleration of one axis in Gs.
+ *
+ * @param axis The axis to read from.
+ * @return Acceleration of the ADXL362 in Gs.
+ */
+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 + (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;
+}
+
+/**
+ * Get the acceleration of all axes in Gs.
+ *
+ * @return An object containing the acceleration measured on each axis of the
+ * ADXL362 in Gs.
+ */
+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 (int32_t 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;
+}
+
+std::string ADXL362::GetSmartDashboardType() const {
+ return "3AxisAccelerometer";
+}
+
+void ADXL362::InitTable(std::shared_ptr<ITable> subtable) {
+ m_table = subtable;
+ UpdateTable();
+}
+
+void ADXL362::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("X", GetX());
+ m_table->PutNumber("Y", GetY());
+ m_table->PutNumber("Z", GetZ());
+ }
+}
+
+std::shared_ptr<ITable> ADXL362::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/ADXRS450_Gyro.cpp b/wpilibc/Athena/src/ADXRS450_Gyro.cpp
new file mode 100644
index 0000000..6b111cf
--- /dev/null
+++ b/wpilibc/Athena/src/ADXRS450_Gyro.cpp
@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "ADXRS450_Gyro.h"
+#include "DriverStation.h"
+#include "LiveWindow/LiveWindow.h"
+#include "Timer.h"
+
+static constexpr double kSamplePeriod = 0.001;
+static constexpr double kCalibrationSampleTime = 5.0;
+static constexpr double kDegreePerSecondPerLSB = 0.0125;
+
+static constexpr uint8_t kRateRegister = 0x00;
+static constexpr uint8_t kTemRegister = 0x02;
+static constexpr uint8_t kLoCSTRegister = 0x04;
+static constexpr uint8_t kHiCSTRegister = 0x06;
+static constexpr uint8_t kQuadRegister = 0x08;
+static constexpr uint8_t kFaultRegister = 0x0A;
+static constexpr uint8_t kPIDRegister = 0x0C;
+static constexpr uint8_t kSNHighRegister = 0x0E;
+static constexpr uint8_t kSNLowRegister = 0x10;
+
+/**
+ * Initialize the gyro.
+ * Calibrate the gyro by running for a number of samples and computing the
+ * center value.
+ * Then use the center value as the Accumulator center value for subsequent
+ * measurements.
+ * It's important to make sure that the robot is not moving while the centering
+ * calculations are in progress, this is typically done when the robot is first
+ * turned on while it's sitting at rest before the competition starts.
+ */
+void ADXRS450_Gyro::Calibrate() {
+ Wait(0.1);
+
+ m_spi.SetAccumulatorCenter(0);
+ m_spi.ResetAccumulator();
+
+ Wait(kCalibrationSampleTime);
+
+ m_spi.SetAccumulatorCenter((int32_t)m_spi.GetAccumulatorAverage());
+ m_spi.ResetAccumulator();
+}
+
+/**
+ * Gyro constructor on onboard CS0.
+ */
+ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
+
+/**
+ * Gyro constructor on the specified SPI port.
+ *
+ * @param port The SPI port the gyro is attached to.
+ */
+ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) : m_spi(port) {
+ m_spi.SetClockRate(3000000);
+ m_spi.SetMSBFirst();
+ m_spi.SetSampleDataOnRising();
+ 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, 0x0c000000u, 0x04000000u,
+ 10u, 16u, true, true);
+
+ Calibrate();
+
+ HALReport(HALUsageReporting::kResourceType_ADXRS450, port);
+ LiveWindow::GetInstance()->AddSensor("ADXRS450_Gyro", port, this);
+}
+
+static bool CalcParity(uint32_t v) {
+ bool parity = false;
+ while (v != 0) {
+ parity = !parity;
+ v = v & (v - 1);
+ }
+ return parity;
+}
+
+static inline uint32_t BytesToIntBE(uint8_t* buf) {
+ uint32_t result = ((uint32_t)buf[0]) << 24;
+ result |= ((uint32_t)buf[1]) << 16;
+ result |= ((uint32_t)buf[2]) << 8;
+ result |= (uint32_t)buf[3];
+ return result;
+}
+
+uint16_t ADXRS450_Gyro::ReadRegister(uint8_t reg) {
+ uint32_t cmd = 0x80000000 | (((uint32_t)reg) << 17);
+ if (!CalcParity(cmd)) cmd |= 1u;
+
+ // big endian
+ uint8_t buf[4] = {(uint8_t)((cmd >> 24) & 0xff),
+ (uint8_t)((cmd >> 16) & 0xff),
+ (uint8_t)((cmd >> 8) & 0xff),
+ (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 (uint16_t)((BytesToIntBE(buf) >> 5) & 0xffff);
+}
+
+/**
+ * Reset the gyro.
+ * Resets the gyro to a heading of zero. This can be used if there is
+ * significant
+ * drift in the gyro and it needs to be recalibrated after it has been running.
+ */
+void ADXRS450_Gyro::Reset() {
+ m_spi.ResetAccumulator();
+}
+
+/**
+ * Return the actual angle in degrees that the robot is currently facing.
+ *
+ * The angle is based on the current accumulator value corrected by the
+ * oversampling rate, the
+ * gyro type and the A/D calibration values.
+ * The angle is continuous, that is it will continue from 360->361 degrees. This
+ * allows algorithms that wouldn't
+ * want to see a discontinuity in the gyro output as it sweeps from 360 to 0 on
+ * the second time around.
+ *
+ * @return the current heading of the robot in degrees. This heading is based on
+ * integration
+ * of the returned rate from the gyro.
+ */
+float ADXRS450_Gyro::GetAngle() const {
+ return (float)(m_spi.GetAccumulatorValue() * kDegreePerSecondPerLSB *
+ kSamplePeriod);
+}
+
+/**
+ * Return the rate of rotation of the gyro
+ *
+ * The rate is based on the most recent reading of the gyro analog value
+ *
+ * @return the current rate in degrees per second
+ */
+double ADXRS450_Gyro::GetRate() const {
+ return (double)m_spi.GetAccumulatorLastValue() * kDegreePerSecondPerLSB;
+}
+
+std::string ADXRS450_Gyro::GetSmartDashboardType() const {
+ return "ADXRS450_Gyro";
+}
diff --git a/wpilibc/Athena/src/AnalogAccelerometer.cpp b/wpilibc/Athena/src/AnalogAccelerometer.cpp
new file mode 100644
index 0000000..5cf41d9
--- /dev/null
+++ b/wpilibc/Athena/src/AnalogAccelerometer.cpp
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogAccelerometer.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Common function for initializing the accelerometer.
+ */
+void AnalogAccelerometer::InitAccelerometer() {
+ HALReport(HALUsageReporting::kResourceType_Accelerometer,
+ m_analogInput->GetChannel());
+ LiveWindow::GetInstance()->AddSensor("Accelerometer",
+ m_analogInput->GetChannel(), this);
+}
+
+/**
+ * Create a new instance of an accelerometer.
+ * The constructor allocates desired analog input.
+ * @param channel The channel number for the analog input the accelerometer is
+ * connected to
+ */
+AnalogAccelerometer::AnalogAccelerometer(int32_t channel) {
+ m_analogInput = std::make_shared<AnalogInput>(channel);
+ InitAccelerometer();
+}
+
+/**
+ * Create a new instance of Accelerometer from an existing AnalogInput.
+ * Make a new instance of accelerometer given an AnalogInput. This is
+ * particularly useful if the port is going to be read as an analog channel as
+ * well as through the Accelerometer class.
+ * @param channel The existing AnalogInput object for the analog input the
+ * accelerometer is connected to
+ */
+AnalogAccelerometer::AnalogAccelerometer(AnalogInput *channel)
+ : m_analogInput(channel, NullDeleter<AnalogInput>()) {
+ if (channel == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitAccelerometer();
+ }
+}
+
+/**
+ * Create a new instance of Accelerometer from an existing AnalogInput.
+ * Make a new instance of accelerometer given an AnalogInput. This is
+ * particularly useful if the port is going to be read as an analog channel as
+ * well as through the Accelerometer class.
+ * @param channel The existing AnalogInput object for the analog input the
+ * accelerometer is connected to
+ */
+AnalogAccelerometer::AnalogAccelerometer(std::shared_ptr<AnalogInput> channel)
+ : m_analogInput(channel) {
+ if (channel == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitAccelerometer();
+ }
+}
+
+/**
+ * Return the acceleration in Gs.
+ *
+ * The acceleration is returned units of Gs.
+ *
+ * @return The current acceleration of the sensor in Gs.
+ */
+float AnalogAccelerometer::GetAcceleration() const {
+ return (m_analogInput->GetAverageVoltage() - m_zeroGVoltage) / m_voltsPerG;
+}
+
+/**
+ * Set the accelerometer sensitivity.
+ *
+ * This sets the sensitivity of the accelerometer used for calculating the
+ * acceleration.
+ * The sensitivity varies by accelerometer model. There are constants defined
+ * for various models.
+ *
+ * @param sensitivity The sensitivity of accelerometer in Volts per G.
+ */
+void AnalogAccelerometer::SetSensitivity(float sensitivity) {
+ m_voltsPerG = sensitivity;
+}
+
+/**
+ * Set the voltage that corresponds to 0 G.
+ *
+ * The zero G voltage varies by accelerometer model. There are constants defined
+ * for various models.
+ *
+ * @param zero The zero G voltage.
+ */
+void AnalogAccelerometer::SetZero(float zero) { m_zeroGVoltage = zero; }
+
+/**
+ * Get the Acceleration for the PID Source parent.
+ *
+ * @return The current acceleration in Gs.
+ */
+double AnalogAccelerometer::PIDGet() { return GetAcceleration(); }
+
+void AnalogAccelerometer::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetAcceleration());
+ }
+}
+
+void AnalogAccelerometer::StartLiveWindowMode() {}
+
+void AnalogAccelerometer::StopLiveWindowMode() {}
+
+std::string AnalogAccelerometer::GetSmartDashboardType() const {
+ return "Accelerometer";
+}
+
+void AnalogAccelerometer::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> AnalogAccelerometer::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/AnalogGyro.cpp b/wpilibc/Athena/src/AnalogGyro.cpp
new file mode 100644
index 0000000..54093ec
--- /dev/null
+++ b/wpilibc/Athena/src/AnalogGyro.cpp
@@ -0,0 +1,255 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogGyro.h"
+#include "AnalogInput.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+#include <climits>
+const uint32_t AnalogGyro::kOversampleBits;
+const uint32_t AnalogGyro::kAverageBits;
+constexpr float AnalogGyro::kSamplesPerSecond;
+constexpr float AnalogGyro::kCalibrationSampleTime;
+constexpr float AnalogGyro::kDefaultVoltsPerDegreePerSecond;
+
+/**
+ * Gyro constructor using the Analog Input channel number.
+ *
+ * @param channel The analog channel the gyro is connected to. Gyros
+ can only be used on on-board Analog Inputs 0-1.
+ */
+AnalogGyro::AnalogGyro(int32_t channel) :
+ AnalogGyro(std::make_shared<AnalogInput>(channel)) {}
+
+/**
+ * Gyro constructor with a precreated AnalogInput object.
+ * Use this constructor when the analog channel needs to be shared.
+ * This object will not clean up the AnalogInput object when using this
+ * constructor.
+ * Gyros can only be used on on-board channels 0-1.
+ * @param channel A pointer to the AnalogInput object that the gyro is connected
+ * to.
+ */
+AnalogGyro::AnalogGyro(AnalogInput *channel)
+ : AnalogGyro(
+ std::shared_ptr<AnalogInput>(channel, NullDeleter<AnalogInput>())) {}
+
+/**
+ * Gyro constructor with a precreated AnalogInput object.
+ * Use this constructor when the analog channel needs to be shared.
+ * This object will not clean up the AnalogInput object when using this
+ * constructor
+ * @param channel A pointer to the AnalogInput object that the gyro is
+ * connected to.
+ */
+AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
+ : m_analog(channel) {
+ if (channel == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitGyro();
+ Calibrate();
+ }
+}
+
+/**
+ * Gyro constructor using the Analog Input channel number with parameters for
+ * presetting the center and offset values. Bypasses calibration.
+ *
+ * @param channel The analog channel the gyro is connected to. Gyros
+ * can only be used on on-board Analog Inputs 0-1.
+ * @param center Preset uncalibrated value to use as the accumulator center value.
+ * @param offset Preset uncalibrated value to use as the gyro offset.
+ */
+AnalogGyro::AnalogGyro(int32_t channel, uint32_t center, float offset) {
+ m_analog = std::make_shared<AnalogInput>(channel);
+ InitGyro();
+ m_center = center;
+ m_offset = offset;
+ m_analog->SetAccumulatorCenter(m_center);
+ m_analog->ResetAccumulator();
+}
+
+/**
+ * Gyro constructor with a precreated AnalogInput object and calibrated parameters.
+ * Use this constructor when the analog channel needs to be shared.
+ * This object will not clean up the AnalogInput object when using this
+ * constructor
+ * @param channel A pointer to the AnalogInput object that the gyro is
+ * connected to.
+ */
+AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel, uint32_t center, float offset) : m_analog(channel) {
+ if (channel == nullptr) {
+ wpi_setWPIError(NullParameter);
+ } else {
+ InitGyro();
+ m_center = center;
+ m_offset = offset;
+ m_analog->SetAccumulatorCenter(m_center);
+ m_analog->ResetAccumulator();
+ }
+}
+
+/**
+ * Reset the gyro.
+ * Resets the gyro to a heading of zero. This can be used if there is
+ * significant
+ * drift in the gyro and it needs to be recalibrated after it has been running.
+ */
+void AnalogGyro::Reset() {
+ if (StatusIsFatal()) return;
+ m_analog->ResetAccumulator();
+}
+
+/**
+ * Initialize the gyro. Calibration is handled by Calibrate().
+ */
+void AnalogGyro::InitGyro() {
+ if (StatusIsFatal()) return;
+
+ if (!m_analog->IsAccumulatorChannel()) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ " channel (must be accumulator channel)");
+ m_analog = nullptr;
+ return;
+ }
+
+ m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
+ m_analog->SetAverageBits(kAverageBits);
+ m_analog->SetOversampleBits(kOversampleBits);
+ float sampleRate =
+ kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
+ m_analog->SetSampleRate(sampleRate);
+ Wait(0.1);
+
+ SetDeadband(0.0f);
+
+ SetPIDSourceType(PIDSourceType::kDisplacement);
+
+ HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
+ LiveWindow::GetInstance()->AddSensor("Gyro", m_analog->GetChannel(), this);
+}
+
+/**
+ * {@inheritDoc}
+ */
+void AnalogGyro::Calibrate() {
+ if (StatusIsFatal()) return;
+
+ m_analog->InitAccumulator();
+
+ Wait(kCalibrationSampleTime);
+
+ int64_t value;
+ uint32_t count;
+ m_analog->GetAccumulatorOutput(value, count);
+
+ m_center = (uint32_t)((float)value / (float)count + .5);
+
+ m_offset = ((float)value / (float)count) - (float)m_center;
+ m_analog->SetAccumulatorCenter(m_center);
+ m_analog->ResetAccumulator();
+}
+
+/**
+ * Return the actual angle in degrees that the robot is currently facing.
+ *
+ * The angle is based on the current accumulator value corrected by the
+ * oversampling rate, the
+ * gyro type and the A/D calibration values.
+ * The angle is continuous, that is it will continue from 360->361 degrees. This
+ * allows algorithms that wouldn't
+ * want to see a discontinuity in the gyro output as it sweeps from 360 to 0 on
+ * the second time around.
+ *
+ * @return the current heading of the robot in degrees. This heading is based on
+ * integration
+ * of the returned rate from the gyro.
+ */
+float AnalogGyro::GetAngle() const {
+ if (StatusIsFatal()) return 0.f;
+
+ int64_t rawValue;
+ uint32_t count;
+ m_analog->GetAccumulatorOutput(rawValue, count);
+
+ int64_t value = rawValue - (int64_t)((float)count * m_offset);
+
+ double scaledValue = value * 1e-9 * (double)m_analog->GetLSBWeight() *
+ (double)(1 << m_analog->GetAverageBits()) /
+ (m_analog->GetSampleRate() * m_voltsPerDegreePerSecond);
+
+ return (float)scaledValue;
+}
+
+/**
+ * Return the rate of rotation of the gyro
+ *
+ * The rate is based on the most recent reading of the gyro analog value
+ *
+ * @return the current rate in degrees per second
+ */
+double AnalogGyro::GetRate() const {
+ if (StatusIsFatal()) return 0.0;
+
+ return (m_analog->GetAverageValue() - ((double)m_center + m_offset)) * 1e-9 *
+ m_analog->GetLSBWeight() /
+ ((1 << m_analog->GetOversampleBits()) * m_voltsPerDegreePerSecond);
+}
+
+/**
+ * Return the gyro offset value. If run after calibration,
+ * the offset value can be used as a preset later.
+ *
+ * @return the current offset value
+ */
+float AnalogGyro::GetOffset() const {
+ return m_offset;
+}
+
+/**
+ * Return the gyro center value. If run after calibration,
+ * the center value can be used as a preset later.
+ *
+ * @return the current center value
+ */
+uint32_t AnalogGyro::GetCenter() const {
+ return m_center;
+}
+
+/**
+ * Set the gyro sensitivity.
+ * This takes the number of volts/degree/second sensitivity of the gyro and uses
+ * it in subsequent
+ * calculations to allow the code to work with multiple gyros. This value is
+ * typically found in
+ * the gyro datasheet.
+ *
+ * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second
+ */
+void AnalogGyro::SetSensitivity(float voltsPerDegreePerSecond) {
+ m_voltsPerDegreePerSecond = voltsPerDegreePerSecond;
+}
+
+/**
+ * Set the size of the neutral zone. Any voltage from the gyro less than this
+ * amount from the center is considered stationary. Setting a deadband will
+ * decrease the amount of drift when the gyro isn't rotating, but will make it
+ * less accurate.
+ *
+ * @param volts The size of the deadband in volts
+ */
+void AnalogGyro::SetDeadband(float volts) {
+ if (StatusIsFatal()) return;
+
+ int32_t deadband = volts * 1e9 / m_analog->GetLSBWeight() *
+ (1 << m_analog->GetOversampleBits());
+ m_analog->SetAccumulatorDeadband(deadband);
+}
+
+std::string AnalogGyro::GetSmartDashboardType() const { return "AnalogGyro"; }
diff --git a/wpilibc/Athena/src/AnalogInput.cpp b/wpilibc/Athena/src/AnalogInput.cpp
new file mode 100644
index 0000000..dc8e872
--- /dev/null
+++ b/wpilibc/Athena/src/AnalogInput.cpp
@@ -0,0 +1,426 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogInput.h"
+#include "Resource.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+#include "HAL/Port.h"
+
+#include <sstream>
+
+static std::unique_ptr<Resource> inputs;
+
+const uint8_t AnalogInput::kAccumulatorModuleNumber;
+const uint32_t AnalogInput::kAccumulatorNumChannels;
+const uint32_t AnalogInput::kAccumulatorChannels[] = {0, 1};
+
+/**
+ * Construct an analog input.
+ *
+ * @param channel The channel number on the roboRIO to represent. 0-3 are
+ * on-board 4-7 are on the MXP port.
+ */
+AnalogInput::AnalogInput(uint32_t channel) {
+ std::stringstream buf;
+ buf << "Analog Input " << channel;
+ Resource::CreateResourceObject(inputs, kAnalogInputs);
+
+ if (!checkAnalogInputChannel(channel)) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ return;
+ }
+
+ if (inputs->Allocate(channel, buf.str()) ==
+ std::numeric_limits<uint32_t>::max()) {
+ CloneError(*inputs);
+ return;
+ }
+
+ m_channel = channel;
+
+ void* port = getPort(channel);
+ int32_t status = 0;
+ m_port = initializeAnalogInputPort(port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ freePort(port);
+
+ LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this);
+ HALReport(HALUsageReporting::kResourceType_AnalogChannel, channel);
+}
+
+/**
+ * Channel destructor.
+ */
+AnalogInput::~AnalogInput() {
+ freeAnalogInputPort(m_port);
+ inputs->Free(m_channel);
+}
+
+/**
+ * Get a sample straight from this channel.
+ * The sample is a 12-bit value representing the 0V to 5V range of the A/D
+ * converter in the module.
+ * The units are in A/D converter codes. Use GetVoltage() to get the analog
+ * value in calibrated units.
+ * @return A sample straight from this channel.
+ */
+int16_t AnalogInput::GetValue() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int16_t value = getAnalogValue(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+}
+
+/**
+ * Get a sample from the output of the oversample and average engine for this
+ * channel.
+ * The sample is 12-bit + the bits configured in SetOversampleBits().
+ * The value configured in SetAverageBits() will cause this value to be averaged
+ * 2**bits number of samples.
+ * This is not a sliding window. The sample will not change until
+ * 2**(OversampleBits + AverageBits) samples
+ * have been acquired from the module on this channel.
+ * Use GetAverageVoltage() to get the analog value in calibrated units.
+ * @return A sample from the oversample and average engine for this channel.
+ */
+int32_t AnalogInput::GetAverageValue() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int32_t value = getAnalogAverageValue(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+}
+
+/**
+ * Get a scaled sample straight from this channel.
+ * The value is scaled to units of Volts using the calibrated scaling data from
+ * GetLSBWeight() and GetOffset().
+ * @return A scaled sample straight from this channel.
+ */
+float AnalogInput::GetVoltage() const {
+ if (StatusIsFatal()) return 0.0f;
+ int32_t status = 0;
+ float voltage = getAnalogVoltage(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return voltage;
+}
+
+/**
+ * Get a scaled sample from the output of the oversample and average engine for
+ * this channel.
+ * The value is scaled to units of Volts using the calibrated scaling data from
+ * GetLSBWeight() and GetOffset().
+ * Using oversampling will cause this value to be higher resolution, but it will
+ * update more slowly.
+ * Using averaging will cause this value to be more stable, but it will update
+ * more slowly.
+ * @return A scaled sample from the output of the oversample and average engine
+ * for this channel.
+ */
+float AnalogInput::GetAverageVoltage() const {
+ if (StatusIsFatal()) return 0.0f;
+ int32_t status = 0;
+ float voltage = getAnalogAverageVoltage(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return voltage;
+}
+
+/**
+ * Get the factory scaling least significant bit weight constant.
+ *
+ * Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+ *
+ * @return Least significant bit weight.
+ */
+uint32_t AnalogInput::GetLSBWeight() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int32_t lsbWeight = getAnalogLSBWeight(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return lsbWeight;
+}
+
+/**
+ * Get the factory scaling offset constant.
+ *
+ * Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
+ *
+ * @return Offset constant.
+ */
+int32_t AnalogInput::GetOffset() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int32_t offset = getAnalogOffset(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return offset;
+}
+
+/**
+ * Get the channel number.
+ * @return The channel number.
+ */
+uint32_t AnalogInput::GetChannel() const {
+ if (StatusIsFatal()) return 0;
+ return m_channel;
+}
+
+/**
+ * Set the number of averaging bits.
+ * This sets the number of averaging bits. The actual number of averaged samples
+ * is 2^bits.
+ * Use averaging to improve the stability of your measurement at the expense of
+ * sampling rate.
+ * The averaging is done automatically in the FPGA.
+ *
+ * @param bits Number of bits of averaging.
+ */
+void AnalogInput::SetAverageBits(uint32_t bits) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setAnalogAverageBits(m_port, bits, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Get the number of averaging bits previously configured.
+ * This gets the number of averaging bits from the FPGA. The actual number of
+ * averaged samples is 2^bits.
+ * The averaging is done automatically in the FPGA.
+ *
+ * @return Number of bits of averaging previously configured.
+ */
+uint32_t AnalogInput::GetAverageBits() const {
+ int32_t status = 0;
+ int32_t averageBits = getAnalogAverageBits(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return averageBits;
+}
+
+/**
+ * Set the number of oversample bits.
+ * This sets the number of oversample bits. The actual number of oversampled
+ * values is 2^bits.
+ * Use oversampling to improve the resolution of your measurements at the
+ * expense of sampling rate.
+ * The oversampling is done automatically in the FPGA.
+ *
+ * @param bits Number of bits of oversampling.
+ */
+void AnalogInput::SetOversampleBits(uint32_t bits) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setAnalogOversampleBits(m_port, bits, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Get the number of oversample bits previously configured.
+ * This gets the number of oversample bits from the FPGA. The actual number of
+ * oversampled values is
+ * 2^bits. The oversampling is done automatically in the FPGA.
+ *
+ * @return Number of bits of oversampling previously configured.
+ */
+uint32_t AnalogInput::GetOversampleBits() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int32_t oversampleBits = getAnalogOversampleBits(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return oversampleBits;
+}
+
+/**
+ * Is the channel attached to an accumulator.
+ *
+ * @return The analog input is attached to an accumulator.
+ */
+bool AnalogInput::IsAccumulatorChannel() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool isAccum = isAccumulatorChannel(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return isAccum;
+}
+
+/**
+ * Initialize the accumulator.
+ */
+void AnalogInput::InitAccumulator() {
+ if (StatusIsFatal()) return;
+ m_accumulatorOffset = 0;
+ int32_t status = 0;
+ initAccumulator(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set an initial value for the accumulator.
+ *
+ * This will be added to all values returned to the user.
+ * @param initialValue The value that the accumulator should start from when
+ * reset.
+ */
+void AnalogInput::SetAccumulatorInitialValue(int64_t initialValue) {
+ if (StatusIsFatal()) return;
+ m_accumulatorOffset = initialValue;
+}
+
+/**
+ * Resets the accumulator to the initial value.
+ */
+void AnalogInput::ResetAccumulator() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ resetAccumulator(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ if (!StatusIsFatal()) {
+ // Wait until the next sample, so the next call to GetAccumulator*()
+ // won't have old values.
+ const float sampleTime = 1.0f / GetSampleRate();
+ const float overSamples = 1 << GetOversampleBits();
+ const float averageSamples = 1 << GetAverageBits();
+ Wait(sampleTime * overSamples * averageSamples);
+ }
+}
+
+/**
+ * Set the center value of the accumulator.
+ *
+ * The center value is subtracted from each A/D value before it is added to the
+ * accumulator. This
+ * is used for the center value of devices like gyros and accelerometers to
+ * take the device offset into account when integrating.
+ *
+ * This center value is based on the output of the oversampled and averaged
+ * source from the accumulator
+ * channel. Because of this, any non-zero oversample bits will affect the size
+ * of the value for this field.
+ */
+void AnalogInput::SetAccumulatorCenter(int32_t center) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setAccumulatorCenter(m_port, center, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the accumulator's deadband.
+ * @param
+ */
+void AnalogInput::SetAccumulatorDeadband(int32_t deadband) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setAccumulatorDeadband(m_port, deadband, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Read the accumulated value.
+ *
+ * Read the value that has been accumulating.
+ * The accumulator is attached after the oversample and average engine.
+ *
+ * @return The 64-bit value accumulated since the last Reset().
+ */
+int64_t AnalogInput::GetAccumulatorValue() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int64_t value = getAccumulatorValue(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value + m_accumulatorOffset;
+}
+
+/**
+ * Read the number of accumulated values.
+ *
+ * Read the count of the accumulated values since the accumulator was last
+ * Reset().
+ *
+ * @return The number of times samples from the channel were accumulated.
+ */
+uint32_t AnalogInput::GetAccumulatorCount() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ uint32_t count = getAccumulatorCount(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return count;
+}
+
+/**
+ * Read the accumulated value and the number of accumulated values atomically.
+ *
+ * This function reads the value and count from the FPGA atomically.
+ * This can be used for averaging.
+ *
+ * @param value Reference to the 64-bit accumulated output.
+ * @param count Reference to the number of accumulation cycles.
+ */
+void AnalogInput::GetAccumulatorOutput(int64_t &value, uint32_t &count) const {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ getAccumulatorOutput(m_port, &value, &count, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ value += m_accumulatorOffset;
+}
+
+/**
+ * Set the sample rate per channel for all analog channels.
+ * The maximum rate is 500kS/s divided by the number of channels in use.
+ * This is 62500 samples/s per channel.
+ * @param samplesPerSecond The number of samples per second.
+ */
+void AnalogInput::SetSampleRate(float samplesPerSecond) {
+ int32_t status = 0;
+ setAnalogSampleRate(samplesPerSecond, &status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Get the current sample rate for all channels
+ *
+ * @return Sample rate.
+ */
+float AnalogInput::GetSampleRate() {
+ int32_t status = 0;
+ float sampleRate = getAnalogSampleRate(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return sampleRate;
+}
+
+/**
+ * Get the Average value for the PID Source base object.
+ *
+ * @return The average voltage.
+ */
+double AnalogInput::PIDGet() {
+ if (StatusIsFatal()) return 0.0;
+ return GetAverageVoltage();
+}
+
+void AnalogInput::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetAverageVoltage());
+ }
+}
+
+void AnalogInput::StartLiveWindowMode() {}
+
+void AnalogInput::StopLiveWindowMode() {}
+
+std::string AnalogInput::GetSmartDashboardType() const {
+ return "Analog Input";
+}
+
+void AnalogInput::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> AnalogInput::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/AnalogOutput.cpp b/wpilibc/Athena/src/AnalogOutput.cpp
new file mode 100644
index 0000000..74a1f9c
--- /dev/null
+++ b/wpilibc/Athena/src/AnalogOutput.cpp
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogOutput.h"
+#include "Resource.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+#include "HAL/Port.h"
+
+#include <limits>
+#include <sstream>
+
+static std::unique_ptr<Resource> outputs;
+
+/**
+ * Construct an analog output on the given channel.
+ * All analog outputs are located on the MXP port.
+ * @param The channel number on the roboRIO to represent.
+ */
+AnalogOutput::AnalogOutput(uint32_t channel) {
+ Resource::CreateResourceObject(outputs, kAnalogOutputs);
+
+ std::stringstream buf;
+ buf << "analog input " << channel;
+
+ if (!checkAnalogOutputChannel(channel)) {
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ m_channel = std::numeric_limits<uint32_t>::max();
+ m_port = nullptr;
+ return;
+ }
+
+ if (outputs->Allocate(channel, buf.str()) ==
+ std::numeric_limits<uint32_t>::max()) {
+ CloneError(*outputs);
+ m_channel = std::numeric_limits<uint32_t>::max();
+ m_port = nullptr;
+ return;
+ }
+
+ m_channel = channel;
+
+ void* port = getPort(m_channel);
+ int32_t status = 0;
+ m_port = initializeAnalogOutputPort(port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ freePort(port);
+
+ LiveWindow::GetInstance()->AddActuator("AnalogOutput", m_channel, this);
+ HALReport(HALUsageReporting::kResourceType_AnalogOutput, m_channel);
+}
+
+/**
+ * Destructor. Frees analog output resource
+ */
+AnalogOutput::~AnalogOutput() {
+ freeAnalogOutputPort(m_port);
+ outputs->Free(m_channel);
+}
+
+/**
+ * Set the value of the analog output
+ *
+ * @param voltage The output value in Volts, from 0.0 to +5.0
+ */
+void AnalogOutput::SetVoltage(float voltage) {
+ int32_t status = 0;
+ setAnalogOutput(m_port, voltage, &status);
+
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Get the voltage of the analog output
+ *
+ * @return The value in Volts, from 0.0 to +5.0
+ */
+float AnalogOutput::GetVoltage() const {
+ int32_t status = 0;
+ float voltage = getAnalogOutput(m_port, &status);
+
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ return voltage;
+}
+
+void AnalogOutput::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetVoltage());
+ }
+}
+
+void AnalogOutput::StartLiveWindowMode() {}
+
+void AnalogOutput::StopLiveWindowMode() {}
+
+std::string AnalogOutput::GetSmartDashboardType() const {
+ return "Analog Output";
+}
+
+void AnalogOutput::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> AnalogOutput::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/AnalogPotentiometer.cpp b/wpilibc/Athena/src/AnalogPotentiometer.cpp
new file mode 100644
index 0000000..1372d84
--- /dev/null
+++ b/wpilibc/Athena/src/AnalogPotentiometer.cpp
@@ -0,0 +1,87 @@
+#include "AnalogPotentiometer.h"
+#include "ControllerPower.h"
+
+/**
+ * Construct an Analog Potentiometer object from a channel number.
+ * @param channel The channel number on the roboRIO to represent. 0-3 are
+ * on-board 4-7 are on the MXP port.
+ * @param fullRange The angular value (in desired units) representing the full
+ * 0-5V range of the input.
+ * @param offset The angular value (in desired units) representing the angular
+ * output at 0V.
+ */
+AnalogPotentiometer::AnalogPotentiometer(int channel, double fullRange,
+ double offset)
+ : m_analog_input(std::make_unique<AnalogInput>(channel)),
+ m_fullRange(fullRange),
+ m_offset(offset) {}
+
+/**
+ * Construct an Analog Potentiometer object from an existing Analog Input
+ * pointer.
+ * @param channel The existing Analog Input pointer
+ * @param fullRange The angular value (in desired units) representing the full
+ * 0-5V range of the input.
+ * @param offset The angular value (in desired units) representing the angular
+ * output at 0V.
+ */
+AnalogPotentiometer::AnalogPotentiometer(AnalogInput *input, double fullRange,
+ double offset)
+ : m_analog_input(input, NullDeleter<AnalogInput>()),
+ m_fullRange(fullRange),
+ m_offset(offset) {}
+
+/**
+ * Construct an Analog Potentiometer object from an existing Analog Input
+ * pointer.
+ * @param channel The existing Analog Input pointer
+ * @param fullRange The angular value (in desired units) representing the full
+ * 0-5V range of the input.
+ * @param offset The angular value (in desired units) representing the angular
+ * output at 0V.
+ */
+AnalogPotentiometer::AnalogPotentiometer(std::shared_ptr<AnalogInput> input,
+ double fullRange, double offset)
+ : m_analog_input(input), m_fullRange(fullRange), m_offset(offset) {}
+
+/**
+ * Get the current reading of the potentiometer.
+ *
+ * @return The current position of the potentiometer (in the units used for
+ * fullRaneg and offset).
+ */
+double AnalogPotentiometer::Get() const {
+ return (m_analog_input->GetVoltage() / ControllerPower::GetVoltage5V()) *
+ m_fullRange +
+ m_offset;
+}
+
+/**
+ * Implement the PIDSource interface.
+ *
+ * @return The current reading.
+ */
+double AnalogPotentiometer::PIDGet() { return Get(); }
+
+/**
+ * @return the Smart Dashboard Type
+ */
+std::string AnalogPotentiometer::GetSmartDashboardType() const {
+ return "Analog Input";
+}
+
+/**
+ * Live Window code, only does anything if live window is activated.
+ */
+void AnalogPotentiometer::InitTable(std::shared_ptr<ITable> subtable) {
+ m_table = subtable;
+ UpdateTable();
+}
+
+void AnalogPotentiometer::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", Get());
+ }
+}
+
+std::shared_ptr<ITable> AnalogPotentiometer::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/AnalogTrigger.cpp b/wpilibc/Athena/src/AnalogTrigger.cpp
new file mode 100644
index 0000000..b49d6a0
--- /dev/null
+++ b/wpilibc/Athena/src/AnalogTrigger.cpp
@@ -0,0 +1,161 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogTrigger.h"
+
+#include "AnalogInput.h"
+#include "Resource.h"
+#include "WPIErrors.h"
+#include "HAL/Port.h"
+
+#include <memory>
+
+/**
+ * Constructor for an analog trigger given a channel number.
+ *
+ * @param channel The channel number on the roboRIO to represent. 0-3 are
+ * on-board 4-7 are on the MXP port.
+ */
+AnalogTrigger::AnalogTrigger(int32_t channel) {
+ void* port = getPort(channel);
+ int32_t status = 0;
+ uint32_t index = 0;
+ m_trigger = initializeAnalogTrigger(port, &index, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ freePort(port);
+ m_index = index;
+
+ HALReport(HALUsageReporting::kResourceType_AnalogTrigger, channel);
+}
+
+/**
+ * Construct an analog trigger given an analog input.
+ * This should be used in the case of sharing an analog channel between the
+ * trigger and an analog input object.
+ * @param channel The pointer to the existing AnalogInput object
+ */
+AnalogTrigger::AnalogTrigger(AnalogInput *input) :
+ AnalogTrigger(input->GetChannel()) {
+}
+
+AnalogTrigger::~AnalogTrigger() {
+ int32_t status = 0;
+ cleanAnalogTrigger(m_trigger, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the upper and lower limits of the analog trigger.
+ * The limits are given in ADC codes. If oversampling is used, the units must
+ * be scaled
+ * appropriately.
+ * @param lower The lower limit of the trigger in ADC codes (12-bit values).
+ * @param upper The upper limit of the trigger in ADC codes (12-bit values).
+ */
+void AnalogTrigger::SetLimitsRaw(int32_t lower, int32_t upper) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setAnalogTriggerLimitsRaw(m_trigger, lower, upper, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the upper and lower limits of the analog trigger.
+ * The limits are given as floating point voltage values.
+ * @param lower The lower limit of the trigger in Volts.
+ * @param upper The upper limit of the trigger in Volts.
+ */
+void AnalogTrigger::SetLimitsVoltage(float lower, float upper) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setAnalogTriggerLimitsVoltage(m_trigger, lower, upper, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Configure the analog trigger to use the averaged vs. raw values.
+ * If the value is true, then the averaged value is selected for the analog
+ * trigger, otherwise
+ * the immediate value is used.
+ * @param useAveragedValue If true, use the Averaged value, otherwise use the
+ * instantaneous reading
+ */
+void AnalogTrigger::SetAveraged(bool useAveragedValue) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setAnalogTriggerAveraged(m_trigger, useAveragedValue, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Configure the analog trigger to use a filtered value.
+ * The analog trigger will operate with a 3 point average rejection filter. This
+ * is designed to
+ * help with 360 degree pot applications for the period where the pot crosses
+ * through zero.
+ * @param useFilteredValue If true, use the 3 point rejection filter, otherwise
+ * use the unfiltered value
+ */
+void AnalogTrigger::SetFiltered(bool useFilteredValue) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setAnalogTriggerFiltered(m_trigger, useFilteredValue, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Return the index of the analog trigger.
+ * This is the FPGA index of this analog trigger instance.
+ * @return The index of the analog trigger.
+ */
+uint32_t AnalogTrigger::GetIndex() const {
+ if (StatusIsFatal()) return std::numeric_limits<uint32_t>::max();
+ return m_index;
+}
+
+/**
+ * Return the InWindow output of the analog trigger.
+ * True if the analog input is between the upper and lower limits.
+ * @return True if the analog input is between the upper and lower limits.
+ */
+bool AnalogTrigger::GetInWindow() {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool result = getAnalogTriggerInWindow(m_trigger, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return result;
+}
+
+/**
+ * Return the TriggerState output of the analog trigger.
+ * True if above upper limit.
+ * False if below lower limit.
+ * If in Hysteresis, maintain previous state.
+ * @return True if above upper limit. False if below lower limit. If in
+ * Hysteresis, maintain previous state.
+ */
+bool AnalogTrigger::GetTriggerState() {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool result = getAnalogTriggerTriggerState(m_trigger, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return result;
+}
+
+/**
+ * Creates an AnalogTriggerOutput object.
+ * Gets an output object that can be used for routing.
+ * Caller is responsible for deleting the AnalogTriggerOutput object.
+ * @param type An enum of the type of output object to create.
+ * @return A pointer to a new AnalogTriggerOutput object.
+ */
+std::shared_ptr<AnalogTriggerOutput> AnalogTrigger::CreateOutput(
+ AnalogTriggerType type) const {
+ if (StatusIsFatal()) return nullptr;
+ return std::shared_ptr<AnalogTriggerOutput>(
+ new AnalogTriggerOutput(*this, type), NullDeleter<AnalogTriggerOutput>());
+}
diff --git a/wpilibc/Athena/src/AnalogTriggerOutput.cpp b/wpilibc/Athena/src/AnalogTriggerOutput.cpp
new file mode 100644
index 0000000..8254275
--- /dev/null
+++ b/wpilibc/Athena/src/AnalogTriggerOutput.cpp
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogTriggerOutput.h"
+#include "AnalogTrigger.h"
+#include "WPIErrors.h"
+
+/**
+ * Create an object that represents one of the four outputs from an analog
+ * trigger.
+ *
+ * Because this class derives from DigitalSource, it can be passed into routing
+ * functions
+ * for Counter, Encoder, etc.
+ *
+ * @param trigger A pointer to the trigger for which this is an output.
+ * @param outputType An enum that specifies the output on the trigger to
+ * represent.
+ */
+AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger &trigger,
+ AnalogTriggerType outputType)
+ : m_trigger(trigger), m_outputType(outputType) {
+ HALReport(HALUsageReporting::kResourceType_AnalogTriggerOutput,
+ trigger.GetIndex(), outputType);
+}
+
+AnalogTriggerOutput::~AnalogTriggerOutput() {
+ if (m_interrupt != nullptr) {
+ int32_t status = 0;
+ cleanInterrupts(m_interrupt, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ m_interrupt = nullptr;
+ m_interrupts->Free(m_interruptIndex);
+ }
+}
+
+/**
+ * Get the state of the analog trigger output.
+ * @return The state of the analog trigger output.
+ */
+bool AnalogTriggerOutput::Get() const {
+ int32_t status = 0;
+ bool result =
+ getAnalogTriggerOutput(m_trigger.m_trigger, m_outputType, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return result;
+}
+
+/**
+ * @return The value to be written to the channel field of a routing mux.
+ */
+uint32_t AnalogTriggerOutput::GetChannelForRouting() const {
+ return (m_trigger.m_index << 2) + m_outputType;
+}
+
+/**
+ * @return The value to be written to the module field of a routing mux.
+ */
+uint32_t AnalogTriggerOutput::GetModuleForRouting() const { return 0; }
+
+/**
+ * @return The value to be written to the module field of a routing mux.
+ */
+bool AnalogTriggerOutput::GetAnalogTriggerForRouting() const { return true; }
diff --git a/wpilibc/Athena/src/BuiltInAccelerometer.cpp b/wpilibc/Athena/src/BuiltInAccelerometer.cpp
new file mode 100644
index 0000000..a504524
--- /dev/null
+++ b/wpilibc/Athena/src/BuiltInAccelerometer.cpp
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "BuiltInAccelerometer.h"
+#include "HAL/HAL.hpp"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Constructor.
+ * @param range The range the accelerometer will measure
+ */
+BuiltInAccelerometer::BuiltInAccelerometer(Range range) {
+ SetRange(range);
+
+ HALReport(HALUsageReporting::kResourceType_Accelerometer, 0, 0,
+ "Built-in accelerometer");
+ LiveWindow::GetInstance()->AddSensor((std::string) "BuiltInAccel", 0, this);
+}
+
+/** {@inheritdoc} */
+void BuiltInAccelerometer::SetRange(Range range) {
+ if (range == kRange_16G) {
+ wpi_setWPIErrorWithContext(
+ ParameterOutOfRange, "16G range not supported (use k2G, k4G, or k8G)");
+ }
+
+ setAccelerometerActive(false);
+ setAccelerometerRange((AccelerometerRange)range);
+ setAccelerometerActive(true);
+}
+
+/**
+ * @return The acceleration of the RoboRIO along the X axis in g-forces
+ */
+double BuiltInAccelerometer::GetX() { return getAccelerometerX(); }
+
+/**
+ * @return The acceleration of the RoboRIO along the Y axis in g-forces
+ */
+double BuiltInAccelerometer::GetY() { return getAccelerometerY(); }
+
+/**
+ * @return The acceleration of the RoboRIO along the Z axis in g-forces
+ */
+double BuiltInAccelerometer::GetZ() { return getAccelerometerZ(); }
+
+std::string BuiltInAccelerometer::GetSmartDashboardType() const {
+ return "3AxisAccelerometer";
+}
+
+void BuiltInAccelerometer::InitTable(std::shared_ptr<ITable> subtable) {
+ m_table = subtable;
+ UpdateTable();
+}
+
+void BuiltInAccelerometer::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("X", GetX());
+ m_table->PutNumber("Y", GetY());
+ m_table->PutNumber("Z", GetZ());
+ }
+}
+
+std::shared_ptr<ITable> BuiltInAccelerometer::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/CANJaguar.cpp b/wpilibc/Athena/src/CANJaguar.cpp
new file mode 100644
index 0000000..fc71e84
--- /dev/null
+++ b/wpilibc/Athena/src/CANJaguar.cpp
@@ -0,0 +1,2013 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2009. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "CANJaguar.h"
+#include "Timer.h"
+#define tNIRIO_i32 int
+#include "NetworkCommunication/CANSessionMux.h"
+#include "WPIErrors.h"
+#include <cstdio>
+#include <cassert>
+#include "LiveWindow/LiveWindow.h"
+
+/* we are on ARM-LE now, not Freescale so no need to swap */
+#define swap16(x) (x)
+#define swap32(x) (x)
+
+/* Compare floats for equality as fixed point numbers */
+#define FXP8_EQ(a, b) ((int16_t)((a)*256.0) == (int16_t)((b)*256.0))
+#define FXP16_EQ(a, b) ((int32_t)((a)*65536.0) == (int32_t)((b)*65536.0))
+
+const int32_t CANJaguar::kControllerRate;
+constexpr double CANJaguar::kApproxBusVoltage;
+
+static const int32_t kSendMessagePeriod = 20;
+static const uint32_t kFullMessageIDMask =
+ (CAN_MSGID_API_M | CAN_MSGID_MFR_M | CAN_MSGID_DTYPE_M);
+
+static const int32_t kReceiveStatusAttempts = 50;
+
+static std::unique_ptr<Resource> allocated;
+
+static int32_t sendMessageHelper(uint32_t messageID, const uint8_t *data,
+ uint8_t dataSize, int32_t period) {
+ static const uint32_t kTrustedMessages[] = {
+ LM_API_VOLT_T_EN, LM_API_VOLT_T_SET, LM_API_SPD_T_EN, LM_API_SPD_T_SET,
+ LM_API_VCOMP_T_EN, LM_API_VCOMP_T_SET, LM_API_POS_T_EN, LM_API_POS_T_SET,
+ LM_API_ICTRL_T_EN, LM_API_ICTRL_T_SET};
+
+ int32_t status = 0;
+
+ for (auto& kTrustedMessage : kTrustedMessages) {
+ if ((kFullMessageIDMask & messageID) == kTrustedMessage) {
+ uint8_t dataBuffer[8];
+ dataBuffer[0] = 0;
+ dataBuffer[1] = 0;
+
+ // Make sure the data will still fit after adjusting for the token.
+ assert(dataSize <= 6);
+
+ for (uint8_t j = 0; j < dataSize; j++) {
+ dataBuffer[j + 2] = data[j];
+ }
+
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ messageID, dataBuffer, dataSize + 2, period, &status);
+
+ return status;
+ }
+ }
+
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(messageID, data, dataSize,
+ period, &status);
+
+ return status;
+}
+
+/**
+ * Common initialization code called by all constructors.
+ */
+void CANJaguar::InitCANJaguar() {
+ m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+
+ bool receivedFirmwareVersion = false;
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ // Request firmware and hardware version only once
+ requestMessage(CAN_IS_FRAME_REMOTE | CAN_MSGID_API_FIRMVER);
+ requestMessage(LM_API_HWVER);
+
+ // Wait until we've gotten all of the status data at least once.
+ for (int i = 0; i < kReceiveStatusAttempts; i++) {
+ Wait(0.001);
+
+ setupPeriodicStatus();
+ updatePeriodicStatus();
+
+ if (!receivedFirmwareVersion &&
+ getMessage(CAN_MSGID_API_FIRMVER, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ m_firmwareVersion = unpackint32_t(dataBuffer);
+ receivedFirmwareVersion = true;
+ }
+
+ if (m_receivedStatusMessage0 && m_receivedStatusMessage1 &&
+ m_receivedStatusMessage2 && receivedFirmwareVersion) {
+ break;
+ }
+ }
+
+ if (!m_receivedStatusMessage0 || !m_receivedStatusMessage1 ||
+ !m_receivedStatusMessage2 || !receivedFirmwareVersion) {
+ wpi_setWPIErrorWithContext(JaguarMessageNotFound, "Status data not found");
+ }
+
+ if (getMessage(LM_API_HWVER, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
+ m_hardwareVersion = dataBuffer[0];
+
+ if (m_deviceNumber < 1 || m_deviceNumber > 63) {
+ std::stringstream buf;
+ buf << "device number \"" << m_deviceNumber
+ << "\" must be between 1 and 63";
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, buf.str());
+ return;
+ }
+
+ if (StatusIsFatal()) return;
+
+ // 3330 was the first shipping RDK firmware version for the Jaguar
+ if (m_firmwareVersion >= 3330 || m_firmwareVersion < 108) {
+ std::stringstream buf;
+ if (m_firmwareVersion < 3330) {
+ buf << "Jag #" << m_deviceNumber << " firmware (" << m_firmwareVersion
+ << ") is too old (must be at least version 108 "
+ "of the FIRST approved firmware)";
+ } else {
+ buf << "Jag #" << m_deviceNumber << " firmware (" << m_firmwareVersion
+ << ") is not FIRST approved (must be at least "
+ "version 108 of the FIRST approved firmware)";
+ }
+ wpi_setWPIErrorWithContext(JaguarVersionError, buf.str());
+ return;
+ }
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ case kVoltage:
+ // No additional configuration required... start enabled.
+ EnableControl();
+ break;
+ default:
+ break;
+ }
+ HALReport(HALUsageReporting::kResourceType_CANJaguar, m_deviceNumber,
+ m_controlMode);
+ LiveWindow::GetInstance()->AddActuator("CANJaguar", m_deviceNumber, this);
+}
+
+/**
+ * Constructor for the CANJaguar device.<br>
+ * By default the device is configured in Percent mode.
+ * The control mode can be changed by calling one of the control modes listed
+ * below.
+ *
+ * @param deviceNumber The address of the Jaguar on the CAN bus.
+ * @see CANJaguar#SetCurrentMode(double, double, double)
+ * @see CANJaguar#SetCurrentMode(PotentiometerTag, double, double, double)
+ * @see CANJaguar#SetCurrentMode(EncoderTag, int, double, double, double)
+ * @see CANJaguar#SetCurrentMode(QuadEncoderTag, int, double, double, double)
+ * @see CANJaguar#SetPercentMode()
+ * @see CANJaguar#SetPercentMode(PotentiometerTag)
+ * @see CANJaguar#SetPercentMode(EncoderTag, int)
+ * @see CANJaguar#SetPercentMode(QuadEncoderTag, int)
+ * @see CANJaguar#SetPositionMode(PotentiometerTag, double, double, double)
+ * @see CANJaguar#SetPositionMode(QuadEncoderTag, int, double, double, double)
+ * @see CANJaguar#SetSpeedMode(EncoderTag, int, double, double, double)
+ * @see CANJaguar#SetSpeedMode(QuadEncoderTag, int, double, double, double)
+ * @see CANJaguar#SetVoltageMode()
+ * @see CANJaguar#SetVoltageMode(PotentiometerTag)
+ * @see CANJaguar#SetVoltageMode(EncoderTag, int)
+ * @see CANJaguar#SetVoltageMode(QuadEncoderTag, int)
+ */
+CANJaguar::CANJaguar(uint8_t deviceNumber)
+ : m_deviceNumber(deviceNumber) {
+ std::stringstream buf;
+ buf << "CANJaguar device number " << m_deviceNumber;
+ Resource::CreateResourceObject(allocated, 63);
+
+ if (allocated->Allocate(m_deviceNumber - 1, buf.str()) ==
+ std::numeric_limits<uint32_t>::max()) {
+ CloneError(*allocated);
+ return;
+ }
+
+ SetPercentMode();
+ InitCANJaguar();
+ ConfigMaxOutputVoltage(kApproxBusVoltage);
+}
+
+CANJaguar::~CANJaguar() {
+ allocated->Free(m_deviceNumber - 1);
+
+ int32_t status;
+
+ // Disable periodic setpoints
+ if (m_controlMode == kPercentVbus)
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ m_deviceNumber | LM_API_VOLT_T_SET, nullptr, 0,
+ CAN_SEND_PERIOD_STOP_REPEATING, &status);
+ else if (m_controlMode == kSpeed)
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ m_deviceNumber | LM_API_SPD_T_SET, nullptr, 0,
+ CAN_SEND_PERIOD_STOP_REPEATING, &status);
+ else if (m_controlMode == kPosition)
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ m_deviceNumber | LM_API_POS_T_SET, nullptr, 0,
+ CAN_SEND_PERIOD_STOP_REPEATING, &status);
+ else if (m_controlMode == kCurrent)
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ m_deviceNumber | LM_API_ICTRL_T_SET, nullptr, 0,
+ CAN_SEND_PERIOD_STOP_REPEATING, &status);
+ else if (m_controlMode == kVoltage)
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ m_deviceNumber | LM_API_VCOMP_T_SET, nullptr, 0,
+ CAN_SEND_PERIOD_STOP_REPEATING, &status);
+
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * @return The CAN ID passed in the constructor
+ */
+uint8_t CANJaguar::getDeviceNumber() const { return m_deviceNumber; }
+
+/**
+ * Sets the output set-point value.
+ *
+ * The scale and the units depend on the mode the Jaguar is in.<br>
+ * In percentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM
+ * Jaguar).<br>
+ * In voltage Mode, the outputValue is in volts. <br>
+ * In current Mode, the outputValue is in amps. <br>
+ * In speed Mode, the outputValue is in rotations/minute.<br>
+ * In position Mode, the outputValue is in rotations.
+ *
+ * @param outputValue The set-point to sent to the motor controller.
+ * @param syncGroup The update group to add this Set() to, pending
+ * UpdateSyncGroup(). If 0, update immediately.
+ */
+void CANJaguar::Set(float outputValue, uint8_t syncGroup) {
+ uint32_t messageID;
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ if (m_safetyHelper && !m_safetyHelper->IsAlive() && m_controlEnabled) {
+ EnableControl();
+ }
+
+ if (m_controlEnabled) {
+ switch (m_controlMode) {
+ case kPercentVbus: {
+ messageID = LM_API_VOLT_T_SET;
+ if (outputValue > 1.0) outputValue = 1.0;
+ if (outputValue < -1.0) outputValue = -1.0;
+ dataSize = packPercentage(dataBuffer,
+ (m_isInverted ? -outputValue : outputValue));
+ } break;
+ case kSpeed: {
+ messageID = LM_API_SPD_T_SET;
+ dataSize = packFXP16_16(dataBuffer,
+ (m_isInverted ? -outputValue : outputValue));
+ } break;
+ case kPosition: {
+ messageID = LM_API_POS_T_SET;
+ dataSize = packFXP16_16(dataBuffer, outputValue);
+ } break;
+ case kCurrent: {
+ messageID = LM_API_ICTRL_T_SET;
+ dataSize = packFXP8_8(dataBuffer, outputValue);
+ } break;
+ case kVoltage: {
+ messageID = LM_API_VCOMP_T_SET;
+ dataSize =
+ packFXP8_8(dataBuffer, (m_isInverted ? -outputValue : outputValue));
+ } break;
+ default:
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "The Jaguar only supports Current, Voltage, "
+ "Position, Speed, and Percent (Throttle) "
+ "modes.");
+ return;
+ }
+ if (syncGroup != 0) {
+ dataBuffer[dataSize] = syncGroup;
+ dataSize++;
+ }
+
+ sendMessage(messageID, dataBuffer, dataSize, kSendMessagePeriod);
+
+ if (m_safetyHelper) m_safetyHelper->Feed();
+ }
+
+ m_value = outputValue;
+
+ verify();
+}
+
+/**
+ * Get the recently set outputValue setpoint.
+ *
+ * The scale and the units depend on the mode the Jaguar is in.<br>
+ * In percentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM
+ * Jaguar).<br>
+ * In voltage Mode, the outputValue is in volts.<br>
+ * In current Mode, the outputValue is in amps.<br>
+ * In speed Mode, the outputValue is in rotations/minute.<br>
+ * In position Mode, the outputValue is in rotations.<br>
+ *
+ * @return The most recently set outputValue setpoint.
+ */
+float CANJaguar::Get() const { return m_value; }
+
+/**
+* Common interface for disabling a motor.
+*
+* @deprecated Call {@link #DisableControl()} instead.
+*/
+void CANJaguar::Disable() { DisableControl(); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @deprecated Call Set instead.
+ *
+ * @param output Write out the PercentVbus value as was computed by the
+ * PIDController
+ */
+void CANJaguar::PIDWrite(float output) {
+ if (m_controlMode == kPercentVbus) {
+ Set(output);
+ } else {
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "PID only supported in PercentVbus mode");
+ }
+}
+
+uint8_t CANJaguar::packPercentage(uint8_t *buffer, double value) {
+ int16_t intValue = (int16_t)(value * 32767.0);
+ *((int16_t *)buffer) = swap16(intValue);
+ return sizeof(int16_t);
+}
+
+uint8_t CANJaguar::packFXP8_8(uint8_t *buffer, double value) {
+ int16_t intValue = (int16_t)(value * 256.0);
+ *((int16_t *)buffer) = swap16(intValue);
+ return sizeof(int16_t);
+}
+
+uint8_t CANJaguar::packFXP16_16(uint8_t *buffer, double value) {
+ int32_t intValue = (int32_t)(value * 65536.0);
+ *((int32_t *)buffer) = swap32(intValue);
+ return sizeof(int32_t);
+}
+
+uint8_t CANJaguar::packint16_t(uint8_t *buffer, int16_t value) {
+ *((int16_t *)buffer) = swap16(value);
+ return sizeof(int16_t);
+}
+
+uint8_t CANJaguar::packint32_t(uint8_t *buffer, int32_t value) {
+ *((int32_t *)buffer) = swap32(value);
+ return sizeof(int32_t);
+}
+
+double CANJaguar::unpackPercentage(uint8_t *buffer) const {
+ int16_t value = *((int16_t *)buffer);
+ value = swap16(value);
+ return value / 32767.0;
+}
+
+double CANJaguar::unpackFXP8_8(uint8_t *buffer) const {
+ int16_t value = *((int16_t *)buffer);
+ value = swap16(value);
+ return value / 256.0;
+}
+
+double CANJaguar::unpackFXP16_16(uint8_t *buffer) const {
+ int32_t value = *((int32_t *)buffer);
+ value = swap32(value);
+ return value / 65536.0;
+}
+
+int16_t CANJaguar::unpackint16_t(uint8_t *buffer) const {
+ int16_t value = *((int16_t *)buffer);
+ return swap16(value);
+}
+
+int32_t CANJaguar::unpackint32_t(uint8_t *buffer) const {
+ int32_t value = *((int32_t *)buffer);
+ return swap32(value);
+}
+
+/**
+ * Send a message to the Jaguar.
+ *
+ * @param messageID The messageID to be used on the CAN bus (device number is
+ * added internally)
+ * @param data The up to 8 bytes of data to be sent with the message
+ * @param dataSize Specify how much of the data in "data" to send
+ * @param periodic If positive, tell Network Communications to send the message
+ * every "period" milliseconds.
+ */
+void CANJaguar::sendMessage(uint32_t messageID, const uint8_t *data,
+ uint8_t dataSize, int32_t period) {
+ int32_t localStatus =
+ sendMessageHelper(messageID | m_deviceNumber, data, dataSize, period);
+
+ if (localStatus < 0) {
+ wpi_setErrorWithContext(localStatus, "sendMessage");
+ }
+}
+
+/**
+ * Request a message from the Jaguar, but don't wait for it to arrive.
+ *
+ * @param messageID The message to request
+ * @param periodic If positive, tell Network Communications to send the message
+ * every "period" milliseconds.
+ */
+void CANJaguar::requestMessage(uint32_t messageID, int32_t period) {
+ sendMessageHelper(messageID | m_deviceNumber, nullptr, 0, period);
+}
+
+/**
+ * Get a previously requested message.
+ *
+ * Jaguar always generates a message with the same message ID when replying.
+ *
+ * @param messageID The messageID to read from the CAN bus (device number is
+ * added internally)
+ * @param data The up to 8 bytes of data that was received with the message
+ * @param dataSize Indicates how much data was received
+ *
+ * @return true if the message was found. Otherwise, no new message is
+ * available.
+ */
+bool CANJaguar::getMessage(uint32_t messageID, uint32_t messageMask,
+ uint8_t *data, uint8_t *dataSize) const {
+ uint32_t targetedMessageID = messageID | m_deviceNumber;
+ int32_t status = 0;
+ uint32_t timeStamp;
+
+ // Caller may have set bit31 for remote frame transmission so clear invalid
+ // bits[31-29]
+ targetedMessageID &= CAN_MSGID_FULL_M;
+
+ // Get the data.
+ FRC_NetworkCommunication_CANSessionMux_receiveMessage(
+ &targetedMessageID, messageMask, data, dataSize, &timeStamp, &status);
+
+ // Do we already have the most recent value?
+ if (status == ERR_CANSessionMux_MessageNotFound)
+ return false;
+ else
+ wpi_setErrorWithContext(status, "receiveMessage");
+
+ return true;
+}
+
+/**
+ * Enables periodic status updates from the Jaguar.
+ */
+void CANJaguar::setupPeriodicStatus() {
+ uint8_t data[8];
+ uint8_t dataSize;
+
+ // Message 0 returns bus voltage, output voltage, output current, and
+ // temperature.
+ static const uint8_t kMessage0Data[] = {
+ LM_PSTAT_VOLTBUS_B0, LM_PSTAT_VOLTBUS_B1, LM_PSTAT_VOLTOUT_B0,
+ LM_PSTAT_VOLTOUT_B1, LM_PSTAT_CURRENT_B0, LM_PSTAT_CURRENT_B1,
+ LM_PSTAT_TEMP_B0, LM_PSTAT_TEMP_B1};
+
+ // Message 1 returns position and speed
+ static const uint8_t kMessage1Data[] = {
+ LM_PSTAT_POS_B0, LM_PSTAT_POS_B1, LM_PSTAT_POS_B2, LM_PSTAT_POS_B3,
+ LM_PSTAT_SPD_B0, LM_PSTAT_SPD_B1, LM_PSTAT_SPD_B2, LM_PSTAT_SPD_B3};
+
+ // Message 2 returns limits and faults
+ static const uint8_t kMessage2Data[] = {LM_PSTAT_LIMIT_CLR, LM_PSTAT_FAULT,
+ LM_PSTAT_END};
+
+ dataSize = packint16_t(data, kSendMessagePeriod);
+ sendMessage(LM_API_PSTAT_PER_EN_S0, data, dataSize);
+ sendMessage(LM_API_PSTAT_PER_EN_S1, data, dataSize);
+ sendMessage(LM_API_PSTAT_PER_EN_S2, data, dataSize);
+
+ dataSize = 8;
+ sendMessage(LM_API_PSTAT_CFG_S0, kMessage0Data, dataSize);
+ sendMessage(LM_API_PSTAT_CFG_S1, kMessage1Data, dataSize);
+ sendMessage(LM_API_PSTAT_CFG_S2, kMessage2Data, dataSize);
+}
+
+/**
+ * Check for new periodic status updates and unpack them into local variables
+ */
+void CANJaguar::updatePeriodicStatus() const {
+ uint8_t data[8];
+ uint8_t dataSize;
+
+ // Check if a new bus voltage/output voltage/current/temperature message
+ // has arrived and unpack the values into the cached member variables
+ if (getMessage(LM_API_PSTAT_DATA_S0, CAN_MSGID_FULL_M, data, &dataSize)) {
+ m_mutex.lock();
+ m_busVoltage = unpackFXP8_8(data);
+ m_outputVoltage = unpackPercentage(data + 2) * m_busVoltage;
+ m_outputCurrent = unpackFXP8_8(data + 4);
+ m_temperature = unpackFXP8_8(data + 6);
+ m_mutex.unlock();
+
+ m_receivedStatusMessage0 = true;
+ }
+
+ // Check if a new position/speed message has arrived and do the same
+ if (getMessage(LM_API_PSTAT_DATA_S1, CAN_MSGID_FULL_M, data, &dataSize)) {
+ m_mutex.lock();
+ m_position = unpackFXP16_16(data);
+ m_speed = unpackFXP16_16(data + 4);
+ m_mutex.unlock();
+
+ m_receivedStatusMessage1 = true;
+ }
+
+ // Check if a new limits/faults message has arrived and do the same
+ if (getMessage(LM_API_PSTAT_DATA_S2, CAN_MSGID_FULL_M, data, &dataSize)) {
+ m_mutex.lock();
+ m_limits = data[0];
+ m_faults = data[1];
+ m_mutex.unlock();
+
+ m_receivedStatusMessage2 = true;
+ }
+}
+
+/**
+ * Check all unverified params and make sure they're equal to their local
+ * cached versions. If a value isn't available, it gets requested. If a value
+ * doesn't match up, it gets set again.
+ */
+void CANJaguar::verify() {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ // If the Jaguar lost power, everything should be considered unverified.
+ if (getMessage(LM_API_STATUS_POWER, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ bool powerCycled = (bool)dataBuffer[0];
+
+ if (powerCycled) {
+ // Clear the power cycled bit
+ dataBuffer[0] = 1;
+ sendMessage(LM_API_STATUS_POWER, dataBuffer, sizeof(uint8_t));
+
+ // Mark everything as unverified
+ m_controlModeVerified = false;
+ m_speedRefVerified = false;
+ m_posRefVerified = false;
+ m_neutralModeVerified = false;
+ m_encoderCodesPerRevVerified = false;
+ m_potentiometerTurnsVerified = false;
+ m_forwardLimitVerified = false;
+ m_reverseLimitVerified = false;
+ m_limitModeVerified = false;
+ m_maxOutputVoltageVerified = false;
+ m_faultTimeVerified = false;
+
+ if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) {
+ m_voltageRampRateVerified = false;
+ } else {
+ m_pVerified = false;
+ m_iVerified = false;
+ m_dVerified = false;
+ }
+
+ // Verify periodic status messages again
+ m_receivedStatusMessage0 = false;
+ m_receivedStatusMessage1 = false;
+ m_receivedStatusMessage2 = false;
+
+ // Remove any old values from netcomms. Otherwise, parameters are
+ // incorrectly marked as verified based on stale messages.
+ getMessage(LM_API_SPD_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_POS_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_SPD_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_POS_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_ICTRL_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_SPD_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_POS_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_ICTRL_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_SPD_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_POS_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_ICTRL_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_CFG_BRAKE_COAST, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize);
+ getMessage(LM_API_CFG_ENC_LINES, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_CFG_POT_TURNS, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_CFG_LIMIT_MODE, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize);
+ getMessage(LM_API_CFG_LIMIT_FWD, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_CFG_LIMIT_REV, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_CFG_MAX_VOUT, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_VOLT_SET_RAMP, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_VCOMP_COMP_RAMP, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize);
+ getMessage(LM_API_CFG_FAULT_TIME, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize);
+ }
+ } else {
+ requestMessage(LM_API_STATUS_POWER);
+ }
+
+ // Verify that any recently set parameters are correct
+ if (!m_controlModeVerified && m_controlEnabled) {
+ if (getMessage(LM_API_STATUS_CMODE, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ ControlMode mode = (ControlMode)dataBuffer[0];
+
+ if (m_controlMode == mode)
+ m_controlModeVerified = true;
+ else
+ // Enable control again to resend the control mode
+ EnableControl();
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_STATUS_CMODE);
+ }
+ }
+
+ if (!m_speedRefVerified) {
+ if (getMessage(LM_API_SPD_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) {
+ uint8_t speedRef = dataBuffer[0];
+
+ if (m_speedReference == speedRef)
+ m_speedRefVerified = true;
+ else
+ // It's wrong - set it again
+ SetSpeedReference(m_speedReference);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_SPD_REF);
+ }
+ }
+
+ if (!m_posRefVerified) {
+ if (getMessage(LM_API_POS_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) {
+ uint8_t posRef = dataBuffer[0];
+
+ if (m_positionReference == posRef)
+ m_posRefVerified = true;
+ else
+ // It's wrong - set it again
+ SetPositionReference(m_positionReference);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_POS_REF);
+ }
+ }
+
+ if (!m_pVerified) {
+ uint32_t message = 0;
+
+ if (m_controlMode == kSpeed)
+ message = LM_API_SPD_PC;
+ else if (m_controlMode == kPosition)
+ message = LM_API_POS_PC;
+ else if (m_controlMode == kCurrent)
+ message = LM_API_ICTRL_PC;
+ else {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return;
+ }
+
+ if (getMessage(message, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) {
+ double p = unpackFXP16_16(dataBuffer);
+
+ if (FXP16_EQ(m_p, p))
+ m_pVerified = true;
+ else
+ // It's wrong - set it again
+ SetP(m_p);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(message);
+ }
+ }
+
+ if (!m_iVerified) {
+ uint32_t message = 0;
+
+ if (m_controlMode == kSpeed)
+ message = LM_API_SPD_IC;
+ else if (m_controlMode == kPosition)
+ message = LM_API_POS_IC;
+ else if (m_controlMode == kCurrent)
+ message = LM_API_ICTRL_IC;
+ else {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return;
+ }
+
+ if (getMessage(message, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) {
+ double i = unpackFXP16_16(dataBuffer);
+
+ if (FXP16_EQ(m_i, i))
+ m_iVerified = true;
+ else
+ // It's wrong - set it again
+ SetI(m_i);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(message);
+ }
+ }
+
+ if (!m_dVerified) {
+ uint32_t message = 0;
+
+ if (m_controlMode == kSpeed)
+ message = LM_API_SPD_DC;
+ else if (m_controlMode == kPosition)
+ message = LM_API_POS_DC;
+ else if (m_controlMode == kCurrent)
+ message = LM_API_ICTRL_DC;
+ else {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return;
+ }
+
+ if (getMessage(message, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) {
+ double d = unpackFXP16_16(dataBuffer);
+
+ if (FXP16_EQ(m_d, d))
+ m_dVerified = true;
+ else
+ // It's wrong - set it again
+ SetD(m_d);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(message);
+ }
+ }
+
+ if (!m_neutralModeVerified) {
+ if (getMessage(LM_API_CFG_BRAKE_COAST, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ NeutralMode mode = (NeutralMode)dataBuffer[0];
+
+ if (mode == m_neutralMode)
+ m_neutralModeVerified = true;
+ else
+ // It's wrong - set it again
+ ConfigNeutralMode(m_neutralMode);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_BRAKE_COAST);
+ }
+ }
+
+ if (!m_encoderCodesPerRevVerified) {
+ if (getMessage(LM_API_CFG_ENC_LINES, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ uint16_t codes = unpackint16_t(dataBuffer);
+
+ if (codes == m_encoderCodesPerRev)
+ m_encoderCodesPerRevVerified = true;
+ else
+ // It's wrong - set it again
+ ConfigEncoderCodesPerRev(m_encoderCodesPerRev);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_ENC_LINES);
+ }
+ }
+
+ if (!m_potentiometerTurnsVerified) {
+ if (getMessage(LM_API_CFG_POT_TURNS, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ uint16_t turns = unpackint16_t(dataBuffer);
+
+ if (turns == m_potentiometerTurns)
+ m_potentiometerTurnsVerified = true;
+ else
+ // It's wrong - set it again
+ ConfigPotentiometerTurns(m_potentiometerTurns);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_POT_TURNS);
+ }
+ }
+
+ if (!m_limitModeVerified) {
+ if (getMessage(LM_API_CFG_LIMIT_MODE, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ LimitMode mode = (LimitMode)dataBuffer[0];
+
+ if (mode == m_limitMode)
+ m_limitModeVerified = true;
+ else {
+ // It's wrong - set it again
+ ConfigLimitMode(m_limitMode);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_LIMIT_MODE);
+ }
+ }
+
+ if (!m_forwardLimitVerified) {
+ if (getMessage(LM_API_CFG_LIMIT_FWD, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ double limit = unpackFXP16_16(dataBuffer);
+
+ if (FXP16_EQ(limit, m_forwardLimit))
+ m_forwardLimitVerified = true;
+ else {
+ // It's wrong - set it again
+ ConfigForwardLimit(m_forwardLimit);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_LIMIT_FWD);
+ }
+ }
+
+ if (!m_reverseLimitVerified) {
+ if (getMessage(LM_API_CFG_LIMIT_REV, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ double limit = unpackFXP16_16(dataBuffer);
+
+ if (FXP16_EQ(limit, m_reverseLimit))
+ m_reverseLimitVerified = true;
+ else {
+ // It's wrong - set it again
+ ConfigReverseLimit(m_reverseLimit);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_LIMIT_REV);
+ }
+ }
+
+ if (!m_maxOutputVoltageVerified) {
+ if (getMessage(LM_API_CFG_MAX_VOUT, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ double voltage = unpackFXP8_8(dataBuffer);
+
+ // The returned max output voltage is sometimes slightly higher or
+ // lower than what was sent. This should not trigger resending
+ // the message.
+ if (std::abs(voltage - m_maxOutputVoltage) < 0.1)
+ m_maxOutputVoltageVerified = true;
+ else {
+ // It's wrong - set it again
+ ConfigMaxOutputVoltage(m_maxOutputVoltage);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_MAX_VOUT);
+ }
+ }
+
+ if (!m_voltageRampRateVerified) {
+ if (m_controlMode == kPercentVbus) {
+ if (getMessage(LM_API_VOLT_SET_RAMP, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ double rate = unpackPercentage(dataBuffer);
+
+ if (FXP16_EQ(rate, m_voltageRampRate))
+ m_voltageRampRateVerified = true;
+ else {
+ // It's wrong - set it again
+ SetVoltageRampRate(m_voltageRampRate);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_VOLT_SET_RAMP);
+ }
+ } else if (m_controlMode == kVoltage) {
+ if (getMessage(LM_API_VCOMP_COMP_RAMP, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ double rate = unpackFXP8_8(dataBuffer);
+
+ if (FXP8_EQ(rate, m_voltageRampRate))
+ m_voltageRampRateVerified = true;
+ else {
+ // It's wrong - set it again
+ SetVoltageRampRate(m_voltageRampRate);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_VCOMP_COMP_RAMP);
+ }
+ }
+ }
+
+ if (!m_faultTimeVerified) {
+ if (getMessage(LM_API_CFG_FAULT_TIME, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ uint16_t faultTime = unpackint16_t(dataBuffer);
+
+ if ((uint16_t)(m_faultTime * 1000.0) == faultTime)
+ m_faultTimeVerified = true;
+ else {
+ // It's wrong - set it again
+ ConfigFaultTime(m_faultTime);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_FAULT_TIME);
+ }
+ }
+
+ if (!m_receivedStatusMessage0 || !m_receivedStatusMessage1 ||
+ !m_receivedStatusMessage2) {
+ // If the periodic status messages haven't been verified as received,
+ // request periodic status messages again and attempt to unpack any
+ // available ones.
+ setupPeriodicStatus();
+ GetTemperature();
+ GetPosition();
+ GetFaults();
+ }
+}
+
+/**
+ * Set the reference source device for speed controller mode.
+ *
+ * Choose encoder as the source of speed feedback when in speed control mode.
+ *
+ * @param reference Specify a speed reference.
+ */
+void CANJaguar::SetSpeedReference(uint8_t reference) {
+ uint8_t dataBuffer[8];
+
+ // Send the speed reference parameter
+ dataBuffer[0] = reference;
+ sendMessage(LM_API_SPD_REF, dataBuffer, sizeof(uint8_t));
+
+ m_speedReference = reference;
+ m_speedRefVerified = false;
+}
+
+/**
+ * Get the reference source device for speed controller mode.
+ *
+ * @return A speed reference indicating the currently selected reference device
+ * for speed controller mode.
+ */
+uint8_t CANJaguar::GetSpeedReference() const { return m_speedReference; }
+
+/**
+ * Set the reference source device for position controller mode.
+ *
+ * Choose between using and encoder and using a potentiometer
+ * as the source of position feedback when in position control mode.
+ *
+ * @param reference Specify a PositionReference.
+ */
+void CANJaguar::SetPositionReference(uint8_t reference) {
+ uint8_t dataBuffer[8];
+
+ // Send the position reference parameter
+ dataBuffer[0] = reference;
+ sendMessage(LM_API_POS_REF, dataBuffer, sizeof(uint8_t));
+
+ m_positionReference = reference;
+ m_posRefVerified = false;
+}
+
+/**
+ * Get the reference source device for position controller mode.
+ *
+ * @return A PositionReference indicating the currently selected reference
+ * device for position controller mode.
+ */
+uint8_t CANJaguar::GetPositionReference() const { return m_positionReference; }
+
+/**
+ * Set the P, I, and D constants for the closed loop modes.
+ *
+ * @param p The proportional gain of the Jaguar's PID controller.
+ * @param i The integral gain of the Jaguar's PID controller.
+ * @param d The differential gain of the Jaguar's PID controller.
+ */
+void CANJaguar::SetPID(double p, double i, double d) {
+ SetP(p);
+ SetI(i);
+ SetD(d);
+}
+
+/**
+ * Set the P constant for the closed loop modes.
+ *
+ * @param p The proportional gain of the Jaguar's PID controller.
+ */
+void CANJaguar::SetP(double p) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ case kVoltage:
+ case kFollower:
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ break;
+ case kSpeed:
+ dataSize = packFXP16_16(dataBuffer, p);
+ sendMessage(LM_API_SPD_PC, dataBuffer, dataSize);
+ break;
+ case kPosition:
+ dataSize = packFXP16_16(dataBuffer, p);
+ sendMessage(LM_API_POS_PC, dataBuffer, dataSize);
+ break;
+ case kCurrent:
+ dataSize = packFXP16_16(dataBuffer, p);
+ sendMessage(LM_API_ICTRL_PC, dataBuffer, dataSize);
+ break;
+ }
+
+ m_p = p;
+ m_pVerified = false;
+}
+
+/**
+ * Set the I constant for the closed loop modes.
+ *
+ * @param i The integral gain of the Jaguar's PID controller.
+ */
+void CANJaguar::SetI(double i) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ case kVoltage:
+ case kFollower:
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ break;
+ case kSpeed:
+ dataSize = packFXP16_16(dataBuffer, i);
+ sendMessage(LM_API_SPD_IC, dataBuffer, dataSize);
+ break;
+ case kPosition:
+ dataSize = packFXP16_16(dataBuffer, i);
+ sendMessage(LM_API_POS_IC, dataBuffer, dataSize);
+ break;
+ case kCurrent:
+ dataSize = packFXP16_16(dataBuffer, i);
+ sendMessage(LM_API_ICTRL_IC, dataBuffer, dataSize);
+ break;
+ }
+
+ m_i = i;
+ m_iVerified = false;
+}
+
+/**
+ * Set the D constant for the closed loop modes.
+ *
+ * @param d The derivative gain of the Jaguar's PID controller.
+ */
+void CANJaguar::SetD(double d) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ case kVoltage:
+ case kFollower:
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ break;
+ case kSpeed:
+ dataSize = packFXP16_16(dataBuffer, d);
+ sendMessage(LM_API_SPD_DC, dataBuffer, dataSize);
+ break;
+ case kPosition:
+ dataSize = packFXP16_16(dataBuffer, d);
+ sendMessage(LM_API_POS_DC, dataBuffer, dataSize);
+ break;
+ case kCurrent:
+ dataSize = packFXP16_16(dataBuffer, d);
+ sendMessage(LM_API_ICTRL_DC, dataBuffer, dataSize);
+ break;
+ }
+
+ m_d = d;
+ m_dVerified = false;
+}
+
+/**
+ * Get the Proportional gain of the controller.
+ *
+ * @return The proportional gain.
+ */
+double CANJaguar::GetP() const {
+ if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return 0.0;
+ }
+
+ return m_p;
+}
+
+/**
+ * Get the Intregral gain of the controller.
+ *
+ * @return The integral gain.
+ */
+double CANJaguar::GetI() const {
+ if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return 0.0;
+ }
+
+ return m_i;
+}
+
+/**
+ * Get the Differential gain of the controller.
+ *
+ * @return The differential gain.
+ */
+double CANJaguar::GetD() const {
+ if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return 0.0;
+ }
+
+ return m_d;
+}
+
+/**
+ * Enable the closed loop controller.
+ *
+ * Start actually controlling the output based on the feedback.
+ * If starting a position controller with an encoder reference,
+ * use the encoderInitialPosition parameter to initialize the
+ * encoder state.
+ *
+ * @param encoderInitialPosition Encoder position to set if position with
+ * encoder reference. Ignored otherwise.
+ */
+void CANJaguar::EnableControl(double encoderInitialPosition) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize = 0;
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ sendMessage(LM_API_VOLT_T_EN, dataBuffer, dataSize);
+ break;
+ case kSpeed:
+ sendMessage(LM_API_SPD_T_EN, dataBuffer, dataSize);
+ break;
+ case kPosition:
+ dataSize = packFXP16_16(dataBuffer, encoderInitialPosition);
+ sendMessage(LM_API_POS_T_EN, dataBuffer, dataSize);
+ break;
+ case kCurrent:
+ sendMessage(LM_API_ICTRL_T_EN, dataBuffer, dataSize);
+ break;
+ case kVoltage:
+ sendMessage(LM_API_VCOMP_T_EN, dataBuffer, dataSize);
+ break;
+ default:
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "The Jaguar only supports Current, Voltage, "
+ "Position, Speed, and Percent (Throttle) "
+ "modes.");
+ return;
+ }
+
+ m_controlEnabled = true;
+ m_controlModeVerified = false;
+}
+
+/**
+ * Disable the closed loop controller.
+ *
+ * Stop driving the output based on the feedback.
+ */
+void CANJaguar::DisableControl() {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize = 0;
+
+ // Disable all control
+ sendMessage(LM_API_VOLT_DIS, dataBuffer, dataSize);
+ sendMessage(LM_API_SPD_DIS, dataBuffer, dataSize);
+ sendMessage(LM_API_POS_DIS, dataBuffer, dataSize);
+ sendMessage(LM_API_ICTRL_DIS, dataBuffer, dataSize);
+ sendMessage(LM_API_VCOMP_DIS, dataBuffer, dataSize);
+
+ // Stop all periodic setpoints
+ sendMessage(LM_API_VOLT_T_SET, dataBuffer, dataSize,
+ CAN_SEND_PERIOD_STOP_REPEATING);
+ sendMessage(LM_API_SPD_T_SET, dataBuffer, dataSize,
+ CAN_SEND_PERIOD_STOP_REPEATING);
+ sendMessage(LM_API_POS_T_SET, dataBuffer, dataSize,
+ CAN_SEND_PERIOD_STOP_REPEATING);
+ sendMessage(LM_API_ICTRL_T_SET, dataBuffer, dataSize,
+ CAN_SEND_PERIOD_STOP_REPEATING);
+ sendMessage(LM_API_VCOMP_T_SET, dataBuffer, dataSize,
+ CAN_SEND_PERIOD_STOP_REPEATING);
+
+ m_controlEnabled = false;
+}
+
+/**
+ * Enable controlling the motor voltage as a percentage of the bus voltage
+ * without any position or speed feedback.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ */
+void CANJaguar::SetPercentMode() {
+ SetControlMode(kPercentVbus);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_NONE);
+}
+
+/**
+ * Enable controlling the motor voltage as a percentage of the bus voltage,
+ * and enable speed sensing from a non-quadrature encoder.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ *
+ * @param tag The constant CANJaguar::Encoder
+ * @param codesPerRev The counts per revolution on the encoder
+ */
+void CANJaguar::SetPercentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) {
+ SetControlMode(kPercentVbus);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+}
+
+/**
+ * Enable controlling the motor voltage as a percentage of the bus voltage,
+ * and enable speed sensing from a non-quadrature encoder.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ *
+ * @param tag The constant CANJaguar::QuadEncoder
+ * @param codesPerRev The counts per revolution on the encoder
+ */
+void CANJaguar::SetPercentMode(CANJaguar::QuadEncoderStruct,
+ uint16_t codesPerRev) {
+ SetControlMode(kPercentVbus);
+ SetPositionReference(LM_REF_ENCODER);
+ SetSpeedReference(LM_REF_QUAD_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+}
+
+/**
+* Enable controlling the motor voltage as a percentage of the bus voltage,
+* and enable position sensing from a potentiometer and no speed feedback.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param potentiometer The constant CANJaguar::Potentiometer
+*/
+void CANJaguar::SetPercentMode(CANJaguar::PotentiometerStruct) {
+ SetControlMode(kPercentVbus);
+ SetPositionReference(LM_REF_POT);
+ SetSpeedReference(LM_REF_NONE);
+ ConfigPotentiometerTurns(1);
+}
+
+/**
+ * Enable controlling the motor current with a PID loop.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ *
+ * @param p The proportional gain of the Jaguar's PID controller.
+ * @param i The integral gain of the Jaguar's PID controller.
+ * @param d The differential gain of the Jaguar's PID controller.
+ */
+void CANJaguar::SetCurrentMode(double p, double i, double d) {
+ SetControlMode(kCurrent);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_NONE);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the motor current with a PID loop, and enable speed
+* sensing from a non-quadrature encoder.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param encoder The constant CANJaguar::Encoder
+* @param p The proportional gain of the Jaguar's PID controller.
+* @param i The integral gain of the Jaguar's PID controller.
+* @param d The differential gain of the Jaguar's PID controller.
+*/
+void CANJaguar::SetCurrentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev,
+ double p, double i, double d) {
+ SetControlMode(kCurrent);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_NONE);
+ ConfigEncoderCodesPerRev(codesPerRev);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the motor current with a PID loop, and enable speed and
+* position sensing from a quadrature encoder.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param endoer The constant CANJaguar::QuadEncoder
+* @param p The proportional gain of the Jaguar's PID controller.
+* @param i The integral gain of the Jaguar's PID controller.
+* @param d The differential gain of the Jaguar's PID controller.
+*/
+void CANJaguar::SetCurrentMode(CANJaguar::QuadEncoderStruct,
+ uint16_t codesPerRev, double p, double i,
+ double d) {
+ SetControlMode(kCurrent);
+ SetPositionReference(LM_REF_ENCODER);
+ SetSpeedReference(LM_REF_QUAD_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the motor current with a PID loop, and enable position
+* sensing from a potentiometer.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param potentiometer The constant CANJaguar::Potentiometer
+* @param p The proportional gain of the Jaguar's PID controller.
+* @param i The integral gain of the Jaguar's PID controller.
+* @param d The differential gain of the Jaguar's PID controller.
+*/
+void CANJaguar::SetCurrentMode(CANJaguar::PotentiometerStruct, double p,
+ double i, double d) {
+ SetControlMode(kCurrent);
+ SetPositionReference(LM_REF_POT);
+ SetSpeedReference(LM_REF_NONE);
+ ConfigPotentiometerTurns(1);
+ SetPID(p, i, d);
+}
+
+/**
+ * Enable controlling the speed with a feedback loop from a non-quadrature
+ * encoder.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ *
+ * @param encoder The constant CANJaguar::Encoder
+ * @param codesPerRev The counts per revolution on the encoder.
+ * @param p The proportional gain of the Jaguar's PID controller.
+ * @param i The integral gain of the Jaguar's PID controller.
+ * @param d The differential gain of the Jaguar's PID controller.
+ */
+void CANJaguar::SetSpeedMode(CANJaguar::EncoderStruct, uint16_t codesPerRev,
+ double p, double i, double d) {
+ SetControlMode(kSpeed);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the speed with a feedback loop from a quadrature
+* encoder.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param encoder The constant CANJaguar::QuadEncoder
+* @param codesPerRev The counts per revolution on the encoder.
+* @param p The proportional gain of the Jaguar's PID controller.
+* @param i The integral gain of the Jaguar's PID controller.
+* @param d The differential gain of the Jaguar's PID controller.
+*/
+void CANJaguar::SetSpeedMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev,
+ double p, double i, double d) {
+ SetControlMode(kSpeed);
+ SetPositionReference(LM_REF_ENCODER);
+ SetSpeedReference(LM_REF_QUAD_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+ SetPID(p, i, d);
+}
+
+/**
+ * Enable controlling the position with a feedback loop using an encoder.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ *
+ * @param encoder The constant CANJaguar::QuadEncoder
+ * @param codesPerRev The counts per revolution on the encoder.
+ * @param p The proportional gain of the Jaguar's PID controller.
+ * @param i The integral gain of the Jaguar's PID controller.
+ * @param d The differential gain of the Jaguar's PID controller.
+ *
+ */
+void CANJaguar::SetPositionMode(CANJaguar::QuadEncoderStruct,
+ uint16_t codesPerRev, double p, double i,
+ double d) {
+ SetControlMode(kPosition);
+ SetPositionReference(LM_REF_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the position with a feedback loop using a
+* potentiometer.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+* @param p The proportional gain of the Jaguar's PID controller.
+* @param i The integral gain of the Jaguar's PID controller.
+* @param d The differential gain of the Jaguar's PID controller.
+*/
+void CANJaguar::SetPositionMode(CANJaguar::PotentiometerStruct, double p,
+ double i, double d) {
+ SetControlMode(kPosition);
+ SetPositionReference(LM_REF_POT);
+ ConfigPotentiometerTurns(1);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the motor voltage without any position or speed
+* feedback.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*/
+void CANJaguar::SetVoltageMode() {
+ SetControlMode(kVoltage);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_NONE);
+}
+
+/**
+* Enable controlling the motor voltage with speed feedback from a
+* non-quadrature encoder and no position feedback.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param encoder The constant CANJaguar::Encoder
+* @param codesPerRev The counts per revolution on the encoder
+*/
+void CANJaguar::SetVoltageMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) {
+ SetControlMode(kVoltage);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+}
+
+/**
+* Enable controlling the motor voltage with position and speed feedback from a
+* quadrature encoder.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param encoder The constant CANJaguar::QuadEncoder
+* @param codesPerRev The counts per revolution on the encoder
+*/
+void CANJaguar::SetVoltageMode(CANJaguar::QuadEncoderStruct,
+ uint16_t codesPerRev) {
+ SetControlMode(kVoltage);
+ SetPositionReference(LM_REF_ENCODER);
+ SetSpeedReference(LM_REF_QUAD_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+}
+
+/**
+* Enable controlling the motor voltage with position feedback from a
+* potentiometer and no speed feedback.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param potentiometer The constant CANJaguar::Potentiometer
+*/
+void CANJaguar::SetVoltageMode(CANJaguar::PotentiometerStruct) {
+ SetControlMode(kVoltage);
+ SetPositionReference(LM_REF_POT);
+ SetSpeedReference(LM_REF_NONE);
+ ConfigPotentiometerTurns(1);
+}
+
+/**
+ * Used internally. In order to set the control mode see the methods listed
+ * below.
+ * Change the control mode of this Jaguar object.
+ *
+ * After changing modes, configure any PID constants or other settings needed
+ * and then EnableControl() to actually change the mode on the Jaguar.
+ *
+ * @param controlMode The new mode.
+ */
+void CANJaguar::SetControlMode(ControlMode controlMode) {
+ // Disable the previous mode
+ DisableControl();
+
+ if (controlMode == kFollower)
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "The Jaguar only supports Current, Voltage, "
+ "Position, Speed, and Percent (Throttle) "
+ "modes.");
+
+ // Update the local mode
+ m_controlMode = controlMode;
+ m_controlModeVerified = false;
+
+ HALReport(HALUsageReporting::kResourceType_CANJaguar, m_deviceNumber,
+ m_controlMode);
+}
+
+/**
+ * Get the active control mode from the Jaguar.
+ *
+ * Ask the Jag what mode it is in.
+ *
+ * @return ControlMode that the Jag is in.
+ */
+CANJaguar::ControlMode CANJaguar::GetControlMode() const {
+ return m_controlMode;
+}
+
+/**
+ * Get the voltage at the battery input terminals of the Jaguar.
+ *
+ * @return The bus voltage in volts.
+ */
+float CANJaguar::GetBusVoltage() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_busVoltage;
+}
+
+/**
+ * Get the voltage being output from the motor terminals of the Jaguar.
+ *
+ * @return The output voltage in volts.
+ */
+float CANJaguar::GetOutputVoltage() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_outputVoltage;
+}
+
+/**
+ * Get the current through the motor terminals of the Jaguar.
+ *
+ * @return The output current in amps.
+ */
+float CANJaguar::GetOutputCurrent() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_outputCurrent;
+}
+
+/**
+ * Get the internal temperature of the Jaguar.
+ *
+ * @return The temperature of the Jaguar in degrees Celsius.
+ */
+float CANJaguar::GetTemperature() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_temperature;
+}
+
+/**
+ * Get the position of the encoder or potentiometer.
+ *
+ * @return The position of the motor in rotations based on the configured
+ * feedback.
+ * @see CANJaguar#ConfigPotentiometerTurns(int)
+ * @see CANJaguar#ConfigEncoderCodesPerRev(int)
+ */
+double CANJaguar::GetPosition() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_position;
+}
+
+/**
+ * Get the speed of the encoder.
+ *
+ * @return The speed of the motor in RPM based on the configured feedback.
+ */
+double CANJaguar::GetSpeed() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_speed;
+}
+
+/**
+ * Get the status of the forward limit switch.
+ *
+ * @return The motor is allowed to turn in the forward direction when true.
+ */
+bool CANJaguar::GetForwardLimitOK() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_limits & kForwardLimit;
+}
+
+/**
+ * Get the status of the reverse limit switch.
+ *
+ * @return The motor is allowed to turn in the reverse direction when true.
+ */
+bool CANJaguar::GetReverseLimitOK() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_limits & kReverseLimit;
+}
+
+/**
+ * Get the status of any faults the Jaguar has detected.
+ *
+ * @return A bit-mask of faults defined by the "Faults" enum.
+ * @see #kCurrentFault
+ * @see #kBusVoltageFault
+ * @see #kTemperatureFault
+ * @see #kGateDriverFault
+ */
+uint16_t CANJaguar::GetFaults() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_faults;
+}
+
+/**
+ * Set the maximum voltage change rate.
+ *
+ * When in PercentVbus or Voltage output mode, the rate at which the voltage
+ * changes can
+ * be limited to reduce current spikes. Set this to 0.0 to disable rate
+ * limiting.
+ *
+ * @param rampRate The maximum rate of voltage change in Percent Voltage mode in
+ * V/s.
+ */
+void CANJaguar::SetVoltageRampRate(double rampRate) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+ uint32_t message;
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ dataSize = packPercentage(
+ dataBuffer, rampRate / (m_maxOutputVoltage * kControllerRate));
+ message = LM_API_VOLT_SET_RAMP;
+ break;
+ case kVoltage:
+ dataSize = packFXP8_8(dataBuffer, rampRate / kControllerRate);
+ message = LM_API_VCOMP_COMP_RAMP;
+ break;
+ default:
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "SetVoltageRampRate only applies in Voltage and Percent mode");
+ return;
+ }
+
+ sendMessage(message, dataBuffer, dataSize);
+
+ m_voltageRampRate = rampRate;
+ m_voltageRampRateVerified = false;
+}
+
+/**
+ * Get the version of the firmware running on the Jaguar.
+ *
+ * @return The firmware version. 0 if the device did not respond.
+ */
+uint32_t CANJaguar::GetFirmwareVersion() const { return m_firmwareVersion; }
+
+/**
+ * Get the version of the Jaguar hardware.
+ *
+ * @return The hardware version. 1: Jaguar, 2: Black Jaguar
+ */
+uint8_t CANJaguar::GetHardwareVersion() const { return m_hardwareVersion; }
+
+/**
+ * Configure what the controller does to the H-Bridge when neutral (not driving
+ * the output).
+ *
+ * This allows you to override the jumper configuration for brake or coast.
+ *
+ * @param mode Select to use the jumper setting or to override it to coast or
+ * brake.
+ */
+void CANJaguar::ConfigNeutralMode(NeutralMode mode) {
+ uint8_t dataBuffer[8];
+
+ // Set the neutral mode
+ sendMessage(LM_API_CFG_BRAKE_COAST, dataBuffer, sizeof(uint8_t));
+
+ m_neutralMode = mode;
+ m_neutralModeVerified = false;
+}
+
+/**
+ * Configure how many codes per revolution are generated by your encoder.
+ *
+ * @param codesPerRev The number of counts per revolution in 1X mode.
+ */
+void CANJaguar::ConfigEncoderCodesPerRev(uint16_t codesPerRev) {
+ uint8_t dataBuffer[8];
+
+ // Set the codes per revolution mode
+ packint16_t(dataBuffer, codesPerRev);
+ sendMessage(LM_API_CFG_ENC_LINES, dataBuffer, sizeof(uint16_t));
+
+ m_encoderCodesPerRev = codesPerRev;
+ m_encoderCodesPerRevVerified = false;
+}
+
+/**
+ * Configure the number of turns on the potentiometer.
+ *
+ * There is no special support for continuous turn potentiometers.
+ * Only integer numbers of turns are supported.
+ *
+ * @param turns The number of turns of the potentiometer.
+ */
+void CANJaguar::ConfigPotentiometerTurns(uint16_t turns) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ // Set the pot turns
+ dataSize = packint16_t(dataBuffer, turns);
+ sendMessage(LM_API_CFG_POT_TURNS, dataBuffer, dataSize);
+
+ m_potentiometerTurns = turns;
+ m_potentiometerTurnsVerified = false;
+}
+
+/**
+ * Configure Soft Position Limits when in Position Controller mode.
+ *
+ * When controlling position, you can add additional limits on top of the limit
+ switch inputs
+ * that are based on the position feedback. If the position limit is reached or
+ the
+ * switch is opened, that direction will be disabled.
+ *
+
+ * @param forwardLimitPosition The position that if exceeded will disable the
+ forward direction.
+ * @param reverseLimitPosition The position that if exceeded will disable the
+ reverse direction.
+ */
+void CANJaguar::ConfigSoftPositionLimits(double forwardLimitPosition,
+ double reverseLimitPosition) {
+ ConfigLimitMode(kLimitMode_SoftPositionLimits);
+ ConfigForwardLimit(forwardLimitPosition);
+ ConfigReverseLimit(reverseLimitPosition);
+}
+
+/**
+ * Disable Soft Position Limits if previously enabled.
+ *
+ * Soft Position Limits are disabled by default.
+ */
+void CANJaguar::DisableSoftPositionLimits() {
+ ConfigLimitMode(kLimitMode_SwitchInputsOnly);
+}
+
+/**
+ * Set the limit mode for position control mode.
+ *
+ * Use ConfigSoftPositionLimits or DisableSoftPositionLimits to set this
+ * automatically.
+ */
+void CANJaguar::ConfigLimitMode(LimitMode mode) {
+ uint8_t dataBuffer[8];
+
+ dataBuffer[0] = mode;
+ sendMessage(LM_API_CFG_LIMIT_MODE, dataBuffer, sizeof(uint8_t));
+
+ m_limitMode = mode;
+ m_limitModeVerified = false;
+}
+
+/**
+* Set the position that if exceeded will disable the forward direction.
+*
+* Use ConfigSoftPositionLimits to set this and the limit mode automatically.
+*/
+void CANJaguar::ConfigForwardLimit(double forwardLimitPosition) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ dataSize = packFXP16_16(dataBuffer, forwardLimitPosition);
+ dataBuffer[dataSize++] = 1;
+ sendMessage(LM_API_CFG_LIMIT_FWD, dataBuffer, dataSize);
+
+ m_forwardLimit = forwardLimitPosition;
+ m_forwardLimitVerified = false;
+}
+
+/**
+* Set the position that if exceeded will disable the reverse direction.
+*
+* Use ConfigSoftPositionLimits to set this and the limit mode automatically.
+*/
+void CANJaguar::ConfigReverseLimit(double reverseLimitPosition) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ dataSize = packFXP16_16(dataBuffer, reverseLimitPosition);
+ dataBuffer[dataSize++] = 0;
+ sendMessage(LM_API_CFG_LIMIT_REV, dataBuffer, dataSize);
+
+ m_reverseLimit = reverseLimitPosition;
+ m_reverseLimitVerified = false;
+}
+
+/**
+ * Configure the maximum voltage that the Jaguar will ever output.
+ *
+ * This can be used to limit the maximum output voltage in all modes so that
+ * motors which cannot withstand full bus voltage can be used safely.
+ *
+ * @param voltage The maximum voltage output by the Jaguar.
+ */
+void CANJaguar::ConfigMaxOutputVoltage(double voltage) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ dataSize = packFXP8_8(dataBuffer, voltage);
+ sendMessage(LM_API_CFG_MAX_VOUT, dataBuffer, dataSize);
+
+ m_maxOutputVoltage = voltage;
+ m_maxOutputVoltageVerified = false;
+}
+
+/**
+ * Configure how long the Jaguar waits in the case of a fault before resuming
+ * operation.
+ *
+ * Faults include over temerature, over current, and bus under voltage.
+ * The default is 3.0 seconds, but can be reduced to as low as 0.5 seconds.
+ *
+ * @param faultTime The time to wait before resuming operation, in seconds.
+ */
+void CANJaguar::ConfigFaultTime(float faultTime) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ if (faultTime < 0.5)
+ faultTime = 0.5;
+ else if (faultTime > 3.0)
+ faultTime = 3.0;
+
+ // Message takes ms
+ dataSize = packint16_t(dataBuffer, (int16_t)(faultTime * 1000.0));
+ sendMessage(LM_API_CFG_FAULT_TIME, dataBuffer, dataSize);
+
+ m_faultTime = faultTime;
+ m_faultTimeVerified = false;
+}
+
+/**
+ * Update all the motors that have pending sets in the syncGroup.
+ *
+ * @param syncGroup A bitmask of groups to generate synchronous output.
+ */
+void CANJaguar::UpdateSyncGroup(uint8_t syncGroup) {
+ sendMessageHelper(CAN_MSGID_API_SYNC, &syncGroup, sizeof(syncGroup),
+ CAN_SEND_PERIOD_NO_REPEAT);
+}
+
+void CANJaguar::SetExpiration(float timeout) {
+ if (m_safetyHelper) m_safetyHelper->SetExpiration(timeout);
+}
+
+float CANJaguar::GetExpiration() const {
+ if (!m_safetyHelper) return 0.0;
+ return m_safetyHelper->GetExpiration();
+}
+
+bool CANJaguar::IsAlive() const {
+ if (!m_safetyHelper) return false;
+ return m_safetyHelper->IsAlive();
+}
+
+bool CANJaguar::IsSafetyEnabled() const {
+ if (!m_safetyHelper) return false;
+ return m_safetyHelper->IsSafetyEnabled();
+}
+
+void CANJaguar::SetSafetyEnabled(bool enabled) {
+ if (m_safetyHelper) m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+void CANJaguar::GetDescription(std::ostringstream& desc) const {
+ desc << "CANJaguar ID " << m_deviceNumber;
+}
+
+uint8_t CANJaguar::GetDeviceID() const { return m_deviceNumber; }
+
+/**
+ * Common interface for stopping the motor
+ * Part of the MotorSafety interface
+ *
+ * @deprecated Call DisableControl instead.
+ */
+void CANJaguar::StopMotor() { DisableControl(); }
+
+/**
+* Common interface for inverting direction of a speed controller.
+* Only works in PercentVbus, speed, and Voltage modes.
+* @param isInverted The state of inversion, true is inverted
+*/
+void CANJaguar::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+/**
+ * Common interface for the inverting direction of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ *
+ */
+bool CANJaguar::GetInverted() const { return m_isInverted; }
+
+void CANJaguar::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if(key == "Mode" && value->IsDouble()) SetControlMode(static_cast<CANSpeedController::ControlMode>(value->GetDouble()));
+ if(IsModePID(m_controlMode) && value->IsDouble()) {
+ if(key == "p") SetP(value->GetDouble());
+ if(key == "i") SetI(value->GetDouble());
+ if(key == "d") SetD(value->GetDouble());
+ }
+ if(key == "Enabled" && value->IsBoolean()) {
+ if (value->GetBoolean()) {
+ EnableControl();
+ } else {
+ DisableControl();
+ }
+ }
+ if(key == "Value" && value->IsDouble()) Set(value->GetDouble());
+}
+
+bool CANJaguar::IsModePID(CANSpeedController::ControlMode mode) const {
+ return mode == kCurrent || mode == kSpeed || mode == kPosition;
+}
+
+void CANJaguar::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutString("~TYPE~", "CANSpeedController");
+ m_table->PutString("Type", "CANJaguar");
+ m_table->PutString("Mode", GetModeName(m_controlMode));
+ if (IsModePID(m_controlMode)) {
+ m_table->PutNumber("p", GetP());
+ m_table->PutNumber("i", GetI());
+ m_table->PutNumber("d", GetD());
+ }
+ m_table->PutBoolean("Enabled", m_controlEnabled);
+ m_table->PutNumber("Value", Get());
+ }
+}
+
+void CANJaguar::StartLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->AddTableListener(this, true);
+ }
+}
+
+void CANJaguar::StopLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string CANJaguar::GetSmartDashboardType() const {
+ return "CANSpeedController";
+}
+
+void CANJaguar::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> CANJaguar::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/CANTalon.cpp b/wpilibc/Athena/src/CANTalon.cpp
new file mode 100644
index 0000000..f937212
--- /dev/null
+++ b/wpilibc/Athena/src/CANTalon.cpp
@@ -0,0 +1,1750 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "CANTalon.h"
+#include "WPIErrors.h"
+#include <unistd.h> // usleep
+#include <sstream>
+/**
+ * Number of adc engineering units per 0 to 3.3V sweep.
+ * This is necessary for scaling Analog Position in rotations/RPM.
+ */
+const double kNativeAdcUnitsPerRotation = 1024.0;
+/**
+ * Number of pulse width engineering units per full rotation.
+ * This is necessary for scaling Pulse Width Decoded Position in rotations/RPM.
+ */
+const double kNativePwdUnitsPerRotation = 4096.0;
+/**
+ * Number of minutes per 100ms unit. Useful for scaling velocities
+ * measured by Talon's 100ms timebase to rotations per minute.
+ */
+const double kMinutesPer100msUnit = 1.0/600.0;
+
+/**
+ * Constructor for the CANTalon device.
+ * @param deviceNumber The CAN ID of the Talon SRX
+ */
+CANTalon::CANTalon(int deviceNumber)
+ : m_deviceNumber(deviceNumber),
+ m_impl(new CanTalonSRX(deviceNumber)),
+ m_safetyHelper(new MotorSafetyHelper(this)) {
+ ApplyControlMode(m_controlMode);
+ m_impl->SetProfileSlotSelect(m_profile);
+ m_isInverted = false;
+}
+/**
+ * Constructor for the CANTalon device.
+ * @param deviceNumber The CAN ID of the Talon SRX
+ * @param controlPeriodMs The period in ms to send the CAN control frame.
+ * Period is bounded to [1ms,95ms].
+ */
+CANTalon::CANTalon(int deviceNumber, int controlPeriodMs)
+ : m_deviceNumber(deviceNumber),
+ m_impl(new CanTalonSRX(deviceNumber,controlPeriodMs)),
+ m_safetyHelper(new MotorSafetyHelper(this)) {
+ ApplyControlMode(m_controlMode);
+ m_impl->SetProfileSlotSelect(m_profile);
+}
+
+CANTalon::~CANTalon() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+ if (m_hasBeenMoved) return;
+ Disable();
+}
+
+/**
+* Write out the PID value as seen in the PIDOutput base object.
+*
+* @deprecated Call Set instead.
+*
+* @param output Write out the PercentVbus value as was computed by the
+* PIDController
+*/
+void CANTalon::PIDWrite(float output) {
+ if (GetControlMode() == kPercentVbus) {
+ Set(output);
+ } else {
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "PID only supported in PercentVbus mode");
+ }
+}
+
+/**
+ * Retrieve the current sensor value. Equivalent to Get().
+ *
+ * @return The current sensor value of the Talon.
+ */
+double CANTalon::PIDGet() { return Get(); }
+
+/**
+ * Gets the current status of the Talon (usually a sensor value).
+ *
+ * In Current mode: returns output current.
+ * In Speed mode: returns current speed.
+ * In Position mode: returns current sensor position.
+ * In PercentVbus and Follower modes: returns current applied throttle.
+ *
+ * @return The current sensor value of the Talon.
+ */
+float CANTalon::Get() const {
+ int value;
+ switch (m_controlMode) {
+ case kVoltage:
+ return GetOutputVoltage();
+ case kCurrent:
+ return GetOutputCurrent();
+ case kSpeed:
+ m_impl->GetSensorVelocity(value);
+ return ScaleNativeUnitsToRpm(m_feedbackDevice, value);
+ case kPosition:
+ m_impl->GetSensorPosition(value);
+ return ScaleNativeUnitsToRotations(m_feedbackDevice, value);
+ case kPercentVbus:
+ case kFollower:
+ default:
+ m_impl->GetAppliedThrottle(value);
+ return (float)value / 1023.0;
+ }
+}
+
+/**
+ * Sets the appropriate output on the talon, depending on the mode.
+ *
+ * In PercentVbus, the output is between -1.0 and 1.0, with 0.0 as stopped.
+ * In Voltage mode, output value is in volts.
+ * In Current mode, output value is in amperes.
+ * In Speed mode, output value is in position change / 10ms.
+ * In Position mode, output value is in encoder ticks or an analog value,
+ * depending on the sensor.
+ * In Follower mode, the output value is the integer device ID of the talon to
+ * duplicate.
+ *
+ * @param outputValue The setpoint value, as described above.
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+void CANTalon::Set(float value, uint8_t syncGroup) {
+ /* feed safety helper since caller just updated our output */
+ m_safetyHelper->Feed();
+ if (m_controlEnabled) {
+ m_setPoint = value; /* cache set point for GetSetpoint() */
+ CTR_Code status = CTR_OKAY;
+ switch (m_controlMode) {
+ case CANSpeedController::kPercentVbus: {
+ m_impl->Set(m_isInverted ? -value : value);
+ status = CTR_OKAY;
+ } break;
+ case CANSpeedController::kFollower: {
+ status = m_impl->SetDemand((int)value);
+ } break;
+ case CANSpeedController::kVoltage: {
+ // Voltage is an 8.8 fixed point number.
+ int volts = int((m_isInverted ? -value : value) * 256);
+ status = m_impl->SetDemand(volts);
+ } break;
+ case CANSpeedController::kSpeed:
+ /* if the caller has provided scaling info, apply it */
+ status = m_impl->SetDemand(ScaleVelocityToNativeUnits(m_feedbackDevice, m_isInverted ? -value : value));
+ break;
+ case CANSpeedController::kPosition:
+ status = m_impl->SetDemand(ScaleRotationsToNativeUnits(m_feedbackDevice, value));
+ break;
+ case CANSpeedController::kCurrent: {
+ double milliamperes = (m_isInverted ? -value : value) * 1000.0; /* mA*/
+ status = m_impl->SetDemand(milliamperes);
+ } break;
+ default:
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "The CAN Talon does not support this control mode.");
+ break;
+ }
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+
+ status = m_impl->SetModeSelect(m_sendMode);
+
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ }
+}
+
+/**
+ * Sets the setpoint to value. Equivalent to Set().
+ */
+void CANTalon::SetSetpoint(float value) { Set(value); }
+
+/**
+ * Resets the integral term and disables the controller.
+ */
+void CANTalon::Reset() {
+ ClearIaccum();
+ Disable();
+}
+
+/**
+ * Disables control of the talon, causing the motor to brake or coast
+ * depending on its mode (see the Talon SRX Software Reference manual
+ * for more information).
+ */
+void CANTalon::Disable() {
+ m_impl->SetModeSelect((int)CANTalon::kDisabled);
+ m_controlEnabled = false;
+}
+
+/**
+ * Enables control of the Talon, allowing the motor to move.
+ */
+void CANTalon::EnableControl() {
+ SetControlMode(m_controlMode);
+ m_controlEnabled = true;
+}
+
+/**
+ * Enables control of the Talon, allowing the motor to move.
+ */
+void CANTalon::Enable() { EnableControl(); }
+
+/**
+ * @return Whether the Talon is currently enabled.
+ */
+bool CANTalon::IsControlEnabled() const { return m_controlEnabled; }
+
+/**
+ * @return Whether the Talon is currently enabled.
+ */
+bool CANTalon::IsEnabled() const { return IsControlEnabled(); }
+
+/**
+ * @param p Proportional constant to use in PID loop.
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+void CANTalon::SetP(double p) {
+ CTR_Code status = m_impl->SetPgain(m_profile, p);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * Set the integration constant of the currently selected profile.
+ *
+ * @param i Integration constant for the currently selected PID profile.
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+void CANTalon::SetI(double i) {
+ CTR_Code status = m_impl->SetIgain(m_profile, i);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * Set the derivative constant of the currently selected profile.
+ *
+ * @param d Derivative constant for the currently selected PID profile.
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+void CANTalon::SetD(double d) {
+ CTR_Code status = m_impl->SetDgain(m_profile, d);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Set the feedforward value of the currently selected profile.
+ *
+ * @param f Feedforward constant for the currently selected PID profile.
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+void CANTalon::SetF(double f) {
+ CTR_Code status = m_impl->SetFgain(m_profile, f);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Set the Izone to a nonzero value to auto clear the integral accumulator
+ * when the absolute value of CloseLoopError exceeds Izone.
+ *
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+void CANTalon::SetIzone(unsigned iz) {
+ CTR_Code status = m_impl->SetIzone(m_profile, iz);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * SRX has two available slots for PID.
+ * @param slotIdx one or zero depending on which slot caller wants.
+ */
+void CANTalon::SelectProfileSlot(int slotIdx) {
+ m_profile = (slotIdx == 0) ? 0 : 1; /* only get two slots for now */
+ CTR_Code status = m_impl->SetProfileSlotSelect(m_profile);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Sets control values for closed loop control.
+ *
+ * @param p Proportional constant.
+ * @param i Integration constant.
+ * @param d Differential constant.
+ * This function does not modify F-gain. Considerable passing a zero for f
+ * using
+ * the four-parameter function.
+ */
+void CANTalon::SetPID(double p, double i, double d) {
+ SetP(p);
+ SetI(i);
+ SetD(d);
+}
+/**
+ * Sets control values for closed loop control.
+ *
+ * @param p Proportional constant.
+ * @param i Integration constant.
+ * @param d Differential constant.
+ * @param f Feedforward constant.
+ */
+void CANTalon::SetPID(double p, double i, double d, double f) {
+ SetP(p);
+ SetI(i);
+ SetD(d);
+ SetF(f);
+}
+/**
+ * Select the feedback device to use in closed-loop
+ */
+void CANTalon::SetFeedbackDevice(FeedbackDevice feedbackDevice) {
+ /* save the selection so that future setters/getters know which scalars to apply */
+ m_feedbackDevice = feedbackDevice;
+ /* pass feedback to actual CAN frame */
+ CTR_Code status = m_impl->SetFeedbackDeviceSelect((int)feedbackDevice);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Select the feedback device to use in closed-loop
+ */
+void CANTalon::SetStatusFrameRateMs(StatusFrameRate stateFrame, int periodMs) {
+ CTR_Code status = m_impl->SetStatusFrameRate((int)stateFrame, periodMs);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * Get the current proportional constant.
+ *
+ * @return double proportional constant for current profile.
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+double CANTalon::GetP() const {
+ CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_P
+ : CanTalonSRX::eProfileParamSlot0_P;
+ // Update the info in m_impl.
+ CTR_Code status = m_impl->RequestParam(param);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ usleep(kDelayForSolicitedSignalsUs); /* small yield for getting response */
+ double p;
+ status = m_impl->GetPgain(m_profile, p);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return p;
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+double CANTalon::GetI() const {
+ CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_I
+ : CanTalonSRX::eProfileParamSlot0_I;
+ // Update the info in m_impl.
+ CTR_Code status = m_impl->RequestParam(param);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ usleep(kDelayForSolicitedSignalsUs); /* small yield for getting response */
+
+ double i;
+ status = m_impl->GetIgain(m_profile, i);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return i;
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+double CANTalon::GetD() const {
+ CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_D
+ : CanTalonSRX::eProfileParamSlot0_D;
+ // Update the info in m_impl.
+ CTR_Code status = m_impl->RequestParam(param);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ usleep(kDelayForSolicitedSignalsUs); /* small yield for getting response */
+
+ double d;
+ status = m_impl->GetDgain(m_profile, d);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return d;
+}
+/**
+ *
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+double CANTalon::GetF() const {
+ CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_F
+ : CanTalonSRX::eProfileParamSlot0_F;
+ // Update the info in m_impl.
+ CTR_Code status = m_impl->RequestParam(param);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+
+ usleep(kDelayForSolicitedSignalsUs); /* small yield for getting response */
+ double f;
+ status = m_impl->GetFgain(m_profile, f);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return f;
+}
+/**
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
+int CANTalon::GetIzone() const {
+ CanTalonSRX::param_t param = m_profile
+ ? CanTalonSRX::eProfileParamSlot1_IZone
+ : CanTalonSRX::eProfileParamSlot0_IZone;
+ // Update the info in m_impl.
+ CTR_Code status = m_impl->RequestParam(param);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ usleep(kDelayForSolicitedSignalsUs);
+
+ int iz;
+ status = m_impl->GetIzone(m_profile, iz);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return iz;
+}
+
+/**
+ * @return the current setpoint; ie, whatever was last passed to Set().
+ */
+double CANTalon::GetSetpoint() const { return m_setPoint; }
+
+/**
+ * Returns the voltage coming in from the battery.
+ *
+ * @return The input voltage in volts.
+ */
+float CANTalon::GetBusVoltage() const {
+ double voltage;
+ CTR_Code status = m_impl->GetBatteryV(voltage);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return voltage;
+}
+
+/**
+ * @return The voltage being output by the Talon, in Volts.
+ */
+float CANTalon::GetOutputVoltage() const {
+ int throttle11;
+ CTR_Code status = m_impl->GetAppliedThrottle(throttle11);
+ float voltage = GetBusVoltage() * (float(throttle11) / 1023.0);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return voltage;
+}
+
+/**
+ * Returns the current going through the Talon, in Amperes.
+ */
+float CANTalon::GetOutputCurrent() const {
+ double current;
+
+ CTR_Code status = m_impl->GetCurrent(current);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+
+ return current;
+}
+
+/**
+ * Returns temperature of Talon, in degrees Celsius.
+ */
+float CANTalon::GetTemperature() const {
+ double temp;
+
+ CTR_Code status = m_impl->GetTemp(temp);
+ if (temp != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return temp;
+}
+/**
+ * Set the position value of the selected sensor. This is useful for zero-ing
+ * quadrature encoders.
+ * Continuous sensors (like analog encoderes) can also partially be set (the
+ * portion of the postion based on overflows).
+ */
+void CANTalon::SetPosition(double pos) {
+ int32_t nativePos = ScaleRotationsToNativeUnits(m_feedbackDevice, pos);
+ CTR_Code status = m_impl->SetSensorPosition(nativePos);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ *
+ * @return The position of the sensor currently providing feedback.
+ * When using analog sensors, 0 units corresponds to 0V, 1023
+ * units corresponds to 3.3V
+ * When using an analog encoder (wrapping around 1023 => 0 is
+ * possible) the units are still 3.3V per 1023 units.
+ * When using quadrature, each unit is a quadrature edge (4X)
+ * mode.
+ */
+double CANTalon::GetPosition() const {
+ int32_t position;
+ CTR_Code status = m_impl->GetSensorPosition(position);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return ScaleNativeUnitsToRotations(m_feedbackDevice, position);
+}
+/**
+ * If sensor and motor are out of phase, sensor can be inverted
+ * (position and velocity multiplied by -1).
+ * @see GetPosition and @see GetSpeed.
+ */
+void CANTalon::SetSensorDirection(bool reverseSensor) {
+ CTR_Code status = m_impl->SetRevFeedbackSensor(reverseSensor ? 1 : 0);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Flips the sign (multiplies by negative one) the throttle values going into
+ * the motor on the talon in closed loop modes. Typically the application
+ * should use SetSensorDirection to keep sensor and motor in phase.
+ * @see SetSensorDirection
+ * However this routine is helpful for reversing the motor direction
+ * when Talon is in slave mode, or when using a single-direction position
+ * sensor in a closed-loop mode.
+ *
+ * @param reverseOutput True if motor output should be flipped; False if not.
+ */
+void CANTalon::SetClosedLoopOutputDirection(bool reverseOutput) {
+ CTR_Code status = m_impl->SetRevMotDuringCloseLoopEn(reverseOutput ? 1 : 0);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Returns the current error in the controller.
+ *
+ * @return the difference between the setpoint and the sensor value.
+ */
+int CANTalon::GetClosedLoopError() const {
+ int error;
+ /* retrieve the closed loop error in native units */
+ CTR_Code status = m_impl->GetCloseLoopErr(error);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return error;
+}
+/**
+ * Set the allowable closed loop error.
+ * @param allowableCloseLoopError allowable closed loop error for selected profile.
+ * mA for Curent closed loop.
+ * Talon Native Units for position and velocity.
+ */
+void CANTalon::SetAllowableClosedLoopErr(uint32_t allowableCloseLoopError)
+{
+ /* grab param enum */
+ CanTalonSRX::param_t param;
+ if (m_profile == 1) {
+ param = CanTalonSRX::eProfileParamSlot1_AllowableClosedLoopErr;
+ } else {
+ param = CanTalonSRX::eProfileParamSlot0_AllowableClosedLoopErr;
+ }
+ /* send allowable close loop er in native units */
+ ConfigSetParameter(param, allowableCloseLoopError);
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ *
+ * @returns The speed of the sensor currently providing feedback.
+ *
+ * The speed units will be in the sensor's native ticks per 100ms.
+ *
+ * For analog sensors, 3.3V corresponds to 1023 units.
+ * So a speed of 200 equates to ~0.645 dV per 100ms or 6.451 dV per
+ * second.
+ * If this is an analog encoder, that likely means 1.9548 rotations
+ * per sec.
+ * For quadrature encoders, each unit corresponds a quadrature edge (4X).
+ * So a 250 count encoder will produce 1000 edge events per
+ * rotation.
+ * An example speed of 200 would then equate to 20% of a rotation
+ * per 100ms,
+ * or 10 rotations per second.
+ */
+double CANTalon::GetSpeed() const {
+ int32_t speed;
+ CTR_Code status = m_impl->GetSensorVelocity(speed);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return ScaleNativeUnitsToRpm(m_feedbackDevice, speed);
+}
+
+/**
+ * Get the position of whatever is in the analog pin of the Talon, regardless of
+ * whether it is actually being used for feedback.
+ *
+ * @returns The 24bit analog value. The bottom ten bits is the ADC (0 - 1023)
+ * on the analog pin of the Talon.
+ * The upper 14 bits tracks the overflows and
+ * underflows (continuous sensor).
+ */
+int CANTalon::GetAnalogIn() const {
+ int position;
+ CTR_Code status = m_impl->GetAnalogInWithOv(position);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return position;
+}
+
+void CANTalon::SetAnalogPosition(int newPosition) {
+ CTR_Code status = m_impl->SetParam(CanTalonSRX::eAinPosition, newPosition);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Get the position of whatever is in the analog pin of the Talon, regardless of
+ * whether it is actually being used for feedback.
+ *
+ * @returns The ADC (0 - 1023) on analog pin of the Talon.
+ */
+int CANTalon::GetAnalogInRaw() const { return GetAnalogIn() & 0x3FF; }
+/**
+ * Get the position of whatever is in the analog pin of the Talon, regardless of
+ * whether it is actually being used for feedback.
+ *
+ * @returns The value (0 - 1023) on the analog pin of the Talon.
+ */
+int CANTalon::GetAnalogInVel() const {
+ int vel;
+ CTR_Code status = m_impl->GetAnalogInVel(vel);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return vel;
+}
+
+/**
+ * Get the position of whatever is in the analog pin of the Talon, regardless of
+ * whether it is actually being used for feedback.
+ *
+ * @returns The value (0 - 1023) on the analog pin of the Talon.
+ */
+int CANTalon::GetEncPosition() const {
+ int position;
+ CTR_Code status = m_impl->GetEncPosition(position);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return position;
+}
+void CANTalon::SetEncPosition(int newPosition) {
+ CTR_Code status = m_impl->SetParam(CanTalonSRX::eEncPosition, newPosition);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * Get the position of whatever is in the analog pin of the Talon, regardless of
+ * whether it is actually being used for feedback.
+ *
+ * @returns The value (0 - 1023) on the analog pin of the Talon.
+ */
+int CANTalon::GetEncVel() const {
+ int vel;
+ CTR_Code status = m_impl->GetEncVel(vel);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return vel;
+}
+int CANTalon::GetPulseWidthPosition() const {
+ int param;
+ CTR_Code status = m_impl->GetPulseWidthPosition(param);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return param;
+}
+void CANTalon::SetPulseWidthPosition(int newPosition)
+{
+ CTR_Code status = m_impl->SetParam(CanTalonSRX::ePwdPosition, newPosition);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+int CANTalon::GetPulseWidthVelocity()const
+{
+ int param;
+ CTR_Code status = m_impl->GetPulseWidthVelocity(param);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return param;
+}
+int CANTalon::GetPulseWidthRiseToFallUs()const
+{
+ int param;
+ CTR_Code status = m_impl->GetPulseWidthRiseToFallUs(param);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return param;
+}
+int CANTalon::GetPulseWidthRiseToRiseUs()const
+{
+ int param;
+ CTR_Code status = m_impl->GetPulseWidthRiseToRiseUs(param);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return param;
+}
+/**
+ * @param which feedback sensor to check it if is connected.
+ * @return status of caller's specified sensor type.
+ */
+CANTalon::FeedbackDeviceStatus CANTalon::IsSensorPresent(FeedbackDevice feedbackDevice)const
+{
+ FeedbackDeviceStatus retval = FeedbackStatusUnknown;
+ int param;
+ /* detecting sensor health depends on which sensor caller cares about */
+ switch (feedbackDevice) {
+ case QuadEncoder:
+ case AnalogPot:
+ case AnalogEncoder:
+ case EncRising:
+ case EncFalling:
+ /* no real good way to tell if these sensor
+ are actually present so return status unknown. */
+ break;
+ case PulseWidth:
+ case CtreMagEncoder_Relative:
+ case CtreMagEncoder_Absolute:
+ /* all of these require pulse width signal to be present. */
+ CTR_Code status = m_impl->IsPulseWidthSensorPresent(param);
+ if (status != CTR_OKAY) {
+ /* we're not getting status info, signal unknown status */
+ } else {
+ /* param is updated */
+ if (param) {
+ /* pulse signal is present, sensor must be working since it always
+ generates a pulse waveform.*/
+ retval = FeedbackStatusPresent;
+ } else {
+ /* no pulse present, sensor disconnected */
+ retval = FeedbackStatusNotPresent;
+ }
+ }
+ break;
+ }
+ return retval;
+}
+/**
+ * @return IO level of QUADA pin.
+ */
+int CANTalon::GetPinStateQuadA() const {
+ int retval;
+ CTR_Code status = m_impl->GetQuadApin(retval);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return retval;
+}
+/**
+ * @return IO level of QUADB pin.
+ */
+int CANTalon::GetPinStateQuadB() const {
+ int retval;
+ CTR_Code status = m_impl->GetQuadBpin(retval);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return retval;
+}
+/**
+ * @return IO level of QUAD Index pin.
+ */
+int CANTalon::GetPinStateQuadIdx() const {
+ int retval;
+ CTR_Code status = m_impl->GetQuadIdxpin(retval);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return retval;
+}
+/**
+ * @return '1' iff forward limit switch is closed, 0 iff switch is open.
+ * This function works regardless if limit switch feature is enabled.
+ */
+int CANTalon::IsFwdLimitSwitchClosed() const {
+ int retval;
+ CTR_Code status = m_impl->GetLimitSwitchClosedFor(
+ retval); /* rename this func, '1' => open, '0' => closed */
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return retval ? 0 : 1;
+}
+/**
+ * @return '1' iff reverse limit switch is closed, 0 iff switch is open.
+ * This function works regardless if limit switch feature is enabled.
+ */
+int CANTalon::IsRevLimitSwitchClosed() const {
+ int retval;
+ CTR_Code status = m_impl->GetLimitSwitchClosedRev(
+ retval); /* rename this func, '1' => open, '0' => closed */
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return retval ? 0 : 1;
+}
+/*
+ * Simple accessor for tracked rise eventso index pin.
+ * @return number of rising edges on idx pin.
+ */
+int CANTalon::GetNumberOfQuadIdxRises() const {
+ int rises;
+ CTR_Code status = m_impl->GetEncIndexRiseEvents(
+ rises); /* rename this func, '1' => open, '0' => closed */
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return rises;
+}
+/*
+ * @param rises integral value to set into index-rises register. Great way to
+ * zero the index count.
+ */
+void CANTalon::SetNumberOfQuadIdxRises(int rises) {
+ CTR_Code status = m_impl->SetParam(
+ CanTalonSRX::eEncIndexRiseEvents,
+ rises); /* rename this func, '1' => open, '0' => closed */
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+bool CANTalon::GetForwardLimitOK() const {
+ int limSwit = 0;
+ int softLim = 0;
+ CTR_Code status = CTR_OKAY;
+ status = m_impl->GetFault_ForSoftLim(softLim);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ status = m_impl->GetFault_ForLim(limSwit);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ /* If either fault is asserted, signal caller we are disabled (with false?) */
+ return (softLim | limSwit) ? false : true;
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+bool CANTalon::GetReverseLimitOK() const {
+ int limSwit = 0;
+ int softLim = 0;
+ CTR_Code status = CTR_OKAY;
+ status = m_impl->GetFault_RevSoftLim(softLim);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ status = m_impl->GetFault_RevLim(limSwit);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ /* If either fault is asserted, signal caller we are disabled (with false?) */
+ return (softLim | limSwit) ? false : true;
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+uint16_t CANTalon::GetFaults() const {
+ uint16_t retval = 0;
+ int val;
+ CTR_Code status = CTR_OKAY;
+
+ /* temperature */
+ val = 0;
+ status = m_impl->GetFault_OverTemp(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kTemperatureFault : 0;
+
+ /* voltage */
+ val = 0;
+ status = m_impl->GetFault_UnderVoltage(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kBusVoltageFault : 0;
+
+ /* fwd-limit-switch */
+ val = 0;
+ status = m_impl->GetFault_ForLim(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kFwdLimitSwitch : 0;
+
+ /* rev-limit-switch */
+ val = 0;
+ status = m_impl->GetFault_RevLim(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kRevLimitSwitch : 0;
+
+ /* fwd-soft-limit */
+ val = 0;
+ status = m_impl->GetFault_ForSoftLim(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kFwdSoftLimit : 0;
+
+ /* rev-soft-limit */
+ val = 0;
+ status = m_impl->GetFault_RevSoftLim(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kRevSoftLimit : 0;
+
+ return retval;
+}
+uint16_t CANTalon::GetStickyFaults() const {
+ uint16_t retval = 0;
+ int val;
+ CTR_Code status = CTR_OKAY;
+
+ /* temperature */
+ val = 0;
+ status = m_impl->GetStckyFault_OverTemp(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kTemperatureFault : 0;
+
+ /* voltage */
+ val = 0;
+ status = m_impl->GetStckyFault_UnderVoltage(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kBusVoltageFault : 0;
+
+ /* fwd-limit-switch */
+ val = 0;
+ status = m_impl->GetStckyFault_ForLim(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kFwdLimitSwitch : 0;
+
+ /* rev-limit-switch */
+ val = 0;
+ status = m_impl->GetStckyFault_RevLim(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kRevLimitSwitch : 0;
+
+ /* fwd-soft-limit */
+ val = 0;
+ status = m_impl->GetStckyFault_ForSoftLim(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kFwdSoftLimit : 0;
+
+ /* rev-soft-limit */
+ val = 0;
+ status = m_impl->GetStckyFault_RevSoftLim(val);
+ if (status != CTR_OKAY)
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval |= (val) ? CANSpeedController::kRevSoftLimit : 0;
+
+ return retval;
+}
+void CANTalon::ClearStickyFaults() {
+ CTR_Code status = m_impl->ClearStickyFaults();
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the maximum voltage change rate. This ramp rate is in affect regardless
+ * of which control mode
+ * the TALON is in.
+ *
+ * When in PercentVbus or Voltage output mode, the rate at which the voltage
+ * changes can
+ * be limited to reduce current spikes. Set this to 0.0 to disable rate
+ * limiting.
+ *
+ * @param rampRate The maximum rate of voltage change in Percent Voltage mode in
+ * V/s.
+ */
+void CANTalon::SetVoltageRampRate(double rampRate) {
+ /* Caller is expressing ramp as Voltage per sec, assuming 12V is full.
+ Talon's throttle ramp is in dThrot/d10ms. 1023 is full fwd, -1023 is
+ full rev. */
+ double rampRatedThrotPer10ms = (rampRate * 1023.0 / 12.0) / 100;
+ CTR_Code status = m_impl->SetRampThrottle((int)rampRatedThrotPer10ms);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+void CANTalon::SetVoltageCompensationRampRate(double rampRate) {
+ /* when in voltage compensation mode, the voltage compensation rate
+ directly caps the change in target voltage */
+ CTR_Code status = CTR_OKAY;
+ status = m_impl->SetVoltageCompensationRate(rampRate / 1000);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Sets a voltage change rate that applies only when a close loop contorl mode
+ * is enabled.
+ * This allows close loop specific ramp behavior.
+ *
+ * @param rampRate The maximum rate of voltage change in Percent Voltage mode in
+ * V/s.
+ */
+void CANTalon::SetCloseLoopRampRate(double rampRate) {
+ CTR_Code status = m_impl->SetCloseLoopRampRate(
+ m_profile, rampRate * 1023.0 / 12.0 / 1000.0);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * @return The version of the firmware running on the Talon
+ */
+uint32_t CANTalon::GetFirmwareVersion() const {
+ int firmwareVersion;
+ CTR_Code status = m_impl->RequestParam(CanTalonSRX::eFirmVers);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ usleep(kDelayForSolicitedSignalsUs);
+ status =
+ m_impl->GetParamResponseInt32(CanTalonSRX::eFirmVers, firmwareVersion);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+
+ /* only sent once on boot */
+ // CTR_Code status = m_impl->GetFirmVers(firmwareVersion);
+ // if (status != CTR_OKAY) {
+ // wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ //}
+
+ return firmwareVersion;
+}
+/**
+ * @return The accumulator for I gain.
+ */
+int CANTalon::GetIaccum() const {
+ CTR_Code status = m_impl->RequestParam(CanTalonSRX::ePidIaccum);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ usleep(kDelayForSolicitedSignalsUs); /* small yield for getting response */
+ int iaccum;
+ status = m_impl->GetParamResponseInt32(CanTalonSRX::ePidIaccum, iaccum);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return iaccum;
+}
+/**
+ * Clear the accumulator for I gain.
+ */
+void CANTalon::ClearIaccum() {
+ CTR_Code status = m_impl->SetParam(CanTalonSRX::ePidIaccum, 0);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+void CANTalon::ConfigNeutralMode(NeutralMode mode) {
+ CTR_Code status = CTR_OKAY;
+ switch (mode) {
+ default:
+ case kNeutralMode_Jumper: /* use default setting in flash based on
+ webdash/BrakeCal button selection */
+ status = m_impl->SetOverrideBrakeType(
+ CanTalonSRX::kBrakeOverride_UseDefaultsFromFlash);
+ break;
+ case kNeutralMode_Brake:
+ status = m_impl->SetOverrideBrakeType(
+ CanTalonSRX::kBrakeOverride_OverrideBrake);
+ break;
+ case kNeutralMode_Coast:
+ status = m_impl->SetOverrideBrakeType(
+ CanTalonSRX::kBrakeOverride_OverrideCoast);
+ break;
+ }
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * @return nonzero if brake is enabled during neutral. Zero if coast is enabled
+ * during neutral.
+ */
+int CANTalon::GetBrakeEnableDuringNeutral() const {
+ int brakeEn = 0;
+ CTR_Code status = m_impl->GetBrakeIsEnabled(brakeEn);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return brakeEn;
+}
+/**
+ * Configure how many codes per revolution are generated by your encoder.
+ *
+ * @param codesPerRev The number of counts per revolution.
+ */
+void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev) {
+ /* first save the scalar so that all getters/setter work as the user expects */
+ m_codesPerRev = codesPerRev;
+ /* next send the scalar to the Talon over CAN. This is so that the Talon can report
+ it to whoever needs it, like the webdash. Don't bother checking the return,
+ this is only for instrumentation and is not necessary for Talon functionality. */
+ (void)m_impl->SetParam(CanTalonSRX::eNumberEncoderCPR, m_codesPerRev);
+}
+
+/**
+ * Configure the number of turns on the potentiometer.
+ *
+ * @param turns The number of turns of the potentiometer.
+ */
+void CANTalon::ConfigPotentiometerTurns(uint16_t turns) {
+ /* first save the scalar so that all getters/setter work as the user expects */
+ m_numPotTurns = turns;
+ /* next send the scalar to the Talon over CAN. This is so that the Talon can report
+ it to whoever needs it, like the webdash. Don't bother checking the return,
+ this is only for instrumentation and is not necessary for Talon functionality. */
+ (void)m_impl->SetParam(CanTalonSRX::eNumberPotTurns, m_numPotTurns);
+}
+
+/**
+ * @deprecated not implemented
+ */
+void CANTalon::ConfigSoftPositionLimits(double forwardLimitPosition,
+ double reverseLimitPosition) {
+ ConfigLimitMode(kLimitMode_SoftPositionLimits);
+ ConfigForwardLimit(forwardLimitPosition);
+ ConfigReverseLimit(reverseLimitPosition);
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+void CANTalon::DisableSoftPositionLimits() {
+ ConfigLimitMode(kLimitMode_SwitchInputsOnly);
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ * Configures the soft limit enable (wear leveled persistent memory).
+ * Also sets the limit switch overrides.
+ */
+void CANTalon::ConfigLimitMode(LimitMode mode) {
+ CTR_Code status = CTR_OKAY;
+ switch (mode) {
+ case kLimitMode_SwitchInputsOnly: /** Only use switches for limits */
+ /* turn OFF both limits. SRX has individual enables and polarity for each
+ * limit switch.*/
+ status = m_impl->SetForwardSoftEnable(false);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ status = m_impl->SetReverseSoftEnable(false);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ /* override enable the limit switches, this circumvents the webdash */
+ status = m_impl->SetOverrideLimitSwitchEn(
+ CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ break;
+ case kLimitMode_SoftPositionLimits: /** Use both switches and soft limits */
+ /* turn on both limits. SRX has individual enables and polarity for each
+ * limit switch.*/
+ status = m_impl->SetForwardSoftEnable(true);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ status = m_impl->SetReverseSoftEnable(true);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ /* override enable the limit switches, this circumvents the webdash */
+ status = m_impl->SetOverrideLimitSwitchEn(
+ CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ break;
+
+ case kLimitMode_SrxDisableSwitchInputs: /** disable both limit switches and
+ soft limits */
+ /* turn on both limits. SRX has individual enables and polarity for each
+ * limit switch.*/
+ status = m_impl->SetForwardSoftEnable(false);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ status = m_impl->SetReverseSoftEnable(false);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ /* override enable the limit switches, this circumvents the webdash */
+ status = m_impl->SetOverrideLimitSwitchEn(
+ CanTalonSRX::kLimitSwitchOverride_DisableFwd_DisableRev);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ break;
+ }
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+void CANTalon::ConfigForwardLimit(double forwardLimitPosition) {
+ CTR_Code status = CTR_OKAY;
+ int32_t nativeLimitPos = ScaleRotationsToNativeUnits(m_feedbackDevice, forwardLimitPosition);
+ status = m_impl->SetForwardSoftLimit(nativeLimitPos);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Change the fwd limit switch setting to normally open or closed.
+ * Talon will disable momentarilly if the Talon's current setting
+ * is dissimilar to the caller's requested setting.
+ *
+ * Since Talon saves setting to flash this should only affect
+ * a given Talon initially during robot install.
+ *
+ * @param normallyOpen true for normally open. false for normally closed.
+ */
+void CANTalon::ConfigFwdLimitSwitchNormallyOpen(bool normallyOpen) {
+ CTR_Code status =
+ m_impl->SetParam(CanTalonSRX::eOnBoot_LimitSwitch_Forward_NormallyClosed,
+ normallyOpen ? 0 : 1);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * Change the rev limit switch setting to normally open or closed.
+ * Talon will disable momentarilly if the Talon's current setting
+ * is dissimilar to the caller's requested setting.
+ *
+ * Since Talon saves setting to flash this should only affect
+ * a given Talon initially during robot install.
+ *
+ * @param normallyOpen true for normally open. false for normally closed.
+ */
+void CANTalon::ConfigRevLimitSwitchNormallyOpen(bool normallyOpen) {
+ CTR_Code status =
+ m_impl->SetParam(CanTalonSRX::eOnBoot_LimitSwitch_Reverse_NormallyClosed,
+ normallyOpen ? 0 : 1);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+void CANTalon::ConfigReverseLimit(double reverseLimitPosition) {
+ CTR_Code status = CTR_OKAY;
+ int32_t nativeLimitPos = ScaleRotationsToNativeUnits(m_feedbackDevice, reverseLimitPosition);
+ status = m_impl->SetReverseSoftLimit(nativeLimitPos);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+void CANTalon::ConfigMaxOutputVoltage(double voltage) {
+ /* config peak throttle when in closed-loop mode in the fwd and rev direction. */
+ ConfigPeakOutputVoltage(voltage, -voltage);
+}
+void CANTalon::ConfigPeakOutputVoltage(double forwardVoltage,double reverseVoltage) {
+ /* bounds checking */
+ if (forwardVoltage > 12)
+ forwardVoltage = 12;
+ else if (forwardVoltage < 0)
+ forwardVoltage = 0;
+ if (reverseVoltage > 0)
+ reverseVoltage = 0;
+ else if (reverseVoltage < -12)
+ reverseVoltage = -12;
+ /* config calls */
+ ConfigSetParameter(CanTalonSRX::ePeakPosOutput, 1023 * forwardVoltage / 12.0);
+ ConfigSetParameter(CanTalonSRX::ePeakNegOutput, 1023 * reverseVoltage / 12.0);
+}
+void CANTalon::ConfigNominalOutputVoltage(double forwardVoltage,double reverseVoltage) {
+ /* bounds checking */
+ if (forwardVoltage > 12)
+ forwardVoltage = 12;
+ else if (forwardVoltage < 0)
+ forwardVoltage = 0;
+ if (reverseVoltage > 0)
+ reverseVoltage = 0;
+ else if (reverseVoltage < -12)
+ reverseVoltage = -12;
+ /* config calls */
+ ConfigSetParameter(CanTalonSRX::eNominalPosOutput,1023*forwardVoltage/12.0);
+ ConfigSetParameter(CanTalonSRX::eNominalNegOutput,1023*reverseVoltage/12.0);
+}
+/**
+ * General set frame. Since the parameter is a general integral type, this can
+ * be used for testing future features.
+ */
+void CANTalon::ConfigSetParameter(uint32_t paramEnum, double value) {
+ CTR_Code status;
+ /* config peak throttle when in closed-loop mode in the positive direction. */
+ status = m_impl->SetParam((CanTalonSRX::param_t)paramEnum,value);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * General get frame. Since the parameter is a general integral type, this can
+ * be used for testing future features.
+ */
+bool CANTalon::GetParameter(uint32_t paramEnum, double & dvalue) const {
+ bool retval = true;
+ /* send the request frame */
+ CTR_Code status = m_impl->RequestParam((CanTalonSRX::param_t)paramEnum);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval = false;
+ }
+ /* small yield for getting response */
+ usleep(kDelayForSolicitedSignalsUs);
+ /* get the last received update */
+ status = m_impl->GetParamResponse((CanTalonSRX::param_t)paramEnum, dvalue);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ retval = false;
+ }
+ return retval;
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+void CANTalon::ConfigFaultTime(float faultTime) {
+ /* SRX does not have fault time. SRX motor drive is only disabled for soft
+ * limits and limit-switch faults. */
+ wpi_setWPIErrorWithContext(IncompatibleMode, "Fault Time not supported.");
+}
+
+/**
+ * Fixup the sendMode so Set() serializes the correct demand value.
+ * Also fills the modeSelecet in the control frame to disabled.
+ * @param mode Control mode to ultimately enter once user calls Set().
+ * @see Set()
+ */
+void CANTalon::ApplyControlMode(CANSpeedController::ControlMode mode) {
+ m_controlMode = mode;
+ HALReport(HALUsageReporting::kResourceType_CANTalonSRX, m_deviceNumber + 1,
+ mode);
+ switch (mode) {
+ case kPercentVbus:
+ m_sendMode = kThrottle;
+ break;
+ case kCurrent:
+ m_sendMode = kCurrentMode;
+ break;
+ case kSpeed:
+ m_sendMode = kSpeedMode;
+ break;
+ case kPosition:
+ m_sendMode = kPositionMode;
+ break;
+ case kVoltage:
+ m_sendMode = kVoltageMode;
+ break;
+ case kFollower:
+ m_sendMode = kFollowerMode;
+ break;
+ }
+ // Keep the talon disabled until Set() is called.
+ CTR_Code status = m_impl->SetModeSelect((int)kDisabled);
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+void CANTalon::SetControlMode(CANSpeedController::ControlMode mode) {
+ if (m_controlMode == mode) {
+ /* we already are in this mode, don't perform disable workaround */
+ } else {
+ ApplyControlMode(mode);
+ }
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+CANSpeedController::ControlMode CANTalon::GetControlMode() const {
+ return m_controlMode;
+}
+
+void CANTalon::SetExpiration(float timeout) {
+ m_safetyHelper->SetExpiration(timeout);
+}
+
+float CANTalon::GetExpiration() const {
+ return m_safetyHelper->GetExpiration();
+}
+
+bool CANTalon::IsAlive() const { return m_safetyHelper->IsAlive(); }
+
+bool CANTalon::IsSafetyEnabled() const {
+ return m_safetyHelper->IsSafetyEnabled();
+}
+
+void CANTalon::SetSafetyEnabled(bool enabled) {
+ m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+void CANTalon::GetDescription(std::ostringstream& desc) const {
+ desc << "CANTalon ID " << m_deviceNumber;
+}
+/**
+ * @param devToLookup FeedbackDevice to lookup the scalar for. Because Talon
+ * allows multiple sensors to be attached simultaneously, caller must
+ * specify which sensor to lookup.
+ * @return The number of native Talon units per rotation of the selected sensor.
+ * Zero if the necessary sensor information is not available.
+ * @see ConfigEncoderCodesPerRev
+ * @see ConfigPotentiometerTurns
+ */
+double CANTalon::GetNativeUnitsPerRotationScalar(FeedbackDevice devToLookup)const
+{
+ bool scalingAvail = false;
+ CTR_Code status = CTR_OKAY;
+ double retval = 0;
+ switch (devToLookup) {
+ case QuadEncoder:
+ { /* When caller wants to lookup Quadrature, the QEI may be in 1x if the selected feedback is edge counter.
+ * Additionally if the quadrature source is the CTRE Mag encoder, then the CPR is known.
+ * This is nice in that the calling app does not require knowing the CPR at all.
+ * So do both checks here.
+ */
+ int32_t qeiPulsePerCount = 4; /* default to 4x */
+ switch (m_feedbackDevice) {
+ case CtreMagEncoder_Relative:
+ case CtreMagEncoder_Absolute:
+ /* we assume the quadrature signal comes from the MagEnc,
+ of which we know the CPR already */
+ retval = kNativePwdUnitsPerRotation;
+ scalingAvail = true;
+ break;
+ case EncRising: /* Talon's QEI is setup for 1x, so perform 1x math */
+ case EncFalling:
+ qeiPulsePerCount = 1;
+ break;
+ case QuadEncoder: /* Talon's QEI is 4x */
+ default: /* pulse width and everything else, assume its regular quad use. */
+ break;
+ }
+ if (scalingAvail) {
+ /* already deduced the scalar above, we're done. */
+ } else {
+ /* we couldn't deduce the scalar just based on the selection */
+ if (0 == m_codesPerRev) {
+ /* caller has never set the CPR. Most likely caller
+ is just using engineering units so fall to the
+ bottom of this func.*/
+ } else {
+ /* Talon expects PPR units */
+ retval = qeiPulsePerCount * m_codesPerRev;
+ scalingAvail = true;
+ }
+ }
+ } break;
+ case EncRising:
+ case EncFalling:
+ if (0 == m_codesPerRev) {
+ /* caller has never set the CPR. Most likely caller
+ is just using engineering units so fall to the
+ bottom of this func.*/
+ } else {
+ /* Talon expects PPR units */
+ retval = 1 * m_codesPerRev;
+ scalingAvail = true;
+ }
+ break;
+ case AnalogPot:
+ case AnalogEncoder:
+ if (0 == m_numPotTurns) {
+ /* caller has never set the CPR. Most likely caller
+ is just using engineering units so fall to the
+ bottom of this func.*/
+ } else {
+ retval = (double)kNativeAdcUnitsPerRotation / m_numPotTurns;
+ scalingAvail = true;
+ }
+ break;
+ case CtreMagEncoder_Relative:
+ case CtreMagEncoder_Absolute:
+ case PulseWidth:
+ retval = kNativePwdUnitsPerRotation;
+ scalingAvail = true;
+ break;
+ }
+ /* handle any detected errors */
+ if (status != CTR_OKAY) {
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ /* if scaling information is not possible, signal caller
+ by returning zero */
+ if (false == scalingAvail)
+ retval = 0;
+ return retval;
+}
+/**
+ * @param fullRotations double precision value representing number of rotations of selected feedback sensor.
+ * If user has never called the config routine for the selected sensor, then the caller
+ * is likely passing rotations in engineering units already, in which case it is returned
+ * as is.
+ * @see ConfigPotentiometerTurns
+ * @see ConfigEncoderCodesPerRev
+ * @return fullRotations in native engineering units of the Talon SRX firmware.
+ */
+int32_t CANTalon::ScaleRotationsToNativeUnits(FeedbackDevice devToLookup,double fullRotations)const
+{
+ /* first assume we don't have config info, prep the default return */
+ int32_t retval = (int32_t)fullRotations;
+ /* retrieve scaling info */
+ double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
+ /* apply scalar if its available */
+ if (scalar > 0)
+ retval = (int32_t)(fullRotations*scalar);
+ return retval;
+}
+/**
+ * @param rpm double precision value representing number of rotations per minute of selected feedback sensor.
+ * If user has never called the config routine for the selected sensor, then the caller
+ * is likely passing rotations in engineering units already, in which case it is returned
+ * as is.
+ * @see ConfigPotentiometerTurns
+ * @see ConfigEncoderCodesPerRev
+ * @return sensor velocity in native engineering units of the Talon SRX firmware.
+ */
+int32_t CANTalon::ScaleVelocityToNativeUnits(FeedbackDevice devToLookup,double rpm)const
+{
+ /* first assume we don't have config info, prep the default return */
+ int32_t retval = (int32_t)rpm;
+ /* retrieve scaling info */
+ double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
+ /* apply scalar if its available */
+ if (scalar > 0)
+ retval = (int32_t)(rpm * kMinutesPer100msUnit * scalar);
+ return retval;
+}
+/**
+ * @param nativePos integral position of the feedback sensor in native Talon SRX units.
+ * If user has never called the config routine for the selected sensor, then the return
+ * will be in TALON SRX units as well to match the behavior in the 2015 season.
+ * @see ConfigPotentiometerTurns
+ * @see ConfigEncoderCodesPerRev
+ * @return double precision number of rotations, unless config was never performed.
+ */
+double CANTalon::ScaleNativeUnitsToRotations(FeedbackDevice devToLookup,int32_t nativePos)const
+{
+ /* first assume we don't have config info, prep the default return */
+ double retval = (double)nativePos;
+ /* retrieve scaling info */
+ double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
+ /* apply scalar if its available */
+ if (scalar > 0)
+ retval = ((double)nativePos) / scalar;
+ return retval;
+}
+/**
+ * @param nativeVel integral velocity of the feedback sensor in native Talon SRX units.
+ * If user has never called the config routine for the selected sensor, then the return
+ * will be in TALON SRX units as well to match the behavior in the 2015 season.
+ * @see ConfigPotentiometerTurns
+ * @see ConfigEncoderCodesPerRev
+ * @return double precision of sensor velocity in RPM, unless config was never performed.
+ */
+double CANTalon::ScaleNativeUnitsToRpm(FeedbackDevice devToLookup, int32_t nativeVel)const
+{
+ /* first assume we don't have config info, prep the default return */
+ double retval = (double)nativeVel;
+ /* retrieve scaling info */
+ double scalar = GetNativeUnitsPerRotationScalar(devToLookup);
+ /* apply scalar if its available */
+ if (scalar > 0)
+ retval = (double)(nativeVel) / (scalar*kMinutesPer100msUnit);
+ return retval;
+}
+
+/**
+ * Enables Talon SRX to automatically zero the Sensor Position whenever an
+ * edge is detected on the index signal.
+ * @param enable boolean input, pass true to enable feature or false to disable.
+ * @param risingEdge boolean input, pass true to clear the position on rising edge,
+ * pass false to clear the position on falling edge.
+ */
+void CANTalon::EnableZeroSensorPositionOnIndex(bool enable, bool risingEdge)
+{
+ if (enable) {
+ /* enable the feature, update the edge polarity first to ensure
+ it is correct before the feature is enabled. */
+ ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity,risingEdge ? 1 : 0);
+ ConfigSetParameter(CanTalonSRX::eClearPositionOnIdx,1);
+ } else {
+ /* disable the feature first, then update the edge polarity. */
+ ConfigSetParameter(CanTalonSRX::eClearPositionOnIdx,0);
+ ConfigSetParameter(CanTalonSRX::eQuadIdxPolarity,risingEdge ? 1 : 0);
+ }
+}
+/**
+* Common interface for inverting direction of a speed controller.
+* Only works in PercentVbus, speed, and Voltage modes.
+* @param isInverted The state of inversion, true is inverted.
+*/
+void CANTalon::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+/**
+ * Common interface for the inverting direction of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ *
+ */
+bool CANTalon::GetInverted() const { return m_isInverted; }
+
+/**
+ * Common interface for stopping the motor
+ * Part of the MotorSafety interface
+ *
+ * @deprecated Call Disable instead.
+*/
+void CANTalon::StopMotor() { Disable(); }
+
+void CANTalon::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if(key == "Mode" && value->IsDouble()) SetControlMode(static_cast<CANSpeedController::ControlMode>(value->GetDouble()));
+ if(key == "p" && value->IsDouble()) SetP(value->GetDouble());
+ if(key == "i" && value->IsDouble()) SetI(value->GetDouble());
+ if(key == "d" && value->IsDouble()) SetD(value->GetDouble());
+ if(key == "f" && value->IsDouble()) SetF(value->GetDouble());
+ if(key == "Enabled" && value->IsBoolean()) {
+ if (value->GetBoolean()) {
+ Enable();
+ } else {
+ Disable();
+ }
+ }
+ if(key == "Value" && value->IsDouble()) Set(value->GetDouble());
+}
+
+bool CANTalon::IsModePID(CANSpeedController::ControlMode mode) const {
+ return mode == kCurrent || mode == kSpeed || mode == kPosition;
+}
+
+void CANTalon::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutString("~TYPE~", "CANSpeedController");
+ m_table->PutString("Type", "CANTalon");
+ m_table->PutString("Mode", GetModeName(m_controlMode));
+ m_table->PutNumber("p", GetP());
+ m_table->PutNumber("i", GetI());
+ m_table->PutNumber("d", GetD());
+ m_table->PutNumber("f", GetF());
+ m_table->PutBoolean("Enabled", IsControlEnabled());
+ m_table->PutNumber("Value", Get());
+ }
+}
+
+void CANTalon::StartLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->AddTableListener(this, true);
+ }
+}
+
+void CANTalon::StopLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string CANTalon::GetSmartDashboardType() const {
+ return "CANSpeedController";
+}
+
+void CANTalon::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> CANTalon::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/CameraServer.cpp b/wpilibc/Athena/src/CameraServer.cpp
new file mode 100644
index 0000000..af319a4
--- /dev/null
+++ b/wpilibc/Athena/src/CameraServer.cpp
@@ -0,0 +1,260 @@
+#include "CameraServer.h"
+#include "WPIErrors.h"
+#include "Utility.h"
+
+#include <iostream>
+#include <chrono>
+#include <cstring>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <netdb.h>
+
+constexpr uint8_t CameraServer::kMagicNumber[];
+
+CameraServer* CameraServer::GetInstance() {
+ static CameraServer instance;
+ return &instance;
+}
+
+CameraServer::CameraServer()
+ : m_camera(),
+ m_serverThread(&CameraServer::Serve, this),
+ m_captureThread(),
+ m_imageMutex(),
+ m_newImageVariable(),
+ m_dataPool(3),
+ m_quality(50),
+ m_autoCaptureStarted(false),
+ m_hwClient(true),
+ m_imageData(nullptr, 0, 0, false) {
+ for (int i = 0; i < 3; i++) m_dataPool.push_back(new uint8_t[kMaxImageSize]);
+}
+
+void CameraServer::FreeImageData(
+ std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData) {
+ if (std::get<3>(imageData))
+ imaqDispose(std::get<0>(imageData));
+ else if (std::get<0>(imageData) != nullptr) {
+ std::lock_guard<priority_recursive_mutex> lock(m_imageMutex);
+ m_dataPool.push_back(std::get<0>(imageData));
+ }
+}
+
+void CameraServer::SetImageData(uint8_t* data, unsigned int size,
+ unsigned int start, bool imaqData) {
+ std::lock_guard<priority_recursive_mutex> lock(m_imageMutex);
+ FreeImageData(m_imageData);
+ m_imageData = std::make_tuple(data, size, start, imaqData);
+ m_newImageVariable.notify_all();
+}
+
+void CameraServer::SetImage(Image const* image) {
+ unsigned int dataSize = 0;
+ uint8_t* data =
+ (uint8_t*)imaqFlatten(image, IMAQ_FLATTEN_IMAGE, IMAQ_COMPRESSION_JPEG,
+ 10 * m_quality, &dataSize);
+
+ // If we're using a HW camera, then find the start of the data
+ bool hwClient;
+ {
+ // Make a local copy of the hwClient variable so that we can safely use it.
+ std::lock_guard<priority_recursive_mutex> lock(m_imageMutex);
+ hwClient = m_hwClient;
+ }
+ unsigned int start = 0;
+ if (hwClient) {
+ while (start < dataSize - 1) {
+ if (data[start] == 0xFF && data[start + 1] == 0xD8)
+ break;
+ else
+ start++;
+ }
+ }
+ dataSize -= start;
+
+ wpi_assert(dataSize > 2);
+ SetImageData(data, dataSize, start, true);
+}
+
+void CameraServer::AutoCapture() {
+ Image* frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
+
+ while (true) {
+ bool hwClient;
+ uint8_t* data = nullptr;
+ {
+ std::lock_guard<priority_recursive_mutex> lock(m_imageMutex);
+ hwClient = m_hwClient;
+ if (hwClient) {
+ data = m_dataPool.back();
+ m_dataPool.pop_back();
+ }
+ }
+
+ if (hwClient) {
+ unsigned int size = m_camera->GetImageData(data, kMaxImageSize);
+ SetImageData(data, size);
+ } else {
+ m_camera->GetImage(frame);
+ SetImage(frame);
+ }
+ }
+}
+
+void CameraServer::StartAutomaticCapture(char const* cameraName) {
+ std::shared_ptr<USBCamera> camera =
+ std::make_shared<USBCamera>(cameraName, true);
+ camera->OpenCamera();
+ StartAutomaticCapture(camera);
+}
+
+void CameraServer::StartAutomaticCapture(std::shared_ptr<USBCamera> camera) {
+ std::lock_guard<priority_recursive_mutex> lock(m_imageMutex);
+ if (m_autoCaptureStarted) return;
+
+ m_camera = camera;
+ m_camera->StartCapture();
+
+ m_captureThread = std::thread(&CameraServer::AutoCapture, this);
+ m_captureThread.detach();
+ m_autoCaptureStarted = true;
+}
+
+bool CameraServer::IsAutoCaptureStarted() {
+ std::lock_guard<priority_recursive_mutex> lock(m_imageMutex);
+ return m_autoCaptureStarted;
+}
+
+void CameraServer::SetSize(unsigned int size) {
+ std::lock_guard<priority_recursive_mutex> lock(m_imageMutex);
+ if (!m_camera) return;
+ if (size == kSize160x120)
+ m_camera->SetSize(160, 120);
+ else if (size == kSize320x240)
+ m_camera->SetSize(320, 240);
+ else if (size == kSize640x480)
+ m_camera->SetSize(640, 480);
+}
+
+void CameraServer::SetQuality(unsigned int quality) {
+ std::lock_guard<priority_recursive_mutex> lock(m_imageMutex);
+ m_quality = quality > 100 ? 100 : quality;
+}
+
+unsigned int CameraServer::GetQuality() {
+ std::lock_guard<priority_recursive_mutex> lock(m_imageMutex);
+ return m_quality;
+}
+
+void CameraServer::Serve() {
+ int sock = socket(AF_INET, SOCK_STREAM, 0);
+
+ if (sock == -1) {
+ wpi_setErrnoError();
+ return;
+ }
+
+ int reuseAddr = 1;
+ if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseAddr,
+ sizeof(reuseAddr)) == -1)
+ wpi_setErrnoError();
+
+ sockaddr_in address, clientAddress;
+
+ memset(&address, 0, sizeof(address));
+ address.sin_family = AF_INET;
+ address.sin_addr.s_addr = htonl(INADDR_ANY);
+ address.sin_port = htons(kPort);
+
+ if (bind(sock, (struct sockaddr*)&address, sizeof(address)) == -1)
+ wpi_setErrnoError();
+
+ if (listen(sock, 10) == -1) wpi_setErrnoError();
+
+ while (true) {
+ socklen_t clientAddressLen = sizeof(clientAddress);
+
+ int conn =
+ accept(sock, (struct sockaddr*)&clientAddress, &clientAddressLen);
+ if (conn == -1) {
+ wpi_setErrnoError();
+ continue;
+ }
+
+ Request req;
+ if (read(conn, &req, sizeof(req)) == -1) {
+ wpi_setErrnoError();
+ close(conn);
+ continue;
+ } else {
+ req.fps = ntohl(req.fps);
+ req.compression = ntohl(req.compression);
+ req.size = ntohl(req.size);
+ }
+
+ // TODO: Support the SW Compression. The rest of the code below will work as
+ // though this
+ // check isn't here
+ if (req.compression != kHardwareCompression) {
+ wpi_setWPIErrorWithContext(IncompatibleState,
+ "Choose \"USB Camera HW\" on the dashboard");
+ close(conn);
+ continue;
+ }
+
+ {
+ // Wait for the camera to be setw
+ std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
+ if (!m_camera) {
+ std::cout << "Camera not yet ready, awaiting first image" << std::endl;
+ m_newImageVariable.wait(lock);
+ }
+ m_hwClient = req.compression == kHardwareCompression;
+ if (!m_hwClient)
+ SetQuality(100 - req.compression);
+ else if (m_camera)
+ m_camera->SetFPS(req.fps);
+ SetSize(req.size);
+ }
+
+ auto period = std::chrono::microseconds(1000000) / req.fps;
+ while (true) {
+ auto startTime = std::chrono::steady_clock::now();
+ std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData;
+ {
+ std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
+ m_newImageVariable.wait(lock);
+ imageData = m_imageData;
+ m_imageData = std::make_tuple<uint8_t*>(nullptr, 0, 0, false);
+ }
+
+ unsigned int size = std::get<1>(imageData);
+ unsigned int netSize = htonl(size);
+ unsigned int start = std::get<2>(imageData);
+ uint8_t* data = std::get<0>(imageData);
+
+ if (data == nullptr) continue;
+
+ if (write(conn, kMagicNumber, sizeof(kMagicNumber)) == -1) {
+ wpi_setErrnoErrorWithContext(
+ "[CameraServer] Error sending magic number");
+ FreeImageData(imageData);
+ break;
+ }
+ if (write(conn, &netSize, sizeof(netSize)) == -1) {
+ wpi_setErrnoErrorWithContext("[CameraServer] Error sending image size");
+ FreeImageData(imageData);
+ break;
+ }
+ if (write(conn, &data[start], sizeof(uint8_t) * size) == -1) {
+ wpi_setErrnoErrorWithContext("[CameraServer] Error sending image data");
+ FreeImageData(imageData);
+ break;
+ }
+ FreeImageData(imageData);
+ std::this_thread::sleep_until(startTime + period);
+ }
+ close(conn);
+ }
+ close(sock);
+}
diff --git a/wpilibc/Athena/src/Compressor.cpp b/wpilibc/Athena/src/Compressor.cpp
new file mode 100644
index 0000000..9ab89e0
--- /dev/null
+++ b/wpilibc/Athena/src/Compressor.cpp
@@ -0,0 +1,277 @@
+/*
+ * Compressor.cpp
+ */
+
+#include "Compressor.h"
+#include "WPIErrors.h"
+
+/**
+ * Constructor
+ *
+ * @param module The PCM ID to use (0-62)
+ */
+Compressor::Compressor(uint8_t pcmID) {
+ m_pcm_pointer = initializeCompressor(pcmID);
+ SetClosedLoopControl(true);
+}
+
+/**
+ * Starts closed-loop control. Note that closed loop control is enabled by
+ * default.
+ */
+void Compressor::Start() { SetClosedLoopControl(true); }
+
+/**
+ * Stops closed-loop control. Note that closed loop control is enabled by
+ * default.
+ */
+void Compressor::Stop() { SetClosedLoopControl(false); }
+
+/**
+ * Check if compressor output is active
+ * @return true if the compressor is on
+ */
+bool Compressor::Enabled() const {
+ int32_t status = 0;
+ bool value;
+
+ value = getCompressor(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+/**
+ * Check if the pressure switch is triggered
+ * @return true if pressure is low
+ */
+bool Compressor::GetPressureSwitchValue() const {
+ int32_t status = 0;
+ bool value;
+
+ value = getPressureSwitch(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+/**
+ * Query how much current the compressor is drawing
+ * @return The current through the compressor, in amps
+ */
+float Compressor::GetCompressorCurrent() const {
+ int32_t status = 0;
+ float value;
+
+ value = getCompressorCurrent(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+/**
+ * Enables or disables automatically turning the compressor on when the
+ * pressure is low.
+ * @param on Set to true to enable closed loop control of the compressor. False
+ * to disable.
+ */
+void Compressor::SetClosedLoopControl(bool on) {
+ int32_t status = 0;
+
+ setClosedLoopControl(m_pcm_pointer, on, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+}
+
+/**
+ * Returns true if the compressor will automatically turn on when the
+ * pressure is low.
+ * @return True if closed loop control of the compressor is enabled. False if
+ * disabled.
+ */
+bool Compressor::GetClosedLoopControl() const {
+ int32_t status = 0;
+ bool value;
+
+ value = getClosedLoopControl(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+
+/**
+ * Query if the compressor output has been disabled due to high current draw.
+ * @return true if PCM is in fault state : Compressor Drive is
+ * disabled due to compressor current being too high.
+ */
+bool Compressor::GetCompressorCurrentTooHighFault() const {
+ int32_t status = 0;
+ bool value;
+
+ value = getCompressorCurrentTooHighFault(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+/**
+ * Query if the compressor output has been disabled due to high current draw
+ * (sticky).
+ * A sticky fault will not clear on device reboot, it must be cleared through
+ * code or the webdash.
+ * @return true if PCM sticky fault is set : Compressor Drive is
+ * disabled due to compressor current being too high.
+ */
+bool Compressor::GetCompressorCurrentTooHighStickyFault() const {
+ int32_t status = 0;
+ bool value;
+
+ value = getCompressorCurrentTooHighStickyFault(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+/**
+ * Query if the compressor output has been disabled due to a short circuit
+ * (sticky).
+ * A sticky fault will not clear on device reboot, it must be cleared through
+ * code or the webdash.
+ * @return true if PCM sticky fault is set : Compressor output
+ * appears to be shorted.
+ */
+bool Compressor::GetCompressorShortedStickyFault() const {
+ int32_t status = 0;
+ bool value;
+
+ value = getCompressorShortedStickyFault(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+/**
+ * Query if the compressor output has been disabled due to a short circuit.
+ * @return true if PCM is in fault state : Compressor output
+ * appears to be shorted.
+ */
+bool Compressor::GetCompressorShortedFault() const {
+ int32_t status = 0;
+ bool value;
+
+ value = getCompressorShortedFault(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+/**
+ * Query if the compressor output does not appear to be wired (sticky).
+ * A sticky fault will not clear on device reboot, it must be cleared through
+ * code or the webdash.
+ * @return true if PCM sticky fault is set : Compressor does not
+ * appear to be wired, i.e. compressor is
+ * not drawing enough current.
+ */
+bool Compressor::GetCompressorNotConnectedStickyFault() const {
+ int32_t status = 0;
+ bool value;
+
+ value = getCompressorNotConnectedStickyFault(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+/**
+ * Query if the compressor output does not appear to be wired.
+ * @return true if PCM is in fault state : Compressor does not
+ * appear to be wired, i.e. compressor is
+ * not drawing enough current.
+ */
+bool Compressor::GetCompressorNotConnectedFault() const {
+ int32_t status = 0;
+ bool value;
+
+ value = getCompressorNotConnectedFault(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+
+ return value;
+}
+/**
+ * Clear ALL sticky faults inside PCM that Compressor is wired to.
+ *
+ * If a sticky fault is set, then it will be persistently cleared. Compressor
+ * drive
+ * maybe momentarily disable while flags are being cleared. Care
+ * should be
+ * taken to not call this too frequently, otherwise normal
+ * compressor
+ * functionality may be prevented.
+ *
+ * If no sticky faults are set then this call will have no effect.
+ */
+void Compressor::ClearAllPCMStickyFaults() {
+ int32_t status = 0;
+
+ clearAllPCMStickyFaults(m_pcm_pointer, &status);
+
+ if (status) {
+ wpi_setWPIError(Timeout);
+ }
+}
+void Compressor::UpdateTable() {
+ if (m_table) {
+ m_table->PutBoolean("Enabled", Enabled());
+ m_table->PutBoolean("Pressure switch", GetPressureSwitchValue());
+ }
+}
+
+void Compressor::StartLiveWindowMode() {}
+
+void Compressor::StopLiveWindowMode() {}
+
+std::string Compressor::GetSmartDashboardType() const { return "Compressor"; }
+
+void Compressor::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Compressor::GetTable() const { return m_table; }
+
+void Compressor::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsBoolean()) return;
+ if (value->GetBoolean())
+ Start();
+ else
+ Stop();
+}
diff --git a/wpilibc/Athena/src/ControllerPower.cpp b/wpilibc/Athena/src/ControllerPower.cpp
new file mode 100644
index 0000000..ea508af
--- /dev/null
+++ b/wpilibc/Athena/src/ControllerPower.cpp
@@ -0,0 +1,176 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "ControllerPower.h"
+
+#include <stdint.h>
+#include <HAL/Power.hpp>
+#include <HAL/HAL.hpp>
+#include "ErrorBase.h"
+
+/**
+ * Get the input voltage to the robot controller
+ * @return The controller input voltage value in Volts
+ */
+double ControllerPower::GetInputVoltage() {
+ int32_t status = 0;
+ double retVal = getVinVoltage(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the input current to the robot controller
+ * @return The controller input current value in Amps
+ */
+double ControllerPower::GetInputCurrent() {
+ int32_t status = 0;
+ double retVal = getVinCurrent(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the voltage of the 6V rail
+ * @return The controller 6V rail voltage value in Volts
+ */
+double ControllerPower::GetVoltage6V() {
+ int32_t status = 0;
+ double retVal = getUserVoltage6V(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the current output of the 6V rail
+ * @return The controller 6V rail output current value in Amps
+ */
+double ControllerPower::GetCurrent6V() {
+ int32_t status = 0;
+ double retVal = getUserCurrent6V(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the enabled state of the 6V rail. The rail may be disabled due to a
+ * controller
+ * brownout, a short circuit on the rail, or controller over-voltage
+ * @return The controller 6V rail enabled value. True for enabled.
+ */
+bool ControllerPower::GetEnabled6V() {
+ int32_t status = 0;
+ bool retVal = getUserActive6V(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the count of the total current faults on the 6V rail since the controller
+ * has booted
+ * @return The number of faults.
+ */
+int ControllerPower::GetFaultCount6V() {
+ int32_t status = 0;
+ int retVal = getUserCurrentFaults6V(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the voltage of the 5V rail
+ * @return The controller 5V rail voltage value in Volts
+ */
+double ControllerPower::GetVoltage5V() {
+ int32_t status = 0;
+ double retVal = getUserVoltage5V(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the current output of the 5V rail
+ * @return The controller 5V rail output current value in Amps
+ */
+double ControllerPower::GetCurrent5V() {
+ int32_t status = 0;
+ double retVal = getUserCurrent5V(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the enabled state of the 5V rail. The rail may be disabled due to a
+ * controller
+ * brownout, a short circuit on the rail, or controller over-voltage
+ * @return The controller 5V rail enabled value. True for enabled.
+ */
+bool ControllerPower::GetEnabled5V() {
+ int32_t status = 0;
+ bool retVal = getUserActive5V(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the count of the total current faults on the 5V rail since the controller
+ * has booted
+ * @return The number of faults
+ */
+int ControllerPower::GetFaultCount5V() {
+ int32_t status = 0;
+ int retVal = getUserCurrentFaults5V(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the voltage of the 3.3V rail
+ * @return The controller 3.3V rail voltage value in Volts
+ */
+double ControllerPower::GetVoltage3V3() {
+ int32_t status = 0;
+ double retVal = getUserVoltage3V3(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the current output of the 3.3V rail
+ * @return The controller 3.3V rail output current value in Amps
+ */
+double ControllerPower::GetCurrent3V3() {
+ int32_t status = 0;
+ double retVal = getUserCurrent3V3(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the enabled state of the 3.3V rail. The rail may be disabled due to a
+ * controller
+ * brownout, a short circuit on the rail, or controller over-voltage
+ * @return The controller 3.3V rail enabled value. True for enabled.
+ */
+bool ControllerPower::GetEnabled3V3() {
+ int32_t status = 0;
+ bool retVal = getUserActive3V3(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Get the count of the total current faults on the 3.3V rail since the
+ * controller has booted
+ * @return The number of faults
+ */
+int ControllerPower::GetFaultCount3V3() {
+ int32_t status = 0;
+ int retVal = getUserCurrentFaults3V3(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
\ No newline at end of file
diff --git a/wpilibc/Athena/src/Counter.cpp b/wpilibc/Athena/src/Counter.cpp
new file mode 100644
index 0000000..cd70de5
--- /dev/null
+++ b/wpilibc/Athena/src/Counter.cpp
@@ -0,0 +1,590 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Counter.h"
+#include "AnalogTrigger.h"
+#include "DigitalInput.h"
+#include "Resource.h"
+#include "WPIErrors.h"
+
+/**
+ * Create an instance of a counter where no sources are selected.
+ * They all must be selected by calling functions to specify the upsource and
+ * the downsource
+ * independently.
+ *
+ * This creates a ChipObject counter and initializes status variables
+ * appropriately.
+ *
+ * The counter will start counting immediately.
+ * @param mode The counter mode
+ */
+Counter::Counter(Mode mode) {
+ int32_t status = 0;
+ m_counter = initializeCounter(mode, &m_index, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ SetMaxPeriod(.5);
+
+ HALReport(HALUsageReporting::kResourceType_Counter, m_index, mode);
+}
+
+/**
+ * Create an instance of a counter from a Digital Source (such as a Digital
+ * Input).
+ * This is used if an existing digital input is to be shared by multiple other
+ * objects such
+ * as encoders or if the Digital Source is not a Digital Input channel (such as
+ * an Analog Trigger).
+ *
+ * The counter will start counting immediately.
+ * @param source A pointer to the existing DigitalSource object. It will be set
+ * as the Up Source.
+ */
+Counter::Counter(DigitalSource *source) : Counter(kTwoPulse) {
+ SetUpSource(source);
+ ClearDownSource();
+}
+
+/**
+ * Create an instance of a counter from a Digital Source (such as a Digital
+ * Input).
+ * This is used if an existing digital input is to be shared by multiple other
+ * objects such
+ * as encoders or if the Digital Source is not a Digital Input channel (such as
+ * an Analog Trigger).
+ *
+ * The counter will start counting immediately.
+ * @param source A pointer to the existing DigitalSource object. It will be
+ * set as the Up Source.
+ */
+Counter::Counter(std::shared_ptr<DigitalSource> source) : Counter(kTwoPulse) {
+ SetUpSource(source);
+ ClearDownSource();
+}
+
+/**
+ * Create an instance of a Counter object.
+ * Create an up-Counter instance given a channel.
+ *
+ * The counter will start counting immediately.
+ * @param channel The DIO channel to use as the up source. 0-9 are on-board,
+ * 10-25 are on the MXP
+ */
+Counter::Counter(int32_t channel) : Counter(kTwoPulse) {
+ SetUpSource(channel);
+ ClearDownSource();
+}
+
+/**
+ * Create an instance of a Counter object.
+ * Create an instance of a simple up-Counter given an analog trigger.
+ * Use the trigger state output from the analog trigger.
+ *
+ * The counter will start counting immediately.
+ * @param trigger The pointer to the existing AnalogTrigger object.
+ */
+DEPRECATED("Use pass-by-reference instead.")
+Counter::Counter(AnalogTrigger *trigger) : Counter(kTwoPulse) {
+ SetUpSource(trigger->CreateOutput(kState));
+ ClearDownSource();
+}
+
+/**
+ * Create an instance of a Counter object.
+ * Create an instance of a simple up-Counter given an analog trigger.
+ * Use the trigger state output from the analog trigger.
+ *
+ * The counter will start counting immediately.
+ * @param trigger The reference to the existing AnalogTrigger object.
+ */
+Counter::Counter(const AnalogTrigger &trigger) : Counter(kTwoPulse) {
+ SetUpSource(trigger.CreateOutput(kState));
+ ClearDownSource();
+}
+
+/**
+ * Create an instance of a Counter object.
+ * Creates a full up-down counter given two Digital Sources
+ * @param encodingType The quadrature decoding mode (1x or 2x)
+ * @param upSource The pointer to the DigitalSource to set as the up source
+ * @param downSource The pointer to the DigitalSource to set as the down source
+ * @param inverted True to invert the output (reverse the direction)
+ */
+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) {}
+
+/**
+ * Create an instance of a Counter object.
+ * Creates a full up-down counter given two Digital Sources
+ * @param encodingType The quadrature decoding mode (1x or 2x)
+ * @param upSource The pointer to the DigitalSource to set as the up source
+ * @param downSource The pointer to the DigitalSource to set as the down source
+ * @param inverted True to invert the output (reverse the direction)
+ */
+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);
+ setCounterAverageSize(m_counter, 1, &status);
+ } else {
+ SetUpSourceEdge(true, true);
+ setCounterAverageSize(m_counter, 2, &status);
+ }
+
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ SetDownSourceEdge(inverted, true);
+}
+
+/**
+ * Delete the Counter object.
+ */
+Counter::~Counter() {
+ SetUpdateWhenEmpty(true);
+
+ int32_t status = 0;
+ freeCounter(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ m_counter = nullptr;
+}
+
+/**
+ * Set the upsource for the counter as a digital input channel.
+ * @param channel The DIO channel to use as the up source. 0-9 are on-board,
+ * 10-25 are on the MXP
+ */
+void Counter::SetUpSource(int32_t channel) {
+ if (StatusIsFatal()) return;
+ SetUpSource(std::make_shared<DigitalInput>(channel));
+}
+
+/**
+ * Set the up counting source to be an analog trigger.
+ * @param analogTrigger The analog trigger object that is used for the Up Source
+ * @param triggerType The analog trigger output that will trigger the counter.
+ */
+void Counter::SetUpSource(AnalogTrigger *analogTrigger,
+ AnalogTriggerType triggerType) {
+ SetUpSource(std::shared_ptr<AnalogTrigger>(analogTrigger,
+ NullDeleter<AnalogTrigger>()),
+ triggerType);
+}
+
+/**
+ * Set the up counting source to be an analog trigger.
+ * @param analogTrigger The analog trigger object that is used for the Up Source
+ * @param triggerType The analog trigger output that will trigger the counter.
+ */
+void Counter::SetUpSource(std::shared_ptr<AnalogTrigger> analogTrigger,
+ AnalogTriggerType triggerType) {
+ if (StatusIsFatal()) return;
+ SetUpSource(analogTrigger->CreateOutput(triggerType));
+}
+
+/**
+ * Set the source object that causes the counter to count up.
+ * Set the up counting DigitalSource.
+ * @param source Pointer to the DigitalSource object to set as the up source
+ */
+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;
+ setCounterUpSource(m_counter, source->GetChannelForRouting(),
+ source->GetAnalogTriggerForRouting(), &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+void Counter::SetUpSource(DigitalSource *source) {
+ SetUpSource(
+ std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
+}
+
+/**
+ * Set the source object that causes the counter to count up.
+ * Set the up counting DigitalSource.
+ * @param source Reference to the DigitalSource object to set as the up source
+ */
+void Counter::SetUpSource(DigitalSource &source) {
+ SetUpSource(
+ std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
+}
+
+/**
+ * Set the edge sensitivity on an up counting source.
+ * Set the up source to either detect rising edges or falling edges or both.
+ * @param risingEdge True to trigger on rising edges
+ * @param fallingEdge True to trigger on falling edges
+ */
+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;
+ setCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Disable the up counting source to the counter.
+ */
+void Counter::ClearUpSource() {
+ if (StatusIsFatal()) return;
+ m_upSource.reset();
+ int32_t status = 0;
+ clearCounterUpSource(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the down counting source to be a digital input channel.
+ * @param channel The DIO channel to use as the up source. 0-9 are on-board,
+ * 10-25 are on the MXP
+ */
+void Counter::SetDownSource(int32_t channel) {
+ if (StatusIsFatal()) return;
+ SetDownSource(std::make_shared<DigitalInput>(channel));
+}
+
+/**
+ * Set the down counting source to be an analog trigger.
+ * @param analogTrigger The analog trigger object that is used for the Down
+ * Source
+ * @param triggerType The analog trigger output that will trigger the counter.
+ */
+void Counter::SetDownSource(AnalogTrigger *analogTrigger,
+ AnalogTriggerType triggerType) {
+ SetDownSource(std::shared_ptr<AnalogTrigger>(analogTrigger, NullDeleter<AnalogTrigger>()), triggerType);
+}
+
+/**
+ * Set the down counting source to be an analog trigger.
+ * @param analogTrigger The analog trigger object that is used for the Down
+ * Source
+ * @param triggerType The analog trigger output that will trigger the counter.
+ */
+void Counter::SetDownSource(std::shared_ptr<AnalogTrigger> analogTrigger,
+ AnalogTriggerType triggerType) {
+ if (StatusIsFatal()) return;
+ SetDownSource(analogTrigger->CreateOutput(triggerType));
+}
+
+/**
+ * Set the source object that causes the counter to count down.
+ * Set the down counting DigitalSource.
+ * @param source Pointer to the DigitalSource object to set as the down source
+ */
+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;
+ setCounterDownSource(m_counter, source->GetChannelForRouting(),
+ source->GetAnalogTriggerForRouting(), &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+void Counter::SetDownSource(DigitalSource *source) {
+ SetDownSource(std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
+}
+
+/**
+ * Set the source object that causes the counter to count down.
+ * Set the down counting DigitalSource.
+ * @param source Reference to the DigitalSource object to set as the down source
+ */
+void Counter::SetDownSource(DigitalSource &source) {
+ SetDownSource(std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
+}
+
+/**
+ * Set the edge sensitivity on a down counting source.
+ * Set the down source to either detect rising edges or falling edges.
+ * @param risingEdge True to trigger on rising edges
+ * @param fallingEdge True to trigger on falling edges
+ */
+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;
+ setCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Disable the down counting source to the counter.
+ */
+void Counter::ClearDownSource() {
+ if (StatusIsFatal()) return;
+ m_downSource.reset();
+ int32_t status = 0;
+ clearCounterDownSource(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set standard up / down counting mode on this counter.
+ * Up and down counts are sourced independently from two inputs.
+ */
+void Counter::SetUpDownCounterMode() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setCounterUpDownMode(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set external direction mode on this counter.
+ * Counts are sourced on the Up counter input.
+ * The Down counter input represents the direction to count.
+ */
+void Counter::SetExternalDirectionMode() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setCounterExternalDirectionMode(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set Semi-period mode on this counter.
+ * Counts up on both rising and falling edges.
+ */
+void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Configure the counter to count in up or down based on the length of the input
+ * pulse.
+ * This mode is most useful for direction sensitive gear tooth sensors.
+ * @param threshold The pulse length beyond which the counter counts the
+ * opposite direction. Units are seconds.
+ */
+void Counter::SetPulseLengthMode(float threshold) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setCounterPulseLengthMode(m_counter, threshold, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Get the Samples to Average which specifies the number of samples of the timer
+ * to
+ * average when calculating the period. Perform averaging to account for
+ * mechanical imperfections or as oversampling to increase resolution.
+ * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+ */
+int Counter::GetSamplesToAverage() const {
+ int32_t status = 0;
+ int32_t samples = getCounterSamplesToAverage(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return samples;
+}
+
+/**
+ * Set the Samples to Average which specifies the number of samples of the timer
+ * to
+ * average when calculating the period. Perform averaging to account for
+ * mechanical imperfections or as oversampling to increase resolution.
+ * @param samplesToAverage The number of samples to average from 1 to 127.
+ */
+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;
+ setCounterSamplesToAverage(m_counter, samplesToAverage, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Read the current counter value.
+ * Read the value at this instant. It may still be running, so it reflects the
+ * current value. Next
+ * time it is read, it might have a different value.
+ */
+int32_t Counter::Get() const {
+ if (StatusIsFatal()) return 0;
+ int32_t status = 0;
+ int32_t value = getCounter(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+}
+
+/**
+ * Reset the Counter to zero.
+ * Set the counter value to zero. This doesn't effect the running state of the
+ * counter, just sets
+ * the current value to zero.
+ */
+void Counter::Reset() {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ resetCounter(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Get the Period of the most recent count.
+ * Returns the time interval of the most recent count. This can be used for
+ * velocity calculations
+ * to determine shaft speed.
+ * @returns The period between the last two pulses in units of seconds.
+ */
+double Counter::GetPeriod() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t status = 0;
+ double value = getCounterPeriod(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+}
+
+/**
+ * Set the maximum period where the device is still considered "moving".
+ * Sets the maximum period where the device is considered moving. This value is
+ * used to determine
+ * the "stopped" state of the counter using the GetStopped method.
+ * @param maxPeriod The maximum period where the counted device is considered
+ * moving in
+ * seconds.
+ */
+void Counter::SetMaxPeriod(double maxPeriod) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setCounterMaxPeriod(m_counter, maxPeriod, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Select whether you want to continue updating the event timer output when
+ * there are no samples captured.
+ * The output of the event timer has a buffer of periods that are averaged and
+ * posted to
+ * a register on the FPGA. When the timer detects that the event source has
+ * stopped
+ * (based on the MaxPeriod) the buffer of samples to be averaged is emptied. If
+ * you
+ * enable the update when empty, you will be notified of the stopped source and
+ * the event
+ * time will report 0 samples. If you disable update when empty, the most
+ * recent average
+ * will remain on the output until a new sample is acquired. You will never see
+ * 0 samples
+ * output (except when there have been no events since an FPGA reset) and you
+ * will likely not
+ * see the stopped bit become true (since it is updated at the end of an average
+ * and there are
+ * no samples to average).
+ * @param enabled True to enable update when empty
+ */
+void Counter::SetUpdateWhenEmpty(bool enabled) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setCounterUpdateWhenEmpty(m_counter, enabled, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Determine if the clock is stopped.
+ * Determine if the clocked input is stopped based on the MaxPeriod value set
+ * using the
+ * SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the device (and
+ * counter) are
+ * assumed to be stopped and it returns true.
+ * @return Returns true if the most recent counter period exceeds the MaxPeriod
+ * value set by
+ * SetMaxPeriod.
+ */
+bool Counter::GetStopped() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = getCounterStopped(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+}
+
+/**
+ * The last direction the counter value changed.
+ * @return The last direction the counter value changed.
+ */
+bool Counter::GetDirection() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = getCounterDirection(m_counter, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+}
+
+/**
+ * Set the Counter to return reversed sensing on the direction.
+ * This allows counters to change the direction they are counting in the case of
+ * 1X and 2X
+ * quadrature encoding only. Any other counter mode isn't supported.
+ * @param reverseDirection true if the value counted should be negated.
+ */
+void Counter::SetReverseDirection(bool reverseDirection) {
+ if (StatusIsFatal()) return;
+ int32_t status = 0;
+ setCounterReverseDirection(m_counter, reverseDirection, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+void Counter::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", Get());
+ }
+}
+
+void Counter::StartLiveWindowMode() {}
+
+void Counter::StopLiveWindowMode() {}
+
+std::string Counter::GetSmartDashboardType() const { return "Counter"; }
+
+void Counter::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Counter::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/DigitalGlitchFilter.cpp b/wpilibc/Athena/src/DigitalGlitchFilter.cpp
new file mode 100644
index 0000000..56147cb
--- /dev/null
+++ b/wpilibc/Athena/src/DigitalGlitchFilter.cpp
@@ -0,0 +1,190 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include <algorithm>
+#include <array>
+
+#include "DigitalGlitchFilter.h"
+#include "Resource.h"
+#include "WPIErrors.h"
+#include "Encoder.h"
+#include "Counter.h"
+#include "Utility.h"
+
+std::array<bool, 3> DigitalGlitchFilter::m_filterAllocated = {{false, false,
+ false}};
+priority_mutex DigitalGlitchFilter::m_mutex;
+
+DigitalGlitchFilter::DigitalGlitchFilter() {
+ std::lock_guard<priority_mutex> sync(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;
+
+ HALReport(HALUsageReporting::kResourceType_DigitalFilter, m_channelIndex);
+}
+
+DigitalGlitchFilter::~DigitalGlitchFilter() {
+ if (m_channelIndex >= 0) {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_filterAllocated[m_channelIndex] = false;
+ }
+}
+
+/**
+ * Assigns the DigitalSource to this glitch filter.
+ *
+ * @param input The DigitalSource to add.
+ */
+void DigitalGlitchFilter::Add(DigitalSource *input) {
+ DoAdd(input, m_channelIndex + 1);
+}
+
+void DigitalGlitchFilter::DoAdd(DigitalSource *input, int requested_index) {
+ // Some sources from Counters and Encoders are null. By pushing the check
+ // here, we catch the issue more generally.
+ if (input) {
+ int32_t status = 0;
+ setFilterSelect(m_digital_ports[input->GetChannelForRouting()],
+ requested_index, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ // Validate that we set it correctly.
+ int actual_index = getFilterSelect(
+ m_digital_ports[input->GetChannelForRouting()], &status);
+ wpi_assertEqual(actual_index, requested_index);
+
+ HALReport(HALUsageReporting::kResourceType_DigitalInput,
+ input->GetChannelForRouting());
+ }
+}
+
+/**
+ * Assigns the Encoder to this glitch filter.
+ *
+ * @param input The Encoder to add.
+ */
+void DigitalGlitchFilter::Add(Encoder *input) {
+ Add(input->m_aSource.get());
+ if (StatusIsFatal()) {
+ return;
+ }
+ Add(input->m_bSource.get());
+}
+
+/**
+ * Assigns the Counter to this glitch filter.
+ *
+ * @param input The Counter to add.
+ */
+void DigitalGlitchFilter::Add(Counter *input) {
+ Add(input->m_upSource.get());
+ if (StatusIsFatal()) {
+ return;
+ }
+ Add(input->m_downSource.get());
+}
+
+/**
+ * Removes a digital input from this filter.
+ *
+ * Removes the DigitalSource from this glitch filter and re-assigns it to
+ * the default filter.
+ *
+ * @param input The DigitalSource to remove.
+ */
+void DigitalGlitchFilter::Remove(DigitalSource *input) {
+ DoAdd(input, 0);
+}
+
+/**
+ * Removes an encoder from this filter.
+ *
+ * Removes the Encoder from this glitch filter and re-assigns it to
+ * the default filter.
+ *
+ * @param input The Encoder to remove.
+ */
+void DigitalGlitchFilter::Remove(Encoder *input) {
+ Remove(input->m_aSource.get());
+ if (StatusIsFatal()) {
+ return;
+ }
+ Remove(input->m_bSource.get());
+}
+
+/**
+ * Removes a counter from this filter.
+ *
+ * Removes the Counter from this glitch filter and re-assigns it to
+ * the default filter.
+ *
+ * @param input The Counter to remove.
+ */
+void DigitalGlitchFilter::Remove(Counter *input) {
+ Remove(input->m_upSource.get());
+ if (StatusIsFatal()) {
+ return;
+ }
+ Remove(input->m_downSource.get());
+}
+
+/**
+ * Sets the number of cycles that the input must not change state for.
+ *
+ * @param fpga_cycles The number of FPGA cycles.
+ */
+void DigitalGlitchFilter::SetPeriodCycles(uint32_t fpga_cycles) {
+ int32_t status = 0;
+ setFilterPeriod(m_channelIndex, fpga_cycles, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Sets the number of nanoseconds that the input must not change state for.
+ *
+ * @param nanoseconds The number of nanoseconds.
+ */
+void DigitalGlitchFilter::SetPeriodNanoSeconds(uint64_t nanoseconds) {
+ int32_t status = 0;
+ uint32_t fpga_cycles =
+ nanoseconds * kSystemClockTicksPerMicrosecond / 4 / 1000;
+ setFilterPeriod(m_channelIndex, fpga_cycles, &status);
+
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Gets the number of cycles that the input must not change state for.
+ *
+ * @return The number of cycles.
+ */
+uint32_t DigitalGlitchFilter::GetPeriodCycles() {
+ int32_t status = 0;
+ uint32_t fpga_cycles = getFilterPeriod(m_channelIndex, &status);
+
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ return fpga_cycles;
+}
+
+/**
+ * Gets the number of nanoseconds that the input must not change state for.
+ *
+ * @return The number of nanoseconds.
+ */
+uint64_t DigitalGlitchFilter::GetPeriodNanoSeconds() {
+ int32_t status = 0;
+ uint32_t fpga_cycles = getFilterPeriod(m_channelIndex, &status);
+
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ return static_cast<uint64_t>(fpga_cycles) * 1000L /
+ static_cast<uint64_t>(kSystemClockTicksPerMicrosecond / 4);
+}
diff --git a/wpilibc/Athena/src/DigitalInput.cpp b/wpilibc/Athena/src/DigitalInput.cpp
new file mode 100644
index 0000000..f6b96f5
--- /dev/null
+++ b/wpilibc/Athena/src/DigitalInput.cpp
@@ -0,0 +1,110 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "DigitalInput.h"
+#include "Resource.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+#include <limits>
+#include <sstream>
+
+/**
+ * Create an instance of a Digital Input class.
+ * Creates a digital input given a channel.
+ *
+ * @param channel The DIO channel 0-9 are on-board, 10-25 are on the MXP port
+ */
+DigitalInput::DigitalInput(uint32_t channel) {
+ std::stringstream buf;
+
+ if (!CheckDigitalChannel(channel)) {
+ buf << "Digital Channel " << channel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ m_channel = std::numeric_limits<uint32_t>::max();
+ return;
+ }
+ m_channel = channel;
+
+ int32_t status = 0;
+ allocateDIO(m_digital_ports[channel], true, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ LiveWindow::GetInstance()->AddSensor("DigitalInput", channel, this);
+ HALReport(HALUsageReporting::kResourceType_DigitalInput, channel);
+}
+
+/**
+ * Free resources associated with the Digital Input class.
+ */
+DigitalInput::~DigitalInput() {
+ if (StatusIsFatal()) return;
+ if (m_interrupt != nullptr) {
+ int32_t status = 0;
+ cleanInterrupts(m_interrupt, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ m_interrupt = nullptr;
+ m_interrupts->Free(m_interruptIndex);
+ }
+
+ int32_t status = 0;
+ freeDIO(m_digital_ports[m_channel], &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Get the value from a digital input channel.
+ * Retrieve the value of a single digital input channel from the FPGA.
+ */
+bool DigitalInput::Get() const {
+ if (StatusIsFatal()) return false;
+ int32_t status = 0;
+ bool value = getDIO(m_digital_ports[m_channel], &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+}
+
+/**
+ * @return The GPIO channel number that this object represents.
+ */
+uint32_t DigitalInput::GetChannel() const { return m_channel; }
+
+/**
+ * @return The value to be written to the channel field of a routing mux.
+ */
+uint32_t DigitalInput::GetChannelForRouting() const { return GetChannel(); }
+
+/**
+ * @return The value to be written to the module field of a routing mux.
+ */
+uint32_t DigitalInput::GetModuleForRouting() const { return 0; }
+
+/**
+ * @return The value to be written to the analog trigger field of a routing mux.
+ */
+bool DigitalInput::GetAnalogTriggerForRouting() const { return false; }
+
+void DigitalInput::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutBoolean("Value", Get());
+ }
+}
+
+void DigitalInput::StartLiveWindowMode() {}
+
+void DigitalInput::StopLiveWindowMode() {}
+
+std::string DigitalInput::GetSmartDashboardType() const {
+ return "DigitalInput";
+}
+
+void DigitalInput::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> DigitalInput::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/DigitalOutput.cpp b/wpilibc/Athena/src/DigitalOutput.cpp
new file mode 100644
index 0000000..d74f93b
--- /dev/null
+++ b/wpilibc/Athena/src/DigitalOutput.cpp
@@ -0,0 +1,230 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "DigitalOutput.h"
+#include "Resource.h"
+#include "WPIErrors.h"
+
+#include <limits>
+#include <sstream>
+
+/**
+ * Create an instance of a digital output.
+ * Create a digital output given a channel.
+ *
+ * @param channel The digital channel 0-9 are on-board, 10-25 are on the MXP
+ * port
+ */
+DigitalOutput::DigitalOutput(uint32_t channel) {
+ std::stringstream buf;
+
+ m_pwmGenerator = (void *)std::numeric_limits<uint32_t>::max();
+ if (!CheckDigitalChannel(channel)) {
+ buf << "Digital Channel " << channel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ m_channel = std::numeric_limits<uint32_t>::max();
+ return;
+ }
+ m_channel = channel;
+
+ int32_t status = 0;
+ allocateDIO(m_digital_ports[channel], false, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ HALReport(HALUsageReporting::kResourceType_DigitalOutput, channel);
+}
+
+/**
+ * Free the resources associated with a digital output.
+ */
+DigitalOutput::~DigitalOutput() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+ if (StatusIsFatal()) return;
+ // Disable the PWM in case it was running.
+ DisablePWM();
+
+ int32_t status = 0;
+ freeDIO(m_digital_ports[m_channel], &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the value of a digital output.
+ * Set the value of a digital output to either one (true) or zero (false).
+ * @param value 1 (true) for high, 0 (false) for disabled
+ */
+void DigitalOutput::Set(uint32_t value) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ setDIO(m_digital_ports[m_channel], value, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * @return The GPIO channel number that this object represents.
+ */
+uint32_t DigitalOutput::GetChannel() const { return m_channel; }
+
+/**
+ * Output a single pulse on the digital output line.
+ * Send a single pulse on the digital output line where the pulse duration is
+ * specified in seconds.
+ * Maximum pulse length is 0.0016 seconds.
+ * @param length The pulselength in seconds
+ */
+void DigitalOutput::Pulse(float length) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ pulse(m_digital_ports[m_channel], length, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Determine if the pulse is still going.
+ * Determine if a previously started pulse is still going.
+ */
+bool DigitalOutput::IsPulsing() const {
+ if (StatusIsFatal()) return false;
+
+ int32_t status = 0;
+ bool value = isPulsing(m_digital_ports[m_channel], &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+}
+
+/**
+ * Change the PWM frequency of the PWM output on a Digital Output line.
+ *
+ * The valid range is from 0.6 Hz to 19 kHz. The frequency resolution is
+ * logarithmic.
+ *
+ * There is only one PWM frequency for all digital channels.
+ *
+ * @param rate The frequency to output all digital output PWM signals.
+ */
+void DigitalOutput::SetPWMRate(float rate) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ setPWMRate(rate, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Enable a PWM Output on this line.
+ *
+ * Allocate one of the 6 DO PWM generator resources from this module.
+ *
+ * Supply the initial duty-cycle to output so as to avoid a glitch when first
+ * starting.
+ *
+ * The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less)
+ * but is reduced the higher the frequency of the PWM signal is.
+ *
+ * @param initialDutyCycle The duty-cycle to start generating. [0..1]
+ */
+void DigitalOutput::EnablePWM(float initialDutyCycle) {
+ if (m_pwmGenerator != (void *)std::numeric_limits<uint32_t>::max()) return;
+
+ int32_t status = 0;
+
+ if (StatusIsFatal()) return;
+ m_pwmGenerator = allocatePWM(&status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ if (StatusIsFatal()) return;
+ setPWMDutyCycle(m_pwmGenerator, initialDutyCycle, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ if (StatusIsFatal()) return;
+ setPWMOutputChannel(m_pwmGenerator, m_channel, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Change this line from a PWM output back to a static Digital Output line.
+ *
+ * Free up one of the 6 DO PWM generator resources that were in use.
+ */
+void DigitalOutput::DisablePWM() {
+ if (StatusIsFatal()) return;
+ if (m_pwmGenerator == (void *)std::numeric_limits<uint32_t>::max()) return;
+
+ int32_t status = 0;
+
+ // Disable the output by routing to a dead bit.
+ setPWMOutputChannel(m_pwmGenerator, kDigitalChannels, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ freePWM(m_pwmGenerator, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ m_pwmGenerator = (void *)std::numeric_limits<uint32_t>::max();
+}
+
+/**
+ * Change the duty-cycle that is being generated on the line.
+ *
+ * The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less)
+ * but is reduced the higher the frequency of the PWM signal is.
+ *
+ * @param dutyCycle The duty-cycle to change to. [0..1]
+ */
+void DigitalOutput::UpdateDutyCycle(float dutyCycle) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ setPWMDutyCycle(m_pwmGenerator, dutyCycle, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * @return The value to be written to the channel field of a routing mux.
+ */
+uint32_t DigitalOutput::GetChannelForRouting() const { return GetChannel(); }
+
+/**
+ * @return The value to be written to the module field of a routing mux.
+ */
+uint32_t DigitalOutput::GetModuleForRouting() const { return 0; }
+
+/**
+ * @return The value to be written to the analog trigger field of a routing mux.
+ */
+bool DigitalOutput::GetAnalogTriggerForRouting() const { return false; }
+
+void DigitalOutput::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsBoolean()) return;
+ Set(value->GetBoolean());
+}
+
+void DigitalOutput::UpdateTable() {}
+
+void DigitalOutput::StartLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void DigitalOutput::StopLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string DigitalOutput::GetSmartDashboardType() const {
+ return "Digital Output";
+}
+
+void DigitalOutput::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> DigitalOutput::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/DoubleSolenoid.cpp b/wpilibc/Athena/src/DoubleSolenoid.cpp
new file mode 100644
index 0000000..4fb2943
--- /dev/null
+++ b/wpilibc/Athena/src/DoubleSolenoid.cpp
@@ -0,0 +1,197 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "DoubleSolenoid.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+#include <sstream>
+
+/**
+ * Constructor.
+ * Uses the default PCM ID of 0
+ * @param forwardChannel The forward channel number on the PCM (0..7).
+ * @param reverseChannel The reverse channel number on the PCM (0..7).
+ */
+DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel)
+ : DoubleSolenoid(GetDefaultSolenoidModule(), forwardChannel, reverseChannel) {}
+
+/**
+ * Constructor.
+ *
+ * @param moduleNumber The CAN ID of the PCM.
+ * @param forwardChannel The forward channel on the PCM to control (0..7).
+ * @param reverseChannel The reverse channel on the PCM to control (0..7).
+ */
+DoubleSolenoid::DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel,
+ uint32_t reverseChannel)
+ : SolenoidBase(moduleNumber),
+ m_forwardChannel(forwardChannel),
+ m_reverseChannel(reverseChannel) {
+ std::stringstream buf;
+ if (!CheckSolenoidModule(m_moduleNumber)) {
+ buf << "Solenoid Module " << m_moduleNumber;
+ wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf.str());
+ return;
+ }
+ if (!CheckSolenoidChannel(m_forwardChannel)) {
+ buf << "Solenoid Module " << m_forwardChannel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ return;
+ }
+ if (!CheckSolenoidChannel(m_reverseChannel)) {
+ buf << "Solenoid Module " << m_reverseChannel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ return;
+ }
+ Resource::CreateResourceObject(
+ m_allocated, m_maxModules * m_maxPorts);
+
+ buf << "Solenoid " << m_forwardChannel << " (Module: " << m_moduleNumber
+ << ")";
+ if (m_allocated->Allocate(
+ m_moduleNumber * kSolenoidChannels + m_forwardChannel, buf.str()) ==
+ std::numeric_limits<uint32_t>::max()) {
+ CloneError(*m_allocated);
+ return;
+ }
+
+ buf << "Solenoid " << m_reverseChannel << " (Module: " << m_moduleNumber
+ << ")";
+ if (m_allocated->Allocate(
+ m_moduleNumber * kSolenoidChannels + m_reverseChannel, buf.str()) ==
+ std::numeric_limits<uint32_t>::max()) {
+ CloneError(*m_allocated);
+ return;
+ }
+
+ m_forwardMask = 1 << m_forwardChannel;
+ m_reverseMask = 1 << m_reverseChannel;
+ HALReport(HALUsageReporting::kResourceType_Solenoid, m_forwardChannel,
+ m_moduleNumber);
+ HALReport(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel,
+ m_moduleNumber);
+ LiveWindow::GetInstance()->AddActuator("DoubleSolenoid", m_moduleNumber,
+ m_forwardChannel, this);
+}
+
+/**
+ * Destructor.
+ */
+DoubleSolenoid::~DoubleSolenoid() {
+ if (CheckSolenoidModule(m_moduleNumber)) {
+ m_allocated->Free(m_moduleNumber * kSolenoidChannels + m_forwardChannel);
+ m_allocated->Free(m_moduleNumber * kSolenoidChannels + m_reverseChannel);
+ }
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param value The value to set (Off, Forward or Reverse)
+ */
+void DoubleSolenoid::Set(Value value) {
+ if (StatusIsFatal()) return;
+ uint8_t rawValue = 0x00;
+
+ switch (value) {
+ case kOff:
+ rawValue = 0x00;
+ break;
+ case kForward:
+ rawValue = m_forwardMask;
+ break;
+ case kReverse:
+ rawValue = m_reverseMask;
+ break;
+ }
+
+ SolenoidBase::Set(rawValue, m_forwardMask | m_reverseMask, m_moduleNumber);
+}
+
+/**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+DoubleSolenoid::Value DoubleSolenoid::Get() const {
+ if (StatusIsFatal()) return kOff;
+ uint8_t value = GetAll(m_moduleNumber);
+
+ if (value & m_forwardMask) return kForward;
+ if (value & m_reverseMask) return kReverse;
+ return kOff;
+}
+/**
+ * Check if the forward solenoid is blacklisted.
+ * If a solenoid is shorted, it is added to the blacklist and
+ * disabled until power cycle, or until faults are cleared.
+ * @see ClearAllPCMStickyFaults()
+ *
+ * @return If solenoid is disabled due to short.
+ */
+bool DoubleSolenoid::IsFwdSolenoidBlackListed() const {
+ int blackList = GetPCMSolenoidBlackList(m_moduleNumber);
+ return (blackList & m_forwardMask) ? 1 : 0;
+}
+/**
+ * Check if the reverse solenoid is blacklisted.
+ * If a solenoid is shorted, it is added to the blacklist and
+ * disabled until power cycle, or until faults are cleared.
+ * @see ClearAllPCMStickyFaults()
+ *
+ * @return If solenoid is disabled due to short.
+ */
+bool DoubleSolenoid::IsRevSolenoidBlackListed() const {
+ int blackList = GetPCMSolenoidBlackList(m_moduleNumber);
+ return (blackList & m_reverseMask) ? 1 : 0;
+}
+
+void DoubleSolenoid::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value,
+ bool isNew) {
+ if (!value->IsString()) return;
+ Value lvalue = kOff;
+ if (value->GetString() == "Forward")
+ lvalue = kForward;
+ else if (value->GetString() == "Reverse")
+ lvalue = kReverse;
+ Set(lvalue);
+}
+
+void DoubleSolenoid::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutString(
+ "Value", (Get() == kForward ? "Forward"
+ : (Get() == kReverse ? "Reverse" : "Off")));
+ }
+}
+
+void DoubleSolenoid::StartLiveWindowMode() {
+ Set(kOff);
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void DoubleSolenoid::StopLiveWindowMode() {
+ Set(kOff);
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string DoubleSolenoid::GetSmartDashboardType() const {
+ return "Double Solenoid";
+}
+
+void DoubleSolenoid::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> DoubleSolenoid::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/DriverStation.cpp b/wpilibc/Athena/src/DriverStation.cpp
new file mode 100644
index 0000000..455a987
--- /dev/null
+++ b/wpilibc/Athena/src/DriverStation.cpp
@@ -0,0 +1,537 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "DriverStation.h"
+#include "AnalogInput.h"
+#include "Timer.h"
+#include "NetworkCommunication/FRCComm.h"
+#include "MotorSafetyHelper.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include <string.h>
+#include "Log.hpp"
+
+// set the logging level
+TLogLevel dsLogLevel = logDEBUG;
+const double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
+
+#define DS_LOG(level) \
+ if (level > dsLogLevel) \
+ ; \
+ else \
+ Log().Get(level)
+
+const uint32_t DriverStation::kJoystickPorts;
+
+/**
+ * DriverStation constructor.
+ *
+ * This is only called once the first time GetInstance() is called
+ */
+DriverStation::DriverStation() {
+ // 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_joystickAxes[i].count = 0;
+ m_joystickPOVs[i].count = 0;
+ m_joystickButtons[i].count = 0;
+ m_joystickDescriptor[i].isXbox = 0;
+ m_joystickDescriptor[i].type = -1;
+ m_joystickDescriptor[i].name[0] = '\0';
+ }
+ // Register that semaphore with the network communications task.
+ // It will signal when new packet data is available.
+ HALSetNewDataSem(&m_packetDataAvailableCond);
+
+ AddToSingletonList();
+
+ m_task = Task("DriverStation", &DriverStation::Run, this);
+}
+
+DriverStation::~DriverStation() {
+ m_isRunning = false;
+ m_task.join();
+
+ // Unregister our semaphore.
+ HALSetNewDataSem(nullptr);
+}
+
+void DriverStation::Run() {
+ m_isRunning = true;
+ int period = 0;
+ while (m_isRunning) {
+ {
+ std::unique_lock<priority_mutex> lock(m_packetDataAvailableMutex);
+ m_packetDataAvailableCond.wait(lock);
+ }
+ GetData();
+ m_waitForDataCond.notify_all();
+
+ if (++period >= 4) {
+ MotorSafetyHelper::CheckMotors();
+ period = 0;
+ }
+ if (m_userInDisabled) HALNetworkCommunicationObserveUserProgramDisabled();
+ if (m_userInAutonomous)
+ HALNetworkCommunicationObserveUserProgramAutonomous();
+ if (m_userInTeleop) HALNetworkCommunicationObserveUserProgramTeleop();
+ if (m_userInTest) HALNetworkCommunicationObserveUserProgramTest();
+ }
+}
+
+/**
+ * Return a pointer to the singleton DriverStation.
+ * @return Pointer to the DS instance
+ */
+DriverStation &DriverStation::GetInstance() {
+ static DriverStation instance;
+ return instance;
+}
+
+/**
+ * Copy data from the DS task for the user.
+ * If no new data exists, it will just be returned, otherwise
+ * the data will be copied from the DS polling loop.
+ */
+void DriverStation::GetData() {
+ // Get the status of all of the joysticks
+ for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
+ HALGetJoystickAxes(stick, &m_joystickAxes[stick]);
+ HALGetJoystickPOVs(stick, &m_joystickPOVs[stick]);
+ HALGetJoystickButtons(stick, &m_joystickButtons[stick]);
+ HALGetJoystickDescriptor(stick, &m_joystickDescriptor[stick]);
+ }
+ m_newControlData.give();
+}
+
+/**
+ * Read the battery voltage.
+ *
+ * @return The battery voltage in Volts.
+ */
+float DriverStation::GetBatteryVoltage() const {
+ int32_t status = 0;
+ float voltage = getVinVoltage(&status);
+ wpi_setErrorWithContext(status, "getVinVoltage");
+
+ return voltage;
+}
+
+/**
+ * Reports errors related to unplugged joysticks
+ * Throttles the errors so that they don't overwhelm the DS
+ */
+void DriverStation::ReportJoystickUnpluggedError(std::string message) {
+ double currentTime = Timer::GetFPGATimestamp();
+ if (currentTime > m_nextMessageTime) {
+ ReportError(message);
+ m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+ }
+}
+
+/**
+ * Returns the number of axes on a given joystick port
+ *
+ * @param stick The joystick port number
+ * @return The number of axes on the indicated joystick
+ */
+int DriverStation::GetStickAxisCount(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+ HALJoystickAxes joystickAxes;
+ HALGetJoystickAxes(stick, &joystickAxes);
+ return joystickAxes.count;
+}
+
+/**
+ * Returns the name of the joystick at the given port
+ *
+ * @param stick The joystick port number
+ * @return The name of the joystick at the given port
+ */
+std::string DriverStation::GetJoystickName(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ }
+ std::string retVal(m_joystickDescriptor[0].name);
+ return retVal;
+}
+
+/**
+ * Returns the type of joystick at a given port
+ *
+ * @param stick The joystick port number
+ * @return The HID type of joystick at the given port
+ */
+int DriverStation::GetJoystickType(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return -1;
+ }
+ return (int)m_joystickDescriptor[stick].type;
+}
+
+/**
+ * Returns a boolean indicating if the controller is an xbox controller.
+ *
+ * @param stick The joystick port number
+ * @return A boolean that is true if the controller is an xbox controller.
+ */
+bool DriverStation::GetJoystickIsXbox(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return false;
+ }
+ return (bool)m_joystickDescriptor[stick].isXbox;
+}
+
+/**
+ * Returns the types of Axes on a given joystick port
+ *
+ * @param stick The joystick port number and the target axis
+ * @return What type of axis the axis is reporting to be
+ */
+int DriverStation::GetJoystickAxisType(uint32_t stick, uint8_t axis) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return -1;
+ }
+ return m_joystickDescriptor[stick].axisTypes[axis];
+}
+
+/**
+ * Returns the number of POVs on a given joystick port
+ *
+ * @param stick The joystick port number
+ * @return The number of POVs on the indicated joystick
+ */
+int DriverStation::GetStickPOVCount(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+ HALJoystickPOVs joystickPOVs;
+ HALGetJoystickPOVs(stick, &joystickPOVs);
+ return joystickPOVs.count;
+}
+
+/**
+ * Returns the number of buttons on a given joystick port
+ *
+ * @param stick The joystick port number
+ * @return The number of buttons on the indicated joystick
+ */
+int DriverStation::GetStickButtonCount(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+ HALJoystickButtons joystickButtons;
+ HALGetJoystickButtons(stick, &joystickButtons);
+ return joystickButtons.count;
+}
+
+/**
+ * Get the value of the axis on a joystick.
+ * This depends on the mapping of the joystick connected to the specified port.
+ *
+ * @param stick The joystick to read.
+ * @param axis The analog axis value to read from the joystick.
+ * @return The value of the axis on the joystick.
+ */
+float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+
+ if (axis >= m_joystickAxes[stick].count) {
+ if (axis >= kMaxJoystickAxes)
+ wpi_setWPIError(BadJoystickAxis);
+ else
+ ReportJoystickUnpluggedError(
+ "WARNING: Joystick Axis missing, check if all controllers are "
+ "plugged in\n");
+ return 0.0f;
+ }
+
+ int8_t value = m_joystickAxes[stick].axes[axis];
+
+ if (value < 0) {
+ return value / 128.0f;
+ } else {
+ return value / 127.0f;
+ }
+}
+
+/**
+ * Get the state of a POV on the joystick.
+ *
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return -1;
+ }
+
+ if (pov >= m_joystickPOVs[stick].count) {
+ if (pov >= kMaxJoystickPOVs)
+ wpi_setWPIError(BadJoystickAxis);
+ else
+ ReportJoystickUnpluggedError(
+ "WARNING: Joystick POV missing, check if all controllers are plugged "
+ "in\n");
+ return -1;
+ }
+
+ return m_joystickPOVs[stick].povs[pov];
+}
+
+/**
+ * The state of the buttons on the joystick.
+ *
+ * @param stick The joystick to read.
+ * @return The state of the buttons on the joystick.
+ */
+uint32_t DriverStation::GetStickButtons(uint32_t stick) const {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return 0;
+ }
+
+ return m_joystickButtons[stick].buttons;
+}
+
+/**
+ * The state of one joystick button. Button indexes begin at 1.
+ *
+ * @param stick The joystick to read.
+ * @param button The button index, beginning at 1.
+ * @return The state of the joystick button.
+ */
+bool DriverStation::GetStickButton(uint32_t stick, uint8_t button) {
+ if (stick >= kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ return false;
+ }
+
+ if (button > m_joystickButtons[stick].count) {
+ ReportJoystickUnpluggedError(
+ "WARNING: Joystick Button missing, check if all controllers are "
+ "plugged in\n");
+ return false;
+ }
+ if (button == 0) {
+ ReportJoystickUnpluggedError(
+ "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
+ return false;
+ }
+ return ((0x1 << (button - 1)) & m_joystickButtons[stick].buttons) != 0;
+}
+
+/**
+ * Check if the DS has enabled the robot
+ * @return True if the robot is enabled and the DS is connected
+ */
+bool DriverStation::IsEnabled() const {
+ HALControlWord controlWord;
+ memset(&controlWord, 0, sizeof(controlWord));
+ HALGetControlWord(&controlWord);
+ return controlWord.enabled && controlWord.dsAttached;
+}
+
+/**
+ * Check if the robot is disabled
+ * @return True if the robot is explicitly disabled or the DS is not connected
+ */
+bool DriverStation::IsDisabled() const {
+ HALControlWord controlWord;
+ memset(&controlWord, 0, sizeof(controlWord));
+ HALGetControlWord(&controlWord);
+ return !(controlWord.enabled && controlWord.dsAttached);
+}
+
+/**
+ * Check if the DS is commanding autonomous mode
+ * @return True if the robot is being commanded to be in autonomous mode
+ */
+bool DriverStation::IsAutonomous() const {
+ HALControlWord controlWord;
+ memset(&controlWord, 0, sizeof(controlWord));
+ HALGetControlWord(&controlWord);
+ return controlWord.autonomous;
+}
+
+/**
+ * Check if the DS is commanding teleop mode
+ * @return True if the robot is being commanded to be in teleop mode
+ */
+bool DriverStation::IsOperatorControl() const {
+ HALControlWord controlWord;
+ memset(&controlWord, 0, sizeof(controlWord));
+ HALGetControlWord(&controlWord);
+ return !(controlWord.autonomous || controlWord.test);
+}
+
+/**
+ * Check if the DS is commanding test mode
+ * @return True if the robot is being commanded to be in test mode
+ */
+bool DriverStation::IsTest() const {
+ HALControlWord controlWord;
+ HALGetControlWord(&controlWord);
+ return controlWord.test;
+}
+
+/**
+ * Check if the DS is attached
+ * @return True if the DS is connected to the robot
+ */
+bool DriverStation::IsDSAttached() const {
+ HALControlWord controlWord;
+ memset(&controlWord, 0, sizeof(controlWord));
+ HALGetControlWord(&controlWord);
+ return controlWord.dsAttached;
+}
+
+/**
+ * Check if the FPGA outputs are enabled. The outputs may be disabled if the
+ * robot is disabled
+ * or e-stopped, the watchdog has expired, or if the roboRIO browns out.
+ * @return True if the FPGA outputs are enabled.
+ */
+bool DriverStation::IsSysActive() const {
+ int32_t status = 0;
+ bool retVal = HALGetSystemActive(&status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Check if the system is browned out.
+ * @return True if the system is browned out
+ */
+bool DriverStation::IsSysBrownedOut() const {
+ int32_t status = 0;
+ bool retVal = HALGetBrownedOut(&status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Has a new control packet from the driver station arrived since the last time
+ * this function was called?
+ * Warning: If you call this function from more than one place at the same time,
+ * you will not get the get the intended behaviour.
+ * @return True if the control data has been updated since the last call.
+ */
+bool DriverStation::IsNewControlData() const {
+ return m_newControlData.tryTake() == false;
+}
+
+/**
+ * Is the driver station attached to a Field Management System?
+ * @return True if the robot is competing on a field being controlled by a Field
+ * Management System
+ */
+bool DriverStation::IsFMSAttached() const {
+ HALControlWord controlWord;
+ HALGetControlWord(&controlWord);
+ return controlWord.fmsAttached;
+}
+
+/**
+ * Return the alliance that the driver station says it is on.
+ * This could return kRed or kBlue
+ * @return The Alliance enum (kRed, kBlue or kInvalid)
+ */
+DriverStation::Alliance DriverStation::GetAlliance() const {
+ HALAllianceStationID allianceStationID;
+ HALGetAllianceStation(&allianceStationID);
+ switch (allianceStationID) {
+ case kHALAllianceStationID_red1:
+ case kHALAllianceStationID_red2:
+ case kHALAllianceStationID_red3:
+ return kRed;
+ case kHALAllianceStationID_blue1:
+ case kHALAllianceStationID_blue2:
+ case kHALAllianceStationID_blue3:
+ return kBlue;
+ default:
+ return kInvalid;
+ }
+}
+
+/**
+ * Return the driver station location on the field
+ * This could return 1, 2, or 3
+ * @return The location of the driver station (1-3, 0 for invalid)
+ */
+uint32_t DriverStation::GetLocation() const {
+ HALAllianceStationID allianceStationID;
+ HALGetAllianceStation(&allianceStationID);
+ switch (allianceStationID) {
+ case kHALAllianceStationID_red1:
+ case kHALAllianceStationID_blue1:
+ return 1;
+ case kHALAllianceStationID_red2:
+ case kHALAllianceStationID_blue2:
+ return 2;
+ case kHALAllianceStationID_red3:
+ case kHALAllianceStationID_blue3:
+ return 3;
+ default:
+ return 0;
+ }
+}
+
+/**
+ * Wait until a new packet comes from the driver station
+ * This blocks on a semaphore, so the waiting is efficient.
+ * This is a good way to delay processing until there is new driver station data
+ * to act on
+ */
+void DriverStation::WaitForData() {
+ std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
+ m_waitForDataCond.wait(lock);
+}
+
+/**
+ * Return the approximate match time
+ * The FMS does not send an official match time to the robots, but does send an
+ * approximate match time.
+ * The value will count down the time remaining in the current period (auto or
+ * teleop).
+ * Warning: This is not an official time (so it cannot be used to dispute ref
+ * calls or guarantee that a function
+ * will trigger before the match ends)
+ * The Practice Match function of the DS approximates the behaviour seen on the
+ * field.
+ * @return Time remaining in current match period (auto or teleop)
+ */
+double DriverStation::GetMatchTime() const {
+ float matchTime;
+ HALGetMatchTime(&matchTime);
+ return (double)matchTime;
+}
+
+/**
+ * Report an error to the DriverStation messages window.
+ * The error is also printed to the program console.
+ */
+void DriverStation::ReportError(std::string error) {
+ std::cout << error << std::endl;
+
+ HALControlWord controlWord;
+ HALGetControlWord(&controlWord);
+ if (controlWord.dsAttached) {
+ HALSetErrorData(error.c_str(), error.size(), 0);
+ }
+}
diff --git a/wpilibc/Athena/src/Encoder.cpp b/wpilibc/Athena/src/Encoder.cpp
new file mode 100644
index 0000000..0fa12df
--- /dev/null
+++ b/wpilibc/Athena/src/Encoder.cpp
@@ -0,0 +1,561 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Encoder.h"
+#include "DigitalInput.h"
+#include "Resource.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Common initialization code for Encoders.
+ * This code allocates resources for Encoders and is common to all constructors.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param reverseDirection If true, counts down instead of up (this is all
+ * relative)
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ * decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts will be
+ * 4x the encoder
+ * spec'd value since all rising and falling edges are counted. If 1X or 2X are
+ * selected then
+ * a counter object will be used and the returned value will either exactly
+ * match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) {
+ m_encodingType = encodingType;
+ switch (encodingType) {
+ case k4X: {
+ m_encodingScale = 4;
+ if (m_aSource->StatusIsFatal()) {
+ CloneError(*m_aSource);
+ return;
+ }
+ if (m_bSource->StatusIsFatal()) {
+ CloneError(*m_bSource);
+ return;
+ }
+ int32_t status = 0;
+ m_encoder = initializeEncoder(
+ m_aSource->GetModuleForRouting(), m_aSource->GetChannelForRouting(),
+ m_aSource->GetAnalogTriggerForRouting(),
+ m_bSource->GetModuleForRouting(), m_bSource->GetChannelForRouting(),
+ m_bSource->GetAnalogTriggerForRouting(), reverseDirection, &m_index,
+ &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ m_counter = nullptr;
+ SetMaxPeriod(.5);
+ break;
+ }
+ case k1X:
+ case k2X: {
+ m_encodingScale = encodingType == k1X ? 1 : 2;
+ m_counter = std::make_unique<Counter>(m_encodingType, m_aSource,
+ m_bSource, reverseDirection);
+ m_index = m_counter->GetFPGAIndex();
+ break;
+ }
+ default:
+ wpi_setErrorWithContext(-1, "Invalid encodingType argument");
+ break;
+ }
+
+ HALReport(HALUsageReporting::kResourceType_Encoder, m_index, encodingType);
+ LiveWindow::GetInstance()->AddSensor("Encoder",
+ m_aSource->GetChannelForRouting(), this);
+}
+
+/**
+ * Encoder constructor.
+ * Construct a Encoder given a and b channels.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param aChannel The a channel DIO channel. 0-9 are on-board, 10-25 are on the
+ * MXP port
+ * @param bChannel The b channel DIO channel. 0-9 are on-board, 10-25 are on the
+ * MXP port
+ * @param reverseDirection represents the orientation of the encoder and inverts
+ * the output values
+ * if necessary so forward represents positive values.
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ * decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts will be
+ * 4x the encoder
+ * spec'd value since all rising and falling edges are counted. If 1X or 2X are
+ * selected then
+ * a counter object will be used and the returned value will either exactly
+ * match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+Encoder::Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection,
+ EncodingType encodingType) {
+ m_aSource = std::make_shared<DigitalInput>(aChannel);
+ m_bSource = std::make_shared<DigitalInput>(bChannel);
+ InitEncoder(reverseDirection, encodingType);
+}
+
+/**
+ * Encoder constructor.
+ * Construct a Encoder given a and b channels as digital inputs. This is used in
+ * the case where the digital inputs are shared. The Encoder class will not
+ * allocate the digital inputs and assume that they already are counted.
+ * The counter will start counting immediately.
+ *
+ * @param aSource The source that should be used for the a channel.
+ * @param bSource the source that should be used for the b channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts
+ * the output values
+ * if necessary so forward represents positive values.
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ * decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts will be
+ * 4x the encoder
+ * spec'd value since all rising and falling edges are counted. If 1X or 2X are
+ * selected then
+ * a counter object will be used and the returned value will either exactly
+ * match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+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(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 constructor.
+ * Construct a Encoder given a and b channels as digital inputs. This is used in
+ * the case
+ * where the digital inputs are shared. The Encoder class will not allocate the
+ * digital inputs
+ * and assume that they already are counted.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param aSource The source that should be used for the a channel.
+ * @param bSource the source that should be used for the b channel.
+ * @param reverseDirection represents the orientation of the encoder and inverts
+ * the output values
+ * if necessary so forward represents positive values.
+ * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ * decoding. If 4X is
+ * selected, then an encoder FPGA object is used and the returned counts will be
+ * 4x the encoder
+ * spec'd value since all rising and falling edges are counted. If 1X or 2X are
+ * selected then
+ * a counter object will be used and the returned value will either exactly
+ * match the spec'd count
+ * or be double (2x) the spec'd count.
+ */
+Encoder::Encoder(DigitalSource &aSource, DigitalSource &bSource,
+ bool reverseDirection, EncodingType encodingType)
+ : m_aSource(&aSource, NullDeleter<DigitalSource>()),
+ m_bSource(&bSource, NullDeleter<DigitalSource>())
+{
+ InitEncoder(reverseDirection, encodingType);
+}
+
+/**
+ * Free the resources for an Encoder.
+ * Frees the FPGA resources associated with an Encoder.
+ */
+Encoder::~Encoder() {
+ if (!m_counter) {
+ int32_t status = 0;
+ freeEncoder(m_encoder, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * The encoding scale factor 1x, 2x, or 4x, per the requested encodingType.
+ * Used to divide raw edge counts down to spec'd counts.
+ */
+int32_t Encoder::GetEncodingScale() const { return m_encodingScale; }
+
+/**
+ * Gets the raw value from the encoder.
+ * The raw value is the actual count unscaled by the 1x, 2x, or 4x scale
+ * factor.
+ * @return Current raw count from the encoder
+ */
+int32_t Encoder::GetRaw() const {
+ if (StatusIsFatal()) return 0;
+ int32_t value;
+ if (m_counter)
+ value = m_counter->Get();
+ else {
+ int32_t status = 0;
+ value = getEncoder(m_encoder, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+ return value;
+}
+
+/**
+ * Gets the current count.
+ * Returns the current count on the Encoder.
+ * This method compensates for the decoding type.
+ *
+ * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale
+ * factor.
+ */
+int32_t Encoder::Get() const {
+ if (StatusIsFatal()) return 0;
+ return (int32_t)(GetRaw() * DecodingScaleFactor());
+}
+
+/**
+ * Reset the Encoder distance to zero.
+ * Resets the current count to zero on the encoder.
+ */
+void Encoder::Reset() {
+ if (StatusIsFatal()) return;
+ if (m_counter)
+ m_counter->Reset();
+ else {
+ int32_t status = 0;
+ resetEncoder(m_encoder, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * Returns the period of the most recent pulse.
+ * Returns the period of the most recent Encoder pulse in seconds.
+ * This method compensates for the decoding type.
+ *
+ * @deprecated Use GetRate() in favor of this method. This returns unscaled
+ * periods and GetRate() scales using value from SetDistancePerPulse().
+ *
+ * @return Period in seconds of the most recent pulse.
+ */
+double Encoder::GetPeriod() const {
+ if (StatusIsFatal()) return 0.0;
+ if (m_counter) {
+ return m_counter->GetPeriod() / DecodingScaleFactor();
+ } else {
+ int32_t status = 0;
+ double period = getEncoderPeriod(m_encoder, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return period;
+ }
+}
+
+/**
+ * Sets the maximum period for stopped detection.
+ * Sets the value that represents the maximum period of the Encoder before it
+ * will assume
+ * that the attached device is stopped. This timeout allows users to determine
+ * if the wheels or
+ * other shaft has stopped rotating.
+ * This method compensates for the decoding type.
+ *
+ * @deprecated Use SetMinRate() in favor of this method. This takes unscaled
+ * periods and SetMinRate() scales using value from SetDistancePerPulse().
+ *
+ * @param maxPeriod The maximum time between rising and falling edges before the
+ * FPGA will
+ * report the device stopped. This is expressed in seconds.
+ */
+void Encoder::SetMaxPeriod(double maxPeriod) {
+ if (StatusIsFatal()) return;
+ if (m_counter) {
+ m_counter->SetMaxPeriod(maxPeriod * DecodingScaleFactor());
+ } else {
+ int32_t status = 0;
+ setEncoderMaxPeriod(m_encoder, maxPeriod, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * Determine if the encoder is stopped.
+ * Using the MaxPeriod value, a boolean is returned that is true if the encoder
+ * is considered
+ * stopped and false if it is still moving. A stopped encoder is one where the
+ * most recent pulse
+ * width exceeds the MaxPeriod.
+ * @return True if the encoder is considered stopped.
+ */
+bool Encoder::GetStopped() const {
+ if (StatusIsFatal()) return true;
+ if (m_counter) {
+ return m_counter->GetStopped();
+ } else {
+ int32_t status = 0;
+ bool value = getEncoderStopped(m_encoder, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+ }
+}
+
+/**
+ * The last direction the encoder value changed.
+ * @return The last direction the encoder value changed.
+ */
+bool Encoder::GetDirection() const {
+ if (StatusIsFatal()) return false;
+ if (m_counter) {
+ return m_counter->GetDirection();
+ } else {
+ int32_t status = 0;
+ bool value = getEncoderDirection(m_encoder, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+ }
+}
+
+/**
+ * The scale needed to convert a raw counter value into a number of encoder
+ * pulses.
+ */
+double Encoder::DecodingScaleFactor() const {
+ if (StatusIsFatal()) return 0.0;
+ switch (m_encodingType) {
+ case k1X:
+ return 1.0;
+ case k2X:
+ return 0.5;
+ case k4X:
+ return 0.25;
+ default:
+ return 0.0;
+ }
+}
+
+/**
+ * Get the distance the robot has driven since the last reset.
+ *
+ * @return The distance driven since the last reset as scaled by the value from
+ * SetDistancePerPulse().
+ */
+double Encoder::GetDistance() const {
+ if (StatusIsFatal()) return 0.0;
+ return GetRaw() * DecodingScaleFactor() * m_distancePerPulse;
+}
+
+/**
+ * Get the current rate of the encoder.
+ * Units are distance per second as scaled by the value from
+ * SetDistancePerPulse().
+ *
+ * @return The current rate of the encoder.
+ */
+double Encoder::GetRate() const {
+ if (StatusIsFatal()) return 0.0;
+ return (m_distancePerPulse / GetPeriod());
+}
+
+/**
+ * Set the minimum rate of the device before the hardware reports it stopped.
+ *
+ * @param minRate The minimum rate. The units are in distance per second as
+ * scaled by the value from SetDistancePerPulse().
+ */
+void Encoder::SetMinRate(double minRate) {
+ if (StatusIsFatal()) return;
+ SetMaxPeriod(m_distancePerPulse / minRate);
+}
+
+/**
+ * Set the distance per pulse for this encoder.
+ * This sets the multiplier used to determine the distance driven based on the
+ * count value
+ * from the encoder.
+ * Do not include the decoding type in this scale. The library already
+ * compensates for the decoding type.
+ * Set this value based on the encoder's rated Pulses per Revolution and
+ * factor in gearing reductions following the encoder shaft.
+ * This distance can be in any units you like, linear or angular.
+ *
+ * @param distancePerPulse The scale factor that will be used to convert pulses
+ * to useful units.
+ */
+void Encoder::SetDistancePerPulse(double distancePerPulse) {
+ if (StatusIsFatal()) return;
+ m_distancePerPulse = distancePerPulse;
+}
+
+/**
+ * Set the direction sensing for this encoder.
+ * This sets the direction sensing on the encoder so that it could count in the
+ * correct
+ * software direction regardless of the mounting.
+ * @param reverseDirection true if the encoder direction should be reversed
+ */
+void Encoder::SetReverseDirection(bool reverseDirection) {
+ if (StatusIsFatal()) return;
+ if (m_counter) {
+ m_counter->SetReverseDirection(reverseDirection);
+ } else {
+ int32_t status = 0;
+ setEncoderReverseDirection(m_encoder, reverseDirection, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * Set the Samples to Average which specifies the number of samples of the timer
+ * to
+ * average when calculating the period. Perform averaging to account for
+ * mechanical imperfections or as oversampling to increase resolution.
+ * @param samplesToAverage The number of samples to average from 1 to 127.
+ */
+void Encoder::SetSamplesToAverage(int samplesToAverage) {
+ if (samplesToAverage < 1 || samplesToAverage > 127) {
+ wpi_setWPIErrorWithContext(
+ ParameterOutOfRange,
+ "Average counter values must be between 1 and 127");
+ }
+ int32_t status = 0;
+ switch (m_encodingType) {
+ case k4X:
+ setEncoderSamplesToAverage(m_encoder, samplesToAverage, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ break;
+ case k1X:
+ case k2X:
+ m_counter->SetSamplesToAverage(samplesToAverage);
+ break;
+ }
+}
+
+/**
+ * Get the Samples to Average which specifies the number of samples of the timer
+ * to
+ * average when calculating the period. Perform averaging to account for
+ * mechanical imperfections or as oversampling to increase resolution.
+ * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+ */
+int Encoder::GetSamplesToAverage() const {
+ int result = 1;
+ int32_t status = 0;
+ switch (m_encodingType) {
+ case k4X:
+ result = getEncoderSamplesToAverage(m_encoder, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ break;
+ case k1X:
+ case k2X:
+ result = m_counter->GetSamplesToAverage();
+ break;
+ }
+ return result;
+}
+
+/**
+ * Implement the PIDSource interface.
+ *
+ * @return The current value of the selected source parameter.
+ */
+double Encoder::PIDGet() {
+ if (StatusIsFatal()) return 0.0;
+ switch (GetPIDSourceType()) {
+ case PIDSourceType::kDisplacement:
+ return GetDistance();
+ case PIDSourceType::kRate:
+ return GetRate();
+ default:
+ return 0.0;
+ }
+}
+
+/**
+ * Set the index source for the encoder. When this source is activated, the
+ * encoder count automatically resets.
+ *
+ * @param channel A DIO channel to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+void Encoder::SetIndexSource(uint32_t channel, Encoder::IndexingType type) {
+ int32_t status = 0;
+ bool activeHigh = (type == kResetWhileHigh) || (type == kResetOnRisingEdge);
+ bool edgeSensitive =
+ (type == kResetOnFallingEdge) || (type == kResetOnRisingEdge);
+
+ setEncoderIndexSource(m_encoder, channel, false, activeHigh, edgeSensitive,
+ &status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the index source for the encoder. When this source is activated, the
+ * encoder count automatically resets.
+ *
+ * @param channel A digital source to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+DEPRECATED("Use pass-by-reference instead.")
+void Encoder::SetIndexSource(DigitalSource *source,
+ Encoder::IndexingType type) {
+ SetIndexSource(*source, type);
+}
+
+/**
+ * Set the index source for the encoder. When this source is activated, the
+ * encoder count automatically resets.
+ *
+ * @param channel A digital source to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+void Encoder::SetIndexSource(const DigitalSource &source,
+ Encoder::IndexingType type) {
+ int32_t status = 0;
+ bool activeHigh = (type == kResetWhileHigh) || (type == kResetOnRisingEdge);
+ bool edgeSensitive =
+ (type == kResetOnFallingEdge) || (type == kResetOnRisingEdge);
+
+ setEncoderIndexSource(m_encoder, source.GetChannelForRouting(),
+ source.GetAnalogTriggerForRouting(), activeHigh,
+ edgeSensitive, &status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+}
+
+void Encoder::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Speed", GetRate());
+ m_table->PutNumber("Distance", GetDistance());
+ m_table->PutNumber("Distance per Tick", m_distancePerPulse);
+ }
+}
+
+void Encoder::StartLiveWindowMode() {}
+
+void Encoder::StopLiveWindowMode() {}
+
+std::string Encoder::GetSmartDashboardType() const {
+ if (m_encodingType == k4X)
+ return "Quadrature Encoder";
+ else
+ return "Encoder";
+}
+
+void Encoder::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Encoder::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/GearTooth.cpp b/wpilibc/Athena/src/GearTooth.cpp
new file mode 100644
index 0000000..b659125
--- /dev/null
+++ b/wpilibc/Athena/src/GearTooth.cpp
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "GearTooth.h"
+#include "LiveWindow/LiveWindow.h"
+
+constexpr double GearTooth::kGearToothThreshold;
+
+/**
+ * Common code called by the constructors.
+ */
+void GearTooth::EnableDirectionSensing(bool directionSensitive) {
+ if (directionSensitive) {
+ SetPulseLengthMode(kGearToothThreshold);
+ }
+}
+
+/**
+ * Construct a GearTooth sensor given a channel.
+ *
+ * @param channel The DIO channel that the sensor is connected to. 0-9 are
+ * on-board, 10-25 are on the MXP.
+ * @param directionSensitive True to enable the pulse length decoding in
+ * hardware to specify count direction.
+ */
+GearTooth::GearTooth(uint32_t channel, bool directionSensitive)
+ : Counter(channel) {
+ EnableDirectionSensing(directionSensitive);
+ LiveWindow::GetInstance()->AddSensor("GearTooth", channel, this);
+}
+
+/**
+ * Construct a GearTooth sensor given a digital input.
+ * This should be used when sharing digital inputs.
+ *
+ * @param source A pointer to the existing DigitalSource object (such as a
+ * DigitalInput)
+ * @param directionSensitive True to enable the pulse length decoding in
+ * hardware to specify count direction.
+ */
+GearTooth::GearTooth(DigitalSource *source, bool directionSensitive)
+ : Counter(source) {
+ EnableDirectionSensing(directionSensitive);
+}
+
+/**
+ * Construct a GearTooth sensor given a digital input.
+ * This should be used when sharing digital inputs.
+ *
+ * @param source A reference to the existing DigitalSource object (such as a
+ * DigitalInput)
+ * @param directionSensitive True to enable the pulse length decoding in
+ * hardware to specify count direction.
+ */
+GearTooth::GearTooth(std::shared_ptr<DigitalSource> source, bool directionSensitive)
+ : Counter(source) {
+ EnableDirectionSensing(directionSensitive);
+}
+
+std::string GearTooth::GetSmartDashboardType() const { return "GearTooth"; }
diff --git a/wpilibc/Athena/src/GyroBase.cpp b/wpilibc/Athena/src/GyroBase.cpp
new file mode 100644
index 0000000..4cc9056
--- /dev/null
+++ b/wpilibc/Athena/src/GyroBase.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "GyroBase.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Get the PIDOutput for the PIDSource base object. Can be set to return
+ * angle or rate using SetPIDSourceType(). Defaults to angle.
+ *
+ * @return The PIDOutput (angle or rate, defaults to angle)
+ */
+double GyroBase::PIDGet() {
+ switch (GetPIDSourceType()) {
+ case PIDSourceType::kRate:
+ return GetRate();
+ case PIDSourceType::kDisplacement:
+ return GetAngle();
+ default:
+ return 0;
+ }
+}
+
+void GyroBase::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetAngle());
+ }
+}
+
+void GyroBase::StartLiveWindowMode() {}
+
+void GyroBase::StopLiveWindowMode() {}
+
+std::string GyroBase::GetSmartDashboardType() const { return "Gyro"; }
+
+void GyroBase::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> GyroBase::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/I2C.cpp b/wpilibc/Athena/src/I2C.cpp
new file mode 100644
index 0000000..79767b3
--- /dev/null
+++ b/wpilibc/Athena/src/I2C.cpp
@@ -0,0 +1,199 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "I2C.h"
+#include "HAL/HAL.hpp"
+#include "HAL/Digital.hpp"
+#include "WPIErrors.h"
+
+/**
+ * Constructor.
+ *
+ * @param port The I2C port to which the device is connected.
+ * @param deviceAddress The address of the device on the I2C bus.
+ */
+I2C::I2C(Port port, uint8_t deviceAddress)
+ : m_port(port), m_deviceAddress(deviceAddress) {
+ int32_t status = 0;
+ i2CInitialize(m_port, &status);
+ // wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ HALReport(HALUsageReporting::kResourceType_I2C, deviceAddress);
+}
+
+/**
+ * Destructor.
+ */
+I2C::~I2C() { i2CClose(m_port); }
+
+/**
+ * Generic transaction.
+ *
+ * This is a lower-level interface to the I2C hardware giving you more control
+ * over each transaction.
+ *
+ * @param dataToSend Buffer of data to send as part of the transaction.
+ * @param sendSize Number of bytes to send as part of the transaction.
+ * @param dataReceived Buffer to read data into.
+ * @param receiveSize Number of bytes to read from the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+bool I2C::Transaction(uint8_t *dataToSend, uint8_t sendSize,
+ uint8_t *dataReceived, uint8_t receiveSize) {
+ int32_t status = 0;
+ status = i2CTransaction(m_port, m_deviceAddress, dataToSend, sendSize,
+ dataReceived, receiveSize);
+ // wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return status < 0;
+}
+
+/**
+ * Attempt to address a device on the I2C bus.
+ *
+ * This allows you to figure out if there is a device on the I2C bus that
+ * responds to the address specified in the constructor.
+ *
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+bool I2C::AddressOnly() {
+ int32_t status = 0;
+ status = Transaction(nullptr, 0, nullptr, 0);
+ return status < 0;
+}
+
+/**
+ * Execute a write transaction with the device.
+ *
+ * Write a single byte to a register on a device and wait until the
+ * transaction is complete.
+ *
+ * @param registerAddress The address of the register on the device to be
+ * written.
+ * @param data The byte to write to the register on the device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+bool I2C::Write(uint8_t registerAddress, uint8_t data) {
+ uint8_t buffer[2];
+ buffer[0] = registerAddress;
+ buffer[1] = data;
+ int32_t status = 0;
+ status = i2CWrite(m_port, m_deviceAddress, buffer, sizeof(buffer));
+ return status < 0;
+}
+
+/**
+ * Execute a bulk write transaction with the device.
+ *
+ * Write multiple bytes to a device and wait until the
+ * transaction is complete.
+ *
+ * @param data The data to write to the register on the device.
+ * @param count The number of bytes to be written.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+bool I2C::WriteBulk(uint8_t *data, uint8_t count) {
+ int32_t status = 0;
+ status = i2CWrite(m_port, m_deviceAddress, data, count);
+ return status < 0;
+}
+
+/**
+ * Execute a read transaction with the device.
+ *
+ * Read bytes from a device.
+ * Most I2C devices will auto-increment the register pointer internally allowing
+ * you to read consecutive registers on a device in a single transaction.
+ *
+ * @param registerAddress The register to read first in the transaction.
+ * @param count The number of bytes to read in the transaction.
+ * @param buffer A pointer to the array of bytes to store the data read from the
+ * device.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+bool I2C::Read(uint8_t registerAddress, uint8_t count, uint8_t *buffer) {
+ if (count < 1) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "count");
+ return true;
+ }
+ if (buffer == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "buffer");
+ return true;
+ }
+ int32_t status = 0;
+ status =
+ Transaction(®isterAddress, sizeof(registerAddress), buffer, count);
+ return status < 0;
+}
+
+/**
+ * Execute a read only transaction with the device.
+ *
+ * Read bytes from a device. This method does not write any data to prompt the
+ * device.
+ *
+ * @param buffer
+ * A pointer to the array of bytes to store the data read from
+ * the device.
+ * @param count
+ The number of bytes to read in the transaction.
+ * @return Transfer Aborted... false for success, true for aborted.
+ */
+bool I2C::ReadOnly(uint8_t count, uint8_t *buffer) {
+ if (count < 1) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "count");
+ return true;
+ }
+ if (buffer == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "buffer");
+ return true;
+ }
+ int32_t status = 0;
+ status = i2CRead(m_port, m_deviceAddress, buffer, count);
+ return status < 0;
+}
+
+/**
+ * Send a broadcast write to all devices on the I2C bus.
+ *
+ * This is not currently implemented!
+ *
+ * @param registerAddress The register to write on all devices on the bus.
+ * @param data The value to write to the devices.
+ */
+[[gnu::warning("I2C::Broadcast() is not implemented.")]]
+void I2C::Broadcast(uint8_t registerAddress, uint8_t data) {}
+
+/**
+ * Verify that a device's registers contain expected values.
+ *
+ * Most devices will have a set of registers that contain a known value that
+ * can be used to identify them. This allows an I2C device driver to easily
+ * verify that the device contains the expected value.
+ *
+ * @pre The device must support and be configured to use register
+ * auto-increment.
+ *
+ * @param registerAddress The base register to start reading from the device.
+ * @param count The size of the field to be verified.
+ * @param expected A buffer containing the values expected from the device.
+ */
+bool I2C::VerifySensor(uint8_t registerAddress, uint8_t count,
+ const uint8_t *expected) {
+ // TODO: Make use of all 7 read bytes
+ uint8_t deviceData[4];
+ for (uint8_t i = 0, curRegisterAddress = registerAddress; i < count;
+ i += 4, curRegisterAddress += 4) {
+ uint8_t 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 (uint8_t j = 0; j < toRead; j++) {
+ if (deviceData[j] != expected[i + j]) return false;
+ }
+ }
+ return true;
+}
diff --git a/wpilibc/Athena/src/Internal/HardwareHLReporting.cpp b/wpilibc/Athena/src/Internal/HardwareHLReporting.cpp
new file mode 100644
index 0000000..2176c80
--- /dev/null
+++ b/wpilibc/Athena/src/Internal/HardwareHLReporting.cpp
@@ -0,0 +1,12 @@
+
+#include "Internal/HardwareHLReporting.h"
+#include "HAL/HAL.hpp"
+
+void HardwareHLReporting::ReportScheduler() {
+ HALReport(HALUsageReporting::kResourceType_Command,
+ HALUsageReporting::kCommand_Scheduler);
+}
+
+void HardwareHLReporting::ReportSmartDashboard() {
+ HALReport(HALUsageReporting::kResourceType_SmartDashboard, 0);
+}
diff --git a/wpilibc/Athena/src/InterruptableSensorBase.cpp b/wpilibc/Athena/src/InterruptableSensorBase.cpp
new file mode 100644
index 0000000..6e66c34
--- /dev/null
+++ b/wpilibc/Athena/src/InterruptableSensorBase.cpp
@@ -0,0 +1,198 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "InterruptableSensorBase.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+
+std::unique_ptr<Resource> InterruptableSensorBase::m_interrupts =
+ std::make_unique<Resource>(interrupt_kNumSystems);
+
+InterruptableSensorBase::InterruptableSensorBase() {
+}
+
+/**
+* Request one of the 8 interrupts asynchronously on this digital input.
+* Request interrupts in asynchronous mode where the user's interrupt handler
+* will be
+* called when the interrupt fires. Users that want control over the thread
+* priority
+* should use the synchronous method with their own spawned thread.
+* The default is interrupt on rising edges only.
+*/
+void InterruptableSensorBase::RequestInterrupts(
+ InterruptHandlerFunction handler, void *param) {
+ if (StatusIsFatal()) return;
+ uint32_t index = m_interrupts->Allocate("Async Interrupt");
+ if (index == std::numeric_limits<uint32_t>::max()) {
+ CloneError(*m_interrupts);
+ return;
+ }
+ m_interruptIndex = index;
+
+ // Creates a manager too
+ AllocateInterrupts(false);
+
+ int32_t status = 0;
+ requestInterrupts(m_interrupt, GetModuleForRouting(), GetChannelForRouting(),
+ GetAnalogTriggerForRouting(), &status);
+ SetUpSourceEdge(true, false);
+ attachInterruptHandler(m_interrupt, handler, param, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+* Request one of the 8 interrupts synchronously on this digital input.
+* Request interrupts in synchronous mode where the user program will have to
+* explicitly
+* wait for the interrupt to occur using WaitForInterrupt.
+* The default is interrupt on rising edges only.
+*/
+void InterruptableSensorBase::RequestInterrupts() {
+ if (StatusIsFatal()) return;
+ uint32_t index = m_interrupts->Allocate("Sync Interrupt");
+ if (index == std::numeric_limits<uint32_t>::max()) {
+ CloneError(*m_interrupts);
+ return;
+ }
+ m_interruptIndex = index;
+
+ AllocateInterrupts(true);
+
+ int32_t status = 0;
+ requestInterrupts(m_interrupt, GetModuleForRouting(), GetChannelForRouting(),
+ GetAnalogTriggerForRouting(), &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ SetUpSourceEdge(true, false);
+}
+
+void InterruptableSensorBase::AllocateInterrupts(bool watcher) {
+ wpi_assert(m_interrupt == nullptr);
+ // Expects the calling leaf class to allocate an interrupt index.
+ int32_t status = 0;
+ m_interrupt = initializeInterrupts(m_interruptIndex, watcher, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Cancel interrupts on this device.
+ * This deallocates all the chipobject structures and disables any interrupts.
+ */
+void InterruptableSensorBase::CancelInterrupts() {
+ if (StatusIsFatal()) return;
+ wpi_assert(m_interrupt != nullptr);
+ int32_t status = 0;
+ cleanInterrupts(m_interrupt, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ m_interrupt = nullptr;
+ m_interrupts->Free(m_interruptIndex);
+}
+
+/**
+ * In synchronous mode, wait for the defined interrupt to occur. You should
+ * <b>NOT</b> attempt to read the
+ * sensor from another thread while waiting for an interrupt. This is not
+ * threadsafe, and can cause
+ * memory corruption
+ * @param timeout Timeout in seconds
+ * @param ignorePrevious If true, ignore interrupts that happened before
+ * WaitForInterrupt was called.
+ * @return What interrupts fired
+ */
+InterruptableSensorBase::WaitResult InterruptableSensorBase::WaitForInterrupt(
+ float timeout, bool ignorePrevious) {
+ if (StatusIsFatal()) return InterruptableSensorBase::kTimeout;
+ wpi_assert(m_interrupt != nullptr);
+ int32_t status = 0;
+ uint32_t result;
+
+ result = waitForInterrupt(m_interrupt, timeout, ignorePrevious, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ return static_cast<WaitResult>(result);
+}
+
+/**
+ * Enable interrupts to occur on this input.
+ * Interrupts are disabled when the RequestInterrupt call is made. This gives
+ * time to do the
+ * setup of the other options before starting to field interrupts.
+ */
+void InterruptableSensorBase::EnableInterrupts() {
+ if (StatusIsFatal()) return;
+ wpi_assert(m_interrupt != nullptr);
+ int32_t status = 0;
+ enableInterrupts(m_interrupt, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Disable Interrupts without without deallocating structures.
+ */
+void InterruptableSensorBase::DisableInterrupts() {
+ if (StatusIsFatal()) return;
+ wpi_assert(m_interrupt != nullptr);
+ int32_t status = 0;
+ disableInterrupts(m_interrupt, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Return the timestamp for the rising interrupt that occurred most recently.
+ * This is in the same time domain as GetClock().
+ * The rising-edge interrupt should be enabled with
+ * {@link #DigitalInput.SetUpSourceEdge}
+ * @return Timestamp in seconds since boot.
+ */
+double InterruptableSensorBase::ReadRisingTimestamp() {
+ if (StatusIsFatal()) return 0.0;
+ wpi_assert(m_interrupt != nullptr);
+ int32_t status = 0;
+ double timestamp = readRisingTimestamp(m_interrupt, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return timestamp;
+}
+
+/**
+ * Return the timestamp for the falling interrupt that occurred most recently.
+ * This is in the same time domain as GetClock().
+ * The falling-edge interrupt should be enabled with
+ * {@link #DigitalInput.SetUpSourceEdge}
+ * @return Timestamp in seconds since boot.
+*/
+double InterruptableSensorBase::ReadFallingTimestamp() {
+ if (StatusIsFatal()) return 0.0;
+ wpi_assert(m_interrupt != nullptr);
+ int32_t status = 0;
+ double timestamp = readFallingTimestamp(m_interrupt, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return timestamp;
+}
+
+/**
+ * Set which edge to trigger interrupts on
+ *
+ * @param risingEdge
+ * true to interrupt on rising edge
+ * @param fallingEdge
+ * true to interrupt on falling edge
+ */
+void InterruptableSensorBase::SetUpSourceEdge(bool risingEdge,
+ bool fallingEdge) {
+ if (StatusIsFatal()) return;
+ if (m_interrupt == nullptr) {
+ wpi_setWPIErrorWithContext(
+ NullParameter,
+ "You must call RequestInterrupts before SetUpSourceEdge");
+ return;
+ }
+ if (m_interrupt != nullptr) {
+ int32_t status = 0;
+ setInterruptUpSourceEdge(m_interrupt, risingEdge, fallingEdge, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
diff --git a/wpilibc/Athena/src/IterativeRobot.cpp b/wpilibc/Athena/src/IterativeRobot.cpp
new file mode 100644
index 0000000..fa1d2ba
--- /dev/null
+++ b/wpilibc/Athena/src/IterativeRobot.cpp
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "IterativeRobot.h"
+
+#include "DriverStation.h"
+#include "HAL/HAL.hpp"
+#include "SmartDashboard/SmartDashboard.h"
+#include "LiveWindow/LiveWindow.h"
+#include "networktables/NetworkTable.h"
+
+constexpr double IterativeRobot::kDefaultPeriod;
+
+/**
+ * Provide an alternate "main loop" via StartCompetition().
+ *
+ * This specific StartCompetition() implements "main loop" behaviour synced with
+ * the DS packets
+ */
+void IterativeRobot::StartCompetition() {
+ HALReport(HALUsageReporting::kResourceType_Framework,
+ HALUsageReporting::kFramework_Iterative);
+
+ LiveWindow *lw = LiveWindow::GetInstance();
+ // first and one-time initialization
+ SmartDashboard::init();
+ NetworkTable::GetTable("LiveWindow")
+ ->GetSubTable("~STATUS~")
+ ->PutBoolean("LW Enabled", false);
+ RobotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HALNetworkCommunicationObserveUserProgramStarting();
+
+ // loop forever, calling the appropriate mode-dependent function
+ lw->SetEnabled(false);
+ while (true) {
+ // 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_disabledInitialized) {
+ lw->SetEnabled(false);
+ DisabledInit();
+ m_disabledInitialized = true;
+ // reset the initialization flags for the other modes
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ m_testInitialized = false;
+ }
+ HALNetworkCommunicationObserveUserProgramDisabled();
+ 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_autonomousInitialized) {
+ lw->SetEnabled(false);
+ AutonomousInit();
+ m_autonomousInitialized = true;
+ // reset the initialization flags for the other modes
+ m_disabledInitialized = false;
+ m_teleopInitialized = false;
+ m_testInitialized = false;
+ }
+ HALNetworkCommunicationObserveUserProgramAutonomous();
+ AutonomousPeriodic();
+ } else if (IsTest()) {
+ // call TestInit() if we are now just entering test mode from
+ // either a different mode or from power-on
+ if (!m_testInitialized) {
+ lw->SetEnabled(true);
+ TestInit();
+ m_testInitialized = true;
+ // reset the initialization flags for the other modes
+ m_disabledInitialized = false;
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ }
+ HALNetworkCommunicationObserveUserProgramTest();
+ TestPeriodic();
+ } else {
+ // call TeleopInit() if we are now just entering teleop mode from
+ // either a different mode or from power-on
+ if (!m_teleopInitialized) {
+ lw->SetEnabled(false);
+ TeleopInit();
+ m_teleopInitialized = true;
+ // reset the initialization flags for the other modes
+ m_disabledInitialized = false;
+ m_autonomousInitialized = false;
+ m_testInitialized = false;
+ Scheduler::GetInstance()->SetEnabled(true);
+ }
+ HALNetworkCommunicationObserveUserProgramTeleop();
+ TeleopPeriodic();
+ }
+ // wait for driver station data so the loop doesn't hog the CPU
+ m_ds.WaitForData();
+ }
+}
+
+/**
+ * Robot-wide initialization code should go here.
+ *
+ * Users should override this method for default Robot-wide initialization which
+ * will be called when the robot is first powered on. It will be called exactly
+ * one time.
+ *
+ * Warning: the Driver Station "Robot Code" light and FMS "Robot Ready"
+ * indicators will be off until RobotInit() exits. Code in RobotInit() that
+ * waits for enable will cause the robot to never indicate that the code is
+ * ready, causing the robot to be bypassed in a match.
+ */
+void IterativeRobot::RobotInit() {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for disabled mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time
+ * the robot enters disabled mode.
+ */
+void IterativeRobot::DisabledInit() {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for autonomous mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time
+ * the robot enters autonomous mode.
+ */
+void IterativeRobot::AutonomousInit() {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for teleop mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time
+ * the robot enters teleop mode.
+ */
+void IterativeRobot::TeleopInit() {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for test mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time
+ * the robot enters test mode.
+ */
+void IterativeRobot::TestInit() {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Periodic code for disabled mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular
+ * rate while the robot is in disabled mode.
+ */
+void IterativeRobot::DisabledPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+ delayTicks(1);
+}
+
+/**
+ * Periodic code for autonomous mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular
+ * rate while the robot is in autonomous mode.
+ */
+void IterativeRobot::AutonomousPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+ delayTicks(1);
+}
+
+/**
+ * Periodic code for teleop mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular
+ * rate while the robot is in teleop mode.
+ */
+void IterativeRobot::TeleopPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+ delayTicks(1);
+}
+
+/**
+ * Periodic code for test mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular
+ * rate while the robot is in test mode.
+ */
+void IterativeRobot::TestPeriodic() {
+ static bool firstRun = true;
+ if (firstRun) {
+ printf("Default %s() method... Overload me!\n", __FUNCTION__);
+ firstRun = false;
+ }
+ delayTicks(1);
+}
diff --git a/wpilibc/Athena/src/Jaguar.cpp b/wpilibc/Athena/src/Jaguar.cpp
new file mode 100644
index 0000000..ce61d36
--- /dev/null
+++ b/wpilibc/Athena/src/Jaguar.cpp
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Jaguar.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Constructor for a Jaguar connected via PWM
+ * @param channel The PWM channel that the Jaguar is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+Jaguar::Jaguar(uint32_t channel) : SafePWM(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);
+ SetRaw(m_centerPwm);
+ SetZeroLatch();
+
+ HALReport(HALUsageReporting::kResourceType_Jaguar, GetChannel());
+ LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ * @param syncGroup Unused interface.
+ */
+void Jaguar::Set(float speed, uint8_t syncGroup) {
+ SetSpeed(m_isInverted ? -speed : speed);
+}
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+float Jaguar::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Jaguar::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+* Common interface for inverting direction of a speed controller.
+* @param isInverted The state of inversion, true is inverted.
+*/
+void Jaguar::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+/**
+ * Common interface for the inverting direction of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ *
+ */
+bool Jaguar::GetInverted() const { return m_isInverted; }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Jaguar::PIDWrite(float output) { Set(output); }
diff --git a/wpilibc/Athena/src/Joystick.cpp b/wpilibc/Athena/src/Joystick.cpp
new file mode 100644
index 0000000..388832c
--- /dev/null
+++ b/wpilibc/Athena/src/Joystick.cpp
@@ -0,0 +1,376 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Joystick.h"
+#include "DriverStation.h"
+#include "WPIErrors.h"
+#include <math.h>
+#include <string.h>
+
+const uint32_t Joystick::kDefaultXAxis;
+const uint32_t Joystick::kDefaultYAxis;
+const uint32_t Joystick::kDefaultZAxis;
+const uint32_t Joystick::kDefaultTwistAxis;
+const uint32_t Joystick::kDefaultThrottleAxis;
+const uint32_t Joystick::kDefaultTriggerButton;
+const uint32_t Joystick::kDefaultTopButton;
+static Joystick *joysticks[DriverStation::kJoystickPorts];
+static bool joySticksInitialized = false;
+
+/**
+ * Construct an instance of a joystick.
+ * The joystick index is the usb port on the drivers station.
+ *
+ * @param port The port on the driver station that the joystick is plugged into
+ * (0-5).
+ */
+Joystick::Joystick(uint32_t port)
+ : Joystick(port, kNumAxisTypes, kNumButtonTypes) {
+ m_axes[kXAxis] = kDefaultXAxis;
+ m_axes[kYAxis] = kDefaultYAxis;
+ m_axes[kZAxis] = kDefaultZAxis;
+ m_axes[kTwistAxis] = kDefaultTwistAxis;
+ m_axes[kThrottleAxis] = kDefaultThrottleAxis;
+
+ m_buttons[kTriggerButton] = kDefaultTriggerButton;
+ m_buttons[kTopButton] = kDefaultTopButton;
+
+ HALReport(HALUsageReporting::kResourceType_Joystick, port);
+}
+
+/**
+ * Version of the constructor to be called by sub-classes.
+ *
+ * This constructor allows the subclass to configure the number of constants
+ * for axes and buttons.
+ *
+ * @param port The port on the driver station that the joystick is plugged into.
+ * @param numAxisTypes The number of axis types in the enum.
+ * @param numButtonTypes The number of button types in the enum.
+ */
+Joystick::Joystick(uint32_t port, uint32_t numAxisTypes,
+ uint32_t numButtonTypes)
+ : m_ds(DriverStation::GetInstance()),
+ m_port(port),
+ m_axes(numAxisTypes),
+ m_buttons(numButtonTypes) {
+ if (!joySticksInitialized) {
+ for (auto& joystick : joysticks) joystick = nullptr;
+ joySticksInitialized = true;
+ }
+ if (m_port >= DriverStation::kJoystickPorts) {
+ wpi_setWPIError(BadJoystickIndex);
+ } else {
+ joysticks[m_port] = this;
+ }
+}
+
+Joystick *Joystick::GetStickForPort(uint32_t port) {
+ Joystick *stick = joysticks[port];
+ if (stick == nullptr) {
+ stick = new Joystick(port);
+ joysticks[port] = stick;
+ }
+ return stick;
+}
+
+/**
+ * Get the X value of the joystick.
+ * This depends on the mapping of the joystick connected to the current port.
+ * @param hand This parameter is ignored for the Joystick class and is only here
+ * to complete the GenericHID interface.
+ */
+float Joystick::GetX(JoystickHand hand) const {
+ return GetRawAxis(m_axes[kXAxis]);
+}
+
+/**
+ * Get the Y value of the joystick.
+ * This depends on the mapping of the joystick connected to the current port.
+ * @param hand This parameter is ignored for the Joystick class and is only here
+ * to complete the GenericHID interface.
+ */
+float Joystick::GetY(JoystickHand hand) const {
+ return GetRawAxis(m_axes[kYAxis]);
+}
+
+/**
+ * Get the Z value of the current joystick.
+ * This depends on the mapping of the joystick connected to the current port.
+ */
+float Joystick::GetZ() const { return GetRawAxis(m_axes[kZAxis]); }
+
+/**
+ * Get the twist value of the current joystick.
+ * This depends on the mapping of the joystick connected to the current port.
+ */
+float Joystick::GetTwist() const { return GetRawAxis(m_axes[kTwistAxis]); }
+
+/**
+ * Get the throttle value of the current joystick.
+ * This depends on the mapping of the joystick connected to the current port.
+ */
+float Joystick::GetThrottle() const {
+ return GetRawAxis(m_axes[kThrottleAxis]);
+}
+
+/**
+ * Get the value of the axis.
+ *
+ * @param axis The axis to read, starting at 0.
+ * @return The value of the axis.
+ */
+float Joystick::GetRawAxis(uint32_t axis) const {
+ return m_ds.GetStickAxis(m_port, axis);
+}
+
+/**
+ * For the current joystick, return the axis determined by the argument.
+ *
+ * This is for cases where the joystick axis is returned programatically,
+ * otherwise one of the
+ * previous functions would be preferable (for example GetX()).
+ *
+ * @param axis The axis to read.
+ * @return The value of the axis.
+ */
+float Joystick::GetAxis(AxisType axis) const {
+ switch (axis) {
+ case kXAxis:
+ return this->GetX();
+ case kYAxis:
+ return this->GetY();
+ case kZAxis:
+ return this->GetZ();
+ case kTwistAxis:
+ return this->GetTwist();
+ case kThrottleAxis:
+ return this->GetThrottle();
+ default:
+ wpi_setWPIError(BadJoystickAxis);
+ return 0.0;
+ }
+}
+
+/**
+ * Read the state of the trigger on the joystick.
+ *
+ * Look up which button has been assigned to the trigger and read its state.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only here
+ * to complete the GenericHID interface.
+ * @return The state of the trigger.
+ */
+bool Joystick::GetTrigger(JoystickHand hand) const {
+ return GetRawButton(m_buttons[kTriggerButton]);
+}
+
+/**
+ * Read the state of the top button on the joystick.
+ *
+ * Look up which button has been assigned to the top and read its state.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only here
+ * to complete the GenericHID interface.
+ * @return The state of the top button.
+ */
+bool Joystick::GetTop(JoystickHand hand) const {
+ return GetRawButton(m_buttons[kTopButton]);
+}
+
+/**
+ * This is not supported for the Joystick.
+ * This method is only here to complete the GenericHID interface.
+ */
+bool Joystick::GetBumper(JoystickHand hand) const {
+ // Joysticks don't have bumpers.
+ return false;
+}
+
+/**
+ * Get the button value (starting at button 1)
+ *
+ * The buttons are returned in a single 16 bit value with one bit representing
+ * the state
+ * of each button. The appropriate button is returned as a boolean value.
+ *
+ * @param button The button number to be read (starting at 1)
+ * @return The state of the button.
+ **/
+bool Joystick::GetRawButton(uint32_t button) const {
+ return m_ds.GetStickButton(m_port, button);
+}
+
+/**
+ * Get the state of a POV on the joystick.
+ *
+ * @param pov The index of the POV to read (starting at 0)
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+int Joystick::GetPOV(uint32_t pov) const {
+ return m_ds.GetStickPOV(m_port, pov);
+}
+
+/**
+ * Get buttons based on an enumerated type.
+ *
+ * The button type will be looked up in the list of buttons and then read.
+ *
+ * @param button The type of button to read.
+ * @return The state of the button.
+ */
+bool Joystick::GetButton(ButtonType button) const {
+ switch (button) {
+ case kTriggerButton:
+ return GetTrigger();
+ case kTopButton:
+ return GetTop();
+ default:
+ return false;
+ }
+}
+
+/**
+ * Get the number of axis for a joystick
+ *
+ * @return the number of axis for the current joystick
+ */
+int Joystick::GetAxisCount() const { return m_ds.GetStickAxisCount(m_port); }
+
+/**
+ * Get the value of isXbox for the joystick.
+ *
+ * @return A boolean that is true if the joystick is an xbox controller.
+ */
+bool Joystick::GetIsXbox() const { return m_ds.GetJoystickIsXbox(m_port); }
+
+/**
+ * Get the HID type of the controller.
+ *
+ * @return the HID type of the controller.
+ */
+Joystick::HIDType Joystick::GetType() const {
+ return static_cast<HIDType>(m_ds.GetJoystickType(m_port));
+}
+
+/**
+ * Get the name of the joystick.
+ *
+ * @return the name of the controller.
+ */
+std::string Joystick::GetName() const { return m_ds.GetJoystickName(m_port); }
+
+// int Joystick::GetAxisType(uint8_t axis) const
+//{
+// return m_ds.GetJoystickAxisType(m_port, axis);
+//}
+
+/**
+ * Get the number of axis for a joystick
+ *
+* @return the number of buttons on the current joystick
+ */
+int Joystick::GetButtonCount() const {
+ return m_ds.GetStickButtonCount(m_port);
+}
+
+/**
+ * Get the number of axis for a joystick
+ *
+ * @return then umber of POVs for the current joystick
+ */
+int Joystick::GetPOVCount() const { return m_ds.GetStickPOVCount(m_port); }
+
+/**
+ * Get the channel currently associated with the specified axis.
+ *
+ * @param axis The axis to look up the channel for.
+ * @return The channel fr the axis.
+ */
+uint32_t Joystick::GetAxisChannel(AxisType axis) const { return m_axes[axis]; }
+
+/**
+ * Set the channel associated with a specified axis.
+ *
+ * @param axis The axis to set the channel for.
+ * @param channel The channel to set the axis to.
+ */
+void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) {
+ m_axes[axis] = channel;
+}
+
+/**
+ * Get the magnitude of the direction vector formed by the joystick's
+ * current position relative to its origin
+ *
+ * @return The magnitude of the direction vector
+ */
+float Joystick::GetMagnitude() const {
+ return sqrt(pow(GetX(), 2) + pow(GetY(), 2));
+}
+
+/**
+ * Get the direction of the vector formed by the joystick and its origin
+ * in radians
+ *
+ * @return The direction of the vector in radians
+ */
+float Joystick::GetDirectionRadians() const { return atan2(GetX(), -GetY()); }
+
+/**
+ * Get the direction of the vector formed by the joystick and its origin
+ * in degrees
+ *
+ * uses acos(-1) to represent Pi due to absence of readily accessible Pi
+ * constant in C++
+ *
+ * @return The direction of the vector in degrees
+ */
+float Joystick::GetDirectionDegrees() const {
+ return (180 / acos(-1)) * GetDirectionRadians();
+}
+
+/**
+ * Set the rumble output for the joystick. The DS currently supports 2 rumble
+ * values,
+ * left rumble and right rumble
+ * @param type Which rumble value to set
+ * @param value The normalized value (0 to 1) to set the rumble to
+ */
+void Joystick::SetRumble(RumbleType type, float value) {
+ if (value < 0)
+ value = 0;
+ else if (value > 1)
+ value = 1;
+ if (type == kLeftRumble)
+ m_leftRumble = value * 65535;
+ else
+ m_rightRumble = value * 65535;
+ HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
+
+/**
+ * Set a single HID output value for the joystick.
+ * @param outputNumber The index of the output to set (1-32)
+ * @param value The value to set the output to
+ */
+
+void Joystick::SetOutput(uint8_t outputNumber, bool value) {
+ m_outputs =
+ (m_outputs & ~(1 << (outputNumber - 1))) | (value << (outputNumber - 1));
+
+ HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
+
+/**
+ * Set all HID output values for the joystick.
+ * @param value The 32 bit output value (1 bit for each output)
+ */
+void Joystick::SetOutputs(uint32_t value) {
+ m_outputs = value;
+ HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
diff --git a/wpilibc/Athena/src/MotorSafetyHelper.cpp b/wpilibc/Athena/src/MotorSafetyHelper.cpp
new file mode 100644
index 0000000..60bba30
--- /dev/null
+++ b/wpilibc/Athena/src/MotorSafetyHelper.cpp
@@ -0,0 +1,139 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "MotorSafetyHelper.h"
+
+#include "DriverStation.h"
+#include "MotorSafety.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+
+#include <stdio.h>
+#include <sstream>
+
+std::set<MotorSafetyHelper*> MotorSafetyHelper::m_helperList;
+priority_recursive_mutex MotorSafetyHelper::m_listMutex;
+
+/**
+ * The constructor for a MotorSafetyHelper object.
+ * The helper object is constructed for every object that wants to implement the
+ * Motor
+ * Safety protocol. The helper object has the code to actually do the timing and
+ * call the
+ * motors Stop() method when the timeout expires. The motor object is expected
+ * to call the
+ * Feed() method whenever the motors value is updated.
+ * @param safeObject a pointer to the motor object implementing MotorSafety.
+ * This is used
+ * to call the Stop() method on the motor.
+ */
+MotorSafetyHelper::MotorSafetyHelper(MotorSafety *safeObject)
+ : m_safeObject(safeObject) {
+ m_enabled = false;
+ m_expiration = DEFAULT_SAFETY_EXPIRATION;
+ m_stopTime = Timer::GetFPGATimestamp();
+
+ std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+ m_helperList.insert(this);
+}
+
+MotorSafetyHelper::~MotorSafetyHelper() {
+ std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+ m_helperList.erase(this);
+}
+
+/**
+ * Feed the motor safety object.
+ * Resets the timer on this object that is used to do the timeouts.
+ */
+void MotorSafetyHelper::Feed() {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
+}
+
+/**
+ * Set the expiration time for the corresponding motor safety object.
+ * @param expirationTime The timeout value in seconds.
+ */
+void MotorSafetyHelper::SetExpiration(float expirationTime) {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ m_expiration = expirationTime;
+}
+
+/**
+ * Retrieve the timeout value for the corresponding motor safety object.
+ * @return the timeout value in seconds.
+ */
+float MotorSafetyHelper::GetExpiration() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ return m_expiration;
+}
+
+/**
+ * Determine if the motor is still operating or has timed out.
+ * @return a true value if the motor is still operating normally and hasn't
+ * timed out.
+ */
+bool MotorSafetyHelper::IsAlive() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
+}
+
+/**
+ * Check if this motor has exceeded its timeout.
+ * This method is called periodically to determine if this motor has exceeded
+ * its timeout
+ * value. If it has, the stop method is called, and the motor is shut down until
+ * its value is
+ * updated again.
+ */
+void MotorSafetyHelper::Check() {
+ DriverStation &ds = DriverStation::GetInstance();
+ if (!m_enabled || ds.IsDisabled() || ds.IsTest()) return;
+
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ if (m_stopTime < Timer::GetFPGATimestamp()) {
+ std::ostringstream desc;
+ m_safeObject->GetDescription(desc);
+ desc << "... Output not updated often enough.";
+ wpi_setWPIErrorWithContext(Timeout, desc.str().c_str());
+ m_safeObject->StopMotor();
+ }
+}
+
+/**
+ * Enable/disable motor safety for this device
+ * Turn on and off the motor safety option for this PWM object.
+ * @param enabled True if motor safety is enforced for this object
+ */
+void MotorSafetyHelper::SetSafetyEnabled(bool enabled) {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ m_enabled = enabled;
+}
+
+/**
+ * Return the state of the motor safety enabled flag
+ * Return if the motor safety is currently enabled for this devicce.
+ * @return True if motor safety is enforced for this device
+ */
+bool MotorSafetyHelper::IsSafetyEnabled() const {
+ std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+ return m_enabled;
+}
+
+/**
+ * Check the motors to see if any have timed out.
+ * This static method is called periodically to poll all the motors and stop
+ * any that have
+ * timed out.
+ */
+void MotorSafetyHelper::CheckMotors() {
+ std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+ for (auto elem : m_helperList) {
+ elem->Check();
+ }
+}
diff --git a/wpilibc/Athena/src/Notifier.cpp b/wpilibc/Athena/src/Notifier.cpp
new file mode 100644
index 0000000..fc3059c
--- /dev/null
+++ b/wpilibc/Athena/src/Notifier.cpp
@@ -0,0 +1,263 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Notifier.h"
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include "HAL/HAL.hpp"
+
+Notifier *Notifier::timerQueueHead = nullptr;
+priority_recursive_mutex Notifier::queueMutex;
+priority_mutex Notifier::halMutex;
+void *Notifier::m_notifier = nullptr;
+std::atomic<int> Notifier::refcount{0};
+
+/**
+ * Create a Notifier for timer event notification.
+ * @param handler The handler is called at the notification time which is set
+ * using StartSingle or StartPeriodic.
+ */
+Notifier::Notifier(TimerEventHandler handler, void *param) {
+ if (handler == nullptr)
+ wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+ m_handler = handler;
+ m_param = param;
+ // do the first time intialization of static variables
+ if (refcount.fetch_add(1) == 0) {
+ int32_t status = 0;
+ {
+ std::lock_guard<priority_mutex> sync(halMutex);
+ if (!m_notifier)
+ m_notifier = initializeNotifier(ProcessQueue, nullptr, &status);
+ }
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+}
+
+/**
+ * Free the resources for a timer event.
+ * All resources will be freed and the timer event will be removed from the
+ * queue if necessary.
+ */
+Notifier::~Notifier() {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ DeleteFromQueue();
+ }
+
+ // Delete the static variables when the last one is going away
+ if (refcount.fetch_sub(1) == 1) {
+ int32_t status = 0;
+ {
+ std::lock_guard<priority_mutex> sync(halMutex);
+ if (m_notifier) {
+ cleanNotifier(m_notifier, &status);
+ m_notifier = nullptr;
+ }
+ }
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ }
+
+ // Acquire the mutex; this makes certain that the handler is
+ // not being executed by the interrupt manager.
+ std::lock_guard<priority_mutex> lock(m_handlerMutex);
+}
+
+/**
+ * Update the alarm hardware to reflect the current first element in the queue.
+ * Compute the time the next alarm should occur based on the current time and
+ * the
+ * period for the first element in the timer queue.
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere
+ * that is taking care of synchronizing access to the queue.
+ */
+void Notifier::UpdateAlarm() {
+ if (timerQueueHead != nullptr) {
+ int32_t status = 0;
+ // This locking is necessary in order to avoid two things:
+ // 1) Race condition issues with calling cleanNotifer() and
+ // updateNotifierAlarm() at the same time.
+ // 2) Avoid deadlock by making it so that this won't block waiting
+ // for the mutex to unlock.
+ // Checking refcount as well is unnecessary, but will not hurt.
+ if (halMutex.try_lock() && refcount != 0) {
+ if (m_notifier)
+ updateNotifierAlarm(m_notifier,
+ (uint32_t)(timerQueueHead->m_expirationTime * 1e6),
+ &status);
+ halMutex.unlock();
+ }
+ wpi_setStaticErrorWithContext(timerQueueHead, status,
+ getHALErrorMessage(status));
+ }
+}
+
+/**
+ * ProcessQueue is called whenever there is a timer interrupt.
+ * We need to wake up and process the current top item in the timer queue as
+ * long
+ * as its scheduled time is after the current time. Then the item is removed or
+ * rescheduled (repetitive events) in the queue.
+ */
+void Notifier::ProcessQueue(uint32_t currentTimeInt, void *params) {
+ Notifier *current;
+ while (true) // keep processing past events until no more
+ {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ double currentTime = currentTimeInt * 1.0e-6;
+ current = timerQueueHead;
+ if (current == nullptr || current->m_expirationTime > currentTime) {
+ break; // no more timer events to process
+ }
+ // need to process this entry
+ timerQueueHead = current->m_nextEvent;
+ if (current->m_periodic) {
+ // if periodic, requeue the event
+ // compute when to put into queue
+ current->InsertInQueue(true);
+ } else {
+ // not periodic; removed from queue
+ current->m_queued = false;
+ }
+ // Take handler mutex while holding queue mutex to make sure
+ // the handler will execute to completion in case we are being deleted.
+ current->m_handlerMutex.lock();
+ }
+
+ current->m_handler(current->m_param); // call the event handler
+ current->m_handlerMutex.unlock();
+ }
+ // reschedule the first item in the queue
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ UpdateAlarm();
+}
+
+/**
+ * Insert this Notifier into the timer queue in right place.
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere
+ * that is taking care of synchronizing access to the queue.
+ * @param reschedule If false, the scheduled alarm is based on the current time
+ * and UpdateAlarm
+ * method is called which will enable the alarm if necessary.
+ * If true, update the time by adding the period (no drift) when rescheduled
+ * periodic from ProcessQueue.
+ * This ensures that the public methods only update the queue after finishing
+ * inserting.
+ */
+void Notifier::InsertInQueue(bool reschedule) {
+ if (reschedule) {
+ m_expirationTime += m_period;
+ } else {
+ m_expirationTime = GetClock() + m_period;
+ }
+ if (m_expirationTime > Timer::kRolloverTime) {
+ m_expirationTime -= Timer::kRolloverTime;
+ }
+ if (timerQueueHead == nullptr ||
+ timerQueueHead->m_expirationTime >= this->m_expirationTime) {
+ // the queue is empty or greater than the new entry
+ // the new entry becomes the first element
+ this->m_nextEvent = timerQueueHead;
+ timerQueueHead = this;
+ if (!reschedule) {
+ // since the first element changed, update alarm, unless we already plan
+ // to
+ UpdateAlarm();
+ }
+ } else {
+ for (Notifier **npp = &(timerQueueHead->m_nextEvent);;
+ npp = &(*npp)->m_nextEvent) {
+ Notifier *n = *npp;
+ if (n == nullptr || n->m_expirationTime > this->m_expirationTime) {
+ *npp = this;
+ this->m_nextEvent = n;
+ break;
+ }
+ }
+ }
+ m_queued = true;
+}
+
+/**
+ * Delete this Notifier from the timer queue.
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere
+ * that is taking care of synchronizing access to the queue.
+ * Remove this Notifier from the timer queue and adjust the next interrupt time
+ * to reflect
+ * the current top of the queue.
+ */
+void Notifier::DeleteFromQueue() {
+ if (m_queued) {
+ m_queued = false;
+ wpi_assert(timerQueueHead != nullptr);
+ if (timerQueueHead == this) {
+ // remove the first item in the list - update the alarm
+ timerQueueHead = this->m_nextEvent;
+ UpdateAlarm();
+ } else {
+ for (Notifier *n = timerQueueHead; n != nullptr; n = n->m_nextEvent) {
+ if (n->m_nextEvent == this) {
+ // this element is the next element from *n from the queue
+ n->m_nextEvent = this->m_nextEvent; // point around this one
+ }
+ }
+ }
+ }
+}
+
+/**
+ * Register for single event notification.
+ * A timer event is queued for a single event after the specified delay.
+ * @param delay Seconds to wait before the handler is called.
+ */
+void Notifier::StartSingle(double delay) {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ m_periodic = false;
+ m_period = delay;
+ DeleteFromQueue();
+ InsertInQueue(false);
+}
+
+/**
+ * Register for periodic event notification.
+ * A timer event is queued for periodic event notification. Each time the
+ * interrupt
+ * occurs, the event will be immediately requeued for the same time interval.
+ * @param period Period in seconds to call the handler starting one period after
+ * the call to this method.
+ */
+void Notifier::StartPeriodic(double period) {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ m_periodic = true;
+ m_period = period;
+ DeleteFromQueue();
+ InsertInQueue(false);
+}
+
+/**
+ * Stop timer events from occuring.
+ * Stop any repeating timer events from occuring. This will also remove any
+ * single
+ * notification events from the queue.
+ * If a timer-based call to the registered handler is in progress, this function
+ * will
+ * block until the handler call is complete.
+ */
+void Notifier::Stop() {
+ {
+ std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+ DeleteFromQueue();
+ }
+ // Wait for a currently executing handler to complete before returning from
+ // Stop()
+ std::lock_guard<priority_mutex> sync(m_handlerMutex);
+}
diff --git a/wpilibc/Athena/src/PIDController.cpp b/wpilibc/Athena/src/PIDController.cpp
new file mode 100644
index 0000000..fdbbb5a
--- /dev/null
+++ b/wpilibc/Athena/src/PIDController.cpp
@@ -0,0 +1,582 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "PIDController.h"
+#include "Notifier.h"
+#include "PIDSource.h"
+#include "PIDOutput.h"
+#include <math.h>
+#include <vector>
+#include "HAL/HAL.hpp"
+
+static const std::string kP = "p";
+static const std::string kI = "i";
+static const std::string kD = "d";
+static const std::string kF = "f";
+static const std::string kSetpoint = "setpoint";
+static const std::string kEnabled = "enabled";
+
+/**
+ * Allocate a PID object with the given constants for P, I, D
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output value
+ * @param period the loop time for doing calculations. This particularly effects
+ * calculations of the
+ * integral and differental terms. The default is 50ms.
+ */
+PIDController::PIDController(float Kp, float Ki, float Kd, PIDSource *source,
+ PIDOutput *output, float period) {
+ Initialize(Kp, Ki, Kd, 0.0f, source, output, period);
+}
+
+/**
+ * Allocate a PID object with the given constants for P, I, D
+ * @param Kp the proportional coefficient
+ * @param Ki the integral coefficient
+ * @param Kd the derivative coefficient
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output value
+ * @param period the loop time for doing calculations. This particularly effects
+ * calculations of the
+ * integral and differental terms. The default is 50ms.
+ */
+PIDController::PIDController(float Kp, float Ki, float Kd, float Kf,
+ PIDSource *source, PIDOutput *output, float period) {
+ Initialize(Kp, Ki, Kd, Kf, source, output, period);
+}
+
+void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
+ PIDSource *source, PIDOutput *output,
+ float period) {
+ m_controlLoop = std::make_unique<Notifier>(PIDController::CallCalculate, this);
+
+ m_P = Kp;
+ m_I = Ki;
+ m_D = Kd;
+ m_F = Kf;
+
+ m_pidInput = source;
+ m_pidOutput = output;
+ m_period = period;
+
+ m_controlLoop->StartPeriodic(m_period);
+
+ static int32_t instances = 0;
+ instances++;
+ HALReport(HALUsageReporting::kResourceType_PIDController, instances);
+}
+
+PIDController::~PIDController() {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Call the Calculate method as a non-static method. This avoids having to
+ * prepend
+ * all local variables in that method with the class pointer. This way the
+ * "this"
+ * pointer will be set up and class variables can be called more easily.
+ * This method is static and called by the Notifier class.
+ * @param controller the address of the PID controller object to use in the
+ * background loop
+ */
+void PIDController::CallCalculate(void *controller) {
+ PIDController *control = (PIDController *)controller;
+ control->Calculate();
+}
+
+/**
+ * Read the input, calculate the output accordingly, and write to the output.
+ * This should only be called by the Notifier indirectly through CallCalculate
+ * and is created during initialization.
+ */
+void PIDController::Calculate() {
+ bool enabled;
+ PIDSource *pidInput;
+ PIDOutput *pidOutput;
+
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ pidInput = m_pidInput;
+ pidOutput = m_pidOutput;
+ enabled = m_enabled;
+ }
+
+ if (pidInput == nullptr) return;
+ if (pidOutput == nullptr) return;
+
+ if (enabled) {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ float input = pidInput->PIDGet();
+ float result;
+ PIDOutput *pidOutput;
+
+ m_error = m_setpoint - input;
+ if (m_continuous) {
+ if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
+ if (m_error > 0) {
+ m_error = m_error - m_maximumInput + m_minimumInput;
+ } else {
+ m_error = m_error + m_maximumInput - m_minimumInput;
+ }
+ }
+ }
+
+ if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+ if (m_P != 0) {
+ double potentialPGain = (m_totalError + m_error) * m_P;
+ if (potentialPGain < m_maximumOutput) {
+ if (potentialPGain > m_minimumOutput)
+ m_totalError += m_error;
+ else
+ m_totalError = m_minimumOutput / m_P;
+ } else {
+ m_totalError = m_maximumOutput / m_P;
+ }
+ }
+
+ m_result = m_D * m_error + m_P * m_totalError + m_setpoint * m_F;
+ }
+ else {
+ if (m_I != 0) {
+ double potentialIGain = (m_totalError + m_error) * m_I;
+ if (potentialIGain < m_maximumOutput) {
+ if (potentialIGain > m_minimumOutput)
+ m_totalError += m_error;
+ else
+ m_totalError = m_minimumOutput / m_I;
+ } else {
+ m_totalError = m_maximumOutput / m_I;
+ }
+ }
+
+ m_result = m_P * m_error + m_I * m_totalError +
+ m_D * (m_prevInput - input) + m_setpoint * m_F;
+ }
+ m_prevInput = input;
+
+ if (m_result > m_maximumOutput)
+ m_result = m_maximumOutput;
+ else if (m_result < m_minimumOutput)
+ m_result = m_minimumOutput;
+
+ pidOutput = m_pidOutput;
+ result = m_result;
+
+ pidOutput->PIDWrite(result);
+
+ // Update the buffer.
+ m_buf.push(m_error);
+ m_bufTotal += m_error;
+ // Remove old elements when buffer is full.
+ if (m_buf.size() > m_bufLength) {
+ m_bufTotal -= m_buf.front();
+ m_buf.pop();
+ }
+ }
+}
+
+/**
+ * Set the PID Controller gain parameters.
+ * Set the proportional, integral, and differential coefficients.
+ * @param p Proportional coefficient
+ * @param i Integral coefficient
+ * @param d Differential coefficient
+ */
+void PIDController::SetPID(double p, double i, double d) {
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ }
+
+ if (m_table != nullptr) {
+ m_table->PutNumber("p", m_P);
+ m_table->PutNumber("i", m_I);
+ m_table->PutNumber("d", m_D);
+ }
+}
+
+/**
+ * Set the PID Controller gain parameters.
+ * Set the proportional, integral, and differential coefficients.
+ * @param p Proportional coefficient
+ * @param i Integral coefficient
+ * @param d Differential coefficient
+ * @param f Feed forward coefficient
+ */
+void PIDController::SetPID(double p, double i, double d, double f) {
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_P = p;
+ m_I = i;
+ m_D = d;
+ m_F = f;
+ }
+
+ if (m_table != nullptr) {
+ m_table->PutNumber("p", m_P);
+ m_table->PutNumber("i", m_I);
+ m_table->PutNumber("d", m_D);
+ m_table->PutNumber("f", m_F);
+ }
+}
+
+/**
+ * Get the Proportional coefficient
+ * @return proportional coefficient
+ */
+double PIDController::GetP() const {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ return m_P;
+}
+
+/**
+ * Get the Integral coefficient
+ * @return integral coefficient
+ */
+double PIDController::GetI() const {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ return m_I;
+}
+
+/**
+ * Get the Differential coefficient
+ * @return differential coefficient
+ */
+double PIDController::GetD() const {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ return m_D;
+}
+
+/**
+ * Get the Feed forward coefficient
+ * @return Feed forward coefficient
+ */
+double PIDController::GetF() const {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ return m_F;
+}
+
+/**
+ * Return the current PID result
+ * This is always centered on zero and constrained the the max and min outs
+ * @return the latest calculated output
+ */
+float PIDController::Get() const {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ return m_result;
+}
+
+/**
+ * Set the PID controller to consider the input to be continuous,
+ * Rather then using the max and min in as constraints, it considers them to
+ * be the same point and automatically calculates the shortest route to
+ * the setpoint.
+ * @param continuous Set to true turns on continuous, false turns off continuous
+ */
+void PIDController::SetContinuous(bool continuous) {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_continuous = continuous;
+}
+
+/**
+ * Sets the maximum and minimum values expected from the input.
+ *
+ * @param minimumInput the minimum value expected from the input
+ * @param maximumInput the maximum value expected from the output
+ */
+void PIDController::SetInputRange(float minimumInput, float maximumInput) {
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_minimumInput = minimumInput;
+ m_maximumInput = maximumInput;
+ }
+
+ SetSetpoint(m_setpoint);
+}
+
+/**
+ * Sets the minimum and maximum values to write.
+ *
+ * @param minimumOutput the minimum value to write to the output
+ * @param maximumOutput the maximum value to write to the output
+ */
+void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_minimumOutput = minimumOutput;
+ m_maximumOutput = maximumOutput;
+ }
+}
+
+/**
+ * Set the setpoint for the PIDController
+ * Clears the queue for GetAvgError().
+ * @param setpoint the desired setpoint
+ */
+void PIDController::SetSetpoint(float setpoint) {
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ 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;
+ }
+
+ // Clear m_buf.
+ m_buf = std::queue<double>();
+ }
+
+ if (m_table != nullptr) {
+ m_table->PutNumber("setpoint", m_setpoint);
+ }
+}
+
+/**
+ * Returns the current setpoint of the PIDController
+ * @return the current setpoint
+ */
+double PIDController::GetSetpoint() const {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ return m_setpoint;
+}
+
+/**
+ * Returns the current difference of the input from the setpoint
+ * @return the current error
+ */
+float PIDController::GetError() const {
+ double pidInput;
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ pidInput = m_pidInput->PIDGet();
+ }
+ return GetSetpoint() - pidInput;
+}
+
+/**
+ * Sets what type of input the PID controller will use
+ */
+void PIDController::SetPIDSourceType(PIDSourceType pidSource) {
+ m_pidInput->SetPIDSourceType(pidSource);
+}
+/**
+ * Returns the type of input the PID controller is using
+ * @return the PID controller input type
+ */
+PIDSourceType PIDController::GetPIDSourceType() const {
+ return m_pidInput->GetPIDSourceType();
+}
+
+/**
+ * Returns the current average of the error over the past few iterations.
+ * You can specify the number of iterations to average with SetToleranceBuffer()
+ * (defaults to 1). This is the same value that is used for OnTarget().
+ * @return the average error
+ */
+float PIDController::GetAvgError() const {
+ float avgError = 0;
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ // Don't divide by zero.
+ if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
+ }
+ return avgError;
+}
+
+/*
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDController::SetTolerance(float percent) {
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_toleranceType = kPercentTolerance;
+ m_tolerance = percent;
+ }
+}
+
+/*
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDController::SetPercentTolerance(float percent) {
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_toleranceType = kPercentTolerance;
+ m_tolerance = percent;
+ }
+}
+
+/*
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDController::SetAbsoluteTolerance(float absTolerance) {
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_toleranceType = kAbsoluteTolerance;
+ m_tolerance = absTolerance;
+ }
+}
+
+/*
+ * Set the number of previous error samples to average for tolerancing. When
+ * determining whether a mechanism is on target, the user may want to use a
+ * rolling average of previous measurements instead of a precise position or
+ * velocity. This is useful for noisy sensors which return a few erroneous
+ * measurements when the mechanism is on target. However, the mechanism will
+ * not register as on target for at least the specified bufLength cycles.
+ * @param bufLength Number of previous cycles to average. Defaults to 1.
+ */
+void PIDController::SetToleranceBuffer(unsigned bufLength) {
+ m_bufLength = bufLength;
+
+ // Cut the buffer down to size if needed.
+ while (m_buf.size() > bufLength) {
+ m_bufTotal -= m_buf.front();
+ m_buf.pop();
+ }
+}
+
+/*
+ * Return true if the error is within the percentage of the total input range,
+ * determined by SetTolerance. This asssumes that the maximum and minimum input
+ * were set using SetInput.
+ * Currently this just reports on target as the actual value passes through the
+ * setpoint.
+ * Ideally it should be based on being within the tolerance for some period of
+ * time.
+ */
+bool PIDController::OnTarget() const {
+ double error = GetAvgError();
+
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ switch (m_toleranceType) {
+ case kPercentTolerance:
+ return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
+ break;
+ case kAbsoluteTolerance:
+ return fabs(error) < m_tolerance;
+ break;
+ case kNoTolerance:
+ // TODO: this case needs an error
+ return false;
+ }
+ return false;
+}
+
+/**
+ * Begin running the PIDController
+ */
+void PIDController::Enable() {
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_enabled = true;
+ }
+
+ if (m_table != nullptr) {
+ m_table->PutBoolean("enabled", true);
+ }
+}
+
+/**
+ * Stop running the PIDController, this sets the output to zero before stopping.
+ */
+void PIDController::Disable() {
+ {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_pidOutput->PIDWrite(0);
+ m_enabled = false;
+ }
+
+ if (m_table != nullptr) {
+ m_table->PutBoolean("enabled", false);
+ }
+}
+
+/**
+ * Return true if PIDController is enabled.
+ */
+bool PIDController::IsEnabled() const {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ return m_enabled;
+}
+
+/**
+ * Reset the previous error,, the integral term, and disable the controller.
+ */
+void PIDController::Reset() {
+ Disable();
+
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_prevInput = 0;
+ m_totalError = 0;
+ m_result = 0;
+}
+
+std::string PIDController::GetSmartDashboardType() const {
+ return "PIDController";
+}
+
+void PIDController::InitTable(std::shared_ptr<ITable> table) {
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+ m_table = table;
+ if (m_table != nullptr) {
+ m_table->PutNumber(kP, GetP());
+ m_table->PutNumber(kI, GetI());
+ m_table->PutNumber(kD, GetD());
+ m_table->PutNumber(kF, GetF());
+ m_table->PutNumber(kSetpoint, GetSetpoint());
+ m_table->PutBoolean(kEnabled, IsEnabled());
+ m_table->AddTableListener(this, false);
+ }
+}
+
+std::shared_ptr<ITable> PIDController::GetTable() const { return m_table; }
+
+void PIDController::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (key == kP || key == kI || key == kD || key == kF) {
+ if (m_P != m_table->GetNumber(kP, 0.0) ||
+ m_I != m_table->GetNumber(kI, 0.0) ||
+ m_D != m_table->GetNumber(kD, 0.0) ||
+ m_F != m_table->GetNumber(kF, 0.0)) {
+ SetPID(m_table->GetNumber(kP, 0.0), m_table->GetNumber(kI, 0.0),
+ m_table->GetNumber(kD, 0.0), m_table->GetNumber(kF, 0.0));
+ }
+ } else if (key == kSetpoint && value->IsDouble() &&
+ m_setpoint != value->GetDouble()) {
+ SetSetpoint(value->GetDouble());
+ } else if (key == kEnabled && value->IsBoolean() &&
+ m_enabled != value->GetBoolean()) {
+ if (value->GetBoolean()) {
+ Enable();
+ } else {
+ Disable();
+ }
+ }
+}
+
+void PIDController::UpdateTable() {}
+
+void PIDController::StartLiveWindowMode() { Disable(); }
+
+void PIDController::StopLiveWindowMode() {}
diff --git a/wpilibc/Athena/src/PWM.cpp b/wpilibc/Athena/src/PWM.cpp
new file mode 100644
index 0000000..6b3b4b7
--- /dev/null
+++ b/wpilibc/Athena/src/PWM.cpp
@@ -0,0 +1,374 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "PWM.h"
+
+#include "Resource.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include "HAL/HAL.hpp"
+
+#include <sstream>
+
+constexpr float PWM::kDefaultPwmPeriod;
+constexpr float PWM::kDefaultPwmCenter;
+const int32_t PWM::kDefaultPwmStepsDown;
+const int32_t PWM::kPwmDisabled;
+
+/**
+ * Allocate a PWM given a channel number.
+ *
+ * Checks channel value range and allocates the appropriate channel.
+ * The allocation is only done to help users ensure that they don't double
+ * assign channels.
+ * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP
+ * port
+ */
+PWM::PWM(uint32_t channel) {
+ std::stringstream buf;
+
+ if (!CheckPWMChannel(channel)) {
+ buf << "PWM Channel " << channel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ return;
+ }
+
+ int32_t status = 0;
+ allocatePWMChannel(m_pwm_ports[channel], &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ m_channel = channel;
+
+ setPWM(m_pwm_ports[m_channel], kPwmDisabled, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ m_eliminateDeadband = false;
+
+ HALReport(HALUsageReporting::kResourceType_PWM, channel);
+}
+
+/**
+ * Free the PWM channel.
+ *
+ * Free the resource associated with the PWM channel and set the value to 0.
+ */
+PWM::~PWM() {
+ int32_t status = 0;
+
+ setPWM(m_pwm_ports[m_channel], kPwmDisabled, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ freePWMChannel(m_pwm_ports[m_channel], &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Optionally eliminate the deadband from a speed controller.
+ * @param eliminateDeadband If true, set the motor curve on the Jaguar to
+ * eliminate
+ * the deadband in the middle of the range. Otherwise, keep the full range
+ * without
+ * modifying any values.
+ */
+void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
+ if (StatusIsFatal()) return;
+ m_eliminateDeadband = eliminateDeadband;
+}
+
+/**
+ * Set the bounds on the PWM values.
+ * This sets the bounds on the PWM values for a particular each type of
+ * controller. The values
+ * determine the upper and lower speeds as well as the deadband bracket.
+ * @param max The Minimum pwm value
+ * @param deadbandMax The high end of the deadband range
+ * @param center The center speed (off)
+ * @param deadbandMin The low end of the deadband range
+ * @param min The minimum pwm value
+ */
+void PWM::SetBounds(int32_t max, int32_t deadbandMax, int32_t center,
+ int32_t deadbandMin, int32_t min) {
+ if (StatusIsFatal()) return;
+ m_maxPwm = max;
+ m_deadbandMaxPwm = deadbandMax;
+ m_centerPwm = center;
+ m_deadbandMinPwm = deadbandMin;
+ m_minPwm = min;
+}
+
+/**
+ * Set the bounds on the PWM pulse widths.
+ * This sets the bounds on the PWM values for a particular type of controller.
+ * The values
+ * determine the upper and lower speeds as well as the deadband bracket.
+ * @param max The max PWM pulse width in ms
+ * @param deadbandMax The high end of the deadband range pulse width in ms
+ * @param center The center (off) pulse width in ms
+ * @param deadbandMin The low end of the deadband pulse width in ms
+ * @param min The minimum pulse width in ms
+ */
+void PWM::SetBounds(double max, double deadbandMax, double center,
+ double deadbandMin, double min) {
+ // calculate the loop time in milliseconds
+ int32_t status = 0;
+ double loopTime =
+ getLoopTiming(&status) / (kSystemClockTicksPerMicrosecond * 1e3);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ if (StatusIsFatal()) return;
+
+ m_maxPwm = (int32_t)((max - kDefaultPwmCenter) / loopTime +
+ kDefaultPwmStepsDown - 1);
+ m_deadbandMaxPwm = (int32_t)((deadbandMax - kDefaultPwmCenter) / loopTime +
+ kDefaultPwmStepsDown - 1);
+ m_centerPwm = (int32_t)((center - kDefaultPwmCenter) / loopTime +
+ kDefaultPwmStepsDown - 1);
+ m_deadbandMinPwm = (int32_t)((deadbandMin - kDefaultPwmCenter) / loopTime +
+ kDefaultPwmStepsDown - 1);
+ m_minPwm = (int32_t)((min - kDefaultPwmCenter) / loopTime +
+ kDefaultPwmStepsDown - 1);
+}
+
+/**
+ * Set the PWM value based on a position.
+ *
+ * This is intended to be used by servos.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @param pos The position to set the servo between 0.0 and 1.0.
+ */
+void PWM::SetPosition(float pos) {
+ if (StatusIsFatal()) return;
+ if (pos < 0.0) {
+ pos = 0.0;
+ } else if (pos > 1.0) {
+ pos = 1.0;
+ }
+
+ // note, need to perform the multiplication below as floating point before
+ // converting to int
+ unsigned short rawValue =
+ (int32_t)((pos * (float)GetFullRangeScaleFactor()) + GetMinNegativePwm());
+ // printf("MinNegPWM: %d FullRangeScaleFactor: %d Raw value: %5d Input
+ //value: %4.4f\n", GetMinNegativePwm(), GetFullRangeScaleFactor(), rawValue,
+ //pos);
+
+ // wpi_assert((rawValue >= GetMinNegativePwm()) && (rawValue <=
+ //GetMaxPositivePwm()));
+ wpi_assert(rawValue != kPwmDisabled);
+
+ // send the computed pwm value to the FPGA
+ SetRaw((unsigned short)rawValue);
+}
+
+/**
+ * Get the PWM value in terms of a position.
+ *
+ * This is intended to be used by servos.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @return The position the servo is set to between 0.0 and 1.0.
+ */
+float PWM::GetPosition() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t value = GetRaw();
+ if (value < GetMinNegativePwm()) {
+ return 0.0;
+ } else if (value > GetMaxPositivePwm()) {
+ return 1.0;
+ } else {
+ return (float)(value - GetMinNegativePwm()) /
+ (float)GetFullRangeScaleFactor();
+ }
+}
+
+/**
+ * Set the PWM value based on a speed.
+ *
+ * This is intended to be used by speed controllers.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetCenterPwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @param speed The speed to set the speed controller between -1.0 and 1.0.
+ */
+void PWM::SetSpeed(float speed) {
+ if (StatusIsFatal()) return;
+ // clamp speed to be in the range 1.0 >= speed >= -1.0
+ if (speed < -1.0) {
+ speed = -1.0;
+ } else if (speed > 1.0) {
+ speed = 1.0;
+ }
+
+ // calculate the desired output pwm value by scaling the speed appropriately
+ int32_t rawValue;
+ if (speed == 0.0) {
+ rawValue = GetCenterPwm();
+ } else if (speed > 0.0) {
+ rawValue = (int32_t)(speed * ((float)GetPositiveScaleFactor()) +
+ ((float)GetMinPositivePwm()) + 0.5);
+ } else {
+ rawValue = (int32_t)(speed * ((float)GetNegativeScaleFactor()) +
+ ((float)GetMaxNegativePwm()) + 0.5);
+ }
+
+ // the above should result in a pwm_value in the valid range
+ wpi_assert((rawValue >= GetMinNegativePwm()) &&
+ (rawValue <= GetMaxPositivePwm()));
+ wpi_assert(rawValue != kPwmDisabled);
+
+ // send the computed pwm value to the FPGA
+ SetRaw(rawValue);
+}
+
+/**
+ * Get the PWM value in terms of speed.
+ *
+ * This is intended to be used by speed controllers.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @return The most recently set speed between -1.0 and 1.0.
+ */
+float PWM::GetSpeed() const {
+ if (StatusIsFatal()) return 0.0;
+ int32_t value = GetRaw();
+ if (value == PWM::kPwmDisabled) {
+ return 0.0;
+ } else if (value > GetMaxPositivePwm()) {
+ return 1.0;
+ } else if (value < GetMinNegativePwm()) {
+ return -1.0;
+ } else if (value > GetMinPositivePwm()) {
+ return (float)(value - GetMinPositivePwm()) /
+ (float)GetPositiveScaleFactor();
+ } else if (value < GetMaxNegativePwm()) {
+ return (float)(value - GetMaxNegativePwm()) /
+ (float)GetNegativeScaleFactor();
+ } else {
+ return 0.0;
+ }
+}
+
+/**
+ * Set the PWM value directly to the hardware.
+ *
+ * Write a raw value to a PWM channel.
+ *
+ * @param value Raw PWM value.
+ */
+void PWM::SetRaw(unsigned short value) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+ setPWM(m_pwm_ports[m_channel], value, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Get the PWM value directly from the hardware.
+ *
+ * Read a raw value from a PWM channel.
+ *
+ * @return Raw PWM control value.
+ */
+unsigned short PWM::GetRaw() const {
+ if (StatusIsFatal()) return 0;
+
+ int32_t status = 0;
+ unsigned short value = getPWM(m_pwm_ports[m_channel], &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ return value;
+}
+
+/**
+ * Slow down the PWM signal for old devices.
+ *
+ * @param mult The period multiplier to apply to this channel
+ */
+void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+
+ switch (mult) {
+ case kPeriodMultiplier_4X:
+ setPWMPeriodScale(m_pwm_ports[m_channel], 3,
+ &status); // Squelch 3 out of 4 outputs
+ break;
+ case kPeriodMultiplier_2X:
+ setPWMPeriodScale(m_pwm_ports[m_channel], 1,
+ &status); // Squelch 1 out of 2 outputs
+ break;
+ case kPeriodMultiplier_1X:
+ setPWMPeriodScale(m_pwm_ports[m_channel], 0,
+ &status); // Don't squelch any outputs
+ break;
+ default:
+ wpi_assert(false);
+ }
+
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+void PWM::SetZeroLatch() {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+
+ latchPWMZero(m_pwm_ports[m_channel], &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+void PWM::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsDouble()) return;
+ SetSpeed(value->GetDouble());
+}
+
+void PWM::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetSpeed());
+ }
+}
+
+void PWM::StartLiveWindowMode() {
+ SetSpeed(0);
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void PWM::StopLiveWindowMode() {
+ SetSpeed(0);
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string PWM::GetSmartDashboardType() const { return "Speed Controller"; }
+
+void PWM::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> PWM::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/PowerDistributionPanel.cpp b/wpilibc/Athena/src/PowerDistributionPanel.cpp
new file mode 100644
index 0000000..fb428cf
--- /dev/null
+++ b/wpilibc/Athena/src/PowerDistributionPanel.cpp
@@ -0,0 +1,190 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "PowerDistributionPanel.h"
+#include "WPIErrors.h"
+#include "HAL/PDP.hpp"
+#include "LiveWindow/LiveWindow.h"
+
+#include <sstream>
+
+PowerDistributionPanel::PowerDistributionPanel() : PowerDistributionPanel(0) {}
+
+/**
+ * Initialize the PDP.
+ */
+PowerDistributionPanel::PowerDistributionPanel(uint8_t module)
+ : m_module(module) {
+ initializePDP(m_module);
+}
+
+/**
+ * Query the input voltage of the PDP
+ * @return The voltage of the PDP in volts
+ */
+double PowerDistributionPanel::GetVoltage() const {
+ int32_t status = 0;
+
+ double voltage = getPDPVoltage(m_module, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return voltage;
+}
+
+/**
+ * Query the temperature of the PDP
+ * @return The temperature of the PDP in degrees Celsius
+ */
+double PowerDistributionPanel::GetTemperature() const {
+ int32_t status = 0;
+
+ double temperature = getPDPTemperature(m_module, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return temperature;
+}
+
+/**
+ * Query the current of a single channel of the PDP
+ * @return The current of one of the PDP channels (channels 0-15) in Amperes
+ */
+double PowerDistributionPanel::GetCurrent(uint8_t channel) const {
+ int32_t status = 0;
+
+ if (!CheckPDPChannel(channel)) {
+ std::stringstream buf;
+ buf << "PDP Channel " << channel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ }
+
+ double current = getPDPChannelCurrent(m_module, channel, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return current;
+}
+
+/**
+ * Query the total current of all monitored PDP channels (0-15)
+ * @return The the total current drawn from the PDP channels in Amperes
+ */
+double PowerDistributionPanel::GetTotalCurrent() const {
+ int32_t status = 0;
+
+ double current = getPDPTotalCurrent(m_module, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return current;
+}
+
+/**
+ * Query the total power drawn from the monitored PDP channels
+ * @return The the total power drawn from the PDP channels in Watts
+ */
+double PowerDistributionPanel::GetTotalPower() const {
+ int32_t status = 0;
+
+ double power = getPDPTotalPower(m_module, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return power;
+}
+
+/**
+ * Query the total energy drawn from the monitored PDP channels
+ * @return The the total energy drawn from the PDP channels in Joules
+ */
+double PowerDistributionPanel::GetTotalEnergy() const {
+ int32_t status = 0;
+
+ double energy = getPDPTotalEnergy(m_module, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+
+ return energy;
+}
+
+/**
+ * Reset the total energy drawn from the PDP
+ * @see PowerDistributionPanel#GetTotalEnergy
+ */
+void PowerDistributionPanel::ResetTotalEnergy() {
+ int32_t status = 0;
+
+ resetPDPTotalEnergy(m_module, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+}
+
+/**
+ * Remove all of the fault flags on the PDP
+ */
+void PowerDistributionPanel::ClearStickyFaults() {
+ int32_t status = 0;
+
+ clearPDPStickyFaults(m_module, &status);
+
+ if (status) {
+ wpi_setWPIErrorWithContext(Timeout, "");
+ }
+}
+
+void PowerDistributionPanel::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Chan0", GetCurrent(0));
+ m_table->PutNumber("Chan1", GetCurrent(1));
+ m_table->PutNumber("Chan2", GetCurrent(2));
+ m_table->PutNumber("Chan3", GetCurrent(3));
+ m_table->PutNumber("Chan4", GetCurrent(4));
+ m_table->PutNumber("Chan5", GetCurrent(5));
+ m_table->PutNumber("Chan6", GetCurrent(6));
+ m_table->PutNumber("Chan7", GetCurrent(7));
+ m_table->PutNumber("Chan8", GetCurrent(8));
+ m_table->PutNumber("Chan9", GetCurrent(9));
+ m_table->PutNumber("Chan10", GetCurrent(10));
+ m_table->PutNumber("Chan11", GetCurrent(11));
+ m_table->PutNumber("Chan12", GetCurrent(12));
+ m_table->PutNumber("Chan13", GetCurrent(13));
+ m_table->PutNumber("Chan14", GetCurrent(14));
+ m_table->PutNumber("Chan15", GetCurrent(15));
+ m_table->PutNumber("Voltage", GetVoltage());
+ m_table->PutNumber("TotalCurrent", GetTotalCurrent());
+ }
+}
+
+void PowerDistributionPanel::StartLiveWindowMode() {}
+
+void PowerDistributionPanel::StopLiveWindowMode() {}
+
+std::string PowerDistributionPanel::GetSmartDashboardType() const {
+ return "PowerDistributionPanel";
+}
+
+void PowerDistributionPanel::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> PowerDistributionPanel::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/Preferences.cpp b/wpilibc/Athena/src/Preferences.cpp
new file mode 100644
index 0000000..95f6a4a
--- /dev/null
+++ b/wpilibc/Athena/src/Preferences.cpp
@@ -0,0 +1,217 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Preferences.h"
+
+#include "WPIErrors.h"
+#include "HAL/HAL.hpp"
+
+#include <stdio.h>
+#include <algorithm>
+
+/** The Preferences table name */
+static const char *kTableName = "Preferences";
+
+void Preferences::Listener::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value,
+ bool isNew) {}
+void Preferences::Listener::ValueChangedEx(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value,
+ unsigned int flags) {
+ source->SetPersistent(key);
+}
+
+Preferences::Preferences() : m_table(NetworkTable::GetTable(kTableName)) {
+ m_table->AddTableListenerEx(&m_listener, NT_NOTIFY_NEW | NT_NOTIFY_IMMEDIATE);
+ HALReport(HALUsageReporting::kResourceType_Preferences, 0);
+}
+
+/**
+ * Get the one and only {@link Preferences} object
+ * @return pointer to the {@link Preferences}
+ */
+Preferences *Preferences::GetInstance() {
+ static Preferences instance;
+ return &instance;
+}
+
+/**
+ * Returns a vector of all the keys
+ * @return a vector of the keys
+ */
+std::vector<std::string> Preferences::GetKeys() { return m_table->GetKeys(); }
+
+/**
+ * Returns the string at the given key. If this table does not have a value
+ * for that position, then the given defaultValue will be returned.
+ * @param key the key
+ * @param defaultValue the value to return if none exists in the table
+ * @return either the value in the table, or the defaultValue
+ */
+std::string Preferences::GetString(llvm::StringRef key,
+ llvm::StringRef defaultValue) {
+ return m_table->GetString(key, defaultValue);
+}
+
+/**
+ * Returns the int at the given key. If this table does not have a value
+ * for that position, then the given defaultValue value will be returned.
+ * @param key the key
+ * @param defaultValue the value to return if none exists in the table
+ * @return either the value in the table, or the defaultValue
+ */
+int Preferences::GetInt(llvm::StringRef key, int defaultValue) {
+ return static_cast<int>(m_table->GetNumber(key, defaultValue));
+}
+
+/**
+ * Returns the double at the given key. If this table does not have a value
+ * for that position, then the given defaultValue value will be returned.
+ * @param key the key
+ * @param defaultValue the value to return if none exists in the table
+ * @return either the value in the table, or the defaultValue
+ */
+double Preferences::GetDouble(llvm::StringRef key, double defaultValue) {
+ return m_table->GetNumber(key, defaultValue);
+}
+
+/**
+ * Returns the float at the given key. If this table does not have a value
+ * for that position, then the given defaultValue value will be returned.
+ * @param key the key
+ * @param defaultValue the value to return if none exists in the table
+ * @return either the value in the table, or the defaultValue
+ */
+float Preferences::GetFloat(llvm::StringRef key, float defaultValue) {
+ return static_cast<float>(m_table->GetNumber(key, defaultValue));
+}
+
+/**
+ * Returns the boolean at the given key. If this table does not have a value
+ * for that position, then the given defaultValue value will be returned.
+ * @param key the key
+ * @param defaultValue the value to return if none exists in the table
+ * @return either the value in the table, or the defaultValue
+ */
+bool Preferences::GetBoolean(llvm::StringRef key, bool defaultValue) {
+ return m_table->GetBoolean(key, defaultValue);
+}
+
+/**
+ * Returns the long (int64_t) at the given key. If this table does not have a
+ * value
+ * for that position, then the given defaultValue value will be returned.
+ * @param key the key
+ * @param defaultValue the value to return if none exists in the table
+ * @return either the value in the table, or the defaultValue
+ */
+int64_t Preferences::GetLong(llvm::StringRef key, int64_t defaultValue) {
+ return static_cast<int64_t>(m_table->GetNumber(key, defaultValue));
+}
+
+/**
+ * Puts the given string into the preferences table.
+ *
+ * <p>The value may not have quotation marks, nor may the key
+ * have any whitespace nor an equals sign</p>
+ *
+ * @param key the key
+ * @param value the value
+ */
+void Preferences::PutString(llvm::StringRef key, llvm::StringRef value) {
+ m_table->PutString(key, value);
+ m_table->SetPersistent(key);
+}
+
+/**
+ * Puts the given int into the preferences table.
+ *
+ * <p>The key may not have any whitespace nor an equals sign</p>
+ *
+ * @param key the key
+ * @param value the value
+ */
+void Preferences::PutInt(llvm::StringRef key, int value) {
+ m_table->PutNumber(key, value);
+ m_table->SetPersistent(key);
+}
+
+/**
+ * Puts the given double into the preferences table.
+ *
+ * <p>The key may not have any whitespace nor an equals sign</p>
+ *
+ * @param key the key
+ * @param value the value
+ */
+void Preferences::PutDouble(llvm::StringRef key, double value) {
+ m_table->PutNumber(key, value);
+ m_table->SetPersistent(key);
+}
+
+/**
+ * Puts the given float into the preferences table.
+ *
+ * <p>The key may not have any whitespace nor an equals sign</p>
+ *
+ * @param key the key
+ * @param value the value
+ */
+void Preferences::PutFloat(llvm::StringRef key, float value) {
+ m_table->PutNumber(key, value);
+ m_table->SetPersistent(key);
+}
+
+/**
+ * Puts the given boolean into the preferences table.
+ *
+ * <p>The key may not have any whitespace nor an equals sign</p>
+ *
+ * @param key the key
+ * @param value the value
+ */
+void Preferences::PutBoolean(llvm::StringRef key, bool value) {
+ m_table->PutBoolean(key, value);
+ m_table->SetPersistent(key);
+}
+
+/**
+ * Puts the given long (int64_t) into the preferences table.
+ *
+ * <p>The key may not have any whitespace nor an equals sign</p>
+ *
+ * @param key the key
+ * @param value the value
+ */
+void Preferences::PutLong(llvm::StringRef key, int64_t value) {
+ m_table->PutNumber(key, value);
+ m_table->SetPersistent(key);
+}
+
+/**
+ * This function is no longer required, as NetworkTables automatically
+ * saves persistent values (which all Preferences values are) periodically
+ * when running as a server.
+ * @deprecated backwards compatibility shim
+ */
+void Preferences::Save() {}
+
+/**
+ * Returns whether or not there is a key with the given name.
+ * @param key the key
+ * @return if there is a value at the given key
+ */
+bool Preferences::ContainsKey(llvm::StringRef key) {
+ return m_table->ContainsKey(key);
+}
+
+/**
+ * Remove a preference
+ * @param key the key
+ */
+void Preferences::Remove(llvm::StringRef key) {
+ m_table->Delete(key);
+}
diff --git a/wpilibc/Athena/src/Relay.cpp b/wpilibc/Athena/src/Relay.cpp
new file mode 100644
index 0000000..17f0ef8
--- /dev/null
+++ b/wpilibc/Athena/src/Relay.cpp
@@ -0,0 +1,293 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code
+ */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Relay.h"
+
+#include "MotorSafetyHelper.h"
+#include "Resource.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+#include "HAL/HAL.hpp"
+
+#include <sstream>
+
+// Allocate each direction separately.
+static std::unique_ptr<Resource> relayChannels;
+
+/**
+ * Relay constructor given a channel.
+ *
+ * This code initializes the relay and reserves all resources that need to be
+ * locked. Initially the relay is set to both lines at 0v.
+ * @param channel The channel number (0-3).
+ * @param direction The direction that the Relay object will control.
+ */
+Relay::Relay(uint32_t channel, Relay::Direction direction)
+ : m_channel(channel), m_direction(direction) {
+ std::stringstream buf;
+ Resource::CreateResourceObject(relayChannels,
+ dio_kNumSystems * kRelayChannels * 2);
+ if (!SensorBase::CheckRelayChannel(m_channel)) {
+ buf << "Relay Channel " << m_channel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ return;
+ }
+
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ buf << "Forward Relay " << m_channel;
+ if (relayChannels->Allocate(m_channel * 2, buf.str()) ==
+ std::numeric_limits<uint32_t>::max()) {
+ CloneError(*relayChannels);
+ return;
+ }
+
+ HALReport(HALUsageReporting::kResourceType_Relay, m_channel);
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ buf << "Reverse Relay " << m_channel;
+ if (relayChannels->Allocate(m_channel * 2 + 1, buf.str()) ==
+ std::numeric_limits<uint32_t>::max()) {
+ CloneError(*relayChannels);
+ return;
+ }
+
+ HALReport(HALUsageReporting::kResourceType_Relay, m_channel + 128);
+ }
+
+ int32_t status = 0;
+ setRelayForward(m_relay_ports[m_channel], false, &status);
+ setRelayReverse(m_relay_ports[m_channel], false, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+ m_safetyHelper->SetSafetyEnabled(false);
+
+ LiveWindow::GetInstance()->AddActuator("Relay", 1, m_channel, this);
+}
+
+/**
+ * Free the resource associated with a relay.
+ * The relay channels are set to free and the relay output is turned off.
+ */
+Relay::~Relay() {
+ int32_t status = 0;
+ setRelayForward(m_relay_ports[m_channel], false, &status);
+ setRelayReverse(m_relay_ports[m_channel], false, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ relayChannels->Free(m_channel * 2);
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ relayChannels->Free(m_channel * 2 + 1);
+ }
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the relay state.
+ *
+ * Valid values depend on which directions of the relay are controlled by the
+ * object.
+ *
+ * When set to kBothDirections, the relay can be any of the four states:
+ * 0v-0v, 0v-12v, 12v-0v, 12v-12v
+ *
+ * When set to kForwardOnly or kReverseOnly, you can specify the constant for
+ * the
+ * direction or you can simply specify kOff and kOn. Using only kOff and
+ * kOn is
+ * recommended.
+ *
+ * @param value The state to set the relay.
+ */
+void Relay::Set(Relay::Value value) {
+ if (StatusIsFatal()) return;
+
+ int32_t status = 0;
+
+ switch (value) {
+ case kOff:
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ setRelayForward(m_relay_ports[m_channel], false, &status);
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ setRelayReverse(m_relay_ports[m_channel], false, &status);
+ }
+ break;
+ case kOn:
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ setRelayForward(m_relay_ports[m_channel], true, &status);
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ setRelayReverse(m_relay_ports[m_channel], true, &status);
+ }
+ break;
+ case kForward:
+ if (m_direction == kReverseOnly) {
+ wpi_setWPIError(IncompatibleMode);
+ break;
+ }
+ if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+ setRelayForward(m_relay_ports[m_channel], true, &status);
+ }
+ if (m_direction == kBothDirections) {
+ setRelayReverse(m_relay_ports[m_channel], false, &status);
+ }
+ break;
+ case kReverse:
+ if (m_direction == kForwardOnly) {
+ wpi_setWPIError(IncompatibleMode);
+ break;
+ }
+ if (m_direction == kBothDirections) {
+ setRelayForward(m_relay_ports[m_channel], false, &status);
+ }
+ if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+ setRelayReverse(m_relay_ports[m_channel], true, &status);
+ }
+ break;
+ }
+
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Get the Relay State
+ *
+ * Gets the current state of the relay.
+ *
+ * When set to kForwardOnly or kReverseOnly, value is returned as kOn/kOff not
+ * kForward/kReverse (per the recommendation in Set)
+ *
+ * @return The current state of the relay as a Relay::Value
+ */
+Relay::Value Relay::Get() const {
+ int32_t status;
+
+ if (getRelayForward(m_relay_ports[m_channel], &status)) {
+ if (getRelayReverse(m_relay_ports[m_channel], &status)) {
+ return kOn;
+ } else {
+ if (m_direction == kForwardOnly) {
+ return kOn;
+ } else {
+ return kForward;
+ }
+ }
+ } else {
+ if (getRelayReverse(m_relay_ports[m_channel], &status)) {
+ if (m_direction == kReverseOnly) {
+ return kOn;
+ } else {
+ return kReverse;
+ }
+ } else {
+ return kOff;
+ }
+ }
+
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+uint32_t Relay::GetChannel() const {
+ return m_channel;
+}
+
+/**
+ * Set the expiration time for the Relay object
+ * @param timeout The timeout (in seconds) for this relay object
+ */
+void Relay::SetExpiration(float timeout) {
+ m_safetyHelper->SetExpiration(timeout);
+}
+
+/**
+ * Return the expiration time for the relay object.
+ * @return The expiration time value.
+ */
+float Relay::GetExpiration() const { return m_safetyHelper->GetExpiration(); }
+
+/**
+ * Check if the relay object is currently alive or stopped due to a timeout.
+ * @returns a bool value that is true if the motor has NOT timed out and should
+ * still be running.
+ */
+bool Relay::IsAlive() const { return m_safetyHelper->IsAlive(); }
+
+/**
+ * Stop the motor associated with this PWM object.
+ * This is called by the MotorSafetyHelper object when it has a timeout for this
+ * relay and needs to stop it from running.
+ */
+void Relay::StopMotor() { Set(kOff); }
+
+/**
+ * Enable/disable motor safety for this device
+ * Turn on and off the motor safety option for this relay object.
+ * @param enabled True if motor safety is enforced for this object
+ */
+void Relay::SetSafetyEnabled(bool enabled) {
+ m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+/**
+ * Check if motor safety is enabled for this object
+ * @returns True if motor safety is enforced for this object
+ */
+bool Relay::IsSafetyEnabled() const {
+ return m_safetyHelper->IsSafetyEnabled();
+}
+
+void Relay::GetDescription(std::ostringstream& desc) const {
+ desc << "Relay " << GetChannel();
+}
+
+void Relay::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsString()) return;
+ if (value->GetString() == "Off") Set(kOff);
+ else if (value->GetString() == "Forward") Set(kForward);
+ else if (value->GetString() == "Reverse") Set(kReverse);
+ else if (value->GetString() == "On") Set(kOn);
+}
+
+void Relay::UpdateTable() {
+ if (m_table != nullptr) {
+ if (Get() == kOn) {
+ m_table->PutString("Value", "On");
+ } else if (Get() == kForward) {
+ m_table->PutString("Value", "Forward");
+ } else if (Get() == kReverse) {
+ m_table->PutString("Value", "Reverse");
+ } else {
+ m_table->PutString("Value", "Off");
+ }
+ }
+}
+
+void Relay::StartLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void Relay::StopLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string Relay::GetSmartDashboardType() const { return "Relay"; }
+
+void Relay::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Relay::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/RobotBase.cpp b/wpilibc/Athena/src/RobotBase.cpp
new file mode 100644
index 0000000..2bd1d13
--- /dev/null
+++ b/wpilibc/Athena/src/RobotBase.cpp
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotBase.h"
+
+#include "DriverStation.h"
+#include "RobotState.h"
+#include "HLUsageReporting.h"
+#include "Internal/HardwareHLReporting.h"
+#include "Utility.h"
+#include "networktables/NetworkTable.h"
+#include <cstring>
+#include "HAL/HAL.hpp"
+#include <cstdio>
+
+RobotBase *RobotBase::m_instance = nullptr;
+
+void RobotBase::setInstance(RobotBase *robot) {
+ wpi_assert(m_instance == nullptr);
+ m_instance = robot;
+}
+
+RobotBase &RobotBase::getInstance() { return *m_instance; }
+
+void RobotBase::robotSetup(RobotBase *robot) {
+ robot->StartCompetition();
+}
+
+/**
+ * Constructor for a generic robot program.
+ * User code should be placed in the constructor that runs before the Autonomous
+ * or Operator
+ * Control period starts. The constructor will run to completion before
+ * Autonomous is entered.
+ *
+ * This must be used to ensure that the communications code starts. In the
+ * future it would be
+ * nice to put this code into it's own task that loads on boot so ensure that it
+ * runs.
+ */
+RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
+ RobotState::SetImplementation(DriverStation::GetInstance());
+ HLUsageReporting::SetImplementation(new HardwareHLReporting());
+
+ RobotBase::setInstance(this);
+
+ NetworkTable::SetNetworkIdentity("Robot");
+
+ FILE *file = nullptr;
+ file = fopen("/tmp/frc_versions/FRC_Lib_Version.ini", "w");
+
+ if (file != nullptr) {
+ fputs("2016 C++ Beta5.0", file);
+ fclose(file);
+ }
+}
+
+/**
+ * Free the resources for a RobotBase class.
+ * This includes deleting all classes that might have been allocated as
+ * Singletons to they
+ * would never be deleted except here.
+ */
+RobotBase::~RobotBase() {
+ SensorBase::DeleteSingletons();
+ delete m_task;
+ m_task = nullptr;
+ m_instance = nullptr;
+}
+
+/**
+ * Determine if the Robot is currently enabled.
+ * @return True if the Robot is currently enabled by the field controls.
+ */
+bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
+
+/**
+ * Determine if the Robot is currently disabled.
+ * @return True if the Robot is currently disabled by the field controls.
+ */
+bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
+
+/**
+ * Determine if the robot is currently in Autonomous mode.
+ * @return True if the robot is currently operating Autonomously as determined
+ * by the field controls.
+ */
+bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
+
+/**
+ * Determine if the robot is currently in Operator Control mode.
+ * @return True if the robot is currently operating in Tele-Op mode as
+ * determined by the field controls.
+ */
+bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
+
+/**
+ * Determine if the robot is currently in Test mode.
+ * @return True if the robot is currently running tests as determined by the
+ * field controls.
+ */
+bool RobotBase::IsTest() const { return m_ds.IsTest(); }
+
+/**
+ * Indicates if new data is available from the driver station.
+ * @return Has new data arrived over the network since the last time this
+ * function was called?
+ */
+bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); }
+
+/**
+ * This class exists for the sole purpose of getting its destructor called when
+ * the module unloads.
+ * Before the module is done unloading, we need to delete the RobotBase derived
+ * singleton. This should delete
+ * the other remaining singletons that were registered. This should also stop
+ * all tasks that are using
+ * the Task class.
+ */
+class RobotDeleter {
+ public:
+ RobotDeleter() {}
+ ~RobotDeleter() { delete &RobotBase::getInstance(); }
+};
+static RobotDeleter g_robotDeleter;
diff --git a/wpilibc/Athena/src/RobotDrive.cpp b/wpilibc/Athena/src/RobotDrive.cpp
new file mode 100644
index 0000000..c1353e9
--- /dev/null
+++ b/wpilibc/Athena/src/RobotDrive.cpp
@@ -0,0 +1,753 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotDrive.h"
+
+#include "CANJaguar.h"
+#include "GenericHID.h"
+#include "Joystick.h"
+#include "Talon.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include <math.h>
+
+#undef max
+#include <algorithm>
+
+const int32_t RobotDrive::kMaxNumberOfMotors;
+
+static auto make_shared_nodelete(SpeedController *ptr) {
+ return std::shared_ptr<SpeedController>(ptr, NullDeleter<SpeedController>());
+}
+
+/*
+ * Driving functions
+ * These functions provide an interface to multiple motors that is used for C
+ * programming
+ * The Drive(speed, direction) function is the main part of the set that makes
+ * it easy
+ * to set speeds and direction independently in one call.
+ */
+
+/**
+ * Common function to initialize all the robot drive constructors.
+ * Create a motor safety object (the real reason for the common code) and
+ * initialize all the motor assignments. The default timeout is set for the
+ * robot drive.
+ */
+void RobotDrive::InitRobotDrive() {
+ m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+ m_safetyHelper->SetSafetyEnabled(true);
+}
+
+/**
+ * Constructor for RobotDrive with 2 motors specified with channel numbers.
+ * Set up parameters for a two wheel drive system where the
+ * left and right motor pwm channels are specified in the call.
+ * This call assumes Talons for controlling the motors.
+ * @param leftMotorChannel The PWM channel number that drives the left motor.
+ * 0-9 are on-board, 10-19 are on the MXP port
+ * @param rightMotorChannel The PWM channel number that drives the right motor.
+ * 0-9 are on-board, 10-19 are on the MXP port
+ */
+RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel) {
+ InitRobotDrive();
+ m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
+ m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
+ SetLeftRightMotorOutputs(0.0, 0.0);
+}
+
+/**
+ * Constructor for RobotDrive with 4 motors specified with channel numbers.
+ * Set up parameters for a four wheel drive system where all four motor
+ * pwm channels are specified in the call.
+ * This call assumes Talons for controlling the motors.
+ * @param frontLeftMotor Front left motor channel number. 0-9 are on-board,
+ * 10-19 are on the MXP port
+ * @param rearLeftMotor Rear Left motor channel number. 0-9 are on-board, 10-19
+ * are on the MXP port
+ * @param frontRightMotor Front right motor channel number. 0-9 are on-board,
+ * 10-19 are on the MXP port
+ * @param rearRightMotor Rear Right motor channel number. 0-9 are on-board,
+ * 10-19 are on the MXP port
+ */
+RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor,
+ uint32_t frontRightMotor, uint32_t 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);
+}
+
+/**
+ * Constructor for RobotDrive with 2 motors specified as SpeedController
+ * objects.
+ * The SpeedController version of the constructor enables programs to use the
+ * RobotDrive classes with
+ * subclasses of the SpeedController objects, for example, versions with ramping
+ * or reshaping of
+ * the curve to suit motor bias or deadband elimination.
+ * @param leftMotor The left SpeedController object used to drive the robot.
+ * @param rightMotor the right SpeedController object used to drive the robot.
+ */
+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);
+}
+
+//TODO: Change to rvalue references & move syntax.
+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;
+}
+
+/**
+ * Constructor for RobotDrive with 4 motors specified as SpeedController
+ * objects.
+ * Speed controller input version of RobotDrive (see previous comments).
+ * @param rearLeftMotor The back left SpeedController object used to drive the
+ * robot.
+ * @param frontLeftMotor The front left SpeedController object used to drive the
+ * robot
+ * @param rearRightMotor The back right SpeedController object used to drive the
+ * robot.
+ * @param frontRightMotor The front right SpeedController object used to drive
+ * the robot.
+ */
+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;
+}
+
+/**
+ * Drive the motors at "outputMagnitude" and "curve".
+ * Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0 represents
+ * stopped and not turning. curve < 0 will turn left and curve > 0 will turn
+ * right.
+ *
+ * The algorithm for steering provides a constant turn radius for any normal
+ * speed range, both forward and backward. Increasing m_sensitivity causes
+ * sharper turns for fixed values of curve.
+ *
+ * This function will most likely be used in an autonomous routine.
+ *
+ * @param outputMagnitude The speed setting for the outside wheel in a turn,
+ * forward or backwards, +1 to -1.
+ * @param curve The rate of turn, constant for different forward speeds. Set
+ * curve < 0 for left turn or curve > 0 for right turn.
+ * Set curve = e^(-r/w) to get a turn radius r for wheelbase w of your robot.
+ * Conversely, turn radius r = -ln(curve)*w for a given value of curve and
+ * wheelbase w.
+ */
+void RobotDrive::Drive(float outputMagnitude, float curve) {
+ float leftOutput, rightOutput;
+ static bool reported = false;
+ if (!reported) {
+ HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
+ HALUsageReporting::kRobotDrive_ArcadeRatioCurve);
+ reported = true;
+ }
+
+ if (curve < 0) {
+ float value = log(-curve);
+ float ratio = (value - m_sensitivity) / (value + m_sensitivity);
+ if (ratio == 0) ratio = .0000000001;
+ leftOutput = outputMagnitude / ratio;
+ rightOutput = outputMagnitude;
+ } else if (curve > 0) {
+ float value = log(curve);
+ float 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);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ * Drive the robot using two joystick inputs. The Y-axis will be selected from
+ * each Joystick object.
+ * @param leftStick The joystick to control the left side of the robot.
+ * @param rightStick The joystick to control the right side of the robot.
+ */
+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);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ * This function lets you pick the axis to be used on each Joystick object for
+ * the left
+ * and right sides of the robot.
+ * @param leftStick The Joystick object to use for the left side of the robot.
+ * @param leftAxis The axis to select on the left side Joystick object.
+ * @param rightStick The Joystick object to use for the right side of the robot.
+ * @param rightAxis The axis to select on the right side Joystick object.
+ */
+void RobotDrive::TankDrive(GenericHID *leftStick, uint32_t leftAxis,
+ GenericHID *rightStick, uint32_t 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, uint32_t leftAxis,
+ GenericHID &rightStick, uint32_t rightAxis,
+ bool squaredInputs) {
+ TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis),
+ squaredInputs);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ * This function lets you directly provide joystick values from any source.
+ * @param leftValue The value of the left stick.
+ * @param rightValue The value of the right stick.
+ */
+void RobotDrive::TankDrive(float leftValue, float rightValue,
+ bool squaredInputs) {
+ static bool reported = false;
+ if (!reported) {
+ HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
+ HALUsageReporting::kRobotDrive_Tank);
+ reported = true;
+ }
+
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ leftValue = Limit(leftValue);
+ rightValue = Limit(rightValue);
+ if (squaredInputs) {
+ if (leftValue >= 0.0) {
+ leftValue = (leftValue * leftValue);
+ } else {
+ leftValue = -(leftValue * leftValue);
+ }
+ if (rightValue >= 0.0) {
+ rightValue = (rightValue * rightValue);
+ } else {
+ rightValue = -(rightValue * rightValue);
+ }
+ }
+
+ SetLeftRightMotorOutputs(leftValue, rightValue);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ * Given a single Joystick, the class assumes the Y axis for the move value and
+ * the X axis
+ * for the rotate value.
+ * (Should add more information here regarding the way that arcade drive works.)
+ * @param stick The joystick to use for Arcade single-stick driving. The Y-axis
+ * will be selected
+ * for forwards/backwards and the X-axis will be selected for rotation rate.
+ * @param squaredInputs If true, the sensitivity will be increased for small
+ * values
+ */
+void RobotDrive::ArcadeDrive(GenericHID *stick, bool squaredInputs) {
+ // simply call the full-featured ArcadeDrive with the appropriate values
+ ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ * Given a single Joystick, the class assumes the Y axis for the move value and
+ * the X axis
+ * for the rotate value.
+ * (Should add more information here regarding the way that arcade drive works.)
+ * @param stick The joystick to use for Arcade single-stick driving. The Y-axis
+ * will be selected
+ * for forwards/backwards and the X-axis will be selected for rotation rate.
+ * @param squaredInputs If true, the sensitivity will be increased for small
+ * values
+ */
+void RobotDrive::ArcadeDrive(GenericHID &stick, bool squaredInputs) {
+ // simply call the full-featured ArcadeDrive with the appropriate values
+ ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ * Given two joystick instances and two axis, compute the values to send to
+ * either two
+ * or four motors.
+ * @param moveStick The Joystick object that represents the forward/backward
+ * direction
+ * @param moveAxis The axis on the moveStick object to use for fowards/backwards
+ * (typically Y_AXIS)
+ * @param rotateStick The Joystick object that represents the rotation value
+ * @param rotateAxis The axis on the rotation object to use for the rotate
+ * right/left (typically X_AXIS)
+ * @param squaredInputs Setting this parameter to true increases the sensitivity
+ * at lower speeds
+ */
+void RobotDrive::ArcadeDrive(GenericHID *moveStick, uint32_t moveAxis,
+ GenericHID *rotateStick, uint32_t rotateAxis,
+ bool squaredInputs) {
+ float moveValue = moveStick->GetRawAxis(moveAxis);
+ float rotateValue = rotateStick->GetRawAxis(rotateAxis);
+
+ ArcadeDrive(moveValue, rotateValue, squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ * Given two joystick instances and two axis, compute the values to send to
+ * either two
+ * or four motors.
+ * @param moveStick The Joystick object that represents the forward/backward
+ * direction
+ * @param moveAxis The axis on the moveStick object to use for fowards/backwards
+ * (typically Y_AXIS)
+ * @param rotateStick The Joystick object that represents the rotation value
+ * @param rotateAxis The axis on the rotation object to use for the rotate
+ * right/left (typically X_AXIS)
+ * @param squaredInputs Setting this parameter to true increases the sensitivity
+ * at lower speeds
+ */
+
+void RobotDrive::ArcadeDrive(GenericHID &moveStick, uint32_t moveAxis,
+ GenericHID &rotateStick, uint32_t rotateAxis,
+ bool squaredInputs) {
+ float moveValue = moveStick.GetRawAxis(moveAxis);
+ float rotateValue = rotateStick.GetRawAxis(rotateAxis);
+
+ ArcadeDrive(moveValue, rotateValue, squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ * This function lets you directly provide joystick values from any source.
+ * @param moveValue The value to use for fowards/backwards
+ * @param rotateValue The value to use for the rotate right/left
+ * @param squaredInputs If set, increases the sensitivity at low speeds
+ */
+void RobotDrive::ArcadeDrive(float moveValue, float rotateValue,
+ bool squaredInputs) {
+ static bool reported = false;
+ if (!reported) {
+ HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
+ HALUsageReporting::kRobotDrive_ArcadeStandard);
+ reported = true;
+ }
+
+ // local variables to hold the computed PWM values for the motors
+ float leftMotorOutput;
+ float rightMotorOutput;
+
+ moveValue = Limit(moveValue);
+ rotateValue = Limit(rotateValue);
+
+ if (squaredInputs) {
+ // square the inputs (while preserving the sign) to increase fine control
+ // while permitting full power
+ if (moveValue >= 0.0) {
+ moveValue = (moveValue * moveValue);
+ } else {
+ moveValue = -(moveValue * moveValue);
+ }
+ if (rotateValue >= 0.0) {
+ rotateValue = (rotateValue * rotateValue);
+ } else {
+ 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);
+}
+
+/**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * A method for driving with Mecanum wheeled robots. There are 4 wheels
+ * on the robot, arranged so that the front and back wheels are toed in 45
+ * degrees.
+ * When looking at the wheels from the top, the roller axles should form an X
+ * across the robot.
+ *
+ * This is designed to be directly driven by joystick axes.
+ *
+ * @param x The speed that the robot should drive in the X direction.
+ * [-1.0..1.0]
+ * @param y The speed that the robot should drive in the Y direction.
+ * This input is inverted to match the forward == -1.0 that joysticks produce.
+ * [-1.0..1.0]
+ * @param rotation The rate of rotation for the robot that is completely
+ * independent of
+ * the translation. [-1.0..1.0]
+ * @param gyroAngle The current angle reading from the gyro. Use this to
+ * implement field-oriented controls.
+ */
+void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation,
+ float gyroAngle) {
+ static bool reported = false;
+ if (!reported) {
+ HALReport(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_syncGroup);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput,
+ m_syncGroup);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput, m_syncGroup);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput,
+ m_syncGroup);
+
+ if (m_syncGroup != 0) {
+ CANJaguar::UpdateSyncGroup(m_syncGroup);
+ }
+
+ m_safetyHelper->Feed();
+}
+
+/**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * A method for driving with Mecanum wheeled robots. There are 4 wheels
+ * on the robot, arranged so that the front and back wheels are toed in 45
+ * degrees.
+ * When looking at the wheels from the top, the roller axles should form an X
+ * across the robot.
+ *
+ * @param magnitude The speed that the robot should drive in a given direction.
+ * [-1.0..1.0]
+ * @param direction The direction the robot should drive in degrees. The
+ * direction and maginitute are
+ * independent of the rotation rate.
+ * @param rotation The rate of rotation for the robot that is completely
+ * independent of
+ * the magnitute or direction. [-1.0..1.0]
+ */
+void RobotDrive::MecanumDrive_Polar(float magnitude, float direction,
+ float rotation) {
+ static bool reported = false;
+ if (!reported) {
+ HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
+ HALUsageReporting::kRobotDrive_MecanumPolar);
+ reported = true;
+ }
+
+ // Normalized for full power along the Cartesian axes.
+ magnitude = Limit(magnitude) * sqrt(2.0);
+ // The rollers are at 45 degree angles.
+ double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
+ double cosD = cos(dirInRad);
+ double sinD = 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_syncGroup);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput,
+ m_syncGroup);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput, m_syncGroup);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput,
+ m_syncGroup);
+
+ if (m_syncGroup != 0) {
+ CANJaguar::UpdateSyncGroup(m_syncGroup);
+ }
+
+ m_safetyHelper->Feed();
+}
+
+/**
+ * Holonomic Drive method for Mecanum wheeled robots.
+ *
+ * This is an alias to MecanumDrive_Polar() for backward compatability
+ *
+ * @param magnitude The speed that the robot should drive in a given direction.
+ * [-1.0..1.0]
+ * @param direction The direction the robot should drive. The direction and
+ * maginitute are
+ * independent of the rotation rate.
+ * @param rotation The rate of rotation for the robot that is completely
+ * independent of
+ * the magnitute or direction. [-1.0..1.0]
+ */
+void RobotDrive::HolonomicDrive(float magnitude, float direction,
+ float rotation) {
+ MecanumDrive_Polar(magnitude, direction, rotation);
+}
+
+/** Set the speed of the right and left motors.
+ * This is used once an appropriate drive setup function is called such as
+ * TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput"
+ * and includes flipping the direction of one side for opposing motors.
+ * @param leftOutput The speed to send to the left side of the robot.
+ * @param rightOutput The speed to send to the right side of the robot.
+ */
+void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput) {
+ wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr);
+
+ if (m_frontLeftMotor != nullptr)
+ m_frontLeftMotor->Set(Limit(leftOutput) * m_maxOutput, m_syncGroup);
+ m_rearLeftMotor->Set(Limit(leftOutput) * m_maxOutput, m_syncGroup);
+
+ if (m_frontRightMotor != nullptr)
+ m_frontRightMotor->Set(-Limit(rightOutput) * m_maxOutput, m_syncGroup);
+ m_rearRightMotor->Set(-Limit(rightOutput) * m_maxOutput, m_syncGroup);
+
+ if (m_syncGroup != 0) {
+ CANJaguar::UpdateSyncGroup(m_syncGroup);
+ }
+
+ m_safetyHelper->Feed();
+}
+
+/**
+ * Limit motor values to the -1.0 to +1.0 range.
+ */
+float RobotDrive::Limit(float num) {
+ if (num > 1.0) {
+ return 1.0;
+ }
+ if (num < -1.0) {
+ return -1.0;
+ }
+ return num;
+}
+
+/**
+ * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+ */
+void RobotDrive::Normalize(double *wheelSpeeds) {
+ double maxMagnitude = fabs(wheelSpeeds[0]);
+ int32_t i;
+ for (i = 1; i < kMaxNumberOfMotors; i++) {
+ double temp = fabs(wheelSpeeds[i]);
+ if (maxMagnitude < temp) maxMagnitude = temp;
+ }
+ if (maxMagnitude > 1.0) {
+ for (i = 0; i < kMaxNumberOfMotors; i++) {
+ wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+ }
+ }
+}
+
+/**
+ * Rotate a vector in Cartesian space.
+ */
+void RobotDrive::RotateVector(double &x, double &y, double angle) {
+ double cosA = cos(angle * (3.14159 / 180.0));
+ double sinA = sin(angle * (3.14159 / 180.0));
+ double xOut = x * cosA - y * sinA;
+ double yOut = x * sinA + y * cosA;
+ x = xOut;
+ y = yOut;
+}
+
+/*
+ * Invert a motor direction.
+ * This is used when a motor should run in the opposite direction as the drive
+ * code would normally run it. Motors that are direct drive would be inverted,
+ * the
+ * Drive code assumes that the motors are geared with one reversal.
+ * @param motor The motor index to invert.
+ * @param isInverted True if the motor should be inverted when operated.
+ */
+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;
+ }
+}
+
+/**
+ * Set the turning sensitivity.
+ *
+ * This only impacts the Drive() entry-point.
+ * @param sensitivity Effectively sets the turning sensitivity (or turn radius
+ * for a given value)
+ */
+void RobotDrive::SetSensitivity(float sensitivity) {
+ m_sensitivity = sensitivity;
+}
+
+/**
+ * Configure the scaling factor for using RobotDrive with motor controllers in a
+ * mode other than PercentVbus.
+ * @param maxOutput Multiplied with the output percentage computed by the drive
+ * functions.
+ */
+void RobotDrive::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
+
+/**
+ * Set the number of the sync group for the motor controllers. If the motor
+ * controllers are {@link CANJaguar}s,
+ * then they will all be added to this sync group, causing them to update their
+ * values at the same time.
+ *
+ * @param syncGroup the update group to add the motor controllers to
+ */
+void RobotDrive::SetCANJaguarSyncGroup(uint8_t syncGroup) {
+ m_syncGroup = syncGroup;
+}
+
+void RobotDrive::SetExpiration(float timeout) {
+ m_safetyHelper->SetExpiration(timeout);
+}
+
+float RobotDrive::GetExpiration() const {
+ return m_safetyHelper->GetExpiration();
+}
+
+bool RobotDrive::IsAlive() const { return m_safetyHelper->IsAlive(); }
+
+bool RobotDrive::IsSafetyEnabled() const {
+ return m_safetyHelper->IsSafetyEnabled();
+}
+
+void RobotDrive::SetSafetyEnabled(bool enabled) {
+ m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+void RobotDrive::GetDescription(std::ostringstream& desc) const {
+ desc << "RobotDrive";
+}
+
+void RobotDrive::StopMotor() {
+ if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Disable();
+ if (m_frontRightMotor != nullptr) m_frontRightMotor->Disable();
+ if (m_rearLeftMotor != nullptr) m_rearLeftMotor->Disable();
+ if (m_rearRightMotor != nullptr) m_rearRightMotor->Disable();
+ m_safetyHelper->Feed();
+}
diff --git a/wpilibc/Athena/src/SPI.cpp b/wpilibc/Athena/src/SPI.cpp
new file mode 100644
index 0000000..a470597
--- /dev/null
+++ b/wpilibc/Athena/src/SPI.cpp
@@ -0,0 +1,303 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "SPI.h"
+
+#include "WPIErrors.h"
+#include "HAL/Digital.hpp"
+
+#include <string.h>
+
+/**
+ * Constructor
+ *
+ * @param SPIport the physical SPI port
+ */
+SPI::SPI(Port SPIport) {
+ m_port = SPIport;
+ int32_t status = 0;
+ spiInitialize(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ static int32_t instances = 0;
+ instances++;
+ HALReport(HALUsageReporting::kResourceType_SPI, instances);
+}
+
+/**
+ * Destructor.
+ */
+SPI::~SPI() { spiClose(m_port); }
+
+/**
+ * Configure the rate of the generated clock signal.
+ *
+ * The default value is 500,000Hz.
+ * The maximum value is 4,000,000Hz.
+ *
+ * @param hz The clock rate in Hertz.
+ */
+void SPI::SetClockRate(double hz) { spiSetSpeed(m_port, hz); }
+
+/**
+ * Configure the order that bits are sent and received on the wire
+ * to be most significant bit first.
+ */
+void SPI::SetMSBFirst() {
+ m_msbFirst = true;
+ spiSetOpts(m_port, (int)m_msbFirst, (int)m_sampleOnTrailing,
+ (int)m_clk_idle_high);
+}
+
+/**
+ * Configure the order that bits are sent and received on the wire
+ * to be least significant bit first.
+ */
+void SPI::SetLSBFirst() {
+ m_msbFirst = false;
+ spiSetOpts(m_port, (int)m_msbFirst, (int)m_sampleOnTrailing,
+ (int)m_clk_idle_high);
+}
+
+/**
+ * Configure that the data is stable on the falling edge and the data
+ * changes on the rising edge.
+ */
+void SPI::SetSampleDataOnFalling() {
+ m_sampleOnTrailing = true;
+ spiSetOpts(m_port, (int)m_msbFirst, (int)m_sampleOnTrailing,
+ (int)m_clk_idle_high);
+}
+
+/**
+ * Configure that the data is stable on the rising edge and the data
+ * changes on the falling edge.
+ */
+void SPI::SetSampleDataOnRising() {
+ m_sampleOnTrailing = false;
+ spiSetOpts(m_port, (int)m_msbFirst, (int)m_sampleOnTrailing,
+ (int)m_clk_idle_high);
+}
+
+/**
+ * Configure the clock output line to be active low.
+ * This is sometimes called clock polarity high or clock idle high.
+ */
+void SPI::SetClockActiveLow() {
+ m_clk_idle_high = true;
+ spiSetOpts(m_port, (int)m_msbFirst, (int)m_sampleOnTrailing,
+ (int)m_clk_idle_high);
+}
+
+/**
+ * Configure the clock output line to be active high.
+ * This is sometimes called clock polarity low or clock idle low.
+ */
+void SPI::SetClockActiveHigh() {
+ m_clk_idle_high = false;
+ spiSetOpts(m_port, (int)m_msbFirst, (int)m_sampleOnTrailing,
+ (int)m_clk_idle_high);
+}
+
+/**
+ * Configure the chip select line to be active high.
+ */
+void SPI::SetChipSelectActiveHigh() {
+ int32_t status = 0;
+ spiSetChipSelectActiveHigh(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Configure the chip select line to be active low.
+ */
+void SPI::SetChipSelectActiveLow() {
+ int32_t status = 0;
+ spiSetChipSelectActiveLow(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Write data to the slave device. Blocks until there is space in the
+ * output FIFO.
+ *
+ * If not running in output only mode, also saves the data received
+ * on the MISO input during the transfer into the receive FIFO.
+ */
+int32_t SPI::Write(uint8_t* data, uint8_t size) {
+ int32_t retVal = 0;
+ retVal = spiWrite(m_port, data, size);
+ return retVal;
+}
+
+/**
+ * Read a word from the receive FIFO.
+ *
+ * Waits for the current transfer to complete if the receive FIFO is empty.
+ *
+ * If the receive FIFO is empty, there is no active transfer, and initiate
+ * is false, errors.
+ *
+ * @param initiate If true, this function pushes "0" into the
+ * transmit buffer and initiates a transfer.
+ * If false, this function assumes that data is
+ * already in the receive FIFO from a previous
+ * write.
+ */
+int32_t SPI::Read(bool initiate, uint8_t* dataReceived, uint8_t size) {
+ int32_t retVal = 0;
+ if (initiate) {
+ auto dataToSend = new uint8_t[size];
+ memset(dataToSend, 0, size);
+ retVal = spiTransaction(m_port, dataToSend, dataReceived, size);
+ } else
+ retVal = spiRead(m_port, dataReceived, size);
+ return retVal;
+}
+
+/**
+ * Perform a simultaneous read/write transaction with the device
+ *
+ * @param dataToSend The data to be written out to the device
+ * @param dataReceived Buffer to receive data from the device
+ * @param size The length of the transaction, in bytes
+ */
+int32_t SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived,
+ uint8_t size) {
+ int32_t retVal = 0;
+ retVal = spiTransaction(m_port, dataToSend, dataReceived, size);
+ return retVal;
+}
+
+/**
+ * Initialize the accumulator.
+ *
+ * @param period Time between reads
+ * @param cmd SPI command to send to request data
+ * @param xfer_size SPI transfer size, in bytes
+ * @param valid_mask Mask to apply to received data for validity checking
+ * @param valid_data After valid_mask is applied, required matching value for
+ * validity checking
+ * @param data_shift Bit shift to apply to received data to get actual data
+ * value
+ * @param data_size Size (in bits) of data field
+ * @param is_signed Is data field signed?
+ * @param big_endian Is device big endian?
+ */
+void SPI::InitAccumulator(double period, uint32_t cmd, uint8_t xfer_size,
+ uint32_t valid_mask, uint32_t valid_value,
+ uint8_t data_shift, uint8_t data_size, bool is_signed,
+ bool big_endian) {
+ int32_t status = 0;
+ spiInitAccumulator(m_port, (uint32_t)(period * 1e6), cmd, xfer_size,
+ valid_mask, valid_value, data_shift, data_size, is_signed,
+ big_endian, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Frees the accumulator.
+ */
+void SPI::FreeAccumulator() {
+ int32_t status = 0;
+ spiFreeAccumulator(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Resets the accumulator to zero.
+ */
+void SPI::ResetAccumulator() {
+ int32_t status = 0;
+ spiResetAccumulator(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the center value of the accumulator.
+ *
+ * The center value is subtracted from each value before it is added to the accumulator. This
+ * is used for the center value of devices like gyros and accelerometers to make integration work
+ * and to take the device offset into account when integrating.
+ */
+void SPI::SetAccumulatorCenter(int32_t center) {
+ int32_t status = 0;
+ spiSetAccumulatorCenter(m_port, center, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the accumulator's deadband.
+ */
+void SPI::SetAccumulatorDeadband(int32_t deadband) {
+ int32_t status = 0;
+ spiSetAccumulatorDeadband(m_port, deadband, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Read the last value read by the accumulator engine.
+ */
+int32_t SPI::GetAccumulatorLastValue() const {
+ int32_t status = 0;
+ int32_t retVal = spiGetAccumulatorLastValue(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Read the accumulated value.
+ *
+ * @return The 64-bit value accumulated since the last Reset().
+ */
+int64_t SPI::GetAccumulatorValue() const {
+ int32_t status = 0;
+ int64_t retVal = spiGetAccumulatorValue(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Read the number of accumulated values.
+ *
+ * Read the count of the accumulated values since the accumulator was last Reset().
+ *
+ * @return The number of times samples from the channel were accumulated.
+ */
+uint32_t SPI::GetAccumulatorCount() const {
+ int32_t status = 0;
+ uint32_t retVal = spiGetAccumulatorCount(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Read the average of the accumulated value.
+ *
+ * @return The accumulated average value (value / count).
+ */
+double SPI::GetAccumulatorAverage() const {
+ int32_t status = 0;
+ double retVal = spiGetAccumulatorAverage(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Read the accumulated value and the number of accumulated values atomically.
+ *
+ * This function reads the value and count atomically.
+ * This can be used for averaging.
+ *
+ * @param value Pointer to the 64-bit accumulated output.
+ * @param count Pointer to the number of accumulation cycles.
+ */
+void SPI::GetAccumulatorOutput(int64_t &value, uint32_t &count) const {
+ int32_t status = 0;
+ spiGetAccumulatorOutput(m_port, &value, &count, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
diff --git a/wpilibc/Athena/src/SafePWM.cpp b/wpilibc/Athena/src/SafePWM.cpp
new file mode 100644
index 0000000..801e3ee
--- /dev/null
+++ b/wpilibc/Athena/src/SafePWM.cpp
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "SafePWM.h"
+
+/**
+ * Constructor for a SafePWM object taking a channel number.
+ * @param channel The PWM channel number 0-9 are on-board, 10-19 are on the MXP
+ * port
+ */
+SafePWM::SafePWM(uint32_t channel) : PWM(channel) {
+ m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+ m_safetyHelper->SetSafetyEnabled(false);
+}
+
+/**
+ * Set the expiration time for the PWM object
+ * @param timeout The timeout (in seconds) for this motor object
+ */
+void SafePWM::SetExpiration(float timeout) {
+ m_safetyHelper->SetExpiration(timeout);
+}
+
+/**
+ * Return the expiration time for the PWM object.
+ * @returns The expiration time value.
+ */
+float SafePWM::GetExpiration() const { return m_safetyHelper->GetExpiration(); }
+
+/**
+ * Check if the PWM object is currently alive or stopped due to a timeout.
+ * @returns a bool value that is true if the motor has NOT timed out and should
+ * still
+ * be running.
+ */
+bool SafePWM::IsAlive() const { return m_safetyHelper->IsAlive(); }
+
+/**
+ * Stop the motor associated with this PWM object.
+ * This is called by the MotorSafetyHelper object when it has a timeout for this
+ * PWM and needs to
+ * stop it from running.
+ */
+void SafePWM::StopMotor() { SetRaw(kPwmDisabled); }
+
+/**
+ * Enable/disable motor safety for this device
+ * Turn on and off the motor safety option for this PWM object.
+ * @param enabled True if motor safety is enforced for this object
+ */
+void SafePWM::SetSafetyEnabled(bool enabled) {
+ m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+/**
+ * Check if motor safety is enabled for this object
+ * @returns True if motor safety is enforced for this object
+ */
+bool SafePWM::IsSafetyEnabled() const {
+ return m_safetyHelper->IsSafetyEnabled();
+}
+
+void SafePWM::GetDescription(std::ostringstream& desc) const {
+ desc << "PWM " << GetChannel();
+}
+
+/**
+ * Feed the MotorSafety timer when setting the speed.
+ * This method is called by the subclass motor whenever it updates its speed,
+ * thereby reseting
+ * the timeout value.
+ * @param speed Value to pass to the PWM class
+ */
+void SafePWM::SetSpeed(float speed) {
+ PWM::SetSpeed(speed);
+ m_safetyHelper->Feed();
+}
diff --git a/wpilibc/Athena/src/SampleRobot.cpp b/wpilibc/Athena/src/SampleRobot.cpp
new file mode 100644
index 0000000..37b134f
--- /dev/null
+++ b/wpilibc/Athena/src/SampleRobot.cpp
@@ -0,0 +1,155 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "SampleRobot.h"
+
+#include "DriverStation.h"
+#include "Timer.h"
+#include "SmartDashboard/SmartDashboard.h"
+#include "LiveWindow/LiveWindow.h"
+#include "networktables/NetworkTable.h"
+
+SampleRobot::SampleRobot() : m_robotMainOverridden(true) {}
+
+/**
+ * Robot-wide initialization code should go here.
+ *
+ * Users should override this method for default Robot-wide initialization which
+ * will be called when the robot is first powered on. It will be called exactly
+ * one time.
+ *
+ * Warning: the Driver Station "Robot Code" light and FMS "Robot Ready"
+ * indicators will be off until RobotInit() exits. Code in RobotInit() that
+ * waits for enable will cause the robot to never indicate that the code is
+ * ready, causing the robot to be bypassed in a match.
+ */
+void SampleRobot::RobotInit() {
+ printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Disabled should go here.
+ * Programmers should override this method to run code that should run while the
+ * field is
+ * disabled.
+ */
+void SampleRobot::Disabled() {
+ printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Autonomous should go here.
+ * Programmers should override this method to run code that should run while the
+ * field is
+ * in the autonomous period. This will be called once each time the robot enters
+ * the
+ * autonomous state.
+ */
+void SampleRobot::Autonomous() {
+ printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Operator control (tele-operated) code should go here.
+ * Programmers should override this method to run code that should run while the
+ * field is
+ * in the Operator Control (tele-operated) period. This is called once each time
+ * the robot
+ * enters the teleop state.
+ */
+void SampleRobot::OperatorControl() {
+ printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Test program should go here.
+ * Programmers should override this method to run code that executes while the
+ * robot is
+ * in test mode. This will be called once whenever the robot enters test mode
+ */
+void SampleRobot::Test() {
+ printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Robot main program for free-form programs.
+ *
+ * This should be overridden by user subclasses if the intent is to not use the
+ * Autonomous() and
+ * OperatorControl() methods. In that case, the program is responsible for
+ * sensing when to run
+ * the autonomous and operator control functions in their program.
+ *
+ * This method will be called immediately after the constructor is called. If it
+ * has not been
+ * overridden by a user subclass (i.e. the default version runs), then the
+ * Autonomous() and
+ * OperatorControl() methods will be called.
+ */
+void SampleRobot::RobotMain() { m_robotMainOverridden = false; }
+
+/**
+ * Start a competition.
+ * This code needs to track the order of the field starting to ensure that
+ * everything happens
+ * in the right order. Repeatedly run the correct method, either Autonomous or
+ * OperatorControl
+ * or Test when the robot is enabled. After running the correct method, wait for
+ * some state to
+ * change, either the other mode starts or the robot is disabled. Then go back
+ * and wait for the
+ * robot to be enabled again.
+ */
+void SampleRobot::StartCompetition() {
+ LiveWindow *lw = LiveWindow::GetInstance();
+
+ HALReport(HALUsageReporting::kResourceType_Framework,
+ HALUsageReporting::kFramework_Sample);
+
+ SmartDashboard::init();
+ NetworkTable::GetTable("LiveWindow")
+ ->GetSubTable("~STATUS~")
+ ->PutBoolean("LW Enabled", false);
+
+ RobotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HALNetworkCommunicationObserveUserProgramStarting();
+
+ RobotMain();
+
+ if (!m_robotMainOverridden) {
+ // first and one-time initialization
+ lw->SetEnabled(false);
+
+ 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();
+ }
+ }
+ }
+}
diff --git a/wpilibc/Athena/src/SensorBase.cpp b/wpilibc/Athena/src/SensorBase.cpp
new file mode 100644
index 0000000..05f9f4f
--- /dev/null
+++ b/wpilibc/Athena/src/SensorBase.cpp
@@ -0,0 +1,183 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "SensorBase.h"
+
+#include "NetworkCommunication/LoadOut.h"
+#include "WPIErrors.h"
+#include "HAL/HAL.hpp"
+#include "HAL/Port.h"
+
+const uint32_t SensorBase::kDigitalChannels;
+const uint32_t SensorBase::kAnalogInputs;
+const uint32_t SensorBase::kSolenoidChannels;
+const uint32_t SensorBase::kSolenoidModules;
+const uint32_t SensorBase::kPwmChannels;
+const uint32_t SensorBase::kRelayChannels;
+const uint32_t SensorBase::kPDPChannels;
+const uint32_t SensorBase::kChassisSlots;
+SensorBase* SensorBase::m_singletonList = nullptr;
+
+static bool portsInitialized = false;
+void* SensorBase::m_digital_ports[kDigitalChannels];
+void* SensorBase::m_relay_ports[kRelayChannels];
+void* SensorBase::m_pwm_ports[kPwmChannels];
+
+/**
+ * Creates an instance of the sensor base and gets an FPGA handle
+ */
+SensorBase::SensorBase() {
+ if (!portsInitialized) {
+ for (uint32_t i = 0; i < kDigitalChannels; i++) {
+ void* port = getPort(i);
+ int32_t status = 0;
+ m_digital_ports[i] = initializeDigitalPort(port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ freePort(port);
+ }
+
+ for (uint32_t i = 0; i < kRelayChannels; i++) {
+ void* port = getPort(i);
+ int32_t status = 0;
+ m_relay_ports[i] = initializeDigitalPort(port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ freePort(port);
+ }
+
+ for (uint32_t i = 0; i < kPwmChannels; i++) {
+ void* port = getPort(i);
+ int32_t status = 0;
+ m_pwm_ports[i] = initializeDigitalPort(port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ freePort(port);
+ }
+ }
+}
+
+/**
+ * Add sensor to the singleton list.
+ * Add this sensor to the list of singletons that need to be deleted when
+ * the robot program exits. Each of the sensors on this list are singletons,
+ * that is they aren't allocated directly with new, but instead are allocated
+ * by the static GetInstance method. As a result, they are never deleted when
+ * the program exits. Consequently these sensors may still be holding onto
+ * resources and need to have their destructors called at the end of the
+ * program.
+ */
+void SensorBase::AddToSingletonList() {
+ m_nextSingleton = m_singletonList;
+ m_singletonList = this;
+}
+
+/**
+ * Delete all the singleton classes on the list.
+ * All the classes that were allocated as singletons need to be deleted so
+ * their resources can be freed.
+ */
+void SensorBase::DeleteSingletons() {
+ for (SensorBase* next = m_singletonList; next != nullptr;) {
+ SensorBase* tmp = next;
+ next = next->m_nextSingleton;
+ delete tmp;
+ }
+ m_singletonList = nullptr;
+}
+
+/**
+ * Check that the solenoid module number is valid.
+ *
+ * @return Solenoid module is valid and present
+ */
+bool SensorBase::CheckSolenoidModule(uint8_t moduleNumber) {
+ if (moduleNumber < 64) return true;
+ return false;
+}
+
+/**
+ * Check that the digital channel number is valid.
+ * Verify that the channel number is one of the legal channel numbers. Channel
+ * numbers are
+ * 1-based.
+ *
+ * @return Digital channel is valid
+ */
+bool SensorBase::CheckDigitalChannel(uint32_t channel) {
+ if (channel < kDigitalChannels) return true;
+ return false;
+}
+
+/**
+ * Check that the digital channel number is valid.
+ * Verify that the channel number is one of the legal channel numbers. Channel
+ * numbers are
+ * 1-based.
+ *
+ * @return Relay channel is valid
+ */
+bool SensorBase::CheckRelayChannel(uint32_t channel) {
+ if (channel < kRelayChannels) return true;
+ return false;
+}
+
+/**
+ * Check that the digital channel number is valid.
+ * Verify that the channel number is one of the legal channel numbers. Channel
+ * numbers are
+ * 1-based.
+ *
+ * @return PWM channel is valid
+ */
+bool SensorBase::CheckPWMChannel(uint32_t channel) {
+ if (channel < kPwmChannels) return true;
+ return false;
+}
+
+/**
+ * Check that the analog input number is value.
+ * Verify that the analog input number is one of the legal channel numbers.
+ * Channel numbers
+ * are 0-based.
+ *
+ * @return Analog channel is valid
+ */
+bool SensorBase::CheckAnalogInput(uint32_t channel) {
+ if (channel < kAnalogInputs) return true;
+ return false;
+}
+
+/**
+ * Check that the analog output number is valid.
+ * Verify that the analog output number is one of the legal channel numbers.
+ * Channel numbers
+ * are 0-based.
+ *
+ * @return Analog channel is valid
+ */
+bool SensorBase::CheckAnalogOutput(uint32_t channel) {
+ if (channel < kAnalogOutputs) return true;
+ return false;
+}
+
+/**
+ * Verify that the solenoid channel number is within limits.
+ *
+ * @return Solenoid channel is valid
+ */
+bool SensorBase::CheckSolenoidChannel(uint32_t channel) {
+ if (channel < kSolenoidChannels) return true;
+ return false;
+}
+
+/**
+ * Verify that the power distribution channel number is within limits.
+ *
+ * @return PDP channel is valid
+ */
+bool SensorBase::CheckPDPChannel(uint32_t channel) {
+ if (channel < kPDPChannels) return true;
+ return false;
+}
diff --git a/wpilibc/Athena/src/SerialPort.cpp b/wpilibc/Athena/src/SerialPort.cpp
new file mode 100644
index 0000000..c4e259a
--- /dev/null
+++ b/wpilibc/Athena/src/SerialPort.cpp
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "SerialPort.h"
+
+#include "HAL/HAL.hpp"
+#include <stdarg.h>
+
+// static ViStatus _VI_FUNCH ioCompleteHandler (ViSession vi, ViEventType
+// eventType, ViEvent event, ViAddr userHandle);
+
+/**
+ * Create an instance of a Serial Port class.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ * @param port The physical port to use
+ * @param dataBits The number of data bits per transfer. Valid values are
+ * between 5 and 8 bits.
+ * @param parity Select the type of parity checking to use.
+ * @param stopBits The number of stop bits to use as defined by the enum
+ * StopBits.
+ */
+SerialPort::SerialPort(uint32_t baudRate, Port port, uint8_t dataBits,
+ SerialPort::Parity parity, SerialPort::StopBits stopBits)
+{
+ int32_t status = 0;
+
+ m_port = port;
+
+ serialInitializePort(port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ serialSetBaudRate(port, baudRate, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ serialSetDataBits(port, dataBits, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ serialSetParity(port, parity, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ serialSetStopBits(port, stopBits, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+
+ // Set the default timeout to 5 seconds.
+ SetTimeout(5.0f);
+
+ // 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_nullptr);
+
+ HALReport(HALUsageReporting::kResourceType_SerialPort, 0);
+}
+
+/**
+ * Destructor.
+ */
+SerialPort::~SerialPort() {
+ int32_t status = 0;
+ serialClose(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the type of flow control to enable on this port.
+ *
+ * By default, flow control is disabled.
+ */
+void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) {
+ int32_t status = 0;
+ serialSetFlowControl(m_port, flowControl, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Enable termination and specify the termination character.
+ *
+ * Termination is currently only implemented for receive.
+ * When the the terminator is recieved, the Read() or Scanf() will return
+ * fewer bytes than requested, stopping after the terminator.
+ *
+ * @param terminator The character to use for termination.
+ */
+void SerialPort::EnableTermination(char terminator) {
+ int32_t status = 0;
+ serialEnableTermination(m_port, terminator, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Disable termination behavior.
+ */
+void SerialPort::DisableTermination() {
+ int32_t status = 0;
+ serialDisableTermination(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Get the number of bytes currently available to read from the serial port.
+ *
+ * @return The number of bytes available to read
+ */
+int32_t SerialPort::GetBytesReceived() {
+ int32_t status = 0;
+ int32_t retVal = serialGetBytesReceived(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Read raw bytes out of the buffer.
+ *
+ * @param buffer Pointer to the buffer to store the bytes in.
+ * @param count The maximum number of bytes to read.
+ * @return The number of bytes actually read into the buffer.
+ */
+uint32_t SerialPort::Read(char *buffer, int32_t count) {
+ int32_t status = 0;
+ int32_t retVal = serialRead(m_port, buffer, count, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Write raw bytes to the buffer.
+ *
+ * @param buffer Pointer to the buffer to read the bytes from.
+ * @param count The maximum number of bytes to write.
+ * @return The number of bytes actually written into the port.
+ */
+uint32_t SerialPort::Write(const std::string &buffer, int32_t count) {
+ int32_t status = 0;
+ int32_t retVal = serialWrite(m_port, buffer.c_str(), count, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return retVal;
+}
+
+/**
+ * Configure the timeout of the serial port.
+ *
+ * This defines the timeout for transactions with the hardware.
+ * It will affect reads and very large writes.
+ *
+ * @param timeout The number of seconds to to wait for I/O.
+ */
+void SerialPort::SetTimeout(float timeout) {
+ int32_t status = 0;
+ serialSetTimeout(m_port, timeout, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Specify the size of the input buffer.
+ *
+ * Specify the amount of data that can be stored before data
+ * from the device is returned to Read or Scanf. If you want
+ * data that is recieved to be returned immediately, set this to 1.
+ *
+ * It the buffer is not filled before the read timeout expires, all
+ * data that has been received so far will be returned.
+ *
+ * @param size The read buffer size.
+ */
+void SerialPort::SetReadBufferSize(uint32_t size) {
+ int32_t status = 0;
+ serialSetReadBufferSize(m_port, size, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Specify the size of the output buffer.
+ *
+ * Specify the amount of data that can be stored before being
+ * transmitted to the device.
+ *
+ * @param size The write buffer size.
+ */
+void SerialPort::SetWriteBufferSize(uint32_t size) {
+ int32_t status = 0;
+ serialSetWriteBufferSize(m_port, size, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Specify the flushing behavior of the output buffer.
+ *
+ * When set to kFlushOnAccess, data is synchronously written to the serial port
+ * after each call to either Printf() or Write().
+ *
+ * When set to kFlushWhenFull, data will only be written to the serial port when
+ * the buffer is full or when Flush() is called.
+ *
+ * @param mode The write buffer mode.
+ */
+void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode) {
+ int32_t status = 0;
+ serialSetWriteMode(m_port, mode, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Force the output buffer to be written to the port.
+ *
+ * This is used when SetWriteBufferMode() is set to kFlushWhenFull to force a
+ * flush before the buffer is full.
+ */
+void SerialPort::Flush() {
+ int32_t status = 0;
+ serialFlush(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Reset the serial port driver to a known state.
+ *
+ * Empty the transmit and receive buffers in the device and formatted I/O.
+ */
+void SerialPort::Reset() {
+ int32_t status = 0;
+ serialClear(m_port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
diff --git a/wpilibc/Athena/src/Servo.cpp b/wpilibc/Athena/src/Servo.cpp
new file mode 100644
index 0000000..4300cc4
--- /dev/null
+++ b/wpilibc/Athena/src/Servo.cpp
@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Servo.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+constexpr float Servo::kMaxServoAngle;
+constexpr float Servo::kMinServoAngle;
+
+constexpr float Servo::kDefaultMaxServoPWM;
+constexpr float Servo::kDefaultMinServoPWM;
+
+/**
+ * @param channel The PWM channel to which the servo is attached. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+Servo::Servo(uint32_t channel) : SafePWM(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);
+
+ // printf("Done initializing servo %d\n", channel);
+}
+
+Servo::~Servo() {
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+/**
+ * Set the servo position.
+ *
+ * Servo values range from 0.0 to 1.0 corresponding to the range of full left to
+ * full right.
+ *
+ * @param value Position from 0.0 to 1.0.
+ */
+void Servo::Set(float value) { SetPosition(value); }
+
+/**
+ * Set the servo to offline.
+ *
+ * Set the servo raw value to 0 (undriven)
+ */
+void Servo::SetOffline() { SetRaw(0); }
+
+/**
+ * Get the servo position.
+ *
+ * Servo values range from 0.0 to 1.0 corresponding to the range of full left to
+ * full right.
+ *
+ * @return Position from 0.0 to 1.0.
+ */
+float Servo::Get() const { return GetPosition(); }
+
+/**
+ * Set the servo angle.
+ *
+ * Assume that the servo angle is linear with respect to the PWM value (big
+ * assumption, need to test).
+ *
+ * Servo angles that are out of the supported range of the servo simply
+ * "saturate" in that direction
+ * In other words, if the servo has a range of (X degrees to Y degrees) than
+ * angles of less than X
+ * result in an angle of X being set and angles of more than Y degrees result in
+ * an angle of Y being set.
+ *
+ * @param degrees The angle in degrees to set the servo.
+ */
+void Servo::SetAngle(float degrees) {
+ if (degrees < kMinServoAngle) {
+ degrees = kMinServoAngle;
+ } else if (degrees > kMaxServoAngle) {
+ degrees = kMaxServoAngle;
+ }
+
+ SetPosition(((float)(degrees - kMinServoAngle)) / GetServoAngleRange());
+}
+
+/**
+ * Get the servo angle.
+ *
+ * Assume that the servo angle is linear with respect to the PWM value (big
+ * assumption, need to test).
+ * @return The angle in degrees to which the servo is set.
+ */
+float Servo::GetAngle() const {
+ return (float)GetPosition() * GetServoAngleRange() + kMinServoAngle;
+}
+
+void Servo::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsDouble()) return;
+ Set(value->GetDouble());
+}
+
+void Servo::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", Get());
+ }
+}
+
+void Servo::StartLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void Servo::StopLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string Servo::GetSmartDashboardType() const { return "Servo"; }
+
+void Servo::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Servo::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/Solenoid.cpp b/wpilibc/Athena/src/Solenoid.cpp
new file mode 100644
index 0000000..5174f98
--- /dev/null
+++ b/wpilibc/Athena/src/Solenoid.cpp
@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Solenoid.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+#include <sstream>
+
+/**
+ * Constructor using the default PCM ID (0).
+ *
+ * @param channel The channel on the PCM to control (0..7).
+ */
+Solenoid::Solenoid(uint32_t channel)
+ : Solenoid(GetDefaultSolenoidModule(), channel) {}
+
+/**
+ * Constructor.
+ *
+ * @param moduleNumber The CAN ID of the PCM the solenoid is attached to
+ * @param channel The channel on the PCM to control (0..7).
+ */
+Solenoid::Solenoid(uint8_t moduleNumber, uint32_t channel)
+ : SolenoidBase(moduleNumber), m_channel(channel) {
+ std::stringstream buf;
+ if (!CheckSolenoidModule(m_moduleNumber)) {
+ buf << "Solenoid Module " << m_moduleNumber;
+ wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf.str());
+ return;
+ }
+ if (!CheckSolenoidChannel(m_channel)) {
+ buf << "Solenoid Module " << m_channel;
+ wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
+ return;
+ }
+ Resource::CreateResourceObject(m_allocated, m_maxModules * m_maxPorts);
+ buf << "Solenoid " << m_channel << " (Module: " << m_moduleNumber << ")";
+ if (m_allocated->Allocate(m_moduleNumber * kSolenoidChannels + m_channel,
+ buf.str()) ==
+ std::numeric_limits<uint32_t>::max()) {
+ CloneError(*m_allocated);
+ return;
+ }
+
+ LiveWindow::GetInstance()->AddActuator("Solenoid", m_moduleNumber, m_channel,
+ this);
+ HALReport(HALUsageReporting::kResourceType_Solenoid, m_channel,
+ m_moduleNumber);
+}
+
+/**
+ * Destructor.
+ */
+Solenoid::~Solenoid() {
+ if (CheckSolenoidModule(m_moduleNumber)) {
+ m_allocated->Free(m_moduleNumber * kSolenoidChannels + m_channel);
+ }
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param on Turn the solenoid output off or on.
+ */
+void Solenoid::Set(bool on) {
+ if (StatusIsFatal()) return;
+ uint8_t value = on ? 0xFF : 0x00;
+ uint8_t mask = 1 << m_channel;
+
+ SolenoidBase::Set(value, mask, m_moduleNumber);
+}
+
+/**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+bool Solenoid::Get() const {
+ if (StatusIsFatal()) return false;
+ uint8_t value = GetAll(m_moduleNumber) & (1 << m_channel);
+ return (value != 0);
+}
+/**
+ * Check if solenoid is blacklisted.
+ * If a solenoid is shorted, it is added to the blacklist and
+ * disabled until power cycle, or until faults are cleared.
+ * @see ClearAllPCMStickyFaults()
+ *
+ * @return If solenoid is disabled due to short.
+ */
+bool Solenoid::IsBlackListed() const {
+ int value = GetPCMSolenoidBlackList(m_moduleNumber) & (1 << m_channel);
+ return (value != 0);
+}
+
+void Solenoid::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if (!value->IsBoolean()) return;
+ Set(value->GetBoolean());
+}
+
+void Solenoid::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutBoolean("Value", Get());
+ }
+}
+
+void Solenoid::StartLiveWindowMode() {
+ Set(false);
+ if (m_table != nullptr) {
+ m_table->AddTableListener("Value", this, true);
+ }
+}
+
+void Solenoid::StopLiveWindowMode() {
+ Set(false);
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string Solenoid::GetSmartDashboardType() const { return "Solenoid"; }
+
+void Solenoid::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Solenoid::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/SolenoidBase.cpp b/wpilibc/Athena/src/SolenoidBase.cpp
new file mode 100644
index 0000000..dfdf1eb
--- /dev/null
+++ b/wpilibc/Athena/src/SolenoidBase.cpp
@@ -0,0 +1,105 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "SolenoidBase.h"
+
+void* SolenoidBase::m_ports[m_maxModules][m_maxPorts];
+std::unique_ptr<Resource> SolenoidBase::m_allocated;
+
+/**
+ * Constructor
+ *
+ * @param moduleNumber The CAN PCM ID.
+ */
+SolenoidBase::SolenoidBase(uint8_t moduleNumber)
+ : m_moduleNumber(moduleNumber) {
+ for (uint32_t i = 0; i < kSolenoidChannels; i++) {
+ void* port = getPortWithModule(moduleNumber, i);
+ int32_t status = 0;
+ SolenoidBase::m_ports[moduleNumber][i] =
+ initializeSolenoidPort(port, &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ freePort(port);
+ }
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param value The value you want to set on the module.
+ * @param mask The channels you want to be affected.
+ */
+void SolenoidBase::Set(uint8_t value, uint8_t mask, int module) {
+ int32_t status = 0;
+ for (int i = 0; i < m_maxPorts; i++) {
+ uint8_t local_mask = 1 << i;
+ if (mask & local_mask)
+ setSolenoid(m_ports[module][i], value & local_mask, &status);
+ }
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Read all 8 solenoids as a single byte
+ *
+ * @return The current value of all 8 solenoids on the module.
+ */
+uint8_t SolenoidBase::GetAll(int module) const {
+ uint8_t value = 0;
+ int32_t status = 0;
+ value = getAllSolenoids(m_ports[module][0], &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+}
+/**
+ * Reads complete solenoid blacklist for all 8 solenoids as a single byte.
+ *
+ * If a solenoid is shorted, it is added to the blacklist and
+ * disabled until power cycle, or until faults are cleared.
+ * @see ClearAllPCMStickyFaults()
+ *
+ * @return The solenoid blacklist of all 8 solenoids on the module.
+ */
+uint8_t SolenoidBase::GetPCMSolenoidBlackList(int module) const {
+ int32_t status = 0;
+ return getPCMSolenoidBlackList(m_ports[module][0], &status);
+}
+/**
+ * @return true if PCM sticky fault is set : The common
+ * highside solenoid voltage rail is too low,
+ * most likely a solenoid channel is shorted.
+ */
+bool SolenoidBase::GetPCMSolenoidVoltageStickyFault(int module) const {
+ int32_t status = 0;
+ return getPCMSolenoidVoltageStickyFault(m_ports[module][0], &status);
+}
+/**
+ * @return true if PCM is in fault state : The common
+ * highside solenoid voltage rail is too low,
+ * most likely a solenoid channel is shorted.
+ */
+bool SolenoidBase::GetPCMSolenoidVoltageFault(int module) const {
+ int32_t status = 0;
+ return getPCMSolenoidVoltageFault(m_ports[module][0], &status);
+}
+/**
+ * Clear ALL sticky faults inside PCM that Compressor is wired to.
+ *
+ * If a sticky fault is set, then it will be persistently cleared. Compressor
+ * drive
+ * maybe momentarily disable while flags are being cleared. Care
+ * should be
+ * taken to not call this too frequently, otherwise normal
+ * compressor
+ * functionality may be prevented.
+ *
+ * If no sticky faults are set then this call will have no effect.
+ */
+void SolenoidBase::ClearAllPCMStickyFaults(int module) {
+ int32_t status = 0;
+ return clearAllPCMStickyFaults_sol(m_ports[module][0], &status);
+}
diff --git a/wpilibc/Athena/src/Talon.cpp b/wpilibc/Athena/src/Talon.cpp
new file mode 100644
index 0000000..dc1cf3b
--- /dev/null
+++ b/wpilibc/Athena/src/Talon.cpp
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Talon.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Constructor for a Talon (original or Talon SR)
+ * @param channel The PWM channel number that the Talon is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+Talon::Talon(uint32_t channel) : SafePWM(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);
+ SetRaw(m_centerPwm);
+ SetZeroLatch();
+
+ HALReport(HALUsageReporting::kResourceType_Talon, GetChannel());
+ LiveWindow::GetInstance()->AddActuator("Talon", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ * @param syncGroup Unused interface.
+ */
+void Talon::Set(float speed, uint8_t syncGroup) {
+ SetSpeed(m_isInverted ? -speed : speed);
+}
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+float Talon::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Talon::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+* common interface for inverting direction of a speed controller
+* @param isInverted The state of inversion true is inverted
+*/
+void Talon::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+/**
+ * Common interface for the inverting direction of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ *
+ */
+bool Talon::GetInverted() const { return m_isInverted; }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Talon::PIDWrite(float output) { Set(output); }
diff --git a/wpilibc/Athena/src/TalonSRX.cpp b/wpilibc/Athena/src/TalonSRX.cpp
new file mode 100644
index 0000000..07438c4
--- /dev/null
+++ b/wpilibc/Athena/src/TalonSRX.cpp
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "TalonSRX.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Construct a TalonSRX connected via PWM
+ * @param channel The PWM channel that the TalonSRX is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+TalonSRX::TalonSRX(uint32_t channel) : SafePWM(channel) {
+ /* Note that the TalonSRX 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);
+ SetRaw(m_centerPwm);
+ SetZeroLatch();
+
+ HALReport(HALUsageReporting::kResourceType_TalonSRX, GetChannel());
+ LiveWindow::GetInstance()->AddActuator("TalonSRX", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ * @param syncGroup Unused interface.
+ */
+void TalonSRX::Set(float speed, uint8_t syncGroup) { SetSpeed(speed); }
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+float TalonSRX::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void TalonSRX::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+* Common interface for inverting direction of a speed controller.
+* @param isInverted The state of inversion, true is inverted.
+*/
+void TalonSRX::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+/**
+ * Common interface for the inverting direction of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ *
+ */
+bool TalonSRX::GetInverted() const { return m_isInverted; }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void TalonSRX::PIDWrite(float output) { Set(output); }
diff --git a/wpilibc/Athena/src/Task.cpp b/wpilibc/Athena/src/Task.cpp
new file mode 100644
index 0000000..9157dca
--- /dev/null
+++ b/wpilibc/Athena/src/Task.cpp
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Task.h"
+
+#include "WPIErrors.h"
+#include <errno.h>
+#include <string.h>
+#include <stdio.h>
+
+#ifndef OK
+#define OK 0
+#endif /* OK */
+#ifndef ERROR
+#define ERROR (-1)
+#endif /* ERROR */
+
+const uint32_t Task::kDefaultPriority;
+
+Task& Task::operator=(Task&& task) {
+ m_thread.swap(task.m_thread);
+ m_taskName = std::move(task.m_taskName);
+
+ return *this;
+}
+
+Task::~Task() {
+ if (m_thread.joinable()) {
+ std::cout << "[HAL] Exited task " << m_taskName << std::endl;
+ }
+}
+
+bool Task::joinable() const noexcept {
+ return m_thread.joinable();
+}
+
+void Task::join() {
+ m_thread.join();
+}
+
+void Task::detach() {
+ m_thread.detach();
+}
+
+std::thread::id Task::get_id() const noexcept {
+ return m_thread.get_id();
+}
+
+std::thread::native_handle_type Task::native_handle() {
+ return m_thread.native_handle();
+}
+
+/**
+ * Verifies a task still exists.
+ *
+ * @return true on success.
+ */
+bool Task::Verify() {
+ TASK id = (TASK)m_thread.native_handle();
+ return verifyTaskID(id) == OK;
+}
+
+/**
+ * Gets the priority of a task.
+ *
+ * @return task priority or 0 if an error occured
+ */
+int32_t Task::GetPriority() {
+ int priority;
+ auto id = m_thread.native_handle();
+ if (HandleError(getTaskPriority(&id, &priority)))
+ return priority;
+ else
+ return 0;
+}
+
+/**
+ * This routine changes a task's priority to a specified priority.
+ * Priorities range from 1, the lowest priority, to 99, the highest priority.
+ * Default task priority is 60.
+ *
+ * @param priority The priority at which the internal thread should run.
+ * @return true on success.
+ */
+bool Task::SetPriority(int32_t priority) {
+ auto id = m_thread.native_handle();
+ return HandleError(setTaskPriority(&id, priority));
+}
+
+/**
+ * Returns the name of the task.
+ *
+ * @return The name of the task.
+ */
+std::string Task::GetName() const { return m_taskName; }
+
+/**
+ * Handles errors generated by task related code.
+ */
+bool Task::HandleError(STATUS results) {
+ if (results != ERROR) return true;
+ int errsv = errno;
+ if (errsv == HAL_taskLib_ILLEGAL_PRIORITY) {
+ wpi_setWPIErrorWithContext(TaskPriorityError, m_taskName.c_str());
+ } else {
+ printf("ERROR: errno=%i", errsv);
+ wpi_setWPIErrorWithContext(TaskError, m_taskName.c_str());
+ }
+ return false;
+}
diff --git a/wpilibc/Athena/src/Timer.cpp b/wpilibc/Athena/src/Timer.cpp
new file mode 100644
index 0000000..53631e7
--- /dev/null
+++ b/wpilibc/Athena/src/Timer.cpp
@@ -0,0 +1,177 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Timer.h"
+
+#include <time.h>
+
+#include "HAL/HAL.hpp"
+#include "Utility.h"
+#include <iostream>
+
+/**
+ * Pause the task for a specified time.
+ *
+ * Pause the execution of the program for a specified period of time given in
+ * seconds.
+ * Motors will continue to run at their last assigned values, and sensors will
+ * continue to
+ * update. Only the task containing the wait will pause until the wait time is
+ * expired.
+ *
+ * @param seconds Length of time to pause, in seconds.
+ */
+void Wait(double seconds) {
+ if (seconds < 0.0) return;
+ delaySeconds(seconds);
+}
+
+/**
+ * Return the FPGA system clock time in seconds.
+ * This is deprecated and just forwards to Timer::GetFPGATimestamp().
+ * @return Robot running time in seconds.
+ */
+double GetClock() { return Timer::GetFPGATimestamp(); }
+
+/**
+ * @brief Gives real-time clock system time with nanosecond resolution
+ * @return The time, just in case you want the robot to start autonomous at 8pm
+ * on Saturday.
+*/
+double GetTime() {
+ struct timespec tp;
+
+ clock_gettime(CLOCK_REALTIME, &tp);
+ double realTime = (double)tp.tv_sec + (double)((double)tp.tv_nsec * 1e-9);
+
+ return (realTime);
+}
+
+//for compatibility with msvc12--see C2864
+const double Timer::kRolloverTime = (1ll << 32) / 1e6;
+/**
+ * Create a new timer object.
+ *
+ * Create a new timer object and reset the time to zero. The timer is initially
+ * not running and
+ * must be started.
+ */
+Timer::Timer() {
+ // Creates a semaphore to control access to critical regions.
+ // Initially 'open'
+ Reset();
+}
+
+/**
+ * Get the current time from the timer. If the clock is running it is derived
+ * from
+ * the current system clock the start time stored in the timer class. If the
+ * clock
+ * is not running, then return the time when it was last stopped.
+ *
+ * @return Current time value for this timer in seconds
+ */
+double Timer::Get() const {
+ double result;
+ double currentTime = GetFPGATimestamp();
+
+ std::lock_guard<priority_mutex> sync(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;
+}
+
+/**
+ * Reset the timer by setting the time to 0.
+ *
+ * Make the timer startTime the current time so new requests will be relative to
+ * now
+ */
+void Timer::Reset() {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ m_accumulatedTime = 0;
+ m_startTime = GetFPGATimestamp();
+}
+
+/**
+ * Start the timer running.
+ * Just set the running flag to true indicating that all time requests should be
+ * relative to the system clock.
+ */
+void Timer::Start() {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ if (!m_running) {
+ m_startTime = GetFPGATimestamp();
+ m_running = true;
+ }
+}
+
+/**
+ * Stop the timer.
+ * This computes the time as of now and clears the running flag, causing all
+ * subsequent time requests to be read from the accumulated time rather than
+ * looking at the system clock.
+ */
+void Timer::Stop() {
+ double temp = Get();
+
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ if (m_running) {
+ m_accumulatedTime = temp;
+ m_running = false;
+ }
+}
+
+/**
+ * Check if the period specified has passed and if it has, advance the start
+ * time by that period. This is useful to decide if it's time to do periodic
+ * work without drifting later by the time it took to get around to checking.
+ *
+ * @param period The period to check for (in seconds).
+ * @return True if the period has passed.
+ */
+bool Timer::HasPeriodPassed(double period) {
+ if (Get() > period) {
+ std::lock_guard<priority_mutex> sync(m_mutex);
+ // Advance the start time by the period.
+ m_startTime += period;
+ // Don't set it to the current time... we want to avoid drift.
+ return true;
+ }
+ return false;
+}
+
+/**
+ * Return the FPGA system clock time in seconds.
+ *
+ * Return the time from the FPGA hardware clock in seconds since the FPGA
+ * started.
+ * Rolls over after 71 minutes.
+ * @returns Robot running time in seconds.
+ */
+double Timer::GetFPGATimestamp() {
+ // FPGA returns the timestamp in microseconds
+ // Call the helper GetFPGATime() in Utility.cpp
+ return GetFPGATime() * 1.0e-6;
+}
+
+// Internal function that reads the PPC timestamp counter.
+extern "C" {
+uint32_t niTimestamp32(void);
+uint64_t niTimestamp64(void);
+}
diff --git a/wpilibc/Athena/src/USBCamera.cpp b/wpilibc/Athena/src/USBCamera.cpp
new file mode 100644
index 0000000..0c57d85
--- /dev/null
+++ b/wpilibc/Athena/src/USBCamera.cpp
@@ -0,0 +1,323 @@
+#include "USBCamera.h"
+
+#include "Utility.h"
+
+#include <regex>
+#include <chrono>
+#include <thread>
+#include <memory>
+#include <iostream>
+#include <iomanip>
+
+// This macro expands the given imaq function to ensure that it is called and
+// properly checked for an error, calling the wpi_setImaqErrorWithContext
+// macro
+// To call it, just give the name of the function and the arguments
+#define SAFE_IMAQ_CALL(funName, ...) \
+ { \
+ unsigned int error = funName(__VA_ARGS__); \
+ if (error != IMAQdxErrorSuccess) \
+ wpi_setImaqErrorWithContext(error, #funName); \
+ }
+
+/**
+ * Helper function to determine the size of a jpeg. The general structure of
+ * how to parse a jpeg for length can be found in this stackoverflow article:
+ * http://stackoverflow.com/a/1602428. Be sure to also read the comments for
+ * the SOS flag explanation.
+ */
+unsigned int USBCamera::GetJpegSize(void* buffer, unsigned int buffSize) {
+ uint8_t* data = (uint8_t*)buffer;
+ if (!wpi_assert(data[0] == 0xff && data[1] == 0xd8)) return 0;
+ unsigned int pos = 2;
+ while (pos < buffSize) {
+ // All control markers start with 0xff, so if this isn't present,
+ // the JPEG is not valid
+ if (!wpi_assert(data[pos] == 0xff)) return 0;
+ unsigned char t = data[pos + 1];
+ // These are RST markers. We just skip them and move onto the next marker
+ if (t == 0x01 || (t >= 0xd0 && t <= 0xd7)) {
+ pos += 2;
+ } else if (t == 0xd9) {
+ // End of Image, add 2 for this and 0-indexed
+ return pos + 2;
+ } else if (!wpi_assert(t != 0xd8)) {
+ // Another start of image, invalid image
+ return 0;
+ } else if (t == 0xda) {
+ // SOS marker. The next two bytes are a 16-bit big-endian int that is
+ // the length of the SOS header, skip that
+ unsigned int len = (((unsigned int)(data[pos + 2] & 0xff)) << 8 |
+ ((unsigned int)data[pos + 3] & 0xff));
+ pos += len + 2;
+ // The next marker is the first marker that is 0xff followed by a non-RST
+ // element. 0xff followed by 0x00 is an escaped 0xff. 0xd0-0xd7 are RST
+ // markers
+ while (data[pos] != 0xff || data[pos + 1] == 0x00 ||
+ (data[pos + 1] >= 0xd0 && data[pos + 1] <= 0xd7)) {
+ pos += 1;
+ if (pos >= buffSize) return 0;
+ }
+ } else {
+ // This is one of several possible markers. The next two bytes are a
+ // 16-bit
+ // big-endian int with the length of the marker header, skip that then
+ // continue searching
+ unsigned int len = (((unsigned int)(data[pos + 2] & 0xff)) << 8 |
+ ((unsigned int)data[pos + 3] & 0xff));
+ pos += len + 2;
+ }
+ }
+
+ return 0;
+}
+
+USBCamera::USBCamera(std::string name, bool useJpeg)
+ : m_name(name),
+ m_useJpeg(useJpeg) {}
+
+void USBCamera::OpenCamera() {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ for (unsigned int i = 0; i < 3; i++) {
+ uInt32 id = 0;
+ // Can't use SAFE_IMAQ_CALL here because we only error on the third time
+ IMAQdxError error = IMAQdxOpenCamera(
+ m_name.c_str(), IMAQdxCameraControlModeController, &id);
+ if (error != IMAQdxErrorSuccess) {
+ // Only error on the 3rd try
+ if (i >= 2) wpi_setImaqErrorWithContext(error, "IMAQdxOpenCamera");
+ // Sleep for a few seconds to ensure the error has been dealt with
+ std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+ } else {
+ m_id = id;
+ m_open = true;
+ return;
+ }
+ }
+}
+
+void USBCamera::CloseCamera() {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ if (!m_open) return;
+ SAFE_IMAQ_CALL(IMAQdxCloseCamera, m_id);
+ m_id = 0;
+ m_open = false;
+}
+
+void USBCamera::StartCapture() {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ if (!m_open || m_active) return;
+ SAFE_IMAQ_CALL(IMAQdxConfigureGrab, m_id);
+ SAFE_IMAQ_CALL(IMAQdxStartAcquisition, m_id);
+ m_active = true;
+}
+
+void USBCamera::StopCapture() {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ if (!m_open || !m_active) return;
+ SAFE_IMAQ_CALL(IMAQdxStopAcquisition, m_id);
+ SAFE_IMAQ_CALL(IMAQdxUnconfigureAcquisition, m_id);
+ m_active = false;
+}
+
+void USBCamera::UpdateSettings() {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ bool wasActive = m_active;
+
+ if (wasActive) StopCapture();
+ if (m_open) CloseCamera();
+ OpenCamera();
+
+ uInt32 count = 0;
+ uInt32 currentMode = 0;
+ SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, nullptr, &count, ¤tMode);
+ auto modes = std::make_unique<IMAQdxVideoMode[]>(count);
+ SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, modes.get(), &count, ¤tMode);
+
+ // Groups are:
+ // 0 - width
+ // 1 - height
+ // 2 - format
+ // 3 - fps
+ std::regex reMode("([0-9]+)\\s*x\\s*([0-9]+)\\s+(.*?)\\s+([0-9.]+)\\s*fps");
+ IMAQdxVideoMode* foundMode = nullptr;
+ IMAQdxVideoMode* currentModePtr = &modes[currentMode];
+ double foundFps = 1000.0;
+
+ // Loop through the modes, and find the match with the lowest fps
+ for (unsigned int i = 0; i < count; i++) {
+ std::cmatch m;
+ if (!std::regex_match(modes[i].Name, m, reMode)) continue;
+ unsigned int width = (unsigned int)std::stoul(m[1].str());
+ unsigned int height = (unsigned int)std::stoul(m[2].str());
+ if (width != m_width) continue;
+ if (height != m_height) continue;
+ double fps = atof(m[4].str().c_str());
+ if (fps < m_fps) continue;
+ if (fps > foundFps) continue;
+ bool isJpeg =
+ m[3].str().compare("jpeg") == 0 || m[3].str().compare("JPEG") == 0;
+ if ((m_useJpeg && !isJpeg) || (!m_useJpeg && isJpeg)) continue;
+ foundMode = &modes[i];
+ foundFps = fps;
+ }
+ if (foundMode != nullptr) {
+ if (foundMode->Value != currentModePtr->Value) {
+ SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, IMAQdxAttributeVideoMode,
+ IMAQdxValueTypeU32, foundMode->Value);
+ }
+ }
+
+ if (m_whiteBalance.compare(AUTO) == 0) {
+ SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_MODE,
+ IMAQdxValueTypeString, AUTO);
+ } else {
+ SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_MODE,
+ IMAQdxValueTypeString, MANUAL);
+ if (m_whiteBalanceValuePresent)
+ SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_VALUE,
+ IMAQdxValueTypeU32, m_whiteBalanceValue);
+ }
+
+ if (m_exposure.compare(AUTO) == 0) {
+ SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_MODE,
+ IMAQdxValueTypeString,
+ std::string("AutoAperaturePriority").c_str());
+ } else {
+ SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_MODE,
+ IMAQdxValueTypeString, MANUAL);
+ if (m_exposureValuePresent) {
+ double minv = 0.0;
+ double maxv = 0.0;
+ SAFE_IMAQ_CALL(IMAQdxGetAttributeMinimum, m_id, ATTR_EX_VALUE,
+ IMAQdxValueTypeF64, &minv);
+ SAFE_IMAQ_CALL(IMAQdxGetAttributeMaximum, m_id, ATTR_EX_VALUE,
+ IMAQdxValueTypeF64, &maxv);
+ double val = minv + ((maxv - minv) * ((double)m_exposureValue / 100.0));
+ SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_VALUE,
+ IMAQdxValueTypeF64, val);
+ }
+ }
+
+ SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_BR_MODE, IMAQdxValueTypeString,
+ MANUAL);
+ double minv = 0.0;
+ double maxv = 0.0;
+ SAFE_IMAQ_CALL(IMAQdxGetAttributeMinimum, m_id, ATTR_BR_VALUE,
+ IMAQdxValueTypeF64, &minv);
+ SAFE_IMAQ_CALL(IMAQdxGetAttributeMaximum, m_id, ATTR_BR_VALUE,
+ IMAQdxValueTypeF64, &maxv);
+ double val = minv + ((maxv - minv) * ((double)m_brightness / 100.0));
+ SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_BR_VALUE, IMAQdxValueTypeF64,
+ val);
+
+ if (wasActive) StartCapture();
+}
+
+void USBCamera::SetFPS(double fps) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ if (m_fps != fps) {
+ m_needSettingsUpdate = true;
+ m_fps = fps;
+ }
+}
+
+void USBCamera::SetSize(unsigned int width, unsigned int height) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ if (m_width != width || m_height != height) {
+ m_needSettingsUpdate = true;
+ m_width = width;
+ m_height = height;
+ }
+}
+
+void USBCamera::SetBrightness(unsigned int brightness) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ if (m_brightness != brightness) {
+ m_needSettingsUpdate = true;
+ m_brightness = brightness;
+ }
+}
+
+unsigned int USBCamera::GetBrightness() {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ return m_brightness;
+}
+
+void USBCamera::SetWhiteBalanceAuto() {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_whiteBalance = AUTO;
+ m_whiteBalanceValue = 0;
+ m_whiteBalanceValuePresent = false;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetWhiteBalanceHoldCurrent() {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_whiteBalance = MANUAL;
+ m_whiteBalanceValue = 0;
+ m_whiteBalanceValuePresent = false;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetWhiteBalanceManual(unsigned int whiteBalance) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_whiteBalance = MANUAL;
+ m_whiteBalanceValue = whiteBalance;
+ m_whiteBalanceValuePresent = true;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetExposureAuto() {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_exposure = AUTO;
+ m_exposureValue = 0;
+ m_exposureValuePresent = false;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetExposureHoldCurrent() {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_exposure = MANUAL;
+ m_exposureValue = 0;
+ m_exposureValuePresent = false;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetExposureManual(unsigned int level) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ m_exposure = MANUAL;
+ if (level > 100)
+ m_exposureValue = 100;
+ else
+ m_exposureValue = level;
+ m_exposureValuePresent = true;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::GetImage(Image* image) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ if (m_needSettingsUpdate || m_useJpeg) {
+ m_needSettingsUpdate = false;
+ m_useJpeg = false;
+ UpdateSettings();
+ }
+ // BufNum is not actually used for anything at our level, since we are
+ // waiting until the next image is ready anyway
+ uInt32 bufNum;
+ SAFE_IMAQ_CALL(IMAQdxGrab, m_id, image, 1, &bufNum);
+}
+
+unsigned int USBCamera::GetImageData(void* buffer, unsigned int bufferSize) {
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+ if (m_needSettingsUpdate || !m_useJpeg) {
+ m_needSettingsUpdate = false;
+ m_useJpeg = true;
+ UpdateSettings();
+ }
+ // BufNum is not actually used for anything at our level
+ uInt32 bufNum;
+ SAFE_IMAQ_CALL(IMAQdxGetImageData, m_id, buffer, bufferSize,
+ IMAQdxBufferNumberModeLast, 0, &bufNum);
+ return GetJpegSize(buffer, bufferSize);
+}
diff --git a/wpilibc/Athena/src/Ultrasonic.cpp b/wpilibc/Athena/src/Ultrasonic.cpp
new file mode 100644
index 0000000..ba25a04
--- /dev/null
+++ b/wpilibc/Athena/src/Ultrasonic.cpp
@@ -0,0 +1,354 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Ultrasonic.h"
+
+#include "Counter.h"
+#include "DigitalInput.h"
+#include "DigitalOutput.h"
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+constexpr double
+ Ultrasonic::kPingTime; ///< Time (sec) for the ping trigger pulse.
+const uint32_t Ultrasonic::kPriority; ///< Priority that the ultrasonic round
+ ///robin task runs.
+constexpr double
+ Ultrasonic::kMaxUltrasonicTime; ///< Max time (ms) between readings.
+constexpr double Ultrasonic::kSpeedOfSoundInchesPerSec;
+Ultrasonic *Ultrasonic::m_firstSensor =
+ nullptr; // head of the ultrasonic sensor list
+Task Ultrasonic::m_task;
+std::atomic<bool> Ultrasonic::m_automaticEnabled{false}; // automatic round robin mode
+priority_mutex Ultrasonic::m_mutex;
+
+/**
+ * Background task that goes through the list of ultrasonic sensors and pings
+ * each one in turn. The counter
+ * is configured to read the timing of the returned echo pulse.
+ *
+ * DANGER WILL ROBINSON, DANGER WILL ROBINSON:
+ * This code runs as a task and assumes that none of the ultrasonic sensors will
+ * change while it's
+ * running. If one does, then this will certainly break. Make sure to disable
+ * automatic mode before changing
+ * anything with the sensors!!
+ */
+void Ultrasonic::UltrasonicChecker() {
+ Ultrasonic *u = nullptr;
+ while (m_automaticEnabled) {
+ if (u == nullptr) u = m_firstSensor;
+ if (u == nullptr) return;
+ if (u->IsEnabled()) u->m_pingChannel->Pulse(kPingTime); // do the ping
+ u = u->m_nextSensor;
+ Wait(0.1); // wait for ping to return
+ }
+}
+
+/**
+ * Initialize the Ultrasonic Sensor.
+ * This is the common code that initializes the ultrasonic sensor given that
+ * there
+ * are two digital I/O channels allocated. If the system was running in
+ * automatic mode (round robin)
+ * when the new sensor is added, it is stopped, the sensor is added, then
+ * automatic mode is
+ * restored.
+ */
+void Ultrasonic::Initialize() {
+ bool originalMode = m_automaticEnabled;
+ SetAutomaticMode(false); // kill task when adding a new sensor
+ // link this instance on the list
+ {
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ m_nextSensor = m_firstSensor;
+ m_firstSensor = 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++;
+ HALReport(HALUsageReporting::kResourceType_Ultrasonic, instances);
+ LiveWindow::GetInstance()->AddSensor("Ultrasonic",
+ m_echoChannel->GetChannel(), this);
+}
+
+/**
+ * Create an instance of the Ultrasonic Sensor
+ * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic
+ * sensors.
+ * @param pingChannel The digital output channel that sends the pulse to
+ * initiate the sensor sending
+ * the ping.
+ * @param echoChannel The digital input channel that receives the echo. The
+ * length of time that the
+ * echo is high represents the round trip time of the ping, and the distance.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+Ultrasonic::Ultrasonic(uint32_t pingChannel, uint32_t 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();
+}
+
+/**
+ * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo
+ * channel and a DigitalOutput
+ * for the ping channel.
+ * @param pingChannel The digital output object that starts the sensor doing a
+ * ping. Requires a 10uS pulse to start.
+ * @param echoChannel The digital input object that times the return pulse to
+ * determine the range.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+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_nextSensor = nullptr;
+ m_units = units;
+ return;
+ }
+ m_units = units;
+ Initialize();
+}
+
+/**
+ * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo
+ * channel and a DigitalOutput
+ * for the ping channel.
+ * @param pingChannel The digital output object that starts the sensor doing a
+ * ping. Requires a 10uS pulse to start.
+ * @param echoChannel The digital input object that times the return pulse to
+ * determine the range.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+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();
+}
+
+/**
+ * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo
+ * channel and a DigitalOutput
+ * for the ping channel.
+ * @param pingChannel The digital output object that starts the sensor doing a
+ * ping. Requires a 10uS pulse to start.
+ * @param echoChannel The digital input object that times the return pulse to
+ * determine the range.
+ * @param units The units returned in either kInches or kMilliMeters
+ */
+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();
+}
+
+/**
+ * Destructor for the ultrasonic sensor.
+ * 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).
+ */
+Ultrasonic::~Ultrasonic() {
+ bool wasAutomaticMode = m_automaticEnabled;
+ SetAutomaticMode(false);
+ wpi_assert(m_firstSensor != nullptr);
+
+ {
+ std::lock_guard<priority_mutex> lock(m_mutex);
+ if (this == m_firstSensor) {
+ m_firstSensor = m_nextSensor;
+ if (m_firstSensor == nullptr) {
+ SetAutomaticMode(false);
+ }
+ } else {
+ wpi_assert(m_firstSensor->m_nextSensor != nullptr);
+ for (Ultrasonic *s = m_firstSensor; s != nullptr; s = s->m_nextSensor) {
+ if (this == s->m_nextSensor) {
+ s->m_nextSensor = s->m_nextSensor->m_nextSensor;
+ break;
+ }
+ }
+ }
+ }
+ if (m_firstSensor != nullptr && wasAutomaticMode) SetAutomaticMode(true);
+}
+
+/**
+ * Turn Automatic mode on/off.
+ * When in Automatic mode, all sensors will fire in round robin, waiting a set
+ * time between each sensor.
+ * @param enabling Set to true if round robin scheduling should start for all
+ * the ultrasonic sensors. This
+ * scheduling method assures that the sensors are non-interfering because no two
+ * sensors fire at the same time.
+ * If another scheduling algorithm is prefered, it can be implemented by
+ * pinging the sensors manually and waiting
+ * for the results to come back.
+ */
+void Ultrasonic::SetAutomaticMode(bool enabling) {
+ if (enabling == m_automaticEnabled) return; // ignore the case of no change
+
+ m_automaticEnabled = enabling;
+ if (enabling) {
+ // enabling automatic mode.
+ // Clear all the counters so no data is valid
+ for (Ultrasonic *u = m_firstSensor; u != nullptr; u = u->m_nextSensor) {
+ u->m_counter.Reset();
+ }
+ // Start round robin task
+ wpi_assert(m_task.Verify() ==
+ false); // should be false since was previously disabled
+ m_task = Task("UltrasonicChecker", &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 {
+ // disabling automatic mode. Wait for background task to stop running.
+ while (m_task.Verify())
+ Wait(0.15); // just a little longer than the ping time for round-robin to
+ // stop
+
+ // clear all the counters (data now invalid) since automatic mode is stopped
+ for (Ultrasonic *u = m_firstSensor; u != nullptr; u = u->m_nextSensor) {
+ u->m_counter.Reset();
+ }
+ m_automaticEnabled = false;
+ m_task.join();
+ }
+}
+
+/**
+ * Single ping to ultrasonic sensor.
+ * Send out a single ping to the ultrasonic sensor. This only works if automatic
+ * (round robin)
+ * mode is disabled. A single ping is sent out, and the counter should count the
+ * semi-period
+ * when it comes in. The counter is reset to make the current value invalid.
+ */
+void Ultrasonic::Ping() {
+ wpi_assert(!m_automaticEnabled);
+ m_counter.Reset(); // reset the counter to zero (invalid data now)
+ m_pingChannel->Pulse(
+ kPingTime); // do the ping to start getting a single range
+}
+
+/**
+ * Check if there is a valid range measurement.
+ * The ranges are accumulated in a counter that will increment on each edge of
+ * the echo (return)
+ * signal. If the count is not at least 2, then the range has not yet been
+ * measured, and is invalid.
+ */
+bool Ultrasonic::IsRangeValid() const { return m_counter.Get() > 1; }
+
+/**
+ * Get the range in inches from the ultrasonic sensor.
+ * @return double Range in inches of the target returned from the ultrasonic
+ * sensor. If there is
+ * no valid value yet, i.e. at least one measurement hasn't completed, then
+ * return 0.
+ */
+double Ultrasonic::GetRangeInches() const {
+ if (IsRangeValid())
+ return m_counter.GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
+ else
+ return 0;
+}
+
+/**
+ * Get the range in millimeters from the ultrasonic sensor.
+ * @return double Range in millimeters of the target returned by the ultrasonic
+ * sensor.
+ * If there is no valid value yet, i.e. at least one measurement hasn't
+ * complted, then return 0.
+ */
+double Ultrasonic::GetRangeMM() const { return GetRangeInches() * 25.4; }
+
+/**
+ * Get the range in the current DistanceUnit for the PIDSource base object.
+ *
+ * @return The range in DistanceUnit
+ */
+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;
+ }
+}
+
+/**
+ * Set the current DistanceUnit that should be used for the PIDSource base
+ * object.
+ *
+ * @param units The DistanceUnit that should be used.
+ */
+void Ultrasonic::SetDistanceUnits(DistanceUnit units) { m_units = units; }
+
+/**
+ * Get the current DistanceUnit that is used for the PIDSource base object.
+ *
+ * @return The type of DistanceUnit that is being used.
+ */
+Ultrasonic::DistanceUnit Ultrasonic::GetDistanceUnits() const {
+ return m_units;
+}
+
+void Ultrasonic::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutNumber("Value", GetRangeInches());
+ }
+}
+
+void Ultrasonic::StartLiveWindowMode() {}
+
+void Ultrasonic::StopLiveWindowMode() {}
+
+std::string Ultrasonic::GetSmartDashboardType() const { return "Ultrasonic"; }
+
+void Ultrasonic::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> Ultrasonic::GetTable() const { return m_table; }
diff --git a/wpilibc/Athena/src/Utility.cpp b/wpilibc/Athena/src/Utility.cpp
new file mode 100644
index 0000000..5998f0f
--- /dev/null
+++ b/wpilibc/Athena/src/Utility.cpp
@@ -0,0 +1,222 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Utility.h"
+
+//#include "NetworkCommunication/FRCComm.h"
+#include "HAL/HAL.hpp"
+#include "Task.h"
+#include <iostream>
+#include <sstream>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <execinfo.h>
+#include <cxxabi.h>
+#include "nivision.h"
+
+/**
+ * Assert implementation.
+ * This allows breakpoints to be set on an assert.
+ * The users don't call this, but instead use the wpi_assert macros in
+ * Utility.h.
+ */
+bool wpi_assert_impl(bool conditionValue, const char *conditionText,
+ const char *message, const char *fileName,
+ uint32_t lineNumber, const char *funcName) {
+ if (!conditionValue) {
+ std::stringstream errorStream;
+
+ errorStream << "Assertion \"" << conditionText << "\" ";
+ errorStream << "on line " << lineNumber << " ";
+ errorStream << "of " << basename(fileName) << " ";
+
+ if (message[0] != '\0') {
+ errorStream << "failed: " << message << std::endl;
+ } else {
+ errorStream << "failed." << std::endl;
+ }
+
+ errorStream << GetStackTrace(2);
+
+ std::string error = errorStream.str();
+
+ // Print the error and send it to the DriverStation
+ std::cout << error << std::endl;
+ HALSetErrorData(error.c_str(), error.size(), 100);
+ }
+
+ 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 char *valueA, const char *valueB,
+ const char *equalityType,
+ const char *message,
+ const char *fileName,
+ uint32_t lineNumber,
+ const char *funcName) {
+ std::stringstream errorStream;
+
+ errorStream << "Assertion \"" << valueA << " " << equalityType << " "
+ << valueB << "\" ";
+ errorStream << "on line " << lineNumber << " ";
+ errorStream << "of " << basename(fileName) << " ";
+
+ if (message[0] != '\0') {
+ errorStream << "failed: " << message << std::endl;
+ } else {
+ errorStream << "failed." << std::endl;
+ }
+
+ errorStream << GetStackTrace(3);
+
+ std::string error = errorStream.str();
+
+ // Print the error and send it to the DriverStation
+ std::cout << error << std::endl;
+ HALSetErrorData(error.c_str(), error.size(), 100);
+}
+
+/**
+ * Assert equal implementation.
+ * This determines whether the two given integers are equal. If not,
+ * the value of each is printed along with an optional message string.
+ * The users don't call this, but instead use the wpi_assertEqual macros in
+ * Utility.h.
+ */
+bool wpi_assertEqual_impl(int valueA, int valueB, const char *valueAString,
+ const char *valueBString, const char *message,
+ const char *fileName, uint32_t lineNumber,
+ const char *funcName) {
+ if (!(valueA == valueB)) {
+ wpi_assertEqual_common_impl(valueAString, valueBString, "==", message,
+ fileName, lineNumber, funcName);
+ }
+ return valueA == valueB;
+}
+
+/**
+ * Assert not equal implementation.
+ * This determines whether the two given integers are equal. If so,
+ * the value of each is printed along with an optional message string.
+ * The users don't call this, but instead use the wpi_assertNotEqual macros in
+ * Utility.h.
+ */
+bool wpi_assertNotEqual_impl(int valueA, int valueB, const char *valueAString,
+ const char *valueBString, const char *message,
+ const char *fileName, uint32_t lineNumber,
+ const char *funcName) {
+ if (!(valueA != valueB)) {
+ wpi_assertEqual_common_impl(valueAString, valueBString, "!=", message,
+ fileName, lineNumber, funcName);
+ }
+ return valueA != valueB;
+}
+
+/**
+ * Return the FPGA Version number.
+ * For now, expect this to be competition year.
+ * @return FPGA Version number.
+ */
+uint16_t GetFPGAVersion() {
+ int32_t status = 0;
+ uint16_t version = getFPGAVersion(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return version;
+}
+
+/**
+ * Return the FPGA Revision number.
+ * The format of the revision is 3 numbers.
+ * The 12 most significant bits are the Major Revision.
+ * the next 8 bits are the Minor Revision.
+ * The 12 least significant bits are the Build Number.
+ * @return FPGA Revision number.
+ */
+uint32_t GetFPGARevision() {
+ int32_t status = 0;
+ uint32_t revision = getFPGARevision(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return revision;
+}
+
+/**
+ * Read the microsecond-resolution timer on the FPGA.
+ *
+ * @return The current time in microseconds according to the FPGA (since FPGA
+ * reset).
+ */
+uint32_t GetFPGATime() {
+ int32_t status = 0;
+ uint32_t time = getFPGATime(&status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+ return time;
+}
+
+/**
+ * Get the state of the "USER" button on the RoboRIO
+ * @return True if the button is currently pressed down
+ */
+bool GetUserButton() {
+ int32_t status = 0;
+
+ bool value = getFPGAButton(&status);
+ wpi_setGlobalError(status);
+
+ return value;
+}
+
+/**
+ * Demangle a C++ symbol, used for printing stack traces.
+ */
+static std::string demangle(char const *mangledSymbol) {
+ char buffer[256];
+ size_t length;
+ int status;
+
+ if (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;
+}
+
+/**
+ * Get a stack trace, ignoring the first "offset" symbols.
+ * @param offset The number of symbols at the top of the stack to ignore
+ */
+std::string GetStackTrace(uint32_t offset) {
+ void *stackTrace[128];
+ int stackSize = backtrace(stackTrace, 128);
+ char **mangledSymbols = backtrace_symbols(stackTrace, stackSize);
+ std::stringstream trace;
+
+ 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]) << std::endl;
+ }
+ }
+
+ free(mangledSymbols);
+
+ return trace.str();
+}
diff --git a/wpilibc/Athena/src/Victor.cpp b/wpilibc/Athena/src/Victor.cpp
new file mode 100644
index 0000000..f8a4970
--- /dev/null
+++ b/wpilibc/Athena/src/Victor.cpp
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Victor.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Constructor for a Victor
+ * @param channel The PWM channel number that the Victor is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+Victor::Victor(uint32_t channel) : SafePWM(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);
+ SetRaw(m_centerPwm);
+ SetZeroLatch();
+
+ LiveWindow::GetInstance()->AddActuator("Victor", GetChannel(), this);
+ HALReport(HALUsageReporting::kResourceType_Victor, GetChannel());
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ * @param syncGroup Unused interface.
+ */
+void Victor::Set(float speed, uint8_t syncGroup) {
+ SetSpeed(m_isInverted ? -speed : speed);
+}
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+float Victor::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Victor::Disable() { SetRaw(kPwmDisabled); }
+/**
+* Common interface for inverting direction of a speed controller.
+* @param isInverted The state of inversion, true is inverted.
+*/
+void Victor::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+/**
+ * Common interface for the inverting direction of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ *
+ */
+bool Victor::GetInverted() const { return m_isInverted; }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Victor::PIDWrite(float output) { Set(output); }
diff --git a/wpilibc/Athena/src/VictorSP.cpp b/wpilibc/Athena/src/VictorSP.cpp
new file mode 100644
index 0000000..ccbc163
--- /dev/null
+++ b/wpilibc/Athena/src/VictorSP.cpp
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "VictorSP.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * 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"
+ */
+
+/**
+ * Constructor for a VictorSP
+ * @param channel The PWM channel that the VictorSP is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+VictorSP::VictorSP(uint32_t channel) : SafePWM(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetRaw(m_centerPwm);
+ SetZeroLatch();
+
+ HALReport(HALUsageReporting::kResourceType_VictorSP, GetChannel());
+ LiveWindow::GetInstance()->AddActuator("VictorSP", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ * @param syncGroup Unused interface.
+ */
+void VictorSP::Set(float speed, uint8_t syncGroup) {
+ SetSpeed(m_isInverted ? -speed : speed);
+}
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+float VictorSP::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for inverting direction of a speed controller.
+ * @param isInverted The state of inversion, true is inverted.
+ */
+void VictorSP::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+/**
+ * Common interface for the inverting direction of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ *
+ */
+bool VictorSP::GetInverted() const { return m_isInverted; }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void VictorSP::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void VictorSP::PIDWrite(float output) { Set(output); }
diff --git a/wpilibc/Athena/src/Vision/AxisCamera.cpp b/wpilibc/Athena/src/Vision/AxisCamera.cpp
new file mode 100644
index 0000000..990c68a
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/AxisCamera.cpp
@@ -0,0 +1,597 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Vision/AxisCamera.h"
+
+#include "WPIErrors.h"
+
+#include <cstring>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <unistd.h>
+#include <netdb.h>
+#include <Timer.h>
+#include <iostream>
+#include <sstream>
+
+static const unsigned int kMaxPacketSize = 1536;
+static const unsigned int kImageBufferAllocationIncrement = 1000;
+
+static const std::string kWhiteBalanceStrings[] = {
+ "auto", "hold", "fixed_outdoor1", "fixed_outdoor2",
+ "fixed_indoor", "fixed_fluor1", "fixed_fluor2",
+};
+
+static const std::string kExposureControlStrings[] = {
+ "auto", "hold", "flickerfree50", "flickerfree60",
+};
+
+static const std::string kResolutionStrings[] = {
+ "640x480", "480x360", "320x240", "240x180", "176x144", "160x120",
+};
+
+static const std::string kRotationStrings[] = {
+ "0", "180",
+};
+
+/**
+ * AxisCamera constructor
+ * @param cameraHost The host to find the camera at, typically an IP address
+ */
+AxisCamera::AxisCamera(std::string const &cameraHost)
+ : m_cameraHost(cameraHost) {
+ m_captureThread = std::thread(&AxisCamera::Capture, this);
+}
+
+AxisCamera::~AxisCamera() {
+ m_done = true;
+ m_captureThread.join();
+}
+
+/*
+ * Return true if the latest image from the camera has not been retrieved by
+ * calling GetImage() yet.
+ * @return true if the image has not been retrieved yet.
+ */
+bool AxisCamera::IsFreshImage() const { return m_freshImage; }
+
+/**
+ * Get an image from the camera and store it in the provided image.
+ * @param image The imaq image to store the result in. This must be an HSL or
+ * RGB image.
+ * @return 1 upon success, zero on a failure
+ */
+int AxisCamera::GetImage(Image *image) {
+ if (m_imageData.size() == 0) {
+ return 0;
+ }
+
+ std::lock_guard<priority_mutex> lock(m_imageDataMutex);
+
+ Priv_ReadJPEGString_C(image, m_imageData.data(), m_imageData.size());
+
+ m_freshImage = false;
+
+ return 1;
+}
+
+/**
+ * Get an image from the camera and store it in the provided image.
+ * @param image The image to store the result in. This must be an HSL or RGB
+ * image
+ * @return 1 upon success, zero on a failure
+ */
+int AxisCamera::GetImage(ColorImage *image) {
+ return GetImage(image->GetImaqImage());
+}
+
+/**
+ * Instantiate a new image object and fill it with the latest image from the
+ * camera.
+ *
+ * The returned pointer is owned by the caller and is their responsibility to
+ * delete.
+ * @return a pointer to an HSLImage object
+ */
+HSLImage *AxisCamera::GetImage() {
+ auto image = new HSLImage();
+ GetImage(image);
+ return image;
+}
+
+/**
+ * Copy an image into an existing buffer.
+ * This copies an image into an existing buffer rather than creating a new image
+ * in memory. That way a new image is only allocated when the image being copied
+ * is
+ * larger than the destination.
+ * This method is called by the PCVideoServer class.
+ * @param imageData The destination image.
+ * @param numBytes The size of the destination image.
+ * @return 0 if failed (no source image or no memory), 1 if success.
+ */
+int AxisCamera::CopyJPEG(char **destImage, unsigned int &destImageSize,
+ unsigned int &destImageBufferSize) {
+ std::lock_guard<priority_mutex> lock(m_imageDataMutex);
+ if (destImage == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "destImage must not be nullptr");
+ return 0;
+ }
+
+ if (m_imageData.size() == 0) return 0; // if no source image
+
+ if (destImageBufferSize <
+ m_imageData.size()) // if current destination buffer too small
+ {
+ if (*destImage != nullptr) delete[] * destImage;
+ destImageBufferSize = m_imageData.size() + kImageBufferAllocationIncrement;
+ *destImage = new char[destImageBufferSize];
+ if (*destImage == nullptr) return 0;
+ }
+ // copy this image into destination buffer
+ if (*destImage == nullptr) {
+ wpi_setWPIErrorWithContext(NullParameter, "*destImage must not be nullptr");
+ }
+
+ std::copy(m_imageData.begin(), m_imageData.end(), *destImage);
+ destImageSize = m_imageData.size();
+ ;
+ return 1;
+}
+
+/**
+ * Request a change in the brightness of the camera images.
+ * @param brightness valid values 0 .. 100
+ */
+void AxisCamera::WriteBrightness(int brightness) {
+ if (brightness < 0 || brightness > 100) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ "Brightness must be from 0 to 100");
+ return;
+ }
+
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_brightness != brightness) {
+ m_brightness = brightness;
+ m_parametersDirty = true;
+ }
+}
+
+/**
+ * @return The configured brightness of the camera images
+ */
+int AxisCamera::GetBrightness() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_brightness;
+}
+
+/**
+ * Request a change in the white balance on the camera.
+ * @param whiteBalance Valid values from the <code>WhiteBalance</code> enum.
+ */
+void AxisCamera::WriteWhiteBalance(AxisCamera::WhiteBalance whiteBalance) {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_whiteBalance != whiteBalance) {
+ m_whiteBalance = whiteBalance;
+ m_parametersDirty = true;
+ }
+}
+
+/**
+ * @return The configured white balances of the camera images
+ */
+AxisCamera::WhiteBalance AxisCamera::GetWhiteBalance() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_whiteBalance;
+}
+
+/**
+ * Request a change in the color level of the camera images.
+ * @param colorLevel valid values are 0 .. 100
+ */
+void AxisCamera::WriteColorLevel(int colorLevel) {
+ if (colorLevel < 0 || colorLevel > 100) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ "Color level must be from 0 to 100");
+ return;
+ }
+
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_colorLevel != colorLevel) {
+ m_colorLevel = colorLevel;
+ m_parametersDirty = true;
+ }
+}
+
+/**
+ * @return The configured color level of the camera images
+ */
+int AxisCamera::GetColorLevel() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_colorLevel;
+}
+
+/**
+ * Request a change in the camera's exposure mode.
+ * @param exposureControl A mode to write in the <code>Exposure</code> enum.
+ */
+void AxisCamera::WriteExposureControl(
+ AxisCamera::ExposureControl exposureControl) {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_exposureControl != exposureControl) {
+ m_exposureControl = exposureControl;
+ m_parametersDirty = true;
+ }
+}
+
+/**
+ * @return The configured exposure control mode of the camera
+ */
+AxisCamera::ExposureControl AxisCamera::GetExposureControl() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_exposureControl;
+}
+
+/**
+ * Request a change in the exposure priority of the camera.
+ * @param exposurePriority Valid values are 0, 50, 100.
+ * 0 = Prioritize image quality
+ * 50 = None
+ * 100 = Prioritize frame rate
+ */
+void AxisCamera::WriteExposurePriority(int exposurePriority) {
+ if (exposurePriority != 0 && exposurePriority != 50 &&
+ exposurePriority != 100) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ "Exposure priority must be from 0, 50, or 100");
+ return;
+ }
+
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_exposurePriority != exposurePriority) {
+ m_exposurePriority = exposurePriority;
+ m_parametersDirty = true;
+ }
+}
+
+/**
+ * @return The configured exposure priority of the camera
+ */
+int AxisCamera::GetExposurePriority() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_exposurePriority;
+}
+
+/**
+ * Write the maximum frames per second that the camera should send
+ * Write 0 to send as many as possible.
+ * @param maxFPS The number of frames the camera should send in a second,
+ * exposure permitting.
+ */
+void AxisCamera::WriteMaxFPS(int maxFPS) {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_maxFPS != maxFPS) {
+ m_maxFPS = maxFPS;
+ m_parametersDirty = true;
+ m_streamDirty = true;
+ }
+}
+
+/**
+ * @return The configured maximum FPS of the camera
+ */
+int AxisCamera::GetMaxFPS() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_maxFPS;
+}
+
+/**
+ * Write resolution value to camera.
+ * @param resolution The camera resolution value to write to the camera.
+ */
+void AxisCamera::WriteResolution(AxisCamera::Resolution resolution) {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_resolution != resolution) {
+ m_resolution = resolution;
+ m_parametersDirty = true;
+ m_streamDirty = true;
+ }
+}
+
+/**
+ * @return The configured resolution of the camera (not necessarily the same
+ * resolution as the most recent image, if it was changed recently.)
+ */
+AxisCamera::Resolution AxisCamera::GetResolution() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_resolution;
+}
+
+/**
+ * Write the rotation value to the camera.
+ * If you mount your camera upside down, use this to adjust the image for you.
+ * @param rotation The angle to rotate the camera
+ * (<code>AxisCamera::Rotation::k0</code>
+ * or <code>AxisCamera::Rotation::k180</code>)
+ */
+void AxisCamera::WriteRotation(AxisCamera::Rotation rotation) {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_rotation != rotation) {
+ m_rotation = rotation;
+ m_parametersDirty = true;
+ m_streamDirty = true;
+ }
+}
+
+/**
+ * @return The configured rotation mode of the camera
+ */
+AxisCamera::Rotation AxisCamera::GetRotation() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_rotation;
+}
+
+/**
+ * Write the compression value to the camera.
+ * @param compression Values between 0 and 100.
+ */
+void AxisCamera::WriteCompression(int compression) {
+ if (compression < 0 || compression > 100) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange,
+ "Compression must be from 0 to 100");
+ return;
+ }
+
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+
+ if (m_compression != compression) {
+ m_compression = compression;
+ m_parametersDirty = true;
+ m_streamDirty = true;
+ }
+}
+
+/**
+ * @return The configured compression level of the camera
+ */
+int AxisCamera::GetCompression() {
+ std::lock_guard<priority_mutex> lock(m_parametersMutex);
+ return m_compression;
+}
+
+/**
+ * Method called in the capture thread to receive images from the camera
+ */
+void AxisCamera::Capture() {
+ int consecutiveErrors = 0;
+
+ // Loop on trying to setup the camera connection. This happens in a background
+ // thread so it shouldn't effect the operation of user programs.
+ while (!m_done) {
+ std::string requestString =
+ "GET /mjpg/video.mjpg HTTP/1.1\n"
+ "User-Agent: HTTPStreamClient\n"
+ "Connection: Keep-Alive\n"
+ "Cache-Control: no-cache\n"
+ "Authorization: Basic RlJDOkZSQw==\n\n";
+ m_captureMutex.lock();
+ m_cameraSocket = CreateCameraSocket(requestString, consecutiveErrors > 5);
+ if (m_cameraSocket != -1) {
+ ReadImagesFromCamera();
+ consecutiveErrors = 0;
+ } else {
+ consecutiveErrors++;
+ }
+ m_captureMutex.unlock();
+ Wait(0.5);
+ }
+}
+
+/**
+ * This function actually reads the images from the camera.
+ */
+void AxisCamera::ReadImagesFromCamera() {
+ char *imgBuffer = nullptr;
+ int imgBufferLength = 0;
+
+ // TODO: these recv calls must be non-blocking. Otherwise if the camera
+ // fails during a read, the code hangs and never retries when the camera comes
+ // back up.
+
+ int counter = 2;
+ while (!m_done) {
+ char initialReadBuffer[kMaxPacketSize] = "";
+ char intermediateBuffer[1];
+ char *trailingPtr = initialReadBuffer;
+ int trailingCounter = 0;
+ while (counter) {
+ // TODO: fix me... this cannot be the most efficient way to approach this,
+ // reading one byte at a time.
+ if (recv(m_cameraSocket, intermediateBuffer, 1, 0) == -1) {
+ wpi_setErrnoErrorWithContext("Failed to read image header");
+ close(m_cameraSocket);
+ return;
+ }
+ strncat(initialReadBuffer, intermediateBuffer, 1);
+ // trailingCounter ensures that we start looking for the 4 byte string
+ // after
+ // there is at least 4 bytes total. Kind of obscure.
+ // look for 2 blank lines (\r\n)
+ if (nullptr != strstr(trailingPtr, "\r\n\r\n")) {
+ --counter;
+ }
+ if (++trailingCounter >= 4) {
+ trailingPtr++;
+ }
+ }
+ counter = 1;
+ char *contentLength = strstr(initialReadBuffer, "Content-Length: ");
+ if (contentLength == nullptr) {
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "No content-length token found in packet");
+ close(m_cameraSocket);
+ if (imgBuffer) delete[] imgBuffer;
+ return;
+ }
+ contentLength = contentLength + 16; // skip past "content length"
+ int readLength = atol(contentLength); // get the image byte count
+
+ // Make sure buffer is large enough
+ if (imgBufferLength < readLength) {
+ if (imgBuffer) delete[] imgBuffer;
+ imgBufferLength = readLength + kImageBufferAllocationIncrement;
+ imgBuffer = new char[imgBufferLength];
+ if (imgBuffer == nullptr) {
+ imgBufferLength = 0;
+ continue;
+ }
+ }
+
+ // Read the image data for "Content-Length" bytes
+ int bytesRead = 0;
+ int remaining = readLength;
+ while (bytesRead < readLength) {
+ int bytesThisRecv =
+ recv(m_cameraSocket, &imgBuffer[bytesRead], remaining, 0);
+ bytesRead += bytesThisRecv;
+ remaining -= bytesThisRecv;
+ }
+
+ // Update image
+ {
+ std::lock_guard<priority_mutex> lock(m_imageDataMutex);
+
+ m_imageData.assign(imgBuffer, imgBuffer + imgBufferLength);
+ m_freshImage = true;
+ }
+
+ if (WriteParameters()) {
+ break;
+ }
+ }
+
+ close(m_cameraSocket);
+}
+
+/**
+ * Send a request to the camera to set all of the parameters. This is called
+ * in the capture thread between each frame. This strategy avoids making lots
+ * of redundant HTTP requests, accounts for failed initial requests, and
+ * avoids blocking calls in the main thread unless necessary.
+ *
+ * This method does nothing if no parameters have been modified since it last
+ * completely successfully.
+ *
+ * @return <code>true</code> if the stream should be restarted due to a
+ * parameter changing.
+ */
+bool AxisCamera::WriteParameters() {
+ if (m_parametersDirty) {
+ std::stringstream request;
+ request << "GET /axis-cgi/admin/param.cgi?action=update";
+
+ m_parametersMutex.lock();
+ request << "&ImageSource.I0.Sensor.Brightness=" << m_brightness;
+ request << "&ImageSource.I0.Sensor.WhiteBalance="
+ << kWhiteBalanceStrings[m_whiteBalance];
+ request << "&ImageSource.I0.Sensor.ColorLevel=" << m_colorLevel;
+ request << "&ImageSource.I0.Sensor.Exposure="
+ << kExposureControlStrings[m_exposureControl];
+ request << "&ImageSource.I0.Sensor.ExposurePriority=" << m_exposurePriority;
+ request << "&Image.I0.Stream.FPS=" << m_maxFPS;
+ request << "&Image.I0.Appearance.Resolution="
+ << kResolutionStrings[m_resolution];
+ request << "&Image.I0.Appearance.Compression=" << m_compression;
+ request << "&Image.I0.Appearance.Rotation=" << kRotationStrings[m_rotation];
+ m_parametersMutex.unlock();
+
+ request << " HTTP/1.1" << std::endl;
+ request << "User-Agent: HTTPStreamClient" << std::endl;
+ request << "Connection: Keep-Alive" << std::endl;
+ request << "Cache-Control: no-cache" << std::endl;
+ request << "Authorization: Basic RlJDOkZSQw==" << std::endl;
+ request << std::endl;
+
+ int socket = CreateCameraSocket(request.str(), false);
+ if (socket == -1) {
+ wpi_setErrnoErrorWithContext("Error setting camera parameters");
+ } else {
+ close(socket);
+ m_parametersDirty = false;
+
+ if (m_streamDirty) {
+ m_streamDirty = false;
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+/**
+ * Create a socket connected to camera
+ * Used to create a connection to the camera for both capturing images and
+ * setting parameters.
+ * @param requestString The initial request string to send upon successful
+ * connection.
+ * @param setError If true, rais an error if there's a problem creating the
+ * connection.
+ * This is only enabled after several unsucessful connections, so a single one
+ * doesn't
+ * cause an error message to be printed if it immediately recovers.
+ * @return -1 if failed, socket handle if successful.
+ */
+int AxisCamera::CreateCameraSocket(std::string const &requestString,
+ bool setError) {
+ struct addrinfo *address = nullptr;
+ int camSocket;
+
+ /* create socket */
+ if ((camSocket = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
+ if (setError)
+ wpi_setErrnoErrorWithContext("Failed to create the camera socket");
+ return -1;
+ }
+
+ if (getaddrinfo(m_cameraHost.c_str(), "80", nullptr, &address) == -1) {
+ if (setError) {
+ wpi_setErrnoErrorWithContext("Failed to create the camera socket");
+ close(camSocket);
+ }
+ return -1;
+ }
+
+ /* connect to server */
+ if (connect(camSocket, address->ai_addr, address->ai_addrlen) == -1) {
+ if (setError)
+ wpi_setErrnoErrorWithContext("Failed to connect to the camera");
+ freeaddrinfo(address);
+ close(camSocket);
+ return -1;
+ }
+
+ freeaddrinfo(address);
+
+ int sent = send(camSocket, requestString.c_str(), requestString.size(), 0);
+ if (sent == -1) {
+ if (setError)
+ wpi_setErrnoErrorWithContext("Failed to send a request to the camera");
+ close(camSocket);
+ return -1;
+ }
+
+ return camSocket;
+}
diff --git a/wpilibc/Athena/src/Vision/BaeUtilities.cpp b/wpilibc/Athena/src/Vision/BaeUtilities.cpp
new file mode 100644
index 0000000..f09ec64
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/BaeUtilities.cpp
@@ -0,0 +1,369 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <string.h>
+#include <math.h>
+#include <stdlib.h>
+#include <stdarg.h>
+
+#include "Vision/BaeUtilities.h"
+#include "Servo.h"
+#include "Timer.h"
+
+/** @file
+ * Utility functions
+ */
+
+/**
+ * debug output flag options:
+ * DEBUG_OFF, DEBUG_MOSTLY_OFF, DEBUG_SCREEN_ONLY, DEBUG_FILE_ONLY,
+ * DEBUG_SCREEN_AND_FILE
+ */
+static DebugOutputType dprintfFlag = DEBUG_OFF;
+
+/**
+ * Set the debug flag to print to screen, file on cRIO, both or neither
+ * @param tempString The format string.
+ */
+void SetDebugFlag(DebugOutputType flag) { dprintfFlag = flag; }
+
+/**
+ * Debug print to a file and/or a terminal window.
+ * Call like you would call printf.
+ * Set functionName in the function if you want the correct function name to
+ * print out.
+ * The file line number will also be printed.
+ * @param tempString The format string.
+ */
+void dprintf(const char *tempString, ...) /* Variable argument list */
+{
+ va_list args; /* Input argument list */
+ int line_number; /* Line number passed in argument */
+ int type;
+ const char *functionName; /* Format passed in argument */
+ const char *fmt; /* Format passed in argument */
+ char text[512]; /* Text string */
+ char outtext[512]; /* Text string */
+ FILE *outfile_fd; /* Output file pointer */
+ char filepath[128]; /* Text string */
+ int fatalFlag = 0;
+ const char *filename;
+ int index;
+ int tempStringLen;
+
+ if (dprintfFlag == DEBUG_OFF) {
+ return;
+ }
+
+ va_start(args, tempString);
+
+ tempStringLen = strlen(tempString);
+ filename = tempString;
+ for (index = 0; index < tempStringLen; index++) {
+ if (tempString[index] == ' ') {
+ printf("ERROR in dprintf: malformed calling sequence (%s)\n", tempString);
+ va_end(args);
+ return;
+ }
+ if (tempString[index] == '\\' || tempString[index] == '/')
+ filename = tempString + index + 1;
+ }
+
+ /* Extract function name */
+ functionName = va_arg(args, const char *);
+
+ /* Extract line number from argument list */
+ line_number = va_arg(args, int);
+
+ /* Extract information type from argument list */
+ type = va_arg(args, int);
+
+ /* Extract format from argument list */
+ fmt = va_arg(args, const char *);
+
+ vsprintf(text, fmt, args);
+
+ va_end(args);
+
+ /* Format output statement */
+ switch (type) {
+ case DEBUG_TYPE:
+ sprintf(outtext, "[%s:%s@%04d] DEBUG %s\n", filename, functionName,
+ line_number, text);
+ break;
+ case INFO_TYPE:
+ sprintf(outtext, "[%s:%s@%04d] INFO %s\n", filename, functionName,
+ line_number, text);
+ break;
+ case ERROR_TYPE:
+ sprintf(outtext, "[%s:%s@%04d] ERROR %s\n", filename, functionName,
+ line_number, text);
+ break;
+ case CRITICAL_TYPE:
+ sprintf(outtext, "[%s:%s@%04d] CRITICAL %s\n", filename, functionName,
+ line_number, text);
+ break;
+ case FATAL_TYPE:
+ fatalFlag = 1;
+ sprintf(outtext, "[%s:%s@%04d] FATAL %s\n", filename, functionName,
+ line_number, text);
+ break;
+ default:
+ printf("ERROR in dprintf: malformed calling sequence\n");
+ return;
+ break;
+ }
+
+ sprintf(filepath, "%s.debug", filename);
+
+ /* Write output statement */
+ switch (dprintfFlag) {
+ default:
+ case DEBUG_OFF:
+ break;
+ case DEBUG_MOSTLY_OFF:
+ if (fatalFlag) {
+ if ((outfile_fd = fopen(filepath, "a+")) != nullptr) {
+ fwrite(outtext, sizeof(char), strlen(outtext), outfile_fd);
+ fclose(outfile_fd);
+ }
+ }
+ break;
+ case DEBUG_SCREEN_ONLY:
+ printf("%s", outtext);
+ break;
+ case DEBUG_FILE_ONLY:
+ if ((outfile_fd = fopen(filepath, "a+")) != nullptr) {
+ fwrite(outtext, sizeof(char), strlen(outtext), outfile_fd);
+ fclose(outfile_fd);
+ }
+ break;
+ case DEBUG_SCREEN_AND_FILE: // BOTH
+ printf("%s", outtext);
+ if ((outfile_fd = fopen(filepath, "a+")) != nullptr) {
+ fwrite(outtext, sizeof(char), strlen(outtext), outfile_fd);
+ fclose(outfile_fd);
+ }
+ break;
+ }
+}
+
+/**
+ * @brief Normalizes a value in a range, used for drive input
+ * @param position The position in the range, starting at 0
+ * @param range The size of the range that position is in
+ * @return The normalized position from -1 to +1
+ */
+double RangeToNormalized(double position, int range) {
+ return (((position * 2.0) / (double)range) - 1.0);
+}
+
+/**
+ * @brief Convert a normalized value to the corresponding value in a range.
+ * This is used to convert normalized values to the servo command range.
+ * @param normalizedValue The normalized value (in the -1 to +1 range)
+ * @param minRange The minimum of the range (0 is default)
+ * @param maxRange The maximum of the range (1 is default)
+ * @return The value in the range corresponding to the input normalized value
+ */
+float NormalizeToRange(float normalizedValue, float minRange, float maxRange) {
+ float range = maxRange - minRange;
+ float temp = (float)((normalizedValue / 2.0) + 0.5) * range;
+ return (temp + minRange);
+}
+float NormalizeToRange(float normalizedValue) {
+ return (float)((normalizedValue / 2.0) + 0.5);
+}
+
+/**
+ * @brief Displays an activity indicator to console.
+ * Call this function like you would call printf.
+ * @param fmt The format string
+*/
+void ShowActivity(char *fmt, ...) {
+ static char activity_indication_string[] = "|/-\\";
+ static int ai = 3;
+ va_list args;
+ char text[1024];
+
+ va_start(args, fmt);
+
+ vsprintf(text, fmt, args);
+
+ ai = ai == 3 ? 0 : ai + 1;
+
+ printf("%c %s \r", activity_indication_string[ai], text);
+ fflush(stdout);
+
+ va_end(args);
+}
+
+#define PI 3.14159265358979
+/**
+ * @brief Calculate sine wave increments (-1.0 to 1.0).
+ * The first time this is called, it sets up the time increment. Subsequent
+ * calls
+ * will give values along the sine wave depending on current time. If the wave
+ * is
+ * stopped and restarted, it must be reinitialized with a new "first call".
+ *
+ * @param period length of time to complete a complete wave
+ * @param sinStart Where to start the sine wave (0.0 = 2 pi, pi/2 = 1.0, etc.)
+ */
+double SinPosition(double *period, double sinStart) {
+ double rtnVal;
+ static double sinePeriod = 0.0;
+ static double timestamp;
+ double sinArg;
+
+ // 1st call
+ if (period != nullptr) {
+ sinePeriod = *period;
+ timestamp = GetTime();
+ return 0.0;
+ }
+
+ // Multiplying by 2*pi to the time difference makes sinePeriod work if it's
+ // measured in seconds.
+ // Adding sinStart to the part multiplied by PI, but not by 2, allows it to
+ // work as described in the comments.
+ sinArg = PI * ((2.0 * (GetTime() - timestamp)) + sinStart) / sinePeriod;
+ rtnVal = sin(sinArg);
+ return (rtnVal);
+}
+
+/**
+ * @brief Find the elapsed time since a specified time.
+ * @param startTime The starting time
+ * @return How long it has been since the starting time
+ */
+double ElapsedTime(double startTime) {
+ double realTime = GetTime();
+ return (realTime - startTime);
+}
+
+/**
+ * @brief Initialize pan parameters
+ * @param period The number of seconds to complete one pan
+ */
+void panInit() {
+ double period = 3.0; // number of seconds for one complete pan
+ SinPosition(&period, 0.0); // initial call to set up time
+}
+
+void panInit(double period) {
+ if (period < 0.0) period = 3.0;
+ SinPosition(&period, 0.0); // initial call to set up time
+}
+
+/**
+ * @brief Move the horizontal servo back and forth.
+ * @param panServo The servo object to move
+ * @param sinStart The position on the sine wave to begin the pan
+ */
+void panForTarget(Servo *panServo) { panForTarget(panServo, 0.0); }
+
+void panForTarget(Servo *panServo, double sinStart) {
+ float normalizedSinPosition = (float)SinPosition(nullptr, sinStart);
+ float newServoPosition = NormalizeToRange(normalizedSinPosition);
+ panServo->Set(newServoPosition);
+ // ShowActivity ("pan x: normalized %f newServoPosition = %f" ,
+ // normalizedSinPosition, newServoPosition );
+}
+
+/** @brief Read a file and return non-comment output string
+
+Call the first time with 0 lineNumber to get the number of lines to read
+Then call with each lineNumber to get one camera parameter. There should
+be one property=value entry on each line, i.e. "exposure=auto"
+
+ * @param inputFile filename to read
+ * @param outputString one string
+ * @param lineNumber if 0, return number of lines; else return that line number
+ * @return int number of lines or -1 if error
+ **/
+int processFile(char *inputFile, char *outputString, int lineNumber) {
+ FILE *infile;
+ const int stringSize = 80; // max size of one line in file
+ char inputStr[stringSize];
+ inputStr[0] = '\0';
+ int lineCount = 0;
+
+ if (lineNumber < 0) return (-1);
+
+ if ((infile = fopen(inputFile, "r")) == nullptr) {
+ printf("Fatal error opening file %s\n", inputFile);
+ return (0);
+ }
+
+ while (!feof(infile)) {
+ if (fgets(inputStr, stringSize, infile) != nullptr) {
+ // Skip empty lines
+ if (emptyString(inputStr)) continue;
+ // Skip comment lines
+ if (inputStr[0] == '#' || inputStr[0] == '!') continue;
+
+ lineCount++;
+ if (lineNumber == 0)
+ continue;
+ else {
+ if (lineCount == lineNumber) break;
+ }
+ }
+ }
+
+ // close file
+ fclose(infile);
+ // if number lines requested return the count
+ if (lineNumber == 0) return (lineCount);
+ // check for input out of range
+ if (lineNumber > lineCount) return (-1);
+ // return the line selected; lineCount guaranteed to be greater than zero
+ stripString(inputStr);
+ strcpy(outputString, inputStr);
+ return (lineCount);
+}
+
+/** Ignore empty string
+ * @param string to check if empty
+ **/
+int emptyString(char *string) {
+ int i, len;
+
+ if (string == nullptr) return (1);
+
+ len = strlen(string);
+ for (i = 0; i < len; i++) {
+ // Ignore the following characters
+ if (string[i] == '\n' || string[i] == '\r' || string[i] == '\t' ||
+ string[i] == ' ')
+ continue;
+ return (0);
+ }
+ return (1);
+}
+
+/** Remove special characters from string
+ * @param string to process
+ **/
+void stripString(char *string) {
+ int i, j, len;
+
+ if (string == nullptr) return;
+
+ len = strlen(string);
+ for (i = 0, j = 0; i < len; i++) {
+ // Remove the following characters from the string
+ if (string[i] == '\n' || string[i] == '\r' || string[i] == '\"') continue;
+ // Copy anything else
+ string[j++] = string[i];
+ }
+ string[j] = '\0';
+}
diff --git a/wpilibc/Athena/src/Vision/BinaryImage.cpp b/wpilibc/Athena/src/Vision/BinaryImage.cpp
new file mode 100644
index 0000000..ea14cbc
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/BinaryImage.cpp
@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Vision/BinaryImage.h"
+#include "WPIErrors.h"
+#include <cstring>
+
+using namespace std;
+
+/**
+ * Get then number of particles for the image.
+ * @returns the number of particles found for the image.
+ */
+int BinaryImage::GetNumberParticles() {
+ int numParticles = 0;
+ int success = imaqCountParticles(m_imaqImage, 1, &numParticles);
+ wpi_setImaqErrorWithContext(success, "Error counting particles");
+ return numParticles;
+}
+
+/**
+ * Get a single particle analysis report.
+ * Get one (of possibly many) particle analysis reports for an image.
+ * @param particleNumber Which particle analysis report to return.
+ * @returns the selected particle analysis report
+ */
+ParticleAnalysisReport BinaryImage::GetParticleAnalysisReport(
+ int particleNumber) {
+ ParticleAnalysisReport par;
+ GetParticleAnalysisReport(particleNumber, &par);
+ return par;
+}
+
+/**
+ * Get a single particle analysis report.
+ * Get one (of possibly many) particle analysis reports for an image.
+ * This version could be more efficient when copying many reports.
+ * @param particleNumber Which particle analysis report to return.
+ * @param par the selected particle analysis report
+ */
+void BinaryImage::GetParticleAnalysisReport(int particleNumber,
+ ParticleAnalysisReport *par) {
+ int success;
+ int numParticles = 0;
+
+ success = imaqGetImageSize(m_imaqImage, &par->imageWidth, &par->imageHeight);
+ wpi_setImaqErrorWithContext(success, "Error getting image size");
+ if (StatusIsFatal()) return;
+
+ success = imaqCountParticles(m_imaqImage, 1, &numParticles);
+ wpi_setImaqErrorWithContext(success, "Error counting particles");
+ if (StatusIsFatal()) return;
+
+ if (particleNumber >= numParticles) {
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "particleNumber");
+ return;
+ }
+
+ par->particleIndex = particleNumber;
+ // Don't bother measuring the rest of the particle if one fails
+ bool good = ParticleMeasurement(particleNumber, IMAQ_MT_CENTER_OF_MASS_X,
+ &par->center_mass_x);
+ good = good && ParticleMeasurement(particleNumber, IMAQ_MT_CENTER_OF_MASS_Y,
+ &par->center_mass_y);
+ good = good &&
+ ParticleMeasurement(particleNumber, IMAQ_MT_AREA, &par->particleArea);
+ good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_TOP,
+ &par->boundingRect.top);
+ good = good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_LEFT,
+ &par->boundingRect.left);
+ good =
+ good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_HEIGHT,
+ &par->boundingRect.height);
+ good =
+ good && ParticleMeasurement(particleNumber, IMAQ_MT_BOUNDING_RECT_WIDTH,
+ &par->boundingRect.width);
+ good = good && ParticleMeasurement(particleNumber, IMAQ_MT_AREA_BY_IMAGE_AREA,
+ &par->particleToImagePercent);
+ good = good && ParticleMeasurement(particleNumber,
+ IMAQ_MT_AREA_BY_PARTICLE_AND_HOLES_AREA,
+ &par->particleQuality);
+
+ if (good) {
+ /* normalized position (-1 to 1) */
+ par->center_mass_x_normalized =
+ NormalizeFromRange(par->center_mass_x, par->imageWidth);
+ par->center_mass_y_normalized =
+ NormalizeFromRange(par->center_mass_y, par->imageHeight);
+ }
+}
+
+/**
+ * Get an ordered vector of particles for the image.
+ * Create a vector of particle analysis reports sorted by size for an image.
+ * The vector contains the actual report structures.
+ * @returns a pointer to the vector of particle analysis reports. The caller
+ * must delete the
+ * vector when finished using it.
+ */
+vector<ParticleAnalysisReport> *
+BinaryImage::GetOrderedParticleAnalysisReports() {
+ auto particles = new vector<ParticleAnalysisReport>;
+ int particleCount = GetNumberParticles();
+ for (int particleIndex = 0; particleIndex < particleCount; particleIndex++) {
+ particles->push_back(GetParticleAnalysisReport(particleIndex));
+ }
+ // TODO: This is pretty inefficient since each compare in the sort copies
+ // both reports being compared... do it manually instead... while we're
+ // at it, we should provide a version that allows a preallocated buffer of
+ // ParticleAnalysisReport structures
+ sort(particles->begin(), particles->end(), CompareParticleSizes);
+ return particles;
+}
+
+/**
+ * Write a binary image to flash.
+ * Writes the binary image to flash on the cRIO for later inspection.
+ * @param fileName the name of the image file written to the flash.
+ */
+void BinaryImage::Write(const char *fileName) {
+ RGBValue colorTable[256];
+ memset(colorTable, 0, sizeof(colorTable));
+ colorTable[0].R = 0;
+ colorTable[1].R = 255;
+ colorTable[0].G = colorTable[1].G = 0;
+ colorTable[0].B = colorTable[1].B = 0;
+ colorTable[0].alpha = colorTable[1].alpha = 0;
+ imaqWriteFile(m_imaqImage, fileName, colorTable);
+}
+
+/**
+ * Measure a single parameter for an image.
+ * Get the measurement for a single parameter about an image by calling the
+ * imaqMeasureParticle
+ * function for the selected parameter.
+ * @param particleNumber which particle in the set of particles
+ * @param whatToMeasure the imaq MeasurementType (what to measure)
+ * @param result the value of the measurement
+ * @returns false on failure, true on success
+ */
+bool BinaryImage::ParticleMeasurement(int particleNumber,
+ MeasurementType whatToMeasure,
+ int *result) {
+ double resultDouble;
+ bool success =
+ ParticleMeasurement(particleNumber, whatToMeasure, &resultDouble);
+ *result = (int)resultDouble;
+ return success;
+}
+
+/**
+ * Measure a single parameter for an image.
+ * Get the measurement for a single parameter about an image by calling the
+ * imaqMeasureParticle
+ * function for the selected parameter.
+ * @param particleNumber which particle in the set of particles
+ * @param whatToMeasure the imaq MeasurementType (what to measure)
+ * @param result the value of the measurement
+ * @returns true on failure, false on success
+ */
+bool BinaryImage::ParticleMeasurement(int particleNumber,
+ MeasurementType whatToMeasure,
+ double *result) {
+ int success;
+ success = imaqMeasureParticle(m_imaqImage, particleNumber, 0, whatToMeasure,
+ result);
+ wpi_setImaqErrorWithContext(success, "Error measuring particle");
+ return !StatusIsFatal();
+}
+
+// Normalizes to [-1,1]
+double BinaryImage::NormalizeFromRange(double position, int range) {
+ return (position * 2.0 / (double)range) - 1.0;
+}
+
+/**
+ * The compare helper function for sort.
+ * This function compares two particle analysis reports as a helper for the sort
+ * function.
+ * @param particle1 The first particle to compare
+ * @param particle2 the second particle to compare
+ * @returns true if particle1 is greater than particle2
+ */
+bool BinaryImage::CompareParticleSizes(ParticleAnalysisReport particle1,
+ ParticleAnalysisReport particle2) {
+ // we want descending sort order
+ return particle1.particleToImagePercent > particle2.particleToImagePercent;
+}
+
+BinaryImage *BinaryImage::RemoveSmallObjects(bool connectivity8, int erosions) {
+ auto result = new BinaryImage();
+ int success = imaqSizeFilter(result->GetImaqImage(), m_imaqImage,
+ connectivity8, erosions, IMAQ_KEEP_LARGE, nullptr);
+ wpi_setImaqErrorWithContext(success, "Error in RemoveSmallObjects");
+ return result;
+}
+
+BinaryImage *BinaryImage::RemoveLargeObjects(bool connectivity8, int erosions) {
+ auto result = new BinaryImage();
+ int success = imaqSizeFilter(result->GetImaqImage(), m_imaqImage,
+ connectivity8, erosions, IMAQ_KEEP_SMALL, nullptr);
+ wpi_setImaqErrorWithContext(success, "Error in RemoveLargeObjects");
+ return result;
+}
+
+BinaryImage *BinaryImage::ConvexHull(bool connectivity8) {
+ auto result = new BinaryImage();
+ int success =
+ imaqConvexHull(result->GetImaqImage(), m_imaqImage, connectivity8);
+ wpi_setImaqErrorWithContext(success, "Error in convex hull operation");
+ return result;
+}
+
+BinaryImage *BinaryImage::ParticleFilter(ParticleFilterCriteria2 *criteria,
+ int criteriaCount) {
+ auto result = new BinaryImage();
+ int numParticles;
+ ParticleFilterOptions2 filterOptions = {0, 0, 0, 1};
+ int success =
+ imaqParticleFilter4(result->GetImaqImage(), m_imaqImage, criteria,
+ criteriaCount, &filterOptions, nullptr, &numParticles);
+ wpi_setImaqErrorWithContext(success, "Error in particle filter operation");
+ return result;
+}
diff --git a/wpilibc/Athena/src/Vision/ColorImage.cpp b/wpilibc/Athena/src/Vision/ColorImage.cpp
new file mode 100644
index 0000000..bbbc242
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/ColorImage.cpp
@@ -0,0 +1,444 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Vision/ColorImage.h"
+
+#include "WPIErrors.h"
+
+ColorImage::ColorImage(ImageType type) : ImageBase(type) {}
+
+/**
+ * Perform a threshold operation on a ColorImage.
+ * Perform a threshold operation on a ColorImage using the ColorMode supplied
+ * as a parameter.
+ * @param colorMode The type of colorspace this operation should be performed in
+ * @returns a pointer to a binary image
+ */
+BinaryImage *ColorImage::ComputeThreshold(ColorMode colorMode, int low1,
+ int high1, int low2, int high2,
+ int low3, int high3) {
+ auto result = new BinaryImage();
+ Range range1 = {low1, high1}, range2 = {low2, high2}, range3 = {low3, high3};
+
+ int success = imaqColorThreshold(result->GetImaqImage(), m_imaqImage, 1,
+ colorMode, &range1, &range2, &range3);
+ wpi_setImaqErrorWithContext(success, "ImaqThreshold error");
+ return result;
+}
+
+/**
+ * Perform a threshold in RGB space.
+ * @param redLow Red low value
+ * @param redHigh Red high value
+ * @param greenLow Green low value
+ * @param greenHigh Green high value
+ * @param blueLow Blue low value
+ * @param blueHigh Blue high value
+ * @returns A pointer to a BinaryImage that represents the result of the
+ * threshold operation.
+ */
+BinaryImage *ColorImage::ThresholdRGB(int redLow, int redHigh, int greenLow,
+ int greenHigh, int blueLow,
+ int blueHigh) {
+ return ComputeThreshold(IMAQ_RGB, redLow, redHigh, greenLow, greenHigh,
+ blueLow, blueHigh);
+}
+
+/**
+ * Perform a threshold in RGB space.
+ * @param threshold a reference to the Threshold object to use.
+ * @returns A pointer to a BinaryImage that represents the result of the
+ * threshold operation.
+ */
+BinaryImage *ColorImage::ThresholdRGB(Threshold &t) {
+ return ComputeThreshold(IMAQ_RGB, t.plane1Low, t.plane1High, t.plane2Low,
+ t.plane2High, t.plane3Low, t.plane3High);
+}
+
+/**
+ * Perform a threshold in HSL space.
+ * @param hueLow Low value for hue
+ * @param hueHigh High value for hue
+ * @param saturationLow Low value for saturation
+ * @param saturationHigh High value for saturation
+ * @param luminenceLow Low value for luminence
+ * @param luminenceHigh High value for luminence
+ * @returns a pointer to a BinaryImage that represents the result of the
+ * threshold operation.
+ */
+BinaryImage *ColorImage::ThresholdHSL(int hueLow, int hueHigh,
+ int saturationLow, int saturationHigh,
+ int luminenceLow, int luminenceHigh) {
+ return ComputeThreshold(IMAQ_HSL, hueLow, hueHigh, saturationLow,
+ saturationHigh, luminenceLow, luminenceHigh);
+}
+
+/**
+ * Perform a threshold in HSL space.
+ * @param threshold a reference to the Threshold object to use.
+ * @returns A pointer to a BinaryImage that represents the result of the
+ * threshold operation.
+ */
+BinaryImage *ColorImage::ThresholdHSL(Threshold &t) {
+ return ComputeThreshold(IMAQ_HSL, t.plane1Low, t.plane1High, t.plane2Low,
+ t.plane2High, t.plane3Low, t.plane3High);
+}
+
+/**
+ * Perform a threshold in HSV space.
+ * @param hueLow Low value for hue
+ * @param hueHigh High value for hue
+ * @param saturationLow Low value for saturation
+ * @param saturationHigh High value for saturation
+ * @param valueLow Low value
+ * @param valueHigh High value
+ * @returns a pointer to a BinaryImage that represents the result of the
+ * threshold operation.
+ */
+BinaryImage *ColorImage::ThresholdHSV(int hueLow, int hueHigh,
+ int saturationLow, int saturationHigh,
+ int valueLow, int valueHigh) {
+ return ComputeThreshold(IMAQ_HSV, hueLow, hueHigh, saturationLow,
+ saturationHigh, valueLow, valueHigh);
+}
+
+/**
+ * Perform a threshold in HSV space.
+ * @param threshold a reference to the Threshold object to use.
+ * @returns A pointer to a BinaryImage that represents the result of the
+ * threshold operation.
+ */
+BinaryImage *ColorImage::ThresholdHSV(Threshold &t) {
+ return ComputeThreshold(IMAQ_HSV, t.plane1Low, t.plane1High, t.plane2Low,
+ t.plane2High, t.plane3Low, t.plane3High);
+}
+
+/**
+ * Perform a threshold in HSI space.
+ * @param hueLow Low value for hue
+ * @param hueHigh High value for hue
+ * @param saturationLow Low value for saturation
+ * @param saturationHigh High value for saturation
+ * @param valueLow Low intensity
+ * @param valueHigh High intensity
+ * @returns a pointer to a BinaryImage that represents the result of the
+ * threshold operation.
+ */
+BinaryImage *ColorImage::ThresholdHSI(int hueLow, int hueHigh,
+ int saturationLow, int saturationHigh,
+ int intensityLow, int intensityHigh) {
+ return ComputeThreshold(IMAQ_HSI, hueLow, hueHigh, saturationLow,
+ saturationHigh, intensityLow, intensityHigh);
+}
+
+/**
+ * Perform a threshold in HSI space.
+ * @param threshold a reference to the Threshold object to use.
+ * @returns A pointer to a BinaryImage that represents the result of the
+ * threshold operation.
+ */
+BinaryImage *ColorImage::ThresholdHSI(Threshold &t) {
+ return ComputeThreshold(IMAQ_HSI, t.plane1Low, t.plane1High, t.plane2Low,
+ t.plane2High, t.plane3Low, t.plane3High);
+}
+
+/**
+ * Extract a color plane from the image
+ * @param mode The ColorMode to use for the plane extraction
+ * @param planeNumber Which plane is to be extracted
+ * @returns A pointer to a MonoImage that represents the extracted plane.
+ */
+MonoImage *ColorImage::ExtractColorPlane(ColorMode mode, int planeNumber) {
+ auto result = new MonoImage();
+ if (m_imaqImage == nullptr) wpi_setWPIError(NullParameter);
+ int success = imaqExtractColorPlanes(
+ m_imaqImage, mode, (planeNumber == 1) ? result->GetImaqImage() : nullptr,
+ (planeNumber == 2) ? result->GetImaqImage() : nullptr,
+ (planeNumber == 3) ? result->GetImaqImage() : nullptr);
+ wpi_setImaqErrorWithContext(success, "Imaq ExtractColorPlanes failed");
+ return result;
+}
+
+/*
+ * Extract the first color plane for an image.
+ * @param mode The color mode in which to operate
+ * @returns a pointer to a MonoImage that is the extracted plane.
+ */
+MonoImage *ColorImage::ExtractFirstColorPlane(ColorMode mode) {
+ return ExtractColorPlane(mode, 1);
+}
+
+/*
+ * Extract the second color plane for an image.
+ * @param mode The color mode in which to operate
+ * @returns a pointer to a MonoImage that is the extracted plane.
+ */
+MonoImage *ColorImage::ExtractSecondColorPlane(ColorMode mode) {
+ return ExtractColorPlane(mode, 2);
+}
+
+/*
+ * Extract the third color plane for an image.
+ * @param mode The color mode in which to operate
+ * @returns a pointer to a MonoImage that is the extracted plane.
+ */
+MonoImage *ColorImage::ExtractThirdColorPlane(ColorMode mode) {
+ return ExtractColorPlane(mode, 3);
+}
+
+/*
+ * Extract the red plane from an RGB image.
+ * @returns a pointer to a MonoImage that is the extraced plane.
+ */
+MonoImage *ColorImage::GetRedPlane() {
+ return ExtractFirstColorPlane(IMAQ_RGB);
+}
+
+/*
+ * Extract the green plane from an RGB image.
+ * @returns a pointer to a MonoImage that is the extraced plane.
+ */
+MonoImage *ColorImage::GetGreenPlane() {
+ return ExtractSecondColorPlane(IMAQ_RGB);
+}
+
+/*
+ * Extract the blue plane from an RGB image.
+ * @returns a pointer to a MonoImage that is the extraced plane.
+ */
+MonoImage *ColorImage::GetBluePlane() {
+ return ExtractThirdColorPlane(IMAQ_RGB);
+}
+
+/*
+ * Extract the Hue plane from an HSL image.
+ * @returns a pointer to a MonoImage that is the extraced plane.
+ */
+MonoImage *ColorImage::GetHSLHuePlane() {
+ return ExtractFirstColorPlane(IMAQ_HSL);
+}
+
+/*
+ * Extract the Hue plane from an HSV image.
+ * @returns a pointer to a MonoImage that is the extraced plane.
+ */
+MonoImage *ColorImage::GetHSVHuePlane() {
+ return ExtractFirstColorPlane(IMAQ_HSV);
+}
+
+/*
+ * Extract the Hue plane from an HSI image.
+ * @returns a pointer to a MonoImage that is the extraced plane.
+ */
+MonoImage *ColorImage::GetHSIHuePlane() {
+ return ExtractFirstColorPlane(IMAQ_HSI);
+}
+
+/*
+ * Extract the Luminance plane from an HSL image.
+ * @returns a pointer to a MonoImage that is the extraced plane.
+ */
+MonoImage *ColorImage::GetLuminancePlane() {
+ return ExtractThirdColorPlane(IMAQ_HSL);
+}
+
+/*
+ * Extract the Value plane from an HSV image.
+ * @returns a pointer to a MonoImage that is the extraced plane.
+ */
+MonoImage *ColorImage::GetValuePlane() {
+ return ExtractThirdColorPlane(IMAQ_HSV);
+}
+
+/*
+ * Extract the Intensity plane from an HSI image.
+ * @returns a pointer to a MonoImage that is the extraced plane.
+ */
+MonoImage *ColorImage::GetIntensityPlane() {
+ return ExtractThirdColorPlane(IMAQ_HSI);
+}
+
+/**
+ * Replace a plane in the ColorImage with a MonoImage
+ * Replaces a single plane in the image with a MonoImage
+ * @param mode The ColorMode in which to operate
+ * @param plane The pointer to the replacement plane as a MonoImage
+ * @param planeNumber The plane number (1, 2, 3) to replace
+ */
+void ColorImage::ReplacePlane(ColorMode mode, MonoImage *plane,
+ int planeNumber) {
+ int success =
+ imaqReplaceColorPlanes(m_imaqImage, (const Image *)m_imaqImage, mode,
+ (planeNumber == 1) ? plane->GetImaqImage() : nullptr,
+ (planeNumber == 2) ? plane->GetImaqImage() : nullptr,
+ (planeNumber == 3) ? plane->GetImaqImage() : nullptr);
+ wpi_setImaqErrorWithContext(success, "Imaq ReplaceColorPlanes failed");
+}
+
+/**
+ * Replace the first color plane with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceFirstColorPlane(ColorMode mode, MonoImage *plane) {
+ ReplacePlane(mode, plane, 1);
+}
+
+/**
+ * Replace the second color plane with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceSecondColorPlane(ColorMode mode, MonoImage *plane) {
+ ReplacePlane(mode, plane, 2);
+}
+
+/**
+ * Replace the third color plane with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceThirdColorPlane(ColorMode mode, MonoImage *plane) {
+ ReplacePlane(mode, plane, 3);
+}
+
+/**
+ * Replace the red color plane with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceRedPlane(MonoImage *plane) {
+ ReplaceFirstColorPlane(IMAQ_RGB, plane);
+}
+
+/**
+ * Replace the green color plane with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceGreenPlane(MonoImage *plane) {
+ ReplaceSecondColorPlane(IMAQ_RGB, plane);
+}
+
+/**
+ * Replace the blue color plane with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceBluePlane(MonoImage *plane) {
+ ReplaceThirdColorPlane(IMAQ_RGB, plane);
+}
+
+/**
+ * Replace the Hue color plane in a HSL image with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceHSLHuePlane(MonoImage *plane) {
+ return ReplaceFirstColorPlane(IMAQ_HSL, plane);
+}
+
+/**
+ * Replace the Hue color plane in a HSV image with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceHSVHuePlane(MonoImage *plane) {
+ return ReplaceFirstColorPlane(IMAQ_HSV, plane);
+}
+
+/**
+ * Replace the first Hue plane in a HSI image with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceHSIHuePlane(MonoImage *plane) {
+ return ReplaceFirstColorPlane(IMAQ_HSI, plane);
+}
+
+/**
+ * Replace the Saturation color plane in an HSL image with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceHSLSaturationPlane(MonoImage *plane) {
+ return ReplaceSecondColorPlane(IMAQ_HSL, plane);
+}
+
+/**
+ * Replace the Saturation color plane in a HSV image with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceHSVSaturationPlane(MonoImage *plane) {
+ return ReplaceSecondColorPlane(IMAQ_HSV, plane);
+}
+
+/**
+ * Replace the Saturation color plane in a HSI image with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceHSISaturationPlane(MonoImage *plane) {
+ return ReplaceSecondColorPlane(IMAQ_HSI, plane);
+}
+
+/**
+ * Replace the Luminance color plane in an HSL image with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceLuminancePlane(MonoImage *plane) {
+ return ReplaceThirdColorPlane(IMAQ_HSL, plane);
+}
+
+/**
+ * Replace the Value color plane in an HSV with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceValuePlane(MonoImage *plane) {
+ return ReplaceThirdColorPlane(IMAQ_HSV, plane);
+}
+
+/**
+ * Replace the Intensity color plane in a HSI image with a MonoImage.
+ * @param mode The color mode in which to operate.
+ * @param plane A pointer to a MonoImage that will replace the specified color
+ * plane.
+ */
+void ColorImage::ReplaceIntensityPlane(MonoImage *plane) {
+ return ReplaceThirdColorPlane(IMAQ_HSI, plane);
+}
+
+// TODO: frcColorEqualize(Image* dest, const Image* source, int
+// colorEqualization) needs to be modified
+// The colorEqualization parameter is discarded and is set to TRUE in the call
+// to imaqColorEqualize.
+void ColorImage::Equalize(bool allPlanes) {
+ // Note that this call uses NI-defined TRUE and FALSE
+ int success = imaqColorEqualize(m_imaqImage, (const Image *)m_imaqImage,
+ (allPlanes) ? TRUE : FALSE);
+ wpi_setImaqErrorWithContext(success, "Imaq ColorEqualize error");
+}
+
+void ColorImage::ColorEqualize() { Equalize(true); }
+
+void ColorImage::LuminanceEqualize() { Equalize(false); }
diff --git a/wpilibc/Athena/src/Vision/FrcError.cpp b/wpilibc/Athena/src/Vision/FrcError.cpp
new file mode 100644
index 0000000..f0b4725
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/FrcError.cpp
@@ -0,0 +1,2403 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "nivision.h"
+#include "Vision/FrcError.h"
+
+/**
+ * Get the error code returned from the NI Vision library
+ * @return The last error code.
+ */
+int GetLastVisionError() {
+ // int errorCode = imaqGetLastVisionError(); // error code: 0 = no error
+ // char* errorText = GetVisionErrorText(errorCode);
+ // dprintf (LOG_DEBUG, "Error = %i %s ", errorCode, errorText);
+ return imaqGetLastError();
+}
+
+/**
+* Get the error text for an NI Vision error code.
+* Note: imaqGetErrorText() is not supported on real time system, so
+* so relevant strings are hardcoded here - the maintained version is
+* in the LabWindows/CVI help file.
+* @param errorCode The error code to find the text for.
+* @return The error text
+*/
+const char* GetVisionErrorText(int errorCode) {
+ const char* errorText;
+
+ switch (errorCode) {
+ default: {
+ errorText = "UNKNOWN_ERROR";
+ break;
+ }
+ case -1074395138: {
+ errorText = "ERR_OCR_REGION_TOO_SMALL";
+ break;
+ }
+ case -1074395139: {
+ errorText = "ERR_IMAQ_QR_DIMENSION_INVALID";
+ break;
+ }
+ case -1074395140: {
+ errorText = "ERR_OCR_CHAR_REPORT_CORRUPTED";
+ break;
+ }
+ case -1074395141: {
+ errorText = "ERR_OCR_NO_TEXT_FOUND";
+ break;
+ }
+ case -1074395142: {
+ errorText = "ERR_QR_DETECTION_MODELTYPE";
+ break;
+ }
+ case -1074395143: {
+ errorText = "ERR_QR_DETECTION_MODE";
+ break;
+ }
+ case -1074395144: {
+ errorText = "ERR_QR_INVALID_BARCODE";
+ break;
+ }
+ case -1074395145: {
+ errorText = "ERR_QR_INVALID_READ";
+ break;
+ }
+ case -1074395146: {
+ errorText = "ERR_QR_DETECTION_VERSION";
+ break;
+ }
+ case -1074395147: {
+ errorText = "ERR_BARCODE_RSSLIMITED";
+ break;
+ }
+ case -1074395148: {
+ errorText = "ERR_OVERLAY_GROUP_NOT_FOUND";
+ break;
+ }
+ case -1074395149: {
+ errorText = "ERR_DUPLICATE_TRANSFORM_TYPE";
+ break;
+ }
+ case -1074395151: {
+ errorText = "ERR_OCR_CORRECTION_FAILED";
+ break;
+ }
+ case -1074395155: {
+ errorText = "ERR_OCR_ORIENT_DETECT_FAILED";
+ break;
+ }
+ case -1074395156: {
+ errorText = "ERR_OCR_SKEW_DETECT_FAILED";
+ break;
+ }
+ case -1074395158: {
+ errorText = "ERR_OCR_INVALID_CONTRASTMODE";
+ break;
+ }
+ case -1074395159: {
+ errorText = "ERR_OCR_INVALID_TOLERANCE";
+ break;
+ }
+ case -1074395160: {
+ errorText = "ERR_OCR_INVALID_MAXPOINTSIZE";
+ break;
+ }
+ case -1074395161: {
+ errorText = "ERR_OCR_INVALID_CORRECTIONLEVEL";
+ break;
+ }
+ case -1074395162: {
+ errorText = "ERR_OCR_INVALID_CORRECTIONMODE";
+ break;
+ }
+ case -1074395163: {
+ errorText = "ERR_OCR_INVALID_CHARACTERPREFERENCE";
+ break;
+ }
+ case -1074395164: {
+ errorText = "ERR_OCR_ADD_WORD_FAILED";
+ break;
+ }
+ case -1074395165: {
+ errorText = "ERR_OCR_WTS_DIR_NOT_FOUND";
+ break;
+ }
+ case -1074395166: {
+ errorText = "ERR_OCR_BIN_DIR_NOT_FOUND";
+ break;
+ }
+ case -1074395167: {
+ errorText = "ERR_OCR_INVALID_OUTPUTDELIMITER";
+ break;
+ }
+ case -1074395168: {
+ errorText = "ERR_OCR_INVALID_AUTOCORRECTIONMODE";
+ break;
+ }
+ case -1074395169: {
+ errorText = "ERR_OCR_INVALID_RECOGNITIONMODE";
+ break;
+ }
+ case -1074395170: {
+ errorText = "ERR_OCR_INVALID_CHARACTERTYPE";
+ break;
+ }
+ case -1074395171: {
+ errorText = "ERR_OCR_INI_FILE_NOT_FOUND";
+ break;
+ }
+ case -1074395172: {
+ errorText = "ERR_OCR_INVALID_CHARACTERSET";
+ break;
+ }
+ case -1074395173: {
+ errorText = "ERR_OCR_INVALID_LANGUAGE";
+ break;
+ }
+ case -1074395174: {
+ errorText = "ERR_OCR_INVALID_AUTOORIENTMODE";
+ break;
+ }
+ case -1074395175: {
+ errorText = "ERR_OCR_BAD_USER_DICTIONARY";
+ break;
+ }
+ case -1074395178: {
+ errorText = "ERR_OCR_RECOGNITION_FAILED";
+ break;
+ }
+ case -1074395179: {
+ errorText = "ERR_OCR_PREPROCESSING_FAILED";
+ break;
+ }
+ case -1074395200: {
+ errorText = "ERR_OCR_INVALID_PARAMETER";
+ break;
+ }
+ case -1074395201: {
+ errorText = "ERR_OCR_LOAD_LIBRARY";
+ break;
+ }
+ case -1074395203: {
+ errorText = "ERR_OCR_LIB_INIT";
+ break;
+ }
+ case -1074395210: {
+ errorText = "ERR_OCR_CANNOT_MATCH_TEXT_TEMPLATE";
+ break;
+ }
+ case -1074395211: {
+ errorText = "ERR_OCR_BAD_TEXT_TEMPLATE";
+ break;
+ }
+ case -1074395212: {
+ errorText = "ERR_OCR_TEMPLATE_WRONG_SIZE";
+ break;
+ }
+ case -1074395233: {
+ errorText = "ERR_TEMPLATE_IMAGE_TOO_LARGE";
+ break;
+ }
+ case -1074395234: {
+ errorText = "ERR_TEMPLATE_IMAGE_TOO_SMALL";
+ break;
+ }
+ case -1074395235: {
+ errorText = "ERR_TEMPLATE_IMAGE_CONTRAST_TOO_LOW";
+ break;
+ }
+ case -1074395237: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR_SHIFT_1";
+ break;
+ }
+ case -1074395238: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR_NOSHIFT";
+ break;
+ }
+ case -1074395239: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR_SHIFT";
+ break;
+ }
+ case -1074395240: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR_ROTATION_1";
+ break;
+ }
+ case -1074395241: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR_NOROTATION";
+ break;
+ }
+ case -1074395242: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR_ROTATION";
+ break;
+ }
+ case -1074395243: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR_4";
+ break;
+ }
+ case -1074395244: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR_3";
+ break;
+ }
+ case -1074395245: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR_2";
+ break;
+ }
+ case -1074395246: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR_1";
+ break;
+ }
+ case -1074395247: {
+ errorText = "ERR_TEMPLATE_DESCRIPTOR";
+ break;
+ }
+ case -1074395248: {
+ errorText = "ERR_TOO_MANY_ROTATION_ANGLE_RANGES";
+ break;
+ }
+ case -1074395249: {
+ errorText = "ERR_ROTATION_ANGLE_RANGE_TOO_LARGE";
+ break;
+ }
+ case -1074395250: {
+ errorText = "ERR_MATCH_SETUP_DATA";
+ break;
+ }
+ case -1074395251: {
+ errorText = "ERR_INVALID_MATCH_MODE";
+ break;
+ }
+ case -1074395252: {
+ errorText = "ERR_LEARN_SETUP_DATA";
+ break;
+ }
+ case -1074395253: {
+ errorText = "ERR_INVALID_LEARN_MODE";
+ break;
+ }
+ case -1074395256: {
+ errorText = "ERR_EVEN_WINDOW_SIZE";
+ break;
+ }
+ case -1074395257: {
+ errorText = "ERR_INVALID_EDGE_DIR";
+ break;
+ }
+ case -1074395258: {
+ errorText = "ERR_BAD_FILTER_WIDTH";
+ break;
+ }
+ case -1074395260: {
+ errorText = "ERR_HEAP_TRASHED";
+ break;
+ }
+ case -1074395261: {
+ errorText = "ERR_GIP_RANGE";
+ break;
+ }
+ case -1074395262: {
+ errorText = "ERR_LCD_BAD_MATCH";
+ break;
+ }
+ case -1074395263: {
+ errorText = "ERR_LCD_NO_SEGMENTS";
+ break;
+ }
+ case -1074395265: {
+ errorText = "ERR_BARCODE";
+ break;
+ }
+ case -1074395267: {
+ errorText = "ERR_COMPLEX_ROOT";
+ break;
+ }
+ case -1074395268: {
+ errorText = "ERR_LINEAR_COEFF";
+ break;
+ }
+ case -1074395269: {
+ errorText = "ERR_nullptr_POINTER";
+ break;
+ }
+ case -1074395270: {
+ errorText = "ERR_DIV_BY_ZERO";
+ break;
+ }
+ case -1074395275: {
+ errorText = "ERR_INVALID_BROWSER_IMAGE";
+ break;
+ }
+ case -1074395276: {
+ errorText = "ERR_LINES_PARALLEL";
+ break;
+ }
+ case -1074395277: {
+ errorText = "ERR_BARCODE_CHECKSUM";
+ break;
+ }
+ case -1074395278: {
+ errorText = "ERR_LCD_NOT_NUMERIC";
+ break;
+ }
+ case -1074395279: {
+ errorText = "ERR_ROI_NOT_POLYGON";
+ break;
+ }
+ case -1074395280: {
+ errorText = "ERR_ROI_NOT_RECT";
+ break;
+ }
+ case -1074395281: {
+ errorText = "ERR_IMAGE_SMALLER_THAN_BORDER";
+ break;
+ }
+ case -1074395282: {
+ errorText = "ERR_CANT_DRAW_INTO_VIEWER";
+ break;
+ }
+ case -1074395283: {
+ errorText = "ERR_INVALID_RAKE_DIRECTION";
+ break;
+ }
+ case -1074395284: {
+ errorText = "ERR_INVALID_EDGE_PROCESS";
+ break;
+ }
+ case -1074395285: {
+ errorText = "ERR_INVALID_SPOKE_DIRECTION";
+ break;
+ }
+ case -1074395286: {
+ errorText = "ERR_INVALID_CONCENTRIC_RAKE_DIRECTION";
+ break;
+ }
+ case -1074395287: {
+ errorText = "ERR_INVALID_LINE";
+ break;
+ }
+ case -1074395290: {
+ errorText = "ERR_SHAPEMATCH_BADTEMPLATE";
+ break;
+ }
+ case -1074395291: {
+ errorText = "ERR_SHAPEMATCH_BADIMAGEDATA";
+ break;
+ }
+ case -1074395292: {
+ errorText = "ERR_POINTS_ARE_COLLINEAR";
+ break;
+ }
+ case -1074395293: {
+ errorText = "ERR_CONTOURID_NOT_FOUND";
+ break;
+ }
+ case -1074395294: {
+ errorText = "ERR_CONTOUR_INDEX_OUT_OF_RANGE";
+ break;
+ }
+ case -1074395295: {
+ errorText = "ERR_INVALID_INTERPOLATIONMETHOD_INTERPOLATEPOINTS";
+ break;
+ }
+ case -1074395296: {
+ errorText = "ERR_INVALID_BARCODETYPE";
+ break;
+ }
+ case -1074395297: {
+ errorText = "ERR_INVALID_PARTICLEINFOMODE";
+ break;
+ }
+ case -1074395298: {
+ errorText = "ERR_COMPLEXPLANE_NOT_REAL_OR_IMAGINARY";
+ break;
+ }
+ case -1074395299: {
+ errorText = "ERR_INVALID_COMPLEXPLANE";
+ break;
+ }
+ case -1074395300: {
+ errorText = "ERR_INVALID_METERARCMODE";
+ break;
+ }
+ case -1074395301: {
+ errorText = "ERR_ROI_NOT_2_LINES";
+ break;
+ }
+ case -1074395302: {
+ errorText = "ERR_INVALID_THRESHOLDMETHOD";
+ break;
+ }
+ case -1074395303: {
+ errorText = "ERR_INVALID_NUM_OF_CLASSES";
+ break;
+ }
+ case -1074395304: {
+ errorText = "ERR_INVALID_MATHTRANSFORMMETHOD";
+ break;
+ }
+ case -1074395305: {
+ errorText = "ERR_INVALID_REFERENCEMODE";
+ break;
+ }
+ case -1074395306: {
+ errorText = "ERR_INVALID_TOOL";
+ break;
+ }
+ case -1074395307: {
+ errorText = "ERR_PRECISION_NOT_GTR_THAN_0";
+ break;
+ }
+ case -1074395308: {
+ errorText = "ERR_INVALID_COLORSENSITIVITY";
+ break;
+ }
+ case -1074395309: {
+ errorText = "ERR_INVALID_WINDOW_THREAD_POLICY";
+ break;
+ }
+ case -1074395310: {
+ errorText = "ERR_INVALID_PALETTE_TYPE";
+ break;
+ }
+ case -1074395311: {
+ errorText = "ERR_INVALID_COLOR_SPECTRUM";
+ break;
+ }
+ case -1074395312: {
+ errorText = "ERR_LCD_CALIBRATE";
+ break;
+ }
+ case -1074395313: {
+ errorText = "ERR_WRITE_FILE_NOT_SUPPORTED";
+ break;
+ }
+ case -1074395316: {
+ errorText = "ERR_INVALID_KERNEL_CODE";
+ break;
+ }
+ case -1074395317: {
+ errorText = "ERR_UNDEF_POINT";
+ break;
+ }
+ case -1074395318: {
+ errorText = "ERR_INSF_POINTS";
+ break;
+ }
+ case -1074395319: {
+ errorText = "ERR_INVALID_SUBPIX_TYPE";
+ break;
+ }
+ case -1074395320: {
+ errorText = "ERR_TEMPLATE_EMPTY";
+ break;
+ }
+ case -1074395321: {
+ errorText = "ERR_INVALID_MORPHOLOGYMETHOD";
+ break;
+ }
+ case -1074395322: {
+ errorText = "ERR_INVALID_TEXTALIGNMENT";
+ break;
+ }
+ case -1074395323: {
+ errorText = "ERR_INVALID_FONTCOLOR";
+ break;
+ }
+ case -1074395324: {
+ errorText = "ERR_INVALID_SHAPEMODE";
+ break;
+ }
+ case -1074395325: {
+ errorText = "ERR_INVALID_DRAWMODE";
+ break;
+ }
+ case -1074395326: {
+ errorText = "ERR_INVALID_DRAWMODE_FOR_LINE";
+ break;
+ }
+ case -1074395327: {
+ errorText = "ERR_INVALID_SCALINGMODE";
+ break;
+ }
+ case -1074395328: {
+ errorText = "ERR_INVALID_INTERPOLATIONMETHOD";
+ break;
+ }
+ case -1074395329: {
+ errorText = "ERR_INVALID_OUTLINEMETHOD";
+ break;
+ }
+ case -1074395330: {
+ errorText = "ERR_INVALID_BORDER_SIZE";
+ break;
+ }
+ case -1074395331: {
+ errorText = "ERR_INVALID_BORDERMETHOD";
+ break;
+ }
+ case -1074395332: {
+ errorText = "ERR_INVALID_COMPAREFUNCTION";
+ break;
+ }
+ case -1074395333: {
+ errorText = "ERR_INVALID_VERTICAL_TEXT_ALIGNMENT";
+ break;
+ }
+ case -1074395334: {
+ errorText = "ERR_INVALID_CONVERSIONSTYLE";
+ break;
+ }
+ case -1074395335: {
+ errorText = "ERR_DISPATCH_STATUS_CONFLICT";
+ break;
+ }
+ case -1074395336: {
+ errorText = "ERR_UNKNOWN_ALGORITHM";
+ break;
+ }
+ case -1074395340: {
+ errorText = "ERR_INVALID_SIZETYPE";
+ break;
+ }
+ case -1074395343: {
+ errorText = "ERR_FILE_FILENAME_nullptr";
+ break;
+ }
+ case -1074395345: {
+ errorText = "ERR_INVALID_FLIPAXIS";
+ break;
+ }
+ case -1074395346: {
+ errorText = "ERR_INVALID_INTERPOLATIONMETHOD_FOR_ROTATE";
+ break;
+ }
+ case -1074395347: {
+ errorText = "ERR_INVALID_3DDIRECTION";
+ break;
+ }
+ case -1074395348: {
+ errorText = "ERR_INVALID_3DPLANE";
+ break;
+ }
+ case -1074395349: {
+ errorText = "ERR_INVALID_SKELETONMETHOD";
+ break;
+ }
+ case -1074395350: {
+ errorText = "ERR_INVALID_VISION_INFO";
+ break;
+ }
+ case -1074395351: {
+ errorText = "ERR_INVALID_RECT";
+ break;
+ }
+ case -1074395352: {
+ errorText = "ERR_INVALID_FEATURE_MODE";
+ break;
+ }
+ case -1074395353: {
+ errorText = "ERR_INVALID_SEARCH_STRATEGY";
+ break;
+ }
+ case -1074395354: {
+ errorText = "ERR_INVALID_COLOR_WEIGHT";
+ break;
+ }
+ case -1074395355: {
+ errorText = "ERR_INVALID_NUM_MATCHES_REQUESTED";
+ break;
+ }
+ case -1074395356: {
+ errorText = "ERR_INVALID_MIN_MATCH_SCORE";
+ break;
+ }
+ case -1074395357: {
+ errorText = "ERR_INVALID_COLOR_IGNORE_MODE";
+ break;
+ }
+ case -1074395360: {
+ errorText = "ERR_COMPLEX_PLANE";
+ break;
+ }
+ case -1074395361: {
+ errorText = "ERR_INVALID_STEEPNESS";
+ break;
+ }
+ case -1074395362: {
+ errorText = "ERR_INVALID_WIDTH";
+ break;
+ }
+ case -1074395363: {
+ errorText = "ERR_INVALID_SUBSAMPLING_RATIO";
+ break;
+ }
+ case -1074395364: {
+ errorText = "ERR_IGNORE_COLOR_SPECTRUM_SET";
+ break;
+ }
+ case -1074395365: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_NOSPECTRUM";
+ break;
+ }
+ case -1074395366: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_NOSHAPE";
+ break;
+ }
+ case -1074395367: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_ROTATION_5";
+ break;
+ }
+ case -1074395368: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_ROTATION_4";
+ break;
+ }
+ case -1074395369: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_ROTATION_3";
+ break;
+ }
+ case -1074395370: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_ROTATION_2";
+ break;
+ }
+ case -1074395371: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_ROTATION_1";
+ break;
+ }
+ case -1074395372: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_NOROTATION";
+ break;
+ }
+ case -1074395373: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_ROTATION";
+ break;
+ }
+ case -1074395374: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_SHIFT_2";
+ break;
+ }
+ case -1074395375: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_SHIFT_1";
+ break;
+ }
+ case -1074395376: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_NOSHIFT";
+ break;
+ }
+ case -1074395377: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_SHIFT";
+ break;
+ }
+ case -1074395378: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_6";
+ break;
+ }
+ case -1074395379: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_5";
+ break;
+ }
+ case -1074395380: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_4";
+ break;
+ }
+ case -1074395381: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_3";
+ break;
+ }
+ case -1074395382: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_2";
+ break;
+ }
+ case -1074395383: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR_1";
+ break;
+ }
+ case -1074395384: {
+ errorText = "ERR_COLOR_TEMPLATE_DESCRIPTOR";
+ break;
+ }
+ case -1074395385: {
+ errorText = "ERR_COLOR_ROTATION_REQUIRES_SHAPE_FEATURE";
+ break;
+ }
+ case -1074395386: {
+ errorText = "ERR_COLOR_MATCH_SETUP_DATA_SHAPE";
+ break;
+ }
+ case -1074395387: {
+ errorText = "ERR_COLOR_MATCH_SETUP_DATA";
+ break;
+ }
+ case -1074395388: {
+ errorText = "ERR_COLOR_LEARN_SETUP_DATA_SHAPE";
+ break;
+ }
+ case -1074395389: {
+ errorText = "ERR_COLOR_LEARN_SETUP_DATA";
+ break;
+ }
+ case -1074395390: {
+ errorText = "ERR_COLOR_TEMPLATE_IMAGE_LUMINANCE_CONTRAST_TOO_LOW";
+ break;
+ }
+ case -1074395391: {
+ errorText = "ERR_COLOR_TEMPLATE_IMAGE_HUE_CONTRAST_TOO_LOW";
+ break;
+ }
+ case -1074395392: {
+ errorText = "ERR_COLOR_TEMPLATE_IMAGE_TOO_LARGE";
+ break;
+ }
+ case -1074395393: {
+ errorText = "ERR_COLOR_TEMPLATE_IMAGE_TOO_SMALL";
+ break;
+ }
+ case -1074395394: {
+ errorText = "ERR_COLOR_SPECTRUM_MASK";
+ break;
+ }
+ case -1074395395: {
+ errorText = "ERR_COLOR_IMAGE_REQUIRED";
+ break;
+ }
+ case -1074395397: {
+ errorText = "ERR_COMPLEX_IMAGE_REQUIRED";
+ break;
+ }
+ case -1074395399: {
+ errorText = "ERR_MULTICORE_INVALID_ARGUMENT";
+ break;
+ }
+ case -1074395400: {
+ errorText = "ERR_MULTICORE_OPERATION";
+ break;
+ }
+ case -1074395401: {
+ errorText = "ERR_INVALID_MATCHFACTOR";
+ break;
+ }
+ case -1074395402: {
+ errorText = "ERR_INVALID_MAXPOINTS";
+ break;
+ }
+ case -1074395403: {
+ errorText = "ERR_EXTRAINFO_VERSION";
+ break;
+ }
+ case -1074395404: {
+ errorText = "ERR_INVALID_INTERPOLATIONMETHOD_FOR_UNWRAP";
+ break;
+ }
+ case -1074395405: {
+ errorText = "ERR_INVALID_TEXTORIENTATION";
+ break;
+ }
+ case -1074395406: {
+ errorText = "ERR_COORDSYS_NOT_FOUND";
+ break;
+ }
+ case -1074395407: {
+ errorText = "ERR_INVALID_CONTRAST";
+ break;
+ }
+ case -1074395408: {
+ errorText = "ERR_INVALID_DETECTION_MODE";
+ break;
+ }
+ case -1074395409: {
+ errorText = "ERR_INVALID_SUBPIXEL_DIVISIONS";
+ break;
+ }
+ case -1074395410: {
+ errorText = "ERR_INVALID_ICONS_PER_LINE";
+ break;
+ }
+ case -1074395549: {
+ errorText = "ERR_NIOCR_INVALID_NUMBER_OF_OBJECTS_TO_VERIFY";
+ break;
+ }
+ case -1074395550: {
+ errorText = "ERR_NIOCR_INVALID_CHARACTER_VALUE";
+ break;
+ }
+ case -1074395551: {
+ errorText = "ERR_NIOCR_RENAME_REFCHAR";
+ break;
+ }
+ case -1074395552: {
+ errorText = "ERR_NIOCR_NOT_A_VALID_CHARACTER_SET";
+ break;
+ }
+ case -1074395553: {
+ errorText = "ERR_NIOCR_INVALID_MIN_BOUNDING_RECT_HEIGHT";
+ break;
+ }
+ case -1074395554: {
+ errorText = "ERR_NIOCR_INVALID_READ_RESOLUTION";
+ break;
+ }
+ case -1074395555: {
+ errorText = "ERR_NIOCR_INVALID_SPACING_RANGE";
+ break;
+ }
+ case -1074395556: {
+ errorText = "ERR_NIOCR_INVALID_BOUNDING_RECT_HEIGHT_RANGE";
+ break;
+ }
+ case -1074395557: {
+ errorText = "ERR_NIOCR_INVALID_BOUNDING_RECT_WIDTH_RANGE";
+ break;
+ }
+ case -1074395558: {
+ errorText = "ERR_NIOCR_INVALID_CHARACTER_SIZE_RANGE";
+ break;
+ }
+ case -1074395559: {
+ errorText = "ERR_NIOCR_INVALID_READ_OPTION";
+ break;
+ }
+ case -1074395560: {
+ errorText = "ERR_NIOCR_INVALID_OBJECT_INDEX";
+ break;
+ }
+ case -1074395561: {
+ errorText = "ERR_NIOCR_INVALID_NUMBER_OF_CHARACTERS";
+ break;
+ }
+ case -1074395562: {
+ errorText = "ERR_NIOCR_BOOLEAN_VALUE_FOR_STRING_ATTRIBUTE";
+ break;
+ }
+ case -1074395563: {
+ errorText = "ERR_NIOCR_UNLICENSED";
+ break;
+ }
+ case -1074395564: {
+ errorText = "ERR_NIOCR_INVALID_PREDEFINED_CHARACTER";
+ break;
+ }
+ case -1074395565: {
+ errorText = "ERR_NIOCR_MUST_BE_SINGLE_CHARACTER";
+ break;
+ }
+ case -1074395566: {
+ errorText = "ERR_NIOCR_BOOLEAN_VALUE_FOR_INTEGER_ATTRIBUTE";
+ break;
+ }
+ case -1074395567: {
+ errorText = "ERR_NIOCR_STRING_VALUE_FOR_BOOLEAN_ATTRIBUTE";
+ break;
+ }
+ case -1074395568: {
+ errorText = "ERR_NIOCR_STRING_VALUE_FOR_INTEGER_ATTRIBUTE";
+ break;
+ }
+ case -1074395569: {
+ errorText = "ERR_NIOCR_INVALID_ATTRIBUTE";
+ break;
+ }
+ case -1074395570: {
+ errorText = "ERR_NIOCR_INTEGER_VALUE_FOR_BOOLEAN_ATTRIBUTE";
+ break;
+ }
+ case -1074395571: {
+ errorText = "ERR_NIOCR_GET_ONLY_ATTRIBUTE";
+ break;
+ }
+ case -1074395572: {
+ errorText = "ERR_NIOCR_INTEGER_VALUE_FOR_STRING_ATTRIBUTE";
+ break;
+ }
+ case -1074395573: {
+ errorText = "ERR_NIOCR_INVALID_CHARACTER_SET_FILE_VERSION";
+ break;
+ }
+ case -1074395574: {
+ errorText = "ERR_NIOCR_CHARACTER_SET_DESCRIPTION_TOO_LONG";
+ break;
+ }
+ case -1074395575: {
+ errorText = "ERR_NIOCR_INVALID_NUMBER_OF_EROSIONS";
+ break;
+ }
+ case -1074395576: {
+ errorText = "ERR_NIOCR_CHARACTER_VALUE_TOO_LONG";
+ break;
+ }
+ case -1074395577: {
+ errorText = "ERR_NIOCR_CHARACTER_VALUE_CANNOT_BE_EMPTYSTRING";
+ break;
+ }
+ case -1074395578: {
+ errorText = "ERR_NIOCR_INVALID_CHARACTER_SET_FILE";
+ break;
+ }
+ case -1074395579: {
+ errorText = "ERR_NIOCR_INVALID_ASPECT_RATIO";
+ break;
+ }
+ case -1074395580: {
+ errorText = "ERR_NIOCR_INVALID_MIN_BOUNDING_RECT_WIDTH";
+ break;
+ }
+ case -1074395581: {
+ errorText = "ERR_NIOCR_INVALID_MAX_VERT_ELEMENT_SPACING";
+ break;
+ }
+ case -1074395582: {
+ errorText = "ERR_NIOCR_INVALID_MAX_HORIZ_ELEMENT_SPACING";
+ break;
+ }
+ case -1074395583: {
+ errorText = "ERR_NIOCR_INVALID_MIN_CHAR_SPACING";
+ break;
+ }
+ case -1074395584: {
+ errorText = "ERR_NIOCR_INVALID_THRESHOLD_LIMITS";
+ break;
+ }
+ case -1074395585: {
+ errorText = "ERR_NIOCR_INVALID_UPPER_THRESHOLD_LIMIT";
+ break;
+ }
+ case -1074395586: {
+ errorText = "ERR_NIOCR_INVALID_LOWER_THRESHOLD_LIMIT";
+ break;
+ }
+ case -1074395587: {
+ errorText = "ERR_NIOCR_INVALID_THRESHOLD_RANGE";
+ break;
+ }
+ case -1074395588: {
+ errorText = "ERR_NIOCR_INVALID_HIGH_THRESHOLD_VALUE";
+ break;
+ }
+ case -1074395589: {
+ errorText = "ERR_NIOCR_INVALID_LOW_THRESHOLD_VALUE";
+ break;
+ }
+ case -1074395590: {
+ errorText = "ERR_NIOCR_INVALID_NUMBER_OF_VALID_CHARACTER_POSITIONS";
+ break;
+ }
+ case -1074395591: {
+ errorText = "ERR_NIOCR_INVALID_CHARACTER_INDEX";
+ break;
+ }
+ case -1074395592: {
+ errorText = "ERR_NIOCR_INVALID_READ_STRATEGY";
+ break;
+ }
+ case -1074395593: {
+ errorText = "ERR_NIOCR_INVALID_NUMBER_OF_BLOCKS";
+ break;
+ }
+ case -1074395594: {
+ errorText = "ERR_NIOCR_INVALID_SUBSTITUTION_CHARACTER";
+ break;
+ }
+ case -1074395595: {
+ errorText = "ERR_NIOCR_INVALID_THRESHOLD_MODE";
+ break;
+ }
+ case -1074395596: {
+ errorText = "ERR_NIOCR_INVALID_CHARACTER_SIZE";
+ break;
+ }
+ case -1074395597: {
+ errorText = "ERR_NIOCR_NOT_A_VALID_SESSION";
+ break;
+ }
+ case -1074395598: {
+ errorText = "ERR_NIOCR_INVALID_ACCEPTANCE_LEVEL";
+ break;
+ }
+ case -1074395600: {
+ errorText = "ERR_INFO_NOT_FOUND";
+ break;
+ }
+ case -1074395601: {
+ errorText = "ERR_INVALID_EDGE_THRESHOLD";
+ break;
+ }
+ case -1074395602: {
+ errorText = "ERR_INVALID_MINIMUM_CURVE_LENGTH";
+ break;
+ }
+ case -1074395603: {
+ errorText = "ERR_INVALID_ROW_STEP";
+ break;
+ }
+ case -1074395604: {
+ errorText = "ERR_INVALID_COLUMN_STEP";
+ break;
+ }
+ case -1074395605: {
+ errorText = "ERR_INVALID_MAXIMUM_END_POINT_GAP";
+ break;
+ }
+ case -1074395606: {
+ errorText = "ERR_INVALID_MINIMUM_FEATURES_TO_MATCH";
+ break;
+ }
+ case -1074395607: {
+ errorText = "ERR_INVALID_MAXIMUM_FEATURES_PER_MATCH";
+ break;
+ }
+ case -1074395608: {
+ errorText = "ERR_INVALID_SUBPIXEL_ITERATIONS";
+ break;
+ }
+ case -1074395609: {
+ errorText = "ERR_INVALID_SUBPIXEL_TOLERANCE";
+ break;
+ }
+ case -1074395610: {
+ errorText = "ERR_INVALID_INITIAL_MATCH_LIST_LENGTH";
+ break;
+ }
+ case -1074395611: {
+ errorText = "ERR_INVALID_MINIMUM_RECTANGLE_DIMENSION";
+ break;
+ }
+ case -1074395612: {
+ errorText = "ERR_INVALID_MINIMUM_FEATURE_RADIUS";
+ break;
+ }
+ case -1074395613: {
+ errorText = "ERR_INVALID_MINIMUM_FEATURE_LENGTH";
+ break;
+ }
+ case -1074395614: {
+ errorText = "ERR_INVALID_MINIMUM_FEATURE_ASPECT_RATIO";
+ break;
+ }
+ case -1074395615: {
+ errorText = "ERR_INVALID_MINIMUM_FEATURE_STRENGTH";
+ break;
+ }
+ case -1074395616: {
+ errorText = "ERR_INVALID_EDGE_FILTER_SIZE";
+ break;
+ }
+ case -1074395617: {
+ errorText = "ERR_INVALID_NUMBER_OF_FEATURES_RANGE";
+ break;
+ }
+ case -1074395618: {
+ errorText = "ERR_TOO_MANY_SCALE_RANGES";
+ break;
+ }
+ case -1074395619: {
+ errorText = "ERR_TOO_MANY_OCCLUSION_RANGES";
+ break;
+ }
+ case -1074395620: {
+ errorText = "ERR_INVALID_CURVE_EXTRACTION_MODE";
+ break;
+ }
+ case -1074395621: {
+ errorText = "ERR_INVALID_LEARN_GEOMETRIC_PATTERN_SETUP_DATA";
+ break;
+ }
+ case -1074395622: {
+ errorText = "ERR_INVALID_MATCH_GEOMETRIC_PATTERN_SETUP_DATA";
+ break;
+ }
+ case -1074395623: {
+ errorText = "ERR_INVALID_SCALE_RANGE";
+ break;
+ }
+ case -1074395624: {
+ errorText = "ERR_INVALID_OCCLUSION_RANGE";
+ break;
+ }
+ case -1074395625: {
+ errorText = "ERR_INVALID_MATCH_CONSTRAINT_TYPE";
+ break;
+ }
+ case -1074395626: {
+ errorText = "ERR_NOT_ENOUGH_TEMPLATE_FEATURES";
+ break;
+ }
+ case -1074395627: {
+ errorText = "ERR_NOT_ENOUGH_TEMPLATE_FEATURES_1";
+ break;
+ }
+ case -1074395628: {
+ errorText = "ERR_INVALID_GEOMETRIC_MATCHING_TEMPLATE";
+ break;
+ }
+ case -1074395629: {
+ errorText = "ERR_INVALID_MAXIMUM_PIXEL_DISTANCE_FROM_LINE";
+ break;
+ }
+ case -1074395630: {
+ errorText = "ERR_INVALID_MAXIMUM_FEATURES_LEARNED";
+ break;
+ }
+ case -1074395631: {
+ errorText = "ERR_INVALID_MIN_MATCH_SEPARATION_DISTANCE";
+ break;
+ }
+ case -1074395632: {
+ errorText = "ERR_INVALID_MIN_MATCH_SEPARATION_ANGLE";
+ break;
+ }
+ case -1074395633: {
+ errorText = "ERR_INVALID_MIN_MATCH_SEPARATION_SCALE";
+ break;
+ }
+ case -1074395634: {
+ errorText = "ERR_INVALID_MAX_MATCH_OVERLAP";
+ break;
+ }
+ case -1074395635: {
+ errorText = "ERR_INVALID_SHAPE_DESCRIPTOR";
+ break;
+ }
+ case -1074395636: {
+ errorText = "ERR_DIRECTX_NOT_FOUND";
+ break;
+ }
+ case -1074395637: {
+ errorText = "ERR_HARDWARE_DOESNT_SUPPORT_NONTEARING";
+ break;
+ }
+ case -1074395638: {
+ errorText = "ERR_INVALID_FILL_STYLE";
+ break;
+ }
+ case -1074395639: {
+ errorText = "ERR_INVALID_HATCH_STYLE";
+ break;
+ }
+ case -1074395640: {
+ errorText = "ERR_TOO_MANY_ZONES";
+ break;
+ }
+ case -1074395641: {
+ errorText = "ERR_DUPLICATE_LABEL";
+ break;
+ }
+ case -1074395642: {
+ errorText = "ERR_LABEL_NOT_FOUND";
+ break;
+ }
+ case -1074395643: {
+ errorText = "ERR_INVALID_NUMBER_OF_MATCH_OPTIONS";
+ break;
+ }
+ case -1074395644: {
+ errorText = "ERR_LABEL_TOO_LONG";
+ break;
+ }
+ case -1074395645: {
+ errorText = "ERR_INVALID_NUMBER_OF_LABELS";
+ break;
+ }
+ case -1074395646: {
+ errorText = "ERR_NO_TEMPLATE_TO_LEARN";
+ break;
+ }
+ case -1074395647: {
+ errorText = "ERR_INVALID_MULTIPLE_GEOMETRIC_TEMPLATE";
+ break;
+ }
+ case -1074395648: {
+ errorText = "ERR_TEMPLATE_NOT_LEARNED";
+ break;
+ }
+ case -1074395649: {
+ errorText = "ERR_INVALID_GEOMETRIC_FEATURE_TYPE";
+ break;
+ }
+ case -1074395650: {
+ errorText = "ERR_CURVE_EXTRACTION_MODE_MUST_BE_SAME";
+ break;
+ }
+ case -1074395651: {
+ errorText = "ERR_EDGE_FILTER_SIZE_MUST_BE_SAME";
+ break;
+ }
+ case -1074395652: {
+ errorText = "ERR_OPENING_NEWER_GEOMETRIC_MATCHING_TEMPLATE";
+ break;
+ }
+ case -1074395653: {
+ errorText = "ERR_OPENING_NEWER_MULTIPLE_GEOMETRIC_TEMPLATE";
+ break;
+ }
+ case -1074395654: {
+ errorText = "ERR_GRADING_INFORMATION_NOT_FOUND";
+ break;
+ }
+ case -1074395655: {
+ errorText = "ERR_ENABLE_CALIBRATION_SUPPORT_MUST_BE_SAME";
+ break;
+ }
+ case -1074395656: {
+ errorText = "ERR_SMOOTH_CONTOURS_MUST_BE_SAME";
+ break;
+ }
+ case -1074395700: {
+ errorText = "ERR_REQUIRES_WIN2000_OR_NEWER";
+ break;
+ }
+ case -1074395701: {
+ errorText = "ERR_INVALID_MATRIX_SIZE_RANGE";
+ break;
+ }
+ case -1074395702: {
+ errorText = "ERR_INVALID_LENGTH";
+ break;
+ }
+ case -1074395703: {
+ errorText = "ERR_INVALID_TYPE_OF_FLATTEN";
+ break;
+ }
+ case -1074395704: {
+ errorText = "ERR_INVALID_COMPRESSION_TYPE";
+ break;
+ }
+ case -1074395705: {
+ errorText = "ERR_DATA_CORRUPTED";
+ break;
+ }
+ case -1074395706: {
+ errorText = "ERR_AVI_SESSION_ALREADY_OPEN";
+ break;
+ }
+ case -1074395707: {
+ errorText = "ERR_AVI_WRITE_SESSION_REQUIRED";
+ break;
+ }
+ case -1074395708: {
+ errorText = "ERR_AVI_READ_SESSION_REQUIRED";
+ break;
+ }
+ case -1074395709: {
+ errorText = "ERR_AVI_UNOPENED_SESSION";
+ break;
+ }
+ case -1074395710: {
+ errorText = "ERR_TOO_MANY_PARTICLES";
+ break;
+ }
+ case -1074395711: {
+ errorText = "ERR_NOT_ENOUGH_REGIONS";
+ break;
+ }
+ case -1074395712: {
+ errorText = "ERR_WRONG_REGION_TYPE";
+ break;
+ }
+ case -1074395713: {
+ errorText = "ERR_VALUE_NOT_IN_ENUM";
+ break;
+ }
+ case -1074395714: {
+ errorText = "ERR_INVALID_AXIS_ORIENTATION";
+ break;
+ }
+ case -1074395715: {
+ errorText = "ERR_INVALID_CALIBRATION_UNIT";
+ break;
+ }
+ case -1074395716: {
+ errorText = "ERR_INVALID_SCALING_METHOD";
+ break;
+ }
+ case -1074395717: {
+ errorText = "ERR_INVALID_RANGE";
+ break;
+ }
+ case -1074395718: {
+ errorText = "ERR_LAB_VERSION";
+ break;
+ }
+ case -1074395719: {
+ errorText = "ERR_BAD_ROI_BOX";
+ break;
+ }
+ case -1074395720: {
+ errorText = "ERR_BAD_ROI";
+ break;
+ }
+ case -1074395721: {
+ errorText = "ERR_INVALID_BIT_DEPTH";
+ break;
+ }
+ case -1074395722: {
+ errorText = "ERR_CLASSIFIER_CLASSIFY_IMAGE_WITH_CUSTOM_SESSION";
+ break;
+ }
+ case -1074395723: {
+ errorText = "ERR_CUSTOMDATA_KEY_NOT_FOUND";
+ break;
+ }
+ case -1074395724: {
+ errorText = "ERR_CUSTOMDATA_INVALID_SIZE";
+ break;
+ }
+ case -1074395725: {
+ errorText = "ERR_DATA_VERSION";
+ break;
+ }
+ case -1074395726: {
+ errorText = "ERR_MATCHFACTOR_OBSOLETE";
+ break;
+ }
+ case -1074395727: {
+ errorText = "ERR_UNSUPPORTED_2D_BARCODE_SEARCH_MODE";
+ break;
+ }
+ case -1074395728: {
+ errorText = "ERR_INVALID_2D_BARCODE_SEARCH_MODE";
+ break;
+ }
+ case -1074395754: {
+ errorText = "ERR_TRIG_TIMEOUT";
+ break;
+ }
+ case -1074395756: {
+ errorText = "ERR_DLL_FUNCTION_NOT_FOUND";
+ break;
+ }
+ case -1074395757: {
+ errorText = "ERR_DLL_NOT_FOUND";
+ break;
+ }
+ case -1074395758: {
+ errorText = "ERR_BOARD_NOT_OPEN";
+ break;
+ }
+ case -1074395760: {
+ errorText = "ERR_BOARD_NOT_FOUND";
+ break;
+ }
+ case -1074395762: {
+ errorText = "ERR_INVALID_NIBLACK_DEVIATION_FACTOR";
+ break;
+ }
+ case -1074395763: {
+ errorText = "ERR_INVALID_NORMALIZATION_METHOD";
+ break;
+ }
+ case -1074395766: {
+ errorText = "ERR_DEPRECATED_FUNCTION";
+ break;
+ }
+ case -1074395767: {
+ errorText = "ERR_INVALID_ALIGNMENT";
+ break;
+ }
+ case -1074395768: {
+ errorText = "ERR_INVALID_SCALE";
+ break;
+ }
+ case -1074395769: {
+ errorText = "ERR_INVALID_EDGE_THICKNESS";
+ break;
+ }
+ case -1074395770: {
+ errorText = "ERR_INVALID_INSPECTION_TEMPLATE";
+ break;
+ }
+ case -1074395771: {
+ errorText = "ERR_OPENING_NEWER_INSPECTION_TEMPLATE";
+ break;
+ }
+ case -1074395772: {
+ errorText = "ERR_INVALID_REGISTRATION_METHOD";
+ break;
+ }
+ case -1074395773: {
+ errorText = "ERR_NO_DEST_IMAGE";
+ break;
+ }
+ case -1074395774: {
+ errorText = "ERR_NO_LABEL";
+ break;
+ }
+ case -1074395775: {
+ errorText = "ERR_ROI_HAS_OPEN_CONTOURS";
+ break;
+ }
+ case -1074395776: {
+ errorText = "ERR_INVALID_USE_OF_COMPACT_SESSION_FILE";
+ break;
+ }
+ case -1074395777: {
+ errorText = "ERR_INCOMPATIBLE_CLASSIFIER_TYPES";
+ break;
+ }
+ case -1074395778: {
+ errorText = "ERR_INVALID_KERNEL_SIZE";
+ break;
+ }
+ case -1074395779: {
+ errorText = "ERR_CANNOT_COMPACT_UNTRAINED";
+ break;
+ }
+ case -1074395780: {
+ errorText = "ERR_INVALID_PARTICLE_TYPE";
+ break;
+ }
+ case -1074395781: {
+ errorText = "ERR_CLASSIFIER_INVALID_ENGINE_TYPE";
+ break;
+ }
+ case -1074395782: {
+ errorText = "ERR_DESCRIPTION_TOO_LONG";
+ break;
+ }
+ case -1074395783: {
+ errorText = "ERR_BAD_SAMPLE_INDEX";
+ break;
+ }
+ case -1074395784: {
+ errorText = "ERR_INVALID_LIMITS";
+ break;
+ }
+ case -1074395785: {
+ errorText = "ERR_NO_PARTICLE";
+ break;
+ }
+ case -1074395786: {
+ errorText = "ERR_INVALID_PARTICLE_OPTIONS";
+ break;
+ }
+ case -1074395787: {
+ errorText = "ERR_INVALID_CLASSIFIER_TYPE";
+ break;
+ }
+ case -1074395788: {
+ errorText = "ERR_NO_SAMPLES";
+ break;
+ }
+ case -1074395789: {
+ errorText = "ERR_OPENING_NEWER_CLASSIFIER_SESSION";
+ break;
+ }
+ case -1074395790: {
+ errorText = "ERR_INVALID_DISTANCE_METRIC";
+ break;
+ }
+ case -1074395791: {
+ errorText = "ERR_CLASSIFIER_INVALID_SESSION_TYPE";
+ break;
+ }
+ case -1074395792: {
+ errorText = "ERR_CLASSIFIER_SESSION_NOT_TRAINED";
+ break;
+ }
+ case -1074395793: {
+ errorText = "ERR_INVALID_OPERATION_ON_COMPACT_SESSION_ATTEMPTED";
+ break;
+ }
+ case -1074395794: {
+ errorText = "ERR_K_TOO_HIGH";
+ break;
+ }
+ case -1074395795: {
+ errorText = "ERR_K_TOO_LOW";
+ break;
+ }
+ case -1074395796: {
+ errorText = "ERR_INVALID_KNN_METHOD";
+ break;
+ }
+ case -1074395797: {
+ errorText = "ERR_INVALID_CLASSIFIER_SESSION";
+ break;
+ }
+ case -1074395798: {
+ errorText = "ERR_INVALID_CUSTOM_SAMPLE";
+ break;
+ }
+ case -1074395799: {
+ errorText = "ERR_INTERNAL";
+ break;
+ }
+ case -1074395800: {
+ errorText = "ERR_PROTECTION";
+ break;
+ }
+ case -1074395801: {
+ errorText = "ERR_TOO_MANY_CONTOURS";
+ break;
+ }
+ case -1074395837: {
+ errorText = "ERR_INVALID_COMPRESSION_RATIO";
+ break;
+ }
+ case -1074395840: {
+ errorText = "ERR_BAD_INDEX";
+ break;
+ }
+ case -1074395875: {
+ errorText = "ERR_BARCODE_PHARMACODE";
+ break;
+ }
+ case -1074395876: {
+ errorText = "ERR_UNSUPPORTED_COLOR_MODE";
+ break;
+ }
+ case -1074395877: {
+ errorText = "ERR_COLORMODE_REQUIRES_CHANGECOLORSPACE2";
+ break;
+ }
+ case -1074395878: {
+ errorText = "ERR_PROP_NODE_WRITE_NOT_SUPPORTED";
+ break;
+ }
+ case -1074395879: {
+ errorText = "ERR_BAD_MEASURE";
+ break;
+ }
+ case -1074395880: {
+ errorText = "ERR_PARTICLE";
+ break;
+ }
+ case -1074395920: {
+ errorText = "ERR_NUMBER_CLASS";
+ break;
+ }
+ case -1074395953: {
+ errorText = "ERR_INVALID_WAVELET_TRANSFORM_MODE";
+ break;
+ }
+ case -1074395954: {
+ errorText = "ERR_INVALID_QUANTIZATION_STEP_SIZE";
+ break;
+ }
+ case -1074395955: {
+ errorText = "ERR_INVALID_MAX_WAVELET_TRANSFORM_LEVEL";
+ break;
+ }
+ case -1074395956: {
+ errorText = "ERR_INVALID_QUALITY";
+ break;
+ }
+ case -1074395957: {
+ errorText = "ERR_ARRAY_SIZE_MISMATCH";
+ break;
+ }
+ case -1074395958: {
+ errorText = "ERR_WINDOW_ID";
+ break;
+ }
+ case -1074395959: {
+ errorText = "ERR_CREATE_WINDOW";
+ break;
+ }
+ case -1074395960: {
+ errorText = "ERR_INIT";
+ break;
+ }
+ case -1074395971: {
+ errorText = "ERR_INVALID_OFFSET";
+ break;
+ }
+ case -1074395972: {
+ errorText = "ERR_DIRECTX_ENUMERATE_FILTERS";
+ break;
+ }
+ case -1074395973: {
+ errorText = "ERR_JPEG2000_UNSUPPORTED_MULTIPLE_LAYERS";
+ break;
+ }
+ case -1074395974: {
+ errorText = "ERR_UNSUPPORTED_JPEG2000_COLORSPACE_METHOD";
+ break;
+ }
+ case -1074395975: {
+ errorText = "ERR_AVI_TIMEOUT";
+ break;
+ }
+ case -1074395976: {
+ errorText = "ERR_NUMBER_OF_PALETTE_COLORS";
+ break;
+ }
+ case -1074395977: {
+ errorText = "ERR_AVI_VERSION";
+ break;
+ }
+ case -1074395978: {
+ errorText = "ERR_INVALID_PARTICLE_NUMBER";
+ break;
+ }
+ case -1074395979: {
+ errorText = "ERR_INVALID_PARTICLE_INFO";
+ break;
+ }
+ case -1074395980: {
+ errorText = "ERR_COM_INITIALIZE";
+ break;
+ }
+ case -1074395981: {
+ errorText = "ERR_INSUFFICIENT_BUFFER_SIZE";
+ break;
+ }
+ case -1074395982: {
+ errorText = "ERR_INVALID_FRAMES_PER_SECOND";
+ break;
+ }
+ case -1074395983: {
+ errorText = "ERR_FILE_NO_SPACE";
+ break;
+ }
+ case -1074395984: {
+ errorText = "ERR_FILE_INVALID_DATA_TYPE";
+ break;
+ }
+ case -1074395985: {
+ errorText = "ERR_FILE_OPERATION";
+ break;
+ }
+ case -1074395986: {
+ errorText = "ERR_FILE_FORMAT";
+ break;
+ }
+ case -1074395987: {
+ errorText = "ERR_FILE_EOF";
+ break;
+ }
+ case -1074395988: {
+ errorText = "ERR_FILE_WRITE";
+ break;
+ }
+ case -1074395989: {
+ errorText = "ERR_FILE_READ";
+ break;
+ }
+ case -1074395990: {
+ errorText = "ERR_FILE_GET_INFO";
+ break;
+ }
+ case -1074395991: {
+ errorText = "ERR_FILE_INVALID_TYPE";
+ break;
+ }
+ case -1074395992: {
+ errorText = "ERR_FILE_PERMISSION";
+ break;
+ }
+ case -1074395993: {
+ errorText = "ERR_FILE_IO_ERR";
+ break;
+ }
+ case -1074395994: {
+ errorText = "ERR_FILE_TOO_MANY_OPEN";
+ break;
+ }
+ case -1074395995: {
+ errorText = "ERR_FILE_NOT_FOUND";
+ break;
+ }
+ case -1074395996: {
+ errorText = "ERR_FILE_OPEN";
+ break;
+ }
+ case -1074395997: {
+ errorText = "ERR_FILE_ARGERR";
+ break;
+ }
+ case -1074395998: {
+ errorText = "ERR_FILE_COLOR_TABLE";
+ break;
+ }
+ case -1074395999: {
+ errorText = "ERR_FILE_FILE_TYPE";
+ break;
+ }
+ case -1074396000: {
+ errorText = "ERR_FILE_FILE_HEADER";
+ break;
+ }
+ case -1074396001: {
+ errorText = "ERR_TOO_MANY_AVI_SESSIONS";
+ break;
+ }
+ case -1074396002: {
+ errorText = "ERR_INVALID_LINEGAUGEMETHOD";
+ break;
+ }
+ case -1074396003: {
+ errorText = "ERR_AVI_DATA_EXCEEDS_BUFFER_SIZE";
+ break;
+ }
+ case -1074396004: {
+ errorText = "ERR_DIRECTX_CERTIFICATION_FAILURE";
+ break;
+ }
+ case -1074396005: {
+ errorText = "ERR_INVALID_AVI_SESSION";
+ break;
+ }
+ case -1074396006: {
+ errorText = "ERR_DIRECTX_UNKNOWN_COMPRESSION_FILTER";
+ break;
+ }
+ case -1074396007: {
+ errorText = "ERR_DIRECTX_INCOMPATIBLE_COMPRESSION_FILTER";
+ break;
+ }
+ case -1074396008: {
+ errorText = "ERR_DIRECTX_NO_FILTER";
+ break;
+ }
+ case -1074396009: {
+ errorText = "ERR_DIRECTX";
+ break;
+ }
+ case -1074396010: {
+ errorText = "ERR_INVALID_FRAME_NUMBER";
+ break;
+ }
+ case -1074396011: {
+ errorText = "ERR_RPC_BIND";
+ break;
+ }
+ case -1074396012: {
+ errorText = "ERR_RPC_EXECUTE";
+ break;
+ }
+ case -1074396013: {
+ errorText = "ERR_INVALID_VIDEO_MODE";
+ break;
+ }
+ case -1074396014: {
+ errorText = "ERR_INVALID_VIDEO_BLIT";
+ break;
+ }
+ case -1074396015: {
+ errorText = "ERR_RPC_EXECUTE_IVB";
+ break;
+ }
+ case -1074396016: {
+ errorText = "ERR_NO_VIDEO_DRIVER";
+ break;
+ }
+ case -1074396017: {
+ errorText = "ERR_OPENING_NEWER_AIM_GRADING_DATA";
+ break;
+ }
+ case -1074396018: {
+ errorText = "ERR_INVALID_EDGE_POLARITY_SEARCH_MODE";
+ break;
+ }
+ case -1074396019: {
+ errorText = "ERR_INVALID_THRESHOLD_PERCENTAGE";
+ break;
+ }
+ case -1074396020: {
+ errorText = "ERR_INVALID_GRADING_MODE";
+ break;
+ }
+ case -1074396021: {
+ errorText = "ERR_INVALID_KERNEL_SIZE_FOR_EDGE_DETECTION";
+ break;
+ }
+ case -1074396022: {
+ errorText = "ERR_INVALID_SEARCH_MODE_FOR_STRAIGHT_EDGE";
+ break;
+ }
+ case -1074396023: {
+ errorText = "ERR_INVALID_ANGLE_TOL_FOR_STRAIGHT_EDGE";
+ break;
+ }
+ case -1074396024: {
+ errorText = "ERR_INVALID_MIN_COVERAGE_FOR_STRAIGHT_EDGE";
+ break;
+ }
+ case -1074396025: {
+ errorText = "ERR_INVALID_ANGLE_RANGE_FOR_STRAIGHT_EDGE";
+ break;
+ }
+ case -1074396026: {
+ errorText = "ERR_INVALID_PROCESS_TYPE_FOR_EDGE_DETECTION";
+ break;
+ }
+ case -1074396032: {
+ errorText = "ERR_TEMPLATEDESCRIPTOR_ROTATION_SEARCHSTRATEGY";
+ break;
+ }
+ case -1074396033: {
+ errorText = "ERR_TEMPLATEDESCRIPTOR_LEARNSETUPDATA";
+ break;
+ }
+ case -1074396034: {
+ errorText = "ERR_TEMPLATEIMAGE_EDGEINFO";
+ break;
+ }
+ case -1074396035: {
+ errorText = "ERR_TEMPLATEIMAGE_NOCIRCLE";
+ break;
+ }
+ case -1074396036: {
+ errorText = "ERR_INVALID_SKELETONMODE";
+ break;
+ }
+ case -1074396037: {
+ errorText = "ERR_TIMEOUT";
+ break;
+ }
+ case -1074396038: {
+ errorText = "ERR_FIND_COORDSYS_MORE_THAN_ONE_EDGE";
+ break;
+ }
+ case -1074396039: {
+ errorText = "ERR_IO_ERROR";
+ break;
+ }
+ case -1074396040: {
+ errorText = "ERR_DRIVER";
+ break;
+ }
+ case -1074396041: {
+ errorText = "ERR_INVALID_2D_BARCODE_TYPE";
+ break;
+ }
+ case -1074396042: {
+ errorText = "ERR_INVALID_2D_BARCODE_CONTRAST";
+ break;
+ }
+ case -1074396043: {
+ errorText = "ERR_INVALID_2D_BARCODE_CELL_SHAPE";
+ break;
+ }
+ case -1074396044: {
+ errorText = "ERR_INVALID_2D_BARCODE_SHAPE";
+ break;
+ }
+ case -1074396045: {
+ errorText = "ERR_INVALID_2D_BARCODE_SUBTYPE";
+ break;
+ }
+ case -1074396046: {
+ errorText = "ERR_INVALID_2D_BARCODE_CONTRAST_FOR_ROI";
+ break;
+ }
+ case -1074396047: {
+ errorText = "ERR_INVALID_LINEAR_AVERAGE_MODE";
+ break;
+ }
+ case -1074396048: {
+ errorText = "ERR_INVALID_CELL_SAMPLE_SIZE";
+ break;
+ }
+ case -1074396049: {
+ errorText = "ERR_INVALID_MATRIX_POLARITY";
+ break;
+ }
+ case -1074396050: {
+ errorText = "ERR_INVALID_ECC_TYPE";
+ break;
+ }
+ case -1074396051: {
+ errorText = "ERR_INVALID_CELL_FILTER_MODE";
+ break;
+ }
+ case -1074396052: {
+ errorText = "ERR_INVALID_DEMODULATION_MODE";
+ break;
+ }
+ case -1074396053: {
+ errorText = "ERR_INVALID_BORDER_INTEGRITY";
+ break;
+ }
+ case -1074396054: {
+ errorText = "ERR_INVALID_CELL_FILL_TYPE";
+ break;
+ }
+ case -1074396055: {
+ errorText = "ERR_INVALID_ASPECT_RATIO";
+ break;
+ }
+ case -1074396056: {
+ errorText = "ERR_INVALID_MATRIX_MIRROR_MODE";
+ break;
+ }
+ case -1074396057: {
+ errorText = "ERR_INVALID_SEARCH_VECTOR_WIDTH";
+ break;
+ }
+ case -1074396058: {
+ errorText = "ERR_INVALID_ROTATION_MODE";
+ break;
+ }
+ case -1074396059: {
+ errorText = "ERR_INVALID_MAX_ITERATIONS";
+ break;
+ }
+ case -1074396060: {
+ errorText = "ERR_JPEG2000_LOSSLESS_WITH_FLOATING_POINT";
+ break;
+ }
+ case -1074396061: {
+ errorText = "ERR_INVALID_WINDOW_SIZE";
+ break;
+ }
+ case -1074396062: {
+ errorText = "ERR_INVALID_TOLERANCE";
+ break;
+ }
+ case -1074396063: {
+ errorText = "ERR_EXTERNAL_ALIGNMENT";
+ break;
+ }
+ case -1074396064: {
+ errorText = "ERR_EXTERNAL_NOT_SUPPORTED";
+ break;
+ }
+ case -1074396065: {
+ errorText = "ERR_CANT_RESIZE_EXTERNAL";
+ break;
+ }
+ case -1074396066: {
+ errorText = "ERR_INVALID_POINTSYMBOL";
+ break;
+ }
+ case -1074396067: {
+ errorText = "ERR_IMAGES_NOT_DIFF";
+ break;
+ }
+ case -1074396068: {
+ errorText = "ERR_INVALID_ACTION";
+ break;
+ }
+ case -1074396069: {
+ errorText = "ERR_INVALID_COLOR_MODE";
+ break;
+ }
+ case -1074396070: {
+ errorText = "ERR_INVALID_FUNCTION";
+ break;
+ }
+ case -1074396071: {
+ errorText = "ERR_INVALID_SCAN_DIRECTION";
+ break;
+ }
+ case -1074396072: {
+ errorText = "ERR_INVALID_BORDER";
+ break;
+ }
+ case -1074396073: {
+ errorText = "ERR_MASK_OUTSIDE_IMAGE";
+ break;
+ }
+ case -1074396074: {
+ errorText = "ERR_INCOMP_SIZE";
+ break;
+ }
+ case -1074396075: {
+ errorText = "ERR_COORD_SYS_SECOND_AXIS";
+ break;
+ }
+ case -1074396076: {
+ errorText = "ERR_COORD_SYS_FIRST_AXIS";
+ break;
+ }
+ case -1074396077: {
+ errorText = "ERR_INCOMP_TYPE";
+ break;
+ }
+ case -1074396079: {
+ errorText = "ERR_INVALID_METAFILE_HANDLE";
+ break;
+ }
+ case -1074396080: {
+ errorText = "ERR_INVALID_IMAGE_TYPE";
+ break;
+ }
+ case -1074396081: {
+ errorText = "ERR_BAD_PASSWORD";
+ break;
+ }
+ case -1074396082: {
+ errorText = "ERR_PALETTE_NOT_SUPPORTED";
+ break;
+ }
+ case -1074396083: {
+ errorText = "ERR_ROLLBACK_TIMEOUT";
+ break;
+ }
+ case -1074396084: {
+ errorText = "ERR_ROLLBACK_DELETE_TIMER";
+ break;
+ }
+ case -1074396085: {
+ errorText = "ERR_ROLLBACK_INIT_TIMER";
+ break;
+ }
+ case -1074396086: {
+ errorText = "ERR_ROLLBACK_START_TIMER";
+ break;
+ }
+ case -1074396087: {
+ errorText = "ERR_ROLLBACK_STOP_TIMER";
+ break;
+ }
+ case -1074396088: {
+ errorText = "ERR_ROLLBACK_RESIZE";
+ break;
+ }
+ case -1074396089: {
+ errorText = "ERR_ROLLBACK_RESOURCE_REINITIALIZE";
+ break;
+ }
+ case -1074396090: {
+ errorText = "ERR_ROLLBACK_RESOURCE_ENABLED";
+ break;
+ }
+ case -1074396091: {
+ errorText = "ERR_ROLLBACK_RESOURCE_UNINITIALIZED_ENABLE";
+ break;
+ }
+ case -1074396092: {
+ errorText = "ERR_ROLLBACK_RESOURCE_NON_EMPTY_INITIALIZE";
+ break;
+ }
+ case -1074396093: {
+ errorText = "ERR_ROLLBACK_RESOURCE_LOCKED";
+ break;
+ }
+ case -1074396094: {
+ errorText = "ERR_ROLLBACK_RESOURCE_CANNOT_UNLOCK";
+ break;
+ }
+ case -1074396095: {
+ errorText = "ERR_CALIBRATION_DUPLICATE_REFERENCE_POINT";
+ break;
+ }
+ case -1074396096: {
+ errorText = "ERR_NOT_AN_OBJECT";
+ break;
+ }
+ case -1074396097: {
+ errorText = "ERR_INVALID_PARTICLE_PARAMETER_VALUE";
+ break;
+ }
+ case -1074396098: {
+ errorText = "ERR_RESERVED_MUST_BE_nullptr";
+ break;
+ }
+ case -1074396099: {
+ errorText = "ERR_CALIBRATION_INFO_SIMPLE_TRANSFORM";
+ break;
+ }
+ case -1074396100: {
+ errorText = "ERR_CALIBRATION_INFO_PERSPECTIVE_PROJECTION";
+ break;
+ }
+ case -1074396101: {
+ errorText = "ERR_CALIBRATION_INFO_MICRO_PLANE";
+ break;
+ }
+ case -1074396102: {
+ errorText = "ERR_CALIBRATION_INFO_6";
+ break;
+ }
+ case -1074396103: {
+ errorText = "ERR_CALIBRATION_INFO_5";
+ break;
+ }
+ case -1074396104: {
+ errorText = "ERR_CALIBRATION_INFO_4";
+ break;
+ }
+ case -1074396105: {
+ errorText = "ERR_CALIBRATION_INFO_3";
+ break;
+ }
+ case -1074396106: {
+ errorText = "ERR_CALIBRATION_INFO_2";
+ break;
+ }
+ case -1074396107: {
+ errorText = "ERR_CALIBRATION_INFO_1";
+ break;
+ }
+ case -1074396108: {
+ errorText = "ERR_CALIBRATION_ERRORMAP";
+ break;
+ }
+ case -1074396109: {
+ errorText = "ERR_CALIBRATION_INVALID_SCALING_FACTOR";
+ break;
+ }
+ case -1074396110: {
+ errorText = "ERR_CALIBRATION_INFO_VERSION";
+ break;
+ }
+ case -1074396111: {
+ errorText = "ERR_CALIBRATION_FAILED_TO_FIND_GRID";
+ break;
+ }
+ case -1074396112: {
+ errorText = "ERR_INCOMP_MATRIX_SIZE";
+ break;
+ }
+ case -1074396113: {
+ errorText = "ERR_CALIBRATION_IMAGE_UNCALIBRATED";
+ break;
+ }
+ case -1074396114: {
+ errorText = "ERR_CALIBRATION_INVALID_ROI";
+ break;
+ }
+ case -1074396115: {
+ errorText = "ERR_CALIBRATION_IMAGE_CORRECTED";
+ break;
+ }
+ case -1074396116: {
+ errorText = "ERR_CALIBRATION_INSF_POINTS";
+ break;
+ }
+ case -1074396117: {
+ errorText = "ERR_MATRIX_SIZE";
+ break;
+ }
+ case -1074396118: {
+ errorText = "ERR_INVALID_STEP_SIZE";
+ break;
+ }
+ case -1074396119: {
+ errorText = "ERR_CUSTOMDATA_INVALID_KEY";
+ break;
+ }
+ case -1074396120: {
+ errorText = "ERR_NOT_IMAGE";
+ break;
+ }
+ case -1074396121: {
+ errorText = "ERR_SATURATION_THRESHOLD_OUT_OF_RANGE";
+ break;
+ }
+ case -1074396122: {
+ errorText = "ERR_DRAWTEXT_COLOR_MUST_BE_GRAYSCALE";
+ break;
+ }
+ case -1074396123: {
+ errorText = "ERR_INVALID_CALIBRATION_MODE";
+ break;
+ }
+ case -1074396124: {
+ errorText = "ERR_INVALID_CALIBRATION_ROI_MODE";
+ break;
+ }
+ case -1074396125: {
+ errorText = "ERR_INVALID_CONTRAST_THRESHOLD";
+ break;
+ }
+ case -1074396126: {
+ errorText = "ERR_ROLLBACK_RESOURCE_CONFLICT_1";
+ break;
+ }
+ case -1074396127: {
+ errorText = "ERR_ROLLBACK_RESOURCE_CONFLICT_2";
+ break;
+ }
+ case -1074396128: {
+ errorText = "ERR_ROLLBACK_RESOURCE_CONFLICT_3";
+ break;
+ }
+ case -1074396129: {
+ errorText = "ERR_ROLLBACK_UNBOUNDED_INTERFACE";
+ break;
+ }
+ case -1074396130: {
+ errorText = "ERR_NOT_RECT_OR_ROTATED_RECT";
+ break;
+ }
+ case -1074396132: {
+ errorText = "ERR_MASK_NOT_TEMPLATE_SIZE";
+ break;
+ }
+ case -1074396133: {
+ errorText = "ERR_THREAD_COULD_NOT_INITIALIZE";
+ break;
+ }
+ case -1074396134: {
+ errorText = "ERR_THREAD_INITIALIZING";
+ break;
+ }
+ case -1074396135: {
+ errorText = "ERR_INVALID_BUTTON_LABEL";
+ break;
+ }
+ case -1074396136: {
+ errorText = "ERR_DIRECTX_INVALID_FILTER_QUALITY";
+ break;
+ }
+ case -1074396137: {
+ errorText = "ERR_DIRECTX_DLL_NOT_FOUND";
+ break;
+ }
+ case -1074396138: {
+ errorText = "ERR_ROLLBACK_NOT_SUPPORTED";
+ break;
+ }
+ case -1074396139: {
+ errorText = "ERR_ROLLBACK_RESOURCE_OUT_OF_MEMORY";
+ break;
+ }
+ case -1074396140: {
+ errorText = "ERR_BARCODE_CODE128_SET";
+ break;
+ }
+ case -1074396141: {
+ errorText = "ERR_BARCODE_CODE128_FNC";
+ break;
+ }
+ case -1074396142: {
+ errorText = "ERR_BARCODE_INVALID";
+ break;
+ }
+ case -1074396143: {
+ errorText = "ERR_BARCODE_TYPE";
+ break;
+ }
+ case -1074396144: {
+ errorText = "ERR_BARCODE_CODE93_SHIFT";
+ break;
+ }
+ case -1074396145: {
+ errorText = "ERR_BARCODE_UPCA";
+ break;
+ }
+ case -1074396146: {
+ errorText = "ERR_BARCODE_MSI";
+ break;
+ }
+ case -1074396147: {
+ errorText = "ERR_BARCODE_I25";
+ break;
+ }
+ case -1074396148: {
+ errorText = "ERR_BARCODE_EAN13";
+ break;
+ }
+ case -1074396149: {
+ errorText = "ERR_BARCODE_EAN8";
+ break;
+ }
+ case -1074396150: {
+ errorText = "ERR_BARCODE_CODE128";
+ break;
+ }
+ case -1074396151: {
+ errorText = "ERR_BARCODE_CODE93";
+ break;
+ }
+ case -1074396152: {
+ errorText = "ERR_BARCODE_CODE39";
+ break;
+ }
+ case -1074396153: {
+ errorText = "ERR_BARCODE_CODABAR";
+ break;
+ }
+ case -1074396154: {
+ errorText = "ERR_IMAGE_TOO_SMALL";
+ break;
+ }
+ case -1074396155: {
+ errorText = "ERR_UNINIT";
+ break;
+ }
+ case -1074396156: {
+ errorText = "ERR_NEED_FULL_VERSION";
+ break;
+ }
+ case -1074396157: {
+ errorText = "ERR_UNREGISTERED";
+ break;
+ }
+ case -1074396158: {
+ errorText = "ERR_MEMORY_ERROR";
+ break;
+ }
+ case -1074396159: {
+ errorText = "ERR_OUT_OF_MEMORY";
+ break;
+ }
+ case -1074396160: {
+ errorText = "ERR_SYSTEM_ERROR";
+ break;
+ }
+ case 0: {
+ errorText = "ERR_SUCCESS";
+ break;
+ }
+ // end National Instruments defined errors
+
+ // begin BAE defined errors
+ case ERR_VISION_GENERAL_ERROR: {
+ errorText = "ERR_VISION_GENERAL_ERROR";
+ break;
+ }
+ case ERR_COLOR_NOT_FOUND: {
+ errorText = "ERR_COLOR_NOT_FOUND";
+ break;
+ }
+ case ERR_PARTICLE_TOO_SMALL: {
+ errorText = "ERR_PARTICLE_TOO_SMALL";
+ break;
+ }
+ case ERR_CAMERA_FAILURE: {
+ errorText = "ERR_CAMERA_FAILURE";
+ break;
+ }
+ case ERR_CAMERA_SOCKET_CREATE_FAILED: {
+ errorText = "ERR_CAMERA_SOCKET_CREATE_FAILED";
+ break;
+ }
+ case ERR_CAMERA_CONNECT_FAILED: {
+ errorText = "ERR_CAMERA_CONNECT_FAILED";
+ break;
+ }
+ case ERR_CAMERA_STALE_IMAGE: {
+ errorText = "ERR_CAMERA_STALE_IMAGE";
+ break;
+ }
+ case ERR_CAMERA_NOT_INITIALIZED: {
+ errorText = "ERR_CAMERA_NOT_INITIALIZED";
+ break;
+ }
+ case ERR_CAMERA_NO_BUFFER_AVAILABLE: {
+ errorText = "ERR_CAMERA_NO_BUFFER_AVAILABLE";
+ break;
+ }
+ case ERR_CAMERA_HEADER_ERROR: {
+ errorText = "ERR_CAMERA_HEADER_ERROR";
+ break;
+ }
+ case ERR_CAMERA_BLOCKING_TIMEOUT: {
+ errorText = "ERR_CAMERA_BLOCKING_TIMEOUT";
+ break;
+ }
+ case ERR_CAMERA_AUTHORIZATION_FAILED: {
+ errorText = "ERR_CAMERA_AUTHORIZATION_FAILED";
+ break;
+ }
+ case ERR_CAMERA_TASK_SPAWN_FAILED: {
+ errorText = "ERR_CAMERA_TASK_SPAWN_FAILED";
+ break;
+ }
+ case ERR_CAMERA_TASK_INPUT_OUT_OF_RANGE: {
+ errorText = "ERR_CAMERA_TASK_INPUT_OUT_OF_RANGE";
+ break;
+ }
+ case ERR_CAMERA_COMMAND_FAILURE: {
+ errorText = "ERR_CAMERA_COMMAND_FAILURE";
+ break;
+ }
+ }
+
+ return errorText;
+}
diff --git a/wpilibc/Athena/src/Vision/HSLImage.cpp b/wpilibc/Athena/src/Vision/HSLImage.cpp
new file mode 100644
index 0000000..5b114c4
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/HSLImage.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Vision/HSLImage.h"
+
+/**
+ * Create a new image that uses the Hue, Saturation, and Luminance planes.
+ */
+HSLImage::HSLImage() : ColorImage(IMAQ_IMAGE_HSL) {}
+
+/**
+ * Create a new image by loading a file.
+ * @param fileName The path of the file to load.
+ */
+HSLImage::HSLImage(const char *fileName) : ColorImage(IMAQ_IMAGE_HSL) {
+ int success = imaqReadFile(m_imaqImage, fileName, nullptr, nullptr);
+ wpi_setImaqErrorWithContext(success, "Imaq ReadFile error");
+}
diff --git a/wpilibc/Athena/src/Vision/ImageBase.cpp b/wpilibc/Athena/src/Vision/ImageBase.cpp
new file mode 100644
index 0000000..f35234a
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/ImageBase.cpp
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Vision/ImageBase.h"
+#include "nivision.h"
+
+/**
+ * Create a new instance of an ImageBase.
+ * Imagebase is the base of all the other image classes. The constructor
+ * creates any type of image and stores the pointer to it in the class.
+ * @param type The type of image to create
+ */
+ImageBase::ImageBase(ImageType type) {
+ m_imaqImage = imaqCreateImage(type, DEFAULT_BORDER_SIZE);
+}
+
+/**
+ * Frees memory associated with an ImageBase.
+ * Destructor frees the imaq image allocated with the class.
+ */
+ImageBase::~ImageBase() {
+ if (m_imaqImage) imaqDispose(m_imaqImage);
+}
+
+/**
+ * Writes an image to a file with the given filename.
+ * Write the image to a file in the flash on the cRIO.
+ * @param fileName The name of the file to write
+ */
+void ImageBase::Write(const char *fileName) {
+ int success = imaqWriteFile(m_imaqImage, fileName, nullptr);
+ wpi_setImaqErrorWithContext(success, "Imaq Image writeFile error");
+}
+
+/**
+ * Gets the height of an image.
+ * @return The height of the image in pixels.
+ */
+int ImageBase::GetHeight() {
+ int height;
+ imaqGetImageSize(m_imaqImage, nullptr, &height);
+ return height;
+}
+
+/**
+ * Gets the width of an image.
+ * @return The width of the image in pixels.
+ */
+int ImageBase::GetWidth() {
+ int width;
+ imaqGetImageSize(m_imaqImage, &width, nullptr);
+ return width;
+}
+
+/**
+ * Access the internal IMAQ Image data structure.
+ *
+ * @return A pointer to the internal IMAQ Image data structure.
+ */
+Image *ImageBase::GetImaqImage() { return m_imaqImage; }
diff --git a/wpilibc/Athena/src/Vision/MonoImage.cpp b/wpilibc/Athena/src/Vision/MonoImage.cpp
new file mode 100644
index 0000000..90703c0
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/MonoImage.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Vision/MonoImage.h"
+#include "nivision.h"
+
+using namespace std;
+
+MonoImage::MonoImage() : ImageBase(IMAQ_IMAGE_U8) {}
+
+/**
+ * Look for ellipses in an image.
+ * Given some input parameters, look for any number of ellipses in an image.
+ * @param ellipseDescriptor Ellipse descriptor
+ * @param curveOptions Curve options
+ * @param shapeDetectionOptions Shape detection options
+ * @param roi Region of Interest
+ * @returns a vector of EllipseMatch structures (0 length vector on no match)
+ */
+vector<EllipseMatch> *MonoImage::DetectEllipses(
+ EllipseDescriptor *ellipseDescriptor, CurveOptions *curveOptions,
+ ShapeDetectionOptions *shapeDetectionOptions, ROI *roi) {
+ int numberOfMatches;
+ EllipseMatch *e =
+ imaqDetectEllipses(m_imaqImage, ellipseDescriptor, curveOptions,
+ shapeDetectionOptions, roi, &numberOfMatches);
+ auto ellipses = new vector<EllipseMatch>;
+ if (e == nullptr) {
+ return ellipses;
+ }
+ for (int i = 0; i < numberOfMatches; i++) {
+ ellipses->push_back(e[i]);
+ }
+ imaqDispose(e);
+ return ellipses;
+}
+
+vector<EllipseMatch> *MonoImage::DetectEllipses(
+ EllipseDescriptor *ellipseDescriptor) {
+ vector<EllipseMatch> *ellipses =
+ DetectEllipses(ellipseDescriptor, nullptr, nullptr, nullptr);
+ return ellipses;
+}
diff --git a/wpilibc/Athena/src/Vision/RGBImage.cpp b/wpilibc/Athena/src/Vision/RGBImage.cpp
new file mode 100644
index 0000000..5469122
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/RGBImage.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Vision/RGBImage.h"
+
+/**
+ * Create a new image that uses Red, Green, and Blue planes.
+ */
+RGBImage::RGBImage() : ColorImage(IMAQ_IMAGE_RGB) {}
+
+/**
+ * Create a new image by loading a file.
+ * @param fileName The path of the file to load.
+ */
+RGBImage::RGBImage(const char *fileName) : ColorImage(IMAQ_IMAGE_RGB) {
+ int success = imaqReadFile(m_imaqImage, fileName, nullptr, nullptr);
+ wpi_setImaqErrorWithContext(success, "Imaq ReadFile error");
+}
diff --git a/wpilibc/Athena/src/Vision/Threshold.cpp b/wpilibc/Athena/src/Vision/Threshold.cpp
new file mode 100644
index 0000000..2e17243
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/Threshold.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Vision/Threshold.h"
+
+Threshold::Threshold(int new_plane1Low, int new_plane1High, int new_plane2Low,
+ int new_plane2High, int new_plane3Low,
+ int new_plane3High) {
+ plane1Low = new_plane1Low;
+ plane1High = new_plane1High;
+ plane2Low = new_plane2Low;
+ plane2High = new_plane2High;
+ plane3Low = new_plane3Low;
+ plane3High = new_plane3High;
+}
diff --git a/wpilibc/Athena/src/Vision/VisionAPI.cpp b/wpilibc/Athena/src/Vision/VisionAPI.cpp
new file mode 100644
index 0000000..163721d
--- /dev/null
+++ b/wpilibc/Athena/src/Vision/VisionAPI.cpp
@@ -0,0 +1,821 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include <stdlib.h>
+#include <stdarg.h>
+
+#include "Vision/BaeUtilities.h"
+#include "Vision/FrcError.h"
+#include "Vision/VisionAPI.h"
+
+int VisionAPI_debugFlag = 1;
+#define DPRINTF \
+ if (VisionAPI_debugFlag) dprintf
+
+/** @file
+ * Image Management functions
+ */
+
+/**
+* @brief Create an image object
+* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_COMPLEX,
+* IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64
+* The border size is defaulted to 3 so that convolutional algorithms work at the
+* edges.
+* When you are finished with the created image, dispose of it by calling
+* frcDispose().
+* To get extended error information, call GetLastError().
+*
+* @param type Type of image to create
+* @return Image* On success, this function returns the created image. On
+* failure, it returns nullptr.
+*/
+Image* frcCreateImage(ImageType type) {
+ return imaqCreateImage(type, DEFAULT_BORDER_SIZE);
+}
+
+/**
+* @brief Dispose of one object. Supports any object created on the heap.
+*
+* @param object object to dispose of
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcDispose(void* object) { return imaqDispose(object); }
+/**
+* @brief Dispose of a list of objects. Supports any object created on the heap.
+*
+* @param functionName The name of the function
+* @param ... A list of pointers to structures that need to be disposed of.
+* The last pointer in the list should always be set to nullptr.
+*
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcDispose(const char* functionName, ...) /* Variable argument list */
+{
+ va_list disposalPtrList; /* Input argument list */
+ void* disposalPtr; /* For iteration */
+ int success, returnValue = 1;
+
+ va_start(disposalPtrList, functionName); /* start of variable list */
+ disposalPtr = va_arg(disposalPtrList, void*);
+ while (disposalPtr != nullptr) {
+ success = imaqDispose(disposalPtr);
+ if (!success) {
+ returnValue = 0;
+ }
+ disposalPtr = va_arg(disposalPtrList, void*);
+ }
+ va_end(disposalPtrList);
+ return returnValue;
+}
+
+/**
+* @brief Copy an image object.
+* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_RGB,
+* IMAQ_IMAGE_HSL.
+*
+* @param dest Copy of image. On failure, dest is nullptr. Must have already been
+* created using frcCreateImage().
+* When you are finished with the created image, dispose of it by calling
+* frcDispose().
+* @param source Image to copy
+*
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcCopyImage(Image* dest, const Image* source) {
+ return imaqDuplicate(dest, source);
+}
+
+/**
+* @brief Crop image without changing the scale.
+* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_RGB,
+* IMAQ_IMAGE_HSL.
+*
+* @param dest Modified image
+* @param source Image to crop
+* @param rect region to process, or IMAQ_NO_RECT
+*
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcCrop(Image* dest, const Image* source, Rect rect) {
+ return imaqScale(dest, source, 1, 1, IMAQ_SCALE_LARGER, rect);
+}
+
+/**
+* @brief Scales the entire image larger or smaller.
+* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_RGB,
+* IMAQ_IMAGE_HSL.
+*
+* @param dest Modified image
+* @param source Image to scale
+* @param xScale the horizontal reduction ratio
+* @param yScale the vertical reduction ratio
+* @param scaleMode IMAQ_SCALE_LARGER or IMAQ_SCALE_SMALLER
+*
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcScale(Image* dest, const Image* source, int xScale, int yScale,
+ ScalingMode scaleMode) {
+ Rect rect = IMAQ_NO_RECT;
+ return imaqScale(dest, source, xScale, yScale, scaleMode, rect);
+}
+
+/**
+ * @brief Creates image object from the information in a file. The file can be
+ * in one of the following formats:
+ * PNG, JPEG, JPEG2000, TIFF, AIPD, or BMP.
+ * Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_COMPLEX,
+ * IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64.
+ *
+ * @param image Image read in
+ * @param fileName File to read. Cannot be nullptr
+ *
+ * @return On success: 1. On failure: 0. To get extended error information, call
+ * GetLastError().
+ */
+int frcReadImage(Image* image, const char* fileName) {
+ return imaqReadFile(image, fileName, nullptr, nullptr);
+}
+
+/**
+* @brief Write image to a file.
+* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL, IMAQ_IMAGE_COMPLEX,
+* IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64.
+*
+* The file type is determined by the extension, as follows:
+*
+* Extension File Type
+* aipd or .apd AIPD
+* .bmp BMP
+* .jpg or .jpeg JPEG
+* .jp2 JPEG2000
+* .png PNG
+* .tif or .tiff TIFF
+*
+*
+* The following are the supported image types for each file type:
+*
+* File Types Image Types
+* AIPD all image types
+* BMP, JPEG 8-bit, RGB
+* PNG, TIFF, JPEG2000 8-bit, 16-bit, RGB, RGBU64
+*
+* @param image Image to write
+* @param fileName File to read. Cannot be nullptr. The extension determines the
+* file format that is written.
+*
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcWriteImage(const Image* image, const char* fileName) {
+ RGBValue* colorTable = nullptr;
+ return imaqWriteFile(image, fileName, colorTable);
+}
+
+/* Measure Intensity functions */
+
+/**
+* @brief Measures the pixel intensities in a rectangle of an image.
+* Outputs intensity based statistics about an image such as Max, Min, Mean and
+* Std Dev of pixel value.
+* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL.
+*
+* Parameter Discussion :
+* Relevant parameters of the HistogramReport include:
+* min, max, mean and stdDev
+* min/max Setting both min and max to 0 causes the function to set
+* min to 0
+* and the max to 255 for 8-bit images and to the actual
+* minimum value and
+* maximum value of the image for all other image types.
+* maxSetting both min and max to 0 causes the function to set max
+* to 255
+* for 8-bit images and to the actual maximum value of the
+* image for
+* all other image types.
+*
+* @param image Image whose histogram the function calculates.
+* @param numClasses The number of classes into which the function separates the
+* pixels.
+* Determines the number of elements in the histogram array returned
+* @param min The minimum pixel value to consider for the histogram.
+* The function does not count pixels with values less than min.
+* @param max The maximum pixel value to consider for the histogram.
+* The function does not count pixels with values greater than max.
+* @param rect Region of interest in the image. If not included, the entire image
+* is used.
+* @return On success, this function returns a report describing the pixel value
+* classification.
+* When you are finished with the report, dispose of it by calling frcDispose().
+* On failure, this function returns nullptr. To get extended error information,
+* call GetLastError().
+*
+*/
+HistogramReport* frcHistogram(const Image* image, int numClasses, float min,
+ float max) {
+ Rect rect = IMAQ_NO_RECT;
+ return frcHistogram(image, numClasses, min, max, rect);
+}
+HistogramReport* frcHistogram(const Image* image, int numClasses, float min,
+ float max, Rect rect) {
+ int success;
+ int fillValue = 1;
+
+ /* create the region of interest */
+ ROI* pRoi = imaqCreateROI();
+ success = imaqAddRectContour(pRoi, rect);
+ if (!success) {
+ GetLastVisionError();
+ return nullptr;
+ }
+
+ /* make a mask from the ROI */
+ Image* pMask = frcCreateImage(IMAQ_IMAGE_U8);
+ success = imaqROIToMask(pMask, pRoi, fillValue, nullptr, nullptr);
+ if (!success) {
+ GetLastVisionError();
+ frcDispose(__FUNCTION__, pRoi, nullptr);
+ return nullptr;
+ }
+
+ /* get a histogram report */
+ HistogramReport* pHr = nullptr;
+ pHr = imaqHistogram(image, numClasses, min, max, pMask);
+
+ /* clean up */
+ frcDispose(__FUNCTION__, pRoi, pMask, nullptr);
+
+ return pHr;
+}
+
+/**
+* @brief Calculates the histogram, or pixel distribution, of a color image.
+* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL.
+*
+* @param image Image whose histogram the function calculates.
+* @param numClasses The number of classes into which the function separates the
+* pixels.
+* Determines the number of elements in the histogram array returned
+* @param mode The color space in which to perform the histogram. Possible values
+* include IMAQ_RGB and IMAQ_HSL.
+* @param mask An optional mask image. This image must be an IMAQ_IMAGE_U8 image.
+* The function calculates the histogram using only those pixels in the image
+* whose
+* corresponding pixels in the mask are non-zero. Set this parameter to nullptr to
+* calculate
+* the histogram of the entire image, or use the simplified call.
+*
+* @return On success, this function returns a report describing the
+* classification
+* of each plane in a HistogramReport.
+* When you are finished with the report, dispose of it by calling frcDispose().
+* On failure, this function returns nullptr.
+* To get extended error information, call imaqGetLastError().
+*/
+ColorHistogramReport* frcColorHistogram(const Image* image, int numClasses,
+ ColorMode mode) {
+ return frcColorHistogram(image, numClasses, mode, nullptr);
+}
+
+ColorHistogramReport* frcColorHistogram(const Image* image, int numClasses,
+ ColorMode mode, Image* mask) {
+ return imaqColorHistogram2((Image*)image, numClasses, mode, nullptr, mask);
+}
+
+/**
+* @brief Measures the pixel intensities in a rectangle of an image.
+* Outputs intensity based statistics about an image such as Max, Min, Mean and
+* Std Dev of pixel value.
+* Supports IMAQ_IMAGE_U8 (grayscale) IMAQ_IMAGE_RGB (color) IMAQ_IMAGE_HSL
+* (color-HSL).
+*
+* @param image The image whose pixel value the function queries
+* @param pixel The coordinates of the pixel that the function queries
+* @param value On return, the value of the specified image pixel. This parameter
+* cannot be nullptr.
+* This data structure contains either grayscale, RGB, HSL, Complex or
+* RGBU64Value depending on the type of image.
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcGetPixelValue(const Image* image, Point pixel, PixelValue* value) {
+ return imaqGetPixel(image, pixel, value);
+}
+
+/* Particle Analysis functions */
+
+/**
+* @brief Filters particles out of an image based on their measurements.
+* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL.
+*
+* @param dest The destination image. If dest is used, it must be the same size
+* as the Source image. It will contain only the filtered particles.
+* @param source The image containing the particles to filter.
+* @param criteria An array of criteria to apply to the particles in the source
+* image. This array cannot be nullptr.
+* See the NIVisionCVI.chm help file for definitions of criteria.
+* @param criteriaCount The number of elements in the criteria array.
+* @param options Binary filter options, including rejectMatches, rejectBorder,
+* and connectivity8.
+* @param rect Area of image to filter. If omitted, the default is entire image.
+* @param numParticles On return, the number of particles left in the image
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcParticleFilter(Image* dest, Image* source,
+ const ParticleFilterCriteria2* criteria,
+ int criteriaCount, const ParticleFilterOptions* options,
+ int* numParticles) {
+ Rect rect = IMAQ_NO_RECT;
+ return frcParticleFilter(dest, source, criteria, criteriaCount, options, rect,
+ numParticles);
+}
+
+int frcParticleFilter(Image* dest, Image* source,
+ const ParticleFilterCriteria2* criteria,
+ int criteriaCount, const ParticleFilterOptions* options,
+ Rect rect, int* numParticles) {
+ ROI* roi = imaqCreateROI();
+ imaqAddRectContour(roi, rect);
+ return imaqParticleFilter3(dest, source, criteria, criteriaCount, options,
+ roi, numParticles);
+}
+
+/**
+* @brief Performs morphological transformations on binary images.
+* Supports IMAQ_IMAGE_U8.
+*
+* @param dest The destination image. The border size of the destination image is
+* not important.
+* @param source The image on which the function performs the morphological
+* operations. The calculation
+* modifies the border of the source image. The border must be at least half as
+* large as the larger
+* dimension of the structuring element. The connected source image for a
+* morphological transformation
+* must have been created with a border capable of supporting the size of the
+* structuring element.
+* A 3 by 3 structuring element requires a minimal border of 1, a 5 by 5
+* structuring element requires a minimal border of 2, and so on.
+* @param method The morphological transform to apply.
+* @param structuringElement The structuring element used in the operation. Omit
+* this parameter if you do not want a custom structuring element.
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcMorphology(Image* dest, Image* source, MorphologyMethod method) {
+ return imaqMorphology(dest, source, method, nullptr);
+}
+
+int frcMorphology(Image* dest, Image* source, MorphologyMethod method,
+ const StructuringElement* structuringElement) {
+ return imaqMorphology(dest, source, method, structuringElement);
+}
+
+/**
+* @brief Eliminates particles that touch the border of the image.
+* Supports IMAQ_IMAGE_U8.
+*
+* @param dest The destination image.
+* @param source The source image. If the image has a border, the function sets
+* all border pixel values to 0.
+* @param connectivity8 specifies the type of connectivity used by the algorithm
+* for particle detection.
+* The connectivity mode directly determines whether an adjacent pixel belongs to
+* the same particle or a
+* different particle. Set to TRUE to use connectivity-8 to determine whether
+* particles are touching
+* Set to FALSE to use connectivity-4 to determine whether particles are
+* touching.
+* The default setting for the simplified call is TRUE
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcRejectBorder(Image* dest, Image* source) {
+ return imaqRejectBorder(dest, source, TRUE);
+}
+
+int frcRejectBorder(Image* dest, Image* source, int connectivity8) {
+ return imaqRejectBorder(dest, source, connectivity8);
+}
+
+/**
+* @brief Counts the number of particles in a binary image.
+* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL.
+* @param image binary (thresholded) image
+* @param numParticles On return, the number of particles.
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcCountParticles(Image* image, int* numParticles) {
+ return imaqCountParticles(image, 1, numParticles);
+}
+
+/**
+* @brief Conduct measurements for a single particle in an images.
+* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16, IMAQ_IMAGE_SGL.
+*
+* @param image image with the particle to analyze. This function modifies the
+* source image.
+* If you need the original image, create a copy of the image using frcCopy()
+* before using this function.
+* @param particleNumber The number of the particle to get information on
+* @param par on return, a particle analysis report containing information about
+* the particle. This structure must be created by the caller.
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcParticleAnalysis(Image* image, int particleNumber,
+ ParticleAnalysisReport* par) {
+ int success = 0;
+
+ /* image information */
+ int height, width;
+ if (!imaqGetImageSize(image, &width, &height)) {
+ return success;
+ }
+ par->imageWidth = width;
+ par->imageHeight = height;
+ par->particleIndex = particleNumber;
+
+ /* center of mass point of the largest particle */
+ double returnDouble;
+ success = imaqMeasureParticle(image, particleNumber, 0,
+ IMAQ_MT_CENTER_OF_MASS_X, &returnDouble);
+ if (!success) {
+ return success;
+ }
+ par->center_mass_x = (int)returnDouble; // pixel
+
+ success = imaqMeasureParticle(image, particleNumber, 0,
+ IMAQ_MT_CENTER_OF_MASS_Y, &returnDouble);
+ if (!success) {
+ return success;
+ }
+ par->center_mass_y = (int)returnDouble; // pixel
+
+ /* particle size statistics */
+ success = imaqMeasureParticle(image, particleNumber, 0, IMAQ_MT_AREA,
+ &returnDouble);
+ if (!success) {
+ return success;
+ }
+ par->particleArea = returnDouble;
+
+ success = imaqMeasureParticle(image, particleNumber, 0,
+ IMAQ_MT_BOUNDING_RECT_TOP, &returnDouble);
+ if (!success) {
+ return success;
+ }
+ par->boundingRect.top = (int)returnDouble;
+
+ success = imaqMeasureParticle(image, particleNumber, 0,
+ IMAQ_MT_BOUNDING_RECT_LEFT, &returnDouble);
+ if (!success) {
+ return success;
+ }
+ par->boundingRect.left = (int)returnDouble;
+
+ success = imaqMeasureParticle(image, particleNumber, 0,
+ IMAQ_MT_BOUNDING_RECT_HEIGHT, &returnDouble);
+ if (!success) {
+ return success;
+ }
+ par->boundingRect.height = (int)returnDouble;
+
+ success = imaqMeasureParticle(image, particleNumber, 0,
+ IMAQ_MT_BOUNDING_RECT_WIDTH, &returnDouble);
+ if (!success) {
+ return success;
+ }
+ par->boundingRect.width = (int)returnDouble;
+
+ /* particle quality statistics */
+ success = imaqMeasureParticle(image, particleNumber, 0,
+ IMAQ_MT_AREA_BY_IMAGE_AREA, &returnDouble);
+ if (!success) {
+ return success;
+ }
+ par->particleToImagePercent = returnDouble;
+
+ success = imaqMeasureParticle(image, particleNumber, 0,
+ IMAQ_MT_AREA_BY_PARTICLE_AND_HOLES_AREA,
+ &returnDouble);
+ if (!success) {
+ return success;
+ }
+ par->particleQuality = returnDouble;
+
+ /* normalized position (-1 to 1) */
+ par->center_mass_x_normalized = RangeToNormalized(par->center_mass_x, width);
+ par->center_mass_y_normalized = RangeToNormalized(par->center_mass_y, height);
+
+ return success;
+}
+
+/* Image Enhancement functions */
+
+/**
+* @brief Improves contrast on a grayscale image.
+* Supports IMAQ_IMAGE_U8, IMAQ_IMAGE_I16.
+* @param dest The destination image.
+* @param source The image to equalize
+* @param min the smallest value used for processing. After processing, all pixel
+* values that are less than or equal to the Minimum in the original image are set
+* to 0 for an 8-bit image. In 16-bit and floating-point images, these pixel
+* values are set to the smallest pixel value found in the original image.
+* @param max the largest value used for processing. After processing, all pixel
+* values that are greater than or equal to the Maximum in the original image are
+* set to 255 for an 8-bit image. In 16-bit and floating-point images, these pixel
+* values are set to the largest pixel value found in the original image.
+* @param mask an 8-bit image that specifies the region of the small image that
+* will be copied. Only those pixels in the Image Src (Small) image that
+* correspond to an equivalent non-zero pixel in the mask image are copied. All
+* other pixels keep their original values. The entire image is processed if Image
+* Mask is nullptr or this parameter is omitted.
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*
+* option defaults:
+* searchRect = IMAQ_NO_RECT
+* minMatchScore = DEFAULT_MINMAX_SCORE (800)
+*/
+int frcEqualize(Image* dest, const Image* source, float min, float max) {
+ return frcEqualize(dest, source, min, max, nullptr);
+}
+
+int frcEqualize(Image* dest, const Image* source, float min, float max,
+ const Image* mask) {
+ return imaqEqualize(dest, source, min, max, mask);
+}
+
+/**
+* @brief Improves contrast on a color image.
+* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL
+*
+* option defaults: colorEqualization = TRUE to equalize all three planes of the
+* image
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+* @param dest The destination image.
+* @param source The image to equalize
+* @param colorEqualization Set this parameter to TRUE to equalize all three
+* planes of the image (the default). Set this parameter to FALSE to equalize only
+* the luminance plane.
+*/
+int frcColorEqualize(Image* dest, const Image* source) {
+ return imaqColorEqualize(dest, source, TRUE);
+}
+
+int frcColorEqualize(Image* dest, const Image* source, int colorEqualization) {
+ return imaqColorEqualize(dest, source, TRUE);
+}
+
+/* Image Conversion functions */
+
+/**
+* @brief Automatically thresholds a grayscale image into a binary image for
+* Particle Analysis based on a smart threshold.
+* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_I16
+* @param dest The destination image.
+* @param source The image to threshold
+* @param windowWidth The width of the rectangular window around the pixel on
+* which the function
+* performs the local threshold. This number must be at least 3 and cannot be
+* larger than the width of source
+* @param windowHeight The height of the rectangular window around the pixel on
+* which the function
+* performs the local threshold. This number must be at least 3 and cannot be
+* larger than the height of source
+* @param method Specifies the local thresholding method the function uses. Value
+* can be IMAQ_NIBLACK
+* (which computes thresholds for each pixel based on its local statistics using
+* the Niblack local thresholding
+* algorithm.), or IMAQ_BACKGROUND_CORRECTION (which does background correction
+* first to eliminate non-uniform
+* lighting effects, then performs thresholding using the Otsu thresholding
+* algorithm)
+* @param deviationWeight Specifies the k constant used in the Niblack local
+* thresholding algorithm, which
+* determines the weight applied to the variance calculation. Valid k constants
+* range from 0 to 1. Setting
+* this value to 0 will increase the performance of the function because the
+* function will not calculate the
+* variance for any of the pixels. The function ignores this value if method is
+* not set to IMAQ_NIBLACK
+* @param type Specifies the type of objects for which you want to look. Values
+* can be IMAQ_BRIGHT_OBJECTS
+* or IMAQ_DARK_OBJECTS.
+* @param replaceValue Specifies the replacement value the function uses for the
+* pixels of the kept objects
+* in the destination image.
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcSmartThreshold(Image* dest, const Image* source,
+ unsigned int windowWidth, unsigned int windowHeight,
+ LocalThresholdMethod method, double deviationWeight,
+ ObjectType type) {
+ float replaceValue = 1.0;
+ return imaqLocalThreshold(dest, source, windowWidth, windowHeight, method,
+ deviationWeight, type, replaceValue);
+}
+
+int frcSmartThreshold(Image* dest, const Image* source,
+ unsigned int windowWidth, unsigned int windowHeight,
+ LocalThresholdMethod method, double deviationWeight,
+ ObjectType type, float replaceValue) {
+ return imaqLocalThreshold(dest, source, windowWidth, windowHeight, method,
+ deviationWeight, type, replaceValue);
+}
+
+/**
+* @brief Converts a grayscale image to a binary image for Particle Analysis
+* based on a fixed threshold.
+* The function sets pixels values outside of the given range to 0. The function
+* sets pixel values
+* within the range to a given value or leaves the values unchanged.
+* Use the simplified call to leave pixel values unchanged.
+* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_I16.
+*
+* @param dest The destination image.
+* @param source The image to threshold
+* @param rangeMin The lower boundary of the range of pixel values to keep
+* @param rangeMax The upper boundary of the range of pixel values to keep.
+*
+* @return int - error code: 0 = error. To get extended error information, call
+* GetLastError().
+*/
+int frcSimpleThreshold(Image* dest, const Image* source, float rangeMin,
+ float rangeMax) {
+ int newValue = 255;
+ return frcSimpleThreshold(dest, source, rangeMin, rangeMax, newValue);
+}
+
+/**
+* @brief Converts a grayscale image to a binary image for Particle Analysis
+* based on a fixed threshold.
+* The function sets pixels values outside of the given range to 0. The function
+* sets
+* pixel values within the range to the given value.
+* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_I16.
+*
+* @param dest The destination image.
+* @param source The image to threshold
+* @param rangeMin The lower boundary of the range of pixel values to keep
+* @param rangeMax The upper boundary of the range of pixel values to keep.
+* @param newValue The replacement value for pixels within the range. Use the
+* simplified call to leave the pixel values unchanged
+*
+* @return int - error code: 0 = error. To get extended error information, call
+* GetLastError().
+*/
+int frcSimpleThreshold(Image* dest, const Image* source, float rangeMin,
+ float rangeMax, float newValue) {
+ int useNewValue = TRUE;
+ return imaqThreshold(dest, source, rangeMin, rangeMax, useNewValue, newValue);
+}
+
+/**
+* @brief Applies a threshold to the Red, Green, and Blue values of a RGB image
+* or the Hue,
+* Saturation, Luminance values for a HSL image.
+* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL.
+* This simpler version filters based on a hue range in the HSL mode.
+*
+* @param dest The destination image. This must be a IMAQ_IMAGE_U8 image.
+* @param source The image to threshold
+* @param mode The color space to perform the threshold in. valid values are:
+* IMAQ_RGB, IMAQ_HSL.
+* @param plane1Range The selection range for the first plane of the image. Set
+* this parameter to nullptr to use a selection range from 0 to 255.
+* @param plane2Range The selection range for the second plane of the image. Set
+* this parameter to nullptr to use a selection range from 0 to 255.
+* @param plane3Range The selection range for the third plane of the image. Set
+* this parameter to nullptr to use a selection range from 0 to 255.
+*
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+* */
+int frcColorThreshold(Image* dest, const Image* source, ColorMode mode,
+ const Range* plane1Range, const Range* plane2Range,
+ const Range* plane3Range) {
+ int replaceValue = 1;
+ return imaqColorThreshold(dest, source, replaceValue, mode, plane1Range,
+ plane2Range, plane3Range);
+}
+
+/**
+* @brief Applies a threshold to the Red, Green, and Blue values of a RGB image
+* or the Hue,
+* Saturation, Luminance values for a HSL image.
+* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL.
+* The simpler version filters based on a hue range in the HSL mode.
+*
+* @param dest The destination image. This must be a IMAQ_IMAGE_U8 image.
+* @param source The image to threshold
+* @param replaceValue Value to assign to selected pixels. Defaults to 1 if
+* simplified call is used.
+* @param mode The color space to perform the threshold in. valid values are:
+* IMAQ_RGB, IMAQ_HSL.
+* @param plane1Range The selection range for the first plane of the image. Set
+* this parameter to nullptr to use a selection range from 0 to 255.
+* @param plane2Range The selection range for the second plane of the image. Set
+* this parameter to nullptr to use a selection range from 0 to 255.
+* @param plane3Range The selection range for the third plane of the image. Set
+* this parameter to nullptr to use a selection range from 0 to 255.
+*
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcColorThreshold(Image* dest, const Image* source, int replaceValue,
+ ColorMode mode, const Range* plane1Range,
+ const Range* plane2Range, const Range* plane3Range) {
+ return imaqColorThreshold(dest, source, replaceValue, mode, plane1Range,
+ plane2Range, plane3Range);
+}
+
+/**
+* @brief A simpler version of ColorThreshold that thresholds hue range in the
+* HSL mode. Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL.
+* @param dest The destination image.
+* @param source The image to threshold
+* @param hueRange The selection range for the hue (color).
+* @param minSaturation The minimum saturation value (1-255). If not used,
+* DEFAULT_SATURATION_THRESHOLD is the default.
+*
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcHueThreshold(Image* dest, const Image* source, const Range* hueRange) {
+ return frcHueThreshold(dest, source, hueRange, DEFAULT_SATURATION_THRESHOLD);
+}
+
+int frcHueThreshold(Image* dest, const Image* source, const Range* hueRange,
+ int minSaturation) {
+ // assume HSL mode
+ ColorMode mode = IMAQ_HSL;
+ // Set saturation 100 - 255
+ Range satRange;
+ satRange.minValue = minSaturation;
+ satRange.maxValue = 255;
+ // Set luminance 100 - 255
+ Range lumRange;
+ lumRange.minValue = 100;
+ lumRange.maxValue = 255;
+ // Replace pixels with 1 if pass threshold filter
+ int replaceValue = 1;
+ return imaqColorThreshold(dest, source, replaceValue, mode, hueRange,
+ &satRange, &lumRange);
+}
+
+/**
+* @brief Extracts the Red, Green, Blue, or Hue, Saturation or Luminance
+* information from a color image.
+* Supports IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64.
+*
+* @param image The source image that the function extracts the planes from.
+* @param mode The color space that the function extracts the planes from. Valid
+* values are IMAQ_RGB, IMAQ_HSL, IMAQ_HSV, IMAQ_HSI.
+* @param plane1 On return, the first extracted plane. Set this parameter to nullptr
+* if you do not need this information. RGB-Red, HSL/HSV/HSI-Hue.
+* @param plane2 On return, the second extracted plane. Set this parameter to
+* nullptr if you do not need this information. RGB-Green, HSL/HSV/HSI-Saturation.
+* @param plane3 On return, the third extracted plane. Set this parameter to nullptr
+* if you do not need this information. RGB-Blue, HSL-Luminance, HSV-Value,
+* HSI-Intensity.
+*
+* @return error code: 0 = error. To get extended error information, call
+* GetLastError().
+*/
+int frcExtractColorPlanes(const Image* image, ColorMode mode, Image* plane1,
+ Image* plane2, Image* plane3) {
+ return imaqExtractColorPlanes(image, mode, plane1, plane2, plane3);
+}
+
+/**
+* @brief Extracts the Hue information from a color image. Supports
+* IMAQ_IMAGE_RGB, IMAQ_IMAGE_HSL, IMAQ_IMAGE_RGB_U64
+*
+* @param image The source image that the function extracts the plane from.
+* @param huePlane On return, the extracted hue plane.
+* @param minSaturation the minimum saturation level required 0-255 (try 50)
+*
+* @return On success: 1. On failure: 0. To get extended error information, call
+* GetLastError().
+*/
+int frcExtractHuePlane(const Image* image, Image* huePlane) {
+ return frcExtractHuePlane(image, huePlane, DEFAULT_SATURATION_THRESHOLD);
+}
+
+int frcExtractHuePlane(const Image* image, Image* huePlane, int minSaturation) {
+ return frcExtractColorPlanes(image, IMAQ_HSL, huePlane, nullptr, nullptr);
+}