bool TalonSRXUtils::TalonSRXCheck(std::shared_ptr<WPI_TalonSRX> talonSRX, const char *subsystem, const char *name) {
int i;
int deviceID = 0;
int fwVersion = 0;
bool talonValid = false;
ErrorCode error = OKAY;
// Configure subsystem and component name
talonSRX->SetName(subsystem, name);
std::printf("2135: TalonSRX Subsystem %s Name %s\n", talonSRX->GetSubsystem().c_str(), talonSRX->GetName().c_str());
// Display Talon SRX firmware versions
deviceID = talonSRX->GetDeviceID();
if ((error = talonSRX->GetLastError()) != OKAY) {
std::printf("2135: ERROR: %s Motor %s GetDeviceID error - %d\n",
subsystem, name, error);
return error;
}
for (i = 0; i < m_retries; i++) {
fwVersion = talonSRX->GetFirmwareVersion();
if ((error = talonSRX->GetLastError()) != OKAY) {
std::printf("2135: ERROR: %s Motor %s ID %d GetFirmwareVersion error - %d\n",
subsystem, name, deviceID, error);
return error;
}
if (fwVersion == m_reqVersion) {
talonValid = true;
break;
}
else {
std::printf("2135: WARNING: %s Motor %s ID %d Incorrect FW version %d.%d expected %d.%d\n",
subsystem, name, deviceID, fwVersion/256, fwVersion%256, m_reqVersion/256, m_reqVersion%256);
}
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
if (talonValid) {
// Initialize Talon SRX to all factory defaults
if ((error = talonSRX->ConfigFactoryDefault(m_timeout)) != OKAY) {
std::printf("2135: ERROR: %s Motor %s ID %d ConfigFactoryDefault error - %d\n",
subsystem, name, deviceID, error);
}
std::printf("2135: %s Motor %s ID %d ver %d.%d is RESPONSIVE and INITIALIZED (error %d)\n",
subsystem, name, deviceID, fwVersion/256, fwVersion&0xff, error);
}
else {
std::printf("2135: ERROR: %s Motor %s ID %d ver %d.%d is UNRESPONSIVE, (error %d)\n",
subsystem, name, deviceID, fwVersion/256, fwVersion&0xff, error);
}
return talonValid;
}