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/lib/Athena/ctre/CanTalonSRX.cpp b/hal/lib/Athena/ctre/CanTalonSRX.cpp
new file mode 100644
index 0000000..56e68e1
--- /dev/null
+++ b/hal/lib/Athena/ctre/CanTalonSRX.cpp
@@ -0,0 +1,1417 @@
+/**
+ * @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.
+ *
+ * If calling application has used the config routines to configure the selected feedback sensor, then all positions are measured in
+ * floating point precision rotations. All sensor velocities are specified in floating point precision RPM.
+ * @see ConfigPotentiometerTurns
+ * @see ConfigEncoderCodesPerRev
+ * HOWEVER, if calling application has not called the config routine for selected feedback sensor, then all getters/setters for
+ * position/velocity use the native engineering units of the Talon SRX firm (just like in 2015). Signals explained below.
+ *
+ * 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
+ */
+#include "HAL/CanTalonSRX.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h" //CAN Comm
+#include <string.h> // memset
+#include <unistd.h> // usleep
+
+#define STATUS_1 0x02041400
+#define STATUS_2 0x02041440
+#define STATUS_3 0x02041480
+#define STATUS_4 0x020414C0
+#define STATUS_5 0x02041500
+#define STATUS_6 0x02041540
+#define STATUS_7 0x02041580
+#define STATUS_8 0x020415C0
+
+#define CONTROL_1 0x02040000
+#define CONTROL_2 0x02040040
+#define CONTROL_3 0x02040080
+
+#define EXPECTED_RESPONSE_TIMEOUT_MS (200)
+#define GET_STATUS1() CtreCanNode::recMsg<TALON_Status_1_General_10ms_t > rx = GetRx<TALON_Status_1_General_10ms_t>(STATUS_1 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS2() CtreCanNode::recMsg<TALON_Status_2_Feedback_20ms_t > rx = GetRx<TALON_Status_2_Feedback_20ms_t>(STATUS_2 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS3() CtreCanNode::recMsg<TALON_Status_3_Enc_100ms_t > rx = GetRx<TALON_Status_3_Enc_100ms_t>(STATUS_3 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS4() CtreCanNode::recMsg<TALON_Status_4_AinTempVbat_100ms_t> rx = GetRx<TALON_Status_4_AinTempVbat_100ms_t>(STATUS_4 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS5() CtreCanNode::recMsg<TALON_Status_5_Startup_OneShot_t > rx = GetRx<TALON_Status_5_Startup_OneShot_t>(STATUS_5 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS6() CtreCanNode::recMsg<TALON_Status_6_Eol_t > rx = GetRx<TALON_Status_6_Eol_t>(STATUS_6 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS7() CtreCanNode::recMsg<TALON_Status_7_Debug_200ms_t > rx = GetRx<TALON_Status_7_Debug_200ms_t>(STATUS_7 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS8() CtreCanNode::recMsg<TALON_Status_8_PulseWid_100ms_t > rx = GetRx<TALON_Status_8_PulseWid_100ms_t>(STATUS_8 | GetDeviceNumber(), EXPECTED_RESPONSE_TIMEOUT_MS)
+
+#define PARAM_REQUEST 0x02041800
+#define PARAM_RESPONSE 0x02041840
+#define PARAM_SET 0x02041880
+
+const int kParamArbIdValue = PARAM_RESPONSE;
+const int kParamArbIdMask = 0xFFFFFFFF;
+
+const double FLOAT_TO_FXP_10_22 = (double)0x400000;
+const double FXP_TO_FLOAT_10_22 = 0.0000002384185791015625;
+
+const double FLOAT_TO_FXP_0_8 = (double)0x100;
+const double FXP_TO_FLOAT_0_8 = 0.00390625;
+
+/* encoder/decoders */
+/** control */
+typedef struct _TALON_Control_1_General_10ms_t {
+ unsigned TokenH:8;
+ unsigned TokenL:8;
+ unsigned DemandH:8;
+ unsigned DemandM:8;
+ unsigned DemandL:8;
+ unsigned ProfileSlotSelect:1;
+ unsigned FeedbackDeviceSelect:4;
+ unsigned OverrideLimitSwitchEn:3;
+ unsigned RevFeedbackSensor:1;
+ unsigned RevMotDuringCloseLoopEn:1;
+ unsigned OverrideBrakeType:2;
+ unsigned ModeSelect:4;
+ unsigned RampThrottle:8;
+} TALON_Control_1_General_10ms_t ;
+typedef struct _TALON_Control_2_Rates_OneShot_t {
+ unsigned Status1Ms:8;
+ unsigned Status2Ms:8;
+ unsigned Status3Ms:8;
+ unsigned Status4Ms:8;
+ unsigned StatusPulWidMs:8; // TALON_Status_8_PulseWid_100ms_t
+} TALON_Control_2_Rates_OneShot_t ;
+typedef struct _TALON_Control_3_ClearFlags_OneShot_t {
+ unsigned ZeroFeedbackSensor:1;
+ unsigned ClearStickyFaults:1;
+} TALON_Control_3_ClearFlags_OneShot_t ;
+
+/** status */
+typedef struct _TALON_Status_1_General_10ms_t {
+ unsigned CloseLoopErrH:8;
+ unsigned CloseLoopErrM:8;
+ unsigned CloseLoopErrL:8;
+ unsigned AppliedThrottle_h3:3;
+ unsigned Fault_RevSoftLim:1;
+ unsigned Fault_ForSoftLim:1;
+ unsigned TokLocked:1;
+ unsigned LimitSwitchClosedRev:1;
+ unsigned LimitSwitchClosedFor:1;
+ unsigned AppliedThrottle_l8:8;
+ unsigned ModeSelect_h1:1;
+ unsigned FeedbackDeviceSelect:4;
+ unsigned LimitSwitchEn:3;
+ unsigned Fault_HardwareFailure:1;
+ unsigned Fault_RevLim:1;
+ unsigned Fault_ForLim:1;
+ unsigned Fault_UnderVoltage:1;
+ unsigned Fault_OverTemp:1;
+ unsigned ModeSelect_b3:3;
+ unsigned TokenSeed:8;
+} TALON_Status_1_General_10ms_t ;
+typedef struct _TALON_Status_2_Feedback_20ms_t {
+ unsigned SensorPositionH:8;
+ unsigned SensorPositionM:8;
+ unsigned SensorPositionL:8;
+ unsigned SensorVelocityH:8;
+ unsigned SensorVelocityL:8;
+ unsigned Current_h8:8;
+ unsigned StckyFault_RevSoftLim:1;
+ unsigned StckyFault_ForSoftLim:1;
+ unsigned StckyFault_RevLim:1;
+ unsigned StckyFault_ForLim:1;
+ unsigned StckyFault_UnderVoltage:1;
+ unsigned StckyFault_OverTemp:1;
+ unsigned Current_l2:2;
+ unsigned reserved2:4;
+ unsigned VelDiv4:1;
+ unsigned PosDiv8:1;
+ unsigned ProfileSlotSelect:1;
+ unsigned BrakeIsEnabled:1;
+} TALON_Status_2_Feedback_20ms_t ;
+typedef struct _TALON_Status_3_Enc_100ms_t {
+ unsigned EncPositionH:8;
+ unsigned EncPositionM:8;
+ unsigned EncPositionL:8;
+ unsigned EncVelH:8;
+ unsigned EncVelL:8;
+ unsigned EncIndexRiseEventsH:8;
+ unsigned EncIndexRiseEventsL:8;
+ unsigned reserved:3;
+ unsigned VelDiv4:1;
+ unsigned PosDiv8:1;
+ unsigned QuadIdxpin:1;
+ unsigned QuadBpin:1;
+ unsigned QuadApin:1;
+} TALON_Status_3_Enc_100ms_t ;
+typedef struct _TALON_Status_4_AinTempVbat_100ms_t {
+ unsigned AnalogInWithOvH:8;
+ unsigned AnalogInWithOvM:8;
+ unsigned AnalogInWithOvL:8;
+ unsigned AnalogInVelH:8;
+ unsigned AnalogInVelL:8;
+ unsigned Temp:8;
+ unsigned BatteryV:8;
+ unsigned reserved:6;
+ unsigned VelDiv4:1;
+ unsigned PosDiv8:1;
+} TALON_Status_4_AinTempVbat_100ms_t ;
+typedef struct _TALON_Status_5_Startup_OneShot_t {
+ unsigned ResetCountH:8;
+ unsigned ResetCountL:8;
+ unsigned ResetFlagsH:8;
+ unsigned ResetFlagsL:8;
+ unsigned FirmVersH:8;
+ unsigned FirmVersL:8;
+} TALON_Status_5_Startup_OneShot_t ;
+typedef struct _TALON_Status_6_Eol_t {
+ unsigned currentAdcUncal_h2:2;
+ unsigned reserved1:5;
+ unsigned SpiCsPin_GadgeteerPin6:1;
+ unsigned currentAdcUncal_l8:8;
+ unsigned tempAdcUncal_h2:2;
+ unsigned reserved2:6;
+ unsigned tempAdcUncal_l8:8;
+ unsigned vbatAdcUncal_h2:2;
+ unsigned reserved3:6;
+ unsigned vbatAdcUncal_l8:8;
+ unsigned analogAdcUncal_h2:2;
+ unsigned reserved4:6;
+ unsigned analogAdcUncal_l8:8;
+} TALON_Status_6_Eol_t ;
+typedef struct _TALON_Status_7_Debug_200ms_t {
+ unsigned TokenizationFails_h8:8;
+ unsigned TokenizationFails_l8:8;
+ unsigned LastFailedToken_h8:8;
+ unsigned LastFailedToken_l8:8;
+ unsigned TokenizationSucceses_h8:8;
+ unsigned TokenizationSucceses_l8:8;
+} TALON_Status_7_Debug_200ms_t ;
+typedef struct _TALON_Status_8_PulseWid_100ms_t {
+ unsigned PulseWidPositionH:8;
+ unsigned PulseWidPositionM:8;
+ unsigned PulseWidPositionL:8;
+ unsigned reserved:6;
+ unsigned VelDiv4:1;
+ unsigned PosDiv8:1;
+ unsigned PeriodUsM8:8;
+ unsigned PeriodUsL8:8;
+ unsigned PulseWidVelH:8;
+ unsigned PulseWidVelL:8;
+} TALON_Status_8_PulseWid_100ms_t ;
+typedef struct _TALON_Param_Request_t {
+ unsigned ParamEnum:8;
+} TALON_Param_Request_t ;
+typedef struct _TALON_Param_Response_t {
+ unsigned ParamEnum:8;
+ unsigned ParamValueL:8;
+ unsigned ParamValueML:8;
+ unsigned ParamValueMH:8;
+ unsigned ParamValueH:8;
+} TALON_Param_Response_t ;
+
+CanTalonSRX::CanTalonSRX(int deviceNumber,int controlPeriodMs): CtreCanNode(deviceNumber), _can_h(0), _can_stat(0)
+{
+ /* bound period to be within [1 ms,95 ms] */
+ if(controlPeriodMs < 1)
+ controlPeriodMs = 1;
+ else if(controlPeriodMs > 95)
+ controlPeriodMs = 95;
+ RegisterRx(STATUS_1 | (UINT8)deviceNumber );
+ RegisterRx(STATUS_2 | (UINT8)deviceNumber );
+ RegisterRx(STATUS_3 | (UINT8)deviceNumber );
+ RegisterRx(STATUS_4 | (UINT8)deviceNumber );
+ RegisterRx(STATUS_5 | (UINT8)deviceNumber );
+ RegisterRx(STATUS_6 | (UINT8)deviceNumber );
+ RegisterRx(STATUS_7 | (UINT8)deviceNumber );
+ RegisterTx(CONTROL_1 | (UINT8)deviceNumber, (UINT8)controlPeriodMs);
+ /* the only default param that is nonzero is limit switch.
+ * Default to using the flash settings. */
+ SetOverrideLimitSwitchEn(kLimitSwitchOverride_UseDefaultsFromFlash);
+}
+/* CanTalonSRX D'tor
+ */
+CanTalonSRX::~CanTalonSRX()
+{
+ if (m_hasBeenMoved){
+ /* Another CANTalonSRX still exists, so
+ don't un-register the periodic control frame */
+ }else{
+ /* un-register the control frame so Talon is disabled */
+ RegisterTx(CONTROL_1 | (UINT8)GetDeviceNumber(), 0);
+ }
+ /* free the stream we used for SetParam/GetParamResponse */
+ if(_can_h){
+ FRC_NetworkCommunication_CANSessionMux_closeStreamSession(_can_h);
+ _can_h = 0;
+ }
+}
+void CanTalonSRX::OpenSessionIfNeedBe()
+{
+ _can_stat = 0;
+ if (_can_h == 0) {
+ /* bit30 - bit8 must match $000002XX. Top bit is not masked to get remote frames */
+ FRC_NetworkCommunication_CANSessionMux_openStreamSession(&_can_h,kParamArbIdValue | GetDeviceNumber(), kParamArbIdMask, kMsgCapacity, &_can_stat);
+ if (_can_stat == 0) {
+ /* success */
+ } else {
+ /* something went wrong, try again later */
+ _can_h = 0;
+ }
+ }
+}
+void CanTalonSRX::ProcessStreamMessages()
+{
+ if(0 == _can_h)
+ OpenSessionIfNeedBe();
+ /* process receive messages */
+ uint32_t i;
+ uint32_t messagesToRead = sizeof(_msgBuff) / sizeof(_msgBuff[0]);
+ uint32_t messagesRead = 0;
+ /* read out latest bunch of messages */
+ _can_stat = 0;
+ if (_can_h){
+ FRC_NetworkCommunication_CANSessionMux_readStreamSession(_can_h,_msgBuff, messagesToRead, &messagesRead, &_can_stat);
+ }
+ /* loop thru each message of interest */
+ for (i = 0; i < messagesRead; ++i) {
+ tCANStreamMessage * msg = _msgBuff + i;
+ if(msg->messageID == (PARAM_RESPONSE | GetDeviceNumber()) ){
+ TALON_Param_Response_t * paramResp = (TALON_Param_Response_t*)msg->data;
+ /* decode value */
+ int32_t val = paramResp->ParamValueH;
+ val <<= 8;
+ val |= paramResp->ParamValueMH;
+ val <<= 8;
+ val |= paramResp->ParamValueML;
+ val <<= 8;
+ val |= paramResp->ParamValueL;
+ /* save latest signal */
+ _sigs[paramResp->ParamEnum] = val;
+ }else{
+ int brkpthere = 42;
+ ++brkpthere;
+ }
+ }
+}
+void CanTalonSRX::Set(double value)
+{
+ if(value > 1)
+ value = 1;
+ else if(value < -1)
+ value = -1;
+ SetDemand(1023*value); /* must be within [-1023,1023] */
+}
+/*---------------------setters and getters that use the 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 CanTalonSRX::SetParamRaw(unsigned paramEnum, int rawBits)
+{
+ /* caller is using param API. Open session if it hasn'T been done. */
+ if(0 == _can_h)
+ OpenSessionIfNeedBe();
+ TALON_Param_Response_t frame;
+ memset(&frame,0,sizeof(frame));
+ frame.ParamEnum = paramEnum;
+ frame.ParamValueH = rawBits >> 0x18;
+ frame.ParamValueMH = rawBits >> 0x10;
+ frame.ParamValueML = rawBits >> 0x08;
+ frame.ParamValueL = rawBits;
+ int32_t status = 0;
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(PARAM_SET | GetDeviceNumber(), (const uint8_t*)&frame, 5, 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+/**
+ * Checks cached CAN frames and updating solicited signals.
+ */
+CTR_Code CanTalonSRX::GetParamResponseRaw(unsigned paramEnum, int & rawBits)
+{
+ CTR_Code retval = CTR_OKAY;
+ /* process received param events. We don't expect many since this API is not used often. */
+ ProcessStreamMessages();
+ /* grab the solicited signal value */
+ sigs_t::iterator i = _sigs.find(paramEnum);
+ if(i == _sigs.end()){
+ retval = CTR_SigNotUpdated;
+ }else{
+ rawBits = i->second;
+ }
+ return retval;
+}
+/**
+ * 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 CanTalonSRX::RequestParam(param_t paramEnum)
+{
+ /* process received param events. We don't expect many since this API is not used often. */
+ ProcessStreamMessages();
+ TALON_Param_Request_t frame;
+ memset(&frame,0,sizeof(frame));
+ frame.ParamEnum = paramEnum;
+ int32_t status = 0;
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(PARAM_REQUEST | GetDeviceNumber(), (const uint8_t*)&frame, 1, 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+
+CTR_Code CanTalonSRX::SetParam(param_t paramEnum, double value)
+{
+ int32_t rawbits = 0;
+ switch(paramEnum){
+ case eProfileParamSlot0_P:/* unsigned 10.22 fixed pt value */
+ case eProfileParamSlot0_I:
+ case eProfileParamSlot0_D:
+ case eProfileParamSlot1_P:
+ case eProfileParamSlot1_I:
+ case eProfileParamSlot1_D:
+ {
+ uint32_t urawbits;
+ value = std::min(value,1023.0); /* bounds check doubles that are outside u10.22 */
+ value = std::max(value,0.0);
+ urawbits = value * FLOAT_TO_FXP_10_22; /* perform unsign arithmetic */
+ rawbits = urawbits; /* copy bits over. SetParamRaw just stuffs into CAN frame with no sense of signedness */
+ } break;
+ case eProfileParamSlot1_F: /* signed 10.22 fixed pt value */
+ case eProfileParamSlot0_F:
+ value = std::min(value, 512.0); /* bounds check doubles that are outside s10.22 */
+ value = std::max(value,-512.0);
+ rawbits = value * FLOAT_TO_FXP_10_22;
+ break;
+ case eProfileParamVcompRate: /* unsigned 0.8 fixed pt value volts per ms */
+ /* within [0,1) volts per ms.
+ Slowest ramp is 1/256 VperMilliSec or 3.072 seconds from 0-to-12V.
+ Fastest ramp is 255/256 VperMilliSec or 12.1ms from 0-to-12V.
+ */
+ if(value <= 0){
+ /* negative or zero (disable), send raw value of zero */
+ rawbits = 0;
+ }else{
+ /* nonzero ramping */
+ rawbits = value * FLOAT_TO_FXP_0_8;
+ /* since whole part is cleared, cap to just under whole unit */
+ if(rawbits > (FLOAT_TO_FXP_0_8-1) )
+ rawbits = (FLOAT_TO_FXP_0_8-1);
+ /* since ramping is nonzero, cap to smallest ramp rate possible */
+ if(rawbits == 0){
+ /* caller is providing a nonzero ramp rate that's too small
+ to serialize, so cap to smallest possible */
+ rawbits = 1;
+ }
+ }
+ break;
+ default: /* everything else is integral */
+ rawbits = (int32_t)value;
+ break;
+ }
+ return SetParamRaw(paramEnum,rawbits);
+}
+CTR_Code CanTalonSRX::GetParamResponse(param_t paramEnum, double & value)
+{
+ int32_t rawbits = 0;
+ CTR_Code retval = GetParamResponseRaw(paramEnum,rawbits);
+ switch(paramEnum){
+ case eProfileParamSlot0_P:/* 10.22 fixed pt value */
+ case eProfileParamSlot0_I:
+ case eProfileParamSlot0_D:
+ case eProfileParamSlot0_F:
+ case eProfileParamSlot1_P:
+ case eProfileParamSlot1_I:
+ case eProfileParamSlot1_D:
+ case eProfileParamSlot1_F:
+ case eCurrent:
+ case eTemp:
+ case eBatteryV:
+ value = ((double)rawbits) * FXP_TO_FLOAT_10_22;
+ break;
+ case eProfileParamVcompRate:
+ value = ((double)rawbits) * FXP_TO_FLOAT_0_8;
+ break;
+ default: /* everything else is integral */
+ value = (double)rawbits;
+ break;
+ }
+ return retval;
+}
+CTR_Code CanTalonSRX::GetParamResponseInt32(param_t paramEnum, int & value)
+{
+ double dvalue = 0;
+ CTR_Code retval = GetParamResponse(paramEnum, dvalue);
+ value = (int32_t)dvalue;
+ return retval;
+}
+/*----- 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 CanTalonSRX::SetPgain(unsigned slotIdx,double gain)
+{
+ if(slotIdx == 0)
+ return SetParam(eProfileParamSlot0_P, gain);
+ return SetParam(eProfileParamSlot1_P, gain);
+}
+CTR_Code CanTalonSRX::SetIgain(unsigned slotIdx,double gain)
+{
+ if(slotIdx == 0)
+ return SetParam(eProfileParamSlot0_I, gain);
+ return SetParam(eProfileParamSlot1_I, gain);
+}
+CTR_Code CanTalonSRX::SetDgain(unsigned slotIdx,double gain)
+{
+ if(slotIdx == 0)
+ return SetParam(eProfileParamSlot0_D, gain);
+ return SetParam(eProfileParamSlot1_D, gain);
+}
+CTR_Code CanTalonSRX::SetFgain(unsigned slotIdx,double gain)
+{
+ if(slotIdx == 0)
+ return SetParam(eProfileParamSlot0_F, gain);
+ return SetParam(eProfileParamSlot1_F, gain);
+}
+CTR_Code CanTalonSRX::SetIzone(unsigned slotIdx,int zone)
+{
+ if(slotIdx == 0)
+ return SetParam(eProfileParamSlot0_IZone, zone);
+ return SetParam(eProfileParamSlot1_IZone, zone);
+}
+CTR_Code CanTalonSRX::SetCloseLoopRampRate(unsigned slotIdx,int closeLoopRampRate)
+{
+ if(slotIdx == 0)
+ return SetParam(eProfileParamSlot0_CloseLoopRampRate, closeLoopRampRate);
+ return SetParam(eProfileParamSlot1_CloseLoopRampRate, closeLoopRampRate);
+}
+CTR_Code CanTalonSRX::SetVoltageCompensationRate(double voltagePerMs)
+{
+ return SetParam(eProfileParamVcompRate, voltagePerMs);
+}
+CTR_Code CanTalonSRX::GetPgain(unsigned slotIdx,double & gain)
+{
+ if(slotIdx == 0)
+ return GetParamResponse(eProfileParamSlot0_P, gain);
+ return GetParamResponse(eProfileParamSlot1_P, gain);
+}
+CTR_Code CanTalonSRX::GetIgain(unsigned slotIdx,double & gain)
+{
+ if(slotIdx == 0)
+ return GetParamResponse(eProfileParamSlot0_I, gain);
+ return GetParamResponse(eProfileParamSlot1_I, gain);
+}
+CTR_Code CanTalonSRX::GetDgain(unsigned slotIdx,double & gain)
+{
+ if(slotIdx == 0)
+ return GetParamResponse(eProfileParamSlot0_D, gain);
+ return GetParamResponse(eProfileParamSlot1_D, gain);
+}
+CTR_Code CanTalonSRX::GetFgain(unsigned slotIdx,double & gain)
+{
+ if(slotIdx == 0)
+ return GetParamResponse(eProfileParamSlot0_F, gain);
+ return GetParamResponse(eProfileParamSlot1_F, gain);
+}
+CTR_Code CanTalonSRX::GetIzone(unsigned slotIdx,int & zone)
+{
+ if(slotIdx == 0)
+ return GetParamResponseInt32(eProfileParamSlot0_IZone, zone);
+ return GetParamResponseInt32(eProfileParamSlot1_IZone, zone);
+}
+CTR_Code CanTalonSRX::GetCloseLoopRampRate(unsigned slotIdx,int & closeLoopRampRate)
+{
+ if(slotIdx == 0)
+ return GetParamResponseInt32(eProfileParamSlot0_CloseLoopRampRate, closeLoopRampRate);
+ return GetParamResponseInt32(eProfileParamSlot1_CloseLoopRampRate, closeLoopRampRate);
+}
+CTR_Code CanTalonSRX::GetVoltageCompensationRate(double & voltagePerMs)
+{
+ return GetParamResponse(eProfileParamVcompRate, voltagePerMs);
+}
+CTR_Code CanTalonSRX::SetSensorPosition(int pos)
+{
+ return SetParam(eSensorPosition, pos);
+}
+CTR_Code CanTalonSRX::SetForwardSoftLimit(int forwardLimit)
+{
+ return SetParam(eProfileParamSoftLimitForThreshold, forwardLimit);
+}
+CTR_Code CanTalonSRX::SetReverseSoftLimit(int reverseLimit)
+{
+ return SetParam(eProfileParamSoftLimitRevThreshold, reverseLimit);
+}
+CTR_Code CanTalonSRX::SetForwardSoftEnable(int enable)
+{
+ return SetParam(eProfileParamSoftLimitForEnable, enable);
+}
+CTR_Code CanTalonSRX::SetReverseSoftEnable(int enable)
+{
+ return SetParam(eProfileParamSoftLimitRevEnable, enable);
+}
+CTR_Code CanTalonSRX::GetForwardSoftLimit(int & forwardLimit)
+{
+ return GetParamResponseInt32(eProfileParamSoftLimitForThreshold, forwardLimit);
+}
+CTR_Code CanTalonSRX::GetReverseSoftLimit(int & reverseLimit)
+{
+ return GetParamResponseInt32(eProfileParamSoftLimitRevThreshold, reverseLimit);
+}
+CTR_Code CanTalonSRX::GetForwardSoftEnable(int & enable)
+{
+ return GetParamResponseInt32(eProfileParamSoftLimitForEnable, enable);
+}
+CTR_Code CanTalonSRX::GetReverseSoftEnable(int & enable)
+{
+ return GetParamResponseInt32(eProfileParamSoftLimitRevEnable, enable);
+}
+/**
+ * Change the periodMs of a TALON's status frame. See kStatusFrame_* enums for what's available.
+ */
+CTR_Code CanTalonSRX::SetStatusFrameRate(unsigned frameEnum, unsigned periodMs)
+{
+ CTR_Code retval = CTR_OKAY;
+ int32_t paramEnum = 0;
+ /* bounds check the period */
+ if(periodMs < 1)
+ periodMs = 1;
+ else if (periodMs > 255)
+ periodMs = 255;
+ uint8_t period = (uint8_t)periodMs;
+ /* lookup the correct param enum based on what frame to rate-change */
+ switch(frameEnum){
+ case kStatusFrame_General:
+ paramEnum = eStatus1FrameRate;
+ break;
+ case kStatusFrame_Feedback:
+ paramEnum = eStatus2FrameRate;
+ break;
+ case kStatusFrame_Encoder:
+ paramEnum = eStatus3FrameRate;
+ break;
+ case kStatusFrame_AnalogTempVbat:
+ paramEnum = eStatus4FrameRate;
+ break;
+ case kStatusFrame_PulseWidthMeas:
+ paramEnum = eStatus8FrameRate;
+ break;
+ default:
+ /* caller's request is not support, return an error code */
+ retval = CTR_InvalidParamValue;
+ break;
+ }
+ /* if lookup was succesful, send set-request out */
+ if(retval == CTR_OKAY){
+ /* paramEnum is updated, sent it out */
+ retval = SetParamRaw(paramEnum, period);
+ }
+ return retval;
+}
+/**
+ * Clear all sticky faults in TALON.
+ */
+CTR_Code CanTalonSRX::ClearStickyFaults()
+{
+ int32_t status = 0;
+ /* build request frame */
+ TALON_Control_3_ClearFlags_OneShot_t frame;
+ memset(&frame,0,sizeof(frame));
+ frame.ClearStickyFaults = 1;
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_3 | GetDeviceNumber(), (const uint8_t*)&frame, sizeof(frame), 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+/*------------------------ 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 CanTalonSRX::GetFault_OverTemp(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->Fault_OverTemp;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetFault_UnderVoltage(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->Fault_UnderVoltage;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetFault_ForLim(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->Fault_ForLim;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetFault_RevLim(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->Fault_RevLim;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetFault_HardwareFailure(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->Fault_HardwareFailure;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetFault_ForSoftLim(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->Fault_ForSoftLim;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetFault_RevSoftLim(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->Fault_RevSoftLim;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetStckyFault_OverTemp(int ¶m)
+{
+ GET_STATUS2();
+ param = rx->StckyFault_OverTemp;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetStckyFault_UnderVoltage(int ¶m)
+{
+ GET_STATUS2();
+ param = rx->StckyFault_UnderVoltage;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetStckyFault_ForLim(int ¶m)
+{
+ GET_STATUS2();
+ param = rx->StckyFault_ForLim;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetStckyFault_RevLim(int ¶m)
+{
+ GET_STATUS2();
+ param = rx->StckyFault_RevLim;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetStckyFault_ForSoftLim(int ¶m)
+{
+ GET_STATUS2();
+ param = rx->StckyFault_ForSoftLim;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetStckyFault_RevSoftLim(int ¶m)
+{
+ GET_STATUS2();
+ param = rx->StckyFault_RevSoftLim;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetAppliedThrottle(int ¶m)
+{
+ GET_STATUS1();
+ int32_t raw = 0;
+ raw |= rx->AppliedThrottle_h3;
+ raw <<= 8;
+ raw |= rx->AppliedThrottle_l8;
+ raw <<= (32-11); /* sign extend */
+ raw >>= (32-11); /* sign extend */
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetCloseLoopErr(int ¶m)
+{
+ GET_STATUS1();
+ int32_t raw = 0;
+ raw |= rx->CloseLoopErrH;
+ raw <<= 8;
+ raw |= rx->CloseLoopErrM;
+ raw <<= 8;
+ raw |= rx->CloseLoopErrL;
+ raw <<= (32-24); /* sign extend */
+ raw >>= (32-24); /* sign extend */
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetFeedbackDeviceSelect(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->FeedbackDeviceSelect;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetModeSelect(int ¶m)
+{
+ GET_STATUS1();
+ uint32_t raw = 0;
+ raw |= rx->ModeSelect_h1;
+ raw <<= 3;
+ raw |= rx->ModeSelect_b3;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetLimitSwitchEn(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->LimitSwitchEn;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetLimitSwitchClosedFor(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->LimitSwitchClosedFor;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetLimitSwitchClosedRev(int ¶m)
+{
+ GET_STATUS1();
+ param = rx->LimitSwitchClosedRev;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetSensorPosition(int ¶m)
+{
+ GET_STATUS2();
+ int32_t raw = 0;
+ raw |= rx->SensorPositionH;
+ raw <<= 8;
+ raw |= rx->SensorPositionM;
+ raw <<= 8;
+ raw |= rx->SensorPositionL;
+ raw <<= (32-24); /* sign extend */
+ raw >>= (32-24); /* sign extend */
+ if(rx->PosDiv8)
+ raw *= 8;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetSensorVelocity(int ¶m)
+{
+ GET_STATUS2();
+ int32_t raw = 0;
+ raw |= rx->SensorVelocityH;
+ raw <<= 8;
+ raw |= rx->SensorVelocityL;
+ raw <<= (32-16); /* sign extend */
+ raw >>= (32-16); /* sign extend */
+ if(rx->VelDiv4)
+ raw *= 4;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetCurrent(double ¶m)
+{
+ GET_STATUS2();
+ uint32_t raw = 0;
+ raw |= rx->Current_h8;
+ raw <<= 2;
+ raw |= rx->Current_l2;
+ param = (double)raw * 0.125 + 0;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetBrakeIsEnabled(int ¶m)
+{
+ GET_STATUS2();
+ param = rx->BrakeIsEnabled;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetEncPosition(int ¶m)
+{
+ GET_STATUS3();
+ int32_t raw = 0;
+ raw |= rx->EncPositionH;
+ raw <<= 8;
+ raw |= rx->EncPositionM;
+ raw <<= 8;
+ raw |= rx->EncPositionL;
+ raw <<= (32-24); /* sign extend */
+ raw >>= (32-24); /* sign extend */
+ if(rx->PosDiv8)
+ raw *= 8;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetEncVel(int ¶m)
+{
+ GET_STATUS3();
+ int32_t raw = 0;
+ raw |= rx->EncVelH;
+ raw <<= 8;
+ raw |= rx->EncVelL;
+ raw <<= (32-16); /* sign extend */
+ raw >>= (32-16); /* sign extend */
+ if(rx->VelDiv4)
+ raw *= 4;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetEncIndexRiseEvents(int ¶m)
+{
+ GET_STATUS3();
+ uint32_t raw = 0;
+ raw |= rx->EncIndexRiseEventsH;
+ raw <<= 8;
+ raw |= rx->EncIndexRiseEventsL;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetQuadApin(int ¶m)
+{
+ GET_STATUS3();
+ param = rx->QuadApin;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetQuadBpin(int ¶m)
+{
+ GET_STATUS3();
+ param = rx->QuadBpin;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetQuadIdxpin(int ¶m)
+{
+ GET_STATUS3();
+ param = rx->QuadIdxpin;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetAnalogInWithOv(int ¶m)
+{
+ GET_STATUS4();
+ int32_t raw = 0;
+ raw |= rx->AnalogInWithOvH;
+ raw <<= 8;
+ raw |= rx->AnalogInWithOvM;
+ raw <<= 8;
+ raw |= rx->AnalogInWithOvL;
+ raw <<= (32-24); /* sign extend */
+ raw >>= (32-24); /* sign extend */
+ if(rx->PosDiv8)
+ raw *= 8;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetAnalogInVel(int ¶m)
+{
+ GET_STATUS4();
+ int32_t raw = 0;
+ raw |= rx->AnalogInVelH;
+ raw <<= 8;
+ raw |= rx->AnalogInVelL;
+ raw <<= (32-16); /* sign extend */
+ raw >>= (32-16); /* sign extend */
+ if(rx->VelDiv4)
+ raw *= 4;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetTemp(double ¶m)
+{
+ GET_STATUS4();
+ uint32_t raw = rx->Temp;
+ param = (double)raw * 0.6451612903 + -50;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetBatteryV(double ¶m)
+{
+ GET_STATUS4();
+ uint32_t raw = rx->BatteryV;
+ param = (double)raw * 0.05 + 4;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetResetCount(int ¶m)
+{
+ GET_STATUS5();
+ uint32_t raw = 0;
+ raw |= rx->ResetCountH;
+ raw <<= 8;
+ raw |= rx->ResetCountL;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetResetFlags(int ¶m)
+{
+ GET_STATUS5();
+ uint32_t raw = 0;
+ raw |= rx->ResetFlagsH;
+ raw <<= 8;
+ raw |= rx->ResetFlagsL;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetFirmVers(int ¶m)
+{
+ GET_STATUS5();
+ uint32_t raw = 0;
+ raw |= rx->FirmVersH;
+ raw <<= 8;
+ raw |= rx->FirmVersL;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::SetDemand(int param)
+{
+ CtreCanNode::txTask<TALON_Control_1_General_10ms_t> toFill = GetTx<TALON_Control_1_General_10ms_t>(CONTROL_1 | GetDeviceNumber());
+ if (toFill.IsEmpty()) return CTR_UnexpectedArbId;
+ toFill->DemandH = param>>16;
+ toFill->DemandM = param>>8;
+ toFill->DemandL = param>>0;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+CTR_Code CanTalonSRX::SetOverrideLimitSwitchEn(int param)
+{
+ CtreCanNode::txTask<TALON_Control_1_General_10ms_t> toFill = GetTx<TALON_Control_1_General_10ms_t>(CONTROL_1 | GetDeviceNumber());
+ if (toFill.IsEmpty()) return CTR_UnexpectedArbId;
+ toFill->OverrideLimitSwitchEn = param;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+CTR_Code CanTalonSRX::SetFeedbackDeviceSelect(int param)
+{
+ CtreCanNode::txTask<TALON_Control_1_General_10ms_t> toFill = GetTx<TALON_Control_1_General_10ms_t>(CONTROL_1 | GetDeviceNumber());
+ if (toFill.IsEmpty()) return CTR_UnexpectedArbId;
+ toFill->FeedbackDeviceSelect = param;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+CTR_Code CanTalonSRX::SetRevMotDuringCloseLoopEn(int param)
+{
+ CtreCanNode::txTask<TALON_Control_1_General_10ms_t> toFill = GetTx<TALON_Control_1_General_10ms_t>(CONTROL_1 | GetDeviceNumber());
+ if (toFill.IsEmpty()) return CTR_UnexpectedArbId;
+ toFill->RevMotDuringCloseLoopEn = param;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+CTR_Code CanTalonSRX::SetOverrideBrakeType(int param)
+{
+ CtreCanNode::txTask<TALON_Control_1_General_10ms_t> toFill = GetTx<TALON_Control_1_General_10ms_t>(CONTROL_1 | GetDeviceNumber());
+ if (toFill.IsEmpty()) return CTR_UnexpectedArbId;
+ toFill->OverrideBrakeType = param;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+CTR_Code CanTalonSRX::SetModeSelect(int param)
+{
+ CtreCanNode::txTask<TALON_Control_1_General_10ms_t> toFill = GetTx<TALON_Control_1_General_10ms_t>(CONTROL_1 | GetDeviceNumber());
+ if (toFill.IsEmpty()) return CTR_UnexpectedArbId;
+ toFill->ModeSelect = param;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+/**
+ * @param modeSelect selects which mode.
+ * @param demand setpt or throttle or masterId to follow.
+ * @return error code, 0 iff successful.
+ * This function has the advantage of atomically setting mode and demand.
+ */
+CTR_Code CanTalonSRX::SetModeSelect(int modeSelect,int demand)
+{
+ CtreCanNode::txTask<TALON_Control_1_General_10ms_t> toFill = GetTx<TALON_Control_1_General_10ms_t>(CONTROL_1 | GetDeviceNumber());
+ if (toFill.IsEmpty()) return CTR_UnexpectedArbId;
+ toFill->ModeSelect = modeSelect;
+ toFill->DemandH = demand>>16;
+ toFill->DemandM = demand>>8;
+ toFill->DemandL = demand>>0;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+CTR_Code CanTalonSRX::SetProfileSlotSelect(int param)
+{
+ CtreCanNode::txTask<TALON_Control_1_General_10ms_t> toFill = GetTx<TALON_Control_1_General_10ms_t>(CONTROL_1 | GetDeviceNumber());
+ if (toFill.IsEmpty()) return CTR_UnexpectedArbId;
+ toFill->ProfileSlotSelect = param;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+CTR_Code CanTalonSRX::SetRampThrottle(int param)
+{
+ CtreCanNode::txTask<TALON_Control_1_General_10ms_t> toFill = GetTx<TALON_Control_1_General_10ms_t>(CONTROL_1 | GetDeviceNumber());
+ if (toFill.IsEmpty()) return CTR_UnexpectedArbId;
+ toFill->RampThrottle = param;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+CTR_Code CanTalonSRX::SetRevFeedbackSensor(int param)
+{
+ CtreCanNode::txTask<TALON_Control_1_General_10ms_t> toFill = GetTx<TALON_Control_1_General_10ms_t>(CONTROL_1 | GetDeviceNumber());
+ if (toFill.IsEmpty()) return CTR_UnexpectedArbId;
+ toFill->RevFeedbackSensor = param ? 1 : 0;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+CTR_Code CanTalonSRX::GetPulseWidthPosition(int ¶m)
+{
+ GET_STATUS8();
+ int32_t raw = 0;
+ raw |= rx->PulseWidPositionH;
+ raw <<= 8;
+ raw |= rx->PulseWidPositionM;
+ raw <<= 8;
+ raw |= rx->PulseWidPositionL;
+ raw <<= (32-24); /* sign extend */
+ raw >>= (32-24); /* sign extend */
+ if(rx->PosDiv8)
+ raw *= 8;
+ param = (int)raw;
+ return rx.err;
+}
+CTR_Code CanTalonSRX::GetPulseWidthVelocity(int ¶m)
+{
+ GET_STATUS8();
+ int32_t raw = 0;
+ raw |= rx->PulseWidVelH;
+ raw <<= 8;
+ raw |= rx->PulseWidVelL;
+ raw <<= (32-16); /* sign extend */
+ raw >>= (32-16); /* sign extend */
+ if(rx->VelDiv4)
+ raw *= 4;
+ param = (int)raw;
+ return rx.err;
+}
+/**
+ * @param param [out] Rise to rise timeperiod in microseconds.
+ */
+CTR_Code CanTalonSRX::GetPulseWidthRiseToRiseUs(int ¶m)
+{
+ GET_STATUS8();
+ uint32_t raw = 0;
+ raw |= rx->PeriodUsM8;
+ raw <<= 8;
+ raw |= rx->PeriodUsL8;
+ param = (int)raw;
+ return rx.err;
+}
+/**
+ * @param param [out] Rise to fall time period in microseconds.
+ */
+CTR_Code CanTalonSRX::GetPulseWidthRiseToFallUs(int ¶m)
+{
+ int temp = 0;
+ int periodUs = 0;
+ /* first grab our 12.12 position */
+ CTR_Code retval1 = GetPulseWidthPosition(temp);
+ /* mask off number of turns */
+ temp &= 0xFFF;
+ /* next grab the waveform period. This value
+ * will be zero if we stop getting pulses **/
+ CTR_Code retval2 = GetPulseWidthRiseToRiseUs(periodUs);
+ /* now we have 0.12 position that is scaled to the waveform period.
+ Use fixed pt multiply to scale our 0.16 period into us.*/
+ param = (temp * periodUs) / BIT12;
+ /* pass the worst error code to caller.
+ Assume largest value is the most pressing error code.*/
+ return (CTR_Code)std::max((int)retval1, (int)retval2);
+}
+CTR_Code CanTalonSRX::IsPulseWidthSensorPresent(int ¶m)
+{
+ int periodUs = 0;
+ CTR_Code retval = GetPulseWidthRiseToRiseUs(periodUs);
+ /* if a nonzero period is present, we are getting good pules.
+ Otherwise the sensor is not present. */
+ if(periodUs != 0)
+ param = 1;
+ else
+ param = 0;
+ return retval;
+}
+//------------------ C interface --------------------------------------------//
+extern "C" {
+void *c_TalonSRX_Create(int deviceNumber, int controlPeriodMs)
+{
+ return new CanTalonSRX(deviceNumber, controlPeriodMs);
+}
+void c_TalonSRX_Destroy(void *handle)
+{
+ delete (CanTalonSRX*)handle;
+}
+CTR_Code c_TalonSRX_SetParam(void *handle, int paramEnum, double value)
+{
+ return ((CanTalonSRX*)handle)->SetParam((CanTalonSRX::param_t)paramEnum, value);
+}
+CTR_Code c_TalonSRX_RequestParam(void *handle, int paramEnum)
+{
+ return ((CanTalonSRX*)handle)->RequestParam((CanTalonSRX::param_t)paramEnum);
+}
+CTR_Code c_TalonSRX_GetParamResponse(void *handle, int paramEnum, double *value)
+{
+ return ((CanTalonSRX*)handle)->GetParamResponse((CanTalonSRX::param_t)paramEnum, *value);
+}
+CTR_Code c_TalonSRX_GetParamResponseInt32(void *handle, int paramEnum, int *value)
+{
+ return ((CanTalonSRX*)handle)->GetParamResponseInt32((CanTalonSRX::param_t)paramEnum, *value);
+}
+CTR_Code c_TalonSRX_SetStatusFrameRate(void *handle, unsigned frameEnum, unsigned periodMs)
+{
+ return ((CanTalonSRX*)handle)->SetStatusFrameRate(frameEnum, periodMs);
+}
+CTR_Code c_TalonSRX_ClearStickyFaults(void *handle)
+{
+ return ((CanTalonSRX*)handle)->ClearStickyFaults();
+}
+CTR_Code c_TalonSRX_GetFault_OverTemp(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetFault_OverTemp(*param);
+}
+CTR_Code c_TalonSRX_GetFault_UnderVoltage(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetFault_UnderVoltage(*param);
+}
+CTR_Code c_TalonSRX_GetFault_ForLim(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetFault_ForLim(*param);
+}
+CTR_Code c_TalonSRX_GetFault_RevLim(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetFault_RevLim(*param);
+}
+CTR_Code c_TalonSRX_GetFault_HardwareFailure(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetFault_HardwareFailure(*param);
+}
+CTR_Code c_TalonSRX_GetFault_ForSoftLim(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetFault_ForSoftLim(*param);
+}
+CTR_Code c_TalonSRX_GetFault_RevSoftLim(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetFault_RevSoftLim(*param);
+}
+CTR_Code c_TalonSRX_GetStckyFault_OverTemp(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetStckyFault_OverTemp(*param);
+}
+CTR_Code c_TalonSRX_GetStckyFault_UnderVoltage(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetStckyFault_UnderVoltage(*param);
+}
+CTR_Code c_TalonSRX_GetStckyFault_ForLim(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetStckyFault_ForLim(*param);
+}
+CTR_Code c_TalonSRX_GetStckyFault_RevLim(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetStckyFault_RevLim(*param);
+}
+CTR_Code c_TalonSRX_GetStckyFault_ForSoftLim(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetStckyFault_ForSoftLim(*param);
+}
+CTR_Code c_TalonSRX_GetStckyFault_RevSoftLim(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetStckyFault_RevSoftLim(*param);
+}
+CTR_Code c_TalonSRX_GetAppliedThrottle(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetAppliedThrottle(*param);
+}
+CTR_Code c_TalonSRX_GetCloseLoopErr(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetCloseLoopErr(*param);
+}
+CTR_Code c_TalonSRX_GetFeedbackDeviceSelect(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetFeedbackDeviceSelect(*param);
+}
+CTR_Code c_TalonSRX_GetModeSelect(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetModeSelect(*param);
+}
+CTR_Code c_TalonSRX_GetLimitSwitchEn(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetLimitSwitchEn(*param);
+}
+CTR_Code c_TalonSRX_GetLimitSwitchClosedFor(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetLimitSwitchClosedFor(*param);
+}
+CTR_Code c_TalonSRX_GetLimitSwitchClosedRev(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetLimitSwitchClosedRev(*param);
+}
+CTR_Code c_TalonSRX_GetSensorPosition(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetSensorPosition(*param);
+}
+CTR_Code c_TalonSRX_GetSensorVelocity(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetSensorVelocity(*param);
+}
+CTR_Code c_TalonSRX_GetCurrent(void *handle, double *param)
+{
+ return ((CanTalonSRX*)handle)->GetCurrent(*param);
+}
+CTR_Code c_TalonSRX_GetBrakeIsEnabled(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetBrakeIsEnabled(*param);
+}
+CTR_Code c_TalonSRX_GetEncPosition(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetEncPosition(*param);
+}
+CTR_Code c_TalonSRX_GetEncVel(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetEncVel(*param);
+}
+CTR_Code c_TalonSRX_GetEncIndexRiseEvents(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetEncIndexRiseEvents(*param);
+}
+CTR_Code c_TalonSRX_GetQuadApin(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetQuadApin(*param);
+}
+CTR_Code c_TalonSRX_GetQuadBpin(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetQuadBpin(*param);
+}
+CTR_Code c_TalonSRX_GetQuadIdxpin(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetQuadIdxpin(*param);
+}
+CTR_Code c_TalonSRX_GetAnalogInWithOv(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetAnalogInWithOv(*param);
+}
+CTR_Code c_TalonSRX_GetAnalogInVel(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetAnalogInVel(*param);
+}
+CTR_Code c_TalonSRX_GetTemp(void *handle, double *param)
+{
+ return ((CanTalonSRX*)handle)->GetTemp(*param);
+}
+CTR_Code c_TalonSRX_GetBatteryV(void *handle, double *param)
+{
+ return ((CanTalonSRX*)handle)->GetBatteryV(*param);
+}
+CTR_Code c_TalonSRX_GetResetCount(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetResetCount(*param);
+}
+CTR_Code c_TalonSRX_GetResetFlags(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetResetFlags(*param);
+}
+CTR_Code c_TalonSRX_GetFirmVers(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetFirmVers(*param);
+}
+CTR_Code c_TalonSRX_SetDemand(void *handle, int param)
+{
+ return ((CanTalonSRX*)handle)->SetDemand(param);
+}
+CTR_Code c_TalonSRX_SetOverrideLimitSwitchEn(void *handle, int param)
+{
+ return ((CanTalonSRX*)handle)->SetOverrideLimitSwitchEn(param);
+}
+CTR_Code c_TalonSRX_SetFeedbackDeviceSelect(void *handle, int param)
+{
+ return ((CanTalonSRX*)handle)->SetFeedbackDeviceSelect(param);
+}
+CTR_Code c_TalonSRX_SetRevMotDuringCloseLoopEn(void *handle, int param)
+{
+ return ((CanTalonSRX*)handle)->SetRevMotDuringCloseLoopEn(param);
+}
+CTR_Code c_TalonSRX_SetOverrideBrakeType(void *handle, int param)
+{
+ return ((CanTalonSRX*)handle)->SetOverrideBrakeType(param);
+}
+CTR_Code c_TalonSRX_SetModeSelect(void *handle, int param)
+{
+ return ((CanTalonSRX*)handle)->SetModeSelect(param);
+}
+CTR_Code c_TalonSRX_SetModeSelect2(void *handle, int modeSelect, int demand)
+{
+ return ((CanTalonSRX*)handle)->SetModeSelect(modeSelect, demand);
+}
+CTR_Code c_TalonSRX_SetProfileSlotSelect(void *handle, int param)
+{
+ return ((CanTalonSRX*)handle)->SetProfileSlotSelect(param);
+}
+CTR_Code c_TalonSRX_SetRampThrottle(void *handle, int param)
+{
+ return ((CanTalonSRX*)handle)->SetRampThrottle(param);
+}
+CTR_Code c_TalonSRX_SetRevFeedbackSensor(void *handle, int param)
+{
+ return ((CanTalonSRX*)handle)->SetRevFeedbackSensor(param);
+}
+CTR_Code c_TalonSRX_GetPulseWidthPosition(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetPulseWidthPosition(*param);
+}
+CTR_Code c_TalonSRX_GetPulseWidthVelocity(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetPulseWidthVelocity(*param);
+}
+CTR_Code c_TalonSRX_GetPulseWidthRiseToFallUs(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetPulseWidthRiseToFallUs(*param);
+}
+CTR_Code c_TalonSRX_GetPulseWidthRiseToRiseUs(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->GetPulseWidthRiseToRiseUs(*param);
+}
+CTR_Code c_TalonSRX_IsPulseWidthSensorPresent(void *handle, int *param)
+{
+ return ((CanTalonSRX*)handle)->IsPulseWidthSensorPresent(*param);
+}
+}
diff --git a/hal/lib/Athena/ctre/CtreCanNode.cpp b/hal/lib/Athena/ctre/CtreCanNode.cpp
new file mode 100644
index 0000000..18cd24b
--- /dev/null
+++ b/hal/lib/Athena/ctre/CtreCanNode.cpp
@@ -0,0 +1,101 @@
+#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
+
+#include "ctre/CtreCanNode.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+#include <string.h> // memset
+#include <unistd.h> // usleep
+
+static const UINT32 kFullMessageIDMask = 0x1fffffff;
+
+CtreCanNode::CtreCanNode(UINT8 deviceNumber)
+{
+ _deviceNumber = deviceNumber;
+}
+CtreCanNode::~CtreCanNode()
+{
+}
+void CtreCanNode::RegisterRx(uint32_t arbId)
+{
+ /* no need to do anything, we just use new API to poll last received message */
+}
+void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs)
+{
+ int32_t status = 0;
+
+ txJob_t job = {0};
+ job.arbId = arbId;
+ job.periodMs = periodMs;
+ _txJobs[arbId] = job;
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( job.arbId,
+ job.toSend,
+ 8,
+ job.periodMs,
+ &status);
+}
+timespec diff(const timespec & start, const timespec & end)
+{
+ timespec temp;
+ if ((end.tv_nsec-start.tv_nsec)<0) {
+ temp.tv_sec = end.tv_sec-start.tv_sec-1;
+ temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;
+ } else {
+ temp.tv_sec = end.tv_sec-start.tv_sec;
+ temp.tv_nsec = end.tv_nsec-start.tv_nsec;
+ }
+ return temp;
+}
+CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeoutMs)
+{
+ CTR_Code retval = CTR_OKAY;
+ int32_t status = 0;
+ uint8_t len = 0;
+ uint32_t timeStamp;
+ /* cap timeout at 999ms */
+ if(timeoutMs > 999)
+ timeoutMs = 999;
+ FRC_NetworkCommunication_CANSessionMux_receiveMessage(&arbId,kFullMessageIDMask,dataBytes,&len,&timeStamp,&status);
+ if(status == 0){
+ /* fresh update */
+ rxEvent_t & r = _rxRxEvents[arbId]; /* lookup entry or make a default new one with all zeroes */
+ clock_gettime(2,&r.time); /* fill in time */
+ memcpy(r.bytes, dataBytes, 8); /* fill in databytes */
+ }else{
+ /* did not get the message */
+ rxRxEvents_t::iterator i = _rxRxEvents.find(arbId);
+ if(i == _rxRxEvents.end()){
+ /* we've never gotten this mesage */
+ retval = CTR_RxTimeout;
+ /* fill caller's buffer with zeros */
+ memset(dataBytes,0,8);
+ }else{
+ /* we've gotten this message before but not recently */
+ memcpy(dataBytes,i->second.bytes,8);
+ /* get the time now */
+ struct timespec temp;
+ clock_gettime(2,&temp); /* get now */
+ /* how long has it been? */
+ temp = diff(i->second.time,temp); /* temp = now - last */
+ if(temp.tv_sec > 0){
+ retval = CTR_RxTimeout;
+ }else if(temp.tv_nsec > ((int32_t)timeoutMs*1000*1000)){
+ retval = CTR_RxTimeout;
+ }else {
+ /* our last update was recent enough */
+ }
+ }
+ }
+
+ return retval;
+}
+void CtreCanNode::FlushTx(uint32_t arbId)
+{
+ int32_t status = 0;
+ txJobs_t::iterator iter = _txJobs.find(arbId);
+ if(iter != _txJobs.end())
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId,
+ iter->second.toSend,
+ 8,
+ iter->second.periodMs,
+ &status);
+}
+
diff --git a/hal/lib/Athena/ctre/PCM.cpp b/hal/lib/Athena/ctre/PCM.cpp
new file mode 100644
index 0000000..58f8bc9
--- /dev/null
+++ b/hal/lib/Athena/ctre/PCM.cpp
@@ -0,0 +1,557 @@
+#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
+
+#include "ctre/PCM.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+#include <string.h> // memset
+#include <unistd.h> // usleep
+/* This can be a constant, as long as nobody needs to update solenoids within
+ 1/50 of a second. */
+static const INT32 kCANPeriod = 20;
+
+#define STATUS_1 0x9041400
+#define STATUS_SOL_FAULTS 0x9041440
+#define STATUS_DEBUG 0x9041480
+
+#define EXPECTED_RESPONSE_TIMEOUT_MS (50)
+#define GET_PCM_STATUS() CtreCanNode::recMsg<PcmStatus_t> rx = GetRx<PcmStatus_t> (STATUS_1|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_PCM_SOL_FAULTS() CtreCanNode::recMsg<PcmStatusFault_t> rx = GetRx<PcmStatusFault_t> (STATUS_SOL_FAULTS|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_PCM_DEBUG() CtreCanNode::recMsg<PcmDebug_t> rx = GetRx<PcmDebug_t> (STATUS_DEBUG|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+
+#define CONTROL_1 0x09041C00 /* PCM_Control */
+#define CONTROL_2 0x09041C40 /* PCM_SupplemControl */
+#define CONTROL_3 0x09041C80 /* PcmControlSetOneShotDur_t */
+
+/* encoder/decoders */
+typedef struct _PcmStatus_t{
+ /* Byte 0 */
+ unsigned SolenoidBits:8;
+ /* Byte 1 */
+ unsigned compressorOn:1;
+ unsigned stickyFaultFuseTripped:1;
+ unsigned stickyFaultCompCurrentTooHigh:1;
+ unsigned faultFuseTripped:1;
+ unsigned faultCompCurrentTooHigh:1;
+ unsigned faultHardwareFailure:1;
+ unsigned isCloseloopEnabled:1;
+ unsigned pressureSwitchEn:1;
+ /* Byte 2*/
+ unsigned battVoltage:8;
+ /* Byte 3 */
+ unsigned solenoidVoltageTop8:8;
+ /* Byte 4 */
+ unsigned compressorCurrentTop6:6;
+ unsigned solenoidVoltageBtm2:2;
+ /* Byte 5 */
+ unsigned StickyFault_dItooHigh :1;
+ unsigned Fault_dItooHigh :1;
+ unsigned moduleEnabled:1;
+ unsigned closedLoopOutput:1;
+ unsigned compressorCurrentBtm4:4;
+ /* Byte 6 */
+ unsigned tokenSeedTop8:8;
+ /* Byte 7 */
+ unsigned tokenSeedBtm8:8;
+}PcmStatus_t;
+
+typedef struct _PcmControl_t{
+ /* Byte 0 */
+ unsigned tokenTop8:8;
+ /* Byte 1 */
+ unsigned tokenBtm8:8;
+ /* Byte 2 */
+ unsigned solenoidBits:8;
+ /* Byte 3*/
+ unsigned reserved:4;
+ unsigned closeLoopOutput:1;
+ unsigned compressorOn:1;
+ unsigned closedLoopEnable:1;
+ unsigned clearStickyFaults:1;
+ /* Byte 4 */
+ unsigned OneShotField_h8:8;
+ /* Byte 5 */
+ unsigned OneShotField_l8:8;
+}PcmControl_t;
+
+typedef struct _PcmControlSetOneShotDur_t{
+ uint8_t sol10MsPerUnit[8];
+}PcmControlSetOneShotDur_t;
+
+typedef struct _PcmStatusFault_t{
+ /* Byte 0 */
+ unsigned SolenoidBlacklist:8;
+ /* Byte 1 */
+ unsigned reserved_bit0 :1;
+ unsigned reserved_bit1 :1;
+ unsigned reserved_bit2 :1;
+ unsigned reserved_bit3 :1;
+ unsigned StickyFault_CompNoCurrent :1;
+ unsigned Fault_CompNoCurrent :1;
+ unsigned StickyFault_SolenoidJumper :1;
+ unsigned Fault_SolenoidJumper :1;
+}PcmStatusFault_t;
+
+typedef struct _PcmDebug_t{
+ unsigned tokFailsTop8:8;
+ unsigned tokFailsBtm8:8;
+ unsigned lastFailedTokTop8:8;
+ unsigned lastFailedTokBtm8:8;
+ unsigned tokSuccessTop8:8;
+ unsigned tokSuccessBtm8:8;
+}PcmDebug_t;
+
+
+/* PCM Constructor - Clears all vars, establishes default settings, starts PCM background process
+ *
+ * @Return - void
+ *
+ * @Param - deviceNumber - Device ID of PCM to be controlled
+ */
+PCM::PCM(UINT8 deviceNumber): CtreCanNode(deviceNumber)
+{
+ RegisterRx(STATUS_1 | deviceNumber );
+ RegisterRx(STATUS_SOL_FAULTS | deviceNumber );
+ RegisterRx(STATUS_DEBUG | deviceNumber );
+ RegisterTx(CONTROL_1 | deviceNumber, kCANPeriod);
+ /* enable close loop */
+ SetClosedLoopControl(1);
+}
+/* PCM D'tor
+ */
+PCM::~PCM()
+{
+
+}
+
+/* Set PCM solenoid state
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ *
+ * @Param - idx - ID of solenoid (0-7)
+ * @Param - en - Enable / Disable identified solenoid
+ */
+CTR_Code PCM::SetSolenoid(unsigned char idx, bool en)
+{
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ if (en)
+ toFill->solenoidBits |= (1ul << (idx));
+ else
+ toFill->solenoidBits &= ~(1ul << (idx));
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+
+/* Clears PCM sticky faults (indicators of past faults
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ *
+ * @Param - clr - Clear / do not clear faults
+ */
+CTR_Code PCM::ClearStickyFaults()
+{
+ int32_t status = 0;
+ uint8_t pcmSupplemControl[] = { 0, 0, 0, 0x80 }; /* only bit set is ClearStickyFaults */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_2 | GetDeviceNumber(), pcmSupplemControl, sizeof(pcmSupplemControl), 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+
+/* Enables PCM Closed Loop Control of Compressor via pressure switch
+ *
+ * @Return - CTR_Code - Error code (if any) for setting solenoid
+ *
+ * @Param - en - Enable / Disable Closed Loop Control
+ */
+CTR_Code PCM::SetClosedLoopControl(bool en)
+{
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ toFill->closedLoopEnable = en;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+/* Get solenoid Blacklist status
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - idx - ID of solenoid [0,7] to fire one shot pulse.
+ */
+CTR_Code PCM::FireOneShotSolenoid(UINT8 idx)
+{
+ CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
+ if(toFill.IsEmpty())return CTR_UnexpectedArbId;
+ /* grab field as it is now */
+ uint16_t oneShotField;
+ oneShotField = toFill->OneShotField_h8;
+ oneShotField <<= 8;
+ oneShotField |= toFill->OneShotField_l8;
+ /* get the caller's channel */
+ uint16_t shift = 2*idx;
+ uint16_t mask = 3; /* two bits wide */
+ uint8_t chBits = (oneShotField >> shift) & mask;
+ /* flip it */
+ chBits = (chBits)%3 + 1;
+ /* clear out 2bits for this channel*/
+ oneShotField &= ~(mask << shift);
+ /* put new field in */
+ oneShotField |= chBits << shift;
+ /* apply field as it is now */
+ toFill->OneShotField_h8 = oneShotField >> 8;
+ toFill->OneShotField_l8 = oneShotField;
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+/* Configure the pulse width of a solenoid channel for one-shot pulse.
+ * Preprogrammed pulsewidth is 10ms resolute and can be between 20ms and 5.1s.
+ * @Return - CTR_Code - Error code (if any)
+ * @Param - idx - ID of solenoid [0,7] to configure.
+ * @Param - durMs - pulse width in ms.
+ */
+CTR_Code PCM::SetOneShotDurationMs(UINT8 idx,uint32_t durMs)
+{
+ /* sanity check caller's param */
+ if(idx > 7)
+ return CTR_InvalidParamValue;
+ /* get latest tx frame */
+ CtreCanNode::txTask<PcmControlSetOneShotDur_t> toFill = GetTx<PcmControlSetOneShotDur_t>(CONTROL_3 | GetDeviceNumber());
+ if(toFill.IsEmpty()){
+ /* only send this out if caller wants to do one-shots */
+ RegisterTx(CONTROL_3 | _deviceNumber, kCANPeriod);
+ /* grab it */
+ toFill = GetTx<PcmControlSetOneShotDur_t>(CONTROL_3 | GetDeviceNumber());
+ }
+ toFill->sol10MsPerUnit[idx] = std::min(durMs/10,(uint32_t)0xFF);
+ /* apply the new data bytes */
+ FlushTx(toFill);
+ return CTR_OKAY;
+}
+
+/* Get solenoid state
+ *
+ * @Return - True/False - True if solenoid enabled, false otherwise
+ *
+ * @Param - idx - ID of solenoid (0-7) to return status of
+ */
+CTR_Code PCM::GetSolenoid(UINT8 idx, bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->SolenoidBits & (1ul<<(idx)) ) ? 1 : 0;
+ return rx.err;
+}
+
+/* Get solenoid state for all solenoids on the PCM
+ *
+ * @Return - Bitfield of solenoid states
+ */
+CTR_Code PCM::GetAllSolenoids(UINT8 &status)
+{
+ GET_PCM_STATUS();
+ status = rx->SolenoidBits;
+ return rx.err;
+}
+
+/* Get pressure switch state
+ *
+ * @Return - True/False - True if pressure adequate, false if low
+ */
+CTR_Code PCM::GetPressure(bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->pressureSwitchEn ) ? 1 : 0;
+ return rx.err;
+}
+
+/* Get compressor state
+ *
+ * @Return - True/False - True if enabled, false if otherwise
+ */
+CTR_Code PCM::GetCompressor(bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->compressorOn);
+ return rx.err;
+}
+
+/* Get closed loop control state
+ *
+ * @Return - True/False - True if closed loop enabled, false if otherwise
+ */
+CTR_Code PCM::GetClosedLoopControl(bool &status)
+{
+ GET_PCM_STATUS();
+ status = (rx->isCloseloopEnabled);
+ return rx.err;
+}
+
+/* Get compressor current draw
+ *
+ * @Return - Amperes - Compressor current
+ */
+CTR_Code PCM::GetCompressorCurrent(float &status)
+{
+ GET_PCM_STATUS();
+ uint32_t temp =(rx->compressorCurrentTop6);
+ temp <<= 4;
+ temp |= rx->compressorCurrentBtm4;
+ status = temp * 0.03125; /* 5.5 fixed pt value in Amps */
+ return rx.err;
+}
+
+/* Get voltage across solenoid rail
+ *
+ * @Return - Volts - Voltage across solenoid rail
+ */
+CTR_Code PCM::GetSolenoidVoltage(float &status)
+{
+ GET_PCM_STATUS();
+ uint32_t raw =(rx->solenoidVoltageTop8);
+ raw <<= 2;
+ raw |= rx->solenoidVoltageBtm2;
+ status = (double) raw * 0.03125; /* 5.5 fixed pt value in Volts */
+ return rx.err;
+}
+
+/* Get hardware fault value
+ *
+ * @Return - True/False - True if hardware failure detected, false if otherwise
+ */
+CTR_Code PCM::GetHardwareFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->faultHardwareFailure;
+ return rx.err;
+}
+
+/* Get compressor fault value
+ *
+ * @Return - True/False - True if shorted compressor detected, false if otherwise
+ */
+CTR_Code PCM::GetCompressorCurrentTooHighFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->faultCompCurrentTooHigh;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorShortedStickyFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->StickyFault_dItooHigh;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorShortedFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->Fault_dItooHigh;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorNotConnectedStickyFault(bool &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = rx->StickyFault_CompNoCurrent;
+ return rx.err;
+}
+CTR_Code PCM::GetCompressorNotConnectedFault(bool &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = rx->Fault_CompNoCurrent;
+ return rx.err;
+}
+
+/* Get solenoid fault value
+ *
+ * @Return - True/False - True if shorted solenoid detected, false if otherwise
+ */
+CTR_Code PCM::GetSolenoidFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->faultFuseTripped;
+ return rx.err;
+}
+
+/* Get compressor sticky fault value
+ *
+ * @Return - True/False - True if solenoid had previously been shorted
+ * (and sticky fault was not cleared), false if otherwise
+ */
+CTR_Code PCM::GetCompressorCurrentTooHighStickyFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->stickyFaultCompCurrentTooHigh;
+ return rx.err;
+}
+
+/* Get solenoid sticky fault value
+ *
+ * @Return - True/False - True if compressor had previously been shorted
+ * (and sticky fault was not cleared), false if otherwise
+ */
+CTR_Code PCM::GetSolenoidStickyFault(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->stickyFaultFuseTripped;
+ return rx.err;
+}
+/* Get battery voltage
+ *
+ * @Return - Volts - Voltage across PCM power ports
+ */
+CTR_Code PCM::GetBatteryVoltage(float &status)
+{
+ GET_PCM_STATUS();
+ status = (float)rx->battVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
+ return rx.err;
+}
+/* Return status of module enable/disable
+ *
+ * @Return - bool - Returns TRUE if PCM is enabled, FALSE if disabled
+ */
+CTR_Code PCM::isModuleEnabled(bool &status)
+{
+ GET_PCM_STATUS();
+ status = rx->moduleEnabled;
+ return rx.err;
+}
+/* Get number of total failed PCM Control Frame
+ *
+ * @Return - Failed Control Frames - Number of failed control frames (tokenization fails)
+ *
+ * @WARNING - Return only valid if [SeekDebugFrames] is enabled
+ * See function SeekDebugFrames
+ * See function EnableSeekDebugFrames
+ */
+CTR_Code PCM::GetNumberOfFailedControlFrames(UINT16 &status)
+{
+ GET_PCM_DEBUG();
+ status = rx->tokFailsTop8;
+ status <<= 8;
+ status |= rx->tokFailsBtm8;
+ return rx.err;
+}
+/* Get raw Solenoid Blacklist
+ *
+ * @Return - BINARY - Raw binary breakdown of Solenoid Blacklist
+ * BIT7 = Solenoid 1, BIT6 = Solenoid 2, etc.
+ *
+ * @WARNING - Return only valid if [SeekStatusFaultFrames] is enabled
+ * See function SeekStatusFaultFrames
+ * See function EnableSeekStatusFaultFrames
+ */
+CTR_Code PCM::GetSolenoidBlackList(UINT8 &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = rx->SolenoidBlacklist;
+ return rx.err;
+}
+/* Get solenoid Blacklist status
+ * - Blacklisted solenoids cannot be enabled until PCM is power cycled
+ *
+ * @Return - True/False - True if Solenoid is blacklisted, false if otherwise
+ *
+ * @Param - idx - ID of solenoid [0,7]
+ *
+ * @WARNING - Return only valid if [SeekStatusFaultFrames] is enabled
+ * See function SeekStatusFaultFrames
+ * See function EnableSeekStatusFaultFrames
+ */
+CTR_Code PCM::IsSolenoidBlacklisted(UINT8 idx, bool &status)
+{
+ GET_PCM_SOL_FAULTS();
+ status = (rx->SolenoidBlacklist & (1ul<<(idx)) )? 1 : 0;
+ return rx.err;
+}
+//------------------ C interface --------------------------------------------//
+extern "C" {
+ void * c_PCM_Init(void) {
+ return new PCM();
+ }
+ CTR_Code c_SetSolenoid(void * handle, unsigned char idx, INT8 param) {
+ return ((PCM*) handle)->SetSolenoid(idx, param);
+ }
+ CTR_Code c_SetClosedLoopControl(void * handle, INT8 param) {
+ return ((PCM*) handle)->SetClosedLoopControl(param);
+ }
+ CTR_Code c_ClearStickyFaults(void * handle, INT8 param) {
+ return ((PCM*) handle)->ClearStickyFaults();
+ }
+ CTR_Code c_GetSolenoid(void * handle, UINT8 idx, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetSolenoid(idx, bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetAllSolenoids(void * handle, UINT8 * status) {
+ return ((PCM*) handle)->GetAllSolenoids(*status);
+ }
+ CTR_Code c_GetPressure(void * handle, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetPressure(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressor(void * handle, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetCompressor(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetClosedLoopControl(void * handle, INT8 * status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetClosedLoopControl(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressorCurrent(void * handle, float * status) {
+ CTR_Code retval = ((PCM*) handle)->GetCompressorCurrent(*status);
+ return retval;
+ }
+ CTR_Code c_GetSolenoidVoltage(void * handle, float*status) {
+ return ((PCM*) handle)->GetSolenoidVoltage(*status);
+ }
+ CTR_Code c_GetHardwareFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetHardwareFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressorFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetCompressorCurrentTooHighFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetSolenoidFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetSolenoidFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetCompressorStickyFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetCompressorCurrentTooHighStickyFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetSolenoidStickyFault(void * handle, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->GetSolenoidStickyFault(bstatus);
+ *status = bstatus;
+ return retval;
+ }
+ CTR_Code c_GetBatteryVoltage(void * handle, float*status) {
+ CTR_Code retval = ((PCM*) handle)->GetBatteryVoltage(*status);
+ return retval;
+ }
+ void c_SetDeviceNumber_PCM(void * handle, UINT8 deviceNumber) {
+ }
+ CTR_Code c_GetNumberOfFailedControlFrames(void * handle, UINT16*status) {
+ return ((PCM*) handle)->GetNumberOfFailedControlFrames(*status);
+ }
+ CTR_Code c_GetSolenoidBlackList(void * handle, UINT8 *status) {
+ return ((PCM*) handle)->GetSolenoidBlackList(*status);
+ }
+ CTR_Code c_IsSolenoidBlacklisted(void * handle, UINT8 idx, INT8*status) {
+ bool bstatus;
+ CTR_Code retval = ((PCM*) handle)->IsSolenoidBlacklisted(idx, bstatus);
+ *status = bstatus;
+ return retval;
+ }
+}
diff --git a/hal/lib/Athena/ctre/PDP.cpp b/hal/lib/Athena/ctre/PDP.cpp
new file mode 100644
index 0000000..cae2264
--- /dev/null
+++ b/hal/lib/Athena/ctre/PDP.cpp
@@ -0,0 +1,231 @@
+#include "ctre/PDP.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h" //CAN Comm
+#include <string.h> // memset
+#include <unistd.h> // usleep
+
+#define STATUS_1 0x8041400
+#define STATUS_2 0x8041440
+#define STATUS_3 0x8041480
+#define STATUS_ENERGY 0x8041740
+
+#define CONTROL_1 0x08041C00 /* PDP_Control_ClearStats */
+
+#define EXPECTED_RESPONSE_TIMEOUT_MS (50)
+#define GET_STATUS1() CtreCanNode::recMsg<PdpStatus1_t> rx = GetRx<PdpStatus1_t>(STATUS_1,EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS2() CtreCanNode::recMsg<PdpStatus2_t> rx = GetRx<PdpStatus2_t>(STATUS_2,EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS3() CtreCanNode::recMsg<PdpStatus3_t> rx = GetRx<PdpStatus3_t>(STATUS_3,EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS_ENERGY() CtreCanNode::recMsg<PDP_Status_Energy_t> rx = GetRx<PDP_Status_Energy_t>(STATUS_ENERGY,EXPECTED_RESPONSE_TIMEOUT_MS)
+
+/* encoder/decoders */
+typedef struct _PdpStatus1_t{
+ unsigned chan1_h8:8;
+ unsigned chan2_h6:6;
+ unsigned chan1_l2:2;
+ unsigned chan3_h4:4;
+ unsigned chan2_l4:4;
+ unsigned chan4_h2:2;
+ unsigned chan3_l6:6;
+ unsigned chan4_l8:8;
+ unsigned chan5_h8:8;
+ unsigned chan6_h6:6;
+ unsigned chan5_l2:2;
+ unsigned reserved4:4;
+ unsigned chan6_l4:4;
+}PdpStatus1_t;
+typedef struct _PdpStatus2_t{
+ unsigned chan7_h8:8;
+ unsigned chan8_h6:6;
+ unsigned chan7_l2:2;
+ unsigned chan9_h4:4;
+ unsigned chan8_l4:4;
+ unsigned chan10_h2:2;
+ unsigned chan9_l6:6;
+ unsigned chan10_l8:8;
+ unsigned chan11_h8:8;
+ unsigned chan12_h6:6;
+ unsigned chan11_l2:2;
+ unsigned reserved4:4;
+ unsigned chan12_l4:4;
+}PdpStatus2_t;
+typedef struct _PdpStatus3_t{
+ unsigned chan13_h8:8;
+ unsigned chan14_h6:6;
+ unsigned chan13_l2:2;
+ unsigned chan15_h4:4;
+ unsigned chan14_l4:4;
+ unsigned chan16_h2:2;
+ unsigned chan15_l6:6;
+ unsigned chan16_l8:8;
+ unsigned internalResBattery_mOhms:8;
+ unsigned busVoltage:8;
+ unsigned temp:8;
+}PdpStatus3_t;
+typedef struct _PDP_Status_Energy_t {
+ unsigned TmeasMs_likelywillbe20ms_:8;
+ unsigned TotalCurrent_125mAperunit_h8:8;
+ unsigned Power_125mWperunit_h4:4;
+ unsigned TotalCurrent_125mAperunit_l4:4;
+ unsigned Power_125mWperunit_m8:8;
+ unsigned Energy_125mWPerUnitXTmeas_h4:4;
+ unsigned Power_125mWperunit_l4:4;
+ unsigned Energy_125mWPerUnitXTmeas_mh8:8;
+ unsigned Energy_125mWPerUnitXTmeas_ml8:8;
+ unsigned Energy_125mWPerUnitXTmeas_l8:8;
+} PDP_Status_Energy_t ;
+
+PDP::PDP(UINT8 deviceNumber): CtreCanNode(deviceNumber)
+{
+ RegisterRx(STATUS_1 | deviceNumber );
+ RegisterRx(STATUS_2 | deviceNumber );
+ RegisterRx(STATUS_3 | deviceNumber );
+}
+/* PDP D'tor
+ */
+PDP::~PDP()
+{
+}
+
+CTR_Code PDP::GetChannelCurrent(UINT8 idx, double ¤t)
+{
+ CTR_Code retval = CTR_InvalidParamValue;
+ uint32_t raw = 0;
+
+ if(idx <= 5){
+ GET_STATUS1();
+ retval = rx.err;
+ switch(idx){
+ case 0: raw = ((uint32_t)rx->chan1_h8 << 2) | rx->chan1_l2; break;
+ case 1: raw = ((uint32_t)rx->chan2_h6 << 4) | rx->chan2_l4; break;
+ case 2: raw = ((uint32_t)rx->chan3_h4 << 6) | rx->chan3_l6; break;
+ case 3: raw = ((uint32_t)rx->chan4_h2 << 8) | rx->chan4_l8; break;
+ case 4: raw = ((uint32_t)rx->chan5_h8 << 2) | rx->chan5_l2; break;
+ case 5: raw = ((uint32_t)rx->chan6_h6 << 4) | rx->chan6_l4; break;
+ default: retval = CTR_InvalidParamValue; break;
+ }
+ }else if(idx <= 11){
+ GET_STATUS2();
+ retval = rx.err;
+ switch(idx){
+ case 6: raw = ((uint32_t)rx->chan7_h8 << 2) | rx->chan7_l2; break;
+ case 7: raw = ((uint32_t)rx->chan8_h6 << 4) | rx->chan8_l4; break;
+ case 8: raw = ((uint32_t)rx->chan9_h4 << 6) | rx->chan9_l6; break;
+ case 9: raw = ((uint32_t)rx->chan10_h2 << 8) | rx->chan10_l8; break;
+ case 10: raw = ((uint32_t)rx->chan11_h8 << 2) | rx->chan11_l2; break;
+ case 11: raw = ((uint32_t)rx->chan12_h6 << 4) | rx->chan12_l4; break;
+ default: retval = CTR_InvalidParamValue; break;
+ }
+ }else if(idx <= 15){
+ GET_STATUS3();
+ retval = rx.err;
+ switch(idx){
+ case 12: raw = ((uint32_t)rx->chan13_h8 << 2) | rx->chan13_l2; break;
+ case 13: raw = ((uint32_t)rx->chan14_h6 << 4) | rx->chan14_l4; break;
+ case 14: raw = ((uint32_t)rx->chan15_h4 << 6) | rx->chan15_l6; break;
+ case 15: raw = ((uint32_t)rx->chan16_h2 << 8) | rx->chan16_l8; break;
+ default: retval = CTR_InvalidParamValue; break;
+ }
+ }
+ /* convert to amps */
+ current = (double)raw * 0.125; /* 7.3 fixed pt value in Amps */
+ /* signal caller with success */
+ return retval;
+}
+CTR_Code PDP::GetVoltage(double &voltage)
+{
+ GET_STATUS3();
+ uint32_t raw = rx->busVoltage;
+ voltage = (double)raw * 0.05 + 4.0; /* 50mV per unit plus 4V. */;
+ return rx.err;
+}
+CTR_Code PDP::GetTemperature(double &tempC)
+{
+ GET_STATUS3();
+ uint32_t raw = rx->temp;
+ tempC = (double)raw * 1.03250836957542 - 67.8564500484966;
+ return rx.err;
+}
+CTR_Code PDP::GetTotalCurrent(double ¤tAmps)
+{
+ GET_STATUS_ENERGY();
+ uint32_t raw;
+ raw = rx->TotalCurrent_125mAperunit_h8;
+ raw <<= 4;
+ raw |= rx->TotalCurrent_125mAperunit_l4;
+ currentAmps = 0.125 * raw;
+ return rx.err;
+}
+CTR_Code PDP::GetTotalPower(double &powerWatts)
+{
+ GET_STATUS_ENERGY();
+ uint32_t raw;
+ raw = rx->Power_125mWperunit_h4;
+ raw <<= 8;
+ raw |= rx->Power_125mWperunit_m8;
+ raw <<= 4;
+ raw |= rx->Power_125mWperunit_l4;
+ powerWatts = 0.125 * raw;
+ return rx.err;
+}
+CTR_Code PDP::GetTotalEnergy(double &energyJoules)
+{
+ GET_STATUS_ENERGY();
+ uint32_t raw;
+ raw = rx->Energy_125mWPerUnitXTmeas_h4;
+ raw <<= 8;
+ raw |= rx->Energy_125mWPerUnitXTmeas_mh8;
+ raw <<= 8;
+ raw |= rx->Energy_125mWPerUnitXTmeas_ml8;
+ raw <<= 8;
+ raw |= rx->Energy_125mWPerUnitXTmeas_l8;
+ energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
+ energyJoules *= 0.001; /* convert from mW to W */
+ energyJoules *= rx->TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
+ return rx.err;
+}
+/* Clear sticky faults.
+ * @Return - CTR_Code - Error code (if any)
+ */
+CTR_Code PDP::ClearStickyFaults()
+{
+ int32_t status = 0;
+ uint8_t pdpControl[] = { 0x80 }; /* only bit set is ClearStickyFaults */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_1 | GetDeviceNumber(), pdpControl, sizeof(pdpControl), 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+
+/* Reset Energy Signals
+ * @Return - CTR_Code - Error code (if any)
+ */
+CTR_Code PDP::ResetEnergy()
+{
+ int32_t status = 0;
+ uint8_t pdpControl[] = { 0x40 }; /* only bit set is ResetEnergy */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_1 | GetDeviceNumber(), pdpControl, sizeof(pdpControl), 0, &status);
+ if(status)
+ return CTR_TxFailed;
+ return CTR_OKAY;
+}
+//------------------ C interface --------------------------------------------//
+extern "C" {
+ void * c_PDP_Init(void)
+ {
+ return new PDP();
+ }
+ CTR_Code c_GetChannelCurrent(void * handle,UINT8 idx, double *status)
+ {
+ return ((PDP*)handle)-> GetChannelCurrent(idx,*status);
+ }
+ CTR_Code c_GetVoltage(void * handle,double *status)
+ {
+ return ((PDP*)handle)-> GetVoltage(*status);
+ }
+ CTR_Code c_GetTemperature(void * handle,double *status)
+ {
+ return ((PDP*)handle)-> GetTemperature(*status);
+ }
+ void c_SetDeviceNumber_PDP(void * handle,UINT8 deviceNumber)
+ {
+ }
+}