Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge "Fixed some bugs with CANJaguar verification in C++"

Browse Source
This commit is contained in:
Jonathan Leitschuh (WPI)
2014-07-23 12:48:17 -07:00
committed by Gerrit Code Review
parent 01ca19f78f b0369342e9
commit c3d1e80a62
1 changed files with 95 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
wpilibc/wpilibC++/lib/CANJaguar.cpp
View File

@@ -22,7 +22,7 @@
/* Compare floats for equality as fixed point numbers */
#define FXP8_EQ(a,b) ((int16_t)((a)*256.0)==(int16_t)((b)*256.0))
#define FXP16_EQ(a,b) ((int16_t)((a)*65536.0)==(int16_t)((b)*65536.0))
#define FXP16_EQ(a,b) ((int32_t)((a)*65536.0)==(int32_t)((b)*65536.0))
const int32_t CANJaguar::kControllerRate;
constexpr double CANJaguar::kApproxBusVoltage;
@@ -622,31 +622,64 @@ void CANJaguar::verify()
dataBuffer[0] = 1;
sendMessage(LM_API_STATUS_POWER, dataBuffer, sizeof(uint8_t));
// Set all configuration data again. This will resend it to the
// Jaguar and mark everything as unverified.
EnableControl();
SetSpeedReference(m_speedReference);
SetPositionReference(m_positionReference);
ConfigNeutralMode(m_neutralMode);
ConfigEncoderCodesPerRev(m_encoderCodesPerRev);
ConfigPotentiometerTurns(m_potentiometerTurns);
// Mark everything as unverified
m_controlModeVerified = false;
m_speedRefVerified = false;
m_posRefVerified = false;
m_neutralModeVerified = false;
m_encoderCodesPerRevVerified = false;
m_potentiometerTurnsVerified = false;
m_forwardLimitVerified = false;
m_reverseLimitVerified = false;
m_limitModeVerified = false;
m_maxOutputVoltageVerified = false;
m_faultTimeVerified = false;
if(m_controlMode == kCurrent || m_controlMode == kSpeed || m_controlMode == kPosition)
SetPID(m_p, m_i, m_d);
if(m_limitMode == kLimitMode_SoftPositionLimits)
ConfigSoftPositionLimits(m_forwardLimit, m_reverseLimit);
if(m_controlMode == kPercentVbus || m_controlMode == kVoltage)
{
m_voltageRampRateVerified = false;
}
else
DisableSoftPositionLimits();
{
m_pVerified = false;
m_iVerified = false;
m_dVerified = false;
}
ConfigMaxOutputVoltage(m_maxOutputVoltage);
// Verify periodic status messages again
m_receivedStatusMessage0 = false;
m_receivedStatusMessage1 = false;
m_receivedStatusMessage2 = false;
if(m_controlMode == kVoltage || m_controlMode == kPercentVbus)
SetVoltageRampRate(m_voltageRampRate);
setupPeriodicStatus();
// Remove any old values from netcomms. Otherwise, parameters are
// incorrectly marked as verified based on stale messages.
getMessage(LM_API_SPD_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_POS_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_SPD_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_POS_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_ICTRL_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_SPD_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_POS_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_ICTRL_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_SPD_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_POS_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_ICTRL_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_CFG_BRAKE_COAST, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_CFG_ENC_LINES, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_CFG_POT_TURNS, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_CFG_LIMIT_MODE, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_CFG_LIMIT_FWD, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_CFG_LIMIT_REV, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_CFG_MAX_VOUT, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_VOLT_SET_RAMP, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_VCOMP_COMP_RAMP, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
getMessage(LM_API_CFG_FAULT_TIME, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
}
}
else
{
requestMessage(LM_API_STATUS_POWER);
}
// Verify that any recently set parameters are correct
if(!m_controlModeVerified)
@@ -914,9 +947,12 @@ void CANJaguar::verify()
{
if(getMessage(LM_API_CFG_MAX_VOUT, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
{
double voltage = unpackFXP16_16(dataBuffer);
double voltage = unpackFXP8_8(dataBuffer);
if(FXP16_EQ(voltage, m_maxOutputVoltage))
// The returned max output voltage is sometimes slightly higher or
// lower than what was sent. This should not trigger resending
// the message.
if(std::abs(voltage - m_maxOutputVoltage) < 0.1)
m_maxOutputVoltageVerified = true;
else
{
@@ -974,6 +1010,40 @@ void CANJaguar::verify()
}
}
}
if(!m_faultTimeVerified)
{
if(getMessage(LM_API_CFG_FAULT_TIME, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
{
uint16_t faultTime = unpackint16_t(dataBuffer);
if((uint16_t)(m_faultTime * 1000.0) == faultTime)
m_faultTimeVerified = true;
else
{
// It's wrong - set it again
ConfigFaultTime(m_faultTime);
}
}
else
{
// Verification is needed but not available - request it again.
requestMessage(LM_API_CFG_FAULT_TIME);
}
}
if(!m_receivedStatusMessage0 ||
!m_receivedStatusMessage1 ||
!m_receivedStatusMessage2)
{
// If the periodic status messages haven't been verified as received,
// request periodic status messages again and attempt to unpack any
// available ones.
setupPeriodicStatus();
GetTemperature();
GetPosition();
GetFaults();
}
}
/**
@@ -1903,6 +1973,9 @@ void CANJaguar::ConfigFaultTime(float faultTime)
uint8_t dataBuffer[8];
uint8_t dataSize;
if(faultTime < 0.5) faultTime = 0.5;
else if(faultTime > 3.0) faultTime = 3.0;
// Message takes ms
dataSize = packint16_t(dataBuffer, (int16_t)(faultTime * 1000.0));
sendMessage(LM_API_CFG_FAULT_TIME, dataBuffer, dataSize);