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, &param);
+	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, &param);
+	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, &param);
+	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, &param);
+	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, &param);
+	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, &param, 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, &param);
+	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, &param);
+	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, &param);
+	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, &param);
+	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, &currentMode,
+			&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;
+}
+}
+}