9 #include "ChipObject/NiFpga.h"
10 #include "CAN/JaguarCANDriver.h"
11 #include "CAN/can_proto.h"
12 #include "NetworkCommunication/UsageReporting.h"
13 #include "WPIErrors.h"
15 #include "LiveWindow/LiveWindow.h"
17 #define swap16(x) ( (((x)>>8) &0x00FF) \
18 | (((x)<<8) &0xFF00) )
19 #define swap32(x) ( (((x)>>24)&0x000000FF) \
20 | (((x)>>8) &0x0000FF00) \
21 | (((x)<<8) &0x00FF0000) \
22 | (((x)<<24)&0xFF000000) )
24 #define kFullMessageIDMask (CAN_MSGID_API_M | CAN_MSGID_MFR_M | CAN_MSGID_DTYPE_M)
26 const int32_t CANJaguar::kControllerRate;
27 constexpr
double CANJaguar::kApproxBusVoltage;
32 void CANJaguar::InitCANJaguar()
35 m_transactionSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE | SEM_DELETE_SAFE);
36 if (m_deviceNumber < 1 || m_deviceNumber > 63)
39 snprintf(buf, 256,
"device number \"%d\" must be between 1 and 63", m_deviceNumber);
40 wpi_setWPIErrorWithContext(ParameterOutOfRange, buf);
47 if (fwVer >= 3330 || fwVer < 101)
52 snprintf(buf, 256,
"Jag #%d firmware (%d) is too old (must be at least version 101 of the FIRST approved firmware)", m_deviceNumber, fwVer);
56 snprintf(buf, 256,
"Jag #%d firmware (%d) is not FIRST approved (must be at least version 101 of the FIRST approved firmware)", m_deviceNumber, fwVer);
58 wpi_setWPIErrorWithContext(JaguarVersionError, buf);
61 switch (m_controlMode)
73 nUsageReporting::report(nUsageReporting::kResourceType_CANJaguar, m_deviceNumber, m_controlMode);
83 : m_deviceNumber (deviceNumber)
84 , m_controlMode (controlMode)
85 , m_transactionSemaphore (NULL)
86 , m_maxOutputVoltage (kApproxBusVoltage)
87 , m_safetyHelper (NULL)
92 CANJaguar::~CANJaguar()
94 delete m_safetyHelper;
95 m_safetyHelper = NULL;
96 semDelete(m_transactionSemaphore);
97 m_transactionSemaphore = NULL;
116 uint8_t dataBuffer[8];
119 if (m_safetyHelper && !m_safetyHelper->
IsAlive())
124 switch(m_controlMode)
128 messageID = LM_API_VOLT_T_SET;
129 if (outputValue > 1.0) outputValue = 1.0;
130 if (outputValue < -1.0) outputValue = -1.0;
131 dataSize = packPercentage(dataBuffer, outputValue);
136 messageID = LM_API_SPD_T_SET;
137 dataSize = packFXP16_16(dataBuffer, outputValue);
142 messageID = LM_API_POS_T_SET;
143 dataSize = packFXP16_16(dataBuffer, outputValue);
148 messageID = LM_API_ICTRL_T_SET;
149 dataSize = packFXP8_8(dataBuffer, outputValue);
154 messageID = LM_API_VCOMP_T_SET;
155 dataSize = packFXP8_8(dataBuffer, outputValue);
163 dataBuffer[dataSize] = syncGroup;
167 if (m_safetyHelper) m_safetyHelper->Feed();
184 uint8_t dataBuffer[8];
187 switch(m_controlMode)
191 if (dataSize ==
sizeof(int16_t))
193 return unpackPercentage(dataBuffer);
198 if (dataSize ==
sizeof(int32_t))
200 return unpackFXP16_16(dataBuffer);
205 if (dataSize ==
sizeof(int32_t))
207 return unpackFXP16_16(dataBuffer);
212 if (dataSize ==
sizeof(int16_t))
214 return unpackFXP8_8(dataBuffer);
219 if (dataSize ==
sizeof(int16_t))
221 return unpackFXP8_8(dataBuffer);
247 if (m_controlMode == kPercentVbus)
253 wpi_setWPIErrorWithContext(IncompatibleMode,
"PID only supported in PercentVbus mode");
257 uint8_t CANJaguar::packPercentage(uint8_t *buffer,
double value)
259 int16_t intValue = (int16_t)(value * 32767.0);
260 *((int16_t*)buffer) = swap16(intValue);
261 return sizeof(int16_t);
264 uint8_t CANJaguar::packFXP8_8(uint8_t *buffer,
double value)
266 int16_t intValue = (int16_t)(value * 256.0);
267 *((int16_t*)buffer) = swap16(intValue);
268 return sizeof(int16_t);
271 uint8_t CANJaguar::packFXP16_16(uint8_t *buffer,
double value)
273 int32_t intValue = (int32_t)(value * 65536.0);
274 *((int32_t*)buffer) = swap32(intValue);
275 return sizeof(int32_t);
278 uint8_t CANJaguar::packint16_t(uint8_t *buffer, int16_t value)
280 *((int16_t*)buffer) = swap16(value);
281 return sizeof(int16_t);
284 uint8_t CANJaguar::packint32_t(uint8_t *buffer, int32_t value)
286 *((int32_t*)buffer) = swap32(value);
287 return sizeof(int32_t);
290 double CANJaguar::unpackPercentage(uint8_t *buffer)
292 int16_t value = *((int16_t*)buffer);
293 value = swap16(value);
294 return value / 32767.0;
297 double CANJaguar::unpackFXP8_8(uint8_t *buffer)
299 int16_t value = *((int16_t*)buffer);
300 value = swap16(value);
301 return value / 256.0;
304 double CANJaguar::unpackFXP16_16(uint8_t *buffer)
306 int32_t value = *((int32_t*)buffer);
307 value = swap32(value);
308 return value / 65536.0;
311 int16_t CANJaguar::unpackint16_t(uint8_t *buffer)
313 int16_t value = *((int16_t*)buffer);
314 return swap16(value);
317 int32_t CANJaguar::unpackint32_t(uint8_t *buffer)
319 int32_t value = *((int32_t*)buffer);
320 return swap32(value);
336 static const uint32_t kTrustedMessages[] = {
337 LM_API_VOLT_T_EN, LM_API_VOLT_T_SET, LM_API_SPD_T_EN, LM_API_SPD_T_SET,
338 LM_API_VCOMP_T_EN, LM_API_VCOMP_T_SET, LM_API_POS_T_EN, LM_API_POS_T_SET,
339 LM_API_ICTRL_T_EN, LM_API_ICTRL_T_SET};
342 for (uint8_t i=0; i<(
sizeof(kTrustedMessages)/
sizeof(kTrustedMessages[0])); i++)
344 if ((kFullMessageIDMask & messageID) == kTrustedMessages[i])
346 uint8_t dataBuffer[8];
353 wpi_setGlobalWPIErrorWithContext(ParameterOutOfRange,
"dataSize > 6");
356 for (uint8_t j=0; j < dataSize; j++)
358 dataBuffer[j + 2] = data[j];
360 FRC_NetworkCommunication_JaguarCANDriver_sendMessage(messageID, dataBuffer, dataSize + 2, &status);
364 FRC_NetworkCommunication_JaguarCANDriver_sendMessage(messageID, data, dataSize, &status);
380 FRC_NetworkCommunication_JaguarCANDriver_receiveMessage(messageID, data, dataSize,
381 (uint32_t)(timeout * 1000), &status);
397 uint32_t ackMessageID = LM_API_ACK | m_deviceNumber;
398 int32_t localStatus = 0;
406 semTake(m_transactionSemaphore, WAIT_FOREVER);
411 localStatus =
sendMessage(messageID | m_deviceNumber, data, dataSize);
412 wpi_setErrorWithContext(localStatus,
"sendMessage");
415 wpi_setErrorWithContext(localStatus,
"receiveMessage");
418 semGive(m_transactionSemaphore);
432 uint32_t targetedMessageID = messageID | m_deviceNumber;
433 int32_t localStatus = 0;
439 if (dataSize != NULL)
445 semTake(m_transactionSemaphore, WAIT_FOREVER);
450 localStatus =
sendMessage(targetedMessageID, NULL, 0);
451 wpi_setErrorWithContext(localStatus,
"sendMessage");
453 targetedMessageID &= 0x1FFFFFFF;
456 wpi_setErrorWithContext(localStatus,
"receiveMessage");
459 semGive(m_transactionSemaphore);
471 uint8_t dataBuffer[8];
473 dataBuffer[0] = reference;
484 uint8_t dataBuffer[8];
488 if (dataSize ==
sizeof(uint8_t))
490 return (SpeedReference)*dataBuffer;
492 return kSpeedRef_None;
505 uint8_t dataBuffer[8];
507 dataBuffer[0] = reference;
518 uint8_t dataBuffer[8];
522 if (dataSize ==
sizeof(uint8_t))
524 return (PositionReference)*dataBuffer;
538 uint8_t dataBuffer[8];
541 switch(m_controlMode)
545 wpi_setWPIErrorWithContext(IncompatibleMode,
"PID constants only apply in Speed, Position, and Current mode");
548 dataSize = packFXP16_16(dataBuffer, p);
550 dataSize = packFXP16_16(dataBuffer, i);
552 dataSize = packFXP16_16(dataBuffer, d);
556 dataSize = packFXP16_16(dataBuffer, p);
558 dataSize = packFXP16_16(dataBuffer, i);
560 dataSize = packFXP16_16(dataBuffer, d);
564 dataSize = packFXP16_16(dataBuffer, p);
566 dataSize = packFXP16_16(dataBuffer, i);
568 dataSize = packFXP16_16(dataBuffer, d);
581 uint8_t dataBuffer[8];
584 switch(m_controlMode)
588 wpi_setWPIErrorWithContext(IncompatibleMode,
"PID constants only apply in Speed, Position, and Current mode");
592 if (dataSize ==
sizeof(int32_t))
594 return unpackFXP16_16(dataBuffer);
599 if (dataSize ==
sizeof(int32_t))
601 return unpackFXP16_16(dataBuffer);
606 if (dataSize ==
sizeof(int32_t))
608 return unpackFXP16_16(dataBuffer);
622 uint8_t dataBuffer[8];
625 switch(m_controlMode)
629 wpi_setWPIErrorWithContext(IncompatibleMode,
"PID constants only apply in Speed, Position, and Current mode");
633 if (dataSize ==
sizeof(int32_t))
635 return unpackFXP16_16(dataBuffer);
640 if (dataSize ==
sizeof(int32_t))
642 return unpackFXP16_16(dataBuffer);
647 if (dataSize ==
sizeof(int32_t))
649 return unpackFXP16_16(dataBuffer);
663 uint8_t dataBuffer[8];
666 switch(m_controlMode)
670 wpi_setWPIErrorWithContext(IncompatibleMode,
"PID constants only apply in Speed, Position, and Current mode");
674 if (dataSize ==
sizeof(int32_t))
676 return unpackFXP16_16(dataBuffer);
681 if (dataSize ==
sizeof(int32_t))
683 return unpackFXP16_16(dataBuffer);
688 if (dataSize ==
sizeof(int32_t))
690 return unpackFXP16_16(dataBuffer);
709 uint8_t dataBuffer[8];
710 uint8_t dataSize = 0;
712 switch(m_controlMode)
721 dataSize = packFXP16_16(dataBuffer, encoderInitialPosition);
740 uint8_t dataBuffer[8];
741 uint8_t dataSize = 0;
743 switch(m_controlMode)
777 m_controlMode = controlMode;
779 nUsageReporting::report(nUsageReporting::kResourceType_CANJaguar, m_deviceNumber, m_controlMode);
791 uint8_t dataBuffer[8];
795 if (dataSize ==
sizeof(int8_t))
797 return (ControlMode)dataBuffer[0];
809 uint8_t dataBuffer[8];
813 if (dataSize ==
sizeof(int16_t))
815 return unpackFXP8_8(dataBuffer);
827 uint8_t dataBuffer[8];
832 if (dataSize ==
sizeof(int16_t))
834 return unpackFXP8_8(dataBuffer);
846 uint8_t dataBuffer[8];
850 if (dataSize ==
sizeof(int16_t))
852 return unpackFXP8_8(dataBuffer);
864 uint8_t dataBuffer[8];
868 if (dataSize ==
sizeof(int16_t))
870 return unpackFXP8_8(dataBuffer);
882 uint8_t dataBuffer[8];
886 if (dataSize ==
sizeof(int32_t))
888 return unpackFXP16_16(dataBuffer);
900 uint8_t dataBuffer[8];
904 if (dataSize ==
sizeof(int32_t))
906 return unpackFXP16_16(dataBuffer);
918 uint8_t dataBuffer[8];
922 if (dataSize ==
sizeof(uint8_t))
924 return (*dataBuffer & kForwardLimit) != 0;
936 uint8_t dataBuffer[8];
940 if (dataSize ==
sizeof(uint8_t))
942 return (*dataBuffer & kReverseLimit) != 0;
954 uint8_t dataBuffer[8];
958 if (dataSize ==
sizeof(uint16_t))
960 return unpackint16_t(dataBuffer);
975 uint8_t dataBuffer[8];
979 if (dataSize ==
sizeof(uint8_t))
981 bool powerCycled = (*dataBuffer != 0);
1005 uint8_t dataBuffer[8];
1008 switch(m_controlMode)
1011 dataSize = packPercentage(dataBuffer, rampRate / (m_maxOutputVoltage * kControllerRate));
1015 dataSize = packFXP8_8(dataBuffer, rampRate / kControllerRate);
1030 uint8_t dataBuffer[8];
1034 getTransaction(0x80000000 | CAN_MSGID_API_FIRMVER, dataBuffer, &dataSize);
1035 if (dataSize ==
sizeof(uint32_t))
1037 return unpackint32_t(dataBuffer);
1049 uint8_t dataBuffer[8];
1053 if (dataSize ==
sizeof(uint8_t)+
sizeof(uint8_t))
1055 if (*dataBuffer == m_deviceNumber)
1057 return *(dataBuffer+1);
1061 return LM_HWVER_JAG_1_0;
1073 uint8_t dataBuffer[8];
1075 dataBuffer[0] = mode;
1076 setTransaction(LM_API_CFG_BRAKE_COAST, dataBuffer,
sizeof(uint8_t));
1086 uint8_t dataBuffer[8];
1089 dataSize = packint16_t(dataBuffer, codesPerRev);
1103 uint8_t dataBuffer[8];
1106 dataSize = packint16_t(dataBuffer, turns);
1122 uint8_t dataBuffer[8];
1125 dataSize = packFXP16_16(dataBuffer, forwardLimitPosition);
1126 dataBuffer[dataSize++] = forwardLimitPosition > reverseLimitPosition;
1129 dataSize = packFXP16_16(dataBuffer, reverseLimitPosition);
1130 dataBuffer[dataSize++] = forwardLimitPosition <= reverseLimitPosition;
1133 dataBuffer[0] = kLimitMode_SoftPositionLimits;
1134 setTransaction(LM_API_CFG_LIMIT_MODE, dataBuffer,
sizeof(uint8_t));
1144 uint8_t dataBuffer[8];
1146 dataBuffer[0] = kLimitMode_SwitchInputsOnly;
1147 setTransaction(LM_API_CFG_LIMIT_MODE, dataBuffer,
sizeof(uint8_t));
1160 uint8_t dataBuffer[8];
1163 m_maxOutputVoltage = voltage;
1164 dataSize = packFXP8_8(dataBuffer, voltage);
1178 uint8_t dataBuffer[8];
1182 dataSize = packint16_t(dataBuffer, (int16_t)(faultTime * 1000.0));
1193 sendMessage(CAN_MSGID_API_SYNC, &syncGroup,
sizeof(syncGroup));
1197 void CANJaguar::SetExpiration(
float timeout)
1199 if (m_safetyHelper) m_safetyHelper->SetExpiration(timeout);
1202 float CANJaguar::GetExpiration()
1204 if (!m_safetyHelper)
return 0.0;
1208 bool CANJaguar::IsAlive()
1210 if (!m_safetyHelper)
return false;
1211 return m_safetyHelper->
IsAlive();
1214 bool CANJaguar::IsSafetyEnabled()
1216 if (!m_safetyHelper)
return false;
1220 void CANJaguar::SetSafetyEnabled(
bool enabled)
1225 void CANJaguar::GetDescription(
char *desc)
1227 sprintf(desc,
"CANJaguar ID %d", m_deviceNumber);
1246 if (m_table != NULL) {
1252 if (m_table != NULL) {
1258 if (m_table != NULL) {
1264 return "Speed Controller";
void SetSpeedReference(SpeedReference reference)
void InitTable(ITable *subTable)
virtual void RemoveTableListener(ITableListener *listener)=0
void ConfigFaultTime(float faultTime)
void AddActuator(const char *subsystem, const char *name, LiveWindowSendable *component)
virtual void PutNumber(std::string key, double value)=0
void SetPID(double p, double i, double d)
void ValueChanged(ITable *source, const std::string &key, EntryValue value, bool isNew)
static int32_t sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize)
virtual void AddTableListener(ITableListener *listener)=0
void ConfigNeutralMode(NeutralMode mode)
void ConfigPotentiometerTurns(uint16_t turns)
void SetSafetyEnabled(bool enabled)
void SetVoltageRampRate(double rampRate)
virtual void getTransaction(uint32_t messageID, uint8_t *data, uint8_t *dataSize)
SpeedReference GetSpeedReference()
virtual Error & GetError()
Retrieve the current error. Get the current error information associated with this sensor...
void ConfigMaxOutputVoltage(double voltage)
void ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition)
void SetPositionReference(PositionReference reference)
void StopLiveWindowMode()
CANJaguar(uint8_t deviceNumber, ControlMode controlMode=kPercentVbus)
static LiveWindow * GetInstance()
ControlMode GetControlMode()
static int32_t receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize, float timeout=0.02)
virtual void Set(float value, uint8_t syncGroup=0)
void ChangeControlMode(ControlMode controlMode)
virtual void PIDWrite(float output)
virtual bool StatusIsFatal() const
Check if the current error code represents a fatal error.
void ConfigEncoderCodesPerRev(uint16_t codesPerRev)
virtual void setTransaction(uint32_t messageID, const uint8_t *data, uint8_t dataSize)
virtual uint32_t GetFirmwareVersion()
uint8_t GetHardwareVersion()
std::string GetSmartDashboardType()
PositionReference GetPositionReference()
void StartLiveWindowMode()
static void UpdateSyncGroup(uint8_t syncGroup)
void EnableControl(double encoderInitialPosition=0.0)
void DisableSoftPositionLimits()