Squashed 'third_party/Phoenix-frc-lib/' content from commit 666d176
Change-Id: Ibaca2fc8ffb1177e786576cc1e4cc9f7a8c98f13
git-subtree-dir: third_party/Phoenix-frc-lib
git-subtree-split: 666d176a08151793044ab74e0005f13d3732ed96
diff --git a/cpp/include/ctre/Phoenix.h b/cpp/include/ctre/Phoenix.h
new file mode 100644
index 0000000..ff1cb68
--- /dev/null
+++ b/cpp/include/ctre/Phoenix.h
@@ -0,0 +1,35 @@
+#pragma once
+#include "ctre/phoenix/CANifier.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/HsvToRgb.h"
+#include "ctre/phoenix/LinearInterpolation.h"
+#include "ctre/phoenix/Motion/MotionProfileStatus.h"
+#include "ctre/phoenix/Motion/TrajectoryPoint.h"
+#include "ctre/phoenix/MotorControl/CAN/TalonSRX.h"
+#include "ctre/phoenix/MotorControl/CAN/VictorSPX.h"
+#include "ctre/phoenix/MotorControl/CAN/WPI_TalonSRX.h"
+#include "ctre/phoenix/MotorControl/CAN/WPI_VictorSPX.h"
+#include "ctre/phoenix/MotorControl/DemandType.h"
+#include "ctre/phoenix/MotorControl/Faults.h"
+#include "ctre/phoenix/MotorControl/FollowerType.h"
+#include "ctre/phoenix/MotorControl/SensorCollection.h"
+#include "ctre/phoenix/MotorControl/IMotorController.h"
+#include "ctre/phoenix/MotorControl/IMotorControllerEnhanced.h"
+#include "ctre/phoenix/Sensors/PigeonIMU.h"
+#include "ctre/phoenix/Signals/MovingAverage.h"
+#include "ctre/phoenix/Tasking/Schedulers/ConcurrentScheduler.h"
+#include "ctre/phoenix/Tasking/ILoopable.h"
+#include "ctre/phoenix/Tasking/IProcessable.h"
+#include "ctre/phoenix/Tasking/ButtonMonitor.h"
+#include "ctre/phoenix/Utilities.h"
+
+using namespace ctre;
+using namespace ctre::phoenix;
+using namespace ctre::phoenix::motion;
+using namespace ctre::phoenix::motorcontrol;
+using namespace ctre::phoenix::motorcontrol::can;
+using namespace ctre::phoenix::sensors;
+using namespace ctre::phoenix::signals;
+using namespace ctre::phoenix::tasking;
+using namespace ctre::phoenix::tasking::schedulers;
diff --git a/cpp/include/ctre/phoenix/CANifier.h b/cpp/include/ctre/phoenix/CANifier.h
new file mode 100644
index 0000000..5e83b22
--- /dev/null
+++ b/cpp/include/ctre/phoenix/CANifier.h
@@ -0,0 +1,140 @@
+#pragma once
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+
+#include <cstdint>
+#include "ctre/phoenix/LowLevel/CANBusAddressable.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/CANifierControlFrame.h"
+#include "ctre/phoenix/CANifierStatusFrame.h"
+#include "ctre/phoenix/CANifierStickyFaults.h"
+#include "ctre/phoenix/CANifierFaults.h"
+#include "ctre/phoenix/CANifierVelocityMeasPeriod.h"
+
+namespace ctre {namespace phoenix {
+ /**
+ * CTRE CANifier
+ *
+ * Device for interfacing common devices to the CAN bus.
+ */
+class CANifier: public CANBusAddressable {
+public:
+ /**
+ * Enum for the LED Output Channels
+ */
+ enum LEDChannel {
+ LEDChannelA = 0, LEDChannelB = 1, LEDChannelC = 2,
+ };
+
+ /**
+ * Enum for the PWM Input Channels
+ */
+ enum PWMChannel {
+ PWMChannel0 = 0, PWMChannel1 = 1, PWMChannel2 = 2, PWMChannel3 = 3,
+ };
+ const int PWMChannelCount = 4;
+
+ /**
+ * General IO Pins on the CANifier
+ */
+ enum GeneralPin {
+ QUAD_IDX = 0, //----- Must match CANifier_CCI enums -----//
+ QUAD_B = 1,
+ QUAD_A = 2,
+ LIMR = 3,
+ LIMF = 4,
+ SDA = 5,
+ SCL = 6,
+ SPI_CS = 7,
+ SPI_MISO_PWM2P = 8,
+ SPI_MOSI_PWM1P = 9,
+ SPI_CLK_PWM0P = 10,
+ };
+
+ /**
+ * Structure to hold the pin values.
+ */
+ struct PinValues {
+ bool QUAD_IDX;
+ bool QUAD_B;
+ bool QUAD_A;
+ bool LIMR;
+ bool LIMF;
+ bool SDA;
+ bool SCL;
+ bool SPI_CS_PWM3;
+ bool SPI_MISO_PWM2;
+ bool SPI_MOSI_PWM1;
+ bool SPI_CLK_PWM0;
+ };
+
+ CANifier(int deviceNumber);
+ ErrorCode SetLEDOutput(double percentOutput, LEDChannel ledChannel);
+ ErrorCode SetGeneralOutput(GeneralPin outputPin, bool outputValue, bool outputEnable);
+ ErrorCode SetGeneralOutputs(int outputBits, int isOutputBits);
+ ErrorCode GetGeneralInputs(PinValues &allPins);
+ bool GetGeneralInput(GeneralPin inputPin);
+ int GetQuadraturePosition();
+ int GetQuadratureVelocity();
+ ErrorCode SetQuadraturePosition(int newPosition, int timeoutMs);
+ ErrorCode ConfigVelocityMeasurementPeriod(
+ CANifierVelocityMeasPeriod period, int timeoutMs);
+ ErrorCode ConfigVelocityMeasurementWindow(int windowSize, int timeoutMs);
+ /**
+ * Gets the bus voltage seen by the motor controller.
+ *
+ * @return The bus voltage value (in volts).
+ */
+ double GetBusVoltage();
+ ErrorCode GetLastError();
+ ErrorCode SetPWMOutput(int pwmChannel, double dutyCycle);
+ ErrorCode EnablePWMOutput(int pwmChannel, bool bEnable);
+ ErrorCode GetPWMInput(PWMChannel pwmChannel, double dutyCycleAndPeriod[]);
+
+ //------ Custom Persistent Params ----------//
+ ErrorCode ConfigSetCustomParam(int newValue, int paramIndex,
+ int timeoutMs);
+ int ConfigGetCustomParam(int paramIndex,
+ int timeoutMs);
+ //------ Generic Param API, typically not used ----------//
+ ErrorCode ConfigSetParameter(ParamEnum param, double value,
+ uint8_t subValue, int ordinal, int timeoutMs);
+ double ConfigGetParameter(ParamEnum param, int ordinal, int timeoutMs);
+
+
+ ErrorCode SetStatusFramePeriod(CANifierStatusFrame statusFrame,
+ int periodMs, int timeoutMs);
+ /**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ * Frame to get the period of.
+ * @param timeoutMs
+ * Timeout value in ms. @see #ConfigOpenLoopRamp
+ * @return Period of the given status frame.
+ */
+ int GetStatusFramePeriod(CANifierStatusFrame frame, int timeoutMs);
+ ErrorCode SetControlFramePeriod(CANifierControlFrame frame, int periodMs);
+ /**
+ * Gets the firmware version of the device.
+ *
+ * @return Firmware version of device.
+ */
+ int GetFirmwareVersion();
+ /**
+ * Returns true if the device has reset since last call.
+ *
+ * @return Has a Device Reset Occurred?
+ */
+ bool HasResetOccurred();
+ ErrorCode GetFaults(CANifierFaults & toFill);
+ ErrorCode GetStickyFaults(CANifierStickyFaults & toFill);
+ ErrorCode ClearStickyFaults(int timeoutMs);
+
+private:
+ void* m_handle;
+ bool _tempPins[11];
+};
+}}
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/include/ctre/phoenix/CTRLogger.h b/cpp/include/ctre/phoenix/CTRLogger.h
new file mode 100644
index 0000000..91f8d43
--- /dev/null
+++ b/cpp/include/ctre/phoenix/CTRLogger.h
@@ -0,0 +1,16 @@
+#include "ctre/phoenix/ErrorCode.h" // ErrorCode
+#include <string>
+
+namespace ctre {
+namespace phoenix {
+
+class CTRLogger {
+public:
+ static void Close();
+ static ErrorCode Log(ErrorCode code, std::string origin);
+ static void Open(int language);
+ //static void Description(ErrorCode code, const char *&shrt, const char *&lng);
+};
+
+}
+}
diff --git a/cpp/include/ctre/phoenix/HsvToRgb.h b/cpp/include/ctre/phoenix/HsvToRgb.h
new file mode 100644
index 0000000..90a89ad
--- /dev/null
+++ b/cpp/include/ctre/phoenix/HsvToRgb.h
@@ -0,0 +1,13 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+
+class HsvToRgb {
+public:
+ static void Convert(double hDegrees, double S, double V, float* r, float* g,
+ float* b);
+};
+
+}
+}
diff --git a/cpp/include/ctre/phoenix/LinearInterpolation.h b/cpp/include/ctre/phoenix/LinearInterpolation.h
new file mode 100644
index 0000000..f445852
--- /dev/null
+++ b/cpp/include/ctre/phoenix/LinearInterpolation.h
@@ -0,0 +1,12 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+
+class LinearInterpolation {
+public:
+ static float Calculate(float x, float x1, float y1, float x2, float y2);
+};
+
+}}
+
diff --git a/cpp/include/ctre/phoenix/MotorControl/CAN/BaseMotorController.h b/cpp/include/ctre/phoenix/MotorControl/CAN/BaseMotorController.h
new file mode 100644
index 0000000..6378f6f
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/CAN/BaseMotorController.h
@@ -0,0 +1,236 @@
+#pragma once
+
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/core/GadgeteerUartClient.h"
+#include "ctre/phoenix/MotorControl/IMotorController.h"
+#include "ctre/phoenix/MotorControl/ControlMode.h"
+#include "ctre/phoenix/MotorControl/DemandType.h"
+#include "ctre/phoenix/MotorControl/Faults.h"
+#include "ctre/phoenix/MotorControl/FollowerType.h"
+#include "ctre/phoenix/MotorControl/StickyFaults.h"
+#include "ctre/phoenix/MotorControl/VelocityMeasPeriod.h"
+#include "ctre/phoenix/Motion/TrajectoryPoint.h"
+#include "ctre/phoenix/Motion/MotionProfileStatus.h"
+/* WPILIB */
+#include "SpeedController.h"
+
+/* forward proto's */
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace lowlevel {
+class MotControllerWithBuffer_LowLevel;
+class MotController_LowLevel;
+}
+}
+}
+}
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+class SensorCollection;
+}
+}
+}
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+/**
+ * Base motor controller features for all CTRE CAN motor controllers.
+ */
+class BaseMotorController: public virtual IMotorController {
+private:
+ ControlMode m_controlMode = ControlMode::PercentOutput;
+ ControlMode m_sendMode = ControlMode::PercentOutput;
+
+ int _arbId = 0;
+ double m_setPoint = 0;
+ bool _invert = false;
+
+ ctre::phoenix::motorcontrol::SensorCollection * _sensorColl;
+protected:
+ void* m_handle;
+ void* GetHandle();
+public:
+ BaseMotorController(int arbId);
+ ~BaseMotorController();
+ BaseMotorController() = delete;
+ BaseMotorController(BaseMotorController const&) = delete;
+ BaseMotorController& operator=(BaseMotorController const&) = delete;
+ int GetDeviceID();
+ virtual void Set(ControlMode Mode, double value);
+ virtual void Set(ControlMode mode, double demand0, double demand1);
+ virtual void Set(ControlMode mode, double demand0, DemandType demand1Type, double demand1);
+ virtual void NeutralOutput();
+ virtual void SetNeutralMode(NeutralMode neutralMode);
+ void EnableHeadingHold(bool enable);
+ void SelectDemandType(bool value);
+ //------ Invert behavior ----------//
+ virtual void SetSensorPhase(bool PhaseSensor);
+ virtual void SetInverted(bool invert);
+ virtual bool GetInverted() const;
+ //----- general output shaping ------------------//
+ virtual ctre::phoenix::ErrorCode ConfigOpenloopRamp(double secondsFromNeutralToFull,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigClosedloopRamp(double secondsFromNeutralToFull,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigPeakOutputForward(double percentOut, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigPeakOutputReverse(double percentOut, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigNominalOutputForward(double percentOut,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigNominalOutputReverse(double percentOut,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigNeutralDeadband(double percentDeadband,
+ int timeoutMs);
+ //------ Voltage Compensation ----------//
+ virtual ctre::phoenix::ErrorCode ConfigVoltageCompSaturation(double voltage, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigVoltageMeasurementFilter(int filterWindowSamples,
+ int timeoutMs);
+ virtual void EnableVoltageCompensation(bool enable);
+ //------ General Status ----------//
+ virtual double GetBusVoltage();
+ virtual double GetMotorOutputPercent();
+ virtual double GetMotorOutputVoltage();
+ virtual double GetOutputCurrent();
+ virtual double GetTemperature();
+ //------ sensor selection ----------//
+ virtual ctre::phoenix::ErrorCode ConfigSelectedFeedbackSensor(
+ RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigSelectedFeedbackSensor(
+ FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigSelectedFeedbackCoefficient(
+ double coefficient, int pidIdx, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigRemoteFeedbackFilter(int deviceID,
+ RemoteSensorSource remoteSensorSource, int remoteOrdinal,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigSensorTerm(SensorTerm sensorTerm,
+ FeedbackDevice feedbackDevice, int timeoutMs);
+
+ //------- sensor status --------- //
+ virtual int GetSelectedSensorPosition(int pidIdx);
+ virtual int GetSelectedSensorVelocity(int pidIdx);
+ virtual ctre::phoenix::ErrorCode SetSelectedSensorPosition(int sensorPos, int pidIdx, int timeoutMs);
+ //------ status frame period changes ----------//
+ virtual ctre::phoenix::ErrorCode SetControlFramePeriod(ControlFrame frame, int periodMs);
+ virtual ctre::phoenix::ErrorCode SetStatusFramePeriod(StatusFrame frame, int periodMs,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode SetStatusFramePeriod(StatusFrameEnhanced frame,
+ int periodMs, int timeoutMs);
+ virtual int GetStatusFramePeriod(StatusFrame frame, int timeoutMs);
+ virtual int GetStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs);
+ //----- velocity signal conditionaing ------//
+ virtual ctre::phoenix::ErrorCode ConfigVelocityMeasurementPeriod(VelocityMeasPeriod period,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigVelocityMeasurementWindow(int windowSize,
+ int timeoutMs);
+ //------ remote limit switch ----------//
+ virtual ctre::phoenix::ErrorCode ConfigForwardLimitSwitchSource(
+ RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+ int deviceID, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigReverseLimitSwitchSource(
+ RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+ int deviceID, int timeoutMs);
+ void OverrideLimitSwitchesEnable(bool enable);
+ //------ local limit switch ----------//
+ virtual ctre::phoenix::ErrorCode ConfigForwardLimitSwitchSource(LimitSwitchSource type,
+ LimitSwitchNormal normalOpenOrClose, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigReverseLimitSwitchSource(LimitSwitchSource type,
+ LimitSwitchNormal normalOpenOrClose, int timeoutMs);
+ //------ soft limit ----------//
+ virtual ctre::phoenix::ErrorCode ConfigForwardSoftLimitThreshold(int forwardSensorLimit,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigReverseSoftLimitThreshold(int reverseSensorLimit,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigForwardSoftLimitEnable(bool enable,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigReverseSoftLimitEnable(bool enable,
+ int timeoutMs);
+ virtual void OverrideSoftLimitsEnable(bool enable);
+ //------ Current Lim ----------//
+ /* not available in base */
+ //------ General Close loop ----------//
+ virtual ctre::phoenix::ErrorCode Config_kP(int slotIdx, double value, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode Config_kI(int slotIdx, double value, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode Config_kD(int slotIdx, double value, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode Config_kF(int slotIdx, double value, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode Config_IntegralZone(int slotIdx, int izone,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigAllowableClosedloopError(int slotIdx,
+ int allowableCloseLoopError, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigMaxIntegralAccumulator(int slotIdx, double iaccum,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigClosedLoopPeakOutput(int slotIdx, double percentOut, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigClosedLoopPeriod(int slotIdx, int loopTimeMs, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigAuxPIDPolarity(bool invert, int timeoutMs);
+
+ //------ Close loop State ----------//
+ virtual ctre::phoenix::ErrorCode SetIntegralAccumulator(double iaccum, int pidIdx,int timeoutMs);
+ virtual int GetClosedLoopError(int pidIdx);
+ virtual double GetIntegralAccumulator(int pidIdx);
+ virtual double GetErrorDerivative(int pidIdx);
+
+ virtual ctre::phoenix::ErrorCode SelectProfileSlot(int slotIdx, int pidIdx);
+
+ virtual int GetClosedLoopTarget(int pidIdx);
+ virtual int GetActiveTrajectoryPosition();
+ virtual int GetActiveTrajectoryVelocity();
+ virtual double GetActiveTrajectoryHeading();
+
+ //------ Motion Profile Settings used in Motion Magic ----------//
+ virtual ctre::phoenix::ErrorCode ConfigMotionCruiseVelocity(int sensorUnitsPer100ms,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigMotionAcceleration(int sensorUnitsPer100msPerSec,
+ int timeoutMs);
+ //------ Motion Profile Buffer ----------//
+ virtual ErrorCode ClearMotionProfileTrajectories();
+ virtual int GetMotionProfileTopLevelBufferCount();
+ virtual ctre::phoenix::ErrorCode PushMotionProfileTrajectory(
+ const ctre::phoenix::motion::TrajectoryPoint & trajPt);
+ virtual bool IsMotionProfileTopLevelBufferFull();
+ virtual void ProcessMotionProfileBuffer();
+ virtual ctre::phoenix::ErrorCode GetMotionProfileStatus(
+ ctre::phoenix::motion::MotionProfileStatus & statusToFill);
+ virtual ctre::phoenix::ErrorCode ClearMotionProfileHasUnderrun(int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ChangeMotionControlFramePeriod(int periodMs);
+ virtual ctre::phoenix::ErrorCode ConfigMotionProfileTrajectoryPeriod(int baseTrajDurationMs, int timeoutMs);
+ //------ error ----------//
+ virtual ctre::phoenix::ErrorCode GetLastError();
+ //------ Faults ----------//
+ virtual ctre::phoenix::ErrorCode GetFaults(Faults & toFill);
+ virtual ctre::phoenix::ErrorCode GetStickyFaults(StickyFaults & toFill);
+ virtual ctre::phoenix::ErrorCode ClearStickyFaults(int timeoutMs);
+ //------ Firmware ----------//
+ virtual int GetFirmwareVersion();
+ virtual bool HasResetOccurred();
+ //------ Custom Persistent Params ----------//
+ virtual ctre::phoenix::ErrorCode ConfigSetCustomParam(int newValue, int paramIndex,
+ int timeoutMs);
+ virtual int ConfigGetCustomParam(int paramIndex,
+ int timeoutMs);
+ //------ Generic Param API, typically not used ----------//
+ virtual ctre::phoenix::ErrorCode ConfigSetParameter(ctre::phoenix::ParamEnum param, double value,
+ uint8_t subValue, int ordinal, int timeoutMs);
+ virtual double ConfigGetParameter(ctre::phoenix::ParamEnum param, int ordinal, int timeoutMs);
+ //------ Misc. ----------//
+ virtual int GetBaseID();
+ virtual ControlMode GetControlMode();
+ // ----- Follower ------//
+ void Follow(IMotorController & masterToFollow, ctre::phoenix::motorcontrol::FollowerType followerType);
+ virtual void Follow(IMotorController & masterToFollow);
+ virtual void ValueUpdated();
+
+ //------ RAW Sensor API ----------//
+ /**
+ * @retrieve object that can get/set individual RAW sensor values.
+ */
+ ctre::phoenix::motorcontrol::SensorCollection & GetSensorCollection();
+};
+
+} // namespace can
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/CAN/TalonSRX.h b/cpp/include/ctre/phoenix/MotorControl/CAN/TalonSRX.h
new file mode 100644
index 0000000..7054efa
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/CAN/TalonSRX.h
@@ -0,0 +1,65 @@
+#pragma once
+
+#include "ctre/phoenix/MotorControl/CAN/BaseMotorController.h"
+#include "ctre/phoenix/MotorControl/IMotorControllerEnhanced.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+
+/**
+ * CTRE Talon SRX Motor Controller when used on CAN Bus.
+ */
+class TalonSRX: public virtual BaseMotorController,
+ public virtual IMotorControllerEnhanced {
+public:
+ TalonSRX(int deviceNumber);
+ virtual ~TalonSRX() {
+ }
+ TalonSRX() = delete;
+ TalonSRX(TalonSRX const&) = delete;
+ TalonSRX& operator=(TalonSRX const&) = delete;
+
+ virtual ctre::phoenix::ErrorCode ConfigSelectedFeedbackSensor(FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigSelectedFeedbackSensor(RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs);
+
+ virtual ctre::phoenix::ErrorCode SetStatusFramePeriod(StatusFrameEnhanced frame,int periodMs, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode SetStatusFramePeriod(StatusFrame frame,int periodMs, int timeoutMs);
+
+ virtual int GetStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs);
+ virtual int GetStatusFramePeriod(StatusFrame frame, int timeoutMs);
+
+ //------ Velocity measurement ----------//
+ virtual ctre::phoenix::ErrorCode ConfigVelocityMeasurementPeriod(VelocityMeasPeriod period,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigVelocityMeasurementWindow(int windowSize,
+ int timeoutMs);
+
+ //------ limit switch ----------//
+ virtual ctre::phoenix::ErrorCode ConfigForwardLimitSwitchSource(
+ LimitSwitchSource limitSwitchSource,
+ LimitSwitchNormal normalOpenOrClose, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigReverseLimitSwitchSource(
+ LimitSwitchSource limitSwitchSource,
+ LimitSwitchNormal normalOpenOrClose, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigForwardLimitSwitchSource(
+ RemoteLimitSwitchSource limitSwitchSource,
+ LimitSwitchNormal normalOpenOrClose, int deviceID, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigReverseLimitSwitchSource(
+ RemoteLimitSwitchSource limitSwitchSource,
+ LimitSwitchNormal normalOpenOrClose, int deviceID, int timeoutMs);
+
+ //------ Current Limit ----------//
+ virtual ctre::phoenix::ErrorCode ConfigPeakCurrentLimit(int amps, int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigPeakCurrentDuration(int milliseconds,
+ int timeoutMs);
+ virtual ctre::phoenix::ErrorCode ConfigContinuousCurrentLimit(int amps, int timeoutMs);
+ virtual void EnableCurrentLimit(bool enable);
+
+};
+// class TalonSRX
+} // namespace can
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/CAN/VictorSPX.h b/cpp/include/ctre/phoenix/MotorControl/CAN/VictorSPX.h
new file mode 100644
index 0000000..a209b85
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/CAN/VictorSPX.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include "ctre/phoenix/MotorControl/CAN/BaseMotorController.h"
+#include "ctre/phoenix/MotorControl/IMotorController.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol{
+namespace can {
+
+/**
+ * VEX Victor SPX Motor Controller when used on CAN Bus.
+ */
+class VictorSPX: public virtual ctre::phoenix::motorcontrol::can::BaseMotorController,
+ public virtual ctre::phoenix::motorcontrol::IMotorController {
+
+public:
+ VictorSPX(int deviceNumber);
+ virtual ~VictorSPX() {
+ }
+ VictorSPX(VictorSPX const&) = delete;
+ VictorSPX& operator=(VictorSPX const&) = delete;
+}; //class VictorSPX
+
+} //namespace can
+} //namespace motorcontrol
+} //namespace phoenix
+} //namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_TalonSRX.h b/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_TalonSRX.h
new file mode 100644
index 0000000..6174ce8
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_TalonSRX.h
@@ -0,0 +1,127 @@
+/**
+ * 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.
+ */
+#pragma once
+
+#include "ctre/phoenix/MotorControl/CAN/TalonSRX.h"
+#include "SmartDashboard/SendableBase.h"
+#include "SmartDashboard/SendableBuilder.h"
+#include "SpeedController.h"
+#include "MotorSafety.h"
+#include "MotorSafetyHelper.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+
+
+class WPI_TalonSRX: public virtual TalonSRX,
+ public virtual frc::SpeedController,
+ public frc::SendableBase,
+ public frc::MotorSafety {
+public:
+ WPI_TalonSRX(int deviceNumber);
+ virtual ~WPI_TalonSRX();
+
+ WPI_TalonSRX() = delete;
+ WPI_TalonSRX(WPI_TalonSRX const&) = delete;
+ WPI_TalonSRX& operator=(WPI_TalonSRX const&) = delete;
+
+ //----------------------- 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().
+ */
+ virtual void Set(double speed);
+ virtual void PIDWrite(double 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.
+ */
+ virtual double Get() const;
+
+ //----------------------- Intercept CTRE calls for motor safety -------------------//
+ virtual void Set(ControlMode mode, double value);
+ virtual void Set(ControlMode mode, double demand0, double demand1);
+ //----------------------- Invert routines -------------------//
+ /**
+ * Common interface for inverting direction of a speed controller.
+ *
+ * @param isInverted The state of inversion, true is inverted.
+ */
+ virtual void SetInverted(bool isInverted);
+ /**
+ * Common interface for returning the inversion state of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ */
+ virtual bool GetInverted() const;
+ //----------------------- turn-motor-off routines-------------------//
+ /**
+ * Common interface for disabling a motor.
+ */
+ virtual void Disable();
+ /**
+ * Common interface to stop the motor until Set is called again.
+ */
+ virtual void StopMotor();
+
+ //----------------------- Motor Safety-------------------//
+
+ /**
+ * Set the safety expiration time.
+ *
+ * @param timeout The timeout (in seconds) for this motor object
+ */
+ void SetExpiration(double timeout);
+
+ /**
+ * Return the safety expiration time.
+ *
+ * @return The expiration time value.
+ */
+ double GetExpiration() const;
+
+ /**
+ * 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 IsAlive() const;
+
+ /**
+ * Check if motor safety is enabled.
+ *
+ * @return True if motor safety is enforced for this object
+ */
+ bool IsSafetyEnabled() const;
+
+ void SetSafetyEnabled(bool enabled);
+
+ void GetDescription(llvm::raw_ostream& desc) const;
+
+protected:
+ virtual void InitSendable(frc::SendableBuilder& builder);
+private:
+ double _speed = 0;
+ std::string _desc;
+ frc::MotorSafetyHelper _safetyHelper;
+};
+
+} // namespace can
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_VictorSPX.h b/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_VictorSPX.h
new file mode 100644
index 0000000..f0c8cd0
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/CAN/WPI_VictorSPX.h
@@ -0,0 +1,127 @@
+/**
+ * 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.
+ */
+#pragma once
+
+#include "ctre/phoenix/MotorControl/CAN/VictorSPX.h"
+#include "SmartDashboard/SendableBase.h"
+#include "SmartDashboard/SendableBuilder.h"
+#include "SpeedController.h"
+#include "MotorSafety.h"
+#include "MotorSafetyHelper.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+
+
+class WPI_VictorSPX: public virtual VictorSPX,
+ public virtual frc::SpeedController,
+ public frc::SendableBase,
+ public frc::MotorSafety {
+public:
+ WPI_VictorSPX(int deviceNumber);
+ virtual ~WPI_VictorSPX();
+
+ WPI_VictorSPX() = delete;
+ WPI_VictorSPX(WPI_VictorSPX const&) = delete;
+ WPI_VictorSPX& operator=(WPI_VictorSPX const&) = delete;
+
+ //----------------------- 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().
+ */
+ virtual void Set(double speed);
+ virtual void PIDWrite(double 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.
+ */
+ virtual double Get() const;
+
+ //----------------------- Intercept CTRE calls for motor safety -------------------//
+ virtual void Set(ControlMode mode, double value);
+ virtual void Set(ControlMode mode, double demand0, double demand1);
+ //----------------------- Invert routines -------------------//
+ /**
+ * Common interface for inverting direction of a speed controller.
+ *
+ * @param isInverted The state of inversion, true is inverted.
+ */
+ virtual void SetInverted(bool isInverted);
+ /**
+ * Common interface for returning the inversion state of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ */
+ virtual bool GetInverted() const;
+ //----------------------- turn-motor-off routines-------------------//
+ /**
+ * Common interface for disabling a motor.
+ */
+ virtual void Disable();
+ /**
+ * Common interface to stop the motor until Set is called again.
+ */
+ virtual void StopMotor();
+
+ //----------------------- Motor Safety-------------------//
+
+ /**
+ * Set the safety expiration time.
+ *
+ * @param timeout The timeout (in seconds) for this motor object
+ */
+ void SetExpiration(double timeout);
+
+ /**
+ * Return the safety expiration time.
+ *
+ * @return The expiration time value.
+ */
+ double GetExpiration() const;
+
+ /**
+ * 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 IsAlive() const;
+
+ /**
+ * Check if motor safety is enabled.
+ *
+ * @return True if motor safety is enforced for this object
+ */
+ bool IsSafetyEnabled() const;
+
+ void SetSafetyEnabled(bool enabled);
+
+ void GetDescription(llvm::raw_ostream& desc) const;
+
+protected:
+ virtual void InitSendable(frc::SendableBuilder& builder);
+private:
+ double _speed = 0;
+ std::string _desc;
+ frc::MotorSafetyHelper _safetyHelper;
+};
+
+} // namespace can
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/DeviceCatalog.h b/cpp/include/ctre/phoenix/MotorControl/DeviceCatalog.h
new file mode 100644
index 0000000..528eecd
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/DeviceCatalog.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#include "IMotorController.h"
+#include <vector>
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+class DeviceCatalog {
+public:
+ void Register(IMotorController *motorController) {
+ _mcs.push_back(motorController);
+ }
+
+ int MotorControllerCount() {
+ return _mcs.size();
+ }
+
+ IMotorController* Get(int idx) {
+ return _mcs[idx];
+ }
+
+ DeviceCatalog & GetInstance() {
+ if (!_instance)
+ _instance = new DeviceCatalog();
+ return *_instance;
+ }
+private:
+ std::vector<IMotorController*> _mcs;
+
+ static DeviceCatalog * _instance;
+};
+
+}
+} // namespace phoenix
+}
+
diff --git a/cpp/include/ctre/phoenix/MotorControl/GroupMotorControllers.h b/cpp/include/ctre/phoenix/MotorControl/GroupMotorControllers.h
new file mode 100644
index 0000000..9ab95bd
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/GroupMotorControllers.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "IMotorController.h"
+#include <vector>
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+class GroupMotorControllers {
+public:
+ static void Register(IMotorController *motorController);
+ static int MotorControllerCount();
+ static IMotorController* Get(int idx);
+
+private:
+ static std::vector<IMotorController*> _mcs;
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
+
diff --git a/cpp/include/ctre/phoenix/MotorControl/IFollower.h b/cpp/include/ctre/phoenix/MotorControl/IFollower.h
new file mode 100644
index 0000000..8f52c3d
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/IFollower.h
@@ -0,0 +1,19 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+/* forward proto */
+class IMotorController;
+
+class IFollower {
+public:
+ virtual ~IFollower(){}
+ virtual void Follow(ctre::phoenix::motorcontrol::IMotorController & masterToFollow) = 0;
+ virtual void ValueUpdated()= 0;
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/IMotorController.h b/cpp/include/ctre/phoenix/MotorControl/IMotorController.h
new file mode 100644
index 0000000..947022e
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/IMotorController.h
@@ -0,0 +1,205 @@
+#pragma once
+
+#include "ctre/phoenix/MotorControl/ControlMode.h"
+#include "ctre/phoenix/MotorControl/ControlFrame.h"
+#include "ctre/phoenix/MotorControl/DemandType.h"
+#include "ctre/phoenix/MotorControl/NeutralMode.h"
+#include "ctre/phoenix/MotorControl/FeedbackDevice.h"
+#include "ctre/phoenix/MotorControl/RemoteSensorSource.h"
+#include "ctre/phoenix/MotorControl/SensorTerm.h"
+#include "ctre/phoenix/MotorControl/StatusFrame.h"
+#include "ctre/phoenix/MotorControl/LimitSwitchType.h"
+#include "ctre/phoenix/MotorControl/Faults.h"
+#include "ctre/phoenix/MotorControl/StickyFaults.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/Motion/TrajectoryPoint.h"
+#include "ctre/phoenix/Motion/MotionProfileStatus.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "IFollower.h"
+/* WPILIB */
+#include "SpeedController.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+class IMotorController: public virtual IFollower {
+public:
+ virtual ~IMotorController() {
+ }
+ //------ Set output routines. ----------//
+ virtual void Set(ControlMode Mode, double demand) = 0;
+ virtual void Set(ControlMode Mode, double demand0, double demand1) = 0;
+ virtual void Set(ControlMode mode, double demand0, DemandType demand1Type, double demand1) = 0;
+ virtual void NeutralOutput() = 0;
+ virtual void SetNeutralMode(NeutralMode neutralMode) = 0;
+
+ //------ Invert behavior ----------//
+ virtual void SetSensorPhase(bool PhaseSensor) = 0;
+ virtual void SetInverted(bool invert) = 0;
+ virtual bool GetInverted() const = 0;
+
+ //----- general output shaping ------------------//
+ virtual ErrorCode ConfigOpenloopRamp(double secondsFromNeutralToFull,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigClosedloopRamp(double secondsFromNeutralToFull,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigPeakOutputForward(double percentOut,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigPeakOutputReverse(double percentOut,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigNominalOutputForward(double percentOut,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigNominalOutputReverse(double percentOut,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigNeutralDeadband(double percentDeadband,
+ int timeoutMs) = 0;
+
+ //------ Voltage Compensation ----------//
+ virtual ErrorCode ConfigVoltageCompSaturation(double voltage,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigVoltageMeasurementFilter(int filterWindowSamples,
+ int timeoutMs) = 0;
+ virtual void EnableVoltageCompensation(bool enable) = 0;
+
+ //------ General Status ----------//
+ virtual double GetBusVoltage() = 0;
+ virtual double GetMotorOutputPercent() = 0;
+ virtual double GetMotorOutputVoltage() = 0;
+ virtual double GetOutputCurrent() = 0;
+ virtual double GetTemperature() = 0;
+
+ //------ sensor selection ----------//
+ virtual ErrorCode ConfigSelectedFeedbackSensor(
+ RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) = 0;
+ virtual ErrorCode ConfigSelectedFeedbackCoefficient(
+ double coefficient, int pidIdx, int timeoutMs) = 0;
+ virtual ErrorCode ConfigRemoteFeedbackFilter(int deviceID,
+ RemoteSensorSource remoteSensorSource, int remoteOrdinal,
+ int timeoutMs)= 0;
+ virtual ErrorCode ConfigSensorTerm(SensorTerm sensorTerm, FeedbackDevice feedbackDevice, int timeoutMs)= 0;
+
+ //------- sensor status --------- //
+ virtual int GetSelectedSensorPosition(int pidIdx) = 0;
+ virtual int GetSelectedSensorVelocity(int pidIdx) = 0;
+ virtual ErrorCode SetSelectedSensorPosition(int sensorPos, int pidIdx,
+ int timeoutMs) = 0;
+
+ //------ status frame period changes ----------//
+ virtual ErrorCode SetControlFramePeriod(ControlFrame frame,
+ int periodMs) = 0;
+ virtual ErrorCode SetStatusFramePeriod(StatusFrame frame, int periodMs,
+ int timeoutMs) = 0;
+ virtual int GetStatusFramePeriod(StatusFrame frame, int timeoutMs) = 0;
+
+ //----- velocity signal conditionaing ------//
+ /* not supported */
+
+ //------ remote limit switch ----------//
+ virtual ErrorCode ConfigForwardLimitSwitchSource(
+ RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+ int deviceID, int timeoutMs) = 0;
+ virtual ErrorCode ConfigReverseLimitSwitchSource(
+ RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+ int deviceID, int timeoutMs) = 0;
+ virtual void OverrideLimitSwitchesEnable(bool enable) = 0;
+
+ //------ local limit switch ----------//
+ /* not supported */
+
+ //------ soft limit ----------//
+ virtual ErrorCode ConfigForwardSoftLimitThreshold(int forwardSensorLimit,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigReverseSoftLimitThreshold(int reverseSensorLimit,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigForwardSoftLimitEnable(bool enable,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigReverseSoftLimitEnable(bool enable,
+ int timeoutMs) = 0;
+ virtual void OverrideSoftLimitsEnable(bool enable) = 0;
+
+ //------ Current Lim ----------//
+ /* not supported */
+
+ //------ Config Close loop ----------//
+ virtual ErrorCode Config_kP(int slotIdx, double value, int timeoutMs) = 0;
+ virtual ErrorCode Config_kI(int slotIdx, double value, int timeoutMs) = 0;
+ virtual ErrorCode Config_kD(int slotIdx, double value, int timeoutMs) = 0;
+ virtual ErrorCode Config_kF(int slotIdx, double value, int timeoutMs) = 0;
+ virtual ErrorCode Config_IntegralZone(int slotIdx, int izone,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigAllowableClosedloopError(int slotIdx,
+ int allowableCloseLoopError, int timeoutMs) = 0;
+ virtual ErrorCode ConfigMaxIntegralAccumulator(int slotIdx, double iaccum,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigClosedLoopPeakOutput(int slotIdx, double percentOut, int timeoutMs) = 0;
+ virtual ErrorCode ConfigClosedLoopPeriod(int slotIdx, int loopTimeMs, int timeoutMs) = 0;
+ virtual ErrorCode ConfigAuxPIDPolarity(bool invert, int timeoutMs) = 0;
+
+ //------ Close loop State ----------//
+ virtual ErrorCode SetIntegralAccumulator(double iaccum, int pidIdx,
+ int timeoutMs) = 0;
+ virtual int GetClosedLoopError(int pidIdx) = 0;
+ virtual double GetIntegralAccumulator(int pidIdx) = 0;
+ virtual double GetErrorDerivative(int pidIdx) = 0;
+
+ virtual ErrorCode SelectProfileSlot(int slotIdx, int pidIdx) = 0;
+
+ virtual int GetClosedLoopTarget(int pidIdx) = 0;
+ virtual int GetActiveTrajectoryPosition() = 0;
+ virtual int GetActiveTrajectoryVelocity() = 0;
+ virtual double GetActiveTrajectoryHeading() = 0;
+
+ //------ Motion Profile Settings used in Motion Magic ----------//
+ virtual ErrorCode ConfigMotionCruiseVelocity(int sensorUnitsPer100ms,
+ int timeoutMs) = 0;
+ virtual ErrorCode ConfigMotionAcceleration(int sensorUnitsPer100msPerSec,
+ int timeoutMs) = 0;
+
+ //------ Motion Profile Buffer ----------//
+ virtual ErrorCode ClearMotionProfileTrajectories()= 0;
+ virtual int GetMotionProfileTopLevelBufferCount()= 0;
+ virtual ErrorCode PushMotionProfileTrajectory(
+ const ctre::phoenix::motion::TrajectoryPoint & trajPt)= 0;
+ virtual bool IsMotionProfileTopLevelBufferFull()= 0;
+ virtual void ProcessMotionProfileBuffer()= 0;
+ virtual ErrorCode GetMotionProfileStatus(
+ ctre::phoenix::motion::MotionProfileStatus & statusToFill)= 0;
+ virtual ErrorCode ClearMotionProfileHasUnderrun(int timeoutMs)= 0;
+ virtual ErrorCode ChangeMotionControlFramePeriod(int periodMs)= 0;
+ virtual ErrorCode ConfigMotionProfileTrajectoryPeriod(int baseTrajDurationMs, int timeoutMs)=0;
+ //------ error ----------//
+ virtual ErrorCode GetLastError() = 0;
+
+ //------ Faults ----------//
+ virtual ErrorCode GetFaults(Faults & toFill) = 0;
+ virtual ErrorCode GetStickyFaults(StickyFaults & toFill) = 0;
+ virtual ErrorCode ClearStickyFaults(int timeoutMs) = 0;
+
+ //------ Firmware ----------//
+ virtual int GetFirmwareVersion() = 0;
+ virtual bool HasResetOccurred() = 0;
+
+ //------ Custom Persistent Params ----------//
+ virtual ErrorCode ConfigSetCustomParam(int newValue, int paramIndex,
+ int timeoutMs) = 0;
+ virtual int ConfigGetCustomParam(int paramIndex, int timeoutMs) = 0;
+
+ //------ Generic Param API, typically not used ----------//
+ virtual ErrorCode ConfigSetParameter(ParamEnum param, double value,
+ uint8_t subValue, int ordinal, int timeoutMs) = 0;
+ virtual double ConfigGetParameter(ParamEnum paramEnum, int ordinal,
+ int timeoutMs) = 0;
+
+ //------ Misc. ----------//
+ virtual int GetBaseID() = 0;
+ virtual int GetDeviceID() = 0;
+ virtual ControlMode GetControlMode() = 0;
+
+ // ----- Follower ------//
+ /* in parent interface */
+};
+
+}
+} // namespace phoenix
+}
diff --git a/cpp/include/ctre/phoenix/MotorControl/IMotorControllerEnhanced.h b/cpp/include/ctre/phoenix/MotorControl/IMotorControllerEnhanced.h
new file mode 100644
index 0000000..e5c429c
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/IMotorControllerEnhanced.h
@@ -0,0 +1,119 @@
+#pragma once
+
+#include "ctre/phoenix/MotorControl/ControlMode.h"
+#include "ctre/phoenix/MotorControl/ControlFrame.h"
+#include "ctre/phoenix/MotorControl/NeutralMode.h"
+#include "ctre/phoenix/MotorControl/FeedbackDevice.h"
+#include "ctre/phoenix/MotorControl/StatusFrame.h"
+#include "ctre/phoenix/MotorControl/LimitSwitchType.h"
+#include "ctre/phoenix/MotorControl/Faults.h"
+#include "ctre/phoenix/MotorControl/StickyFaults.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/Motion/TrajectoryPoint.h"
+#include "ctre/phoenix/Motion/MotionProfileStatus.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "IFollower.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+class IMotorControllerEnhanced: public virtual IMotorController {
+public:
+ virtual ~IMotorControllerEnhanced() {
+ }
+
+ //------ Set output routines. ----------//
+ /* in parent */
+
+ //------ Invert behavior ----------//
+ /* in parent */
+
+ //----- general output shaping ------------------//
+ /* in parent */
+
+ //------ Voltage Compensation ----------//
+ /* in parent */
+
+ //------ General Status ----------//
+ /* in parent */
+
+ //------ sensor selection ----------//
+ /* expand the options */
+ virtual ErrorCode ConfigSelectedFeedbackSensor(
+ FeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) = 0;
+ virtual ErrorCode ConfigSelectedFeedbackSensor(
+ RemoteFeedbackDevice feedbackDevice, int pidIdx, int timeoutMs) = 0;
+
+ //------- sensor status --------- //
+ /* in parent */
+
+ //------ status frame period changes ----------//
+ virtual ErrorCode SetStatusFramePeriod(StatusFrame frame, int periodMs,
+ int timeoutMs) = 0;
+ virtual ErrorCode SetStatusFramePeriod(StatusFrameEnhanced frame,
+ int periodMs, int timeoutMs) = 0;
+ virtual int GetStatusFramePeriod(StatusFrame frame, int timeoutMs) = 0;
+ virtual int GetStatusFramePeriod(StatusFrameEnhanced frame, int timeoutMs) = 0;
+
+ //----- velocity signal conditionaing ------//
+ virtual ErrorCode ConfigVelocityMeasurementPeriod(VelocityMeasPeriod period,
+ int timeoutMs)= 0;
+ virtual ErrorCode ConfigVelocityMeasurementWindow(int windowSize,
+ int timeoutMs)= 0;
+
+ //------ remote limit switch ----------//
+ virtual ErrorCode ConfigForwardLimitSwitchSource(
+ RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+ int deviceID, int timeoutMs) = 0;
+ virtual ErrorCode ConfigReverseLimitSwitchSource(
+ RemoteLimitSwitchSource type, LimitSwitchNormal normalOpenOrClose,
+ int deviceID, int timeoutMs) = 0;
+
+ //------ local limit switch ----------//
+ virtual ErrorCode ConfigForwardLimitSwitchSource(LimitSwitchSource type,
+ LimitSwitchNormal normalOpenOrClose, int timeoutMs)= 0;
+ virtual ErrorCode ConfigReverseLimitSwitchSource(LimitSwitchSource type,
+ LimitSwitchNormal normalOpenOrClose, int timeoutMs)= 0;
+
+ //------ soft limit ----------//
+ /* in parent */
+
+ //------ Current Lim ----------//
+ virtual ErrorCode ConfigPeakCurrentLimit(int amps, int timeoutMs)= 0;
+ virtual ErrorCode ConfigPeakCurrentDuration(int milliseconds,
+ int timeoutMs)= 0;
+ virtual ErrorCode ConfigContinuousCurrentLimit(int amps, int timeoutMs)= 0;
+ virtual void EnableCurrentLimit(bool enable)= 0;
+
+ //------ General Close loop ----------//
+ /* in parent */
+
+ //------ Motion Profile Settings used in Motion Magic and Motion Profile ----------//
+ /* in parent */
+
+ //------ Motion Profile Buffer ----------//
+ /* in parent */
+
+ //------ error ----------//
+ /* in parent */
+
+ //------ Faults ----------//
+ /* in parent */
+
+ //------ Firmware ----------//
+ /* in parent */
+
+ //------ Custom Persistent Params ----------//
+ /* in parent */
+
+ //------ Generic Param API, typically not used ----------//
+ /* in parent */
+
+ //------ Misc. ----------//
+ /* in parent */
+
+}; // class IMotorControllerEnhanced
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/MotorControl/SensorCollection.h b/cpp/include/ctre/phoenix/MotorControl/SensorCollection.h
new file mode 100644
index 0000000..63b5894
--- /dev/null
+++ b/cpp/include/ctre/phoenix/MotorControl/SensorCollection.h
@@ -0,0 +1,197 @@
+#pragma once
+
+#include "ctre/phoenix/ErrorCode.h"
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+class BaseMotorController;
+}
+}
+}
+}
+
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+
+class SensorCollection {
+public:
+
+ /**
+ * 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 GetAnalogIn();
+
+ /**
+ * 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 SetAnalogPosition(int newPosition, int 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 GetAnalogInRaw();
+
+ /**
+ * 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 GetAnalogInVel();
+
+ /**
+ * Get the quadrature position of the Talon, regardless of whether
+ * it is actually being used for feedback.
+ *
+ * @return the quadrature position.
+ */
+
+ int GetQuadraturePosition();
+
+ /**
+ * 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 SetQuadraturePosition(int newPosition, int timeoutMs);
+
+ /**
+ * Get the quadrature velocity, regardless of whether
+ * it is actually being used for feedback.
+ *
+ * @return the quadrature velocity in units per 100ms.
+ */
+
+ int GetQuadratureVelocity();
+
+ /**
+ * Gets pulse width position, regardless of whether
+ * it is actually being used for feedback.
+ *
+ * @return the pulse width position.
+ */
+
+ int GetPulseWidthPosition();
+
+ /**
+ * 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 SetPulseWidthPosition(int newPosition, int 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 GetPulseWidthVelocity();
+
+ /**
+ * Gets pulse width rise to fall time.
+ *
+ * @return the pulse width rise to fall time in microseconds.
+ */
+
+ int GetPulseWidthRiseToFallUs();
+
+ /**
+ * Gets pulse width rise to rise time.
+ *
+ * @return the pulse width rise to rise time in microseconds.
+ */
+
+ int GetPulseWidthRiseToRiseUs();
+
+ /**
+ * Gets pin state quad a.
+ *
+ * @return the pin state of quad a (1 if asserted, 0 if not asserted).
+ */
+
+ int GetPinStateQuadA();
+
+ /**
+ * Gets pin state quad b.
+ *
+ * @return Digital level of QUADB pin (1 if asserted, 0 if not asserted).
+ */
+
+ int GetPinStateQuadB();
+
+ /**
+ * Gets pin state quad index.
+ *
+ * @return Digital level of QUAD Index pin (1 if asserted, 0 if not asserted).
+ */
+
+ int GetPinStateQuadIdx();
+
+ /**
+ * 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 IsFwdLimitSwitchClosed();
+
+ /**
+ * 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 IsRevLimitSwitchClosed();
+
+private:
+ SensorCollection(void * handle);
+ friend class ctre::phoenix::motorcontrol::can::BaseMotorController;
+ void* _handle;
+
+};
+
+} // namespace motorcontrol
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/RCRadio3Ch.h b/cpp/include/ctre/phoenix/RCRadio3Ch.h
new file mode 100644
index 0000000..aaea7b4
--- /dev/null
+++ b/cpp/include/ctre/phoenix/RCRadio3Ch.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <vector>
+#include "ctre/phoenix/CANifier.h"
+#include "ctre/phoenix/Tasking/IProcessable.h"
+
+namespace ctre{
+namespace phoenix {
+
+class RCRadio3Ch : public ctre::phoenix::tasking::IProcessable{
+public:
+ enum Channel{
+ Channel1,
+ Channel2,
+ Channel3,
+ };
+ enum Status{
+ LossOfCAN,
+ LossOfPwm,
+ Okay,
+ };
+ Status CurrentStatus = Status::Okay;
+
+ RCRadio3Ch(ctre::phoenix::CANifier *canifier);
+ float GetDutyCycleUs(Channel channel);
+ float GetDutyCyclePerc(Channel channel);
+ bool GetSwitchValue(Channel channel);
+ float GetPeriodUs(Channel channel);
+
+ //ILoopable
+ void Process();
+
+private:
+ int _errorCodes[4];
+ ctre::phoenix::CANifier *_canifier;
+
+
+ //This is only a 2d array??
+ double _dutyCycleAndPeriods[4][2] =
+ {
+ { 0, 0 },
+ { 0, 0 },
+ { 0, 0 },
+ { 0, 0 },
+ };
+ double Interpolate(std::vector<double> &xData, std::vector<double> &yData, double x, bool extrapolate);
+};
+
+}}
diff --git a/cpp/include/ctre/phoenix/Sensors/PigeonIMU.h b/cpp/include/ctre/phoenix/Sensors/PigeonIMU.h
new file mode 100644
index 0000000..a360f74
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Sensors/PigeonIMU.h
@@ -0,0 +1,303 @@
+/*
+ * 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
+ */
+
+#pragma once
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+#include <string>
+#include "ctre/phoenix/LowLevel/CANBusAddressable.h"
+#include "ctre/phoenix/paramEnum.h"
+#include "ctre/phoenix/ErrorCode.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_ControlFrame.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_Faults.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_StatusFrame.h"
+#include "ctre/phoenix/Sensors/PigeonIMU_StickyFaults.h"
+
+/* forward prototype */
+namespace ctre {
+namespace phoenix {
+namespace motorcontrol {
+namespace can {
+class TalonSRX;
+}
+}
+}
+}
+
+namespace ctre {
+namespace phoenix {
+namespace sensors {
+/**
+ * Pigeon IMU Class.
+ * Class supports communicating over CANbus and over ribbon-cable (CAN Talon SRX).
+ */
+class PigeonIMU: public CANBusAddressable {
+public:
+ /** Data object for holding fusion information. */
+ struct FusionStatus {
+ double heading;
+ bool bIsValid;
+ bool bIsFusing;
+ std::string description;
+ /**
+ * Same as GetLastError()
+ */
+ int lastError;
+ };
+ /** Various calibration modes supported by Pigeon. */
+ enum CalibrationMode {
+ BootTareGyroAccel = 0,
+ Temperature = 1,
+ Magnetometer12Pt = 2,
+ Magnetometer360 = 3,
+ Accelerometer = 5,
+ };
+ /** Overall state of the Pigeon. */
+ enum PigeonState {
+ NoComm, Initializing, Ready, UserCalibration,
+ };
+ /**
+ * Data object for status on current calibration and general status.
+ *
+ * Pigeon has many calibration modes supported for a variety of uses.
+ * The modes generally collects and saves persistently information that makes
+ * the Pigeon signals more accurate. This includes collecting temperature, gyro, accelerometer,
+ * and compass information.
+ *
+ * For FRC use-cases, typically compass and temperature calibration is not required.
+ *
+ * Additionally when motion driver software in the Pigeon boots, it will perform a fast boot calibration
+ * to initially bias gyro and setup accelerometer.
+ *
+ * These modes can be enabled with the EnterCalibration mode.
+ *
+ * When a calibration mode is entered, caller can expect...
+ *
+ * - PigeonState to reset to Initializing and bCalIsBooting is set to true. Pigeon LEDs will blink the boot pattern.
+ * This is similar to the normal boot cal, however it can an additional ~30 seconds since calibration generally
+ * requires more information.
+ * currentMode will reflect the user's selected calibration mode.
+ *
+ * - PigeonState will eventually settle to UserCalibration and Pigeon LEDs will show cal specific blink patterns.
+ * bCalIsBooting is now false.
+ *
+ * - Follow the instructions in the Pigeon User Manual to meet the calibration specific requirements.
+ * When finished calibrationError will update with the result.
+ * Pigeon will solid-fill LEDs with red (for failure) or green (for success) for ~5 seconds.
+ * Pigeon then perform boot-cal to cleanly apply the newly saved calibration data.
+ */
+ struct GeneralStatus {
+ /**
+ * The current state of the motion driver. This reflects if the sensor signals are accurate.
+ * Most calibration modes will force Pigeon to reinit the motion driver.
+ */
+ PigeonIMU::PigeonState state;
+ /**
+ * The currently applied calibration mode if state is in UserCalibration or if bCalIsBooting is true.
+ * Otherwise it holds the last selected calibration mode (when calibrationError was updated).
+ */
+ PigeonIMU::CalibrationMode currentMode;
+ /**
+ * The error code for the last calibration mode.
+ * Zero represents a successful cal (with solid green LEDs at end of cal)
+ * and nonzero is a failed calibration (with solid red LEDs at end of cal).
+ * Different calibration
+ */
+ int calibrationError;
+ /**
+ * After caller requests a calibration mode, pigeon will perform a boot-cal before
+ * entering the requested mode. During this period, this flag is set to true.
+ */
+ bool bCalIsBooting;
+ /**
+ * general string description of current status
+ */
+ std::string description;
+ /**
+ * Temperature in Celsius
+ */
+ double tempC;
+ /**
+ * Number of seconds Pigeon has been up (since boot).
+ * This register is reset on power boot or processor reset.
+ * Register is capped at 255 seconds with no wrap around.
+ */
+ int upTimeSec;
+ /**
+ * Number of times the Pigeon has automatically rebiased the gyro.
+ * This counter overflows from 15 -> 0 with no cap.
+ */
+ int noMotionBiasCount;
+ /**
+ * Number of times the Pigeon has temperature compensated the various signals.
+ * This counter overflows from 15 -> 0 with no cap.
+ */
+ int tempCompensationCount;
+ /**
+ * Same as GetLastError()
+ */
+ int lastError;
+ };
+
+ PigeonIMU(int deviceNumber);
+ PigeonIMU(ctre::phoenix::motorcontrol::can::TalonSRX * talonSrx);
+
+ int SetYaw(double angleDeg, int timeoutMs);
+ int AddYaw(double angleDeg, int timeoutMs);
+ int SetYawToCompass(int timeoutMs);
+
+ int SetFusedHeading(double angleDeg, int timeoutMs);
+ int AddFusedHeading(double angleDeg, int timeoutMs);
+ int SetFusedHeadingToCompass(int timeoutMs);
+ int SetAccumZAngle(double angleDeg, int timeoutMs);
+
+ int ConfigTemperatureCompensationEnable(bool bTempCompEnable,
+ int timeoutMs);
+
+ int SetCompassDeclination(double angleDegOffset, int timeoutMs);
+ int SetCompassAngle(double angleDeg, int timeoutMs);
+
+ int EnterCalibrationMode(CalibrationMode calMode, int timeoutMs);
+ int GetGeneralStatus(PigeonIMU::GeneralStatus & genStatusToFill);
+ int GetLastError();
+ int Get6dQuaternion(double wxyz[4]);
+ int GetYawPitchRoll(double ypr[3]);
+ int GetAccumGyro(double xyz_deg[3]);
+ double GetAbsoluteCompassHeading();
+ double GetCompassHeading();
+ double GetCompassFieldStrength();
+ double GetTemp();
+ PigeonState GetState();
+ uint32_t GetUpTime();
+ int GetRawMagnetometer(int16_t rm_xyz[3]);
+
+ int GetBiasedMagnetometer(int16_t bm_xyz[3]);
+ int GetBiasedAccelerometer(int16_t ba_xyz[3]);
+ int GetRawGyro(double xyz_dps[3]);
+ int GetAccelerometerAngles(double tiltAngles[3]);
+ /**
+ * @param status object reference to fill with fusion status flags.
+ * @return The fused heading in degrees.
+ */
+ double GetFusedHeading(FusionStatus & status);
+ /**
+ * @return The fused heading in degrees.
+ */
+ double GetFusedHeading();
+ uint32_t GetResetCount();
+ uint32_t GetResetFlags();
+ uint32_t GetFirmVers();
+
+ bool HasResetOccurred();
+
+ static std::string ToString(PigeonIMU::PigeonState state);
+ static std::string ToString(CalibrationMode cm);
+
+ ErrorCode ConfigSetCustomParam(int newValue, int paramIndex, int timeoutMs);
+ int ConfigGetCustomParam(int paramIndex, int timeoutMs);
+ ErrorCode ConfigSetParameter(ParamEnum param, double value,
+ uint8_t subValue, int ordinal, int timeoutMs);
+ double ConfigGetParameter(ctre::phoenix::ParamEnum param, int ordinal, int timeoutMs);
+
+ ErrorCode SetStatusFramePeriod(PigeonIMU_StatusFrame statusFrame, int periodMs,
+ int timeoutMs);
+
+ /**
+ * Gets the period of the given status frame.
+ *
+ * @param frame
+ * Frame to get the period of.
+ * @param timeoutMs
+ * Timeout value in ms. @see #ConfigOpenLoopRamp
+ * @return Period of the given status frame.
+ */
+ int GetStatusFramePeriod(PigeonIMU_StatusFrame frame,
+ int timeoutMs) ;
+ ErrorCode SetControlFramePeriod(PigeonIMU_ControlFrame frame,
+ int periodMs);
+ int GetFirmwareVersion() ;
+ ErrorCode GetFaults(PigeonIMU_Faults & toFill) ;
+ ErrorCode GetStickyFaults(PigeonIMU_StickyFaults & toFill);
+ ErrorCode ClearStickyFaults(int timeoutMs);
+
+
+
+
+ void* GetLowLevelHandle() {
+ return _handle;
+ }
+private:
+ /** firmware state reported over CAN */
+ enum MotionDriverState {
+ Init0 = 0,
+ WaitForPowerOff = 1,
+ ConfigAg = 2,
+ SelfTestAg = 3,
+ StartDMP = 4,
+ ConfigCompass_0 = 5,
+ ConfigCompass_1 = 6,
+ ConfigCompass_2 = 7,
+ ConfigCompass_3 = 8,
+ ConfigCompass_4 = 9,
+ ConfigCompass_5 = 10,
+ SelfTestCompass = 11,
+ WaitForGyroStable = 12,
+ AdditionalAccelAdjust = 13,
+ Idle = 14,
+ Calibration = 15,
+ LedInstrum = 16,
+ Error = 31,
+ };
+ /** sub command for the various Set param enums */
+ enum TareType {
+ SetValue = 0x00, AddOffset = 0x01, MatchCompass = 0x02, SetOffset = 0xFF,
+ };
+ /** data storage for reset signals */
+ struct ResetStats {
+ int32_t resetCount;
+ int32_t resetFlags;
+ int32_t firmVers;
+ bool hasReset;
+ };
+ ResetStats _resetStats = { 0, 0, 0, false };
+
+ /** Portion of the arbID for all status and control frames. */
+ void* _handle;
+ uint32_t _deviceNumber;
+ uint32_t _usageHist = 0;
+ uint64_t _cache;
+ uint32_t _len;
+
+ /** overall threshold for when frame data is too old */
+ const uint32_t EXPECTED_RESPONSE_TIMEOUT_MS = (200);
+
+ int PrivateSetParameter(ParamEnum paramEnum, TareType tareType,
+ double angleDeg, int timeoutMs);
+
+ PigeonIMU::PigeonState GetState(int errCode, const uint64_t & statusFrame);
+ double GetTemp(const uint64_t & statusFrame);
+};
+} // namespace signals
+} // namespace phoenix
+} // namespace ctre
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/include/ctre/phoenix/Signals/IInvertable.h b/cpp/include/ctre/phoenix/Signals/IInvertable.h
new file mode 100644
index 0000000..b964b1b
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Signals/IInvertable.h
@@ -0,0 +1,16 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace signals {
+
+class IInvertable {
+public:
+ virtual ~IInvertable(){}
+ virtual void SetInverted(bool invert) = 0;
+ virtual bool GetInverted() const = 0;
+};
+
+} // namespace Signals
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/Signals/IOutputSignal.h b/cpp/include/ctre/phoenix/Signals/IOutputSignal.h
new file mode 100644
index 0000000..ff909fe
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Signals/IOutputSignal.h
@@ -0,0 +1,15 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+namespace signals {
+
+class IOutputSignal {
+public:
+ virtual ~IOutputSignal(){}
+ virtual void Set(double value) = 0;
+};
+
+} // namespace Signals
+} // namespace phoenix
+} // namespace ctre
diff --git a/cpp/include/ctre/phoenix/Signals/MovingAverage.h b/cpp/include/ctre/phoenix/Signals/MovingAverage.h
new file mode 100644
index 0000000..862b3d5
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Signals/MovingAverage.h
@@ -0,0 +1,92 @@
+/*
+ * 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
+ */
+
+namespace ctre {
+namespace phoenix {
+namespace signals {
+
+class MovingAverage {
+private:
+
+ int _in; //!< head ptr for ringbuffer
+ int _ou; //!< tail ptr for ringbuffer
+ int _cnt; //!< number of element in ring buffer
+ int _cap; //!< capacity of ring buffer
+ float _sum; //!< sum of all elements in ring buffer
+ float * _d; //!< ring buffer
+public:
+ MovingAverage(int capacity) {
+ _cap = capacity;
+ _d = new float[_cap];
+ Clear();
+ }
+ float Process(float input) {
+ Push(input);
+ return _sum / (float) _cnt;
+ }
+ void Clear() {
+ _in = 0;
+ _ou = 0;
+ _cnt = 0;
+
+ _sum = 0;
+ }
+ void Push(float d) {
+ /* process it */
+ _sum += d;
+
+ /* if full, pop one */
+ if (_cnt >= _cap)
+ Pop();
+
+ /* push new one */
+ _d[_in] = d;
+ if (++_in >= _cap)
+ _in = 0;
+ ++_cnt;
+ }
+ void Pop() {
+ /* get the oldest */
+ float d = _d[_ou];
+
+ /* process it */
+ _sum -= d;
+
+ /* pop it */
+ if (++_ou >= _cap)
+ _ou = 0;
+ --_cnt;
+ }
+ //-------------- Properties --------------//
+ float GetSum() {
+ return _sum;
+ }
+ int GetCount() {
+ return _cnt;
+ }
+};
+
+} // namespace Signals
+} // namespace phoenix
+} // namespace ctre
+
diff --git a/cpp/include/ctre/phoenix/Stopwatch.h b/cpp/include/ctre/phoenix/Stopwatch.h
new file mode 100644
index 0000000..eb0c9ff
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Stopwatch.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <time.h>
+
+namespace ctre {
+namespace phoenix {
+
+class Stopwatch {
+public:
+ void Start();
+ unsigned int DurationMs();
+ float Duration();
+
+private:
+ unsigned long _t0 = 0;
+ unsigned long _t1 = 0;
+ float _scalar = 0.001f / CLOCKS_PER_SEC;
+};
+
+}}
diff --git a/cpp/include/ctre/phoenix/Tasking/ButtonMonitor.h b/cpp/include/ctre/phoenix/Tasking/ButtonMonitor.h
new file mode 100644
index 0000000..2578d35
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Tasking/ButtonMonitor.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include "ctre/phoenix/Tasking/ILoopable.h"
+#include "ctre/phoenix/Tasking/IProcessable.h"
+#include <functional>
+
+#ifndef CTR_EXCLUDE_WPILIB_CLASSES
+
+/* forward proto's */
+namespace frc {
+ class GenericHID;
+}
+
+namespace ctre {
+namespace phoenix {
+namespace tasking {
+
+class ButtonMonitor: public IProcessable, public ILoopable {
+public:
+
+ class IButtonPressEventHandler {
+ public:
+ virtual ~IButtonPressEventHandler(){}
+ virtual void OnButtonPress(int idx, bool isDown) = 0;
+ };
+
+ ButtonMonitor(frc::GenericHID * controller, int buttonIndex, IButtonPressEventHandler * ButtonPressEventHandler);
+ ButtonMonitor(const ButtonMonitor & rhs);
+ virtual ~ButtonMonitor() { }
+
+ /* IProcessable */
+ virtual void Process();
+
+ /* ILoopable */
+ virtual void OnStart();
+ virtual void OnLoop();
+ virtual bool IsDone();
+ virtual void OnStop();
+
+private:
+ frc::GenericHID * _gameCntrlr;
+ int _btnIdx;
+ IButtonPressEventHandler * _handler;
+ bool _isDown = false;
+};
+}
+}
+}
+#endif // CTR_EXCLUDE_WPILIB_CLASSES
diff --git a/cpp/include/ctre/phoenix/Tasking/ILoopable.h b/cpp/include/ctre/phoenix/Tasking/ILoopable.h
new file mode 100644
index 0000000..db1dd50
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Tasking/ILoopable.h
@@ -0,0 +1,13 @@
+#pragma once
+
+namespace ctre { namespace phoenix { namespace tasking {
+
+class ILoopable{
+public:
+ virtual ~ILoopable(){}
+ virtual void OnStart() = 0;
+ virtual void OnLoop() = 0;
+ virtual bool IsDone() = 0;
+ virtual void OnStop() = 0;
+};
+}}}
diff --git a/cpp/include/ctre/phoenix/Tasking/IProcessable.h b/cpp/include/ctre/phoenix/Tasking/IProcessable.h
new file mode 100644
index 0000000..0e93180
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Tasking/IProcessable.h
@@ -0,0 +1,9 @@
+#pragma once
+namespace ctre { namespace phoenix { namespace tasking{
+
+class IProcessable {
+public:
+ virtual ~IProcessable(){}
+ virtual void Process() = 0;
+};
+}}}
diff --git a/cpp/include/ctre/phoenix/Tasking/Schedulers/ConcurrentScheduler.h b/cpp/include/ctre/phoenix/Tasking/Schedulers/ConcurrentScheduler.h
new file mode 100644
index 0000000..7481d29
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Tasking/Schedulers/ConcurrentScheduler.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <vector>
+#include "ctre/phoenix/Tasking/ILoopable.h"
+#include "ctre/phoenix/Tasking/IProcessable.h"
+
+namespace ctre {
+namespace phoenix {
+namespace tasking {
+namespace schedulers {
+
+class ConcurrentScheduler: public ILoopable, public IProcessable {
+public:
+ std::vector<ILoopable*> _loops;
+ std::vector<bool> _enabs;
+
+ ConcurrentScheduler();
+ virtual ~ConcurrentScheduler();
+ void Add(ILoopable *aLoop, bool enable = true);
+ void RemoveAll();
+ void Start(ILoopable *toStart);
+ void Stop(ILoopable *toStop);
+ void StartAll();
+ void StopAll();
+
+ //IProcessable
+ void Process();
+
+ //ILoopable
+ bool Iterated();
+ void OnStart();
+ void OnLoop();
+ void OnStop();
+ bool IsDone();
+};
+}
+}
+}
+}
diff --git a/cpp/include/ctre/phoenix/Tasking/Schedulers/SequentialScheduler.h b/cpp/include/ctre/phoenix/Tasking/Schedulers/SequentialScheduler.h
new file mode 100644
index 0000000..f550b31
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Tasking/Schedulers/SequentialScheduler.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <vector>
+#include "ctre/phoenix/Tasking/ILoopable.h"
+#include "ctre/phoenix/Tasking/IProcessable.h"
+
+namespace ctre { namespace phoenix { namespace tasking { namespace schedulers {
+
+class SequentialScheduler: public ILoopable, public IProcessable{
+public:
+ bool _running = false;
+ std::vector<ILoopable*> _loops;
+ unsigned int _idx = 0;
+ bool _iterated = false;
+
+ SequentialScheduler();
+ virtual ~SequentialScheduler();
+
+ void Add(ILoopable *aLoop);
+ ILoopable * GetCurrent();
+ void RemoveAll();
+ void Start();
+ void Stop();
+
+ //IProcessable
+ void Process();
+
+ //ILoopable
+ void OnStart();
+ void OnLoop();
+ void OnStop();
+ bool IsDone();
+};
+}}}}
diff --git a/cpp/include/ctre/phoenix/Utilities.h b/cpp/include/ctre/phoenix/Utilities.h
new file mode 100644
index 0000000..ce591c8
--- /dev/null
+++ b/cpp/include/ctre/phoenix/Utilities.h
@@ -0,0 +1,20 @@
+#pragma once
+
+namespace ctre {
+namespace phoenix {
+
+class Utilities {
+public:
+ static float abs(float f);
+ static float bound(float value, float capValue = 1);
+ static float cap(float value, float peak);
+ static void Deadband(float &value, float deadband = -.10);
+ static bool IsWithin(float value, float compareTo, float allowDelta);
+ static int SmallerOf(int value_1, int value_2);
+ static void Split_1(float forward, float turn, float *left, float *right);
+ static void Split_2(float left, float right, float *forward, float *turn);
+private:
+ static bool Contains(char array[], char item);
+};
+
+}}