Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/hal/include/HAL/Accelerometer.hpp b/hal/include/HAL/Accelerometer.hpp
new file mode 100644
index 0000000..103fb2a
--- /dev/null
+++ b/hal/include/HAL/Accelerometer.hpp
@@ -0,0 +1,15 @@
+#pragma once
+
+enum AccelerometerRange {
+ kRange_2G = 0,
+ kRange_4G = 1,
+ kRange_8G = 2,
+};
+
+extern "C" {
+ void setAccelerometerActive(bool);
+ void setAccelerometerRange(AccelerometerRange);
+ double getAccelerometerX();
+ double getAccelerometerY();
+ double getAccelerometerZ();
+}
diff --git a/hal/include/HAL/Analog.hpp b/hal/include/HAL/Analog.hpp
new file mode 100644
index 0000000..2aa5e43
--- /dev/null
+++ b/hal/include/HAL/Analog.hpp
@@ -0,0 +1,78 @@
+#pragma once
+
+#include <stdint.h>
+
+enum AnalogTriggerType
+{
+ kInWindow = 0,
+ kState = 1,
+ kRisingPulse = 2,
+ kFallingPulse = 3
+};
+
+extern "C"
+{
+ // Analog output functions
+ void* initializeAnalogOutputPort(void* port_pointer, int32_t *status);
+ void freeAnalogOutputPort(void* analog_port_pointer);
+ void setAnalogOutput(void* analog_port_pointer, double voltage, int32_t *status);
+ double getAnalogOutput(void* analog_port_pointer, int32_t *status);
+ bool checkAnalogOutputChannel(uint32_t pin);
+
+ // Analog input functions
+ void* initializeAnalogInputPort(void* port_pointer, int32_t *status);
+ void freeAnalogInputPort(void* analog_port_pointer);
+ bool checkAnalogModule(uint8_t module);
+ bool checkAnalogInputChannel(uint32_t pin);
+
+ void setAnalogSampleRate(double samplesPerSecond, int32_t *status);
+ float getAnalogSampleRate(int32_t *status);
+ void setAnalogAverageBits(void* analog_port_pointer, uint32_t bits, int32_t *status);
+ uint32_t getAnalogAverageBits(void* analog_port_pointer, int32_t *status);
+ void setAnalogOversampleBits(void* analog_port_pointer, uint32_t bits, int32_t *status);
+ uint32_t getAnalogOversampleBits(void* analog_port_pointer, int32_t *status);
+ int16_t getAnalogValue(void* analog_port_pointer, int32_t *status);
+ int32_t getAnalogAverageValue(void* analog_port_pointer, int32_t *status);
+ int32_t getAnalogVoltsToValue(void* analog_port_pointer, double voltage, int32_t *status);
+ float getAnalogVoltage(void* analog_port_pointer, int32_t *status);
+ float getAnalogAverageVoltage(void* analog_port_pointer, int32_t *status);
+ uint32_t getAnalogLSBWeight(void* analog_port_pointer, int32_t *status);
+ int32_t getAnalogOffset(void* analog_port_pointer, int32_t *status);
+
+ bool isAccumulatorChannel(void* analog_port_pointer, int32_t *status);
+ void initAccumulator(void* analog_port_pointer, int32_t *status);
+ void resetAccumulator(void* analog_port_pointer, int32_t *status);
+ void setAccumulatorCenter(void* analog_port_pointer, int32_t center, int32_t *status);
+ void setAccumulatorDeadband(void* analog_port_pointer, int32_t deadband, int32_t *status);
+ int64_t getAccumulatorValue(void* analog_port_pointer, int32_t *status);
+ uint32_t getAccumulatorCount(void* analog_port_pointer, int32_t *status);
+ void getAccumulatorOutput(void* analog_port_pointer, int64_t *value, uint32_t *count,
+ int32_t *status);
+
+ void* initializeAnalogTrigger(void* port_pointer, uint32_t *index, int32_t *status);
+ void cleanAnalogTrigger(void* analog_trigger_pointer, int32_t *status);
+ void setAnalogTriggerLimitsRaw(void* analog_trigger_pointer, int32_t lower, int32_t upper,
+ int32_t *status);
+ void setAnalogTriggerLimitsVoltage(void* analog_trigger_pointer, double lower, double upper,
+ int32_t *status);
+ void setAnalogTriggerAveraged(void* analog_trigger_pointer, bool useAveragedValue,
+ int32_t *status);
+ void setAnalogTriggerFiltered(void* analog_trigger_pointer, bool useFilteredValue,
+ int32_t *status);
+ bool getAnalogTriggerInWindow(void* analog_trigger_pointer, int32_t *status);
+ bool getAnalogTriggerTriggerState(void* analog_trigger_pointer, int32_t *status);
+ bool getAnalogTriggerOutput(void* analog_trigger_pointer, AnalogTriggerType type,
+ int32_t *status);
+
+ //// Float JNA Hack
+ // Float
+ int getAnalogSampleRateIntHack(int32_t *status);
+ int getAnalogVoltageIntHack(void* analog_port_pointer, int32_t *status);
+ int getAnalogAverageVoltageIntHack(void* analog_port_pointer, int32_t *status);
+
+ // Doubles
+ void setAnalogSampleRateIntHack(int samplesPerSecond, int32_t *status);
+ int32_t getAnalogVoltsToValueIntHack(void* analog_port_pointer, int voltage, int32_t *status);
+ void setAnalogTriggerLimitsVoltageIntHack(void* analog_trigger_pointer, int lower, int upper,
+ int32_t *status);
+}
diff --git a/hal/include/HAL/CAN.hpp b/hal/include/HAL/CAN.hpp
new file mode 100644
index 0000000..c10450b
--- /dev/null
+++ b/hal/include/HAL/CAN.hpp
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <stdint.h>
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+
+void canTxSend(uint32_t arbID, uint8_t length, int32_t period = CAN_SEND_PERIOD_NO_REPEAT);
+
+void canTxPackInt8 (uint32_t arbID, uint8_t offset, uint8_t value);
+void canTxPackInt16(uint32_t arbID, uint8_t offset, uint16_t value);
+void canTxPackInt32(uint32_t arbID, uint8_t offset, uint32_t value);
+void canTxPackFXP16(uint32_t arbID, uint8_t offset, double value);
+void canTxPackFXP32(uint32_t arbID, uint8_t offset, double value);
+
+uint8_t canTxUnpackInt8 (uint32_t arbID, uint8_t offset);
+uint32_t canTxUnpackInt32(uint32_t arbID, uint8_t offset);
+uint16_t canTxUnpackInt16(uint32_t arbID, uint8_t offset);
+double canTxUnpackFXP16(uint32_t arbID, uint8_t offset);
+double canTxUnpackFXP32(uint32_t arbID, uint8_t offset);
+
+bool canRxReceive(uint32_t arbID);
+
+uint8_t canRxUnpackInt8 (uint32_t arbID, uint8_t offset);
+uint16_t canRxUnpackInt16(uint32_t arbID, uint8_t offset);
+uint32_t canRxUnpackInt32(uint32_t arbID, uint8_t offset);
+double canRxUnpackFXP16(uint32_t arbID, uint8_t offset);
+double canRxUnpackFXP32(uint32_t arbID, uint8_t offset);
diff --git a/hal/include/HAL/CanTalonSRX.h b/hal/include/HAL/CanTalonSRX.h
new file mode 100644
index 0000000..4cbff71
--- /dev/null
+++ b/hal/include/HAL/CanTalonSRX.h
@@ -0,0 +1,421 @@
+/**
+ * @brief CAN TALON SRX driver.
+ *
+ * The TALON SRX is designed to instrument all runtime signals periodically. The default periods are chosen to support 16 TALONs
+ * with 10ms update rate for control (throttle or setpoint). However these can be overridden with SetStatusFrameRate. @see SetStatusFrameRate
+ * The getters for these unsolicited signals are auto generated at the bottom of this module.
+ *
+ * Likewise most control signals are sent periodically using the fire-and-forget CAN API.
+ * The setters for these unsolicited signals are auto generated at the bottom of this module.
+ *
+ * Signals that are not available in an unsolicited fashion are the Close Loop gains.
+ * For teams that have a single profile for their TALON close loop they can use either the webpage to configure their TALONs once
+ * or set the PIDF,Izone,CloseLoopRampRate,etc... once in the robot application. These parameters are saved to flash so once they are
+ * loaded in the TALON, they will persist through power cycles and mode changes.
+ *
+ * For teams that have one or two profiles to switch between, they can use the same strategy since there are two slots to choose from
+ * and the ProfileSlotSelect is periodically sent in the 10 ms control frame.
+ *
+ * For teams that require changing gains frequently, they can use the soliciting API to get and set those parameters. Most likely
+ * they will only need to set them in a periodic fashion as a function of what motion the application is attempting.
+ * If this API is used, be mindful of the CAN utilization reported in the driver station.
+ *
+ * Encoder position is measured in encoder edges. Every edge is counted (similar to roboRIO 4X mode).
+ * Analog position is 10 bits, meaning 1024 ticks per rotation (0V => 3.3V).
+ * Use SetFeedbackDeviceSelect to select which sensor type you need. Once you do that you can use GetSensorPosition()
+ * and GetSensorVelocity(). These signals are updated on CANBus every 20ms (by default).
+ * If a relative sensor is selected, you can zero (or change the current value) using SetSensorPosition.
+ *
+ * Analog Input and quadrature position (and velocity) are also explicitly reported in GetEncPosition, GetEncVel, GetAnalogInWithOv, GetAnalogInVel.
+ * These signals are available all the time, regardless of what sensor is selected at a rate of 100ms. This allows easy instrumentation
+ * for "in the pits" checking of all sensors regardless of modeselect. The 100ms rate is overridable for teams who want to acquire sensor
+ * data for processing, not just instrumentation. Or just select the sensor using SetFeedbackDeviceSelect to get it at 20ms.
+ *
+ * Velocity is in position ticks / 100ms.
+ *
+ * All output units are in respect to duty cycle (throttle) which is -1023(full reverse) to +1023 (full forward).
+ * This includes demand (which specifies duty cycle when in duty cycle mode) and rampRamp, which is in throttle units per 10ms (if nonzero).
+ *
+ * Pos and velocity close loops are calc'd as
+ * err = target - posOrVel.
+ * iErr += err;
+ * if( (IZone!=0) and abs(err) > IZone)
+ * ClearIaccum()
+ * output = P X err + I X iErr + D X dErr + F X target
+ * dErr = err - lastErr
+ * P, I,and D gains are always positive. F can be negative.
+ * Motor direction can be reversed using SetRevMotDuringCloseLoopEn if sensor and motor are out of phase.
+ * Similarly feedback sensor can also be reversed (multiplied by -1) if you prefer the sensor to be inverted.
+ *
+ * P gain is specified in throttle per error tick. For example, a value of 102 is ~9.9% (which is 102/1023) throttle per 1
+ * ADC unit(10bit) or 1 quadrature encoder edge depending on selected sensor.
+ *
+ * I gain is specified in throttle per integrated error. For example, a value of 10 equates to ~0.99% (which is 10/1023)
+ * for each accumulated ADC unit(10bit) or 1 quadrature encoder edge depending on selected sensor.
+ * Close loop and integral accumulator runs every 1ms.
+ *
+ * D gain is specified in throttle per derivative error. For example a value of 102 equates to ~9.9% (which is 102/1023)
+ * per change of 1 unit (ADC or encoder) per ms.
+ *
+ * I Zone is specified in the same units as sensor position (ADC units or quadrature edges). If pos/vel error is outside of
+ * this value, the integrated error will auto-clear...
+ * if( (IZone!=0) and abs(err) > IZone)
+ * ClearIaccum()
+ * ...this is very useful in preventing integral windup and is highly recommended if using full PID to keep stability low.
+ *
+ * CloseLoopRampRate is in throttle units per 1ms. Set to zero to disable ramping.
+ * Works the same as RampThrottle but only is in effect when a close loop mode and profile slot is selected.
+ *
+ * auto generated using spreadsheet and WpiClassGen.csproj
+ * @link https://docs.google.com/spreadsheets/d/1OU_ZV7fZLGYUQ-Uhc8sVAmUmWTlT8XBFYK8lfjg_tac/edit#gid=1766046967
+ */
+#ifndef CanTalonSRX_H_
+#define CanTalonSRX_H_
+#include "ctre/ctre.h" //BIT Defines + Typedefs
+#include "ctre/CtreCanNode.h"
+#include <FRC_NetworkCommunication/CANSessionMux.h> //CAN Comm
+#include <map>
+#include <atomic>
+class CanTalonSRX : public CtreCanNode
+{
+private:
+ // Use this for determining whether the default move constructor has been
+ // called; this prevents us from calling the destructor twice.
+ struct HasBeenMoved {
+ HasBeenMoved(HasBeenMoved&& other) {
+ other.moved = true;
+ moved = false;
+ }
+ HasBeenMoved() = default;
+ std::atomic<bool> moved{false};
+ operator bool() const { return moved; }
+ } m_hasBeenMoved;
+
+ //---------------------- Vars for opening a CAN stream if caller needs signals that require soliciting */
+ uint32_t _can_h; //!< Session handle for catching response params.
+ int32_t _can_stat; //!< Session handle status.
+ struct tCANStreamMessage _msgBuff[20];
+ static int const kMsgCapacity = 20;
+ typedef std::map<uint32_t, uint32_t> sigs_t;
+ sigs_t _sigs; //!< Catches signal updates that are solicited. Expect this to be very few.
+ void OpenSessionIfNeedBe();
+ void ProcessStreamMessages();
+ /**
+ * Send a one shot frame to set an arbitrary signal.
+ * Most signals are in the control frame so avoid using this API unless you have to.
+ * Use this api for...
+ * -A motor controller profile signal eProfileParam_XXXs. These are backed up in flash. If you are gain-scheduling then call this periodically.
+ * -Default brake and limit switch signals... eOnBoot_XXXs. Avoid doing this, use the override signals in the control frame.
+ * Talon will automatically send a PARAM_RESPONSE after the set, so GetParamResponse will catch the latest value after a couple ms.
+ */
+ CTR_Code SetParamRaw(uint32_t paramEnum, int32_t rawBits);
+ /**
+ * Checks cached CAN frames and updating solicited signals.
+ */
+ CTR_Code GetParamResponseRaw(uint32_t paramEnum, int32_t & rawBits);
+public:
+ static const int kDefaultControlPeriodMs = 10; //!< default control update rate is 10ms.
+ CanTalonSRX(int deviceNumber = 0,int controlPeriodMs = kDefaultControlPeriodMs);
+ ~CanTalonSRX();
+ void Set(double value);
+ /* mode select enumerations */
+ static const int kMode_DutyCycle = 0; //!< Demand is 11bit signed duty cycle [-1023,1023].
+ static const int kMode_PositionCloseLoop = 1; //!< Position PIDF.
+ static const int kMode_VelocityCloseLoop = 2; //!< Velocity PIDF.
+ static const int kMode_CurrentCloseLoop = 3; //!< Current close loop - not done.
+ static const int kMode_VoltCompen = 4; //!< Voltage Compensation Mode - not done. Demand is fixed pt target 8.8 volts.
+ static const int kMode_SlaveFollower = 5; //!< Demand is the 6 bit Device ID of the 'master' TALON SRX.
+ static const int kMode_NoDrive = 15; //!< Zero the output (honors brake/coast) regardless of demand. Might be useful if we need to change modes but can't atomically change all the signals we want in between.
+ /* limit switch enumerations */
+ static const int kLimitSwitchOverride_UseDefaultsFromFlash = 1;
+ static const int kLimitSwitchOverride_DisableFwd_DisableRev = 4;
+ static const int kLimitSwitchOverride_DisableFwd_EnableRev = 5;
+ static const int kLimitSwitchOverride_EnableFwd_DisableRev = 6;
+ static const int kLimitSwitchOverride_EnableFwd_EnableRev = 7;
+ /* brake override enumerations */
+ static const int kBrakeOverride_UseDefaultsFromFlash = 0;
+ static const int kBrakeOverride_OverrideCoast = 1;
+ static const int kBrakeOverride_OverrideBrake = 2;
+ /* feedback device enumerations */
+ static const int kFeedbackDev_DigitalQuadEnc=0;
+ static const int kFeedbackDev_AnalogPot=2;
+ static const int kFeedbackDev_AnalogEncoder=3;
+ static const int kFeedbackDev_CountEveryRisingEdge=4;
+ static const int kFeedbackDev_CountEveryFallingEdge=5;
+ static const int kFeedbackDev_PosIsPulseWidth=8;
+ /* ProfileSlotSelect enumerations*/
+ static const int kProfileSlotSelect_Slot0 = 0;
+ static const int kProfileSlotSelect_Slot1 = 1;
+ /* status frame rate types */
+ static const int kStatusFrame_General = 0;
+ static const int kStatusFrame_Feedback = 1;
+ static const int kStatusFrame_Encoder = 2;
+ static const int kStatusFrame_AnalogTempVbat = 3;
+ static const int kStatusFrame_PulseWidthMeas = 4;
+ /**
+ * Signal enumeration for generic signal access.
+ * Although every signal is enumerated, only use this for traffic that must be solicited.
+ * Use the auto generated getters/setters at bottom of this header as much as possible.
+ */
+ typedef enum _param_t{
+ eProfileParamSlot0_P=1,
+ eProfileParamSlot0_I=2,
+ eProfileParamSlot0_D=3,
+ eProfileParamSlot0_F=4,
+ eProfileParamSlot0_IZone=5,
+ eProfileParamSlot0_CloseLoopRampRate=6,
+ eProfileParamSlot1_P=11,
+ eProfileParamSlot1_I=12,
+ eProfileParamSlot1_D=13,
+ eProfileParamSlot1_F=14,
+ eProfileParamSlot1_IZone=15,
+ eProfileParamSlot1_CloseLoopRampRate=16,
+ eProfileParamSoftLimitForThreshold=21,
+ eProfileParamSoftLimitRevThreshold=22,
+ eProfileParamSoftLimitForEnable=23,
+ eProfileParamSoftLimitRevEnable=24,
+ eOnBoot_BrakeMode=31,
+ eOnBoot_LimitSwitch_Forward_NormallyClosed=32,
+ eOnBoot_LimitSwitch_Reverse_NormallyClosed=33,
+ eOnBoot_LimitSwitch_Forward_Disable=34,
+ eOnBoot_LimitSwitch_Reverse_Disable=35,
+ eFault_OverTemp=41,
+ eFault_UnderVoltage=42,
+ eFault_ForLim=43,
+ eFault_RevLim=44,
+ eFault_HardwareFailure=45,
+ eFault_ForSoftLim=46,
+ eFault_RevSoftLim=47,
+ eStckyFault_OverTemp=48,
+ eStckyFault_UnderVoltage=49,
+ eStckyFault_ForLim=50,
+ eStckyFault_RevLim=51,
+ eStckyFault_ForSoftLim=52,
+ eStckyFault_RevSoftLim=53,
+ eAppliedThrottle=61,
+ eCloseLoopErr=62,
+ eFeedbackDeviceSelect=63,
+ eRevMotDuringCloseLoopEn=64,
+ eModeSelect=65,
+ eProfileSlotSelect=66,
+ eRampThrottle=67,
+ eRevFeedbackSensor=68,
+ eLimitSwitchEn=69,
+ eLimitSwitchClosedFor=70,
+ eLimitSwitchClosedRev=71,
+ eSensorPosition=73,
+ eSensorVelocity=74,
+ eCurrent=75,
+ eBrakeIsEnabled=76,
+ eEncPosition=77,
+ eEncVel=78,
+ eEncIndexRiseEvents=79,
+ eQuadApin=80,
+ eQuadBpin=81,
+ eQuadIdxpin=82,
+ eAnalogInWithOv=83,
+ eAnalogInVel=84,
+ eTemp=85,
+ eBatteryV=86,
+ eResetCount=87,
+ eResetFlags=88,
+ eFirmVers=89,
+ eSettingsChanged=90,
+ eQuadFilterEn=91,
+ ePidIaccum=93,
+ eStatus1FrameRate=94, // TALON_Status_1_General_10ms_t
+ eStatus2FrameRate=95, // TALON_Status_2_Feedback_20ms_t
+ eStatus3FrameRate=96, // TALON_Status_3_Enc_100ms_t
+ eStatus4FrameRate=97, // TALON_Status_4_AinTempVbat_100ms_t
+ eStatus6FrameRate=98, // TALON_Status_6_Eol_t
+ eStatus7FrameRate=99, // TALON_Status_7_Debug_200ms_t
+ eClearPositionOnIdx=100,
+ //reserved,
+ //reserved,
+ //reserved,
+ ePeakPosOutput=104,
+ eNominalPosOutput=105,
+ ePeakNegOutput=106,
+ eNominalNegOutput=107,
+ eQuadIdxPolarity=108,
+ eStatus8FrameRate=109, // TALON_Status_8_PulseWid_100ms_t
+ eAllowPosOverflow=110,
+ eProfileParamSlot0_AllowableClosedLoopErr=111,
+ eNumberPotTurns=112,
+ eNumberEncoderCPR=113,
+ ePwdPosition=114,
+ eAinPosition=115,
+ eProfileParamVcompRate=116,
+ eProfileParamSlot1_AllowableClosedLoopErr=117,
+ }param_t;
+ /*---------------------setters and getters that use the solicated param request/response-------------*//**
+ * Send a one shot frame to set an arbitrary signal.
+ * Most signals are in the control frame so avoid using this API unless you have to.
+ * Use this api for...
+ * -A motor controller profile signal eProfileParam_XXXs. These are backed up in flash. If you are gain-scheduling then call this periodically.
+ * -Default brake and limit switch signals... eOnBoot_XXXs. Avoid doing this, use the override signals in the control frame.
+ * Talon will automatically send a PARAM_RESPONSE after the set, so GetParamResponse will catch the latest value after a couple ms.
+ */
+ CTR_Code SetParam(param_t paramEnum, double value);
+ /**
+ * Asks TALON to immedietely respond with signal value. This API is only used for signals that are not sent periodically.
+ * This can be useful for reading params that rarely change like Limit Switch settings and PIDF values.
+ * @param param to request.
+ */
+ CTR_Code RequestParam(param_t paramEnum);
+ CTR_Code GetParamResponse(param_t paramEnum, double & value);
+ CTR_Code GetParamResponseInt32(param_t paramEnum, int & value);
+ /*----- getters and setters that use param request/response. These signals are backed up in flash and will survive a power cycle. ---------*/
+ /*----- If your application requires changing these values consider using both slots and switch between slot0 <=> slot1. ------------------*/
+ /*----- If your application requires changing these signals frequently then it makes sense to leverage this API. --------------------------*/
+ /*----- Getters don't block, so it may require several calls to get the latest value. --------------------------*/
+ CTR_Code SetPgain(unsigned slotIdx,double gain);
+ CTR_Code SetIgain(unsigned slotIdx,double gain);
+ CTR_Code SetDgain(unsigned slotIdx,double gain);
+ CTR_Code SetFgain(unsigned slotIdx,double gain);
+ CTR_Code SetIzone(unsigned slotIdx,int zone);
+ CTR_Code SetCloseLoopRampRate(unsigned slotIdx,int closeLoopRampRate);
+ CTR_Code SetVoltageCompensationRate(double voltagePerMs);
+ CTR_Code SetSensorPosition(int pos);
+ CTR_Code SetForwardSoftLimit(int forwardLimit);
+ CTR_Code SetReverseSoftLimit(int reverseLimit);
+ CTR_Code SetForwardSoftEnable(int enable);
+ CTR_Code SetReverseSoftEnable(int enable);
+ CTR_Code GetPgain(unsigned slotIdx,double & gain);
+ CTR_Code GetIgain(unsigned slotIdx,double & gain);
+ CTR_Code GetDgain(unsigned slotIdx,double & gain);
+ CTR_Code GetFgain(unsigned slotIdx,double & gain);
+ CTR_Code GetIzone(unsigned slotIdx,int & zone);
+ CTR_Code GetCloseLoopRampRate(unsigned slotIdx,int & closeLoopRampRate);
+ CTR_Code GetVoltageCompensationRate(double & voltagePerMs);
+ CTR_Code GetForwardSoftLimit(int & forwardLimit);
+ CTR_Code GetReverseSoftLimit(int & reverseLimit);
+ CTR_Code GetForwardSoftEnable(int & enable);
+ CTR_Code GetReverseSoftEnable(int & enable);
+ /**
+ * Change the periodMs of a TALON's status frame. See kStatusFrame_* enums for what's available.
+ */
+ CTR_Code SetStatusFrameRate(unsigned frameEnum, unsigned periodMs);
+ /**
+ * Clear all sticky faults in TALON.
+ */
+ CTR_Code ClearStickyFaults();
+ /*------------------------ auto generated. This API is optimal since it uses the fire-and-forget CAN interface ----------------------*/
+ /*------------------------ These signals should cover the majority of all use cases. ----------------------------------*/
+ CTR_Code GetFault_OverTemp(int ¶m);
+ CTR_Code GetFault_UnderVoltage(int ¶m);
+ CTR_Code GetFault_ForLim(int ¶m);
+ CTR_Code GetFault_RevLim(int ¶m);
+ CTR_Code GetFault_HardwareFailure(int ¶m);
+ CTR_Code GetFault_ForSoftLim(int ¶m);
+ CTR_Code GetFault_RevSoftLim(int ¶m);
+ CTR_Code GetStckyFault_OverTemp(int ¶m);
+ CTR_Code GetStckyFault_UnderVoltage(int ¶m);
+ CTR_Code GetStckyFault_ForLim(int ¶m);
+ CTR_Code GetStckyFault_RevLim(int ¶m);
+ CTR_Code GetStckyFault_ForSoftLim(int ¶m);
+ CTR_Code GetStckyFault_RevSoftLim(int ¶m);
+ CTR_Code GetAppliedThrottle(int ¶m);
+ CTR_Code GetCloseLoopErr(int ¶m);
+ CTR_Code GetFeedbackDeviceSelect(int ¶m);
+ CTR_Code GetModeSelect(int ¶m);
+ CTR_Code GetLimitSwitchEn(int ¶m);
+ CTR_Code GetLimitSwitchClosedFor(int ¶m);
+ CTR_Code GetLimitSwitchClosedRev(int ¶m);
+ CTR_Code GetSensorPosition(int ¶m);
+ CTR_Code GetSensorVelocity(int ¶m);
+ CTR_Code GetCurrent(double ¶m);
+ CTR_Code GetBrakeIsEnabled(int ¶m);
+ CTR_Code GetEncPosition(int ¶m);
+ CTR_Code GetEncVel(int ¶m);
+ CTR_Code GetEncIndexRiseEvents(int ¶m);
+ CTR_Code GetQuadApin(int ¶m);
+ CTR_Code GetQuadBpin(int ¶m);
+ CTR_Code GetQuadIdxpin(int ¶m);
+ CTR_Code GetAnalogInWithOv(int ¶m);
+ CTR_Code GetAnalogInVel(int ¶m);
+ CTR_Code GetTemp(double ¶m);
+ CTR_Code GetBatteryV(double ¶m);
+ CTR_Code GetResetCount(int ¶m);
+ CTR_Code GetResetFlags(int ¶m);
+ CTR_Code GetFirmVers(int ¶m);
+ CTR_Code SetDemand(int param);
+ CTR_Code SetOverrideLimitSwitchEn(int param);
+ CTR_Code SetFeedbackDeviceSelect(int param);
+ CTR_Code SetRevMotDuringCloseLoopEn(int param);
+ CTR_Code SetOverrideBrakeType(int param);
+ CTR_Code SetModeSelect(int param);
+ CTR_Code SetModeSelect(int modeSelect,int demand);
+ CTR_Code SetProfileSlotSelect(int param);
+ CTR_Code SetRampThrottle(int param);
+ CTR_Code SetRevFeedbackSensor(int param);
+ CTR_Code GetPulseWidthPosition(int ¶m);
+ CTR_Code GetPulseWidthVelocity(int ¶m);
+ CTR_Code GetPulseWidthRiseToFallUs(int ¶m);
+ CTR_Code GetPulseWidthRiseToRiseUs(int ¶m);
+ CTR_Code IsPulseWidthSensorPresent(int ¶m);
+};
+extern "C" {
+ void *c_TalonSRX_Create(int deviceNumber, int controlPeriodMs);
+ void c_TalonSRX_Destroy(void *handle);
+ CTR_Code c_TalonSRX_SetParam(void *handle, int paramEnum, double value);
+ CTR_Code c_TalonSRX_RequestParam(void *handle, int paramEnum);
+ CTR_Code c_TalonSRX_GetParamResponse(void *handle, int paramEnum, double *value);
+ CTR_Code c_TalonSRX_GetParamResponseInt32(void *handle, int paramEnum, int *value);
+ CTR_Code c_TalonSRX_SetStatusFrameRate(void *handle, unsigned frameEnum, unsigned periodMs);
+ CTR_Code c_TalonSRX_ClearStickyFaults(void *handle);
+ CTR_Code c_TalonSRX_GetFault_OverTemp(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_UnderVoltage(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_ForLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_RevLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_HardwareFailure(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_ForSoftLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFault_RevSoftLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_OverTemp(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_UnderVoltage(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_ForLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_RevLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_ForSoftLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetStckyFault_RevSoftLim(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetAppliedThrottle(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetCloseLoopErr(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFeedbackDeviceSelect(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetModeSelect(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetLimitSwitchEn(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetLimitSwitchClosedFor(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetLimitSwitchClosedRev(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetSensorPosition(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetSensorVelocity(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetCurrent(void *handle, double *param);
+ CTR_Code c_TalonSRX_GetBrakeIsEnabled(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetEncPosition(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetEncVel(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetEncIndexRiseEvents(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetQuadApin(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetQuadBpin(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetQuadIdxpin(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetAnalogInWithOv(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetAnalogInVel(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetTemp(void *handle, double *param);
+ CTR_Code c_TalonSRX_GetBatteryV(void *handle, double *param);
+ CTR_Code c_TalonSRX_GetResetCount(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetResetFlags(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetFirmVers(void *handle, int *param);
+ CTR_Code c_TalonSRX_SetDemand(void *handle, int param);
+ CTR_Code c_TalonSRX_SetOverrideLimitSwitchEn(void *handle, int param);
+ CTR_Code c_TalonSRX_SetFeedbackDeviceSelect(void *handle, int param);
+ CTR_Code c_TalonSRX_SetRevMotDuringCloseLoopEn(void *handle, int param);
+ CTR_Code c_TalonSRX_SetOverrideBrakeType(void *handle, int param);
+ CTR_Code c_TalonSRX_SetModeSelect(void *handle, int param);
+ CTR_Code c_TalonSRX_SetModeSelect2(void *handle, int modeSelect, int demand);
+ CTR_Code c_TalonSRX_SetProfileSlotSelect(void *handle, int param);
+ CTR_Code c_TalonSRX_SetRampThrottle(void *handle, int param);
+ CTR_Code c_TalonSRX_SetRevFeedbackSensor(void *handle, int param);
+ CTR_Code c_TalonSRX_GetPulseWidthPosition(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetPulseWidthVelocity(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetPulseWidthRiseToFallUs(void *handle, int *param);
+ CTR_Code c_TalonSRX_GetPulseWidthRiseToRiseUs(void *handle, int *param);
+ CTR_Code c_TalonSRX_IsPulseWidthSensorPresent(void *handle, int *param);
+}
+#endif
+
diff --git a/hal/include/HAL/Compressor.hpp b/hal/include/HAL/Compressor.hpp
new file mode 100644
index 0000000..465a63b
--- /dev/null
+++ b/hal/include/HAL/Compressor.hpp
@@ -0,0 +1,33 @@
+/**
+ * Compressor.h
+ * Methods for interacting with a compressor with the CAN PCM device
+ */
+
+#include <stdint.h>
+
+#ifndef __HAL_COMPRESSOR_H__
+#define __HAL_COMPRESSOR_H__
+
+extern "C" {
+ void *initializeCompressor(uint8_t module);
+ bool checkCompressorModule(uint8_t module);
+
+ bool getCompressor(void *pcm_pointer, int32_t *status);
+
+ void setClosedLoopControl(void *pcm_pointer, bool value, int32_t *status);
+ bool getClosedLoopControl(void *pcm_pointer, int32_t *status);
+
+ bool getPressureSwitch(void *pcm_pointer, int32_t *status);
+ float getCompressorCurrent(void *pcm_pointer, int32_t *status);
+
+ bool getCompressorCurrentTooHighFault(void *pcm_pointer, int32_t *status);
+ bool getCompressorCurrentTooHighStickyFault(void *pcm_pointer, int32_t *status);
+ bool getCompressorShortedStickyFault(void *pcm_pointer, int32_t *status);
+ bool getCompressorShortedFault(void *pcm_pointer, int32_t *status);
+ bool getCompressorNotConnectedStickyFault(void *pcm_pointer, int32_t *status);
+ bool getCompressorNotConnectedFault(void *pcm_pointer, int32_t *status);
+ void clearAllPCMStickyFaults(void *pcm_pointer, int32_t *status);
+}
+
+#endif
+
diff --git a/hal/include/HAL/Digital.hpp b/hal/include/HAL/Digital.hpp
new file mode 100644
index 0000000..64851f1
--- /dev/null
+++ b/hal/include/HAL/Digital.hpp
@@ -0,0 +1,139 @@
+#pragma once
+#include <stdint.h>
+
+#include "HAL/cpp/priority_mutex.h"
+
+enum Mode
+{
+ kTwoPulse = 0,
+ kSemiperiod = 1,
+ kPulseLength = 2,
+ kExternalDirection = 3
+};
+
+priority_recursive_mutex& spiGetSemaphore(uint8_t port);
+
+extern "C"
+{
+ void* initializeDigitalPort(void* port_pointer, int32_t *status);
+ void freeDigitalPort(void* digital_port_pointer);
+ bool checkPWMChannel(void* digital_port_pointer);
+ bool checkRelayChannel(void* digital_port_pointer);
+
+ void setPWM(void* digital_port_pointer, unsigned short value, int32_t *status);
+ bool allocatePWMChannel(void* digital_port_pointer, int32_t *status);
+ void freePWMChannel(void* digital_port_pointer, int32_t *status);
+ unsigned short getPWM(void* digital_port_pointer, int32_t *status);
+ void latchPWMZero(void* digital_port_pointer, int32_t *status);
+ void setPWMPeriodScale(void* digital_port_pointer, uint32_t squelchMask, int32_t *status);
+ void* allocatePWM(int32_t *status);
+ void freePWM(void* pwmGenerator, int32_t *status);
+ void setPWMRate(double rate, int32_t *status);
+ void setPWMDutyCycle(void* pwmGenerator, double dutyCycle, int32_t *status);
+ void setPWMOutputChannel(void* pwmGenerator, uint32_t pin, int32_t *status);
+
+ void setRelayForward(void* digital_port_pointer, bool on, int32_t *status);
+ void setRelayReverse(void* digital_port_pointer, bool on, int32_t *status);
+ bool getRelayForward(void* digital_port_pointer, int32_t *status);
+ bool getRelayReverse(void* digital_port_pointer, int32_t *status);
+
+ bool allocateDIO(void* digital_port_pointer, bool input, int32_t *status);
+ void freeDIO(void* digital_port_pointer, int32_t *status);
+ void setDIO(void* digital_port_pointer, short value, int32_t *status);
+ bool getDIO(void* digital_port_pointer, int32_t *status);
+ bool getDIODirection(void* digital_port_pointer, int32_t *status);
+ void pulse(void* digital_port_pointer, double pulseLength, int32_t *status);
+ bool isPulsing(void* digital_port_pointer, int32_t *status);
+ bool isAnyPulsing(int32_t *status);
+
+ void setFilterSelect(void* digital_port_pointer, int filter_index,
+ int32_t* status);
+ int getFilterSelect(void* digital_port_pointer, int32_t* status);
+
+ void setFilterPeriod(int filter_index, uint32_t value, int32_t* status);
+ uint32_t getFilterPeriod(int filter_index, int32_t* status);
+
+ void* initializeCounter(Mode mode, uint32_t *index, int32_t *status);
+ void freeCounter(void* counter_pointer, int32_t *status);
+ void setCounterAverageSize(void* counter_pointer, int32_t size, int32_t *status);
+ void setCounterUpSource(void* counter_pointer, uint32_t pin, bool analogTrigger, int32_t *status);
+ void setCounterUpSourceEdge(void* counter_pointer, bool risingEdge, bool fallingEdge,
+ int32_t *status);
+ void clearCounterUpSource(void* counter_pointer, int32_t *status);
+ void setCounterDownSource(void* counter_pointer, uint32_t pin, bool analogTrigger, int32_t *status);
+ void setCounterDownSourceEdge(void* counter_pointer, bool risingEdge, bool fallingEdge,
+ int32_t *status);
+ void clearCounterDownSource(void* counter_pointer, int32_t *status);
+ void setCounterUpDownMode(void* counter_pointer, int32_t *status);
+ void setCounterExternalDirectionMode(void* counter_pointer, int32_t *status);
+ void setCounterSemiPeriodMode(void* counter_pointer, bool highSemiPeriod, int32_t *status);
+ void setCounterPulseLengthMode(void* counter_pointer, double threshold, int32_t *status);
+ int32_t getCounterSamplesToAverage(void* counter_pointer, int32_t *status);
+ void setCounterSamplesToAverage(void* counter_pointer, int samplesToAverage, int32_t *status);
+ void resetCounter(void* counter_pointer, int32_t *status);
+ int32_t getCounter(void* counter_pointer, int32_t *status);
+ double getCounterPeriod(void* counter_pointer, int32_t *status);
+ void setCounterMaxPeriod(void* counter_pointer, double maxPeriod, int32_t *status);
+ void setCounterUpdateWhenEmpty(void* counter_pointer, bool enabled, int32_t *status);
+ bool getCounterStopped(void* counter_pointer, int32_t *status);
+ bool getCounterDirection(void* counter_pointer, int32_t *status);
+ void setCounterReverseDirection(void* counter_pointer, bool reverseDirection, int32_t *status);
+
+ void* initializeEncoder(uint8_t port_a_module, uint32_t port_a_pin, bool port_a_analog_trigger,
+ uint8_t port_b_module, uint32_t port_b_pin, bool port_b_analog_trigger,
+ bool reverseDirection, int32_t *index, int32_t *status); // TODO: fix routing
+ void freeEncoder(void* encoder_pointer, int32_t *status);
+ void resetEncoder(void* encoder_pointer, int32_t *status);
+ int32_t getEncoder(void* encoder_pointer, int32_t *status); // Raw value
+ double getEncoderPeriod(void* encoder_pointer, int32_t *status);
+ void setEncoderMaxPeriod(void* encoder_pointer, double maxPeriod, int32_t *status);
+ bool getEncoderStopped(void* encoder_pointer, int32_t *status);
+ bool getEncoderDirection(void* encoder_pointer, int32_t *status);
+ void setEncoderReverseDirection(void* encoder_pointer, bool reverseDirection, int32_t *status);
+ void setEncoderSamplesToAverage(void* encoder_pointer, uint32_t samplesToAverage,
+ int32_t *status);
+ uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t *status);
+ void setEncoderIndexSource(void *encoder_pointer, uint32_t pin, bool analogTrigger, bool activeHigh,
+ bool edgeSensitive, int32_t *status);
+
+ uint16_t getLoopTiming(int32_t *status);
+
+ void spiInitialize(uint8_t port, int32_t *status);
+ int32_t spiTransaction(uint8_t port, uint8_t *dataToSend, uint8_t *dataReceived, uint8_t size);
+ int32_t spiWrite(uint8_t port, uint8_t* dataToSend, uint8_t sendSize);
+ int32_t spiRead(uint8_t port, uint8_t *buffer, uint8_t count);
+ void spiClose(uint8_t port);
+ void spiSetSpeed(uint8_t port, uint32_t speed);
+ void spiSetOpts(uint8_t port, int msb_first, int sample_on_trailing, int clk_idle_high);
+ void spiSetChipSelectActiveHigh(uint8_t port, int32_t *status);
+ void spiSetChipSelectActiveLow(uint8_t port, int32_t *status);
+ int32_t spiGetHandle(uint8_t port);
+ void spiSetHandle(uint8_t port, int32_t handle);
+
+ void spiInitAccumulator(uint8_t port, uint32_t period, uint32_t cmd,
+ uint8_t xfer_size, uint32_t valid_mask,
+ uint32_t valid_value, uint8_t data_shift,
+ uint8_t data_size, bool is_signed, bool big_endian,
+ int32_t *status);
+ void spiFreeAccumulator(uint8_t port, int32_t *status);
+ void spiResetAccumulator(uint8_t port, int32_t *status);
+ void spiSetAccumulatorCenter(uint8_t port, int32_t center, int32_t *status);
+ void spiSetAccumulatorDeadband(uint8_t port, int32_t deadband, int32_t *status);
+ int32_t spiGetAccumulatorLastValue(uint8_t port, int32_t *status);
+ int64_t spiGetAccumulatorValue(uint8_t port, int32_t *status);
+ uint32_t spiGetAccumulatorCount(uint8_t port, int32_t *status);
+ double spiGetAccumulatorAverage(uint8_t port, int32_t *status);
+ void spiGetAccumulatorOutput(uint8_t port, int64_t *value, uint32_t *count,
+ int32_t *status);
+
+ void i2CInitialize(uint8_t port, int32_t *status);
+ int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend, uint8_t sendSize, uint8_t *dataReceived, uint8_t receiveSize);
+ int32_t i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend, uint8_t sendSize);
+ int32_t i2CRead(uint8_t port, uint8_t deviceAddress, uint8_t *buffer, uint8_t count);
+ void i2CClose(uint8_t port);
+
+ //// Float JNA Hack
+ // double
+ void setPWMRateIntHack(int rate, int32_t *status);
+ void setPWMDutyCycleIntHack(void* pwmGenerator, int32_t dutyCycle, int32_t *status);
+}
diff --git a/hal/include/HAL/Errors.hpp b/hal/include/HAL/Errors.hpp
new file mode 100644
index 0000000..35ed961
--- /dev/null
+++ b/hal/include/HAL/Errors.hpp
@@ -0,0 +1,64 @@
+#pragma once
+
+#define CTR_RxTimeout_MESSAGE "CTRE CAN Recieve Timeout"
+#define CTR_TxTimeout_MESSAGE "CTRE CAN Transmit Timeout"
+#define CTR_InvalidParamValue_MESSAGE "CTRE CAN Invalid Parameter"
+#define CTR_UnexpectedArbId_MESSAGE "CTRE Unexpected Arbitration ID (CAN Node ID)"
+#define CTR_TxFailed_MESSAGE "CTRE CAN Transmit Error"
+#define CTR_SigNotUpdated_MESSAGE "CTRE CAN Signal Not Updated"
+
+#define NiFpga_Status_FifoTimeout_MESSAGE "NIFPGA: FIFO timeout error"
+#define NiFpga_Status_TransferAborted_MESSAGE "NIFPGA: Transfer aborted error"
+#define NiFpga_Status_MemoryFull_MESSAGE "NIFPGA: Memory Allocation failed, memory full"
+#define NiFpga_Status_SoftwareFault_MESSAGE "NIFPGA: Unexepected software error"
+#define NiFpga_Status_InvalidParameter_MESSAGE "NIFPGA: Invalid Parameter"
+#define NiFpga_Status_ResourceNotFound_MESSAGE "NIFPGA: Resource not found"
+#define NiFpga_Status_ResourceNotInitialized_MESSAGE "NIFPGA: Resource not initialized"
+#define NiFpga_Status_HardwareFault_MESSAGE "NIFPGA: Hardware Fault"
+#define NiFpga_Status_IrqTimeout_MESSAGE "NIFPGA: Interrupt timeout"
+
+#define ERR_CANSessionMux_InvalidBuffer_MESSAGE "CAN: Invalid Buffer"
+#define ERR_CANSessionMux_MessageNotFound_MESSAGE "CAN: Message not found"
+#define WARN_CANSessionMux_NoToken_MESSAGE "CAN: No token"
+#define ERR_CANSessionMux_NotAllowed_MESSAGE "CAN: Not allowed"
+#define ERR_CANSessionMux_NotInitialized_MESSAGE "CAN: Not initialized"
+
+#define SAMPLE_RATE_TOO_HIGH 1001
+#define SAMPLE_RATE_TOO_HIGH_MESSAGE "HAL: Analog module sample rate is too high"
+#define VOLTAGE_OUT_OF_RANGE 1002
+#define VOLTAGE_OUT_OF_RANGE_MESSAGE "HAL: Voltage to convert to raw value is out of range [0; 5]"
+#define LOOP_TIMING_ERROR 1004
+#define LOOP_TIMING_ERROR_MESSAGE "HAL: Digital module loop timing is not the expected value"
+#define SPI_WRITE_NO_MOSI 1012
+#define SPI_WRITE_NO_MOSI_MESSAGE "HAL: Cannot write to SPI port with no MOSI output"
+#define SPI_READ_NO_MISO 1013
+#define SPI_READ_NO_MISO_MESSAGE "HAL: Cannot read from SPI port with no MISO input"
+#define SPI_READ_NO_DATA 1014
+#define SPI_READ_NO_DATA_MESSAGE "HAL: No data available to read from SPI"
+#define INCOMPATIBLE_STATE 1015
+#define INCOMPATIBLE_STATE_MESSAGE "HAL: Incompatible State: The operation cannot be completed"
+#define NO_AVAILABLE_RESOURCES -1004
+#define NO_AVAILABLE_RESOURCES_MESSAGE "HAL: No available resources to allocate"
+#define NULL_PARAMETER -1005
+#define NULL_PARAMETER_MESSAGE "HAL: A pointer parameter to a method is NULL"
+#define ANALOG_TRIGGER_LIMIT_ORDER_ERROR -1010
+#define ANALOG_TRIGGER_LIMIT_ORDER_ERROR_MESSAGE "HAL: AnalogTrigger limits error. Lower limit > Upper Limit"
+#define ANALOG_TRIGGER_PULSE_OUTPUT_ERROR -1011
+#define ANALOG_TRIGGER_PULSE_OUTPUT_ERROR_MESSAGE "HAL: Attempted to read AnalogTrigger pulse output."
+#define PARAMETER_OUT_OF_RANGE -1028
+#define PARAMETER_OUT_OF_RANGE_MESSAGE "HAL: A parameter is out of range."
+#define RESOURCE_IS_ALLOCATED -1029
+#define RESOURCE_IS_ALLOCATED_MESSAGE "HAL: Resource already allocated"
+
+#define VI_ERROR_SYSTEM_ERROR_MESSAGE "HAL - VISA: System Error";
+#define VI_ERROR_INV_OBJECT_MESSAGE "HAL - VISA: Invalid Object"
+#define VI_ERROR_RSRC_LOCKED_MESSAGE "HAL - VISA: Resource Locked"
+#define VI_ERROR_RSRC_NFOUND_MESSAGE "HAL - VISA: Resource Not Found"
+#define VI_ERROR_INV_RSRC_NAME_MESSAGE "HAL - VISA: Invalid Resource Name"
+#define VI_ERROR_QUEUE_OVERFLOW_MESSAGE "HAL - VISA: Queue Overflow"
+#define VI_ERROR_IO_MESSAGE "HAL - VISA: General IO Error"
+#define VI_ERROR_ASRL_PARITY_MESSAGE "HAL - VISA: Parity Error"
+#define VI_ERROR_ASRL_FRAMING_MESSAGE "HAL - VISA: Framing Error"
+#define VI_ERROR_ASRL_OVERRUN_MESSAGE "HAL - VISA: Buffer Overrun Error"
+#define VI_ERROR_RSRC_BUSY_MESSAGE "HAL - VISA: Resource Busy"
+#define VI_ERROR_INV_PARAMETER_MESSAGE "HAL - VISA: Invalid Parameter"
diff --git a/hal/include/HAL/HAL.hpp b/hal/include/HAL/HAL.hpp
new file mode 100644
index 0000000..f09a5d2
--- /dev/null
+++ b/hal/include/HAL/HAL.hpp
@@ -0,0 +1,268 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2013. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include <stdint.h>
+#include <cmath>
+
+#include "Accelerometer.hpp"
+#include "Analog.hpp"
+#include "Compressor.hpp"
+#include "Digital.hpp"
+#include "Solenoid.hpp"
+#include "Notifier.hpp"
+#include "Interrupts.hpp"
+#include "Errors.hpp"
+#include "PDP.hpp"
+#include "Power.hpp"
+#include "SerialPort.hpp"
+
+#include "Utilities.hpp"
+#include "Semaphore.hpp"
+#include "Task.hpp"
+
+#define HAL_IO_CONFIG_DATA_SIZE 32
+#define HAL_SYS_STATUS_DATA_SIZE 44
+#define HAL_USER_STATUS_DATA_SIZE (984 - HAL_IO_CONFIG_DATA_SIZE - HAL_SYS_STATUS_DATA_SIZE)
+
+#define HALFRC_NetworkCommunication_DynamicType_DSEnhancedIO_Input 17
+#define HALFRC_NetworkCommunication_DynamicType_DSEnhancedIO_Output 18
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Header 19
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Extra1 20
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Vertices1 21
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Extra2 22
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Vertices2 23
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Joystick 24
+#define HALFRC_NetworkCommunication_DynamicType_Kinect_Custom 25
+
+namespace HALUsageReporting
+{
+ enum tResourceType
+ {
+ kResourceType_Controller,
+ kResourceType_Module,
+ kResourceType_Language,
+ kResourceType_CANPlugin,
+ kResourceType_Accelerometer,
+ kResourceType_ADXL345,
+ kResourceType_AnalogChannel,
+ kResourceType_AnalogTrigger,
+ kResourceType_AnalogTriggerOutput,
+ kResourceType_CANJaguar,
+ kResourceType_Compressor,
+ kResourceType_Counter,
+ kResourceType_Dashboard,
+ kResourceType_DigitalInput,
+ kResourceType_DigitalOutput,
+ kResourceType_DriverStationCIO,
+ kResourceType_DriverStationEIO,
+ kResourceType_DriverStationLCD,
+ kResourceType_Encoder,
+ kResourceType_GearTooth,
+ kResourceType_Gyro,
+ kResourceType_I2C,
+ kResourceType_Framework,
+ kResourceType_Jaguar,
+ kResourceType_Joystick,
+ kResourceType_Kinect,
+ kResourceType_KinectStick,
+ kResourceType_PIDController,
+ kResourceType_Preferences,
+ kResourceType_PWM,
+ kResourceType_Relay,
+ kResourceType_RobotDrive,
+ kResourceType_SerialPort,
+ kResourceType_Servo,
+ kResourceType_Solenoid,
+ kResourceType_SPI,
+ kResourceType_Task,
+ kResourceType_Ultrasonic,
+ kResourceType_Victor,
+ kResourceType_Button,
+ kResourceType_Command,
+ kResourceType_AxisCamera,
+ kResourceType_PCVideoServer,
+ kResourceType_SmartDashboard,
+ kResourceType_Talon,
+ kResourceType_HiTechnicColorSensor,
+ kResourceType_HiTechnicAccel,
+ kResourceType_HiTechnicCompass,
+ kResourceType_SRF08,
+ kResourceType_AnalogOutput,
+ kResourceType_VictorSP,
+ kResourceType_TalonSRX,
+ kResourceType_CANTalonSRX,
+ kResourceType_ADXL362,
+ kResourceType_ADXRS450,
+ kResourceType_RevSPARK,
+ kResourceType_MindsensorsSD540,
+ kResourceType_DigitalFilter,
+ };
+
+ enum tInstances
+ {
+ kLanguage_LabVIEW = 1,
+ kLanguage_CPlusPlus = 2,
+ kLanguage_Java = 3,
+ kLanguage_Python = 4,
+
+ kCANPlugin_BlackJagBridge = 1,
+ kCANPlugin_2CAN = 2,
+
+ kFramework_Iterative = 1,
+ kFramework_Sample = 2,
+ kFramework_CommandControl = 3,
+
+ kRobotDrive_ArcadeStandard = 1,
+ kRobotDrive_ArcadeButtonSpin = 2,
+ kRobotDrive_ArcadeRatioCurve = 3,
+ kRobotDrive_Tank = 4,
+ kRobotDrive_MecanumPolar = 5,
+ kRobotDrive_MecanumCartesian = 6,
+
+ kDriverStationCIO_Analog = 1,
+ kDriverStationCIO_DigitalIn = 2,
+ kDriverStationCIO_DigitalOut = 3,
+
+ kDriverStationEIO_Acceleration = 1,
+ kDriverStationEIO_AnalogIn = 2,
+ kDriverStationEIO_AnalogOut = 3,
+ kDriverStationEIO_Button = 4,
+ kDriverStationEIO_LED = 5,
+ kDriverStationEIO_DigitalIn = 6,
+ kDriverStationEIO_DigitalOut = 7,
+ kDriverStationEIO_FixedDigitalOut = 8,
+ kDriverStationEIO_PWM = 9,
+ kDriverStationEIO_Encoder = 10,
+ kDriverStationEIO_TouchSlider = 11,
+
+ kADXL345_SPI = 1,
+ kADXL345_I2C = 2,
+
+ kCommand_Scheduler = 1,
+
+ kSmartDashboard_Instance = 1,
+ };
+}
+
+struct HALControlWord {
+ uint32_t enabled : 1;
+ uint32_t autonomous : 1;
+ uint32_t test :1;
+ uint32_t eStop : 1;
+ uint32_t fmsAttached:1;
+ uint32_t dsAttached:1;
+ uint32_t control_reserved : 26;
+};
+
+enum HALAllianceStationID {
+ kHALAllianceStationID_red1,
+ kHALAllianceStationID_red2,
+ kHALAllianceStationID_red3,
+ kHALAllianceStationID_blue1,
+ kHALAllianceStationID_blue2,
+ kHALAllianceStationID_blue3,
+};
+
+/* The maximum number of axes that will be stored in a single HALJoystickAxes
+ struct. This is used for allocating buffers, not bounds checking, since
+ there are usually less axes in practice. */
+static const size_t kMaxJoystickAxes = 12;
+static const size_t kMaxJoystickPOVs = 12;
+
+struct HALJoystickAxes {
+ uint16_t count;
+ int16_t axes[kMaxJoystickAxes];
+};
+
+struct HALJoystickPOVs {
+ uint16_t count;
+ int16_t povs[kMaxJoystickPOVs];
+};
+
+struct HALJoystickButtons {
+ uint32_t buttons;
+ uint8_t count;
+};
+
+struct HALJoystickDescriptor {
+ uint8_t isXbox;
+ uint8_t type;
+ char name[256];
+ uint8_t axisCount;
+ uint8_t axisTypes[kMaxJoystickAxes];
+ uint8_t buttonCount;
+ uint8_t povCount;
+};
+
+inline float intToFloat(int value)
+{
+ return (float)value;
+}
+
+inline int floatToInt(float value)
+{
+ return round(value);
+}
+
+extern "C"
+{
+ extern const uint32_t dio_kNumSystems;
+ extern const uint32_t solenoid_kNumDO7_0Elements;
+ extern const uint32_t interrupt_kNumSystems;
+ extern const uint32_t kSystemClockTicksPerMicrosecond;
+
+ void* getPort(uint8_t pin);
+ void* getPortWithModule(uint8_t module, uint8_t pin);
+ void freePort(void* port);
+ const char* getHALErrorMessage(int32_t code);
+
+ uint16_t getFPGAVersion(int32_t *status);
+ uint32_t getFPGARevision(int32_t *status);
+ uint32_t getFPGATime(int32_t *status);
+
+ bool getFPGAButton(int32_t *status);
+
+ int HALSetErrorData(const char *errors, int errorsLength, int wait_ms);
+
+ int HALGetControlWord(HALControlWord *data);
+ int HALGetAllianceStation(enum HALAllianceStationID *allianceStation);
+ int HALGetJoystickAxes(uint8_t joystickNum, HALJoystickAxes *axes);
+ int HALGetJoystickPOVs(uint8_t joystickNum, HALJoystickPOVs *povs);
+ int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons *buttons);
+ int HALGetJoystickDescriptor(uint8_t joystickNum, HALJoystickDescriptor *desc);
+ int HALGetJoystickIsXbox(uint8_t joystickNum);
+ int HALGetJoystickType(uint8_t joystickNum);
+ char* HALGetJoystickName(uint8_t joystickNum);
+ int HALGetJoystickAxisType(uint8_t joystickNum, uint8_t axis);
+ int HALSetJoystickOutputs(uint8_t joystickNum, uint32_t outputs, uint16_t leftRumble, uint16_t rightRumble);
+ int HALGetMatchTime(float *matchTime);
+
+ void HALSetNewDataSem(MULTIWAIT_ID sem);
+
+ bool HALGetSystemActive(int32_t *status);
+ bool HALGetBrownedOut(int32_t *status);
+
+ int HALInitialize(int mode = 0);
+ void HALNetworkCommunicationObserveUserProgramStarting();
+ void HALNetworkCommunicationObserveUserProgramDisabled();
+ void HALNetworkCommunicationObserveUserProgramAutonomous();
+ void HALNetworkCommunicationObserveUserProgramTeleop();
+ void HALNetworkCommunicationObserveUserProgramTest();
+
+ uint32_t HALReport(uint8_t resource, uint8_t instanceNumber, uint8_t context = 0,
+ const char *feature = NULL);
+}
+
+// TODO: HACKS for now...
+extern "C"
+{
+
+ void NumericArrayResize();
+ void RTSetCleanupProc();
+ void EDVR_CreateReference();
+ void Occur();
+}
diff --git a/hal/include/HAL/Interrupts.hpp b/hal/include/HAL/Interrupts.hpp
new file mode 100644
index 0000000..a41ca6b
--- /dev/null
+++ b/hal/include/HAL/Interrupts.hpp
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <stdint.h>
+
+#include <iostream>
+#include "errno.h"
+
+extern "C"
+{
+ typedef void (*InterruptHandlerFunction)(uint32_t interruptAssertedMask, void *param);
+
+ void* initializeInterrupts(uint32_t interruptIndex, bool watcher, int32_t *status);
+ void cleanInterrupts(void* interrupt_pointer, int32_t *status);
+
+ uint32_t waitForInterrupt(void* interrupt_pointer, double timeout, bool ignorePrevious, int32_t *status);
+ void enableInterrupts(void* interrupt_pointer, int32_t *status);
+ void disableInterrupts(void* interrupt_pointer, int32_t *status);
+ double readRisingTimestamp(void* interrupt_pointer, int32_t *status);
+ double readFallingTimestamp(void* interrupt_pointer, int32_t *status);
+ void requestInterrupts(void* interrupt_pointer, uint8_t routing_module, uint32_t routing_pin,
+ bool routing_analog_trigger, int32_t *status);
+ void attachInterruptHandler(void* interrupt_pointer, InterruptHandlerFunction handler,
+ void* param, int32_t *status);
+ void setInterruptUpSourceEdge(void* interrupt_pointer, bool risingEdge, bool fallingEdge,
+ int32_t *status);
+}
diff --git a/hal/include/HAL/Notifier.hpp b/hal/include/HAL/Notifier.hpp
new file mode 100644
index 0000000..f16572f
--- /dev/null
+++ b/hal/include/HAL/Notifier.hpp
@@ -0,0 +1,11 @@
+#pragma once
+
+#include <stdint.h>
+
+extern "C"
+{
+ void* initializeNotifier(void (*ProcessQueue)(uint32_t, void*), void* param, int32_t *status);
+ void cleanNotifier(void* notifier_pointer, int32_t *status);
+
+ void updateNotifierAlarm(void* notifier_pointer, uint32_t triggerTime, int32_t *status);
+}
diff --git a/hal/include/HAL/PDP.hpp b/hal/include/HAL/PDP.hpp
new file mode 100644
index 0000000..54ab6fa
--- /dev/null
+++ b/hal/include/HAL/PDP.hpp
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <stdint.h>
+
+extern "C"
+{
+ void initializePDP(uint8_t module);
+ double getPDPTemperature(uint8_t module, int32_t *status);
+ double getPDPVoltage(uint8_t module, int32_t *status);
+ double getPDPChannelCurrent(uint8_t module, uint8_t channel, int32_t *status);
+ double getPDPTotalCurrent(uint8_t module, int32_t *status);
+ double getPDPTotalPower(uint8_t module, int32_t *status);
+ double getPDPTotalEnergy(uint8_t module, int32_t *status);
+ void resetPDPTotalEnergy(uint8_t module, int32_t *status);
+ void clearPDPStickyFaults(uint8_t module, int32_t *status);
+}
diff --git a/hal/include/HAL/Port.h b/hal/include/HAL/Port.h
new file mode 100644
index 0000000..8679c26
--- /dev/null
+++ b/hal/include/HAL/Port.h
@@ -0,0 +1,7 @@
+#pragma once
+
+typedef struct port_t
+{
+ uint8_t pin;
+ uint8_t module;
+} Port;
diff --git a/hal/include/HAL/Power.hpp b/hal/include/HAL/Power.hpp
new file mode 100644
index 0000000..b430c1e
--- /dev/null
+++ b/hal/include/HAL/Power.hpp
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <stdint.h>
+
+extern "C"
+{
+ float getVinVoltage(int32_t *status);
+ float getVinCurrent(int32_t *status);
+ float getUserVoltage6V(int32_t *status);
+ float getUserCurrent6V(int32_t *status);
+ bool getUserActive6V(int32_t *status);
+ int getUserCurrentFaults6V(int32_t *status);
+ float getUserVoltage5V(int32_t *status);
+ float getUserCurrent5V(int32_t *status);
+ bool getUserActive5V(int32_t *status);
+ int getUserCurrentFaults5V(int32_t *status);
+ float getUserVoltage3V3(int32_t *status);
+ float getUserCurrent3V3(int32_t *status);
+ bool getUserActive3V3(int32_t *status);
+ int getUserCurrentFaults3V3(int32_t *status);
+}
diff --git a/hal/include/HAL/Semaphore.hpp b/hal/include/HAL/Semaphore.hpp
new file mode 100644
index 0000000..6b9c9e3
--- /dev/null
+++ b/hal/include/HAL/Semaphore.hpp
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "cpp/priority_condition_variable.h"
+#include "cpp/priority_mutex.h"
+
+typedef priority_mutex* MUTEX_ID;
+typedef priority_condition_variable* MULTIWAIT_ID;
+typedef priority_condition_variable::native_handle_type NATIVE_MULTIWAIT_ID;
+
+extern "C" {
+ MUTEX_ID initializeMutexNormal();
+ void deleteMutex(MUTEX_ID sem);
+ void takeMutex(MUTEX_ID sem);
+ bool tryTakeMutex(MUTEX_ID sem);
+ void giveMutex(MUTEX_ID sem);
+
+ MULTIWAIT_ID initializeMultiWait();
+ void deleteMultiWait(MULTIWAIT_ID sem);
+ void takeMultiWait(MULTIWAIT_ID sem, MUTEX_ID m);
+ void giveMultiWait(MULTIWAIT_ID sem);
+}
diff --git a/hal/include/HAL/SerialPort.hpp b/hal/include/HAL/SerialPort.hpp
new file mode 100644
index 0000000..89f1817
--- /dev/null
+++ b/hal/include/HAL/SerialPort.hpp
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <stdint.h>
+
+extern "C"
+{
+ void serialInitializePort(uint8_t port, int32_t *status);
+ void serialSetBaudRate(uint8_t port, uint32_t baud, int32_t *status);
+ void serialSetDataBits(uint8_t port, uint8_t bits, int32_t *status);
+ void serialSetParity(uint8_t port, uint8_t parity, int32_t *status);
+ void serialSetStopBits(uint8_t port, uint8_t stopBits, int32_t *status);
+ void serialSetWriteMode(uint8_t port, uint8_t mode, int32_t *status);
+ void serialSetFlowControl(uint8_t port, uint8_t flow, int32_t *status);
+ void serialSetTimeout(uint8_t port, float timeout, int32_t *status);
+ void serialEnableTermination(uint8_t port, char terminator, int32_t *status);
+ void serialDisableTermination(uint8_t port, int32_t *status);
+ void serialSetReadBufferSize(uint8_t port, uint32_t size, int32_t *status);
+ void serialSetWriteBufferSize(uint8_t port, uint32_t size, int32_t *status);
+ int32_t serialGetBytesReceived(uint8_t port, int32_t *status);
+ uint32_t serialRead(uint8_t port, char* buffer, int32_t count, int32_t *status);
+ uint32_t serialWrite(uint8_t port, const char *buffer, int32_t count, int32_t *status);
+ void serialFlush(uint8_t port, int32_t *status);
+ void serialClear(uint8_t port, int32_t *status);
+ void serialClose(uint8_t port, int32_t *status);
+}
diff --git a/hal/include/HAL/Solenoid.hpp b/hal/include/HAL/Solenoid.hpp
new file mode 100644
index 0000000..3b51f21
--- /dev/null
+++ b/hal/include/HAL/Solenoid.hpp
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <stdint.h>
+
+extern "C"
+{
+ void* initializeSolenoidPort(void* port_pointer, int32_t *status);
+ void freeSolenoidPort(void* solenoid_port_pointer);
+ bool checkSolenoidModule(uint8_t module);
+
+ bool getSolenoid(void* solenoid_port_pointer, int32_t *status);
+ uint8_t getAllSolenoids(void* solenoid_port_pointer, int32_t *status);
+ void setSolenoid(void* solenoid_port_pointer, bool value, int32_t *status);
+
+ int getPCMSolenoidBlackList(void* solenoid_port_pointer, int32_t *status);
+ bool getPCMSolenoidVoltageStickyFault(void* solenoid_port_pointer, int32_t *status);
+ bool getPCMSolenoidVoltageFault(void* solenoid_port_pointer, int32_t *status);
+ void clearAllPCMStickyFaults_sol(void *solenoid_port_pointer, int32_t *status);
+}
diff --git a/hal/include/HAL/Task.hpp b/hal/include/HAL/Task.hpp
new file mode 100644
index 0000000..3feeec8
--- /dev/null
+++ b/hal/include/HAL/Task.hpp
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <stdint.h>
+#include <pthread.h>
+
+#ifndef _FUNCPTR_DEFINED
+#define _FUNCPTR_DEFINED
+#ifdef __cplusplus
+typedef int (*FUNCPTR)(...);
+/* ptr to function returning int */
+#else
+typedef int (*FUNCPTR) (); /* ptr to function returning int */
+#endif /* __cplusplus */
+#endif /* _FUNCPTR_DEFINED */
+
+#ifndef _STATUS_DEFINED
+#define _STATUS_DEFINED
+typedef int STATUS;
+#endif /* _STATUS_DEFINED */
+
+#ifndef OK
+#define OK 0
+#endif /* OK */
+#ifndef ERROR
+#define ERROR (-1)
+#endif /* ERROR */
+
+#define NULL_TASK NULL
+typedef pthread_t* TASK;
+
+extern "C" {
+ // Note: These constants used to be declared extern and were defined in
+ // Task.cpp. This caused issues with the JNI bindings for java, and so the
+ // instantiations were moved here.
+ const int32_t HAL_taskLib_ILLEGAL_PRIORITY = 22; // 22 is EINVAL
+
+ STATUS verifyTaskID(TASK task);
+ STATUS setTaskPriority(TASK task, int priority); // valid priority [1..99]
+ STATUS getTaskPriority(TASK task, int* priority);
+}
diff --git a/hal/include/HAL/Utilities.hpp b/hal/include/HAL/Utilities.hpp
new file mode 100644
index 0000000..df8ff5e
--- /dev/null
+++ b/hal/include/HAL/Utilities.hpp
@@ -0,0 +1,13 @@
+#pragma once
+
+#include <stdint.h>
+
+extern "C"
+{
+ extern const int32_t HAL_NO_WAIT;
+ extern const int32_t HAL_WAIT_FOREVER;
+
+ void delayTicks(int32_t ticks);
+ void delayMillis(double ms);
+ void delaySeconds(double s);
+}
diff --git a/hal/include/HAL/cpp/Resource.hpp b/hal/include/HAL/cpp/Resource.hpp
new file mode 100644
index 0000000..ae31996
--- /dev/null
+++ b/hal/include/HAL/cpp/Resource.hpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "../Errors.hpp"
+#include "HAL/cpp/priority_mutex.h"
+#include <stdint.h>
+
+#include <vector>
+
+// TODO: Replace this with something appropriate to avoid conflicts with
+// wpilibC++ Resource class (which performs an essentially identical function).
+namespace hal {
+
+/**
+ * The Resource class is a convenient way to track allocated resources.
+ * It tracks them as indicies in the range [0 .. elements - 1].
+ * E.g. the library uses this to track hardware channel allocation.
+ *
+ * The Resource class does not allocate the hardware channels or other
+ * resources; it just tracks which indices were marked in use by
+ * Allocate and not yet freed by Free.
+ */
+class Resource
+{
+public:
+ Resource(const Resource&) = delete;
+ Resource& operator=(const Resource&) = delete;
+ explicit Resource(uint32_t size);
+ virtual ~Resource() = default;
+ static void CreateResourceObject(Resource **r, uint32_t elements);
+ uint32_t Allocate(const char *resourceDesc);
+ uint32_t Allocate(uint32_t index, const char *resourceDesc);
+ void Free(uint32_t index);
+
+private:
+ std::vector<bool> m_isAllocated;
+ priority_recursive_mutex m_allocateLock;
+
+ static priority_recursive_mutex m_createLock;
+};
+
+} // namespace hal
diff --git a/hal/include/HAL/cpp/Semaphore.hpp b/hal/include/HAL/cpp/Semaphore.hpp
new file mode 100644
index 0000000..6a6374b
--- /dev/null
+++ b/hal/include/HAL/cpp/Semaphore.hpp
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <cstdint>
+#include <condition_variable>
+
+#include "HAL/cpp/priority_mutex.h"
+
+class Semaphore {
+ public:
+ explicit Semaphore(uint32_t count = 0);
+ Semaphore(Semaphore&&);
+ Semaphore& operator=(Semaphore&&);
+
+ void give();
+ void take();
+
+ // @return true if semaphore was locked successfully. false if not.
+ bool tryTake();
+
+ static const int32_t kNoWait = 0;
+ static const int32_t kWaitForever = -1;
+
+ static const uint32_t kEmpty = 0;
+ static const uint32_t kFull = 1;
+
+ private:
+ priority_mutex m_mutex;
+ std::condition_variable_any m_condition;
+ uint32_t m_count = 0;
+};
diff --git a/hal/include/HAL/cpp/priority_condition_variable.h b/hal/include/HAL/cpp/priority_condition_variable.h
new file mode 100644
index 0000000..e8ab9e5
--- /dev/null
+++ b/hal/include/HAL/cpp/priority_condition_variable.h
@@ -0,0 +1,117 @@
+#pragma once
+
+/* std::condition_variable provides the native_handle() method to return its
+ * underlying pthread_cond_t*. WPILib uses this to interface with the FRC
+ * network communication library. Since WPILib uses a custom mutex class and
+ * not std::mutex, std::condition_variable_any must be used instead.
+ * std::condition_variable_any doesn't expose its internal handle, so this
+ * class provides the same interface and implementation in addition to a
+ * native_handle() method.
+ */
+
+#include <condition_variable>
+#include <memory>
+#include "priority_mutex.h"
+
+class priority_condition_variable {
+ typedef std::chrono::system_clock clock_t;
+
+ public:
+ typedef std::condition_variable::native_handle_type native_handle_type;
+
+ priority_condition_variable() : m_mutex(std::make_shared<std::mutex>()) {}
+ ~priority_condition_variable() = default;
+
+ priority_condition_variable(const priority_condition_variable&) = delete;
+ priority_condition_variable& operator=(const priority_condition_variable&) = delete;
+
+ void notify_one() noexcept {
+ std::lock_guard<std::mutex> lock(*m_mutex);
+ m_cond.notify_one();
+ }
+
+ void notify_all() noexcept {
+ std::lock_guard<std::mutex> lock(*m_mutex);
+ m_cond.notify_all();
+ }
+
+ template<typename Lock>
+ void wait(Lock& _lock) {
+ std::shared_ptr<std::mutex> _mutex = m_mutex;
+ std::unique_lock<std::mutex> my_lock(*_mutex);
+ Unlock<Lock> unlock(_lock);
+
+ // *mutex must be unlocked before re-locking _lock so move
+ // ownership of *_mutex lock to an object with shorter lifetime.
+ std::unique_lock<std::mutex> my_lock2(std::move(my_lock));
+ m_cond.wait(my_lock2);
+ }
+
+ template<typename Lock, typename Predicate>
+ void wait(Lock& lock, Predicate p) {
+ while (!p()) { wait(lock); }
+ }
+
+ template<typename Lock, typename Clock, typename Duration>
+ std::cv_status wait_until(Lock& _lock,
+ const std::chrono::time_point<Clock, Duration>& atime) {
+ std::shared_ptr<std::mutex> _mutex = m_mutex;
+ std::unique_lock<std::mutex> my_lock(*_mutex);
+ Unlock<Lock> unlock(_lock);
+
+ // *_mutex must be unlocked before re-locking _lock so move
+ // ownership of *_mutex lock to an object with shorter lifetime.
+ std::unique_lock<std::mutex> my_lock2(std::move(my_lock));
+ return m_cond.wait_until(my_lock2, atime);
+ }
+
+ template<typename Lock, typename Clock, typename Duration, typename Predicate>
+ bool wait_until(Lock& lock,
+ const std::chrono::time_point<Clock, Duration>& atime, Predicate p) {
+ while (!p()) {
+ if (wait_until(lock, atime) == std::cv_status::timeout) {
+ return p();
+ }
+ }
+ return true;
+ }
+
+ template<typename Lock, typename Rep, typename Period>
+ std::cv_status wait_for(Lock& lock, const std::chrono::duration<Rep, Period>& rtime) {
+ return wait_until(lock, clock_t::now() + rtime);
+ }
+
+ template<typename Lock, typename Rep, typename Period, typename Predicate>
+ bool wait_for(Lock& lock, const std::chrono::duration<Rep, Period>& rtime,
+ Predicate p) {
+ return wait_until(lock, clock_t::now() + rtime, std::move(p));
+ }
+
+ native_handle_type native_handle() {
+ return m_cond.native_handle();
+ }
+
+ private:
+ std::condition_variable m_cond;
+ std::shared_ptr<std::mutex> m_mutex;
+
+ // scoped unlock - unlocks in ctor, re-locks in dtor
+ template<typename Lock>
+ struct Unlock {
+ explicit Unlock(Lock& lk) : m_lock(lk) { lk.unlock(); }
+
+ ~Unlock() /*noexcept(false)*/ {
+ if (std::uncaught_exception()) {
+ try { m_lock.lock(); } catch(...) {}
+ }
+ else {
+ m_lock.lock();
+ }
+ }
+
+ Unlock(const Unlock&) = delete;
+ Unlock& operator=(const Unlock&) = delete;
+
+ Lock& m_lock;
+ };
+};
diff --git a/hal/include/HAL/cpp/priority_mutex.h b/hal/include/HAL/cpp/priority_mutex.h
new file mode 100644
index 0000000..16e88f6
--- /dev/null
+++ b/hal/include/HAL/cpp/priority_mutex.h
@@ -0,0 +1,76 @@
+#pragma once
+
+// Allows usage with std::lock_guard without including <mutex> separately
+#include <mutex>
+
+#ifdef FRC_SIMULATOR
+// We do not want to use pthreads if in the simulator; however, in the
+// simulator, we do not care about priority inversion.
+typedef std::mutex priority_mutex;
+typedef std::recursive_mutex priority_recursive_mutex;
+#else // Covers rest of file.
+
+#include <pthread.h>
+
+class priority_recursive_mutex {
+ public:
+ typedef pthread_mutex_t *native_handle_type;
+
+ constexpr priority_recursive_mutex() noexcept = default;
+ priority_recursive_mutex(const priority_recursive_mutex &) = delete;
+ priority_recursive_mutex &operator=(const priority_recursive_mutex &) = delete;
+
+ // Lock the mutex, blocking until it's available.
+ void lock();
+
+ // Unlock the mutex.
+ void unlock();
+
+ // Tries to lock the mutex.
+ bool try_lock() noexcept;
+
+ pthread_mutex_t *native_handle();
+
+ private:
+ // Do the equivalent of setting PTHREAD_PRIO_INHERIT and
+ // PTHREAD_MUTEX_RECURSIVE_NP.
+#if __WORDSIZE == 64
+ pthread_mutex_t m_mutex = {
+ {0, 0, 0, 0, 0x20 | PTHREAD_MUTEX_RECURSIVE_NP, 0, 0, {0, 0}}};
+#else
+ pthread_mutex_t m_mutex = {
+ {0, 0, 0, 0x20 | PTHREAD_MUTEX_RECURSIVE_NP, 0, {0}}};
+#endif
+};
+
+class priority_mutex {
+ public:
+ typedef pthread_mutex_t *native_handle_type;
+
+ constexpr priority_mutex() noexcept = default;
+ priority_mutex(const priority_mutex &) = delete;
+ priority_mutex &operator=(const priority_mutex &) = delete;
+
+ // Lock the mutex, blocking until it's available.
+ void lock();
+
+ // Unlock the mutex.
+ void unlock();
+
+ // Tries to lock the mutex.
+ bool try_lock() noexcept;
+
+ pthread_mutex_t *native_handle();
+
+ private:
+ // Do the equivalent of setting PTHREAD_PRIO_INHERIT.
+#if __WORDSIZE == 64
+ pthread_mutex_t m_mutex = {
+ {0, 0, 0, 0, 0x20, 0, 0, {0, 0}}};
+#else
+ pthread_mutex_t m_mutex = {
+ {0, 0, 0, 0x20, 0, {0}}};
+#endif
+};
+
+#endif // FRC_SIMULATOR