Squashed 'third_party/Phoenix-frc-lib/' content from commit 666d176
Change-Id: Ibaca2fc8ffb1177e786576cc1e4cc9f7a8c98f13
git-subtree-dir: third_party/Phoenix-frc-lib
git-subtree-split: 666d176a08151793044ab74e0005f13d3732ed96
diff --git a/cpp/src/CANifier.cpp b/cpp/src/CANifier.cpp
new file mode 100644
index 0000000..7b18849
--- /dev/null
+++ b/cpp/src/CANifier.cpp
@@ -0,0 +1,454 @@
+/*
+ * Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics. All rights
+ * reserved.
+ *
+ * Cross The Road Electronics (CTRE) licenses to you the right to
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ *
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL,
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+#include "ctre/phoenix/CANifier.h"
+#include "ctre/phoenix/CCI/CANifier_CCI.h"
+#include "ctre/phoenix/CTRLogger.h"
+#include "HAL/HAL.h"
+
+namespace ctre {
+namespace phoenix {
+ /**
+ * Constructor.
+ * @param deviceNumber The CAN Device ID of the CANifier.
+ */
+CANifier::CANifier(int deviceNumber): CANBusAddressable(deviceNumber)
+{
+ m_handle = c_CANifier_Create1(deviceNumber);
+ HAL_Report(HALUsageReporting::kResourceType_CANifier, deviceNumber + 1);
+}
+
+/**
+ * Sets the LED Output
+ * @param percentOutput Output duty cycle expressed as percentage.
+ * @param ledChannel Channel to set the output of.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetLEDOutput(double percentOutput, LEDChannel ledChannel) {
+ /* convert float to integral fixed pt */
+ if (percentOutput > 1) {
+ percentOutput = 1;
+ }
+ if (percentOutput < 0) {
+ percentOutput = 0;
+ }
+ int dutyCycle = (int) (percentOutput * 1023); // [0,1023]
+
+ return c_CANifier_SetLEDOutput(m_handle, dutyCycle, ledChannel);
+}
+
+/**
+ * Sets the output of a General Pin
+ * @param outputPin The pin to use as output.
+ * @param outputValue The desired output state.
+ * @param outputEnable Whether this pin is an output. "True" enables output.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetGeneralOutput(GeneralPin outputPin, bool outputValue,
+ bool outputEnable) {
+ return c_CANifier_SetGeneralOutput(m_handle, outputPin, outputValue,
+ outputEnable);
+}
+
+/**
+ * Sets the output of all General Pin
+ * @param outputBits A bit mask of all the output states. LSB->MSB is in the order of the #GeneralPin enum.
+ * @param isOutputBits A boolean bit mask that sets the pins to be outputs or inputs. A bit of 1 enables output.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetGeneralOutputs(int outputBits, int isOutputBits) {
+ return c_CANifier_SetGeneralOutputs(m_handle, outputBits, isOutputBits);
+}
+
+/**
+ * Sets the output of all General Pin
+ * @param allPins A structure to fill with the current state of all pins.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::GetGeneralInputs(CANifier::PinValues &allPins) {
+ ErrorCode err = c_CANifier_GetGeneralInputs(m_handle, _tempPins, sizeof(_tempPins));
+ allPins.LIMF = _tempPins[LIMF];
+ allPins.LIMR = _tempPins[LIMR];
+ allPins.QUAD_A = _tempPins[QUAD_A];
+ allPins.QUAD_B = _tempPins[QUAD_B];
+ allPins.QUAD_IDX = _tempPins[QUAD_IDX];
+ allPins.SCL = _tempPins[SCL];
+ allPins.SDA = _tempPins[SDA];
+ allPins.SPI_CLK_PWM0 = _tempPins[SPI_CLK_PWM0P];
+ allPins.SPI_MOSI_PWM1 = _tempPins[SPI_MOSI_PWM1P];
+ allPins.SPI_MISO_PWM2 = _tempPins[SPI_MISO_PWM2P];
+ allPins.SPI_CS_PWM3 = _tempPins[SPI_CS];
+ return err;
+}
+
+/**
+ * Gets the state of the specified pin
+ * @param inputPin The index of the pin.
+ * @return The state of the pin.
+ */
+bool CANifier::GetGeneralInput(GeneralPin inputPin) {
+ bool retval = false;
+ (void)c_CANifier_GetGeneralInput(m_handle, inputPin, &retval);
+ return retval;
+}
+
+/**
+ * Gets the position of the quadrature encoder.
+ * @return The Position of the encoder.
+ */
+int CANifier::GetQuadraturePosition() {
+ int retval = 0;
+ (void)c_CANifier_GetQuadraturePosition(m_handle, &retval);
+ return retval;
+}
+/**
+ * Sets the position of the quadrature encoder.
+ * @param newPosition
+ * @return ErrorCode generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetQuadraturePosition(int newPosition, int timeoutMs) {
+ return c_CANifier_SetQuadraturePosition(m_handle, newPosition, timeoutMs);
+}
+/**
+ * Gets the velocity of the quadrature encoder.
+ * @return The Velocity of the encoder.
+ */
+int CANifier::GetQuadratureVelocity() {
+ int retval = 0;
+ (void)c_CANifier_GetQuadratureVelocity(m_handle, &retval);
+ return retval;
+}
+/**
+ * Configures the period of each velocity sample.
+ * Every 1ms a position value is sampled, and the delta between that sample
+ * and the position sampled kPeriod ms ago is inserted into a filter.
+ * kPeriod is configured with this function.
+ *
+ * @param period
+ * Desired period for the velocity measurement. @see
+ * #VelocityMeasPeriod
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::ConfigVelocityMeasurementPeriod(CANifierVelocityMeasPeriod period, int timeoutMs) {
+ return c_CANifier_ConfigVelocityMeasurementPeriod(m_handle, period, timeoutMs);
+}
+/**
+ * Sets the number of velocity samples used in the rolling average velocity
+ * measurement.
+ *
+ * @param windowSize
+ * Number of samples in the rolling average of velocity
+ * measurement. Valid values are 1,2,4,8,16,32. If another
+ * value is specified, it will truncate to nearest support value.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::ConfigVelocityMeasurementWindow(int windowSize, int timeoutMs) {
+ return c_CANifier_ConfigVelocityMeasurementWindow(m_handle, windowSize, timeoutMs);
+}
+/**
+ * Gets the bus voltage seen by the device.
+ *
+ * @return The bus voltage value (in volts).
+ */
+double CANifier::GetBusVoltage() {
+ double param = 0;
+ c_CANifier_GetBusVoltage(m_handle, ¶m);
+ return param;
+}
+
+/**
+ * Call GetLastError() generated by this object.
+ * Not all functions return an error code but can
+ * potentially report errors.
+ *
+ * This function can be used to retrieve those error codes.
+ *
+ * @return The last ErrorCode generated.
+ */
+ErrorCode CANifier::GetLastError() {
+ return c_CANifier_GetLastError(m_handle);
+}
+
+/**
+ * Sets the PWM Output
+ * Currently supports PWM 0, PWM 1, and PWM 2
+ * @param pwmChannel Index of the PWM channel to output.
+ * @param dutyCycle Duty Cycle (0 to 1) to output. Default period of the signal is 4.2 ms.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetPWMOutput(int pwmChannel, double dutyCycle) {
+ if (dutyCycle < 0) {
+ dutyCycle = 0;
+ } else if (dutyCycle > 1) {
+ dutyCycle = 1;
+ }
+ if (pwmChannel < 0) {
+ pwmChannel = 0;
+ }
+
+ int dutyCyc10bit = (int) (1023 * dutyCycle);
+
+ return c_CANifier_SetPWMOutput(m_handle, (int) pwmChannel,
+ dutyCyc10bit);
+}
+
+/**
+ * Enables PWM Outputs
+ * Currently supports PWM 0, PWM 1, and PWM 2
+ * @param pwmChannel Index of the PWM channel to enable.
+ * @param bEnable "True" enables output on the pwm channel.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::EnablePWMOutput(int pwmChannel, bool bEnable) {
+ if (pwmChannel < 0) {
+ pwmChannel = 0;
+ }
+
+ return c_CANifier_EnablePWMOutput(m_handle, (int) pwmChannel,
+ bEnable);
+}
+
+/**
+ * Gets the PWM Input
+ * @param pwmChannel PWM channel to get.
+ * @param dutyCycleAndPeriod Double array to hold Duty Cycle [0] and Period [1].
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::GetPWMInput(PWMChannel pwmChannel, double dutyCycleAndPeriod[]) {
+ return c_CANifier_GetPWMInput(m_handle, pwmChannel,
+ dutyCycleAndPeriod);
+}
+
+//------ Custom Persistent Params ----------//
+
+/**
+ * Sets the value of a custom parameter. This is for arbitrary use.
+ *
+ * Sometimes it is necessary to save calibration/duty cycle/output
+ * information in the device. Particularly if the
+ * device is part of a subsystem that can be replaced.
+ *
+ * @param newValue
+ * Value for custom parameter.
+ * @param paramIndex
+ * Index of custom parameter. [0-1]
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::ConfigSetCustomParam(int newValue,
+ int paramIndex, int timeoutMs) {
+ return c_CANifier_ConfigSetCustomParam(m_handle, newValue, paramIndex, timeoutMs);
+}
+/**
+ * Gets the value of a custom parameter. This is for arbitrary use.
+ *
+ * Sometimes it is necessary to save calibration/duty cycle/output
+ * information in the device. Particularly if the
+ * device is part of a subsystem that can be replaced.
+ *
+ * @param paramIndex
+ * Index of custom parameter. [0-1]
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Value of the custom param.
+ */
+int CANifier::ConfigGetCustomParam(
+ int paramIndex, int timeoutMs) {
+ int readValue;
+ c_CANifier_ConfigGetCustomParam(m_handle, &readValue, paramIndex, timeoutMs);
+ return readValue;
+}
+
+//------ Generic Param API, typically not used ----------//
+/**
+ * Sets a parameter. Generally this is not used.
+ * This can be utilized in
+ * - Using new features without updating API installation.
+ * - Errata workarounds to circumvent API implementation.
+ * - Allows for rapid testing / unit testing of firmware.
+ *
+ * @param param
+ * Parameter enumeration.
+ * @param value
+ * Value of parameter.
+ * @param subValue
+ * Subvalue for parameter. Maximum value of 255.
+ * @param ordinal
+ * Ordinal of parameter.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::ConfigSetParameter(ParamEnum param, double value,
+ uint8_t subValue, int ordinal, int timeoutMs) {
+ return c_CANifier_ConfigSetParameter(m_handle, param, value, subValue, ordinal, timeoutMs);
+
+}
+/**
+ * Gets a parameter. Generally this is not used.
+ * This can be utilized in
+ * - Using new features without updating API installation.
+ * - Errata workarounds to circumvent API implementation.
+ * - Allows for rapid testing / unit testing of firmware.
+ *
+ * @param param
+ * Parameter enumeration.
+ * @param ordinal
+ * Ordinal of parameter.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Value of parameter.
+ */
+double CANifier::ConfigGetParameter(ParamEnum param, int ordinal, int timeoutMs) {
+ double value = 0;
+ c_CANifier_ConfigGetParameter(m_handle, param, &value, ordinal, timeoutMs);
+ return value;
+}
+
+//------ Frames ----------//
+/**
+ * Sets the period of the given status frame.
+ *
+ * @param statusFrame
+ * Frame whose period is to be changed.
+ * @param periodMs
+ * Period in ms for the given frame.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetStatusFramePeriod(CANifierStatusFrame statusFrame, int periodMs,
+ int timeoutMs) {
+ return c_CANifier_SetStatusFramePeriod(m_handle, statusFrame, periodMs,
+ timeoutMs);
+}
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ * Frame to get the period of.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int CANifier::GetStatusFramePeriod(CANifierStatusFrame frame,
+ int timeoutMs) {
+ int periodMs = 0;
+ c_CANifier_GetStatusFramePeriod(m_handle, frame, &periodMs, timeoutMs);
+ return periodMs;
+}
+/**
+ * Sets the period of the given control frame.
+ *
+ * @param frame
+ * Frame whose period is to be changed.
+ * @param periodMs
+ * Period in ms for the given frame.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::SetControlFramePeriod(CANifierControlFrame frame,
+ int periodMs) {
+ return c_CANifier_SetControlFramePeriod(m_handle, frame, periodMs);
+}
+//------ Firmware ----------//
+/**
+ * Gets the firmware version of the device.
+ *
+ * @return Firmware version of device.
+ */
+int CANifier::GetFirmwareVersion() {
+ int retval = -1;
+ c_CANifier_GetFirmwareVersion(m_handle, &retval);
+ return retval;
+}
+/**
+ * Returns true if the device has reset since last call.
+ *
+ * @return Has a Device Reset Occurred?
+ */
+bool CANifier::HasResetOccurred() {
+ bool retval = false;
+ c_CANifier_HasResetOccurred(m_handle, &retval);
+ return retval;
+}
+//------ Faults ----------//
+/**
+ * Gets the CANifier fault status
+ *
+ * @param toFill
+ * Container for fault statuses.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::GetFaults(CANifierFaults & toFill) {
+ int faultBits;
+ ErrorCode retval = c_CANifier_GetFaults(m_handle, &faultBits);
+ toFill = CANifierFaults(faultBits);
+ return retval;
+}
+/**
+ * Gets the CANifier sticky fault status
+ *
+ * @param toFill
+ * Container for sticky fault statuses.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::GetStickyFaults(CANifierStickyFaults & toFill) {
+ int faultBits;
+ ErrorCode retval = c_CANifier_GetFaults(m_handle, &faultBits);
+ toFill = CANifierStickyFaults(faultBits);
+ return retval;
+}
+/**
+ * Clears the Sticky Faults
+ *
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode CANifier::ClearStickyFaults(int timeoutMs) {
+ return c_CANifier_ClearStickyFaults(m_handle, timeoutMs);
+}
+
+} // phoenix
+} // ctre
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/src/CTRLogger.cpp b/cpp/src/CTRLogger.cpp
new file mode 100644
index 0000000..e9c5a4b
--- /dev/null
+++ b/cpp/src/CTRLogger.cpp
@@ -0,0 +1,31 @@
+#include "ctre/phoenix/CTRLogger.h"
+#include "ctre/phoenix/CCI/Logger_CCI.h" // c_Logger_*
+#include <execinfo.h>
+
+namespace ctre {
+namespace phoenix {
+
+void CTRLogger::Open(int language) {
+ c_Logger_Open(language, true);
+}
+ErrorCode CTRLogger::Log(ErrorCode code, std::string origin) {
+ void *buf[100];
+ char **strings;
+ int size = backtrace(buf, 100);
+ strings = backtrace_symbols(buf, size);
+ std::string stackTrace;
+ for (int i = 1; i < size; i++) {
+ stackTrace += strings[i];
+ stackTrace += "\n";
+ }
+ return c_Logger_Log(code, origin.c_str(), 3, stackTrace.c_str());
+}
+void CTRLogger::Close() {
+ c_Logger_Close();
+}
+//void CTRLogger::Description(ErrorCode code, const char *&shrt, const char *&lng) {
+// c_Logger_Description(code, shrt, lng);
+//}
+
+}
+}
diff --git a/cpp/src/CompileTest.cpp b/cpp/src/CompileTest.cpp
new file mode 100644
index 0000000..47e4f64
--- /dev/null
+++ b/cpp/src/CompileTest.cpp
@@ -0,0 +1,2 @@
+#include "ctre/Phoenix.h"
+
diff --git a/cpp/src/HsvToRgb.cpp b/cpp/src/HsvToRgb.cpp
new file mode 100644
index 0000000..c7d5f16
--- /dev/null
+++ b/cpp/src/HsvToRgb.cpp
@@ -0,0 +1,106 @@
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+
+#include "ctre/phoenix/HsvToRgb.h"
+#include <math.h>
+
+namespace ctre {
+namespace phoenix {
+/**
+ * Convert hue/saturation/and value into RGB values
+ *
+ * @param hDegrees Hue in degrees
+ * @param S Saturation with range of 0 to 1
+ * @param V Value with range of 0 to 1
+ * @param r Calculated Red value of RGB
+ * @param g Calculated Green value of RGB
+ * @param b Calculated Blue value of RGB
+ */
+void HsvToRgb::Convert(double hDegrees, double S, double V, float* r, float* g,
+ float* b) {
+ double R, G, B;
+ double H = hDegrees;
+
+ //Handles wrap-around
+ if (H < 0) {
+ H += 360;
+ };
+ if (H >= 360) {
+ H -= 360;
+ };
+
+ if (V <= 0)
+ R = G = B = 0;
+ else if (S <= 0)
+ R = G = B = V;
+ else {
+ double hf = H / 60.0;
+ int i = (int) floor(hf);
+ double f = hf - i;
+ double pv = V * (1 - S);
+ double qv = V * (1 - S * f);
+ double tv = V * (1 - S * (1 - f));
+ switch (i) {
+ //Red is dominant color
+ case 0:
+ R = V;
+ G = tv;
+ B = pv;
+ break;
+
+ //Green is dominant color
+ case 1:
+ R = qv;
+ G = V;
+ B = pv;
+ break;
+ case 2:
+ R = pv;
+ G = V;
+ B = tv;
+ break;
+
+ //Blue is dominant color
+ case 3:
+ R = pv;
+ G = qv;
+ B = V;
+ break;
+ case 4:
+ R = tv;
+ G = pv;
+ B = V;
+ break;
+
+ //Red is dominant color
+ case 5:
+ R = V;
+ G = pv;
+ B = qv;
+ break;
+
+ //Back-up case statements, in case our math is wrong
+ case 6:
+ R = V;
+ G = tv;
+ B = pv;
+ break;
+ case -1:
+ R = V;
+ G = pv;
+ B = qv;
+ break;
+
+ //Color is not defined
+ default:
+ //pretend color is black and white
+ R = G = B = V;
+ break;
+ }
+ }
+ *r = (float) R;
+ *g = (float) G;
+ *b = (float) B;
+}
+}
+}
+#endif
diff --git a/cpp/src/LinearInterpolation.cpp b/cpp/src/LinearInterpolation.cpp
new file mode 100644
index 0000000..dc5c358
--- /dev/null
+++ b/cpp/src/LinearInterpolation.cpp
@@ -0,0 +1,13 @@
+#include "ctre/phoenix/LinearInterpolation.h"
+
+namespace ctre {
+namespace phoenix {
+float LinearInterpolation::Calculate(float x, float x1, float y1, float x2,
+ float y2) {
+ float m = (y2 - y1) / (x2 - x1);
+
+ float retval = m * (x - x1) + y1;
+ return retval;
+}
+}
+}
diff --git a/cpp/src/MotorControl/CAN/BaseMotorController.cpp b/cpp/src/MotorControl/CAN/BaseMotorController.cpp
new file mode 100644
index 0000000..b67451e
--- /dev/null
+++ b/cpp/src/MotorControl/CAN/BaseMotorController.cpp
@@ -0,0 +1,1738 @@
+#include "ctre/phoenix/MotorControl/CAN/BaseMotorController.h"
+#include "ctre/phoenix/MotorControl/SensorCollection.h"
+#include "ctre/phoenix/CCI/MotController_CCI.h"
+#include "ctre/phoenix/LowLevel/MotControllerWithBuffer_LowLevel.h"
+
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motorcontrol;
+using namespace ctre::phoenix::motorcontrol::can;
+using namespace ctre::phoenix::motorcontrol::lowlevel;
+
+//--------------------- Constructors -----------------------------//
+/**
+ *
+ * Constructor for motor controllers.
+ * @param arbId
+ */
+BaseMotorController::BaseMotorController(int arbId) {
+ m_handle = c_MotController_Create1(arbId);
+ _arbId = arbId;
+
+ _sensorColl = new motorcontrol::SensorCollection((void*) m_handle);
+}
+/**
+ *
+ * Destructor
+ */
+BaseMotorController::~BaseMotorController() {
+ delete _sensorColl;
+ _sensorColl = 0;
+}
+/**
+ * @return CCI handle for child classes.
+ */
+void* BaseMotorController::GetHandle() {
+ return m_handle;
+}
+/**
+ * Returns the Device ID
+ *
+ * @return Device number.
+ */
+int BaseMotorController::GetDeviceID() {
+ int devID = 0;
+ (void) c_MotController_GetDeviceNumber(m_handle, &devID);
+ return devID;
+}
+//------ Set output routines. ----------//
+/**
+ * @param Mode Sets the appropriate output on the talon, depending on the mode.
+ * @param value The output value to apply.
+ *
+ * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
+ * In Current mode, output value is in amperes.
+ * In Velocity mode, output value is in position change / 100ms.
+ * 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 value The setpoint value, as described above.
+ *
+ *
+ * Standard Driving Example:
+ * _talonLeft.set(ControlMode.PercentOutput, leftJoy);
+ * _talonRght.set(ControlMode.PercentOutput, rghtJoy);
+ */
+void BaseMotorController::Set(ControlMode Mode, double value) {
+ Set(Mode, value, DemandType_Neutral, 0);
+}
+/**
+ * @param mode Sets the appropriate output on the talon, depending on the mode.
+ * @param demand0 The output value to apply.
+ * such as advanced feed forward and/or auxiliary close-looping in firmware.
+ * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
+ * In Current mode, output value is in amperes.
+ * In Velocity mode, output value is in position change / 100ms.
+ * In Position mode, output value is in encoder ticks or an analog value,
+ * depending on the sensor. See
+ * In Follower mode, the output value is the integer device ID of the talon to
+ * duplicate.
+ *
+ * @param demand1 Supplemental value. This will also be control mode specific for future features.
+ */
+
+void BaseMotorController::Set(ControlMode mode, double demand0, double demand1) {
+ Set(mode, demand0, DemandType_Neutral, demand1);
+}
+/**
+ * @param mode Sets the appropriate output on the talon, depending on the mode.
+ * @param demand0 The output value to apply.
+ * such as advanced feed forward and/or auxiliary close-looping in firmware.
+ * In PercentOutput, the output is between -1.0 and 1.0, with 0.0 as stopped.
+ * In Current mode, output value is in amperes.
+ * In Velocity mode, output value is in position change / 100ms.
+ * In Position mode, output value is in encoder ticks or an analog value,
+ * depending on the sensor. See
+ * In Follower mode, the output value is the integer device ID of the talon to
+ * duplicate.
+ *
+ * @param demand1Type The demand type for demand1.
+ * Neutral: Ignore demand1 and apply no change to the demand0 output.
+ * AuxPID: Use demand1 to set the target for the auxiliary PID 1.
+ * ArbitraryFeedForward: Use demand1 as an arbitrary additive value to the
+ * demand0 output. In PercentOutput the demand0 output is the motor output,
+ * and in closed-loop modes the demand0 output is the output of PID0.
+ * @param demand1 Supplmental output value. Units match the set mode.
+ *
+ *
+ * Arcade Drive Example:
+ * _talonLeft.set(ControlMode::PercentOutput, joyForward, DemandType_ArbitraryFeedForward, +joyTurn);
+ * _talonRght.set(ControlMode::PercentOutput, joyForward, DemandType_ArbitraryFeedForward, -joyTurn);
+ *
+ * Drive Straight Example:
+ * Note: Selected Sensor Configuration is necessary for both PID0 and PID1.
+ * _talonLeft.follow(_talonRght, FollwerType_AuxOutput1);
+ * _talonRght.set(ControlMode::PercentOutput, joyForward, DemandType_AuxPID, desiredRobotHeading);
+ *
+ * Drive Straight to a Distance Example:
+ * Note: Other configurations (sensor selection, PID gains, etc.) need to be set.
+ * _talonLeft.follow(_talonRght, FollwerType_AuxOutput1);
+ * _talonRght.set(ControlMode::MotionMagic, targetDistance, DemandType_AuxPID, desiredRobotHeading);
+ */
+void BaseMotorController::Set(ControlMode mode, double demand0, DemandType demand1Type, double demand1) {
+ m_controlMode = mode;
+ m_sendMode = mode;
+ m_setPoint = demand0;
+
+ uint32_t work;
+ switch (m_controlMode) {
+ case ControlMode::PercentOutput:
+ //case ControlMode::TimedPercentOutput:
+ c_MotController_Set_4(m_handle, (int) m_sendMode, demand0, demand1, demand1Type);
+ break;
+ case ControlMode::Follower:
+ /* did caller specify device ID */
+ if ((0 <= demand0) && (demand0 <= 62)) { // [0,62]
+ work = (uint32_t) GetBaseID();
+ work >>= 16;
+ work <<= 8;
+ work |= (uint8_t) demand0;
+ } else {
+ work = (uint32_t) demand0;
+ }
+ /* single precision guarantees 16bits of integral precision,
+ * so float/double cast on work is safe */
+ c_MotController_Set_4(m_handle, (int) m_sendMode, (double)work, demand1, demand1Type);
+ break;
+ case ControlMode::Velocity:
+ case ControlMode::Position:
+ case ControlMode::MotionMagic:
+ //case ControlMode::MotionMagicArc:
+ case ControlMode::MotionProfile:
+ case ControlMode::MotionProfileArc:
+ c_MotController_Set_4(m_handle, (int) m_sendMode, demand0, demand1, demand1Type);
+ break;
+ case ControlMode::Current:
+ c_MotController_SetDemand(m_handle, (int) m_sendMode,
+ (int) (1000 * demand0), 0); /* milliamps */
+ break;
+ case ControlMode::Disabled:
+ /* fall thru...*/
+ default:
+ c_MotController_SetDemand(m_handle, (int) m_sendMode, 0, 0);
+ break;
+ }
+}
+/**
+ * Neutral the motor output by setting control mode to disabled.
+ */
+void BaseMotorController::NeutralOutput() {
+ Set(ControlMode::Disabled, 0);
+}
+/**
+ * Sets the mode of operation during neutral throttle output.
+ *
+ * @param neutralMode
+ * The desired mode of operation when the Controller output
+ * throttle is neutral (ie brake/coast)
+ **/
+void BaseMotorController::SetNeutralMode(NeutralMode neutralMode) {
+ c_MotController_SetNeutralMode(m_handle, neutralMode);
+}
+/**
+ * Enables a future feature called "Heading Hold".
+ * For now this simply updates the CAN signal to the motor controller.
+ * Future firmware updates will use this.
+ *
+ * @param enable true/false enable
+ */
+void BaseMotorController::EnableHeadingHold(bool enable) {
+ (void)enable;
+ /* this routine is moot as the Set() call updates the signal on each call */
+ //c_MotController_EnableHeadingHold(m_handle, enable);
+}
+/**
+ * For now this simply updates the CAN signal to the motor controller.
+ * Future firmware updates will use this to control advanced auxiliary loop behavior.
+ *
+ * @param value
+ */
+void BaseMotorController::SelectDemandType(bool value) {
+ (void)value;
+ /* this routine is moot as the Set() call updates the signal on each call */
+ //c_MotController_SelectDemandType(m_handle, value);
+}
+
+//------ Invert behavior ----------//
+/**
+ * Sets the phase of the sensor. Use when controller forward/reverse output
+ * doesn't correlate to appropriate forward/reverse reading of sensor.
+ * Pick a value so that positive PercentOutput yields a positive change in sensor.
+ * After setting this, user can freely call SetInvert() with any value.
+ *
+ * @param PhaseSensor
+ * Indicates whether to invert the phase of the sensor.
+ */
+void BaseMotorController::SetSensorPhase(bool PhaseSensor) {
+ c_MotController_SetSensorPhase(m_handle, PhaseSensor);
+}
+
+/**
+ * Inverts the hbridge output of the motor controller.
+ *
+ * This does not impact sensor phase and should not be used to correct sensor polarity.
+ *
+ * This will invert the hbridge output but NOT the LEDs.
+ * This ensures....
+ * - Green LEDs always represents positive request from robot-controller/closed-looping mode.
+ * - Green LEDs correlates to forward limit switch.
+ * - Green LEDs correlates to forward soft limit.
+ *
+ * @param invert
+ * Invert state to set.
+ */
+void BaseMotorController::SetInverted(bool invert) {
+ _invert = invert; /* cache for getter */
+ c_MotController_SetInverted(m_handle, _invert);
+}
+/**
+ * @return invert setting of motor output.
+ */
+bool BaseMotorController::GetInverted() const {
+ return _invert;
+}
+
+//----- general output shaping ------------------//
+/**
+ * Configures the open-loop ramp rate of throttle output.
+ *
+ * @param secondsFromNeutralToFull
+ * Minimum desired time to go from neutral to full throttle. A
+ * value of '0' will disable the ramp.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigOpenloopRamp(
+ double secondsFromNeutralToFull, int timeoutMs) {
+ return c_MotController_ConfigOpenLoopRamp(m_handle,
+ secondsFromNeutralToFull, timeoutMs);
+}
+
+/**
+ * Configures the closed-loop ramp rate of throttle output.
+ *
+ * @param secondsFromNeutralToFull
+ * Minimum desired time to go from neutral to full throttle. A
+ * value of '0' will disable the ramp.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigClosedloopRamp(
+ double secondsFromNeutralToFull, int timeoutMs) {
+ return c_MotController_ConfigClosedLoopRamp(m_handle,
+ secondsFromNeutralToFull, timeoutMs);
+}
+
+/**
+ * Configures the forward peak output percentage.
+ *
+ * @param percentOut
+ * Desired peak output percentage. [0,+1]
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigPeakOutputForward(double percentOut,
+ int timeoutMs) {
+ return c_MotController_ConfigPeakOutputForward(m_handle, percentOut,
+ timeoutMs);
+}
+
+/**
+ * Configures the reverse peak output percentage.
+ *
+ * @param percentOut
+ * Desired peak output percentage. [-1,0]
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigPeakOutputReverse(double percentOut,
+ int timeoutMs) {
+ return c_MotController_ConfigPeakOutputReverse(m_handle, percentOut,
+ timeoutMs);
+}
+/**
+ * Configures the forward nominal output percentage.
+ *
+ * @param percentOut
+ * Nominal (minimum) percent output. [0,+1]
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigNominalOutputForward(double percentOut,
+ int timeoutMs) {
+ return c_MotController_ConfigNominalOutputForward(m_handle, percentOut,
+ timeoutMs);
+}
+/**
+ * Configures the reverse nominal output percentage.
+ *
+ * @param percentOut
+ * Nominal (minimum) percent output. [-1,0]
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigNominalOutputReverse(double percentOut,
+ int timeoutMs) {
+ return c_MotController_ConfigNominalOutputReverse(m_handle, percentOut,
+ timeoutMs);
+}
+/**
+ * Configures the output deadband percentage.
+ *
+ * @param percentDeadband
+ * Desired deadband percentage. Minimum is 0.1%, Maximum is
+ * 25%. Pass 0.04 for 4% (factory default).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigNeutralDeadband(double percentDeadband,
+ int timeoutMs) {
+ return c_MotController_ConfigNeutralDeadband(m_handle, percentDeadband,
+ timeoutMs);
+}
+
+//------ Voltage Compensation ----------//
+/**
+ * Configures the Voltage Compensation saturation voltage.
+ *
+ * @param voltage
+ * This is the max voltage to apply to the hbridge when voltage
+ * compensation is enabled. For example, if 10 (volts) is specified
+ * and a TalonSRX is commanded to 0.5 (PercentOutput, closed-loop, etc)
+ * then the TalonSRX will attempt to apply a duty-cycle to produce 5V.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigVoltageCompSaturation(double voltage,
+ int timeoutMs) {
+ return c_MotController_ConfigVoltageCompSaturation(m_handle, voltage,
+ timeoutMs);
+}
+
+/**
+ * Configures the voltage measurement filter.
+ *
+ * @param filterWindowSamples
+ * Number of samples in the rolling average of voltage
+ * measurement.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigVoltageMeasurementFilter(
+ int filterWindowSamples, int timeoutMs) {
+ return c_MotController_ConfigVoltageMeasurementFilter(m_handle,
+ filterWindowSamples, timeoutMs);
+}
+
+/**
+ * Enables voltage compensation. If enabled, voltage compensation works in
+ * all control modes.
+ *
+ * @param enable
+ * Enable state of voltage compensation.
+ **/
+void BaseMotorController::EnableVoltageCompensation(bool enable) {
+ c_MotController_EnableVoltageCompensation(m_handle, enable);
+}
+
+//------ General Status ----------//
+/**
+ * Gets the bus voltage seen by the device.
+ *
+ * @return The bus voltage value (in volts).
+ */
+double BaseMotorController::GetBusVoltage() {
+ double param = 0;
+ c_MotController_GetBusVoltage(m_handle, ¶m);
+ return param;
+}
+
+/**
+ * Gets the output percentage of the motor controller.
+ *
+ * @return Output of the motor controller (in percent). [-1,+1]
+ */
+double BaseMotorController::GetMotorOutputPercent() {
+ double param = 0;
+ c_MotController_GetMotorOutputPercent(m_handle, ¶m);
+ return param;
+}
+
+/**
+ * @return applied voltage to motor in volts.
+ */
+double BaseMotorController::GetMotorOutputVoltage() {
+ return GetBusVoltage() * GetMotorOutputPercent();
+}
+
+/**
+ * Gets the output current of the motor controller.
+ *
+ * @return The output current (in amps).
+ */
+double BaseMotorController::GetOutputCurrent() {
+ double param = 0;
+ c_MotController_GetOutputCurrent(m_handle, ¶m);
+ return param;
+}
+/**
+ * Gets the temperature of the motor controller.
+ *
+ * @return Temperature of the motor controller (in 'C)
+ */
+double BaseMotorController::GetTemperature() {
+ double param = 0;
+ c_MotController_GetTemperature(m_handle, ¶m);
+ return param;
+}
+
+//------ sensor selection ----------//
+/**
+ * Select the remote feedback device for the motor controller.
+ * Most CTRE CAN motor controllers will support remote sensors over CAN.
+ *
+ * @param feedbackDevice
+ * Remote Feedback Device to select.
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSelectedFeedbackSensor(
+ RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) {
+ return c_MotController_ConfigSelectedFeedbackSensor(m_handle,
+ feedbackDevice, pidIdx, timeoutMs);
+}
+/**
+ * Select the feedback device for the motor controller.
+ *
+ * @param feedbackDevice
+ * Feedback Device to select.
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSelectedFeedbackSensor(
+ FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) {
+ return c_MotController_ConfigSelectedFeedbackSensor(m_handle,
+ feedbackDevice, pidIdx, timeoutMs);
+}
+
+/**
+ * The Feedback Coefficient is a scalar applied to the value of the
+ * feedback sensor. Useful when you need to scale your sensor values
+ * within the closed-loop calculations. Default value is 1.
+ *
+ * Selected Feedback Sensor register in firmware is the decoded sensor value
+ * multiplied by the Feedback Coefficient.
+ *
+ * @param coefficient
+ * Feedback Coefficient value. Maximum value of 1.
+ * Resolution is 1/(2^16). Cannot be 0.
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSelectedFeedbackCoefficient(
+ double coefficient, int pidIdx, int timeoutMs) {
+ return c_MotController_ConfigSelectedFeedbackCoefficient(m_handle,
+ coefficient, pidIdx, timeoutMs);
+}
+
+/**
+ * Select what remote device and signal to assign to Remote Sensor 0 or Remote Sensor 1.
+ * After binding a remote device and signal to Remote Sensor X, you may select Remote Sensor X
+ * as a PID source for closed-loop features.
+ *
+ * @param deviceID
+ * The CAN ID of the remote sensor device.
+ * @param remoteSensorSource
+ * The remote sensor device and signal type to bind.
+ * @param remoteOrdinal
+ * 0 for configuring Remote Sensor 0
+ * 1 for configuring Remote Sensor 1
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigRemoteFeedbackFilter(int deviceID,
+ RemoteSensorSource remoteSensorSource, int remoteOrdinal,
+ int timeoutMs) {
+ return c_MotController_ConfigRemoteFeedbackFilter(m_handle, deviceID,
+ (int) remoteSensorSource, remoteOrdinal, timeoutMs);
+}
+/**
+ * Select what sensor term should be bound to switch feedback device.
+ * Sensor Sum = Sensor Sum Term 0 - Sensor Sum Term 1
+ * Sensor Difference = Sensor Diff Term 0 - Sensor Diff Term 1
+ * The four terms are specified with this routine. Then Sensor Sum/Difference
+ * can be selected for closed-looping.
+ *
+ * @param sensorTerm Which sensor term to bind to a feedback source.
+ * @param feedbackDevice The sensor signal to attach to sensorTerm.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSensorTerm(SensorTerm sensorTerm,
+ FeedbackDevice feedbackDevice, int timeoutMs) {
+ return c_MotController_ConfigSensorTerm(m_handle, (int) sensorTerm,
+ (int) feedbackDevice, timeoutMs);
+}
+
+//------- sensor status --------- //
+/**
+ * Get the selected sensor position (in raw sensor units).
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * See Phoenix-Documentation for how to interpret.
+ *
+ * @return Position of selected sensor (in raw sensor units).
+ */
+int BaseMotorController::GetSelectedSensorPosition(int pidIdx) {
+ int retval;
+ c_MotController_GetSelectedSensorPosition(m_handle, &retval, pidIdx);
+ return retval;
+}
+/**
+ * Get the selected sensor velocity.
+ *
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @return selected sensor (in raw sensor units) per 100ms.
+ * See Phoenix-Documentation for how to interpret.
+ */
+int BaseMotorController::GetSelectedSensorVelocity(int pidIdx) {
+ int retval;
+ c_MotController_GetSelectedSensorVelocity(m_handle, &retval, pidIdx);
+ return retval;
+}
+/**
+ * Sets the sensor position to the given value.
+ *
+ * @param sensorPos
+ * Position to set for the selected sensor (in raw sensor units).
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::SetSelectedSensorPosition(int sensorPos,
+ int pidIdx, int timeoutMs) {
+ return c_MotController_SetSelectedSensorPosition(m_handle, sensorPos,
+ pidIdx, timeoutMs);
+}
+
+//------ status frame period changes ----------//
+/**
+ * Sets the period of the given control frame.
+ *
+ * @param frame
+ * Frame whose period is to be changed.
+ * @param periodMs
+ * Period in ms for the given frame.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::SetControlFramePeriod(ControlFrame frame,
+ int periodMs) {
+ return c_MotController_SetControlFramePeriod(m_handle, frame, periodMs);
+}
+/**
+ * Sets the period of the given status frame.
+ *
+ * User ensure CAN Bus utilization is not high.
+ *
+ * This setting is not persistent and is lost when device is reset.
+ * If this is a concern, calling application can use HasReset()
+ * to determine if the status frame needs to be reconfigured.
+ *
+ * @param frame
+ * Frame whose period is to be changed.
+ * @param periodMs
+ * Period in ms for the given frame.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::SetStatusFramePeriod(StatusFrame frame,
+ int periodMs, int timeoutMs) {
+ return c_MotController_SetStatusFramePeriod(m_handle, frame, periodMs,
+ timeoutMs);
+}
+/**
+ * Sets the period of the given status frame.
+ *
+ * User ensure CAN Bus utilization is not high.
+ *
+ * This setting is not persistent and is lost when device is reset.
+ * If this is a concern, calling application can use HasReset()
+ * to determine if the status frame needs to be reconfigured.
+ *
+ * @param frame
+ * Frame whose period is to be changed.
+ * @param periodMs
+ * Period in ms for the given frame.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::SetStatusFramePeriod(StatusFrameEnhanced frame,
+ int periodMs, int timeoutMs) {
+ return c_MotController_SetStatusFramePeriod(m_handle, frame, periodMs,
+ timeoutMs);
+}
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ * Frame to get the period of.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int BaseMotorController::GetStatusFramePeriod(StatusFrame frame,
+ int timeoutMs) {
+ int periodMs = 0;
+ c_MotController_GetStatusFramePeriod(m_handle, frame, &periodMs, timeoutMs);
+ return periodMs;
+}
+
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ * Frame to get the period of.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int BaseMotorController::GetStatusFramePeriod(StatusFrameEnhanced frame,
+ int timeoutMs) {
+ int periodMs = 0;
+ c_MotController_GetStatusFramePeriod(m_handle, frame, &periodMs, timeoutMs);
+ return periodMs;
+}
+
+//----- velocity signal conditioning ------//
+
+/**
+ * Configures the period of each velocity sample.
+ * Every 1ms a position value is sampled, and the delta between that sample
+ * and the position sampled kPeriod ms ago is inserted into a filter.
+ * kPeriod is configured with this function.
+ *
+ * @param period
+ * Desired period for the velocity measurement. @see
+ * #VelocityMeasPeriod
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigVelocityMeasurementPeriod(
+ VelocityMeasPeriod period, int timeoutMs) {
+ return c_MotController_ConfigVelocityMeasurementPeriod(m_handle, period,
+ timeoutMs);
+}
+/**
+ * Sets the number of velocity samples used in the rolling average velocity
+ * measurement.
+ *
+ * @param windowSize
+ * Number of samples in the rolling average of velocity
+ * measurement. Valid values are 1,2,4,8,16,32. If another
+ * value is specified, it will truncate to nearest support value.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigVelocityMeasurementWindow(int windowSize,
+ int timeoutMs) {
+ return c_MotController_ConfigVelocityMeasurementWindow(m_handle, windowSize,
+ timeoutMs);
+}
+
+//------ remote limit switch ----------//
+/**
+ * Configures the forward limit switch for a remote source.
+ * For example, a CAN motor controller may need to monitor the Limit-F pin
+ * of another Talon or CANifier.
+ *
+ * @param type
+ * Remote limit switch source.
+ * User can choose between a remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ * Setting for normally open, normally closed, or disabled. This setting
+ * matches the web-based configuration drop down.
+ * @param deviceID
+ * Device ID of remote source (Talon SRX or CANifier device ID).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigForwardLimitSwitchSource(
+ RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+ int deviceID, int timeoutMs) {
+ LimitSwitchSource cciType = LimitSwitchRoutines::Promote(type);
+ return c_MotController_ConfigForwardLimitSwitchSource(m_handle, cciType,
+ normalOpenOrClose, deviceID, timeoutMs);
+}
+/**
+ * Configures the reverse limit switch for a remote source.
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon or CANifier.
+ *
+ * @param type
+ * Remote limit switch source.
+ * User can choose between a remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ * Setting for normally open, normally closed, or disabled. This setting
+ * matches the web-based configuration drop down.
+ * @param deviceID
+ * Device ID of remote source (Talon SRX or CANifier device ID).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigReverseLimitSwitchSource(
+ RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+ int deviceID, int timeoutMs) {
+ LimitSwitchSource cciType = LimitSwitchRoutines::Promote(type);
+ return c_MotController_ConfigReverseLimitSwitchSource(m_handle, cciType,
+ normalOpenOrClose, deviceID, timeoutMs);
+}
+/**
+ * Sets the enable state for limit switches.
+ *
+ * This routine can be used to DISABLE the limit switch feature.
+ * This is helpful to force off the limit switch detection.
+ * For example, a module can leave limit switches enable for home-ing
+ * a continuous mechanism, and once done this routine can force off
+ * disabling of the motor controller.
+ *
+ * Limit switches must be enabled using the Config routines first.
+ *
+ * @param enable
+ * Enable state for limit switches.
+ */
+void BaseMotorController::OverrideLimitSwitchesEnable(bool enable) {
+ c_MotController_OverrideLimitSwitchesEnable(m_handle, enable);
+}
+
+//------ local limit switch ----------//
+/**
+ * Configures a limit switch for a local/remote source.
+ *
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon, CANifier, or local Gadgeteer feedback connector.
+ *
+ * If the sensor is remote, a device ID of zero is assumed.
+ * If that's not desired, use the four parameter version of this function.
+ *
+ * @param type
+ * Limit switch source. @see #LimitSwitchSource
+ * User can choose between the feedback connector, remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ * Setting for normally open, normally closed, or disabled. This setting
+ * matches the web-based configuration drop down.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigForwardLimitSwitchSource(
+ LimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+ int timeoutMs) {
+ return c_MotController_ConfigForwardLimitSwitchSource(m_handle, type,
+ normalOpenOrClose, 0, timeoutMs);
+}
+/**
+ * Configures a limit switch for a local/remote source.
+ *
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon, CANifier, or local Gadgeteer feedback connector.
+ *
+ * If the sensor is remote, a device ID of zero is assumed.
+ * If that's not desired, use the four parameter version of this function.
+ *
+ * @param type
+ * Limit switch source. @see #LimitSwitchSource
+ * User can choose between the feedback connector, remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ * Setting for normally open, normally closed, or disabled. This setting
+ * matches the web-based configuration drop down.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigReverseLimitSwitchSource(
+ LimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+ int timeoutMs) {
+ return c_MotController_ConfigReverseLimitSwitchSource(m_handle, type,
+ normalOpenOrClose, 0, timeoutMs);
+}
+
+//------ soft limit ----------//
+/**
+ * Configures the forward soft limit threshold.
+ *
+ * @param forwardSensorLimit
+ * Forward Sensor Position Limit (in raw sensor units).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigForwardSoftLimitThreshold(int forwardSensorLimit,
+ int timeoutMs) {
+ return c_MotController_ConfigForwardSoftLimitThreshold(m_handle, forwardSensorLimit,
+ timeoutMs);
+}
+
+/**
+ * Configures the reverse soft limit threshold.
+ *
+ * @param reverseSensorLimit
+ * Reverse Sensor Position Limit (in raw sensor units).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigReverseSoftLimitThreshold(int reverseSensorLimit,
+ int timeoutMs) {
+ return c_MotController_ConfigReverseSoftLimitThreshold(m_handle, reverseSensorLimit,
+ timeoutMs);
+}
+
+/**
+ * Configures the forward soft limit enable .
+ *
+ * @param enable
+ * True to enable soft limit. False to disable.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigForwardSoftLimitEnable(bool enable,
+ int timeoutMs) {
+ return c_MotController_ConfigForwardSoftLimitEnable(m_handle, enable,
+ timeoutMs);
+}
+
+
+/**
+ * Configures the reverse soft limit enable.
+ *
+ * @param enable
+ * True to enable soft limit. False to disable.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigReverseSoftLimitEnable(bool enable,
+ int timeoutMs) {
+ return c_MotController_ConfigReverseSoftLimitEnable(m_handle, enable,
+ timeoutMs);
+}
+
+/**
+ * Can be used to override-disable the soft limits.
+ * This function can be used to quickly disable soft limits without
+ * having to modify the persistent configuration.
+ *
+ * @param enable
+ * Enable state for soft limit switches.
+ **/
+void BaseMotorController::OverrideSoftLimitsEnable(bool enable) {
+ c_MotController_OverrideSoftLimitsEnable(m_handle, enable);
+}
+
+//------ Current Lim ----------//
+/* not available in base */
+
+//------ General Close loop ----------//
+/**
+ * Sets the 'P' constant in the given parameter slot.
+ *
+ * @param slotIdx
+ * Parameter slot for the constant.
+ * @param value
+ * Value of the P constant.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::Config_kP(int slotIdx, double value,
+ int timeoutMs) {
+ return c_MotController_Config_kP(m_handle, slotIdx, value, timeoutMs);
+}
+
+/**
+ * Sets the 'I' constant in the given parameter slot.
+ *
+ * @param slotIdx
+ * Parameter slot for the constant.
+ * @param value
+ * Value of the I constant.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::Config_kI(int slotIdx, double value,
+ int timeoutMs) {
+ return c_MotController_Config_kI(m_handle, slotIdx, value, timeoutMs);
+}
+
+/**
+ * Sets the 'D' constant in the given parameter slot.
+ *
+ * @param slotIdx
+ * Parameter slot for the constant.
+ * @param value
+ * Value of the D constant.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::Config_kD(int slotIdx, double value,
+ int timeoutMs) {
+ return c_MotController_Config_kD(m_handle, slotIdx, value, timeoutMs);
+}
+
+/**
+ * Sets the 'F' constant in the given parameter slot.
+ *
+ * @param slotIdx
+ * Parameter slot for the constant.
+ * @param value
+ * Value of the F constant.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::Config_kF(int slotIdx, double value,
+ int timeoutMs) {
+ return c_MotController_Config_kF(m_handle, slotIdx, value, timeoutMs);
+}
+
+/**
+ * Sets the Integral Zone constant in the given parameter slot.
+ * If the (absolute) closed-loop error is outside of this zone, integral accumulator
+ * is automatically cleared. This ensures than integral wind up events will stop after
+ * the sensor gets far enough from its target.
+ *
+ * @param slotIdx
+ * Parameter slot for the constant.
+ * @param izone
+ * Value of the Integral Zone constant (closed loop error units X 1ms).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::Config_IntegralZone(int slotIdx, int izone,
+ int timeoutMs) {
+ return c_MotController_Config_IntegralZone(m_handle, slotIdx, izone,
+ timeoutMs);
+}
+
+/**
+ * Sets the allowable closed-loop error in the given parameter slot.
+ * If (absolute) closed-loop error is within this value, the motor output is neutral.
+ *
+ * @param slotIdx
+ * Parameter slot for the constant.
+ * @param allowableCloseLoopError
+ * Value of the allowable closed-loop error.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigAllowableClosedloopError(int slotIdx,
+ int allowableCloseLoopError, int timeoutMs) {
+ return c_MotController_ConfigAllowableClosedloopError(m_handle, slotIdx,
+ allowableCloseLoopError, timeoutMs);
+}
+
+/**
+ * Sets the maximum integral accumulator in the given parameter slot.
+ *
+ * @param slotIdx
+ * Parameter slot for the constant.
+ * @param iaccum
+ * Value of the maximum integral accumulator (closed loop error units X 1ms).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigMaxIntegralAccumulator(int slotIdx,
+ double iaccum, int timeoutMs) {
+ return c_MotController_ConfigMaxIntegralAccumulator(m_handle, slotIdx,
+ iaccum, timeoutMs);
+}
+
+/**
+ * Sets the peak closed-loop output. This peak output is slot-specific and
+ * is applied to the output of the associated PID loop.
+ * This setting is seperate from the generic Peak Output setting.
+ *
+ * @param slotIdx
+ * Parameter slot for the constant.
+ * @param percentOut
+ * Peak Percent Output from 0 to 1. This value is absolute and
+ * the magnitude will apply in both forward and reverse directions.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigClosedLoopPeakOutput(int slotIdx, double percentOut, int timeoutMs) {
+ return c_MotController_ConfigClosedLoopPeakOutput(m_handle, slotIdx, percentOut, timeoutMs);
+}
+
+/**
+ * Sets the loop time (in milliseconds) of the PID closed-loop calculations.
+ * Default value is 1 ms.
+ *
+ * @param slotIdx
+ * Parameter slot for the constant.
+ * @param loopTimeMs
+ * Loop timing of the closed-loop calculations. Minimum value of
+ * 1 ms, maximum of 64 ms.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigClosedLoopPeriod(int slotIdx, int loopTimeMs, int timeoutMs) {
+ return c_MotController_ConfigClosedLoopPeriod(m_handle, slotIdx, loopTimeMs, timeoutMs);
+}
+
+/**
+ * Configures the Polarity of the Auxiliary PID (PID1).
+ *
+ * Standard Polarity:
+ * Primary Output = PID0 + PID1
+ * Auxiliary Output = PID0 - PID1
+ *
+ * Inverted Polarity:
+ * Primary Output = PID0 - PID1
+ * Auxiliary Output = PID0 + PID1
+ *
+ * @param invert
+ * If true, use inverted PID1 output polarity.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for config
+ * success and report an error if it times out. If zero, no
+ * blocking or checking is performed.
+ * @return Error Code
+ */
+ ErrorCode BaseMotorController::ConfigAuxPIDPolarity(bool invert, int timeoutMs){
+ return ConfigSetParameter(ParamEnum::ePIDLoopPolarity, invert, 0, 1, timeoutMs);
+ }
+
+/**
+ * Sets the integral accumulator. Typically this is used to clear/zero
+ * the integral accumulator, however some use cases may require seeding
+ * the accumulator for a faster response.
+ *
+ * @param iaccum
+ * Value to set for the integral accumulator (closed loop error units X 1ms).
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::SetIntegralAccumulator(double iaccum, int pidIdx,
+ int timeoutMs) {
+ return c_MotController_SetIntegralAccumulator(m_handle, iaccum, pidIdx,
+ timeoutMs);
+}
+
+/**
+ * Gets the closed-loop error.
+ * The units depend on which control mode is in use.
+ * See Phoenix-Documentation information on units.
+ *
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @return Closed-loop error value.
+ */
+int BaseMotorController::GetClosedLoopError(int pidIdx) {
+ int closedLoopError = 0;
+ c_MotController_GetClosedLoopError(m_handle, &closedLoopError, pidIdx);
+ return closedLoopError;
+}
+
+/**
+ * Gets the iaccum value.
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @return Integral accumulator value (Closed-loop error X 1ms).
+ */
+double BaseMotorController::GetIntegralAccumulator(int pidIdx) {
+ double iaccum = 0;
+ c_MotController_GetIntegralAccumulator(m_handle, &iaccum, pidIdx);
+ return iaccum;
+}
+
+
+/**
+ * Gets the derivative of the closed-loop error.
+ *
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @return The error derivative value.
+ */
+double BaseMotorController::GetErrorDerivative(int pidIdx) {
+ double derror = 0;
+ c_MotController_GetErrorDerivative(m_handle, &derror, pidIdx);
+ return derror;
+}
+
+/**
+ * Selects which profile slot to use for closed-loop control.
+ *
+ * @param slotIdx
+ * Profile slot to select.
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ **/
+ErrorCode BaseMotorController::SelectProfileSlot(int slotIdx, int pidIdx) {
+ return c_MotController_SelectProfileSlot(m_handle, slotIdx, pidIdx);
+}
+/**
+ * Gets the current target of a given closed loop.
+ *
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @return The closed loop target.
+ */
+int BaseMotorController::GetClosedLoopTarget(int pidIdx) {
+ int param = 0;
+ c_MotController_GetClosedLoopTarget(m_handle, ¶m, pidIdx);
+ return param;
+}
+/**
+ * Gets the active trajectory target position using
+ * MotionMagic/MotionProfile control modes.
+ *
+ * @return The Active Trajectory Position in sensor units.
+ */
+int BaseMotorController::GetActiveTrajectoryPosition() {
+ int param = 0;
+ c_MotController_GetActiveTrajectoryPosition(m_handle, ¶m);
+ return param;
+}
+/**
+ * Gets the active trajectory target velocity using
+ * MotionMagic/MotionProfile control modes.
+ *
+ * @return The Active Trajectory Velocity in sensor units per 100ms.
+ */
+int BaseMotorController::GetActiveTrajectoryVelocity() {
+ int param = 0;
+ c_MotController_GetActiveTrajectoryVelocity(m_handle, ¶m);
+ return param;
+}
+/**
+ * Gets the active trajectory target heading using
+ * MotionMagicArc/MotionProfileArc control modes.
+ *
+ * @return The Active Trajectory Heading in degreees.
+ */
+double BaseMotorController::GetActiveTrajectoryHeading() {
+ double param = 0;
+ c_MotController_GetActiveTrajectoryHeading(m_handle, ¶m);
+ return param;
+}
+
+//------ Motion Profile Settings used in Motion Magic and Motion Profile ----------//
+
+/**
+ * Sets the Motion Magic Cruise Velocity. This is the peak target velocity
+ * that the motion magic curve generator can use.
+ *
+ * @param sensorUnitsPer100ms
+ * Motion Magic Cruise Velocity (in raw sensor units per 100 ms).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigMotionCruiseVelocity(
+ int sensorUnitsPer100ms, int timeoutMs) {
+ return c_MotController_ConfigMotionCruiseVelocity(m_handle,
+ sensorUnitsPer100ms, timeoutMs);
+}
+/**
+ * Sets the Motion Magic Acceleration. This is the target acceleration
+ * that the motion magic curve generator can use.
+ *
+ * @param sensorUnitsPer100msPerSec
+ * Motion Magic Acceleration (in raw sensor units per 100 ms per
+ * second).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigMotionAcceleration(
+ int sensorUnitsPer100msPerSec, int timeoutMs) {
+ return c_MotController_ConfigMotionAcceleration(m_handle,
+ sensorUnitsPer100msPerSec, timeoutMs);
+}
+
+//------ Motion Profile Buffer ----------//
+/**
+ * Clear the buffered motion profile in both motor controller's RAM (bottom), and in the API
+ * (top).
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ClearMotionProfileTrajectories() {
+ return c_MotController_ClearMotionProfileTrajectories(m_handle);
+}
+/**
+ * Retrieve just the buffer count for the api-level (top) buffer.
+ * This routine performs no CAN or data structure lookups, so its fast and ideal
+ * if caller needs to quickly poll the progress of trajectory points being
+ * emptied into motor controller's RAM. Otherwise just use GetMotionProfileStatus.
+ * @return number of trajectory points in the top buffer.
+ */
+int BaseMotorController::GetMotionProfileTopLevelBufferCount() {
+ int param = 0;
+ c_MotController_GetMotionProfileTopLevelBufferCount(m_handle, ¶m);
+ return param;
+}
+/**
+ * Push another trajectory point into the top level buffer (which is emptied
+ * into the motor controller's bottom buffer as room allows).
+ * @param trajPt to push into buffer.
+ * The members should be filled in with these values...
+ *
+ * targPos: servo position in sensor units.
+ * targVel: velocity to feed-forward in sensor units
+ * per 100ms.
+ * profileSlotSelect0 Which slot to get PIDF gains. PID is used for position servo. F is used
+ * as the Kv constant for velocity feed-forward. Typically this is hardcoded
+ * to the a particular slot, but you are free gain schedule if need be.
+ * Choose from [0,3]
+ * profileSlotSelect1 Which slot to get PIDF gains for auxiliary PId.
+ * This only has impact during MotionProfileArc Control mode.
+ * Choose from [0,1].
+ * isLastPoint set to nonzero to signal motor controller to keep processing this
+ * trajectory point, instead of jumping to the next one
+ * when timeDurMs expires. Otherwise MP executer will
+ * eventually see an empty buffer after the last point
+ * expires, causing it to assert the IsUnderRun flag.
+ * However this may be desired if calling application
+ * never wants to terminate the MP.
+ * zeroPos set to nonzero to signal motor controller to "zero" the selected
+ * position sensor before executing this trajectory point.
+ * Typically the first point should have this set only thus
+ * allowing the remainder of the MP positions to be relative to
+ * zero.
+ * timeDur Duration to apply this trajectory pt.
+ * This time unit is ADDED to the exising base time set by
+ * configMotionProfileTrajectoryPeriod().
+ * @return CTR_OKAY if trajectory point push ok. ErrorCode if buffer is
+ * full due to kMotionProfileTopBufferCapacity.
+ */
+ErrorCode BaseMotorController::PushMotionProfileTrajectory(
+ const ctre::phoenix::motion::TrajectoryPoint & trajPt) {
+ ErrorCode retval = c_MotController_PushMotionProfileTrajectory_2(m_handle,
+ trajPt.position, trajPt.velocity, trajPt.auxiliaryPos,
+ trajPt.profileSlotSelect0, trajPt.profileSlotSelect1, trajPt.isLastPoint, trajPt.zeroPos,
+ (int)trajPt.timeDur);
+ return retval;
+}
+/**
+ * Retrieve just the buffer full for the api-level (top) buffer.
+ * This routine performs no CAN or data structure lookups, so its fast and ideal
+ * if caller needs to quickly poll. Otherwise just use GetMotionProfileStatus.
+ * @return number of trajectory points in the top buffer.
+ */
+bool BaseMotorController::IsMotionProfileTopLevelBufferFull() {
+ bool retval = false;
+ c_MotController_IsMotionProfileTopLevelBufferFull(m_handle, &retval);
+ return retval;
+}
+/**
+ * This must be called periodically to funnel the trajectory points from the
+ * API's top level buffer to the controller's bottom level buffer. Recommendation
+ * is to call this twice as fast as the execution rate of the motion profile.
+ * So if MP is running with 20ms trajectory points, try calling this routine
+ * every 10ms. All motion profile functions are thread-safe through the use of
+ * a mutex, so there is no harm in having the caller utilize threading.
+ */
+void BaseMotorController::ProcessMotionProfileBuffer() {
+ c_MotController_ProcessMotionProfileBuffer(m_handle);
+}
+/**
+ * Retrieve all status information.
+ * For best performance, Caller can snapshot all status information regarding the
+ * motion profile executer.
+ *
+ * @param [out] statusToFill Caller supplied object to fill.
+ *
+ * The members are filled, as follows...
+ *
+ * topBufferRem: The available empty slots in the trajectory buffer.
+ * The robot API holds a "top buffer" of trajectory points, so your applicaion
+ * can dump several points at once. The API will then stream them into the
+ * low-level buffer, allowing the motor controller to act on them.
+ *
+ * topBufferRem: The number of points in the top trajectory buffer.
+ *
+ * btmBufferCnt: The number of points in the low level controller buffer.
+ *
+ * hasUnderrun: Set if isUnderrun ever gets set.
+ * Only is cleared by clearMotionProfileHasUnderrun() to ensure
+ *
+ * isUnderrun: This is set if controller needs to shift a point from its buffer into
+ * the active trajectory point however
+ * the buffer is empty.
+ * This gets cleared automatically when is resolved.
+ *
+ * activePointValid: True if the active trajectory point has not empty, false otherwise. The members in activePoint are only valid if this signal is set.
+ *
+ * isLast: is set/cleared based on the MP executer's current
+ * trajectory point's IsLast value. This assumes
+ * IsLast was set when PushMotionProfileTrajectory
+ * was used to insert the currently processed trajectory
+ * point.
+ *
+ * profileSlotSelect0: The currently processed trajectory point's
+ * selected slot. This can differ in the currently selected slot used
+ * for Position and Velocity servo modes. Must be within [0,3].
+*
+ * profileSlotSelect1: The currently processed trajectory point's
+ * selected slot for auxiliary PID. This can differ in the currently selected slot used
+ * for Position and Velocity servo modes. Must be within [0,1].
+ *
+ * outputEnable: The current output mode of the motion profile
+ * executer (disabled, enabled, or hold). When changing the set()
+ * value in MP mode, it's important to check this signal to
+ * confirm the change takes effect before interacting with the top buffer.
+ */
+ErrorCode BaseMotorController::GetMotionProfileStatus(
+ ctre::phoenix::motion::MotionProfileStatus & statusToFill) {
+
+ int outputEnable = 0;
+ ErrorCode retval = c_MotController_GetMotionProfileStatus_2(m_handle,
+ &statusToFill.topBufferRem, &statusToFill.topBufferCnt,
+ &statusToFill.btmBufferCnt, &statusToFill.hasUnderrun,
+ &statusToFill.isUnderrun, &statusToFill.activePointValid,
+ &statusToFill.isLast, &statusToFill.profileSlotSelect0,
+ &outputEnable, &statusToFill.timeDurMs, &statusToFill.profileSlotSelect1);
+
+ statusToFill.outputEnable =
+ (ctre::phoenix::motion::SetValueMotionProfile) outputEnable;
+
+ return retval;
+}
+/**
+ * Clear the "Has Underrun" flag. Typically this is called after application
+ * has confirmed an underrun had occured.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ClearMotionProfileHasUnderrun(int timeoutMs) {
+ return c_MotController_ClearMotionProfileHasUnderrun(m_handle, timeoutMs);
+}
+/**
+ * Calling application can opt to speed up the handshaking between the robot API
+ * and the controller to increase the download rate of the controller's Motion Profile.
+ * Ideally the period should be no more than half the period of a trajectory
+ * point.
+ * @param periodMs The transmit period in ms.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ChangeMotionControlFramePeriod(int periodMs) {
+ return c_MotController_ChangeMotionControlFramePeriod(m_handle, periodMs);
+}
+/**
+ * When trajectory points are processed in the motion profile executer, the MPE determines
+ * how long to apply the active trajectory point by summing baseTrajDurationMs with the
+ * timeDur of the trajectory point (see TrajectoryPoint).
+ *
+ * This allows general selection of the execution rate of the points with 1ms resolution,
+ * while allowing some degree of change from point to point.
+ * @param baseTrajDurationMs The base duration time of every trajectory point.
+ * This is summed with the trajectory points unique timeDur.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigMotionProfileTrajectoryPeriod(int baseTrajDurationMs, int timeoutMs) {
+ return c_MotController_ConfigMotionProfileTrajectoryPeriod(m_handle, baseTrajDurationMs, timeoutMs);
+}
+
+//------ error ----------//
+/**
+ * Gets the last error generated by this object.
+ * Not all functions return an error code but can potentially report errors.
+ * This function can be used to retrieve those error codes.
+ *
+ * @return Last Error Code generated by a function.
+ */
+ErrorCode BaseMotorController::GetLastError() {
+ return c_MotController_GetLastError(m_handle);
+}
+
+//------ Faults ----------//
+/**
+ * Polls the various fault flags.
+ * @param toFill Caller's object to fill with latest fault flags.
+ * @return Last Error Code generated by a function.
+ */
+ErrorCode BaseMotorController::GetFaults(Faults & toFill) {
+ int faultBits;
+ ErrorCode retval = c_MotController_GetFaults(m_handle, &faultBits);
+ toFill = Faults(faultBits);
+ return retval;
+}
+/**
+ * Polls the various sticky fault flags.
+ * @param toFill Caller's object to fill with latest sticky fault flags.
+ * @return Last Error Code generated by a function.
+ */
+ErrorCode BaseMotorController::GetStickyFaults(StickyFaults & toFill) {
+ int faultBits;
+ ErrorCode retval = c_MotController_GetStickyFaults(m_handle, &faultBits);
+ toFill = StickyFaults(faultBits);
+ return retval;
+}
+/**
+ * Clears all sticky faults.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Last Error Code generated by a function.
+ */
+ErrorCode BaseMotorController::ClearStickyFaults(int timeoutMs) {
+ return c_MotController_ClearStickyFaults(m_handle, timeoutMs);
+}
+
+//------ Firmware ----------//
+/**
+ * Gets the firmware version of the device.
+ *
+ * @return Firmware version of device. For example: version 1-dot-2 is 0x0102.
+ */
+int BaseMotorController::GetFirmwareVersion() {
+ int retval = -1;
+ c_MotController_GetFirmwareVersion(m_handle, &retval);
+ return retval;
+}
+/**
+ * Returns true if the device has reset since last call.
+ *
+ * @return Has a Device Reset Occurred?
+ */
+bool BaseMotorController::HasResetOccurred() {
+ bool retval = false;
+ c_MotController_HasResetOccurred(m_handle, &retval);
+ return retval;
+}
+
+//------ Custom Persistent Params ----------//
+/**
+ * Sets the value of a custom parameter. This is for arbitrary use.
+ *
+ * Sometimes it is necessary to save calibration/limit/target
+ * information in the device. Particularly if the
+ * device is part of a subsystem that can be replaced.
+ *
+ * @param newValue
+ * Value for custom parameter.
+ * @param paramIndex
+ * Index of custom parameter [0,1]
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSetCustomParam(int newValue,
+ int paramIndex, int timeoutMs) {
+ return c_MotController_ConfigSetCustomParam(m_handle, newValue, paramIndex,
+ timeoutMs);
+}
+
+/**
+ * Gets the value of a custom parameter.
+ *
+ * @param paramIndex
+ * Index of custom parameter [0,1].
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Value of the custom param.
+ */
+int BaseMotorController::ConfigGetCustomParam(int paramIndex, int timeoutMs) {
+ int readValue;
+ c_MotController_ConfigGetCustomParam(m_handle, &readValue, paramIndex,
+ timeoutMs);
+ return readValue;
+}
+
+//------ Generic Param API ----------//
+/**
+ * Sets a parameter. Generally this is not used.
+ * This can be utilized in
+ * - Using new features without updating API installation.
+ * - Errata workarounds to circumvent API implementation.
+ * - Allows for rapid testing / unit testing of firmware.
+ *
+ * @param param
+ * Parameter enumeration.
+ * @param value
+ * Value of parameter.
+ * @param subValue
+ * Subvalue for parameter. Maximum value of 255.
+ * @param ordinal
+ * Ordinal of parameter.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode BaseMotorController::ConfigSetParameter(ParamEnum param, double value,
+ uint8_t subValue, int ordinal, int timeoutMs) {
+ return c_MotController_ConfigSetParameter(m_handle, param, value, subValue,
+ ordinal, timeoutMs);
+}
+
+/**
+ * Gets a parameter.
+ *
+ * @param param
+ * Parameter enumeration.
+ * @param ordinal
+ * Ordinal of parameter.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Value of parameter.
+ */
+double BaseMotorController::ConfigGetParameter(ParamEnum param, int ordinal,
+ int timeoutMs) {
+ double value = 0;
+ c_MotController_ConfigGetParameter(m_handle, param, &value, ordinal,
+ timeoutMs);
+ return (double) value;
+}
+
+//------ Misc. ----------//
+int BaseMotorController::GetBaseID() {
+ return _arbId;
+}
+/**
+ * @return control mode motor controller is in
+ */
+ControlMode BaseMotorController::GetControlMode() {
+ return m_controlMode;
+}
+
+// ----- Follower ------//
+/**
+ * Set the control mode and output value so that this motor controller will
+ * follow another motor controller. Currently supports following Victor SPX
+ * and Talon SRX.
+ *
+ * @param masterToFollow
+ * Motor Controller object to follow.
+ * @param followerType
+ * Type of following control. Use AuxOutput1 to follow the master
+ * device's auxiliary output 1.
+ * Use PercentOutput for standard follower mode.
+ */
+void BaseMotorController::Follow(IMotorController & masterToFollow, FollowerType followerType) {
+ uint32_t baseId = masterToFollow.GetBaseID();
+ uint32_t id24 = (uint16_t) (baseId >> 0x10);
+ id24 <<= 8;
+ id24 |= (uint8_t) (baseId);
+
+ switch (followerType) {
+ case FollowerType_PercentOutput:
+ Set(ControlMode::Follower, (double) id24);
+ break;
+ case FollowerType_AuxOutput1:
+ /* follow the motor controller, but set the aux flag
+ * to ensure we follow the processed output */
+ Set(ControlMode::Follower, (double) id24, DemandType_AuxPID, 0);
+ break;
+ default:
+ NeutralOutput();
+ break;
+ }
+}
+/**
+ * Set the control mode and output value so that this motor controller will
+ * follow another motor controller.
+ * Currently supports following Victor SPX and Talon SRX.
+ */
+void BaseMotorController::Follow(IMotorController & masterToFollow) {
+ Follow(masterToFollow, FollowerType_PercentOutput);
+}
+/** When master makes a device, this routine is called to signal the update. */
+void BaseMotorController::ValueUpdated() {
+ //do nothing
+}
+/**
+ * @return object that can get/set individual raw sensor values.
+ */
+ctre::phoenix::motorcontrol::SensorCollection & BaseMotorController::GetSensorCollection() {
+ return *_sensorColl;
+}
diff --git a/cpp/src/MotorControl/CAN/TalonSRX.cpp b/cpp/src/MotorControl/CAN/TalonSRX.cpp
new file mode 100644
index 0000000..c18800c
--- /dev/null
+++ b/cpp/src/MotorControl/CAN/TalonSRX.cpp
@@ -0,0 +1,343 @@
+#include "ctre/phoenix/MotorControl/CAN/TalonSRX.h"
+#include "ctre/phoenix/CCI/MotController_CCI.h"
+#include "HAL/HAL.h"
+
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motorcontrol::can;
+using namespace ctre::phoenix::motorcontrol;
+
+/**
+ * Constructor
+ * @param deviceNumber [0,62]
+ */
+TalonSRX::TalonSRX(int deviceNumber) :
+ BaseMotorController(deviceNumber | 0x02040000) {
+ HAL_Report(HALUsageReporting::kResourceType_CANTalonSRX, deviceNumber + 1);
+}
+/**
+ * Select the feedback device for the motor controller.
+ *
+ * @param feedbackDevice
+ * Feedback Device to select.
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigSelectedFeedbackSensor(FeedbackDevice feedbackDevice,
+ int pidIdx, int timeoutMs) {
+ return BaseMotorController::ConfigSelectedFeedbackSensor(feedbackDevice,
+ pidIdx, timeoutMs);
+}
+/**
+ * Select the remote feedback device for the motor controller.
+ * Most CTRE CAN motor controllers will support remote sensors over CAN.
+ *
+ * @param feedbackDevice
+ * Remote Feedback Device to select.
+ * @param pidIdx
+ * 0 for Primary closed-loop. 1 for auxiliary closed-loop.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigSelectedFeedbackSensor(RemoteFeedbackDevice feedbackDevice,
+ int pidIdx, int timeoutMs) {
+ return BaseMotorController::ConfigSelectedFeedbackSensor(feedbackDevice,
+ pidIdx, timeoutMs);
+}
+/**
+ * Sets the period of the given status frame.
+ *
+ * User ensure CAN Bus utilization is not high.
+ *
+ * This setting is not persistent and is lost when device is reset.
+ * If this is a concern, calling application can use HasReset()
+ * to determine if the status frame needs to be reconfigured.
+ *
+ * @param frame
+ * Frame whose period is to be changed.
+ * @param periodMs
+ * Period in ms for the given frame.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::SetStatusFramePeriod(StatusFrameEnhanced frame,
+ int periodMs, int timeoutMs) {
+ return BaseMotorController::SetStatusFramePeriod(frame, periodMs, timeoutMs);
+}
+/**
+ * Sets the period of the given status frame.
+ *
+ * User ensure CAN Bus utilization is not high.
+ *
+ * This setting is not persistent and is lost when device is reset.
+ * If this is a concern, calling application can use HasReset()
+ * to determine if the status frame needs to be reconfigured.
+ *
+ * @param frame
+ * Frame whose period is to be changed.
+ * @param periodMs
+ * Period in ms for the given frame.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::SetStatusFramePeriod(StatusFrame frame,
+ int periodMs, int timeoutMs) {
+ return BaseMotorController::SetStatusFramePeriod(frame, periodMs, timeoutMs);
+}
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ * Frame to get the period of.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int TalonSRX::GetStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs) {
+ return BaseMotorController::GetStatusFramePeriod(frame, timeoutMs);
+}
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ * Frame to get the period of.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int TalonSRX::GetStatusFramePeriod(StatusFrame frame, int timeoutMs) {
+ return BaseMotorController::GetStatusFramePeriod(frame, timeoutMs);
+}
+/**
+ * Configures the period of each velocity sample.
+ * Every 1ms a position value is sampled, and the delta between that sample
+ * and the position sampled kPeriod ms ago is inserted into a filter.
+ * kPeriod is configured with this function.
+ *
+ * @param period
+ * Desired period for the velocity measurement. @see
+ * #VelocityMeasPeriod
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigVelocityMeasurementPeriod(VelocityMeasPeriod period,
+ int timeoutMs) {
+ return BaseMotorController::ConfigVelocityMeasurementPeriod(period,
+ timeoutMs);
+}
+/**
+ * Sets the number of velocity samples used in the rolling average velocity
+ * measurement.
+ *
+ * @param windowSize
+ * Number of samples in the rolling average of velocity
+ * measurement. Valid values are 1,2,4,8,16,32. If another
+ * value is specified, it will truncate to nearest support value.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigVelocityMeasurementWindow(int windowSize,
+ int timeoutMs) {
+ return BaseMotorController::ConfigVelocityMeasurementWindow(windowSize,
+ timeoutMs);
+}
+/**
+ * Configures a limit switch for a local/remote source.
+ *
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon, CANifier, or local Gadgeteer feedback connector.
+ *
+ * If the sensor is remote, a device ID of zero is assumed.
+ * If that's not desired, use the four parameter version of this function.
+ *
+ * @param limitSwitchSource
+ * Limit switch source.
+ * User can choose between the feedback connector, remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ * Setting for normally open, normally closed, or disabled. This setting
+ * matches the web-based configuration drop down.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigForwardLimitSwitchSource(
+ LimitSwitchSource limitSwitchSource,
+ LimitSwitchNormal normalOpenOrClose, int timeoutMs) {
+
+ return BaseMotorController::ConfigForwardLimitSwitchSource(
+ limitSwitchSource, normalOpenOrClose, timeoutMs);
+}
+/**
+ * Configures a limit switch for a local/remote source.
+ *
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon, CANifier, or local Gadgeteer feedback connector.
+ *
+ * If the sensor is remote, a device ID of zero is assumed.
+ * If that's not desired, use the four parameter version of this function.
+ *
+ * @param limitSwitchSource
+ * Limit switch source. @see #LimitSwitchSource
+ * User can choose between the feedback connector, remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ * Setting for normally open, normally closed, or disabled. This setting
+ * matches the web-based configuration drop down.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigReverseLimitSwitchSource(
+ LimitSwitchSource limitSwitchSource,
+ LimitSwitchNormal normalOpenOrClose, int timeoutMs) {
+ return BaseMotorController::ConfigReverseLimitSwitchSource(
+ limitSwitchSource, normalOpenOrClose, timeoutMs);
+}
+/**
+ * Configures the forward limit switch for a remote source.
+ * For example, a CAN motor controller may need to monitor the Limit-F pin
+ * of another Talon or CANifier.
+ *
+ * @param limitSwitchSource
+ * Remote limit switch source.
+ * User can choose between a remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ * Setting for normally open, normally closed, or disabled. This setting
+ * matches the web-based configuration drop down.
+ * @param deviceID
+ * Device ID of remote source (Talon SRX or CANifier device ID).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigForwardLimitSwitchSource(
+ RemoteLimitSwitchSource limitSwitchSource,
+ LimitSwitchNormal normalOpenOrClose, int deviceID, int timeoutMs) {
+
+ return BaseMotorController::ConfigForwardLimitSwitchSource(
+ limitSwitchSource, normalOpenOrClose, deviceID, timeoutMs);
+}
+/**
+ * Configures the reverse limit switch for a remote source.
+ * For example, a CAN motor controller may need to monitor the Limit-R pin
+ * of another Talon or CANifier.
+ *
+ * @param limitSwitchSource
+ * Remote limit switch source.
+ * User can choose between a remote Talon SRX, CANifier, or deactivate the feature.
+ * @param normalOpenOrClose
+ * Setting for normally open, normally closed, or disabled. This setting
+ * matches the web-based configuration drop down.
+ * @param deviceID
+ * Device ID of remote source (Talon SRX or CANifier device ID).
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigReverseLimitSwitchSource(
+ RemoteLimitSwitchSource limitSwitchSource,
+ LimitSwitchNormal normalOpenOrClose, int deviceID, int timeoutMs) {
+
+ return BaseMotorController::ConfigReverseLimitSwitchSource(
+ limitSwitchSource, normalOpenOrClose, deviceID, timeoutMs);
+}
+
+//------ Current Lim ----------//
+/**
+ * Configure the peak allowable current (when current limit is enabled).
+ *
+ * Current limit is activated when current exceeds the peak limit for longer than the peak duration.
+ * Then software will limit to the continuous limit.
+ * This ensures current limiting while allowing for momentary excess current events.
+ *
+ * For simpler current-limiting (single threshold) use ConfigContinuousCurrentLimit()
+ * and set the peak to zero: ConfigPeakCurrentLimit(0).
+ *
+ * @param amps Amperes to limit.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigPeakCurrentLimit(int amps, int timeoutMs) {
+ return c_MotController_ConfigPeakCurrentLimit(m_handle, amps, timeoutMs);
+}
+/**
+ * Configure the peak allowable duration (when current limit is enabled).
+ *
+ * Current limit is activated when current exceeds the peak limit for longer than the peak duration.
+ * Then software will limit to the continuous limit.
+ * This ensures current limiting while allowing for momentary excess current events.
+ *
+ * For simpler current-limiting (single threshold) use ConfigContinuousCurrentLimit()
+ * and set the peak to zero: ConfigPeakCurrentLimit(0).
+ *
+ * @param milliseconds How long to allow current-draw past peak limit.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigPeakCurrentDuration(int milliseconds, int timeoutMs) {
+ return c_MotController_ConfigPeakCurrentDuration(m_handle, milliseconds,
+ timeoutMs);
+}
+/**
+ * Configure the continuous allowable current-draw (when current limit is enabled).
+ *
+ * Current limit is activated when current exceeds the peak limit for longer than the peak duration.
+ * Then software will limit to the continuous limit.
+ * This ensures current limiting while allowing for momentary excess current events.
+ *
+ * For simpler current-limiting (single threshold) use ConfigContinuousCurrentLimit()
+ * and set the peak to zero: ConfigPeakCurrentLimit(0).
+ *
+ * @param amps Amperes to limit.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ */
+ctre::phoenix::ErrorCode TalonSRX::ConfigContinuousCurrentLimit(int amps, int timeoutMs) {
+ return c_MotController_ConfigContinuousCurrentLimit(m_handle, amps, timeoutMs);
+}
+/**
+ * Enable or disable Current Limit.
+ * @param enable
+ * Enable state of current limit.
+ * @see ConfigPeakCurrentLimit, ConfigPeakCurrentDuration, ConfigContinuousCurrentLimit
+ */
+void TalonSRX::EnableCurrentLimit(bool enable) {
+ c_MotController_EnableCurrentLimit(m_handle, enable);
+}
+
diff --git a/cpp/src/MotorControl/CAN/VictorSPX.cpp b/cpp/src/MotorControl/CAN/VictorSPX.cpp
new file mode 100644
index 0000000..11bcdf8
--- /dev/null
+++ b/cpp/src/MotorControl/CAN/VictorSPX.cpp
@@ -0,0 +1,13 @@
+#include "ctre/phoenix/MotorControl/CAN/VictorSPX.h"
+#include "HAL/HAL.h"
+
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motorcontrol::can;
+/**
+ * Constructor
+ * @param deviceNumber [0,62]
+ */
+VictorSPX::VictorSPX(int deviceNumber) :
+ BaseMotorController(deviceNumber | 0x01040000) {
+ HAL_Report(HALUsageReporting::kResourceType_CTRE_future1, deviceNumber + 1);
+ }
diff --git a/cpp/src/MotorControl/CAN/WPI_TalonSRX.cpp b/cpp/src/MotorControl/CAN/WPI_TalonSRX.cpp
new file mode 100644
index 0000000..30a8bc9
--- /dev/null
+++ b/cpp/src/MotorControl/CAN/WPI_TalonSRX.cpp
@@ -0,0 +1,157 @@
+/**
+ * WPI Compliant motor controller class.
+ * WPILIB's object model requires many interfaces to be implemented to use
+ * the various features.
+ * This includes...
+ * - Software PID loops running in the robot controller
+ * - LiveWindow/Test mode features
+ * - Motor Safety (auto-turn off of motor if Set stops getting called)
+ * - Single Parameter set that assumes a simple motor controller.
+ */
+#include "ctre/phoenix/MotorControl/CAN/WPI_TalonSRX.h"
+#include "ctre/phoenix/MotorControl/CAN/BaseMotorController.h"
+#include "HAL/HAL.h"
+#include <sstream>
+
+using namespace ctre::phoenix::motorcontrol::can;
+
+
+/** Constructor */
+WPI_TalonSRX::WPI_TalonSRX(int deviceNumber) :
+ BaseMotorController(deviceNumber | 0x02040000),
+ TalonSRX(deviceNumber),
+ _safetyHelper(this) {
+ HAL_Report(HALUsageReporting::kResourceType_CTRE_future2, deviceNumber + 1);
+ /* build string description */
+ std::stringstream work;
+ work << "Talon SRX " << deviceNumber;
+ _desc = work.str();
+ /* prep motor safety */
+ _safetyHelper.SetExpiration(0.0);
+ _safetyHelper.SetSafetyEnabled(false);
+ SetName("Talon SRX ", deviceNumber);
+}
+WPI_TalonSRX::~WPI_TalonSRX() {
+ /* MT */
+}
+
+//----------------------- set/get routines for WPILIB interfaces -------------------//
+/**
+ * Common interface for setting the speed of a simple speed controller.
+ *
+ * @param speed The speed to set. Value should be between -1.0 and 1.0.
+ * Value is also saved for Get().
+ */
+void WPI_TalonSRX::Set(double speed) {
+ _speed = speed;
+ TalonSRX::Set(ControlMode::PercentOutput, speed);
+ _safetyHelper.Feed();
+}
+void WPI_TalonSRX::PIDWrite(double output) {
+ Set(output);
+}
+
+/**
+ * Common interface for getting the current set speed of a speed controller.
+ *
+ * @return The current set speed. Value is between -1.0 and 1.0.
+ */
+double WPI_TalonSRX::Get() const {
+ return _speed;
+}
+
+//----------------------- Intercept CTRE calls for motor safety -------------------//
+void WPI_TalonSRX::Set(ControlMode mode, double value) {
+ /* intercept the advanced Set and feed motor-safety */
+ TalonSRX::Set(mode, value);
+ _safetyHelper.Feed();
+}
+void WPI_TalonSRX::Set(ControlMode mode, double demand0, double demand1) {
+ /* intercept the advanced Set and feed motor-safety */
+ TalonSRX::Set(mode, demand0, demand1);
+ _safetyHelper.Feed();
+}
+//----------------------- Invert routines -------------------//
+/**
+ * Common interface for inverting direction of a speed controller.
+ *
+ * @param isInverted The state of inversion, true is inverted.
+ */
+void WPI_TalonSRX::SetInverted(bool isInverted) {
+ TalonSRX::SetInverted(isInverted);
+}
+/**
+ * Common interface for returning the inversion state of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ */
+bool WPI_TalonSRX::GetInverted() const {
+ return BaseMotorController::GetInverted();
+}
+//----------------------- turn-motor-off routines-------------------//
+/**
+ * Common interface for disabling a motor.
+ */
+void WPI_TalonSRX::Disable() {
+ NeutralOutput();
+}
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void WPI_TalonSRX::StopMotor() {
+ NeutralOutput();
+}
+
+//----------------------- Motor Safety-------------------//
+
+/**
+ * Set the safety expiration time.
+ *
+ * @param timeout The timeout (in seconds) for this motor object
+ */
+void WPI_TalonSRX::SetExpiration(double timeout) {
+ _safetyHelper.SetExpiration(timeout);
+}
+
+/**
+ * Return the safety expiration time.
+ *
+ * @return The expiration time value.
+ */
+double WPI_TalonSRX::GetExpiration() const {
+ return _safetyHelper.GetExpiration();
+}
+
+/**
+ * Check if the motor is currently alive or stopped due to a timeout.
+ *
+ * @return a bool value that is true if the motor has NOT timed out and should
+ * still be running.
+ */
+bool WPI_TalonSRX::IsAlive() const {
+ return _safetyHelper.IsAlive();
+}
+
+/**
+ * Check if motor safety is enabled.
+ *
+ * @return True if motor safety is enforced for this object
+ */
+bool WPI_TalonSRX::IsSafetyEnabled() const {
+ return _safetyHelper.IsSafetyEnabled();
+}
+
+void WPI_TalonSRX::SetSafetyEnabled(bool enabled) {
+ _safetyHelper.SetSafetyEnabled(enabled);
+}
+
+void WPI_TalonSRX::GetDescription(llvm::raw_ostream& desc) const {
+ desc << _desc.c_str();
+}
+
+void WPI_TalonSRX::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Speed Controller");
+ builder.SetSafeState([=]() {StopMotor();});
+ builder.AddDoubleProperty("Value", [=]() {return Get();},
+ [=](double value) {Set(value);});
+}
diff --git a/cpp/src/MotorControl/CAN/WPI_VictorSPX.cpp b/cpp/src/MotorControl/CAN/WPI_VictorSPX.cpp
new file mode 100644
index 0000000..cc8ddbd
--- /dev/null
+++ b/cpp/src/MotorControl/CAN/WPI_VictorSPX.cpp
@@ -0,0 +1,157 @@
+/**
+ * WPI Compliant motor controller class.
+ * WPILIB's object model requires many interfaces to be implemented to use
+ * the various features.
+ * This includes...
+ * - Software PID loops running in the robot controller
+ * - LiveWindow/Test mode features
+ * - Motor Safety (auto-turn off of motor if Set stops getting called)
+ * - Single Parameter set that assumes a simple motor controller.
+ */
+#include "ctre/phoenix/MotorControl/CAN/WPI_VictorSPX.h"
+#include "ctre/phoenix/MotorControl/CAN/BaseMotorController.h"
+#include "HAL/HAL.h"
+#include <sstream>
+
+using namespace ctre::phoenix::motorcontrol::can;
+
+
+/** Constructor */
+WPI_VictorSPX::WPI_VictorSPX(int deviceNumber) :
+ BaseMotorController(deviceNumber | 0x01040000),
+ VictorSPX(deviceNumber),
+ _safetyHelper(this) {
+ HAL_Report(HALUsageReporting::kResourceType_CTRE_future3, deviceNumber + 1);
+ /* build string description */
+ std::stringstream work;
+ work << "Victor SPX " << deviceNumber;
+ _desc = work.str();
+ /* prep motor safety */
+ _safetyHelper.SetExpiration(0.0);
+ _safetyHelper.SetSafetyEnabled(false);
+ SetName("Victor SPX ", deviceNumber);
+}
+WPI_VictorSPX::~WPI_VictorSPX() {
+ /* MT */
+}
+
+//----------------------- set/get routines for WPILIB interfaces -------------------//
+/**
+ * Common interface for setting the speed of a simple speed controller.
+ *
+ * @param speed The speed to set. Value should be between -1.0 and 1.0.
+ * Value is also saved for Get().
+ */
+void WPI_VictorSPX::Set(double speed) {
+ _speed = speed;
+ VictorSPX::Set(ControlMode::PercentOutput, speed);
+ _safetyHelper.Feed();
+}
+void WPI_VictorSPX::PIDWrite(double output) {
+ Set(output);
+}
+
+/**
+ * Common interface for getting the current set speed of a speed controller.
+ *
+ * @return The current set speed. Value is between -1.0 and 1.0.
+ */
+double WPI_VictorSPX::Get() const {
+ return _speed;
+}
+
+//----------------------- Intercept CTRE calls for motor safety -------------------//
+void WPI_VictorSPX::Set(ControlMode mode, double value) {
+ /* intercept the advanced Set and feed motor-safety */
+ VictorSPX::Set(mode, value);
+ _safetyHelper.Feed();
+}
+void WPI_VictorSPX::Set(ControlMode mode, double demand0, double demand1) {
+ /* intercept the advanced Set and feed motor-safety */
+ VictorSPX::Set(mode, demand0, demand1);
+ _safetyHelper.Feed();
+}
+//----------------------- Invert routines -------------------//
+/**
+ * Common interface for inverting direction of a speed controller.
+ *
+ * @param isInverted The state of inversion, true is inverted.
+ */
+void WPI_VictorSPX::SetInverted(bool isInverted) {
+ VictorSPX::SetInverted(isInverted);
+}
+/**
+ * Common interface for returning the inversion state of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ */
+bool WPI_VictorSPX::GetInverted() const {
+ return BaseMotorController::GetInverted();
+}
+//----------------------- turn-motor-off routines-------------------//
+/**
+ * Common interface for disabling a motor.
+ */
+void WPI_VictorSPX::Disable() {
+ NeutralOutput();
+}
+/**
+ * Common interface to stop the motor until Set is called again.
+ */
+void WPI_VictorSPX::StopMotor() {
+ NeutralOutput();
+}
+
+//----------------------- Motor Safety-------------------//
+
+/**
+ * Set the safety expiration time.
+ *
+ * @param timeout The timeout (in seconds) for this motor object
+ */
+void WPI_VictorSPX::SetExpiration(double timeout) {
+ _safetyHelper.SetExpiration(timeout);
+}
+
+/**
+ * Return the safety expiration time.
+ *
+ * @return The expiration time value.
+ */
+double WPI_VictorSPX::GetExpiration() const {
+ return _safetyHelper.GetExpiration();
+}
+
+/**
+ * Check if the motor is currently alive or stopped due to a timeout.
+ *
+ * @return a bool value that is true if the motor has NOT timed out and should
+ * still be running.
+ */
+bool WPI_VictorSPX::IsAlive() const {
+ return _safetyHelper.IsAlive();
+}
+
+/**
+ * Check if motor safety is enabled.
+ *
+ * @return True if motor safety is enforced for this object
+ */
+bool WPI_VictorSPX::IsSafetyEnabled() const {
+ return _safetyHelper.IsSafetyEnabled();
+}
+
+void WPI_VictorSPX::SetSafetyEnabled(bool enabled) {
+ _safetyHelper.SetSafetyEnabled(enabled);
+}
+
+void WPI_VictorSPX::GetDescription(llvm::raw_ostream& desc) const {
+ desc << _desc.c_str();
+}
+
+void WPI_VictorSPX::InitSendable(frc::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Speed Controller");
+ builder.SetSafeState([=]() {StopMotor();});
+ builder.AddDoubleProperty("Value", [=]() {return Get();},
+ [=](double value) {Set(value);});
+}
diff --git a/cpp/src/MotorControl/DeviceCatalog.cpp b/cpp/src/MotorControl/DeviceCatalog.cpp
new file mode 100644
index 0000000..c59f43a3
--- /dev/null
+++ b/cpp/src/MotorControl/DeviceCatalog.cpp
@@ -0,0 +1,6 @@
+#include <ctre/phoenix/MotorControl/DeviceCatalog.h>
+
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motorcontrol;
+
+DeviceCatalog * DeviceCatalog::_instance = 0;
diff --git a/cpp/src/MotorControl/GroupMotorControllers.cpp b/cpp/src/MotorControl/GroupMotorControllers.cpp
new file mode 100644
index 0000000..d2e126a
--- /dev/null
+++ b/cpp/src/MotorControl/GroupMotorControllers.cpp
@@ -0,0 +1,25 @@
+#include "ctre/phoenix/MotorControl/GroupMotorControllers.h"
+
+using namespace ctre::phoenix;
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+std::vector<IMotorController*> GroupMotorControllers::_mcs;
+
+void GroupMotorControllers::Register(IMotorController *motorController) {
+ _mcs.push_back(motorController);
+}
+
+int GroupMotorControllers::MotorControllerCount() {
+ return _mcs.size();
+}
+
+IMotorController* GroupMotorControllers::Get(int idx) {
+ return _mcs[idx];
+}
+
+}
+}
+}
diff --git a/cpp/src/MotorControl/SensorCollection.cpp b/cpp/src/MotorControl/SensorCollection.cpp
new file mode 100644
index 0000000..a1ff1b1
--- /dev/null
+++ b/cpp/src/MotorControl/SensorCollection.cpp
@@ -0,0 +1,241 @@
+#include "ctre/phoenix/MotorControl/SensorCollection.h"
+#include "ctre/phoenix/CCI/MotController_CCI.h"
+
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motorcontrol;
+
+SensorCollection::SensorCollection(void * handle) {
+ _handle = handle;
+}
+
+/**
+ * Get the position of whatever is in the analog pin of the Talon, regardless of
+ * whether it is actually being used for feedback.
+ *
+ * @return 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 SensorCollection::GetAnalogIn() {
+ int retval = 0;
+ c_MotController_GetAnalogIn(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Sets analog position.
+ *
+ * @param newPosition The new position.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ *
+ * @return an ErrorCode.
+ */
+
+ErrorCode SensorCollection::SetAnalogPosition(int newPosition, int timeoutMs) {
+ return c_MotController_SetAnalogPosition(_handle, newPosition, timeoutMs);
+}
+
+/**
+ * Get the position of whatever is in the analog pin of the Talon, regardless of whether
+ * it is actually being used for feedback.
+ *
+ * @return the ADC (0 - 1023) on analog pin of the Talon.
+ */
+
+int SensorCollection::GetAnalogInRaw() {
+ int retval = 0;
+ c_MotController_GetAnalogInRaw(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Get the velocity of whatever is in the analog pin of the Talon, regardless of
+ * whether it is actually being used for feedback.
+ *
+ * @return the speed in units per 100ms where 1024 units is one rotation.
+ */
+
+int SensorCollection::GetAnalogInVel() {
+ int retval = 0;
+ c_MotController_GetAnalogInVel(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Get the quadrature position of the Talon, regardless of whether
+ * it is actually being used for feedback.
+ *
+ * @return the quadrature position.
+ */
+
+int SensorCollection::GetQuadraturePosition() {
+ int retval = 0;
+ c_MotController_GetQuadraturePosition(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Change the quadrature reported position. Typically this is used to "zero" the
+ * sensor. This only works with Quadrature sensor. To set the selected sensor position
+ * regardless of what type it is, see SetSelectedSensorPosition in the motor controller class.
+ *
+ * @param newPosition The position value to apply to the sensor.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ *
+ * @return error code.
+ */
+
+ErrorCode SensorCollection::SetQuadraturePosition(int newPosition,
+ int timeoutMs) {
+ return c_MotController_SetQuadraturePosition(_handle, newPosition,
+ timeoutMs);
+}
+
+/**
+ * Get the quadrature velocity, regardless of whether
+ * it is actually being used for feedback.
+ *
+ * @return the quadrature velocity in units per 100ms.
+ */
+
+int SensorCollection::GetQuadratureVelocity() {
+ int retval = 0;
+ c_MotController_GetQuadratureVelocity(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Gets pulse width position, regardless of whether
+ * it is actually being used for feedback.
+ *
+ * @return the pulse width position.
+ */
+
+int SensorCollection::GetPulseWidthPosition() {
+ int retval = 0;
+ c_MotController_GetPulseWidthPosition(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Sets pulse width position.
+ *
+ * @param newPosition The position value to apply to the sensor.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ *
+ * @return an ErrErrorCode
+ */
+ErrorCode SensorCollection::SetPulseWidthPosition(int newPosition,
+ int timeoutMs) {
+ return c_MotController_SetPulseWidthPosition(_handle, newPosition,
+ timeoutMs);
+}
+
+/**
+ * Gets pulse width velocity, regardless of whether
+ * it is actually being used for feedback.
+ *
+ * @return the pulse width velocity in units per 100ms (where 4096 units is 1 rotation).
+ */
+
+int SensorCollection::GetPulseWidthVelocity() {
+ int retval = 0;
+ c_MotController_GetPulseWidthVelocity(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Gets pulse width rise to fall time.
+ *
+ * @return the pulse width rise to fall time in microseconds.
+ */
+
+int SensorCollection::GetPulseWidthRiseToFallUs() {
+ int retval = 0;
+ c_MotController_GetPulseWidthRiseToFallUs(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Gets pulse width rise to rise time.
+ *
+ * @return the pulse width rise to rise time in microseconds.
+ */
+
+int SensorCollection::GetPulseWidthRiseToRiseUs() {
+ int retval = 0;
+ c_MotController_GetPulseWidthRiseToRiseUs(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Gets pin state quad a.
+ *
+ * @return the pin state of quad a (1 if asserted, 0 if not asserted).
+ */
+
+int SensorCollection::GetPinStateQuadA() {
+ int retval = 0;
+ c_MotController_GetPinStateQuadA(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Gets pin state quad b.
+ *
+ * @return Digital level of QUADB pin (1 if asserted, 0 if not asserted).
+ */
+
+int SensorCollection::GetPinStateQuadB() {
+ int retval = 0;
+ c_MotController_GetPinStateQuadB(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Gets pin state quad index.
+ *
+ * @return Digital level of QUAD Index pin (1 if asserted, 0 if not asserted).
+ */
+
+int SensorCollection::GetPinStateQuadIdx() {
+ int retval = 0;
+ c_MotController_GetPinStateQuadIdx(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Is forward limit switch closed.
+ *
+ * @return '1' iff forward limit switch is closed, 0 iff switch is open. This function works
+ * regardless if limit switch feature is enabled.
+ */
+
+int SensorCollection::IsFwdLimitSwitchClosed() {
+ int retval = 0;
+ c_MotController_IsFwdLimitSwitchClosed(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Is reverse limit switch closed.
+ *
+ * @return '1' iff reverse limit switch is closed, 0 iff switch is open. This function works
+ * regardless if limit switch feature is enabled.
+ */
+
+int SensorCollection::IsRevLimitSwitchClosed() {
+ int retval = 0;
+ c_MotController_IsRevLimitSwitchClosed(_handle, &retval);
+ return retval;
+}
diff --git a/cpp/src/RCRadio3Ch.cpp b/cpp/src/RCRadio3Ch.cpp
new file mode 100644
index 0000000..2fea763
--- /dev/null
+++ b/cpp/src/RCRadio3Ch.cpp
@@ -0,0 +1,113 @@
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+
+#include "ctre/phoenix/RCRadio3Ch.h"
+#include <vector>
+
+namespace ctre {
+namespace phoenix {
+
+RCRadio3Ch::RCRadio3Ch(ctre::phoenix::CANifier *canifier) {
+ _canifier = canifier;
+}
+
+float RCRadio3Ch::GetDutyCycleUs(Channel channel) {
+ return _dutyCycleAndPeriods[(int) channel][0];
+}
+
+float RCRadio3Ch::GetDutyCyclePerc(Channel channel) {
+ float retval = RCRadio3Ch::GetDutyCycleUs(channel);
+
+ std::vector<double> xData = { 1000, 2000 };
+ std::vector<double> yData = { -1, 1 };
+
+ retval = RCRadio3Ch::Interpolate(xData, yData, retval, true);
+
+ if (retval < -1) {
+ retval = -1;
+ } else if (retval > +1) {
+ retval = +1;
+ }
+
+ return retval;
+}
+
+bool RCRadio3Ch::GetSwitchValue(Channel channel) {
+ float retval = RCRadio3Ch::GetDutyCyclePerc(channel);
+
+ return retval > 0.5f;
+}
+
+float RCRadio3Ch::GetPeriodUs(Channel channel) {
+ return _dutyCycleAndPeriods[(int) channel][1];
+}
+
+void RCRadio3Ch::Process() {
+ //Does some error code stuff, which we don't have...
+ _errorCodes[0] = _canifier->GetPWMInput(
+ ctre::phoenix::CANifier::PWMChannel::PWMChannel0, _dutyCycleAndPeriods[0]);
+ _errorCodes[1] = _canifier->GetPWMInput(
+ ctre::phoenix::CANifier::PWMChannel::PWMChannel1, _dutyCycleAndPeriods[1]);
+ _errorCodes[2] = _canifier->GetPWMInput(
+ ctre::phoenix::CANifier::PWMChannel::PWMChannel2, _dutyCycleAndPeriods[2]);
+ _errorCodes[3] = _canifier->GetPWMInput(
+ ctre::phoenix::CANifier::PWMChannel::PWMChannel3, _dutyCycleAndPeriods[3]);
+
+ Status health = Status::Okay;
+ if (health == Status::Okay) {
+ if (_errorCodes[0] < 0) {
+ health = Status::LossOfCAN;
+ }
+ if (_errorCodes[1] < 0) {
+ health = Status::LossOfCAN;
+ }
+ if (_errorCodes[2] < 0) {
+ health = Status::LossOfCAN;
+ }
+ if (_errorCodes[3] < 0) {
+ health = Status::LossOfCAN;
+ }
+ }
+
+ if (health == Status::Okay) {
+ if (RCRadio3Ch::GetPeriodUs(RCRadio3Ch::Channel1) == 0) {
+ health = Status::LossOfPwm;
+ }
+ if (RCRadio3Ch::GetPeriodUs(RCRadio3Ch::Channel2) == 0) {
+ health = Status::LossOfPwm;
+ }
+ if (RCRadio3Ch::GetPeriodUs(RCRadio3Ch::Channel3) == 0) {
+ health = Status::LossOfPwm;
+ }
+ }
+ CurrentStatus = health; //Will have to change this to a getter and a setter
+}
+
+double RCRadio3Ch::Interpolate(std::vector<double> &xData,
+ std::vector<double> &yData, double x, bool extrapolate) {
+ int size = xData.size();
+
+ int i = 0; // find left end of interval for interpolation
+ if (x >= xData[size - 2]) // special case: beyond right end
+ {
+ i = size - 2;
+ } else {
+ while (x > xData[i + 1])
+ i++;
+ }
+ double xL = xData[i], yL = yData[i], xR = xData[i + 1], yR = yData[i + 1]; // points on either side (unless beyond ends)
+ if (!extrapolate) // if beyond ends of array and not extrapolating
+ {
+ if (x < xL)
+ yR = yL;
+ if (x > xR)
+ yL = yR;
+ }
+
+ double dydx = (yR - yL) / (xR - xL); // gradient
+
+ return yL + dydx * (x - xL);
+}
+
+}
+}
+#endif
diff --git a/cpp/src/Sensors/PigeonIMU.cpp b/cpp/src/Sensors/PigeonIMU.cpp
new file mode 100644
index 0000000..4755e43
--- /dev/null
+++ b/cpp/src/Sensors/PigeonIMU.cpp
@@ -0,0 +1,779 @@
+/*
+ * Software License Agreement
+ *
+ * Copyright (C) Cross The Road Electronics. All rights
+ * reserved.
+ *
+ * Cross The Road Electronics (CTRE) licenses to you the right to
+ * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software
+ * API Libraries ONLY when in use with Cross The Road Electronics hardware products.
+ *
+ * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
+ * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
+ * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
+ * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL,
+ * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
+ * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
+ * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
+ * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
+ * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
+ * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE
+ */
+
+#include <memory>
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+#include "ctre/phoenix/Sensors/PigeonIMU.h"
+#include "ctre/phoenix/CTRLogger.h"
+#include "ctre/phoenix/CCI/Logger_CCI.h"
+#include "ctre/phoenix/CCI/PigeonIMU_CCI.h"
+#include "ctre/phoenix/MotorControl/CAN/TalonSRX.h"
+
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+
+#include "Utility.h"
+#include "HAL/HAL.h"
+
+using namespace ctre::phoenix::motorcontrol::can;
+
+namespace ctre {
+namespace phoenix {
+namespace sensors {
+
+/**
+ * Create a Pigeon object that communicates with Pigeon on CAN Bus.
+ * @param deviceNumber CAN Device Id of Pigeon [0,62]
+ */
+PigeonIMU::PigeonIMU(int deviceNumber) :
+ CANBusAddressable(deviceNumber) {
+ _handle = c_PigeonIMU_Create1(deviceNumber);
+ _deviceNumber = deviceNumber;
+ HAL_Report(HALUsageReporting::kResourceType_PigeonIMU, _deviceNumber + 1);
+}
+
+/**
+ * Create a Pigeon object that communciates with Pigeon through the Gadgeteer ribbon cable connected to a Talon on CAN Bus.
+ * @param talonSrx Object for the TalonSRX connected via ribbon cable.
+ */
+PigeonIMU::PigeonIMU(ctre::phoenix::motorcontrol::can::TalonSRX * talonSrx) :
+ CANBusAddressable(0) {
+ _handle = c_PigeonIMU_Create2(talonSrx->GetDeviceID());
+ _deviceNumber = talonSrx->GetDeviceID();
+ HAL_Report(HALUsageReporting::kResourceType_PigeonIMU, _deviceNumber + 1);
+ HAL_Report(HALUsageReporting::kResourceType_CTRE_future0, _deviceNumber + 1); //record as Pigeon-via-Uart
+}
+
+/**
+ * Sets the Yaw register to the specified value.
+ *
+ * @param angleDeg Degree of Yaw [+/- 23040 degrees]
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetYaw(double angleDeg, int timeoutMs) {
+ int errCode = c_PigeonIMU_SetYaw(_handle, angleDeg, timeoutMs);
+ return errCode;
+}
+/**
+ * Atomically add to the Yaw register.
+ *
+ * @param angleDeg Degrees to add to the Yaw register.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::AddYaw(double angleDeg, int timeoutMs) {
+ int errCode = c_PigeonIMU_AddYaw(_handle, angleDeg, timeoutMs);
+ return errCode;
+}
+/**
+ * Sets the Yaw register to match the current compass value.
+ *
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetYawToCompass(int timeoutMs) {
+ int errCode = c_PigeonIMU_SetYawToCompass(_handle, timeoutMs);
+ return errCode;
+}
+/**
+ * Sets the Fused Heading to the specified value.
+ *
+ * @param angleDeg Degree of heading [+/- 23040 degrees]
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetFusedHeading(double angleDeg, int timeoutMs) {
+ int errCode = c_PigeonIMU_SetFusedHeading(_handle, angleDeg, timeoutMs);
+ return errCode;
+}
+/**
+ * Sets the AccumZAngle.
+ *
+ * @param angleDeg Degrees to set AccumZAngle to.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetAccumZAngle(double angleDeg, int timeoutMs) {
+ int errCode = c_PigeonIMU_SetAccumZAngle(_handle, angleDeg, timeoutMs);
+ return errCode;
+}
+/**
+ * Enable/Disable Temp compensation. Pigeon defaults with this on at boot.
+ *
+ * @param bTempCompEnable Set to "True" to enable temperature compensation.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::ConfigTemperatureCompensationEnable(bool bTempCompEnable,
+ int timeoutMs) {
+ int errCode = c_PigeonIMU_ConfigTemperatureCompensationEnable(_handle,
+ bTempCompEnable, timeoutMs);
+ return errCode;
+}
+/**
+ * Atomically add to the Fused Heading register.
+ *
+ * @param angleDeg Degrees to add to the Fused Heading register.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::AddFusedHeading(double angleDeg, int timeoutMs) {
+ int errCode = c_PigeonIMU_AddFusedHeading(_handle, angleDeg, timeoutMs);
+ return errCode;
+}
+/**
+ * Sets the Fused Heading register to match the current compass value.
+ *
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetFusedHeadingToCompass(int timeoutMs) {
+ int errCode = c_PigeonIMU_SetFusedHeadingToCompass(_handle, timeoutMs);
+ return errCode;
+}
+/**
+ * Set the declination for compass. Declination is the difference between
+ * Earth Magnetic north, and the geographic "True North".
+ *
+ * @param angleDegOffset Degrees to set Compass Declination to.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetCompassDeclination(double angleDegOffset, int timeoutMs) {
+ int errCode = c_PigeonIMU_SetCompassDeclination(_handle, angleDegOffset,
+ timeoutMs);
+ return errCode;
+}
+/**
+ * Sets the compass angle. Although compass is absolute [0,360) degrees, the
+ * continuous compass register holds the wrap-arounds.
+ *
+ * @param angleDeg Degrees to set continuous compass angle to.
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::SetCompassAngle(double angleDeg, int timeoutMs) {
+ int errCode = c_PigeonIMU_SetCompassAngle(_handle, angleDeg, timeoutMs);
+ return errCode;
+}
+//----------------------- Calibration routines -----------------------//
+/**
+ * Enters the Calbration mode. See the Pigeon IMU documentation for More
+ * information on Calibration.
+ *
+ * @param calMode Calibration to execute
+ * @param timeoutMs
+ * Timeout value in ms. If nonzero, function will wait for
+ * config success and report an error if it times out.
+ * If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::EnterCalibrationMode(CalibrationMode calMode, int timeoutMs) {
+ return c_PigeonIMU_EnterCalibrationMode(_handle, calMode, timeoutMs);
+}
+/**
+ * Get the status of the current (or previousley complete) calibration.
+ *
+ * @param statusToFill Container for the status information.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+int PigeonIMU::GetGeneralStatus(PigeonIMU::GeneralStatus & statusToFill) {
+ int state;
+ int currentMode;
+ int calibrationError;
+ int bCalIsBooting;
+ double tempC;
+ int upTimeSec;
+ int noMotionBiasCount;
+ int tempCompensationCount;
+ int lastError;
+
+ int errCode = c_PigeonIMU_GetGeneralStatus(_handle, &state, ¤tMode,
+ &calibrationError, &bCalIsBooting, &tempC, &upTimeSec,
+ &noMotionBiasCount, &tempCompensationCount, &lastError);
+
+ statusToFill.currentMode = (PigeonIMU::CalibrationMode) currentMode;
+ statusToFill.calibrationError = calibrationError;
+ statusToFill.bCalIsBooting = bCalIsBooting;
+ statusToFill.state = (PigeonIMU::PigeonState) state;
+ statusToFill.tempC = tempC;
+ statusToFill.noMotionBiasCount = noMotionBiasCount;
+ statusToFill.tempCompensationCount = tempCompensationCount;
+ statusToFill.upTimeSec = upTimeSec;
+ statusToFill.lastError = errCode;
+
+ /* build description string */
+ if (errCode != 0) { // same as NoComm
+ statusToFill.description =
+ "Status frame was not received, check wired connections and web-based config.";
+ } else if (statusToFill.bCalIsBooting) {
+ statusToFill.description =
+ "Pigeon is boot-caling to properly bias accel and gyro. Do not move Pigeon. When finished biasing, calibration mode will start.";
+ } else if (statusToFill.state == UserCalibration) {
+ /* mode specific descriptions */
+ switch (currentMode) {
+ case BootTareGyroAccel:
+ statusToFill.description =
+ "Boot-Calibration: Gyro and Accelerometer are being biased.";
+ break;
+ case Temperature:
+ statusToFill.description =
+ "Temperature-Calibration: Pigeon is collecting temp data and will finish when temp range is reached. "
+ "Do not move Pigeon.";
+ break;
+ case Magnetometer12Pt:
+ statusToFill.description =
+ "Magnetometer Level 1 calibration: Orient the Pigeon PCB in the 12 positions documented in the User's Manual.";
+ break;
+ case Magnetometer360:
+ statusToFill.description =
+ "Magnetometer Level 2 calibration: Spin robot slowly in 360' fashion. ";
+ break;
+ case Accelerometer:
+ statusToFill.description =
+ "Accelerometer Calibration: Pigeon PCB must be placed on a level source. Follow User's Guide for how to level surfacee. ";
+ break;
+ }
+ } else if (statusToFill.state == Ready) {
+ /* definitely not doing anything cal-related. So just instrument the motion driver state */
+ statusToFill.description =
+ "Pigeon is running normally. Last CAL error code was ";
+ statusToFill.description += std::to_string(calibrationError);
+ statusToFill.description += ".";
+ } else if (statusToFill.state == Initializing) {
+ /* definitely not doing anything cal-related. So just instrument the motion driver state */
+ statusToFill.description =
+ "Pigeon is boot-caling to properly bias accel and gyro. Do not move Pigeon.";
+ } else {
+ statusToFill.description = "Not enough data to determine status.";
+ }
+
+ return errCode;
+}
+//----------------------- General Error status -----------------------//
+/**
+ * Call GetLastError() generated by this object.
+ * Not all functions return an error code but can
+ * potentially report errors.
+ *
+ * This function can be used to retrieve those error codes.
+ *
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetLastError() {
+ return c_PigeonIMU_GetLastError(_handle);
+}
+
+
+//----------------------- Strongly typed Signal decoders -----------------------//
+/**
+ * Get 6d Quaternion data.
+ *
+ * @param wxyz Array to fill with quaternion data w[0], x[1], y[2], z[3]
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::Get6dQuaternion(double wxyz[4]) {
+ int errCode = c_PigeonIMU_Get6dQuaternion(_handle, wxyz);
+ return errCode;
+}
+/**
+ * Get Yaw, Pitch, and Roll data.
+ *
+ * @param ypr Array to fill with yaw[0], pitch[1], and roll[2] data
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetYawPitchRoll(double ypr[3]) {
+ int errCode = c_PigeonIMU_GetYawPitchRoll(_handle, ypr);
+ return errCode;
+}
+/**
+ * Get AccumGyro data.
+ * AccumGyro is the integrated gyro value on each axis.
+ *
+ * @param xyz_deg Array to fill with x[0], y[1], and z[2] AccumGyro data
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetAccumGyro(double xyz_deg[3]) {
+ int errCode = c_PigeonIMU_GetAccumGyro(_handle, xyz_deg);
+ return errCode;
+}
+/**
+ * Get the absolute compass heading.
+ * @return compass heading [0,360) degrees.
+ */
+double PigeonIMU::GetAbsoluteCompassHeading() {
+ double retval;
+ c_PigeonIMU_GetAbsoluteCompassHeading(_handle, &retval);
+ return retval;
+}
+/**
+ * Get the continuous compass heading.
+ * @return continuous compass heading [-23040, 23040) degrees. Use
+ * SetCompassHeading to modify the wrap-around portion.
+ */
+double PigeonIMU::GetCompassHeading() {
+ double retval;
+ c_PigeonIMU_GetCompassHeading(_handle, &retval);
+ return retval;
+}
+/**
+ * Gets the compass' measured magnetic field strength.
+ * @return field strength in Microteslas (uT).
+ */
+double PigeonIMU::GetCompassFieldStrength() {
+ double retval;
+ c_PigeonIMU_GetCompassFieldStrength(_handle, &retval);
+ return retval;
+}
+/**
+ * Gets the temperature of the pigeon.
+ *
+ * @return Temperature in ('C)
+ */
+double PigeonIMU::GetTemp() {
+ double tempC;
+ c_PigeonIMU_GetTemp(_handle, &tempC);
+ return tempC;
+}
+/**
+ * Gets the current Pigeon state
+ *
+ * @return PigeonState enum
+ */
+PigeonIMU::PigeonState PigeonIMU::GetState() {
+ int retval;
+ c_PigeonIMU_GetState(_handle, &retval);
+ return (PigeonIMU::PigeonState) retval;
+}
+/**
+ * Gets the current Pigeon uptime.
+ *
+ * @return How long has Pigeon been running in whole seconds. Value caps at
+ * 255.
+ */
+uint32_t PigeonIMU::GetUpTime() {
+ int timeSec;
+ c_PigeonIMU_GetUpTime(_handle, &timeSec);
+ return timeSec;
+}
+/**
+ * Get Raw Magnetometer data.
+ *
+ * @param rm_xyz Array to fill with x[0], y[1], and z[2] data
+ * Number is equal to 0.6 microTeslas per unit.
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetRawMagnetometer(int16_t rm_xyz[3]) {
+ int errCode = c_PigeonIMU_GetRawMagnetometer(_handle, rm_xyz);
+ return errCode;
+}
+/**
+ * Get Biased Magnetometer data.
+ *
+ * @param bm_xyz Array to fill with x[0], y[1], and z[2] data
+ * Number is equal to 0.6 microTeslas per unit.
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetBiasedMagnetometer(int16_t bm_xyz[3]) {
+ int errCode = c_PigeonIMU_GetBiasedMagnetometer(_handle, bm_xyz);
+ return errCode;
+}
+/**
+ * Get Biased Accelerometer data.
+ *
+ * @param ba_xyz Array to fill with x[0], y[1], and z[2] data
+ * These are in fixed point notation Q2.14. eg. 16384 = 1G
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetBiasedAccelerometer(int16_t ba_xyz[3]) {
+ int errCode = c_PigeonIMU_GetBiasedAccelerometer(_handle, ba_xyz);
+ return errCode;
+}
+/**
+ * Get Raw Gyro data.
+ *
+ * @param xyz_dps Array to fill with x[0], y[1], and z[2] data in degrees per second.
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetRawGyro(double xyz_dps[3]) {
+ int errCode = c_PigeonIMU_GetRawGyro(_handle, xyz_dps);
+ return errCode;
+}
+/**
+ * Get Accelerometer tilt angles.
+ *
+ * @param tiltAngles Array to fill with x[0], y[1], and z[2] angles in degrees.
+ * @return The last ErrorCode generated.
+ */
+int PigeonIMU::GetAccelerometerAngles(double tiltAngles[3]) {
+ int errCode = c_PigeonIMU_GetAccelerometerAngles(_handle, tiltAngles);
+ return errCode;
+}
+/**
+ * Get the current Fusion Status (including fused heading)
+ *
+ * @param status object reference to fill with fusion status flags.
+ * Caller may pass null if flags are not needed.
+ * @return The fused heading in degrees.
+ */
+double PigeonIMU::GetFusedHeading(FusionStatus & status) {
+ int bIsFusing, bIsValid;
+ double fusedHeading;
+ int lastError;
+
+ int errCode = c_PigeonIMU_GetFusedHeading2(_handle, &bIsFusing, &bIsValid,
+ &fusedHeading, &lastError);
+
+ std::string description;
+
+ if (errCode != 0) {
+ bIsFusing = false;
+ bIsValid = false;
+ description =
+ "Could not receive status frame. Check wiring and web-config.";
+ } else {
+ if (bIsValid == false) {
+ description = "Fused Heading is not valid.";
+ } else if (bIsFusing == false) {
+ description = "Fused Heading is valid.";
+ } else {
+ description = "Fused Heading is valid and is fusing compass.";
+ }
+ }
+
+ /* fill caller's struct */
+ status.heading = fusedHeading;
+ status.bIsFusing = (bool) bIsFusing;
+ status.bIsValid = (bool) bIsValid;
+ status.description = description;
+ status.lastError = errCode;
+
+ return fusedHeading;
+}
+/**
+ * Gets the Fused Heading
+ *
+ * @return The fused heading in degrees.
+ */
+double PigeonIMU::GetFusedHeading() {
+ double fusedHeading;
+ c_PigeonIMU_GetFusedHeading1(_handle, &fusedHeading);
+ return fusedHeading;
+}
+//----------------------- Startup/Reset status -----------------------//
+/**
+ * Use HasResetOccurred() instead.
+ */
+uint32_t PigeonIMU::GetResetCount() {
+ int retval;
+ c_PigeonIMU_GetResetCount(_handle, &retval);
+ return retval;
+}
+uint32_t PigeonIMU::GetResetFlags() {
+ int retval;
+ c_PigeonIMU_GetResetCount(_handle, &retval);
+ return (uint32_t) retval;
+}
+/**
+ * @return holds the version of the device. Device must be powered cycled at least once.
+ */
+uint32_t PigeonIMU::GetFirmVers() {
+ int retval;
+ c_PigeonIMU_GetFirmwareVersion(_handle, &retval);
+ return retval;
+}
+/**
+ * @return true iff a reset has occurred since last call.
+ */
+bool PigeonIMU::HasResetOccurred() {
+ bool retval = false;
+ c_PigeonIMU_HasResetOccurred(_handle, &retval);
+ return retval;
+}
+
+/**
+ * Convert PigeonState to string.
+ * This function is static.
+ */
+std::string PigeonIMU::ToString(PigeonState state) {
+ std::string retval = "Unknown";
+ switch (state) {
+ case Initializing:
+ return "Initializing";
+ case Ready:
+ return "Ready";
+ case UserCalibration:
+ return "UserCalibration";
+ case NoComm:
+ return "NoComm";
+ }
+ return retval;
+}
+
+/**
+ * Convert CalibrationMode to string.
+ * This function is static.
+ */
+std::string PigeonIMU::ToString(CalibrationMode cm) {
+ std::string retval = "Unknown";
+ switch (cm) {
+ case BootTareGyroAccel:
+ return "BootTareGyroAccel";
+ case Temperature:
+ return "Temperature";
+ case Magnetometer12Pt:
+ return "Magnetometer12Pt";
+ case Magnetometer360:
+ return "Magnetometer360";
+ case Accelerometer:
+ return "Accelerometer";
+ }
+ return retval;
+}
+/**
+ * Sets the value of a custom parameter. This is for arbitrary use.
+ *
+ * Sometimes it is necessary to save calibration/declination/offset
+ * information in the device. Particularly if the
+ * device is part of a subsystem that can be replaced.
+ *
+ * @param newValue
+ * Value for custom parameter.
+ * @param paramIndex
+ * Index of custom parameter. [0-1]
+ * @param timeoutMs
+* Timeout value in ms. If nonzero, function will wait for
+* config success and report an error if it times out.
+* If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::ConfigSetCustomParam(int newValue, int paramIndex,
+ int timeoutMs) {
+ return c_PigeonIMU_ConfigSetCustomParam(_handle, newValue, paramIndex,
+ timeoutMs);
+}
+/**
+ * Gets the value of a custom parameter. This is for arbitrary use.
+ *
+ * Sometimes it is necessary to save calibration/declination/offset
+ * information in the device. Particularly if the
+ * device is part of a subsystem that can be replaced.
+ *
+ * @param paramIndex
+ * Index of custom parameter. [0-1]
+ * @param timeoutMs
+* Timeout value in ms. If nonzero, function will wait for
+* config success and report an error if it times out.
+* If zero, no blocking or checking is performed.
+ * @return Value of the custom param.
+ */
+int PigeonIMU::ConfigGetCustomParam(int paramIndex, int timeoutMs) {
+ int readValue;
+ c_PigeonIMU_ConfigGetCustomParam(_handle, &readValue, paramIndex,
+ timeoutMs);
+ return readValue;
+}
+/**
+ * Sets a parameter. Generally this is not used.
+ * This can be utilized in
+ * - Using new features without updating API installation.
+ * - Errata workarounds to circumvent API implementation.
+ * - Allows for rapid testing / unit testing of firmware.
+ *
+ * @param param
+ * Parameter enumeration.
+ * @param value
+ * Value of parameter.
+ * @param subValue
+ * Subvalue for parameter. Maximum value of 255.
+ * @param ordinal
+ * Ordinal of parameter.
+ * @param timeoutMs
+* Timeout value in ms. If nonzero, function will wait for
+* config success and report an error if it times out.
+* If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::ConfigSetParameter(ctre::phoenix::ParamEnum param, double value,
+ uint8_t subValue, int ordinal, int timeoutMs) {
+ return c_PigeonIMU_ConfigSetParameter(_handle, param, value, subValue,
+ ordinal, timeoutMs);
+
+}
+/**
+ * Gets a parameter. Generally this is not used.
+ * This can be utilized in
+ * - Using new features without updating API installation.
+ * - Errata workarounds to circumvent API implementation.
+ * - Allows for rapid testing / unit testing of firmware.
+ *
+ * @param param
+ * Parameter enumeration.
+ * @param ordinal
+ * Ordinal of parameter.
+ * @param timeoutMs
+* Timeout value in ms. If nonzero, function will wait for
+* config success and report an error if it times out.
+* If zero, no blocking or checking is performed.
+ * @return Value of parameter.
+ */
+double PigeonIMU::ConfigGetParameter(ctre::phoenix::ParamEnum param, int ordinal,
+ int timeoutMs) {
+ double value = 0;
+ c_PigeonIMU_ConfigGetParameter(_handle, param, &value, ordinal, timeoutMs);
+ return value;
+}
+
+
+//------ Frames ----------//
+/**
+ * Sets the period of the given status frame.
+ *
+ * @param statusFrame
+ * Frame whose period is to be changed.
+ * @param periodMs
+ * Period in ms for the given frame.
+ * @param timeoutMs
+* Timeout value in ms. If nonzero, function will wait for
+* config success and report an error if it times out.
+* If zero, no blocking or checking is performed.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::SetStatusFramePeriod(PigeonIMU_StatusFrame statusFrame,
+ int periodMs, int timeoutMs) {
+ return c_PigeonIMU_SetStatusFramePeriod(_handle, statusFrame, periodMs,
+ timeoutMs);
+}
+/**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ * Frame to get the period of.
+ * @param timeoutMs
+* Timeout value in ms. If nonzero, function will wait for
+* config success and report an error if it times out.
+* If zero, no blocking or checking is performed.
+ * @return Period of the given status frame.
+ */
+int PigeonIMU::GetStatusFramePeriod(PigeonIMU_StatusFrame frame,
+ int timeoutMs) {
+ int periodMs = 0;
+ c_PigeonIMU_GetStatusFramePeriod(_handle, frame, &periodMs, timeoutMs);
+ return periodMs;
+}
+/**
+ * Sets the period of the given control frame.
+ *
+ * @param frame
+ * Frame whose period is to be changed.
+ * @param periodMs
+ * Period in ms for the given frame.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::SetControlFramePeriod(PigeonIMU_ControlFrame frame,
+ int periodMs) {
+ return c_PigeonIMU_SetControlFramePeriod(_handle, frame, periodMs);
+}
+//------ Firmware ----------//
+/**
+ * Gets the firmware version of the device.
+ *
+ * @return The firmware version of the device. Device must be powered
+ * cycled at least once.
+ */
+int PigeonIMU::GetFirmwareVersion() {
+ int retval = -1;
+ c_PigeonIMU_GetFirmwareVersion(_handle, &retval);
+ return retval;
+}
+//------ Faults ----------//
+/**
+ * Gets the fault status
+ *
+ * @param toFill
+ * Container for fault statuses.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::GetFaults(PigeonIMU_Faults & toFill) {
+ int faultBits;
+ ErrorCode retval = c_PigeonIMU_GetFaults(_handle, &faultBits);
+ toFill = PigeonIMU_Faults(faultBits);
+ return retval;
+}
+/**
+ * Gets the sticky fault status
+ *
+ * @param toFill
+ * Container for sticky fault statuses.
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::GetStickyFaults(PigeonIMU_StickyFaults & toFill) {
+ int faultBits;
+ ErrorCode retval = c_PigeonIMU_GetFaults(_handle, &faultBits);
+ toFill = PigeonIMU_StickyFaults(faultBits);
+ return retval;
+}
+/**
+ * Clears the Sticky Faults
+ *
+ * @return Error Code generated by function. 0 indicates no error.
+ */
+ErrorCode PigeonIMU::ClearStickyFaults(int timeoutMs) {
+ return c_PigeonIMU_ClearStickyFaults(_handle, timeoutMs);
+}
+
+} // namespace signals
+} // namespace phoenix
+} // namespace ctre
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/src/Stopwatch.cpp b/cpp/src/Stopwatch.cpp
new file mode 100644
index 0000000..522965b
--- /dev/null
+++ b/cpp/src/Stopwatch.cpp
@@ -0,0 +1,20 @@
+#include "ctre/phoenix/Stopwatch.h"
+
+namespace ctre {
+namespace phoenix {
+
+void Stopwatch::Start(){
+ _t0 = (float)clock();
+}
+unsigned int Stopwatch::DurationMs(){
+ return Stopwatch::Duration() * 1000;
+}
+float Stopwatch::Duration(){
+ _t1 = (float)clock();
+ long retval = _t1 - _t0;
+ if(retval < 0) retval = 0;
+ return retval * _scalar;
+}
+
+} // namespace phoenix
+}
diff --git a/cpp/src/Tasking/ButtonMonitor.cpp b/cpp/src/Tasking/ButtonMonitor.cpp
new file mode 100644
index 0000000..d420e34
--- /dev/null
+++ b/cpp/src/Tasking/ButtonMonitor.cpp
@@ -0,0 +1,46 @@
+#include "ctre/phoenix/Tasking/ButtonMonitor.h"
+#include <GenericHID.h> // WPILIB
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+
+namespace ctre {
+namespace phoenix {
+namespace tasking {
+
+ButtonMonitor::ButtonMonitor(frc::GenericHID * controller, int buttonIndex,
+ IButtonPressEventHandler * ButtonPressEventHandler) {
+ _gameCntrlr = controller;
+ _btnIdx = buttonIndex;
+ _handler = ButtonPressEventHandler;
+}
+ButtonMonitor::ButtonMonitor(const ButtonMonitor & rhs) {
+ _gameCntrlr = rhs._gameCntrlr;
+ _btnIdx = rhs._btnIdx;
+ _handler = rhs._handler;
+}
+
+void ButtonMonitor::Process() {
+ bool down = ((frc::GenericHID*)_gameCntrlr)->GetRawButton(_btnIdx);
+
+ if (!_isDown && down)
+ _handler->OnButtonPress(_btnIdx, down);
+
+ _isDown = down;
+}
+
+void ButtonMonitor::OnStart() {
+}
+void ButtonMonitor::OnLoop() {
+ Process();
+}
+bool ButtonMonitor::IsDone() {
+ return false;
+}
+void ButtonMonitor::OnStop() {
+}
+
+} // namespace tasking
+} // namespace phoenix
+} // namespace ctre
+
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/src/Tasking/Schedulers/ConcurrentScheduler.cpp b/cpp/src/Tasking/Schedulers/ConcurrentScheduler.cpp
new file mode 100644
index 0000000..c8d6773
--- /dev/null
+++ b/cpp/src/Tasking/Schedulers/ConcurrentScheduler.cpp
@@ -0,0 +1,88 @@
+#include "ctre/phoenix/Tasking/Schedulers/ConcurrentScheduler.h"
+
+namespace ctre {
+namespace phoenix {
+namespace tasking {
+namespace schedulers {
+
+ConcurrentScheduler::ConcurrentScheduler() {
+}
+ConcurrentScheduler::~ConcurrentScheduler() {
+}
+void ConcurrentScheduler::Add(ILoopable *aLoop, bool enable) {
+ _loops.push_back(aLoop);
+ _enabs.push_back(enable);
+}
+void ConcurrentScheduler::RemoveAll() {
+ _loops.clear();
+ _enabs.clear();
+}
+void ConcurrentScheduler::Start(ILoopable* toStart) {
+ for (int i = 0; i < (int) _loops.size(); ++i) {
+ ILoopable* lp = (ILoopable*) _loops[i];
+
+ if (lp == toStart) {
+ _enabs[i] = true;
+ lp->OnStart();
+ return;
+ }
+ }
+
+}
+void ConcurrentScheduler::Stop(ILoopable* toStop) {
+ for (int i = 0; i < (int) _loops.size(); ++i) {
+ ILoopable* lp = (ILoopable*) _loops[i];
+
+ if (lp == toStop) {
+ _enabs[i] = false;
+ lp->OnStop();
+ return;
+ }
+ }
+}
+void ConcurrentScheduler::StartAll() { //All Loops
+ for (auto loop : _loops) {
+ loop->OnStart();
+ }
+ for (auto enable : _enabs) {
+ enable = true;
+ }
+}
+void ConcurrentScheduler::StopAll() { //All Loops
+ for (auto loop : _loops) {
+ loop->OnStop();
+ }
+ for (auto enable : _enabs) {
+ enable = false;
+ }
+}
+void ConcurrentScheduler::Process() {
+ for (int i = 0; i < (int) _loops.size(); ++i) {
+ ILoopable* loop = (ILoopable*) _loops[i];
+ bool en = (bool) _enabs[i];
+ if (en) {
+ loop->OnLoop();
+ } else {
+ /* Current ILoopable is turned off, don't call OnLoop for it */
+ }
+ }
+}
+/* ILoopable */
+void ConcurrentScheduler::OnStart() {
+ ConcurrentScheduler::StartAll();
+}
+void ConcurrentScheduler::OnLoop() {
+ ConcurrentScheduler::Process();
+}
+void ConcurrentScheduler::OnStop() {
+ ConcurrentScheduler::StopAll();
+}
+bool ConcurrentScheduler::IsDone() {
+ return false;
+}
+
+} // namespace schedulers
+} // namespace tasking
+} // namespace phoenix
+} // namespace ctre
+
diff --git a/cpp/src/Tasking/Schedulers/SequentialScheduler.cpp b/cpp/src/Tasking/Schedulers/SequentialScheduler.cpp
new file mode 100644
index 0000000..edac7bd
--- /dev/null
+++ b/cpp/src/Tasking/Schedulers/SequentialScheduler.cpp
@@ -0,0 +1,86 @@
+#include "ctre/phoenix/Tasking/Schedulers/SequentialScheduler.h"
+
+namespace ctre {
+namespace phoenix {
+namespace tasking {
+namespace schedulers {
+
+SequentialScheduler::SequentialScheduler() {
+}
+SequentialScheduler::~SequentialScheduler() {
+}
+void SequentialScheduler::Add(ILoopable *aLoop) {
+ _loops.push_back(aLoop);
+}
+ILoopable * SequentialScheduler::GetCurrent() {
+ ILoopable* retval = nullptr;
+ if (_idx < _loops.size()) {
+ retval = _loops[_idx];
+ }
+ return retval;
+}
+
+void SequentialScheduler::RemoveAll() {
+ _loops.clear();
+}
+void SequentialScheduler::Start() {
+ /* reset iterator regardless of loopable container */
+ _idx = 0;
+ /* do we have any to loop? */
+ if (_idx >= _loops.size()) {
+ /* there are no loopables */
+ _running = false;
+ } else {
+ /* start the first one */
+ _loops[_idx]->OnStart();
+ _running = true;
+ }
+
+}
+void SequentialScheduler::Stop() {
+ for (unsigned int i = 0; i < _loops.size(); i++) {
+ _loops[i]->OnStop();
+ }
+ _running = false;
+}
+void SequentialScheduler::Process() {
+ if (_idx < _loops.size()) {
+ if (_running) {
+ ILoopable* loop = _loops[_idx];
+ loop->OnLoop();
+ if (loop->IsDone()) {
+ /* iterate to next loopable */
+ ++_idx;
+ if (_idx < _loops.size()) {
+ /* callback to start it */
+ _loops[_idx]->OnStart();
+ }
+ }
+ }
+ } else {
+ _running = false;
+ }
+}
+/* ILoopable */
+void SequentialScheduler::OnStart() {
+ SequentialScheduler::Start();
+}
+void SequentialScheduler::OnLoop() {
+ SequentialScheduler::Process();
+}
+void SequentialScheduler::OnStop() {
+ SequentialScheduler::Stop();
+}
+bool SequentialScheduler::IsDone() {
+ /* Have to return something to know if we are done */
+ if (_running == false)
+ return true;
+ else
+ return false;
+}
+
+} // namespace schedulers
+} // namespace tasking
+} // namespace phoenix
+} // namespace ctre
+
diff --git a/cpp/src/Utilities.cpp b/cpp/src/Utilities.cpp
new file mode 100644
index 0000000..5c749b2
--- /dev/null
+++ b/cpp/src/Utilities.cpp
@@ -0,0 +1,69 @@
+#include "ctre/phoenix/Utilities.h"
+
+namespace ctre {
+namespace phoenix {
+
+float Utilities::abs(float f) {
+ if (f >= 0)
+ return f;
+ return -f;
+}
+
+float Utilities::bound(float value, float capValue) {
+ if (value > capValue)
+ return capValue;
+ if (value < -capValue)
+ return -capValue;
+ return value;
+}
+
+float Utilities::cap(float value, float peak) {
+ if (value < -peak)
+ return -peak;
+ if (value > +peak)
+ return +peak;
+ return value;
+}
+
+bool Utilities::Contains(char array[], char item) {
+ //Not sure how to implement in c++ yet, made private
+ (void)array;
+ (void)item;
+ return false;
+}
+
+void Utilities::Deadband(float &value, float deadband) {
+ if (value < -deadband) {
+ /* outside of deadband */
+ } else if (value > +deadband) {
+ /* outside of deadband */
+ } else {
+ /* within 10% so zero it */
+ value = 0;
+ }
+}
+
+bool Utilities::IsWithin(float value, float compareTo, float allowDelta) {
+ float f = value - compareTo;
+ if (f < 0)
+ f *= -1;
+ return (f < allowDelta);
+}
+
+int Utilities::SmallerOf(int value_1, int value_2) {
+ if (value_1 > value_2)
+ return value_2;
+ else
+ return value_1;
+}
+
+void Utilities::Split_1(float forward, float turn, float *left, float *right) {
+ *left = forward + turn;
+ *right = forward - turn;
+}
+void Utilities::Split_2(float left, float right, float *forward, float *turn) {
+ *forward = (left + right) * 0.5f;
+ *turn = (left - right) * 0.5f;
+}
+}
+}