diff --git a/sysid-application/src/integrationtest-generation/native/cpp/GenerationTest.cpp b/sysid-application/src/integrationtest-generation/native/cpp/GenerationTest.cpp index ea056ff7..053b15ea 100644 --- a/sysid-application/src/integrationtest-generation/native/cpp/GenerationTest.cpp +++ b/sysid-application/src/integrationtest-generation/native/cpp/GenerationTest.cpp @@ -44,6 +44,7 @@ wpi::StringMap> {std::string{sysid::motorcontroller::kVictorSPX.name}, kGeneralEncs}, {std::string{sysid::motorcontroller::kTalonSRX.name}, kTalonEncs}, {std::string{sysid::motorcontroller::kTalonFX.name}, kBuiltInEncs}, + {std::string{sysid::motorcontroller::kTalonFXPro.name}, kBuiltInEncs}, {std::string{sysid::motorcontroller::kSPARKMAXBrushless.name}, kSMaxEncs}, {std::string{sysid::motorcontroller::kSPARKMAXBrushed.name}, kSMaxEncs}, @@ -66,6 +67,7 @@ wpi::StringMap> gyroCtorMap = { {std::string{sysid::gyro::kAnalogGyro.name}, kAnalogCtors}, {std::string{sysid::gyro::kPigeon.name}, kPigeonCtors}, {std::string{sysid::gyro::kPigeon2.name}, kAnalogCtors}, + {std::string{sysid::gyro::kPigeon2Pro.name}, kAnalogCtors}, {std::string{sysid::gyro::kADXRS450.name}, kADXRS450Ctors}, {std::string{sysid::gyro::kNavX.name}, kNavXCtors}, {std::string{sysid::gyro::kADIS16448.name}, kADIS16448Ctors}, @@ -140,6 +142,8 @@ class GenerationTest : public ::testing::Test { motorController == sysid::motorcontroller::kVictorSPX.name || motorController == sysid::motorcontroller::kTalonFX.name) { voltageAccessor = "CTRE"; + } else if (motorController == sysid::motorcontroller::kTalonFXPro.name) { + voltageAccessor = "CTRE (Pro)"; } else if (motorController == sysid::motorcontroller::kVenom.name) { voltageAccessor = "Venom"; } else { @@ -176,6 +180,8 @@ class GenerationTest : public ::testing::Test { FindInLog(fmt::format("Pigeon, {}", pigeonPortStr)); } else if (gyro == sysid::gyro::kPigeon2.name) { FindInLog(fmt::format("Pigeon2, {} (CAN)", gyroCtor)); + } else if (gyro == sysid::gyro::kPigeon2Pro.name) { + FindInLog(fmt::format("Pigeon2, {} (CAN) (Pro)", gyroCtor)); } else if (gyro == sysid::gyro::kADXRS450.name || gyro == sysid::gyro::kADIS16448.name || gyro == sysid::gyro::kADIS16470.name) { diff --git a/sysid-application/src/main/native/cpp/generation/ConfigManager.cpp b/sysid-application/src/main/native/cpp/generation/ConfigManager.cpp index 65d2ff27..df346bc5 100644 --- a/sysid-application/src/main/native/cpp/generation/ConfigManager.cpp +++ b/sysid-application/src/main/native/cpp/generation/ConfigManager.cpp @@ -25,6 +25,9 @@ wpi::json ConfigManager::Generate(size_t occupied) { // Create the JSON to return. wpi::json json; + // Keep a CANivore name vector around that we can push our names into + std::vector canivoreNames; + // Add motor ports. json["primary motor ports"] = SliceVector(m_config.primaryMotorPorts, occupied); @@ -38,6 +41,13 @@ wpi::json ConfigManager::Generate(size_t occupied) { } json["motor controllers"] = motorControllers; + // Add CANivore busses + canivoreNames.clear(); + for (size_t i = 0; i < occupied; i++) { + canivoreNames.push_back(std::string(m_config.canivoreNames[i].data())); + } + json["canivore names"] = canivoreNames; + // Add motor inversions. json["primary motors inverted"] = SliceVector(m_config.primaryMotorsInverted, occupied); @@ -47,6 +57,8 @@ wpi::json ConfigManager::Generate(size_t occupied) { // Add encoder ports. json["primary encoder ports"] = m_config.primaryEncoderPorts; json["secondary encoder ports"] = m_config.secondaryEncoderPorts; + json["encoder canivore name"] = + std::string(m_config.encoderCANivoreName.data()); // Add encoder type. json["encoder type"] = m_config.encoderType.name; @@ -63,6 +75,7 @@ wpi::json ConfigManager::Generate(size_t occupied) { // Add gyro type and constructor. json["gyro"] = m_config.gyro.name; json["gyro ctor"] = m_config.gyroCtor; + json["gyro canivore name"] = std::string(m_config.gyroCANivoreName.data()); // Add advanced encoder settings. json["encoding"] = m_config.encoding; @@ -79,10 +92,12 @@ void ConfigManager::ReadJSON(std::string_view path) { constexpr const char* json_keys[] = {"primary motor ports", "secondary motor ports", "motor controllers", + "canivore names", "primary motors inverted", "secondary motors inverted", "primary encoder ports", "secondary encoder ports", + "encoder canivore name", "encoder type", "primary encoder inverted", "secondary encoder inverted", @@ -91,9 +106,11 @@ void ConfigManager::ReadJSON(std::string_view path) { "gearing denominator", "gyro", "gyro ctor", + "gyro canivore name", "encoding", "number of samples per average", - "velocity measurement period"}; + "velocity measurement period", + "is drivetrain"}; // Read JSON from the specified path. std::error_code ec; diff --git a/sysid-application/src/main/native/cpp/generation/HardwareType.cpp b/sysid-application/src/main/native/cpp/generation/HardwareType.cpp index 5631c1e6..80622c8d 100644 --- a/sysid-application/src/main/native/cpp/generation/HardwareType.cpp +++ b/sysid-application/src/main/native/cpp/generation/HardwareType.cpp @@ -25,6 +25,9 @@ HardwareType sysid::motorcontroller::FromMotorControllerName( if (name == "TalonFX") { return sysid::motorcontroller::kTalonFX; } + if (name == "TalonFX (Pro)") { + return sysid::motorcontroller::kTalonFXPro; + } if (name == "SPARK MAX (Brushless)") { return sysid::motorcontroller::kSPARKMAXBrushless; } @@ -45,6 +48,9 @@ HardwareType sysid::encoder::FromEncoderName(std::string_view name) { if (name == "CANCoder") { return sysid::encoder::kCANCoder; } + if (name == "CANcoder (Pro)") { + return sysid::encoder::kCANcoderPro; + } if (name == "Built-in") { return sysid::encoder::kBuiltInSetting; } diff --git a/sysid-application/src/main/native/cpp/view/Analyzer.cpp b/sysid-application/src/main/native/cpp/view/Analyzer.cpp index 082df046..b11ca4bb 100644 --- a/sysid-application/src/main/native/cpp/view/Analyzer.cpp +++ b/sysid-application/src/main/native/cpp/view/Analyzer.cpp @@ -35,6 +35,7 @@ Analyzer::Analyzer(glass::Storage& storage, wpi::Logger& logger) m_presets["WPILib (Pre-2020)"] = presets::kWPILibOld; m_presets["CANCoder"] = presets::kCTRECANCoder; m_presets["CTRE"] = presets::kCTREDefault; + m_presets["CTRE (Pro)"] = presets::kCTREProDefault; m_presets["REV Brushless Encoder Port"] = presets::kREVNEOBuiltIn; m_presets["REV Brushed Encoder Port"] = presets::kREVNonNEO; m_presets["REV Data Port"] = presets::kREVNonNEO; diff --git a/sysid-application/src/main/native/cpp/view/Generator.cpp b/sysid-application/src/main/native/cpp/view/Generator.cpp index 2d7ae061..eee8be04 100644 --- a/sysid-application/src/main/native/cpp/view/Generator.cpp +++ b/sysid-application/src/main/native/cpp/view/Generator.cpp @@ -127,7 +127,7 @@ void Generator::RoboRIOEncoderSetup(bool drive) { "CTRE Magnetic Encoder and REV Throughbore Encoder"); } -void Generator::CANCoderSetup(bool drive) { +void Generator::CANCoderSetup(bool drive, bool usePro) { ImGui::SetNextItemWidth(ImGui::GetFontSize() * 2); ImGui::InputInt(drive ? "L CANCoder Port" : "CANCoder Port", &m_settings.primaryEncoderPorts[0], 0, 0); @@ -142,6 +142,13 @@ void Generator::CANCoderSetup(bool drive) { ImGui::Checkbox("Right Encoder Inverted", &m_settings.secondaryEncoderInverted); } + + ImGui::SetNextItemWidth(80); + ImGui::InputText("CANcoder CANivore Name", + m_settings.encoderCANivoreName.data(), + m_settings.encoderCANivoreName.size()); + + m_settings.cancoderUsingPro = usePro; } void Generator::RegularEncoderSetup(bool drive) { @@ -167,8 +174,10 @@ void Generator::UpdateFromConfig() { // Setting right Idxs for the GUI if (mainMotorController == sysid::motorcontroller::kTalonSRX || - mainMotorController == sysid::motorcontroller::kTalonFX) { - if (mainMotorController == sysid::motorcontroller::kTalonFX) { + mainMotorController == sysid::motorcontroller::kTalonFX || + mainMotorController == sysid::motorcontroller::kTalonFXPro) { + if (mainMotorController == sysid::motorcontroller::kTalonFX || + mainMotorController == sysid::motorcontroller::kTalonFXPro) { m_encoderIdx = GetNewIdx(ArrayConcat(kBuiltInEncoders, kGeneralEncoders), encoderTypeName); } else { @@ -292,6 +301,7 @@ void Generator::Display() { auto& mc = m_settings.motorControllers; auto& pi = m_settings.primaryMotorsInverted; auto& si = m_settings.secondaryMotorsInverted; + auto& cn = m_settings.canivoreNames; // Ensure that our vector contains i+1 elements. if (pm.size() == i) { @@ -300,6 +310,7 @@ void Generator::Display() { mc.emplace_back(motorControllerNames[0]); pi.emplace_back(false); si.emplace_back(false); + cn.emplace_back(std::array{'r', 'i', 'o', '\0'}); } // Make sure elements have unique IDs. @@ -362,6 +373,15 @@ void Generator::Display() { ImGui::Checkbox("R Inverted", &si[i]); } + // Add CANivore name if we are using a CTRE motor controller + if (m_settings.motorControllers[i] == sysid::motorcontroller::kTalonFX || + m_settings.motorControllers[i] == sysid::motorcontroller::kTalonFXPro) { + ImGui::SetNextItemWidth(80); + ImGui::InputText("Motor CANivore Name", + m_settings.canivoreNames[i].data(), + m_settings.canivoreNames[i].size()); + } + ImGui::PopID(); // If we selected Spark Max with Brushed mode, set our flag to true. @@ -383,13 +403,15 @@ void Generator::Display() { // Add encoder selection. ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple); if (mainMotorController == sysid::motorcontroller::kTalonSRX || - mainMotorController == sysid::motorcontroller::kTalonFX) { - if (mainMotorController == sysid::motorcontroller::kTalonFX) { + mainMotorController == sysid::motorcontroller::kTalonFX || + mainMotorController == sysid::motorcontroller::kTalonFXPro) { + if (mainMotorController == sysid::motorcontroller::kTalonFX || + mainMotorController == sysid::motorcontroller::kTalonFXPro) { GetEncoder(ArrayConcat(kBuiltInEncoders, kGeneralEncoders)); if (m_encoderIdx < 1) { RegularEncoderSetup(drive); - } else if (m_encoderIdx == 1) { - CANCoderSetup(drive); + } else if (m_encoderIdx == 1 || m_encoderIdx == 2) { + CANCoderSetup(drive, m_encoderIdx == 2); } else { RoboRIOEncoderSetup(drive); } @@ -397,8 +419,8 @@ void Generator::Display() { GetEncoder(ArrayConcat(kTalonSRXEncoders, kGeneralEncoders)); if (m_encoderIdx <= 1) { RegularEncoderSetup(drive); - } else if (m_encoderIdx == 2) { - CANCoderSetup(drive); + } else if (m_encoderIdx == 2 || m_encoderIdx == 3) { + CANCoderSetup(drive, m_encoderIdx == 3); } else { RoboRIOEncoderSetup(drive); } @@ -414,8 +436,8 @@ void Generator::Display() { // You're not allowed to invert the NEO Built-in encoder RegularEncoderSetup(drive); } - } else if (m_encoderIdx == 2) { - CANCoderSetup(drive); + } else if (m_encoderIdx == 2 || m_encoderIdx == 3) { + CANCoderSetup(drive, m_encoderIdx == 3); } else { RoboRIOEncoderSetup(drive); } @@ -423,15 +445,15 @@ void Generator::Display() { GetEncoder(ArrayConcat(kBuiltInEncoders, kGeneralEncoders)); if (m_encoderIdx == 0) { RegularEncoderSetup(drive); - } else if (m_encoderIdx == 1) { - CANCoderSetup(drive); + } else if (m_encoderIdx == 1 || m_encoderIdx == 2) { + CANCoderSetup(drive, m_encoderIdx == 2); } else { RoboRIOEncoderSetup(drive); } } else { GetEncoder(kGeneralEncoders); - if (m_encoderIdx == 0) { - CANCoderSetup(drive); + if (m_encoderIdx == 0 || m_encoderIdx == 1) { + CANCoderSetup(drive, m_encoderIdx == 1); } else { RoboRIOEncoderSetup(drive); } @@ -441,7 +463,10 @@ void Generator::Display() { if (!((mainMotorController == sysid::motorcontroller::kVenom && m_settings.encoderType == sysid::encoder::kBuiltInSetting) || (mainMotorController == sysid::motorcontroller::kSPARKMAXBrushless && - m_settings.encoderType == sysid::encoder::kSMaxEncoderPort))) { + m_settings.encoderType == sysid::encoder::kSMaxEncoderPort) || + (mainMotorController == sysid::motorcontroller::kTalonFXPro && + m_settings.encoderType == sysid::encoder::kBuiltInSetting) || + m_settings.encoderType == sysid::encoder::kCANcoderPro)) { // Samples Per Average Setting ImGui::SetNextItemWidth(ImGui::GetFontSize() * 2); ImGui::InputInt("Samples Per Average", &m_settings.numSamples, 0, 0); @@ -451,7 +476,8 @@ void Generator::Display() { "CPRs."); // Add Velocity Measurement Period - if (m_settings.encoderType != sysid::encoder::kRoboRIO) { + if (m_settings.encoderType != sysid::encoder::kRoboRIO && + m_settings.encoderType != sysid::encoder::kCANcoderPro) { ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); ImGui::Combo("Time Measurement Window", &m_periodIdx, kCTREPeriods, IM_ARRAYSIZE(kCTREPeriods)); @@ -489,9 +515,16 @@ void Generator::Display() { if (m_isTalon) { m_settings.gyroCtor = "WPI_TalonSRX-" + m_settings.gyroCtor; } - } else if (gyroType == sysid::gyro::kPigeon2) { + } else if (gyroType == sysid::gyro::kPigeon2 || + gyroType == sysid::gyro::kPigeon2Pro) { ImGui::InputInt("CAN ID", &m_gyroPort, 0, 0); - m_settings.gyroCtor = std::to_string(m_gyroPort); + + ImGui::SetNextItemWidth(80); + ImGui::InputText("Gyro CANivore Name", m_settings.gyroCANivoreName.data(), + m_settings.gyroCANivoreName.size()); + m_settings.gyroCtor = std::to_string(m_gyroPort) + ", " + + std::string{m_settings.gyroCANivoreName.begin(), + m_settings.gyroCANivoreName.end()}; } else if (gyroType == sysid::gyro::kADXRS450) { ImGui::Combo("SPI Port", &m_gyroParam, kADXRS450Ctors, IM_ARRAYSIZE(kADXRS450Ctors)); @@ -536,7 +569,8 @@ void Generator::Display() { sysid::CreateTooltip( "This is the number of encoder counts per revolution for your encoder.\n" "Common values for this are here:\nCTRE Magnetic Encoder: 4096\nFalcon " - "500 Integrated: 2048\nREV Throughbore: 8192\nNEO (and NEO 550) " + "500 Integrated: 2048\nFalcon 500 running Phoenix Pro (Pro already " + "handles this value): 1\nREV Throughbore: 8192\nNEO (and NEO 550) " "Integrated " "Encoders (REV already handles this value): 1"); diff --git a/sysid-application/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h b/sysid-application/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h index bb670e3a..4b13c6c3 100644 --- a/sysid-application/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h +++ b/sysid-application/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h @@ -121,6 +121,15 @@ constexpr FeedbackControllerPreset kCTRECANCoder{1.0 / 12.0, 60.0, 1_ms, true, 81.5_ms}; constexpr FeedbackControllerPreset kCTREDefault{1023.0 / 12.0, 0.1, 1_ms, false, 81.5_ms}; +/** + * https://api.ctr-electronics.com/phoenixpro/release/cpp/classctre_1_1phoenixpro_1_1hardware_1_1core_1_1_core_c_a_ncoder.html#a718a1a214b58d3c4543e88e3cb51ade5 + * + * Phoenix Pro uses standard units and Voltage output. This means the output + * is 1.0, time factor is 1.0, and closed loop operates at 1 millisecond. All + * Pro devices make use of Kalman filters default-tuned to lowest latency, which + * in testing is roughly 1 millisecond + */ +constexpr FeedbackControllerPreset kCTREProDefault{1.0, 1.0, 1_ms, true, 1_ms}; /** * https://github.com/wpilibsuite/sysid/issues/258#issuecomment-1010658237 diff --git a/sysid-application/src/main/native/include/sysid/generation/ConfigManager.h b/sysid-application/src/main/native/include/sysid/generation/ConfigManager.h index 8b776d68..eb93391e 100644 --- a/sysid-application/src/main/native/include/sysid/generation/ConfigManager.h +++ b/sysid-application/src/main/native/include/sysid/generation/ConfigManager.h @@ -23,6 +23,11 @@ namespace sysid { * inversions, etc. It also includes numerical data such as EPR. */ struct ConfigSettings { + /** + * Maximum length of a CANivore name is 32 characters + */ + static constexpr int kMaxCANivoreNameLength = 32; + /** * Ports for the primary motor controllers. For general mechanisms this is the * motor ports that will be used. For drivetrains these are the left-side @@ -42,6 +47,12 @@ struct ConfigSettings { wpi::SmallVector motorControllers = { sysid::motorcontroller::kPWM, sysid::motorcontroller::kPWM}; + /** + * The CANivore name if it's on a Non-RIO bus + */ + wpi::SmallVector, 3> canivoreNames = + {{'r', 'i', 'o', '\0'}, {'r', 'i', 'o', '\0'}}; + /** * If the primary motor controllers (general mechanism motors / left * drivetrain motors) should be inverted or not. @@ -73,6 +84,12 @@ struct ConfigSettings { */ std::array secondaryEncoderPorts = {2, 3}; + /** + * CANivore names for the CANcoder sensors if they're not on the RIO bus. + */ + std::array encoderCANivoreName = {'r', 'i', 'o', + '\0'}; + /** * If the primary encoder (general mechanism motors / left drivetrain encoder) * should be inverted or not. @@ -85,6 +102,11 @@ struct ConfigSettings { */ bool secondaryEncoderInverted = false; + /** + * If using a CANcoder and it's using Pro. + */ + bool cancoderUsingPro = false; + /** * The counts per revolution of the encoder. */ @@ -110,6 +132,12 @@ struct ConfigSettings { */ std::string gyroCtor = "0"; + /** + * The CANivore name of a Pigeon2 if it's on a Non-RIO bus + */ + std::array gyroCANivoreName = {'r', 'i', 'o', + '\0'}; + /** * If the encoder is plugged into the roboRIO, there's the option to set it's * encoding setting to reduce noise. @@ -136,18 +164,22 @@ struct ConfigSettings { const ConfigSettings kRomiConfig{{0}, {1}, {sysid::motorcontroller::kPWM}, + {{'r', 'i', 'o', '\0'}}, {true}, {false}, sysid::encoder::kRoboRIO, {4, 5}, {6, 7}, + {{'r', 'i', 'o', '\0'}}, + false, false, false, 1440.0, 1.0, 1.0, sysid::gyro::kRomiGyro, - ""}; + "", + {'r', 'i', 'o', '\0'}}; /** * This class manages generating the JSON configuration from a reference to the diff --git a/sysid-application/src/main/native/include/sysid/generation/HardwareType.h b/sysid-application/src/main/native/include/sysid/generation/HardwareType.h index 7378a710..6ad447c1 100644 --- a/sysid-application/src/main/native/include/sysid/generation/HardwareType.h +++ b/sysid-application/src/main/native/include/sysid/generation/HardwareType.h @@ -58,14 +58,14 @@ constexpr HardwareType kPWM{"PWM"}; constexpr HardwareType kTalonSRX{"TalonSRX"}; constexpr HardwareType kVictorSPX{"VictorSPX"}; constexpr HardwareType kTalonFX{"TalonFX"}; +constexpr HardwareType kTalonFXPro{"TalonFX (Pro)"}; constexpr HardwareType kSPARKMAXBrushless{"SPARK MAX (Brushless)"}; constexpr HardwareType kSPARKMAXBrushed{"SPARK MAX (Brushed)"}; constexpr HardwareType kVenom{"Venom"}; -constexpr std::array kMotorControllers = { - kPWM, kTalonSRX, kVictorSPX, - kTalonFX, kSPARKMAXBrushless, kSPARKMAXBrushed, - kVenom}; +constexpr std::array kMotorControllers = { + kPWM, kTalonSRX, kVictorSPX, kTalonFX, + kTalonFXPro, kSPARKMAXBrushless, kSPARKMAXBrushed, kVenom}; /** * Returns an existing motor controller HardwareType from a string_view. Throws @@ -80,13 +80,14 @@ HardwareType FromMotorControllerName(std::string_view name); namespace encoder { constexpr HardwareType kRoboRIO{"roboRIO quadrature"}; constexpr HardwareType kCANCoder{"CANCoder"}; +constexpr HardwareType kCANcoderPro{"CANcoder (Pro)"}; constexpr HardwareType kBuiltInSetting{"Built-in"}; constexpr HardwareType kCTRETachometer{"Tachometer"}; constexpr HardwareType kSMaxEncoderPort{"Encoder Port"}; constexpr HardwareType kSMaxDataPort{"Data Port"}; -constexpr std::array kEncoders = { - kRoboRIO, kCANCoder, kBuiltInSetting, +constexpr std::array kEncoders = { + kRoboRIO, kCANCoder, kCANcoderPro, kBuiltInSetting, kCTRETachometer, kSMaxEncoderPort, kSMaxDataPort}; /** @@ -107,6 +108,7 @@ constexpr HardwareType kADIS16470{"ADIS16470"}; constexpr HardwareType kNavX{"NavX"}; constexpr HardwareType kPigeon{"Pigeon"}; constexpr HardwareType kPigeon2{"Pigeon2"}; +constexpr HardwareType kPigeon2Pro{"Pigeon2 (Pro)"}; constexpr HardwareType kRomiGyro{"Romi"}; constexpr HardwareType kNoGyroOption{"None"}; diff --git a/sysid-application/src/main/native/include/sysid/view/Analyzer.h b/sysid-application/src/main/native/include/sysid/view/Analyzer.h index fd06d286..bc639dfc 100644 --- a/sysid-application/src/main/native/include/sysid/view/Analyzer.h +++ b/sysid-application/src/main/native/include/sysid/view/Analyzer.h @@ -58,6 +58,7 @@ class Analyzer : public glass::View { "WPILib (2020-)", "WPILib (Pre-2020)", "CANCoder", + "CTRE (Pro)", "CTRE", "REV Brushless Encoder Port", "REV Brushed Encoder Port", diff --git a/sysid-application/src/main/native/include/sysid/view/Generator.h b/sysid-application/src/main/native/include/sysid/view/Generator.h index 699c16a4..3892b3f0 100644 --- a/sysid-application/src/main/native/include/sysid/view/Generator.h +++ b/sysid-application/src/main/native/include/sysid/view/Generator.h @@ -62,8 +62,9 @@ struct DisplayNameStorage { static constexpr auto kMotorControllerNames = DisplayNameStorage(sysid::motorcontroller::kMotorControllers); -static constexpr std::array kGeneralEncoders{ +static constexpr std::array kGeneralEncoders{ sysid::encoder::kCANCoder.displayName, + sysid::encoder::kCANcoderPro.displayName, sysid::encoder::kRoboRIO.displayName}; static constexpr std::array kTalonSRXEncoders{ @@ -150,8 +151,9 @@ class Generator : public glass::View { * Displays the widgets to setup the CANCoder * * @param drive True if the encoder setup is for a Drivetrain. + * @param usePro True if CANcoder is using Pro firmware */ - void CANCoderSetup(bool drive); + void CANCoderSetup(bool drive, bool usePro); /** * Displays the encoder options for a specific combination of motor controller diff --git a/sysid-library/src/main/cpp/generation/SysIdSetup.cpp b/sysid-library/src/main/cpp/generation/SysIdSetup.cpp index 762eaf9d..48015f3f 100644 --- a/sysid-library/src/main/cpp/generation/SysIdSetup.cpp +++ b/sysid-library/src/main/cpp/generation/SysIdSetup.cpp @@ -29,6 +29,8 @@ #define EXPAND_STRINGIZE(s) STRINGIZE(s) #define STRINGIZE(s) #s +using namespace ctre::phoenixpro; + namespace sysid { wpi::json GetConfigJson() { @@ -58,6 +60,7 @@ wpi::json GetConfigJson() { void AddMotorController( int port, std::string_view controller, bool inverted, + std::string_view canivore, std::vector>* controllers) { if (controller == "TalonSRX" || controller == "VictorSPX" || controller == "TalonFX") { @@ -66,7 +69,8 @@ void AddMotorController( controllers->push_back(std::make_unique(port)); } else if (controller == "TalonFX") { fmt::print("Setup TalonFX\n"); - controllers->emplace_back(std::make_unique(port)); + controllers->emplace_back(std::make_unique( + port, std::string{canivore.begin(), canivore.end()})); } else { fmt::print("Setup VictorSPX\n"); controllers->emplace_back(std::make_unique(port)); @@ -77,6 +81,18 @@ void AddMotorController( ctreController->ConfigFactoryDefault(); ctreController->SetInverted(inverted); ctreController->SetNeutralMode(motorcontrol::NeutralMode::Brake); + } else if (controller == "TalonFX (Pro)") { + fmt::print("Setup TalonFX (Pro)\n"); + controllers->emplace_back(std::make_unique( + port, std::string{canivore.begin(), canivore.end()})); + auto* ctreController = + dynamic_cast(controllers->back().get()); + configs::TalonFXConfiguration configs{}; + configs.MotorOutput.Inverted = + inverted ? ctre::phoenixpro::signals::InvertedValue:: + CounterClockwise_Positive + : ctre::phoenixpro::signals::InvertedValue::Clockwise_Positive; + ctreController->GetConfigurator().Apply(configs); } else if (controller == "SPARK MAX (Brushless)" || controller == "SPARK MAX (Brushed)") { if (controller == "SPARK MAX (Brushless)") { @@ -161,7 +177,9 @@ void SetupEncoders( std::string_view encoderType, bool isEncoding, int period, double cpr, double gearing, int numSamples, std::string_view controllerName, frc::MotorController* controller, bool encoderInverted, - const std::vector& encoderPorts, std::unique_ptr& cancoder, + const std::vector& encoderPorts, + const std::string& encoderCANivoreName, std::unique_ptr& cancoder, + std::unique_ptr& cancoderPro, std::unique_ptr& revEncoderPort, std::unique_ptr& revDataPort, std::unique_ptr& encoder, std::function& position, @@ -176,7 +194,23 @@ void SetupEncoders( SetDefaultDataCollection(position, rate); #endif if (encoderType == "Built-in") { - if (wpi::starts_with(controllerName, "Talon")) { + if (controllerName == "TalonFX (Pro)") { + fmt::print("Setup Built-in+TalonFX (Pro)\n"); + position = [=] { + return dynamic_cast(controller) + ->GetPosition() + .GetValue() + .value() / + gearing; + }; + rate = [=] { + return dynamic_cast(controller) + ->GetVelocity() + .GetValue() + .value() / + gearing; + }; + } else if (wpi::starts_with(controllerName, "Talon")) { FeedbackDevice feedbackDevice; if (controllerName == "TalonSRX") { fmt::print("Setup Built-in+TalonSRX\n"); @@ -241,7 +275,7 @@ void SetupEncoders( combinedCPR, numSamples, encoderInverted, position, rate); } else if (encoderType == "CANCoder") { fmt::print("Setup CANCoder\n"); - cancoder = std::make_unique(encoderPorts[0]); + cancoder = std::make_unique(encoderPorts[0], encoderCANivoreName); cancoder->ConfigSensorDirection(encoderInverted); sensors::SensorVelocityMeasPeriod cancoderPeriod = @@ -252,6 +286,24 @@ void SetupEncoders( position = [=, &cancoder] { return cancoder->GetPosition() / combinedCPR; }; rate = [=, &cancoder] { return cancoder->GetVelocity() / combinedCPR; }; + } else if (encoderType == "CANcoder (Pro)") { + fmt::print("Setup CANCoder (Pro)\n"); + cancoderPro = std::make_unique(encoderPorts[0], + encoderCANivoreName); + configs::CANcoderConfiguration cfg; + cfg.MagnetSensor.SensorDirection = + encoderInverted ? ctre::phoenixpro::signals::SensorDirectionValue:: + Clockwise_Positive + : ctre::phoenixpro::signals::SensorDirectionValue:: + CounterClockwise_Positive; + cancoderPro->GetConfigurator().Apply(cfg); + + position = [=, &cancoderPro] { + return cancoderPro->GetPosition().GetValue().value() / combinedCPR; + }; + rate = [=, &cancoderPro] { + return cancoderPro->GetVelocity().GetValue().value() / combinedCPR; + }; } else { fmt::print("Setup roboRIO quadrature\n"); if (isEncoding) { @@ -303,12 +355,32 @@ void SetupGyro( std::unique_ptr& ADIS16448Gyro, std::unique_ptr& ADIS16470Gyro, std::unique_ptr& pigeon, + std::unique_ptr& pigeonpro, std::unique_ptr& tempTalon, - std::function& gyroPosition, std::function& gyroRate) { + const std::string& gyroCANivoreName, std::function& gyroPosition, + std::function& gyroRate) { #ifndef __FRC_ROBORIO__ sysid::SetDefaultDataCollection(gyroPosition, gyroRate); #endif - if (wpi::starts_with(gyroType, "Pigeon")) { + if (gyroType == "Pigeon2 (Pro)") { + std::string portStr; + portStr = gyroCtor; + int canID = std::stoi(portStr); + pigeonpro = std::make_unique( + canID, gyroCANivoreName); + fmt::print("Setup Pigeon2 (Pro), {}\n", portStr); + + // setup functions + gyroPosition = [&] { + return units::radian_t{pigeonpro->GetYaw().GetValue()}.value(); + }; + + gyroRate = [&] { + return units::radians_per_second_t{ + pigeonpro->GetAngularVelocityZ().GetValue()} + .value(); + }; + } else if (wpi::starts_with(gyroType, "Pigeon")) { std::string portStr; if (wpi::contains(gyroCtor, "WPI_TalonSRX")) { portStr = wpi::split(gyroCtor, "-").second; @@ -343,16 +415,17 @@ void SetupGyro( talon = tempTalon.get(); portStr = fmt::format("{} (plugged to other motorcontroller)", portStr); } - pigeon = std::make_unique(talon); + pigeon = std::make_unique(talon); fmt::print("Setup Pigeon, {}\n", portStr); } else { portStr = fmt::format("{} (CAN)", portStr); if (gyroType == "Pigeon") { - pigeon = std::make_unique(srxPort); + pigeon = std::make_unique(srxPort); fmt::print("Setup Pigeon, {}\n", portStr); } else { - pigeon = std::make_unique(srxPort); + pigeon = std::make_unique( + srxPort, gyroCANivoreName); fmt::print("Setup Pigeon2, {}\n", portStr); } } @@ -371,7 +444,6 @@ void SetupGyro( units::degrees_per_second_t rate{xyz_dps[2]}; return units::radians_per_second_t{rate}.value(); }; - SetDefaultDataCollection(gyroPosition, gyroRate); } else if (wpi::starts_with(gyroType, "ADIS")) { auto port = GetSPIPort(gyroCtor); if (gyroType == "ADIS16448") { @@ -469,7 +541,13 @@ void SetMotorControllers( units::volt_t motorVoltage, const std::vector>& controllers) { for (auto&& controller : controllers) { - controller->SetVoltage(motorVoltage); + auto* ctreController = dynamic_cast(controller.get()); + if (ctreController) { + ctreController->SetControl( + controls::VoltageOut{motorVoltage, true, false}); + } else { + controller->SetVoltage(motorVoltage); + } } } diff --git a/sysid-library/src/main/cpp/logging/SysIdLogger.cpp b/sysid-library/src/main/cpp/logging/SysIdLogger.cpp index a9d2faab..0c76d359 100644 --- a/sysid-library/src/main/cpp/logging/SysIdLogger.cpp +++ b/sysid-library/src/main/cpp/logging/SysIdLogger.cpp @@ -10,6 +10,7 @@ // #include #include +#include #include #include #include @@ -51,6 +52,14 @@ double SysIdLogger::MeasureVoltage( if constexpr (frc::RobotBase::IsSimulation()) { fmt::print("Recording SPARK MAX voltage\n"); } + } else if (controllerNames[i] == "TalonFX (Pro)") { + auto* ctreController = + dynamic_cast(controller); + sum += ctreController->GetDutyCycle().GetValue().value() * + ctreController->GetSupplyVoltage().GetValue().value(); + if constexpr (frc::RobotBase::IsSimulation()) { + fmt::print("Recording CTRE (Pro) voltage\n"); + } } else if (wpi::starts_with(controllerNames[i], "Talon") || wpi::starts_with(controllerNames[i], "Victor")) { auto* ctreController = dynamic_cast(controller); diff --git a/sysid-library/src/main/include/sysid/generation/SysIdSetup.h b/sysid-library/src/main/include/sysid/generation/SysIdSetup.h index d0be0642..71ef8286 100644 --- a/sysid-library/src/main/include/sysid/generation/SysIdSetup.h +++ b/sysid-library/src/main/include/sysid/generation/SysIdSetup.h @@ -10,7 +10,6 @@ #include #include -#include #include #include #include @@ -21,6 +20,12 @@ #include #include +/* Keep CTRE includes below ADIS16448_IMU header to allow Windows to build */ +#include +#include +#include +#include + namespace sysid { wpi::json GetConfigJson(); @@ -31,15 +36,17 @@ wpi::json GetConfigJson(); * * @param[in] port The port number that the motor controller is plugged into. * @param[in] controller The type of motor controller, should be one of these: - * "PWM", "TalonSRX", "VictorSPX", "TalonFX", - * "SPARK MAX (Brushless)", "SPARK AX (Brushed)", "Venom" + * "PWM", "TalonSRX", "VictorSPX", "TalonFX", "TalonFX + * (Pro)" "SPARK MAX (Brushless)", "SPARK AX (Brushed)", "Venom" * @param[in] inverted True if the motor controller should be inverted, false if * not. + * @param[in] canivore Name of the CANivore bus the motor controller is on. * @param[in, out] controllers A reference to the vector storing the motor * controller objects */ void AddMotorController( int port, std::string_view controller, bool inverted, + std::string_view canivore, std::vector>* controllers); /** @@ -59,7 +66,8 @@ void SetMotorControllers( * * @param[in] encoderType The type of encoder used, should be one of these: * "Built-in", "Tachometer", "CANCoder", - * "roboRIO quadrature", "Encoder Port", "Data Port" + * "CANcoder (Pro)", "roboRIO quadrature", + * "Encoder Port", "Data Port" * @param[in] isEncoding True if the encoder should be in an encoding setting * (only applies to Encoders plugged into a roboRIO) * @param[in] period The measurement period used to calculate velocity of the @@ -76,7 +84,10 @@ void SetMotorControllers( * @param[in] encoderPorts Port number for the encoder if its not plugged into a * motor controller. 2 ports should be used for roboRIO * encoders, 1 port should be used for CANCoder. + * @param[in] encoderCANivoreName The name of the CANivore bus the encoder is + * on. * @param[in, out] cancoder A reference to a CANCoder object + * @param[in, out] cancoderPro A reference to a CANcoder Pro object * @param[in, out] revEncoderPort A reference to a REV Encoder Port object * @param[in, out] revDataPort A reference to a REV Data Port object * @param[in, out] encoder A reference to a roboRIO encoder object @@ -89,7 +100,9 @@ void SetupEncoders( std::string_view encoderType, bool isEncoding, int period, double cpr, double gearing, int numSamples, std::string_view controllerName, frc::MotorController* controller, bool encoderInverted, - const std::vector& encoderPorts, std::unique_ptr& cancoder, + const std::vector& encoderPorts, + const std::string& encoderCANivoreName, std::unique_ptr& cancoder, + std::unique_ptr& cancoderPro, std::unique_ptr& revEncoderPort, std::unique_ptr& revDataPort, std::unique_ptr& encoder, std::function& position, @@ -122,8 +135,11 @@ void SetupEncoders( * @param[in, out] ADIS16448Gyro A pointer to an ADIS16448_IMU object. * @param[in, out] ADIS16470Gyro A pointer to an ADIS16470_IMU object. * @param[in, out] pigeon A pointer to a Pigeon IMU Object + * @param[in, out] pigeonpro A pointer to a Pigeon2 Pro Object * @param[in, out] tempTalon A pointer to a TalonSRX object mean to store a * Talon that the Pigeon IMU is plugged into. + * @param[in] gyroCANivoreName A reference to the name of the CANivore the Gyro + * is plugged into. * @param[out] gyroPosition A reference to a function that is supposed to return * the gyro position * @param[out] gyroRate A reference to a function that is supposed to return the @@ -139,8 +155,10 @@ void SetupGyro( std::unique_ptr& ADIS16448Gyro, std::unique_ptr& ADIS16470Gyro, std::unique_ptr& pigeon, + std::unique_ptr& pigeonpro, std::unique_ptr& tempTalon, - std::function& gyroPosition, std::function& gyroRate); + const std::string& gyroCANivoreName, std::function& gyroPosition, + std::function& gyroRate); /** * Sets specified data collection functions to return zero. This is to avoid diff --git a/sysid-projects/drive/src/main/cpp/DriveRobot.cpp b/sysid-projects/drive/src/main/cpp/DriveRobot.cpp index 4775ff03..86e53346 100644 --- a/sysid-projects/drive/src/main/cpp/DriveRobot.cpp +++ b/sysid-projects/drive/src/main/cpp/DriveRobot.cpp @@ -42,6 +42,13 @@ DriveRobot::DriveRobot() : frc::TimedRobot(5_ms) { std::vector rightMotorsInverted = m_json.at("secondary motors inverted").get>(); + std::vector canivoreNames = + m_json.at("canivore names").get>(); + std::string encoderCANivoreName = + m_json.at("encoder canivore name").get(); + std::string gyroCANivoreName = + m_json.at("gyro canivore name").get(); + std::string encoderType = m_json.at("encoder type").get(); bool leftEncoderInverted = m_json.at("primary encoder inverted").get(); @@ -64,30 +71,33 @@ DriveRobot::DriveRobot() : frc::TimedRobot(5_ms) { fmt::print("Setup motors\n"); for (size_t i = 0; i < leftPorts.size(); i++) { sysid::AddMotorController(leftPorts[i], m_controllerNames[i], - leftMotorsInverted[i], &m_leftControllers); + leftMotorsInverted[i], canivoreNames[i], + &m_leftControllers); sysid::AddMotorController(rightPorts[i], m_controllerNames[i], - rightMotorsInverted[i], &m_rightControllers); + rightMotorsInverted[i], canivoreNames[i], + &m_rightControllers); } fmt::print("Setup encoders\n"); - sysid::SetupEncoders(encoderType, isEncoding, period, cpr, gearing, - numSamples, m_controllerNames[0], - m_leftControllers.at(0).get(), leftEncoderInverted, - leftEncoderPorts, m_leftCancoder, m_leftRevEncoderPort, - m_leftRevDataPort, m_leftEncoder, m_leftPosition, - m_leftRate); - sysid::SetupEncoders(encoderType, isEncoding, period, cpr, gearing, - numSamples, m_controllerNames[0], - m_rightControllers.at(0).get(), rightEncoderInverted, - rightEncoderPorts, m_rightCancoder, - m_rightRevEncoderPort, m_rightRevDataPort, - m_rightEncoder, m_rightPosition, m_rightRate); + sysid::SetupEncoders( + encoderType, isEncoding, period, cpr, gearing, numSamples, + m_controllerNames[0], m_leftControllers.at(0).get(), + leftEncoderInverted, leftEncoderPorts, encoderCANivoreName, + m_leftCancoder, m_leftCancoderPro, m_leftRevEncoderPort, + m_leftRevDataPort, m_leftEncoder, m_leftPosition, m_leftRate); + sysid::SetupEncoders( + encoderType, isEncoding, period, cpr, gearing, numSamples, + m_controllerNames[0], m_rightControllers.at(0).get(), + rightEncoderInverted, rightEncoderPorts, encoderCANivoreName, + m_rightCancoder, m_rightCancoderPro, m_rightRevEncoderPort, + m_rightRevDataPort, m_rightEncoder, m_rightPosition, m_rightRate); fmt::print("Setup gyro\n"); sysid::SetupGyro(gyroType, gyroCtor, leftPorts, rightPorts, m_controllerNames, m_leftControllers, m_rightControllers, m_gyro, m_ADIS16448Gyro, m_ADIS16470Gyro, m_pigeon, - m_tempTalon, m_gyroPosition, m_gyroRate); + m_pigeonPro, m_tempTalon, gyroCANivoreName, m_gyroPosition, + m_gyroRate); } catch (std::exception& e) { fmt::print("Project failed: {}\n", e.what()); std::exit(-1); diff --git a/sysid-projects/drive/src/main/include/DriveRobot.h b/sysid-projects/drive/src/main/include/DriveRobot.h index 7f12d322..8ee7f748 100644 --- a/sysid-projects/drive/src/main/include/DriveRobot.h +++ b/sysid-projects/drive/src/main/include/DriveRobot.h @@ -9,7 +9,6 @@ #include #include -#include #include #include #include @@ -20,6 +19,11 @@ #include #include +/* Keep CTRE includes below ADIS16448_IMU header to allow Windows to build */ +#include +#include +#include + #include "sysid/logging/SysIdDrivetrainLogger.h" class DriveRobot : public frc::TimedRobot { @@ -62,8 +66,11 @@ class DriveRobot : public frc::TimedRobot { std::unique_ptr m_leftCancoder; std::unique_ptr m_rightCancoder; + std::unique_ptr m_leftCancoderPro; + std::unique_ptr m_rightCancoderPro; std::unique_ptr m_tempTalon; std::unique_ptr m_pigeon; + std::unique_ptr m_pigeonPro; sysid::SysIdDrivetrainLogger m_logger; }; diff --git a/sysid-projects/mechanism/src/main/cpp/MechanismRobot.cpp b/sysid-projects/mechanism/src/main/cpp/MechanismRobot.cpp index d8bcd32d..02186abd 100644 --- a/sysid-projects/mechanism/src/main/cpp/MechanismRobot.cpp +++ b/sysid-projects/mechanism/src/main/cpp/MechanismRobot.cpp @@ -14,8 +14,6 @@ #include #include -#include "sysid/generation/SysIdSetup.h" - MechanismRobot::MechanismRobot() : frc::TimedRobot(5_ms) { try { m_json = sysid::GetConfigJson(); @@ -24,10 +22,14 @@ MechanismRobot::MechanismRobot() : frc::TimedRobot(5_ms) { m_json.at("primary motor ports").get>(); m_controllerNames = m_json.at("motor controllers").get>(); + std::vector canivoreNames = + m_json.at("canivore names").get>(); std::vector encoderPorts = m_json.at("primary encoder ports").get>(); std::vector motorsInverted = m_json.at("primary motors inverted").get>(); + std::string encoderCANivoreName = + m_json.at("encoder canivore name").get(); std::string encoderType = m_json.at("encoder type").get(); bool encoderInverted = m_json.at("primary encoder inverted").get(); @@ -46,15 +48,16 @@ MechanismRobot::MechanismRobot() : frc::TimedRobot(5_ms) { fmt::print("Initializing Motors\n"); for (size_t i = 0; i < ports.size(); i++) { sysid::AddMotorController(ports[i], m_controllerNames[i], - motorsInverted[i], &m_controllers); + motorsInverted[i], canivoreNames[i], + &m_controllers); } fmt::print("Initializing encoder\n"); - sysid::SetupEncoders(encoderType, isEncoding, period, cpr, gearing, - numSamples, m_controllerNames[0], - m_controllers.front().get(), encoderInverted, - encoderPorts, m_cancoder, m_revEncoderPort, - m_revDataPort, m_encoder, m_position, m_rate); + sysid::SetupEncoders( + encoderType, isEncoding, period, cpr, gearing, numSamples, + m_controllerNames[0], m_controllers.front().get(), encoderInverted, + encoderPorts, encoderCANivoreName, m_cancoder, m_cancoderPro, + m_revEncoderPort, m_revDataPort, m_encoder, m_position, m_rate); } catch (std::exception& e) { fmt::print("Project failed: {}\n", e.what()); std::exit(-1); diff --git a/sysid-projects/mechanism/src/main/include/MechanismRobot.h b/sysid-projects/mechanism/src/main/include/MechanismRobot.h index 598a1c66..23eeae3d 100644 --- a/sysid-projects/mechanism/src/main/include/MechanismRobot.h +++ b/sysid-projects/mechanism/src/main/include/MechanismRobot.h @@ -9,7 +9,6 @@ #include #include -#include #include #include #include @@ -18,8 +17,13 @@ #include #include +#include "sysid/generation/SysIdSetup.h" #include "sysid/logging/SysIdGeneralMechanismLogger.h" +/* Keep CTRE includes below other headers to allow Windows to build */ +#include +#include + class MechanismRobot : public frc::TimedRobot { public: MechanismRobot(); @@ -46,6 +50,7 @@ class MechanismRobot : public frc::TimedRobot { std::unique_ptr m_revEncoderPort; std::unique_ptr m_revDataPort; std::unique_ptr m_cancoder; + std::unique_ptr m_cancoderPro; std::unique_ptr m_encoder; sysid::SysIdGeneralMechanismLogger m_logger; }; diff --git a/vendordeps/PhoenixProAnd5-frc2023-latest.json b/vendordeps/PhoenixProAnd5.json similarity index 100% rename from vendordeps/PhoenixProAnd5-frc2023-latest.json rename to vendordeps/PhoenixProAnd5.json