diff --git a/cmake/run-cpp-tests.sh b/cmake/run-cpp-tests.sh index 1bf24cf224..1cb92b27aa 100755 --- a/cmake/run-cpp-tests.sh +++ b/cmake/run-cpp-tests.sh @@ -6,11 +6,11 @@ if [ $(which sshpass) ] then sshpass -p "" ssh admin@10.1.90.2 killall FRCUserProgram sshpass -p "" scp target/cmake/wpilibc/wpilibC++IntegrationTests/FRCUserProgram admin@10.1.90.2:/home/admin - sshpass -p "" ssh admin@10.1.90.2 ./FRCUserProgram + sshpass -p "" ssh admin@10.1.90.2 ./FRCUserProgram $* else ssh admin@10.1.90.2 killall FRCUserProgram scp target/cmake/wpilibc/wpilibC++IntegrationTests/FRCUserProgram admin@10.1.90.2:/home/admin - ssh admin@10.1.90.2 ./FRCUserProgram + ssh admin@10.1.90.2 ./FRCUserProgram $* fi diff --git a/hal/lib/Athena/NetworkCommunication/CANInterfacePlugin.h b/hal/lib/Athena/NetworkCommunication/CANInterfacePlugin.h index a973c31d2a..bf0abef86a 100644 --- a/hal/lib/Athena/NetworkCommunication/CANInterfacePlugin.h +++ b/hal/lib/Athena/NetworkCommunication/CANInterfacePlugin.h @@ -11,7 +11,9 @@ #include #define CAN_IS_FRAME_REMOTE 0x80000000 -#define CAN_MESSAGE_ID_MASK 0x1FFFFFFF +#define CAN_IS_FRAME_11BIT 0x40000000 +#define CAN_29BIT_MESSAGE_ID_MASK 0x1FFFFFFF +#define CAN_11BIT_MESSAGE_ID_MASK 0x000007FF class CANInterfacePlugin { @@ -56,6 +58,6 @@ public: * @param interface A pointer to an object that inherits from CANInterfacePlugin and implements * the pure virtual interface. If NULL, unregister the current plugin. */ -void FRC_NetworkCommunication_JaguarCANDriver_registerInterface(CANInterfacePlugin* interface); +void FRC_NetworkCommunication_CANSessionMux_registerInterface(CANInterfacePlugin* interface); #endif // __CANInterfacePlugin_h__ diff --git a/hal/lib/Athena/NetworkCommunication/CANSessionMux.h b/hal/lib/Athena/NetworkCommunication/CANSessionMux.h new file mode 100644 index 0000000000..d31c76c1a2 --- /dev/null +++ b/hal/lib/Athena/NetworkCommunication/CANSessionMux.h @@ -0,0 +1,61 @@ +// CANSessionMux.h +// +// Defines the API for building a CAN Interface Plugin to support +// PWM-cable-free CAN motor control on FRC robots. This allows you +// to connect any CAN interface to the secure Jaguar CAN driver. +// + +#ifndef __CANSessionMux_h__ +#define __CANSessionMux_h__ + +#if defined(__vxworks) +#include +#else +#include +#endif + +#define CAN_SEND_PERIOD_NO_REPEAT 0 +#define CAN_SEND_PERIOD_STOP_REPEATING -1 + +/* Flags in the upper bits of the messageID */ +#define CAN_IS_FRAME_REMOTE 0x80000000 +#define CAN_IS_FRAME_11BIT 0x40000000 + +#define ERR_CANSessionMux_InvalidBuffer -44086 +#define ERR_CANSessionMux_MessageNotFound -44087 +#define WARN_CANSessionMux_NoToken 44087 +#define ERR_CANSessionMux_NotAllowed -44088 +#define ERR_CANSessionMux_NotInitialized -44089 + +struct tCANStreamMessage{ + uint32_t messageID; + uint32_t timeStamp; + uint8_t data[8]; + uint8_t dataSize; +}; + +namespace nCANSessionMux +{ + void sendMessage_wrapper(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t periodMs, int32_t *status); + void receiveMessage_wrapper(uint32_t *messageID, uint32_t messageIDMask, uint8_t *data, uint8_t *dataSize, uint32_t *timeStamp, int32_t *status); + void openStreamSession(uint32_t *sessionHandle, uint32_t messageID, uint32_t messageIDMask, uint32_t maxMessages, int32_t *status); + void closeStreamSession(uint32_t sessionHandle); + void readStreamSession(uint32_t sessionHandle, struct tCANStreamMessage *messages, uint32_t messagesToRead, uint32_t *messagesRead, int32_t *status); +} + +#ifdef __cplusplus +extern "C" +{ +#endif + + void FRC_NetworkCommunication_CANSessionMux_sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t periodMs, int32_t *status); + void FRC_NetworkCommunication_CANSessionMux_receiveMessage(uint32_t *messageID, uint32_t messageIDMask, uint8_t *data, uint8_t *dataSize, uint32_t *timeStamp, int32_t *status); + void FRC_NetworkCommunication_CANSessionMux_openStreamSession(uint32_t *sessionHandle, uint32_t messageID, uint32_t messageIDMask, uint32_t maxMessages, int32_t *status); + void FRC_NetworkCommunication_CANSessionMux_closeStreamSession(uint32_t sessionHandle); + void FRC_NetworkCommunication_CANSessionMux_readStreamSession(uint32_t sessionHandle, struct tCANStreamMessage *messages, uint32_t messagesToRead, uint32_t *messagesRead, int32_t *status); + +#ifdef __cplusplus +} +#endif + +#endif // __CANSessionMux_h__ diff --git a/hal/lib/Athena/NetworkCommunication/JaguarCANDriver.h b/hal/lib/Athena/NetworkCommunication/JaguarCANDriver.h deleted file mode 100644 index c7ded49749..0000000000 --- a/hal/lib/Athena/NetworkCommunication/JaguarCANDriver.h +++ /dev/null @@ -1,56 +0,0 @@ -// JaguarCANDriver.h -// -// Defines the API for building a CAN Interface Plugin to support -// PWM-cable-free CAN motor control on FRC robots. This allows you -// to connect any CAN interface to the secure Jaguar CAN driver. -// - -#ifndef __JaguarCANDriver_h__ -#define __JaguarCANDriver_h__ - -#if defined(__vxworks) -#include -#else -#include -#include -#endif -#ifdef USE_THRIFT -#include "NetCommRPCComm.h" -#include -#endif -namespace nJaguarCANDriver -{ - void sendMessage_wrapper(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t *status); - void receiveMessage_wrapper(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, uint32_t timeoutMs, int32_t *status); - int32_t receiveMessageStart_wrapper(uint32_t messageID, uint32_t occurRefNum, uint32_t timeoutMs, int32_t *status); -#if defined (__vxworks) - int32_t receiveMessageStart_sem_wrapper(uint32_t messageID, uint32_t semaphoreID, uint32_t timeoutMs, int32_t *status); -#else - int32_t receiveMessageStart_mutex_wrapper(uint32_t messageID, pthread_mutex_t *mutex, uint32_t timeoutMs, int32_t *status); -#endif - void receiveMessageComplete_wrapper(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, int32_t *status); -#ifdef USE_THRIFT - void checkEvent_CAN(std::vector< CANEvent >& events); -#endif -} - -#ifdef __cplusplus -extern "C" -{ -#endif - - void FRC_NetworkCommunication_JaguarCANDriver_sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t *status); - void FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, uint32_t timeoutMs, int32_t *status); - int32_t FRC_NetworkCommunication_JaguarCANDriver_receiveMessageStart(uint32_t messageID, uint32_t occurRefNum, uint32_t timeoutMs, int32_t *status); -#if defined (__vxworks) - int32_t FRC_NetworkCommunication_JaguarCANDriver_receiveMessageStart_sem(uint32_t messageID, uint32_t semaphoreID, uint32_t timeoutMs, int32_t *status); -#else - int32_t FRC_NetworkCommunication_JaguarCANDriver_receiveMessageStart_mutex(uint32_t messageID, pthread_mutex_t *mutex, uint32_t timeoutMs, int32_t *status); -#endif - void FRC_NetworkCommunication_JaguarCANDriver_receiveMessageComplete(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, int32_t *status); - -#ifdef __cplusplus -} -#endif - -#endif // __JaguarCANDriver_h__ diff --git a/hal/lib/Athena/Solenoid.cpp b/hal/lib/Athena/Solenoid.cpp index bc6a4a1458..ff33855a77 100644 --- a/hal/lib/Athena/Solenoid.cpp +++ b/hal/lib/Athena/Solenoid.cpp @@ -22,20 +22,20 @@ void initializePCM() { if(!pcmModulesInitialized) { modules[0] = new PCM(50); modules[1] = new PCM(51); - + pcmModulesInitialized = true; } } void* initializeSolenoidPort(void *port_pointer, int32_t *status) { initializePCM(); - + Port* port = (Port*) port_pointer; - + solenoid_port_t *solenoid_port = new solenoid_port_t; solenoid_port->module = modules[port->module - 1]; solenoid_port->pin = port->pin; - + return solenoid_port; } @@ -46,15 +46,14 @@ bool checkSolenoidModule(uint8_t module) { bool getSolenoid(void* solenoid_port_pointer, int32_t *status) { solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer; bool value; - + *status = port->module->GetSolenoid(port->pin, value); - + return value; } void setSolenoid(void* solenoid_port_pointer, bool value, int32_t *status) { solenoid_port_t* port = (solenoid_port_t*) solenoid_port_pointer; - - *status = port->module->SetSolenoid(port->pin, value); -} + port->module->SetSolenoid(port->pin, value); +} diff --git a/hal/lib/Athena/ctre/PCM.cpp b/hal/lib/Athena/ctre/PCM.cpp index e8a23c1005..9371bac1dd 100644 --- a/hal/lib/Athena/ctre/PCM.cpp +++ b/hal/lib/Athena/ctre/PCM.cpp @@ -1,13 +1,21 @@ #pragma GCC diagnostic ignored "-Wmissing-field-initializers" #include "PCM.h" -#include "NetworkCommunication/JaguarCANDriver.h" +#include "NetworkCommunication/CANSessionMux.h" #include // memset #include // usleep + +static const UINT32 kFullMessageIDMask = 0x1fffffff; + +/* This can be a constant, as long as nobody needs to updatie solenoids within + 1/50 of a second. */ +static const INT32 kCANPeriod = 20; + + /* 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) @@ -94,9 +102,9 @@ CTR_Code PCM::SetClosedLoopControl(bool en) { } /* Get solenoid state - * + * * @Return - True/False - True if solenoid enabled, false otherwise - * + * * @Param - idx - ID of solenoid (1-8) to return status of */ CTR_Code PCM::GetSolenoid(UINT8 idx, bool &status) { @@ -108,7 +116,7 @@ CTR_Code PCM::GetSolenoid(UINT8 idx, bool &status) { } /* Get pressure switch state - * + * * @Return - True/False - True if pressure adequate, false if low */ CTR_Code PCM::GetPressure(bool &status) { @@ -119,7 +127,7 @@ CTR_Code PCM::GetPressure(bool &status) { } /* Get compressor state - * + * * @Return - True/False - True if enabled, false if otherwise */ CTR_Code PCM::GetCompressor(bool &status) { @@ -130,7 +138,7 @@ CTR_Code PCM::GetCompressor(bool &status) { } /* Get closed loop control state - * + * * @Return - True/False - True if closed loop enabled, false if otherwise */ CTR_Code PCM::GetClosedLoopControl(bool &status) { @@ -141,8 +149,8 @@ CTR_Code PCM::GetClosedLoopControl(bool &status) { } /* Get compressor current draw - * - * @Return - Amperes - Compressor current + * + * @Return - Amperes - Compressor current */ CTR_Code PCM::GetCompressorCurrent(float &status) { uint16_t bt = _PcmStatus.compressorCurrentTop6; @@ -155,7 +163,7 @@ CTR_Code PCM::GetCompressorCurrent(float &status) { } /* Get voltage across solenoid rail - * + * * @Return - Volts - Voltage across solenoid rail */ CTR_Code PCM::GetSolenoidVoltage(float &status) { @@ -169,7 +177,7 @@ CTR_Code PCM::GetSolenoidVoltage(float &status) { } /* Get hardware fault value - * + * * @Return - True/False - True if hardware failure detected, false if otherwise */ CTR_Code PCM::GetHardwareFault(bool &status) { @@ -180,7 +188,7 @@ CTR_Code PCM::GetHardwareFault(bool &status) { } /* Get compressor fault value - * + * * @Return - True/False - True if shorted compressor detected, false if otherwise */ CTR_Code PCM::GetCompressorFault(bool &status) { @@ -191,7 +199,7 @@ CTR_Code PCM::GetCompressorFault(bool &status) { } /* Get solenoid fault value - * + * * @Return - True/False - True if shorted solenoid detected, false if otherwise */ CTR_Code PCM::GetSolenoidFault(bool &status) { @@ -203,7 +211,7 @@ CTR_Code PCM::GetSolenoidFault(bool &status) { // Past Faults /* Get compressor sticky fault value - * + * * @Return - True/False - True if solenoid had previously been shorted * (and sticky fault was not cleared), false if otherwise */ @@ -215,7 +223,7 @@ CTR_Code PCM::GetCompressorStickyFault(bool &status) { } /* Get solenoid sticky fault value - * + * * @Return - True/False - True if compressor had previously been shorted * (and sticky fault was not cleared), false if otherwise */ @@ -226,7 +234,7 @@ CTR_Code PCM::GetSolenoidStickyFault(bool &status) { /* fix this */ return CTR_OKAY; } /* Get battery voltage - * + * * @Return - Volts - Voltage across PCM power ports */ CTR_Code PCM::GetBatteryVoltage(float &status) { @@ -236,9 +244,9 @@ CTR_Code PCM::GetBatteryVoltage(float &status) { return CTR_OKAY; } /* 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 @@ -252,10 +260,10 @@ CTR_Code PCM::GetNumberOfFailedControlFrames(UINT16 &status) { return CTR_OKAY; } /* 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 @@ -268,11 +276,11 @@ CTR_Code PCM::GetSolenoidBlackList(UINT8 &status) { } /* 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 - * + * * @WARNING - Return only valid if [SeekStatusFaultFrames] is enabled * See function SeekStatusFaultFrames * See function EnableSeekStatusFaultFrames @@ -313,7 +321,8 @@ void PCM::ReadStatusFrame(void) { PcmStatus_t frame = {0}; UINT8 size = 0; INT32 status = 0; - FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(&PCM_settings.statusFrameID, (uint8_t *)&frame, &size, 0 , &status); + UINT32 timeStamp = 0; + FRC_NetworkCommunication_CANSessionMux_receiveMessage(&PCM_settings.statusFrameID, kFullMessageIDMask, (uint8_t *)&frame, &size, &timeStamp, &status); if (status == 0) { _timeSinceLastRx = 0; _PcmStatus = frame; @@ -326,7 +335,8 @@ void PCM::ReadStatusFaultFrame(void) { PcmStatusFault_t frame= {0}; UINT8 size = 0; INT32 status = 0; - FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(&PCM_settings.statusFaultFrameID, (uint8_t *)&frame, &size, 0, &status); + UINT32 timeStamp = 0; + FRC_NetworkCommunication_CANSessionMux_receiveMessage(&PCM_settings.statusFaultFrameID, kFullMessageIDMask, (uint8_t *)&frame, &size, &timeStamp, &status); if (status == 0) { _timeSinceLastRx = 0; _PcmStatusFault = frame; @@ -339,7 +349,8 @@ void PCM::ReadDebugFrame(void) { PcmDebug_t frame= {0}; UINT8 size = 0; INT32 status = 0; - FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(&PCM_settings.debugFrameID, (uint8_t *)&frame, &size, 0, &status); + UINT32 timeStamp = 0; + FRC_NetworkCommunication_CANSessionMux_receiveMessage(&PCM_settings.debugFrameID, kFullMessageIDMask, (uint8_t *)&frame, &size, &timeStamp, &status); if (status == 0) { _timeSinceLastRx = 0; _PcmDebug = frame; @@ -351,7 +362,7 @@ void * PCM::ThreadFunc() { while(_threadIsRunning){ int32_t status = 0; - FRC_NetworkCommunication_JaguarCANDriver_sendMessage(PCM_settings.controlFrameID, (const uint8_t *)&_PcmControl, sizeof(_PcmControl), &status); + FRC_NetworkCommunication_CANSessionMux_sendMessage(PCM_settings.controlFrameID, (const uint8_t *)&_PcmControl, sizeof(_PcmControl), kCANPeriod, &status); if(status == 0){ /* success */ _timeSinceLastTx = 0; @@ -473,4 +484,3 @@ extern "C" { return retval; } } - diff --git a/hal/lib/Athena/ctre/PCM.h b/hal/lib/Athena/ctre/PCM.h index 3e39147d8a..440fcbb8b0 100644 --- a/hal/lib/Athena/ctre/PCM.h +++ b/hal/lib/Athena/ctre/PCM.h @@ -1,7 +1,7 @@ #ifndef PCM_H_ #define PCM_H_ #include "ctre.h" //BIT Defines + Typedefs -#include //CAN Comm +#include //CAN Comm #include /* encoder/decoders */ typedef struct _PcmStatus_t{ @@ -75,7 +75,7 @@ class PCM public: PCM(UINT8 deviceNumber=50); ~PCM(); - + /* Set PCM solenoid state * @Return - CTR_Code - Error code (if any) for setting solenoid * @Param - idx - ID of solenoid (1-8) @@ -94,7 +94,7 @@ public: * @Param - clr - Clear / do not clear faults */ CTR_Code ClearStickyFaults(bool clr); - + /* Get solenoid state * * @Return - CTR_Code - Error code (if any) @@ -172,7 +172,7 @@ public: * @Param - status - Voltage across PCM power ports in Volts (V) */ CTR_Code GetBatteryVoltage(float &status); - + /* Set PCM Device Number and according CAN frame IDs * @Return - void * @Param - deviceNumber - Device number of PCM to control @@ -186,7 +186,7 @@ public: * See function EnableSeekDebugFrames */ CTR_Code GetNumberOfFailedControlFrames(UINT16 &status); - + /* Get raw Solenoid Blacklist * @Return - CTR_Code - Error code (if any) * @Param - status - Raw binary breakdown of Solenoid Blacklist diff --git a/hal/lib/Athena/ctre/PDP.cpp b/hal/lib/Athena/ctre/PDP.cpp index 6bcb629deb..555c201104 100644 --- a/hal/lib/Athena/ctre/PDP.cpp +++ b/hal/lib/Athena/ctre/PDP.cpp @@ -1,7 +1,10 @@ #include "PDP.h" -#include "NetworkCommunication/JaguarCANDriver.h" +#include "NetworkCommunication/CANSessionMux.h" //CAN Comm #include // memset #include // usleep + +#define kFullMessageIDMask 0x1fffffff + PDP::PDP(UINT8 deviceNumber) { memset(&_status1, 0, sizeof(_status1)); @@ -107,7 +110,9 @@ uint64_t PDP::ReadCurrents(uint8_t api) uint64_t frame = 0; UINT8 size = 0; INT32 status = 0; - FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(&PDP_Settings.frameIDs[api], (uint8_t *)&frame, &size, 0 , &status); + UINT32 timeStamp = 0; + + FRC_NetworkCommunication_CANSessionMux_receiveMessage(&PDP_Settings.frameIDs[api], kFullMessageIDMask, (uint8_t *)&frame, &size, &timeStamp, &status); if (status == 0) { _timeSinceLastRx = 0; } else { diff --git a/hal/lib/Athena/ctre/PDP.h b/hal/lib/Athena/ctre/PDP.h index b38424c8e2..46713a02f9 100644 --- a/hal/lib/Athena/ctre/PDP.h +++ b/hal/lib/Athena/ctre/PDP.h @@ -1,7 +1,7 @@ #ifndef PDP_H_ #define PDP_H_ #include "ctre.h" //BIT Defines + Typedefs -#include //CAN Comm +#include //CAN Comm #include /* encoder/decoders */ typedef struct _PdpStatus1_t{ diff --git a/ni-libraries/libFRC_NetworkCommunication.so.1 b/ni-libraries/libFRC_NetworkCommunication.so.1 index 7aa74fe26b..dc2d123d99 100755 Binary files a/ni-libraries/libFRC_NetworkCommunication.so.1 and b/ni-libraries/libFRC_NetworkCommunication.so.1 differ diff --git a/wpilibc/wpilibC++/include/CAN/JaguarCANDriver.h b/wpilibc/wpilibC++/include/CAN/JaguarCANDriver.h deleted file mode 100644 index c7ded49749..0000000000 --- a/wpilibc/wpilibC++/include/CAN/JaguarCANDriver.h +++ /dev/null @@ -1,56 +0,0 @@ -// JaguarCANDriver.h -// -// Defines the API for building a CAN Interface Plugin to support -// PWM-cable-free CAN motor control on FRC robots. This allows you -// to connect any CAN interface to the secure Jaguar CAN driver. -// - -#ifndef __JaguarCANDriver_h__ -#define __JaguarCANDriver_h__ - -#if defined(__vxworks) -#include -#else -#include -#include -#endif -#ifdef USE_THRIFT -#include "NetCommRPCComm.h" -#include -#endif -namespace nJaguarCANDriver -{ - void sendMessage_wrapper(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t *status); - void receiveMessage_wrapper(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, uint32_t timeoutMs, int32_t *status); - int32_t receiveMessageStart_wrapper(uint32_t messageID, uint32_t occurRefNum, uint32_t timeoutMs, int32_t *status); -#if defined (__vxworks) - int32_t receiveMessageStart_sem_wrapper(uint32_t messageID, uint32_t semaphoreID, uint32_t timeoutMs, int32_t *status); -#else - int32_t receiveMessageStart_mutex_wrapper(uint32_t messageID, pthread_mutex_t *mutex, uint32_t timeoutMs, int32_t *status); -#endif - void receiveMessageComplete_wrapper(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, int32_t *status); -#ifdef USE_THRIFT - void checkEvent_CAN(std::vector< CANEvent >& events); -#endif -} - -#ifdef __cplusplus -extern "C" -{ -#endif - - void FRC_NetworkCommunication_JaguarCANDriver_sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t *status); - void FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, uint32_t timeoutMs, int32_t *status); - int32_t FRC_NetworkCommunication_JaguarCANDriver_receiveMessageStart(uint32_t messageID, uint32_t occurRefNum, uint32_t timeoutMs, int32_t *status); -#if defined (__vxworks) - int32_t FRC_NetworkCommunication_JaguarCANDriver_receiveMessageStart_sem(uint32_t messageID, uint32_t semaphoreID, uint32_t timeoutMs, int32_t *status); -#else - int32_t FRC_NetworkCommunication_JaguarCANDriver_receiveMessageStart_mutex(uint32_t messageID, pthread_mutex_t *mutex, uint32_t timeoutMs, int32_t *status); -#endif - void FRC_NetworkCommunication_JaguarCANDriver_receiveMessageComplete(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, int32_t *status); - -#ifdef __cplusplus -} -#endif - -#endif // __JaguarCANDriver_h__ diff --git a/wpilibc/wpilibC++/include/CANJaguar.h b/wpilibc/wpilibC++/include/CANJaguar.h index d1ce734574..fe54fd9550 100644 --- a/wpilibc/wpilibC++/include/CANJaguar.h +++ b/wpilibc/wpilibC++/include/CANJaguar.h @@ -15,6 +15,8 @@ #include "LiveWindow/LiveWindowSendable.h" #include "tables/ITable.h" +#include + /** * Luminary Micro Jaguar Speed Control */ @@ -43,7 +45,6 @@ public: // SpeedController interface virtual float Get(); virtual void Set(float value, uint8_t syncGroup=0); - void SetNoAck(float value, uint8_t syncGroup=0); virtual void Disable(); // PIDOutput interface @@ -71,7 +72,6 @@ public: bool GetForwardLimitOK(); bool GetReverseLimitOK(); uint16_t GetFaults(); - bool GetPowerCycled(); void SetVoltageRampRate(double rampRate); virtual uint32_t GetFirmwareVersion(); uint8_t GetHardwareVersion(); @@ -105,18 +105,52 @@ protected: int16_t unpackint16_t(uint8_t *buffer); int32_t unpackint32_t(uint8_t *buffer); virtual void setTransaction(uint32_t messageID, const uint8_t *data, uint8_t dataSize); - virtual void getTransaction(uint32_t messageID, uint8_t *data, uint8_t *dataSize); + virtual bool getTransaction(uint32_t messageID, uint8_t *data, uint8_t *dataSize); static int32_t sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize); - //#define kTimeoutCan 0.02 - #define kTimeoutCan 0.0 - static int32_t receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, float timeout = kTimeoutCan); + static int32_t receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize); uint8_t m_deviceNumber; ControlMode m_controlMode; MUTEX_ID m_transactionSemaphore; double m_maxOutputVoltage; + enum CANValue { + CAN_VALUE = 0x1, + CAN_SPEED_REFERENCE = 0x2, + CAN_POSITION_REFERENCE = 0x4, + CAN_P = 0x8, CAN_I = 0x10, CAN_D = 0x20, + CAN_BUS_VOLTAGE = 0x40, + CAN_OUTPUT_VOLTAGE = 0x80, + CAN_OUTPUT_CURRENT = 0x100, + CAN_TEMPERATURE = 0x200, + CAN_POSITION = 0x400, + CAN_SPEED = 0x800, + CAN_LIMITS = 0x1000, + CAN_FAULTS = 0x2000, + CAN_FIRMWARE_VERSION = 0x4000, + CAN_HARDWARE_VERSION = 0x8000, + CAN_EVERYTHING = 0xffff + }; + + bool isUnverified(CANValue value) const; + void verifyCANValues(); + + // Keep track of what cached CAN values have been verified. + CANValue m_verified_values; + + // Cached CAN data + float m_value; + SpeedReference m_speedReference; + PositionReference m_positionReference; + double m_p, m_i, m_d; + float m_busVoltage, m_outputVoltage, m_outputCurrent, m_temperature; + double m_position, m_speed; + uint8_t m_limits; + uint16_t m_faults; + uint32_t m_firmwareVersion; + uint8_t m_hardwareVersion; + MotorSafetyHelper *m_safetyHelper; void ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew); @@ -126,7 +160,7 @@ protected: std::string GetSmartDashboardType(); void InitTable(ITable *subTable); ITable * GetTable(); - + ITable *m_table; private: diff --git a/wpilibc/wpilibC++/include/NetworkCommunication/CANInterfacePlugin.h b/wpilibc/wpilibC++/include/NetworkCommunication/CANInterfacePlugin.h index a973c31d2a..bf0abef86a 100644 --- a/wpilibc/wpilibC++/include/NetworkCommunication/CANInterfacePlugin.h +++ b/wpilibc/wpilibC++/include/NetworkCommunication/CANInterfacePlugin.h @@ -11,7 +11,9 @@ #include #define CAN_IS_FRAME_REMOTE 0x80000000 -#define CAN_MESSAGE_ID_MASK 0x1FFFFFFF +#define CAN_IS_FRAME_11BIT 0x40000000 +#define CAN_29BIT_MESSAGE_ID_MASK 0x1FFFFFFF +#define CAN_11BIT_MESSAGE_ID_MASK 0x000007FF class CANInterfacePlugin { @@ -56,6 +58,6 @@ public: * @param interface A pointer to an object that inherits from CANInterfacePlugin and implements * the pure virtual interface. If NULL, unregister the current plugin. */ -void FRC_NetworkCommunication_JaguarCANDriver_registerInterface(CANInterfacePlugin* interface); +void FRC_NetworkCommunication_CANSessionMux_registerInterface(CANInterfacePlugin* interface); #endif // __CANInterfacePlugin_h__ diff --git a/wpilibc/wpilibC++/include/NetworkCommunication/CANSessionMux.h b/wpilibc/wpilibC++/include/NetworkCommunication/CANSessionMux.h new file mode 100644 index 0000000000..d31c76c1a2 --- /dev/null +++ b/wpilibc/wpilibC++/include/NetworkCommunication/CANSessionMux.h @@ -0,0 +1,61 @@ +// CANSessionMux.h +// +// Defines the API for building a CAN Interface Plugin to support +// PWM-cable-free CAN motor control on FRC robots. This allows you +// to connect any CAN interface to the secure Jaguar CAN driver. +// + +#ifndef __CANSessionMux_h__ +#define __CANSessionMux_h__ + +#if defined(__vxworks) +#include +#else +#include +#endif + +#define CAN_SEND_PERIOD_NO_REPEAT 0 +#define CAN_SEND_PERIOD_STOP_REPEATING -1 + +/* Flags in the upper bits of the messageID */ +#define CAN_IS_FRAME_REMOTE 0x80000000 +#define CAN_IS_FRAME_11BIT 0x40000000 + +#define ERR_CANSessionMux_InvalidBuffer -44086 +#define ERR_CANSessionMux_MessageNotFound -44087 +#define WARN_CANSessionMux_NoToken 44087 +#define ERR_CANSessionMux_NotAllowed -44088 +#define ERR_CANSessionMux_NotInitialized -44089 + +struct tCANStreamMessage{ + uint32_t messageID; + uint32_t timeStamp; + uint8_t data[8]; + uint8_t dataSize; +}; + +namespace nCANSessionMux +{ + void sendMessage_wrapper(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t periodMs, int32_t *status); + void receiveMessage_wrapper(uint32_t *messageID, uint32_t messageIDMask, uint8_t *data, uint8_t *dataSize, uint32_t *timeStamp, int32_t *status); + void openStreamSession(uint32_t *sessionHandle, uint32_t messageID, uint32_t messageIDMask, uint32_t maxMessages, int32_t *status); + void closeStreamSession(uint32_t sessionHandle); + void readStreamSession(uint32_t sessionHandle, struct tCANStreamMessage *messages, uint32_t messagesToRead, uint32_t *messagesRead, int32_t *status); +} + +#ifdef __cplusplus +extern "C" +{ +#endif + + void FRC_NetworkCommunication_CANSessionMux_sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t periodMs, int32_t *status); + void FRC_NetworkCommunication_CANSessionMux_receiveMessage(uint32_t *messageID, uint32_t messageIDMask, uint8_t *data, uint8_t *dataSize, uint32_t *timeStamp, int32_t *status); + void FRC_NetworkCommunication_CANSessionMux_openStreamSession(uint32_t *sessionHandle, uint32_t messageID, uint32_t messageIDMask, uint32_t maxMessages, int32_t *status); + void FRC_NetworkCommunication_CANSessionMux_closeStreamSession(uint32_t sessionHandle); + void FRC_NetworkCommunication_CANSessionMux_readStreamSession(uint32_t sessionHandle, struct tCANStreamMessage *messages, uint32_t messagesToRead, uint32_t *messagesRead, int32_t *status); + +#ifdef __cplusplus +} +#endif + +#endif // __CANSessionMux_h__ diff --git a/wpilibc/wpilibC++/include/NetworkCommunication/JaguarCANDriver.h b/wpilibc/wpilibC++/include/NetworkCommunication/JaguarCANDriver.h deleted file mode 100644 index c7ded49749..0000000000 --- a/wpilibc/wpilibC++/include/NetworkCommunication/JaguarCANDriver.h +++ /dev/null @@ -1,56 +0,0 @@ -// JaguarCANDriver.h -// -// Defines the API for building a CAN Interface Plugin to support -// PWM-cable-free CAN motor control on FRC robots. This allows you -// to connect any CAN interface to the secure Jaguar CAN driver. -// - -#ifndef __JaguarCANDriver_h__ -#define __JaguarCANDriver_h__ - -#if defined(__vxworks) -#include -#else -#include -#include -#endif -#ifdef USE_THRIFT -#include "NetCommRPCComm.h" -#include -#endif -namespace nJaguarCANDriver -{ - void sendMessage_wrapper(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t *status); - void receiveMessage_wrapper(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, uint32_t timeoutMs, int32_t *status); - int32_t receiveMessageStart_wrapper(uint32_t messageID, uint32_t occurRefNum, uint32_t timeoutMs, int32_t *status); -#if defined (__vxworks) - int32_t receiveMessageStart_sem_wrapper(uint32_t messageID, uint32_t semaphoreID, uint32_t timeoutMs, int32_t *status); -#else - int32_t receiveMessageStart_mutex_wrapper(uint32_t messageID, pthread_mutex_t *mutex, uint32_t timeoutMs, int32_t *status); -#endif - void receiveMessageComplete_wrapper(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, int32_t *status); -#ifdef USE_THRIFT - void checkEvent_CAN(std::vector< CANEvent >& events); -#endif -} - -#ifdef __cplusplus -extern "C" -{ -#endif - - void FRC_NetworkCommunication_JaguarCANDriver_sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t *status); - void FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, uint32_t timeoutMs, int32_t *status); - int32_t FRC_NetworkCommunication_JaguarCANDriver_receiveMessageStart(uint32_t messageID, uint32_t occurRefNum, uint32_t timeoutMs, int32_t *status); -#if defined (__vxworks) - int32_t FRC_NetworkCommunication_JaguarCANDriver_receiveMessageStart_sem(uint32_t messageID, uint32_t semaphoreID, uint32_t timeoutMs, int32_t *status); -#else - int32_t FRC_NetworkCommunication_JaguarCANDriver_receiveMessageStart_mutex(uint32_t messageID, pthread_mutex_t *mutex, uint32_t timeoutMs, int32_t *status); -#endif - void FRC_NetworkCommunication_JaguarCANDriver_receiveMessageComplete(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, int32_t *status); - -#ifdef __cplusplus -} -#endif - -#endif // __JaguarCANDriver_h__ diff --git a/wpilibc/wpilibC++/include/NetworkCommunication/UsageReporting.h b/wpilibc/wpilibC++/include/NetworkCommunication/UsageReporting.h index 91d4ceba86..918ac5ae63 100644 --- a/wpilibc/wpilibC++/include/NetworkCommunication/UsageReporting.h +++ b/wpilibc/wpilibC++/include/NetworkCommunication/UsageReporting.h @@ -26,7 +26,7 @@ namespace nUsageReporting kResourceType_CANPlugin, kResourceType_Accelerometer, kResourceType_ADXL345, - kResourceType_AnalogInput, + kResourceType_AnalogChannel, kResourceType_AnalogTrigger, kResourceType_AnalogTriggerOutput, kResourceType_CANJaguar, diff --git a/wpilibc/wpilibC++/lib/CANJaguar.cpp b/wpilibc/wpilibC++/lib/CANJaguar.cpp index f9158aa0c1..519ee152aa 100644 --- a/wpilibc/wpilibC++/lib/CANJaguar.cpp +++ b/wpilibc/wpilibC++/lib/CANJaguar.cpp @@ -6,7 +6,7 @@ #include "CANJaguar.h" #define tNIRIO_i32 int -#include "CAN/JaguarCANDriver.h" +#include "NetworkCommunication/CANSessionMux.h" #include "CAN/can_proto.h" //#include "NetworkCommunication/UsageReporting.h" #include "WPIErrors.h" @@ -23,19 +23,8 @@ const int32_t CANJaguar::kControllerRate; constexpr double CANJaguar::kApproxBusVoltage; - - -void _sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t *status) -{ - FRC_NetworkCommunication_JaguarCANDriver_sendMessage(messageID,data,dataSize,status); -} -void _receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, uint32_t timeoutMs, int32_t *status) -{ - FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(messageID,data,dataSize,timeoutMs,status); - -} - - +// TODO: Make this a parameter +const int kDefaultCANPeriod = 20; /** * Common initialization code called by all constructors. @@ -54,7 +43,8 @@ void CANJaguar::InitCANJaguar() uint32_t fwVer = GetFirmwareVersion(); if (StatusIsFatal()) return; - // 3330 was the first shipping RDK firmware version for the Jaguar + + // 3330 was the first shipping RDK firmware version for the Jaguar if (fwVer >= 3330 || fwVer < 101) { char buf[256]; @@ -69,6 +59,7 @@ void CANJaguar::InitCANJaguar() wpi_setWPIErrorWithContext(JaguarVersionError, buf); return; } + switch (m_controlMode) { case kPercentVbus: @@ -81,6 +72,26 @@ void CANJaguar::InitCANJaguar() } m_safetyHelper = new MotorSafetyHelper(this); + + m_value = 0.0f; + m_speedReference = kSpeedRef_None; + m_positionReference = kPosRef_None; + m_p = 0.0; + m_i = 0.0; + m_d = 0.0; + m_busVoltage = 0.0f; + m_outputVoltage = 0.0f; + m_outputCurrent = 0.0f; + m_temperature = 0.0f; + m_position = 0.0; + m_speed = 0.0; + m_limits = 0x00; + m_faults = 0x0000; + m_firmwareVersion = 0; + m_hardwareVersion = 0; + + m_verified_values = CAN_EVERYTHING; + HALReport(HALUsageReporting::kResourceType_CANJaguar, m_deviceNumber, m_controlMode); LiveWindow::GetInstance()->AddActuator("CANJaguar", m_deviceNumber, this); } @@ -177,90 +188,6 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup) setTransaction(messageID, dataBuffer, dataSize); if (m_safetyHelper) m_safetyHelper->Feed(); } -void CANJaguar::SetNoAck(float outputValue, uint8_t syncGroup) -{ - uint32_t messageID; - uint8_t dataBuffer[8]; - uint8_t dataSize; - - if (m_safetyHelper && !m_safetyHelper->IsAlive()) - { - EnableControl(); - } - - switch(m_controlMode) - { - case kPercentVbus: - { - messageID = LM_API_VOLT_T_SET_NO_ACK; //LM_API_VOLT_T_SET; - if (outputValue > 1.0) outputValue = 1.0; - if (outputValue < -1.0) outputValue = -1.0; - dataSize = packPercentage(dataBuffer, outputValue); - } - break; - case kSpeed: - { - messageID = LM_API_SPD_T_SET; - dataSize = packFXP16_16(dataBuffer, outputValue); - } - break; - case kPosition: - { - messageID = LM_API_POS_T_SET; - dataSize = packFXP16_16(dataBuffer, outputValue); - } - break; - case kCurrent: - { - messageID = LM_API_ICTRL_T_SET; - dataSize = packFXP8_8(dataBuffer, outputValue); - } - break; - case kVoltage: - { - messageID = LM_API_VCOMP_T_SET_NO_ACK; // LM_API_VCOMP_T_SET; - dataSize = packFXP8_8(dataBuffer, outputValue); - } - break; - default: - return; - } - if (syncGroup != 0) - { - dataBuffer[dataSize] = syncGroup; - dataSize++; - } - { //setTransaction(messageID, dataBuffer, dataSize); - - //uint32_t ackMessageID = LM_API_ACK | m_deviceNumber; - int32_t localStatus = 0; - - // If there was an error on this object and it wasn't a timeout, refuse to talk to the device - // Call ClearError() on the object to try again - //if (StatusIsFatal() && GetError().GetCode() != -44087) - // return; - - // Make sure we don't have more than one transaction with the same Jaguar outstanding. - takeMutex(m_transactionSemaphore); - - // Throw away any stale acks. - //receiveMessage(&ackMessageID, NULL, 0, 0.0f); - - // Send the message with the data. - localStatus = sendMessage(messageID | m_deviceNumber, dataBuffer, dataSize); - wpi_setErrorWithContext(localStatus, "sendMessage"); - - // Wait for an ack. - //localStatus = receiveMessage(&ackMessageID, NULL, 0); - //wpi_setErrorWithContext(localStatus, "receiveMessage"); - - // Transaction complete. - giveMutex(m_transactionSemaphore); - } - - - if (m_safetyHelper) m_safetyHelper->Feed(); -} /** * Get the recently set outputValue setpoint. @@ -282,42 +209,37 @@ float CANJaguar::Get() switch(m_controlMode) { case kPercentVbus: - getTransaction(LM_API_VOLT_SET, dataBuffer, &dataSize); - if (dataSize == sizeof(int16_t)) - { - return unpackPercentage(dataBuffer); - } - break; + if(getTransaction(LM_API_VOLT_SET, dataBuffer, &dataSize)) { + m_value = unpackPercentage(dataBuffer); + } + break; + case kSpeed: - getTransaction(LM_API_SPD_SET, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; + if(getTransaction(LM_API_SPD_SET, dataBuffer, &dataSize)) { + m_value = unpackFXP16_16(dataBuffer); + } + break; + case kPosition: - getTransaction(LM_API_POS_SET, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; + if(getTransaction(LM_API_POS_SET, dataBuffer, &dataSize)) { + m_value = unpackFXP16_16(dataBuffer); + } + break; + case kCurrent: - getTransaction(LM_API_ICTRL_SET, dataBuffer, &dataSize); - if (dataSize == sizeof(int16_t)) - { - return unpackFXP8_8(dataBuffer); - } - break; + if(getTransaction(LM_API_ICTRL_SET, dataBuffer, &dataSize)) { + m_value = unpackFXP8_8(dataBuffer); + } + break; + case kVoltage: - getTransaction(LM_API_VCOMP_SET, dataBuffer, &dataSize); - if (dataSize == sizeof(int16_t)) - { - return unpackFXP8_8(dataBuffer); - } + if(getTransaction(LM_API_VCOMP_SET, dataBuffer, &dataSize)) { + m_value = unpackFXP8_8(dataBuffer); + } break; } - return 0.0; + + return m_value; } /** @@ -428,7 +350,7 @@ int32_t CANJaguar::unpackint32_t(uint8_t *buffer) */ int32_t CANJaguar::sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize) { - static const uint32_t kTrustedMessages[] = { + static const uint32_t kTrustedMessages[] = { LM_API_VOLT_T_EN, LM_API_VOLT_T_SET, LM_API_SPD_T_EN, LM_API_SPD_T_SET, LM_API_VCOMP_T_EN, LM_API_VCOMP_T_SET, LM_API_POS_T_EN, LM_API_POS_T_SET, LM_API_ICTRL_T_EN, LM_API_ICTRL_T_SET}; @@ -452,14 +374,16 @@ int32_t CANJaguar::sendMessage(uint32_t messageID, const uint8_t *data, uint8_t { dataBuffer[j + 2] = data[j]; } -//TODO: put this back when CAN shows up - _sendMessage(messageID, dataBuffer, dataSize + 2, &status); + + FRC_NetworkCommunication_CANSessionMux_sendMessage(messageID, dataBuffer, dataSize + 2, kDefaultCANPeriod, &status); + return status; } } - //TODO: put this back when CAN shows up - _sendMessage(messageID, data, dataSize, &status); - return status; + + FRC_NetworkCommunication_CANSessionMux_sendMessage(messageID, data, dataSize, kDefaultCANPeriod, &status); + + return status; } /** @@ -468,15 +392,15 @@ int32_t CANJaguar::sendMessage(uint32_t messageID, const uint8_t *data, uint8_t * @param messageID The messageID to read from the CAN bus * @param data The up to 8 bytes of data that was received with the message * @param dataSize Indicates how much data was received - * @param timeout Specify how long to wait for a message (in seconds) * @return Status of receive call */ -int32_t CANJaguar::receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, float timeout) +int32_t CANJaguar::receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize) { + uint32_t timeStamp; int32_t status = 0; - //TODO: put this back when CAN shows up - _receiveMessage(messageID, data, dataSize, - (uint32_t)(timeout * 1000), &status); + + FRC_NetworkCommunication_CANSessionMux_receiveMessage(messageID, kFullMessageIDMask, data, dataSize, &timeStamp, &status); + return status; } @@ -492,7 +416,6 @@ int32_t CANJaguar::receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *d */ void CANJaguar::setTransaction(uint32_t messageID, const uint8_t *data, uint8_t dataSize) { - uint32_t ackMessageID = LM_API_ACK | m_deviceNumber; int32_t localStatus = 0; // If there was an error on this object and it wasn't a timeout, refuse to talk to the device @@ -503,14 +426,9 @@ void CANJaguar::setTransaction(uint32_t messageID, const uint8_t *data, uint8_t // Make sure we don't have more than one transaction with the same Jaguar outstanding. takeMutex(m_transactionSemaphore); - // Throw away any stale acks. - receiveMessage(&ackMessageID, NULL, 0, 0.0f); // Send the message with the data. localStatus = sendMessage(messageID | m_deviceNumber, data, dataSize); wpi_setErrorWithContext(localStatus, "sendMessage"); - // Wait for an ack. - localStatus = receiveMessage(&ackMessageID, NULL, 0); - wpi_setErrorWithContext(localStatus, "receiveMessage"); // Transaction complete. giveMutex(m_transactionSemaphore); @@ -524,26 +442,29 @@ void CANJaguar::setTransaction(uint32_t messageID, const uint8_t *data, uint8_t * @param messageID The messageID to read from the CAN bus (device number is added internally) * @param data The up to 8 bytes of data that was received with the message * @param dataSize Indicates how much data was received + * + * @return True if the message was found. Otherwise, no new message is available, + * so a cached value will be used. */ -void CANJaguar::getTransaction(uint32_t messageID, uint8_t *data, uint8_t *dataSize) +bool CANJaguar::getTransaction(uint32_t messageID, uint8_t *data, uint8_t *dataSize) { + uint32_t targetedMessageID = messageID | m_deviceNumber; int32_t localStatus = 0; + bool messageFound = true; - // If there was an error on this object and it wasn't a timeout, refuse to talk to the device - // Call ClearError() on the object to try again - if (StatusIsFatal() && GetError().GetCode() != -44087) + // If there was an error on this object and it wasn't a message not found, + // refuse to talk to the device. Call ClearError() on the object to try again + if (StatusIsFatal() && GetError().GetCode() != ERR_CANSessionMux_MessageNotFound) { if (dataSize != NULL) *dataSize = 0; - return; + return false; } // Make sure we don't have more than one transaction with the same Jaguar outstanding. takeMutex(m_transactionSemaphore); - // Throw away any stale responses. - receiveMessage(&targetedMessageID, NULL, 0, 0.0f); // Send the message requesting data. localStatus = sendMessage(targetedMessageID, NULL, 0); wpi_setErrorWithContext(localStatus, "sendMessage"); @@ -551,10 +472,40 @@ void CANJaguar::getTransaction(uint32_t messageID, uint8_t *data, uint8_t *dataS targetedMessageID &= 0x1FFFFFFF; // Wait for the data. localStatus = receiveMessage(&targetedMessageID, data, dataSize); - wpi_setErrorWithContext(localStatus, "receiveMessage"); + + if(localStatus == ERR_CANSessionMux_MessageNotFound) +{ + messageFound = false; + } else { + wpi_setErrorWithContext(localStatus, "receiveMessage"); + } // Transaction complete. giveMutex(m_transactionSemaphore); + + return messageFound; +} + +/** + * Check if a CAN value has been set but not verified yet. + */ +bool CANJaguar::isUnverified(CANValue value) const +{ + return !(m_verified_values & value); +} + +/** + * Check all unverified values and make sure they're equal to their local + * cached versions. + * + * If a value isn't available, it gets requested. If a value doesn't match up, + * it gets set again. + */ +void CANJaguar::verifyCANValues() +{ + if(isUnverified(CAN_VALUE)) { + + } } /** @@ -582,12 +533,12 @@ CANJaguar::SpeedReference CANJaguar::GetSpeedReference() uint8_t dataBuffer[8]; uint8_t dataSize; - getTransaction(LM_API_SPD_REF, dataBuffer, &dataSize); - if (dataSize == sizeof(uint8_t)) - { - return (SpeedReference)*dataBuffer; - } - return kSpeedRef_None; + if(getTransaction(LM_API_SPD_REF, dataBuffer, &dataSize)) + { + m_speedReference = (SpeedReference)*dataBuffer; + } + + return m_speedReference; } /** @@ -616,12 +567,12 @@ CANJaguar::PositionReference CANJaguar::GetPositionReference() uint8_t dataBuffer[8]; uint8_t dataSize; - getTransaction(LM_API_POS_REF, dataBuffer, &dataSize); - if (dataSize == sizeof(uint8_t)) + if(getTransaction(LM_API_POS_REF, dataBuffer, &dataSize)) { - return (PositionReference)*dataBuffer; + m_positionReference = (PositionReference)*dataBuffer; } - return kPosRef_None; + + return m_positionReference; } /** @@ -686,28 +637,26 @@ double CANJaguar::GetP() wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); break; case kSpeed: - getTransaction(LM_API_SPD_PC, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; + if(getTransaction(LM_API_SPD_PC, dataBuffer, &dataSize)) + { + m_p = unpackFXP16_16(dataBuffer); + } + break; case kPosition: - getTransaction(LM_API_POS_PC, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; + if(getTransaction(LM_API_POS_PC, dataBuffer, &dataSize)) + { + m_p = unpackFXP16_16(dataBuffer); + } + break; case kCurrent: - getTransaction(LM_API_ICTRL_PC, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; + if(getTransaction(LM_API_ICTRL_PC, dataBuffer, &dataSize)) + { + m_p = unpackFXP16_16(dataBuffer); + } + break; } - return 0.0; + + return m_p; } /** @@ -717,38 +666,36 @@ double CANJaguar::GetP() */ double CANJaguar::GetI() { - uint8_t dataBuffer[8]; - uint8_t dataSize; + uint8_t dataBuffer[8]; + uint8_t dataSize; - switch(m_controlMode) - { - case kPercentVbus: - case kVoltage: - wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); - break; - case kSpeed: - getTransaction(LM_API_SPD_IC, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; - case kPosition: - getTransaction(LM_API_POS_IC, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; - case kCurrent: - getTransaction(LM_API_ICTRL_IC, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; - } - return 0.0; + switch(m_controlMode) + { + case kPercentVbus: + case kVoltage: + wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); + break; + case kSpeed: + if(getTransaction(LM_API_SPD_IC, dataBuffer, &dataSize)) + { + m_i = unpackFXP16_16(dataBuffer); + } + break; + case kPosition: + if(getTransaction(LM_API_POS_IC, dataBuffer, &dataSize)) + { + m_i = unpackFXP16_16(dataBuffer); + } + break; + case kCurrent: + if(getTransaction(LM_API_ICTRL_IC, dataBuffer, &dataSize)) + { + m_i = unpackFXP16_16(dataBuffer); + } + break; + } + + return m_i; } /** @@ -758,38 +705,36 @@ double CANJaguar::GetI() */ double CANJaguar::GetD() { - uint8_t dataBuffer[8]; - uint8_t dataSize; + uint8_t dataBuffer[8]; + uint8_t dataSize; - switch(m_controlMode) - { - case kPercentVbus: - case kVoltage: - wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); - break; - case kSpeed: - getTransaction(LM_API_SPD_DC, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; - case kPosition: - getTransaction(LM_API_POS_DC, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; - case kCurrent: - getTransaction(LM_API_ICTRL_DC, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - break; - } - return 0.0; + switch(m_controlMode) + { + case kPercentVbus: + case kVoltage: + wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode"); + break; + case kSpeed: + if(getTransaction(LM_API_SPD_DC, dataBuffer, &dataSize)) + { + m_d = unpackFXP16_16(dataBuffer); + } + break; + case kPosition: + if(getTransaction(LM_API_POS_DC, dataBuffer, &dataSize)) + { + m_d = unpackFXP16_16(dataBuffer); + } + break; + case kCurrent: + if(getTransaction(LM_API_ICTRL_DC, dataBuffer, &dataSize)) + { + m_d = unpackFXP16_16(dataBuffer); + } + break; + } + + return m_d; } /** @@ -889,12 +834,12 @@ CANJaguar::ControlMode CANJaguar::GetControlMode() uint8_t dataBuffer[8]; uint8_t dataSize; - getTransaction(LM_API_STATUS_CMODE, dataBuffer, &dataSize); - if (dataSize == sizeof(int8_t)) - { - return (ControlMode)dataBuffer[0]; - } - return kPercentVbus; + if(getTransaction(LM_API_STATUS_CMODE, dataBuffer, &dataSize)) + { + m_controlMode = (ControlMode)dataBuffer[0]; + } + + return m_controlMode; } /** @@ -907,12 +852,12 @@ float CANJaguar::GetBusVoltage() uint8_t dataBuffer[8]; uint8_t dataSize; - getTransaction(LM_API_STATUS_VOLTBUS, dataBuffer, &dataSize); - if (dataSize == sizeof(int16_t)) - { - return unpackFXP8_8(dataBuffer); - } - return 0.0; + if(getTransaction(LM_API_STATUS_VOLTBUS, dataBuffer, &dataSize)) + { + m_busVoltage = unpackFXP8_8(dataBuffer); + } + + return m_busVoltage; } /** @@ -922,16 +867,15 @@ float CANJaguar::GetBusVoltage() */ float CANJaguar::GetOutputVoltage() { - uint8_t dataBuffer[8]; - uint8_t dataSize; + uint8_t dataBuffer[8]; + uint8_t dataSize; - // Read the volt out which is in Volts units. - getTransaction(LM_API_STATUS_VOUT, dataBuffer, &dataSize); - if (dataSize == sizeof(int16_t)) - { - return unpackFXP8_8(dataBuffer); - } - return 0.0; + if(getTransaction(LM_API_STATUS_VOUT, dataBuffer, &dataSize)) + { + m_outputVoltage = unpackFXP8_8(dataBuffer); + } + + return m_outputVoltage; } /** @@ -941,15 +885,15 @@ float CANJaguar::GetOutputVoltage() */ float CANJaguar::GetOutputCurrent() { - uint8_t dataBuffer[8]; - uint8_t dataSize; + uint8_t dataBuffer[8]; + uint8_t dataSize; - getTransaction(LM_API_STATUS_CURRENT, dataBuffer, &dataSize); - if (dataSize == sizeof(int16_t)) - { - return unpackFXP8_8(dataBuffer); - } - return 0.0; + if(getTransaction(LM_API_STATUS_CURRENT, dataBuffer, &dataSize)) + { + m_outputCurrent = unpackFXP8_8(dataBuffer); + } + + return m_outputCurrent; } /** @@ -959,15 +903,15 @@ float CANJaguar::GetOutputCurrent() */ float CANJaguar::GetTemperature() { - uint8_t dataBuffer[8]; - uint8_t dataSize; + uint8_t dataBuffer[8]; + uint8_t dataSize; - getTransaction(LM_API_STATUS_TEMP, dataBuffer, &dataSize); - if (dataSize == sizeof(int16_t)) - { - return unpackFXP8_8(dataBuffer); - } - return 0.0; + if(getTransaction(LM_API_STATUS_TEMP, dataBuffer, &dataSize)) + { + m_temperature = unpackFXP8_8(dataBuffer); + } + + return m_temperature; } /** @@ -980,12 +924,12 @@ double CANJaguar::GetPosition() uint8_t dataBuffer[8]; uint8_t dataSize; - getTransaction(LM_API_STATUS_POS, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - return 0.0; + if(getTransaction(LM_API_STATUS_POS, dataBuffer, &dataSize)) + { + m_position = unpackFXP16_16(dataBuffer); + } + + return m_position; } /** @@ -998,12 +942,12 @@ double CANJaguar::GetSpeed() uint8_t dataBuffer[8]; uint8_t dataSize; - getTransaction(LM_API_STATUS_SPD, dataBuffer, &dataSize); - if (dataSize == sizeof(int32_t)) - { - return unpackFXP16_16(dataBuffer); - } - return 0.0; + if(getTransaction(LM_API_STATUS_SPD, dataBuffer, &dataSize)) + { + m_speed = unpackFXP16_16(dataBuffer); + } + + return m_speed; } /** @@ -1016,12 +960,12 @@ bool CANJaguar::GetForwardLimitOK() uint8_t dataBuffer[8]; uint8_t dataSize; - getTransaction(LM_API_STATUS_LIMIT, dataBuffer, &dataSize); - if (dataSize == sizeof(uint8_t)) - { - return (*dataBuffer & kForwardLimit) != 0; - } - return 0; + if(getTransaction(LM_API_STATUS_LIMIT, dataBuffer, &dataSize)) +{ + m_limits = dataBuffer[0]; + } + + return m_limits & kForwardLimit; } /** @@ -1031,15 +975,15 @@ bool CANJaguar::GetForwardLimitOK() */ bool CANJaguar::GetReverseLimitOK() { - uint8_t dataBuffer[8]; - uint8_t dataSize; + uint8_t dataBuffer[8]; + uint8_t dataSize; - getTransaction(LM_API_STATUS_LIMIT, dataBuffer, &dataSize); - if (dataSize == sizeof(uint8_t)) - { - return (*dataBuffer & kReverseLimit) != 0; - } - return 0; + if(getTransaction(LM_API_STATUS_LIMIT, dataBuffer, &dataSize)) +{ + m_limits = dataBuffer[0]; + } + + return m_limits & kReverseLimit; } /** @@ -1052,14 +996,15 @@ uint16_t CANJaguar::GetFaults() uint8_t dataBuffer[8]; uint8_t dataSize; - getTransaction(LM_API_STATUS_FAULT, dataBuffer, &dataSize); - if (dataSize == sizeof(uint16_t)) - { - return unpackint16_t(dataBuffer); - } - return 0; + if(getTransaction(LM_API_STATUS_FAULT, dataBuffer, &dataSize)) + { + m_faults = unpackint16_t(dataBuffer);; + } + + return m_faults; } +#if 0 /** * Check if the Jaguar's power has been cycled since this was last called. * @@ -1089,6 +1034,7 @@ bool CANJaguar::GetPowerCycled() } return 0; } +#endif /** * Set the maximum voltage change rate. @@ -1125,16 +1071,16 @@ void CANJaguar::SetVoltageRampRate(double rampRate) */ uint32_t CANJaguar::GetFirmwareVersion() { - uint8_t dataBuffer[8]; + uint8_t dataBuffer[8]; uint8_t dataSize; - // Set the MSB to tell the 2CAN that this is a remote message. - getTransaction(0x80000000 | CAN_MSGID_API_FIRMVER, dataBuffer, &dataSize); - if (dataSize == sizeof(uint32_t)) - { - return unpackint32_t(dataBuffer); - } - return 0; + // Set the MSB to tell the 2CAN that this is a remote message. + if(getTransaction(0x80000000 | CAN_MSGID_API_FIRMVER, dataBuffer, &dataSize)) + { + m_firmwareVersion = unpackint32_t(dataBuffer); + } + + return m_firmwareVersion; } /** @@ -1147,16 +1093,13 @@ uint8_t CANJaguar::GetHardwareVersion() uint8_t dataBuffer[8]; uint8_t dataSize; - getTransaction(LM_API_HWVER, dataBuffer, &dataSize); - if (dataSize == sizeof(uint8_t)+sizeof(uint8_t)) - { - if (*dataBuffer == m_deviceNumber) - { - return *(dataBuffer+1); - } - } - // Assume Gray Jag if there is no response - return LM_HWVER_JAG_1_0; + // Only get once, since this shoudn't change. + if(!m_hardwareVersion && getTransaction(LM_API_HWVER, dataBuffer, &dataSize)) + { + m_hardwareVersion = *(dataBuffer+1); + } + + return m_hardwareVersion; } /** @@ -1337,37 +1280,47 @@ void CANJaguar::StopMotor() DisableControl(); } -void CANJaguar::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) { +void CANJaguar::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) +{ Set(value.f); } -void CANJaguar::UpdateTable() { - if (m_table != NULL) { +void CANJaguar::UpdateTable() +{ + if (m_table != NULL) +{ m_table->PutNumber("Value", Get()); } } -void CANJaguar::StartLiveWindowMode() { - if (m_table != NULL) { +void CANJaguar::StartLiveWindowMode() +{ + if (m_table != NULL) +{ m_table->AddTableListener("Value", this, true); } } -void CANJaguar::StopLiveWindowMode() { - if (m_table != NULL) { +void CANJaguar::StopLiveWindowMode() +{ + if (m_table != NULL) +{ m_table->RemoveTableListener(this); } } -std::string CANJaguar::GetSmartDashboardType() { +std::string CANJaguar::GetSmartDashboardType() +{ return "Speed Controller"; } -void CANJaguar::InitTable(ITable *subTable) { +void CANJaguar::InitTable(ITable *subTable) +{ m_table = subTable; UpdateTable(); } -ITable * CANJaguar::GetTable() { +ITable * CANJaguar::GetTable() +{ return m_table; } diff --git a/wpilibc/wpilibC++IntegrationTests/include/TestBench.h b/wpilibc/wpilibC++IntegrationTests/include/TestBench.h index d977d355fe..66ad4208e4 100644 --- a/wpilibc/wpilibC++IntegrationTests/include/TestBench.h +++ b/wpilibc/wpilibC++IntegrationTests/include/TestBench.h @@ -42,4 +42,7 @@ public: static const uint32_t kFakePressureSwitchChannel = 11; static const uint32_t kFakeSolenoid1Channel = 12; static const uint32_t kFakeSolenoid2Channel = 13; + + /* CAN IDs */ + static const uint32_t kCANJaguarID = 1; }; diff --git a/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp b/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp new file mode 100644 index 0000000000..3de9bbb1e0 --- /dev/null +++ b/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2014. 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 the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "WPILib.h" +#include "gtest/gtest.h" +#include "TestBench.h" + +class CANJaguarTest : public testing::Test { +protected: + CANJaguar *m_jaguar; + + virtual void SetUp() { + Wait(0.04); + m_jaguar = new CANJaguar(TestBench::kCANJaguarID); + } + + virtual void TearDown() { + delete m_jaguar; + } + + void Reset() { + m_jaguar->ChangeControlMode(CANJaguar::kPercentVbus); + m_jaguar->Set(0.0f); + } +}; + +#if 0 +TEST_F(CANJaguarTest, Speed) { + Reset(); + + for(;;) { + m_jaguar->Set(1.0f); + + std::cout << "Temperature = " << m_jaguar->GetTemperature() << std::endl; + std::cout << "Current = " << m_jaguar->GetOutputCurrent() << std::endl; + + Wait(0.1); + } +} +#endif