diff --git a/wpilibc/wpilibC++/include/CANJaguar.h b/wpilibc/wpilibC++/include/CANJaguar.h index f6db4a47ac..f5f2e1e988 100644 --- a/wpilibc/wpilibC++/include/CANJaguar.h +++ b/wpilibc/wpilibC++/include/CANJaguar.h @@ -196,6 +196,16 @@ protected: uint32_t m_firmwareVersion; uint8_t m_hardwareVersion; + // Which status values have we received at least once? + bool m_receivedBusVoltage; + bool m_receivedOutputVoltage; + bool m_receivedOutputCurrent; + bool m_receivedTemperature; + bool m_receivedPosition; + bool m_receivedSpeed; + bool m_receivedLimits; + bool m_receivedFaults; + void verify(); MotorSafetyHelper *m_safetyHelper; diff --git a/wpilibc/wpilibC++/lib/CANJaguar.cpp b/wpilibc/wpilibC++/lib/CANJaguar.cpp index 7b2dc7b9b7..9c8d01be39 100644 --- a/wpilibc/wpilibC++/lib/CANJaguar.cpp +++ b/wpilibc/wpilibC++/lib/CANJaguar.cpp @@ -30,6 +30,8 @@ constexpr double CANJaguar::kApproxBusVoltage; static const int32_t kSendMessagePeriod = 20; static const uint32_t kFullMessageIDMask = (CAN_MSGID_API_M | CAN_MSGID_MFR_M | CAN_MSGID_DTYPE_M); +static const int32_t kReceiveStatusAttempts = 5000; + static int32_t sendMessageHelper(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t period) { static const uint32_t kTrustedMessages[] = { @@ -135,7 +137,41 @@ void CANJaguar::InitCANJaguar() requestMessage(CAN_IS_FRAME_REMOTE | CAN_MSGID_API_FIRMVER); requestMessage(LM_API_HWVER); - Wait(0.003); + m_receivedBusVoltage = false; + m_receivedOutputVoltage = false; + m_receivedOutputCurrent = false; + m_receivedTemperature = false; + m_receivedPosition = false; + m_receivedSpeed = false; + m_receivedLimits = false; + m_receivedFaults = false; + + // Wait until we've gotten all of the status data at least once. + for(int i = 0; i < kReceiveStatusAttempts; i++) + { + Wait(0.001); + + GetBusVoltage(); + GetOutputVoltage(); + GetOutputCurrent(); + GetTemperature(); + GetPosition(); + GetSpeed(); + GetForwardLimitOK(); + GetFaults(); + + if(m_receivedBusVoltage && + m_receivedOutputVoltage && + m_receivedOutputCurrent && + m_receivedTemperature && + m_receivedPosition && + m_receivedSpeed && + m_receivedLimits && + m_receivedFaults) + { + break; + } + } if(getMessage(CAN_MSGID_API_FIRMVER, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) m_firmwareVersion = unpackint32_t(dataBuffer); @@ -1431,6 +1467,7 @@ float CANJaguar::GetBusVoltage() if(getMessage(LM_API_STATUS_VOLTBUS, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { m_busVoltage = unpackFXP8_8(dataBuffer); + m_receivedBusVoltage = true; } return m_busVoltage; @@ -1449,6 +1486,7 @@ float CANJaguar::GetOutputVoltage() if(getMessage(LM_API_STATUS_VOUT, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { m_outputVoltage = unpackFXP8_8(dataBuffer); + m_receivedOutputVoltage = true; } return m_outputVoltage; @@ -1467,6 +1505,7 @@ float CANJaguar::GetOutputCurrent() if(getMessage(LM_API_STATUS_CURRENT, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { m_outputCurrent = unpackFXP8_8(dataBuffer); + m_receivedOutputCurrent = true; } return m_outputCurrent; @@ -1485,6 +1524,7 @@ float CANJaguar::GetTemperature() if(getMessage(LM_API_STATUS_TEMP, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { m_temperature = unpackFXP8_8(dataBuffer); + m_receivedTemperature = true; } return m_temperature; @@ -1503,6 +1543,7 @@ double CANJaguar::GetPosition() if(getMessage(LM_API_STATUS_POS, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { m_position = unpackFXP16_16(dataBuffer); + m_receivedPosition = true; } return m_position; @@ -1521,6 +1562,7 @@ double CANJaguar::GetSpeed() if(getMessage(LM_API_STATUS_SPD, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { m_speed = unpackFXP16_16(dataBuffer); + m_receivedSpeed = true; } return m_speed; @@ -1539,6 +1581,7 @@ bool CANJaguar::GetForwardLimitOK() if(getMessage(LM_API_STATUS_LIMIT, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { m_limits = dataBuffer[0]; + m_receivedLimits = true; } return m_limits & kForwardLimit; @@ -1557,6 +1600,7 @@ bool CANJaguar::GetReverseLimitOK() if(getMessage(LM_API_STATUS_LIMIT, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { m_limits = dataBuffer[0]; + m_receivedLimits = true; } return m_limits & kReverseLimit; @@ -1575,6 +1619,7 @@ uint16_t CANJaguar::GetFaults() if(getMessage(LM_API_STATUS_FAULT, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { m_faults = unpackint16_t(dataBuffer); + m_receivedFaults = true; } return m_faults; diff --git a/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp b/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp index 701569c8bc..34ac42c1a5 100644 --- a/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp +++ b/wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp @@ -51,17 +51,6 @@ protected: } }; -/*TEST_F(CANJaguarTest, QuickTest) { - m_jaguar->SetSpeedMode(CANJaguar::Encoder, 360, 0.0, 0.0, 0.0); - - while(DriverStation::GetInstance()->IsEnabled()) { - std::cout << m_jaguar->GetPosition() << std::endl; - std::cout << m_jaguar->GetSpeed() << std::endl; - std::cout << m_jaguar->GetTemperature() << std::endl; - Wait(0.02); - } -}*/ - /** * Checks the default status data for reasonable values to confirm that we're * really getting status data from the Jaguar. @@ -78,7 +67,7 @@ TEST_F(CANJaguarTest, InitialStatus) { EXPECT_FLOAT_EQ(m_jaguar->GetOutputCurrent(), 0.0) << "Output current is non-zero."; - EXPECT_NEAR(m_jaguar->GetTemperature(), kTempe, 5.0) + EXPECT_NEAR(m_jaguar->GetTemperature(), kExpectedTemperature, 5.0) << "Temperature is not a plausible value."; EXPECT_EQ(m_jaguar->GetFaults(), 0) diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java index ec8840b7e6..257345b603 100644 --- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java +++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java @@ -140,7 +140,32 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW requestMessage(CANJNI.CAN_IS_FRAME_REMOTE | CANJNI.CAN_MSGID_API_FIRMVER); requestMessage(CANJNI.LM_API_HWVER); - Timer.delay(0.003); + // Wait until we've gotten all of the status data at least once. + for(int i = 0; i < kReceiveStatusAttempts; i++) + { + Timer.delay(0.001); + + getBusVoltage(); + getOutputVoltage(); + getOutputCurrent(); + getTemperature(); + getPosition(); + getSpeed(); + getForwardLimitOK(); + getFaults(); + + if(m_receivedBusVoltage && + m_receivedOutputVoltage && + m_receivedOutputCurrent && + m_receivedTemperature && + m_receivedPosition && + m_receivedSpeed && + m_receivedLimits && + m_receivedFaults) + { + break; + } + } // Don't catch the CANMessageNotFoundException - we need to know the // firmware version to continue. @@ -1227,6 +1252,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_STATUS_VOLTBUS, CANJNI.CAN_MSGID_FULL_M, data); + m_receivedBusVoltage = true; m_busVoltage = unpackFXP8_8(data); } catch(CANMessageNotFoundException e) {} @@ -1243,6 +1269,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_STATUS_VOUT, CANJNI.CAN_MSGID_FULL_M, data); + m_receivedOutputVoltage = true; m_outputVoltage = unpackFXP8_8(data); } catch(CANMessageNotFoundException e) {} @@ -1259,6 +1286,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_STATUS_CURRENT, CANJNI.CAN_MSGID_FULL_M, data); + m_receivedOutputCurrent = true; m_outputCurrent = unpackFXP8_8(data); } catch(CANMessageNotFoundException e) {} @@ -1275,6 +1303,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_STATUS_TEMP, CANJNI.CAN_MSGID_FULL_M, data); + m_receivedTemperature = true; m_temperature = unpackFXP8_8(data); } catch(CANMessageNotFoundException e) {} @@ -1291,6 +1320,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_STATUS_POS, CANJNI.CAN_MSGID_FULL_M, data); + m_receivedPosition = true; m_position = unpackFXP16_16(data); } catch(CANMessageNotFoundException e) {} @@ -1307,6 +1337,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_STATUS_SPD, CANJNI.CAN_MSGID_FULL_M, data); + m_receivedSpeed = true; m_speed = unpackFXP16_16(data); } catch(CANMessageNotFoundException e) {} @@ -1323,6 +1354,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_STATUS_LIMIT, CANJNI.CAN_MSGID_FULL_M, data); + m_receivedLimits = true; m_limits = data[0]; } catch(CANMessageNotFoundException e) {} @@ -1339,6 +1371,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_STATUS_LIMIT, CANJNI.CAN_MSGID_FULL_M, data); + m_receivedLimits = true; m_limits = data[0]; } catch(CANMessageNotFoundException e) {} @@ -1359,6 +1392,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_STATUS_FAULT, CANJNI.CAN_MSGID_FULL_M, data); + m_receivedFaults = true; m_faults = unpackINT16(data); } catch(CANMessageNotFoundException e) {} @@ -1612,6 +1646,18 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW int m_firmwareVersion = 0; byte m_hardwareVersion = (byte)0; + // Which status values have we received at least once? + boolean m_receivedBusVoltage = false; + boolean m_receivedOutputVoltage = false; + boolean m_receivedOutputCurrent = false; + boolean m_receivedTemperature = false; + boolean m_receivedPosition = false; + boolean m_receivedSpeed = false; + boolean m_receivedLimits = false; + boolean m_receivedFaults = false; + + static final int kReceiveStatusAttempts = 50; + static void sendMessageHelper(int messageID, byte[] data, int dataSize, int period) throws CANMessageNotFoundException { final int[] kTrustedMessages = { CANJNI.LM_API_VOLT_T_EN, CANJNI.LM_API_VOLT_T_SET, CANJNI.LM_API_SPD_T_EN, CANJNI.LM_API_SPD_T_SET,