From 1cafebc5a302eaf9796733ffa799298c119bb579 Mon Sep 17 00:00:00 2001 From: thomasclark Date: Thu, 26 Jun 2014 11:54:58 -0400 Subject: [PATCH] CANJaguar waits in the constructor for initial status data The C++ and Java CANJaguar constructors now wait up to 50 ms for the status data to arrive at least one time. They break early if it arrives sooner. This allows status getters to always work immediately after the object is constructed. Change-Id: I4a1f1538837c11b24d45e87251743b6c106ddefb --- wpilibc/wpilibC++/include/CANJaguar.h | 10 ++++ wpilibc/wpilibC++/lib/CANJaguar.cpp | 47 +++++++++++++++++- .../src/CANJaguarTest.cpp | 13 +---- .../java/edu/wpi/first/wpilibj/CANJaguar.java | 48 ++++++++++++++++++- 4 files changed, 104 insertions(+), 14 deletions(-) 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,