From b0369342e98de601044ca9b6626a4b04ef93b78f Mon Sep 17 00:00:00 2001 From: thomasclark Date: Wed, 23 Jul 2014 15:22:26 -0400 Subject: [PATCH] Fixed some bugs with CANJaguar verification in C++ Change-Id: I3f17c090e26c6019523eb92eb47714464aa01baf --- wpilibc/wpilibC++/lib/CANJaguar.cpp | 117 ++++++++++++++++++++++------ 1 file changed, 95 insertions(+), 22 deletions(-) diff --git a/wpilibc/wpilibC++/lib/CANJaguar.cpp b/wpilibc/wpilibC++/lib/CANJaguar.cpp index 560fbd5173..737e942129 100644 --- a/wpilibc/wpilibC++/lib/CANJaguar.cpp +++ b/wpilibc/wpilibC++/lib/CANJaguar.cpp @@ -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);