From d50755544f67a016db73aae7415428b44d8b7894 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 30 Jan 2025 19:59:19 -0600 Subject: [PATCH 01/12] Try to get rev 2025 fixing (doesn't build) --- lib/cpp/subzero/autonomous/AutoFactory.cpp | 2 +- lib/cpp/subzero/motor/PidMotorController.cpp | 11 +++++++++-- lib/include/subzero/autonomous/AutoFactory.h | 2 +- lib/include/subzero/motor/PidMotorController.h | 17 ++++++++++------- .../subzero/motor/PidMotorControllerPair.h | 2 +- .../singleaxis/BaseSingleAxisSubsystem.h | 6 +++--- lib/include/subzero/vision/TargetTracker.h | 2 +- 7 files changed, 26 insertions(+), 16 deletions(-) diff --git a/lib/cpp/subzero/autonomous/AutoFactory.cpp b/lib/cpp/subzero/autonomous/AutoFactory.cpp index 86fed04..c7a08c8 100644 --- a/lib/cpp/subzero/autonomous/AutoFactory.cpp +++ b/lib/cpp/subzero/autonomous/AutoFactory.cpp @@ -11,7 +11,7 @@ bool AutoFactory::AutoFileExists(const std::string fileName) { std::error_code error_code; std::unique_ptr fileBuffer = - wpi::MemoryBuffer::GetFile(filePath, error_code); + wpi::MemoryBuffer::GetFile(filePath); if (fileBuffer == nullptr || error_code) { return false; diff --git a/lib/cpp/subzero/motor/PidMotorController.cpp b/lib/cpp/subzero/motor/PidMotorController.cpp index 0a4e23c..d896ff5 100644 --- a/lib/cpp/subzero/motor/PidMotorController.cpp +++ b/lib/cpp/subzero/motor/PidMotorController.cpp @@ -74,7 +74,7 @@ void PidMotorController< m_absolutePositionEnabled = false; frc::SmartDashboard::PutNumber(m_name + "commanded rpm", rpm.value()); m_controller.SetReference(rpm.value(), - rev::CANSparkBase::ControlType::kVelocity); + rev::spark::SparkBase::ControlType::kVelocity); } template ::UpdatePidSettings(PidSettings settings) { + + rev::spark::SparkMax m; + + rev::spark::SparkMaxConfig m; + + m.Configure() + if (settings.p != m_settings.p) { ConsoleWriter.logInfo("PidMotorController", "Setting P to %.6f for %s", settings.p, m_name.c_str()); @@ -136,7 +143,7 @@ void PidMotorController #include #include -#include +#include #include #include diff --git a/lib/include/subzero/motor/PidMotorController.h b/lib/include/subzero/motor/PidMotorController.h index 897372a..1536e85 100644 --- a/lib/include/subzero/motor/PidMotorController.h +++ b/lib/include/subzero/motor/PidMotorController.h @@ -2,8 +2,9 @@ #include #include -#include -#include +#include +#include +#include #include #include @@ -110,8 +111,9 @@ class PidMotorController : public IPidMotorController { * * @param factor */ + // TODO: find missing SetPositionConversionFactor() method inline void SetEncoderConversionFactor(double factor) override { - m_encoder.SetPositionConversionFactor(factor); + // m_encoder.SetPositionConversionFactor(factor); } /** @@ -120,9 +122,10 @@ class PidMotorController : public IPidMotorController { * * @param factor */ + // TODO: find missing SetPositionConversionFactor() method inline void SetAbsoluteEncoderConversionFactor(double factor) override { if (m_absEncoder) { - m_absEncoder->SetPositionConversionFactor(factor); + // m_absEncoder->SetPositionConversionFactor(factor); } } @@ -205,7 +208,7 @@ class PidMotorControllerTuner { }; class RevPidMotorController - : public PidMotorController {}; + : public PidMotorController {}; } // namespace subzero \ No newline at end of file diff --git a/lib/include/subzero/motor/PidMotorControllerPair.h b/lib/include/subzero/motor/PidMotorControllerPair.h index bef09b8..6abdc30 100644 --- a/lib/include/subzero/motor/PidMotorControllerPair.h +++ b/lib/include/subzero/motor/PidMotorControllerPair.h @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include #include diff --git a/lib/include/subzero/singleaxis/BaseSingleAxisSubsystem.h b/lib/include/subzero/singleaxis/BaseSingleAxisSubsystem.h index 0eb0583..2aaf620 100644 --- a/lib/include/subzero/singleaxis/BaseSingleAxisSubsystem.h +++ b/lib/include/subzero/singleaxis/BaseSingleAxisSubsystem.h @@ -9,9 +9,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/lib/include/subzero/vision/TargetTracker.h b/lib/include/subzero/vision/TargetTracker.h index 02bbad9..df6a144 100644 --- a/lib/include/subzero/vision/TargetTracker.h +++ b/lib/include/subzero/vision/TargetTracker.h @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include #include From 9c2acfb14bd7ad78af5cbf8385d787587106e65c Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 31 Jan 2025 16:49:41 -0600 Subject: [PATCH 02/12] Comment out errors; code builds doesn't work --- lib/cpp/subzero/motor/PidMotorController.cpp | 70 +++++++++---------- .../subzero/motor/PidMotorController.h | 11 +++ 2 files changed, 46 insertions(+), 35 deletions(-) diff --git a/lib/cpp/subzero/motor/PidMotorController.cpp b/lib/cpp/subzero/motor/PidMotorController.cpp index d896ff5..7af6be6 100644 --- a/lib/cpp/subzero/motor/PidMotorController.cpp +++ b/lib/cpp/subzero/motor/PidMotorController.cpp @@ -128,41 +128,41 @@ void PidMotorController::UpdatePidSettings(PidSettings settings) { - rev::spark::SparkMax m; - - rev::spark::SparkMaxConfig m; - - m.Configure() - - if (settings.p != m_settings.p) { - ConsoleWriter.logInfo("PidMotorController", "Setting P to %.6f for %s", - settings.p, m_name.c_str()); - m_controller.SetP(settings.p); - } - - if (settings.i != m_settings.i) { - ConsoleWriter.logInfo("PidMotorController", "Setting I to %.6f for %s", - settings.i, m_name.c_str()); - m_controller.SetP(settings.i); - } - - if (settings.d != m_settings.d) { - ConsoleWriter.logInfo("PidMotorController", "Setting D to %.6f for %s", - settings.d, m_name.c_str()); - m_controller.SetD(settings.d); - } - - if (settings.iZone != m_settings.iZone) { - ConsoleWriter.logInfo("PidMotorController", "Setting IZone to %.6f for %s", - settings.iZone, m_name.c_str()); - m_controller.SetIZone(settings.iZone); - } - - if (settings.ff != m_settings.ff) { - ConsoleWriter.logInfo("PidMotorController", "Setting FF to %.6f for %s", - settings.ff, m_name.c_str()); - m_controller.SetFF(settings.ff); - } + // rev::spark::SparkMax m; + + // rev::spark::SparkMaxConfig m; + + // m.Configure() + + // if (settings.p != m_settings.p) { + // ConsoleWriter.logInfo("PidMotorController", "Setting P to %.6f for %s", + // settings.p, m_name.c_str()); + // m_controller.SetP(settings.p); + // } + + // if (settings.i != m_settings.i) { + // ConsoleWriter.logInfo("PidMotorController", "Setting I to %.6f for %s", + // settings.i, m_name.c_str()); + // m_controller.SetP(settings.i); + // } + + // if (settings.d != m_settings.d) { + // ConsoleWriter.logInfo("PidMotorController", "Setting D to %.6f for %s", + // settings.d, m_name.c_str()); + // m_controller.SetD(settings.d); + // } + + // if (settings.iZone != m_settings.iZone) { + // ConsoleWriter.logInfo("PidMotorController", "Setting IZone to %.6f for %s", + // settings.iZone, m_name.c_str()); + // m_controller.SetIZone(settings.iZone); + // } + + // if (settings.ff != m_settings.ff) { + // ConsoleWriter.logInfo("PidMotorController", "Setting FF to %.6f for %s", + // settings.ff, m_name.c_str()); + // m_controller.SetFF(settings.ff); + // } m_settings = settings; } \ No newline at end of file diff --git a/lib/include/subzero/motor/PidMotorController.h b/lib/include/subzero/motor/PidMotorController.h index 1536e85..10fc355 100644 --- a/lib/include/subzero/motor/PidMotorController.h +++ b/lib/include/subzero/motor/PidMotorController.h @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include @@ -181,6 +183,13 @@ class PidMotorControllerTuner { m_controller.GetPidSettings().ff); } + /// @brief Sets the setpoint of the pid controller + /// @param setPoint The setpoint of the PID + void SetSetpoint(double setPoint); + + /// @brief Sets the PID coefficients + void SetPidCoefficients(double p, double i, double d, double iZone, double ff); + /** * @brief Call this within the Periodic method of the encapsulating subsystem * @@ -207,6 +216,8 @@ class PidMotorControllerTuner { &m_controller; }; + + class RevPidMotorController : public PidMotorController Date: Fri, 31 Jan 2025 18:24:08 -0600 Subject: [PATCH 03/12] Use rev pid config (builds) --- lib/cpp/subzero/motor/PidMotorController.cpp | 126 ++++++++++-------- .../subzero/motor/IPidMotorController.h | 2 + .../subzero/motor/PidMotorController.h | 21 ++- 3 files changed, 78 insertions(+), 71 deletions(-) diff --git a/lib/cpp/subzero/motor/PidMotorController.cpp b/lib/cpp/subzero/motor/PidMotorController.cpp index 7af6be6..aa90240 100644 --- a/lib/cpp/subzero/motor/PidMotorController.cpp +++ b/lib/cpp/subzero/motor/PidMotorController.cpp @@ -5,8 +5,8 @@ using namespace subzero; template -PidMotorController:: + typename TAbsoluteEncoder, typename TPidConfig> +PidMotorController:: PidMotorController(std::string name, TMotor &motor, TRelativeEncoder &encoder, TController &controller, PidSettings pidSettings, TAbsoluteEncoder *absEncoder, @@ -16,36 +16,37 @@ PidMotorController:: m_pidController{ frc::PIDController{pidSettings.p, pidSettings.i, pidSettings.d}}, m_maxRpm{maxRpm} { + // Doing it here so the PID controllers themselves get updated UpdatePidSettings(pidSettings); } template + typename TAbsoluteEncoder, typename TPidConfig> void PidMotorController::Set(units::volt_t volts) { + TAbsoluteEncoder, TPidConfig>::Set(units::volt_t volts) { frc::SmartDashboard::PutNumber(m_name + " Commanded volts", volts.value()); m_motor.SetVoltage(volts); } template + typename TAbsoluteEncoder, typename TPidConfig> void PidMotorController::Set(double percentage) { + TAbsoluteEncoder, TPidConfig>::Set(double percentage) { m_motor.Set(percentage); } template + typename TAbsoluteEncoder, typename TPidConfig> void PidMotorController::SetPidTolerance(double tolerance) { + TAbsoluteEncoder, TPidConfig>::SetPidTolerance(double tolerance) { m_pidController.SetTolerance(tolerance); } template + typename TAbsoluteEncoder, typename TPidConfig> void PidMotorController::Update() { + TAbsoluteEncoder, TPidConfig>::Update() { if (m_absolutePositionEnabled) { // ConsoleWriter.logVerbose( // m_name, @@ -67,10 +68,10 @@ void PidMotorController + typename TAbsoluteEncoder, typename TPidConfig> void PidMotorController< TMotor, TController, TRelativeEncoder, - TAbsoluteEncoder>::RunWithVelocity(units::revolutions_per_minute_t rpm) { + TAbsoluteEncoder, TPidConfig>::RunWithVelocity(units::revolutions_per_minute_t rpm) { m_absolutePositionEnabled = false; frc::SmartDashboard::PutNumber(m_name + "commanded rpm", rpm.value()); m_controller.SetReference(rpm.value(), @@ -78,9 +79,9 @@ void PidMotorController< } template + typename TAbsoluteEncoder, typename TPidConfig> void PidMotorController::RunWithVelocity(double percentage) { + TAbsoluteEncoder, TPidConfig>::RunWithVelocity(double percentage) { if (abs(percentage) > 1.0) { ConsoleWriter.logError("PidMotorController", "Incorrect percentages for motor %s: Value=%.4f ", @@ -92,9 +93,9 @@ void PidMotorController + typename TAbsoluteEncoder, typename TPidConfig> void PidMotorController::RunToPosition(double position) { + TAbsoluteEncoder, TPidConfig>::RunToPosition(double position) { ConsoleWriter.logVerbose(m_name + " Target position", position); Stop(); m_pidController.Reset(); @@ -103,10 +104,10 @@ void PidMotorController + typename TAbsoluteEncoder, typename TPidConfig> std::optional PidMotorController::GetAbsoluteEncoderPosition() { + TAbsoluteEncoder, TPidConfig>::GetAbsoluteEncoderPosition() { if (m_absEncoder) { return m_absEncoder->GetPosition(); } @@ -115,54 +116,61 @@ PidMotorController + typename TAbsoluteEncoder, typename TPidConfig> void PidMotorController::Stop() { + TAbsoluteEncoder, TPidConfig>::Stop() { m_absolutePositionEnabled = false; m_motor.Set(0); } +// TODO: make PidSettings params optional since only one is used at a time template + typename TAbsoluteEncoder, typename TPidConfig> void PidMotorController::UpdatePidSettings(PidSettings - settings) { - - // rev::spark::SparkMax m; - - // rev::spark::SparkMaxConfig m; - - // m.Configure() - - // if (settings.p != m_settings.p) { - // ConsoleWriter.logInfo("PidMotorController", "Setting P to %.6f for %s", - // settings.p, m_name.c_str()); - // m_controller.SetP(settings.p); - // } - - // if (settings.i != m_settings.i) { - // ConsoleWriter.logInfo("PidMotorController", "Setting I to %.6f for %s", - // settings.i, m_name.c_str()); - // m_controller.SetP(settings.i); - // } - - // if (settings.d != m_settings.d) { - // ConsoleWriter.logInfo("PidMotorController", "Setting D to %.6f for %s", - // settings.d, m_name.c_str()); - // m_controller.SetD(settings.d); - // } - - // if (settings.iZone != m_settings.iZone) { - // ConsoleWriter.logInfo("PidMotorController", "Setting IZone to %.6f for %s", - // settings.iZone, m_name.c_str()); - // m_controller.SetIZone(settings.iZone); - // } - - // if (settings.ff != m_settings.ff) { - // ConsoleWriter.logInfo("PidMotorController", "Setting FF to %.6f for %s", - // settings.ff, m_name.c_str()); - // m_controller.SetFF(settings.ff); - // } + TAbsoluteEncoder, TPidConfig>::UpdatePidSettings(PidSettings settings) { + + bool changed = false; + + if (settings.p != m_settings.p) { + ConsoleWriter.logInfo("PidMotorController", "Setting P to %.6f for %s", + settings.p, m_name.c_str()); + + m_config.closedLoop.P(settings.p); + changed = true; + } + + if (settings.i != m_settings.i) { + ConsoleWriter.logInfo("PidMotorController", "Setting I to %.6f for %s", + settings.i, m_name.c_str()); + m_config.closedLoop.I(settings.i); + changed = true; + } + + if (settings.d != m_settings.d) { + ConsoleWriter.logInfo("PidMotorController", "Setting D to %.6f for %s", + settings.d, m_name.c_str()); + m_config.closedLoop.D(settings.d); + changed = true; + } + + if (settings.iZone != m_settings.iZone) { + ConsoleWriter.logInfo("PidMotorController", "Setting IZone to %.6f for %s", + settings.iZone, m_name.c_str()); + m_config.closedLoop.IZone(settings.iZone); + changed = true; + } + + if (settings.ff != m_settings.ff) { + ConsoleWriter.logInfo("PidMotorController", "Setting FF to %.6f for %s", + settings.ff, m_name.c_str()); + m_config.closedLoop.VelocityFF(settings.ff); + changed = true; + } + + if (changed) { + m_motor.Configure(m_config, rev::spark::SparkBase::ResetMode::kNoResetSafeParameters, + rev::spark::SparkBase::PersistMode::kPersistParameters); + } m_settings = settings; } \ No newline at end of file diff --git a/lib/include/subzero/motor/IPidMotorController.h b/lib/include/subzero/motor/IPidMotorController.h index 3971245..fbd1d43 100644 --- a/lib/include/subzero/motor/IPidMotorController.h +++ b/lib/include/subzero/motor/IPidMotorController.h @@ -7,6 +7,8 @@ #include namespace subzero { + +// TODO: Add slot support inside of PidSettings struct struct PidSettings { double p, i, d, iZone, ff; }; diff --git a/lib/include/subzero/motor/PidMotorController.h b/lib/include/subzero/motor/PidMotorController.h index 10fc355..8cf136f 100644 --- a/lib/include/subzero/motor/PidMotorController.h +++ b/lib/include/subzero/motor/PidMotorController.h @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -27,7 +28,7 @@ namespace subzero { * @tparam TAbsoluteEncoder */ template + typename TAbsoluteEncoder, typename TPidConfig> class PidMotorController : public IPidMotorController { public: /** @@ -139,13 +140,15 @@ class PidMotorController : public IPidMotorController { inline const PidSettings &GetPidSettings() override { return m_settings; } - void UpdatePidSettings(PidSettings settings) override; + void UpdatePidSettings(PidSettings settings); protected: TMotor &m_motor; TController &m_controller; TRelativeEncoder &m_encoder; TAbsoluteEncoder *m_absEncoder; + // TODO, use template type for config + rev::spark::SparkFlexConfig m_config; PidSettings m_settings; frc::PIDController m_pidController; bool m_absolutePositionEnabled = false; @@ -168,8 +171,9 @@ template &controller) + TAbsoluteEncoder, rev::spark::SparkFlexConfig> &controller) : m_controller{controller} { frc::SmartDashboard::PutNumber(m_controller.m_name + " P Gain", m_controller.GetPidSettings().p); @@ -183,13 +187,6 @@ class PidMotorControllerTuner { m_controller.GetPidSettings().ff); } - /// @brief Sets the setpoint of the pid controller - /// @param setPoint The setpoint of the PID - void SetSetpoint(double setPoint); - - /// @brief Sets the PID coefficients - void SetPidCoefficients(double p, double i, double d, double iZone, double ff); - /** * @brief Call this within the Periodic method of the encapsulating subsystem * @@ -212,7 +209,7 @@ class PidMotorControllerTuner { } private: - PidMotorController + PidMotorController &m_controller; }; @@ -221,5 +218,5 @@ class PidMotorControllerTuner { class RevPidMotorController : public PidMotorController {}; + rev::spark::SparkAbsoluteEncoder, rev::spark::SparkBaseConfig> {}; } // namespace subzero \ No newline at end of file From 9df00f6e91c3e94f44c88b2fb9bb2dba7b6563bc Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 31 Jan 2025 18:52:14 -0600 Subject: [PATCH 04/12] Fix `SetEncoderConversionFactor()` and `SetAbsoluteEncoderConversionFactor()` --- lib/include/subzero/motor/PidMotorController.h | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/lib/include/subzero/motor/PidMotorController.h b/lib/include/subzero/motor/PidMotorController.h index 8cf136f..83f4532 100644 --- a/lib/include/subzero/motor/PidMotorController.h +++ b/lib/include/subzero/motor/PidMotorController.h @@ -43,6 +43,8 @@ class PidMotorController : public IPidMotorController { * @param maxRpm Max RPM of the motor; used to set velocity based on * percentage */ + + // TODO: make `absEncoder` an `std::optional<>` instead of a raw pointer explicit PidMotorController(std::string name, TMotor &motor, TRelativeEncoder &encoder, TController &controller, PidSettings pidSettings, @@ -116,7 +118,11 @@ class PidMotorController : public IPidMotorController { */ // TODO: find missing SetPositionConversionFactor() method inline void SetEncoderConversionFactor(double factor) override { - // m_encoder.SetPositionConversionFactor(factor); + m_config.encoder.PositionConversionFactor(factor); + m_config.encoder.VelocityConversionFactor(factor); + + m_motor.Configure(m_config, rev::spark::SparkBase::ResetMode::kNoResetSafeParameters, + rev::spark::SparkBase::PersistMode::kPersistParameters); } /** @@ -128,7 +134,11 @@ class PidMotorController : public IPidMotorController { // TODO: find missing SetPositionConversionFactor() method inline void SetAbsoluteEncoderConversionFactor(double factor) override { if (m_absEncoder) { - // m_absEncoder->SetPositionConversionFactor(factor); + m_config.absoluteEncoder.PositionConversionFactor(factor); + m_config.absoluteEncoder.VelocityConversionFactor(factor); + + m_motor.Configure(m_config, rev::spark::SparkBase::ResetMode::kNoResetSafeParameters, + rev::spark::SparkBase::PersistMode::kPersistParameters); } } From 4755bb528ae8175726620895b09c19b364979b38 Mon Sep 17 00:00:00 2001 From: Lint Action Date: Sat, 1 Feb 2025 01:38:30 +0000 Subject: [PATCH 05/12] Fix code style issues with clang_format --- lib/cpp/subzero/motor/PidMotorController.cpp | 60 ++++++++++--------- .../subzero/motor/PidMotorController.h | 32 +++++----- .../singleaxis/BaseSingleAxisSubsystem.h | 2 +- 3 files changed, 50 insertions(+), 44 deletions(-) diff --git a/lib/cpp/subzero/motor/PidMotorController.cpp b/lib/cpp/subzero/motor/PidMotorController.cpp index 909cde6..361bfd5 100644 --- a/lib/cpp/subzero/motor/PidMotorController.cpp +++ b/lib/cpp/subzero/motor/PidMotorController.cpp @@ -6,47 +6,50 @@ using namespace subzero; template -PidMotorController:: - PidMotorController(std::string name, TMotor &motor, - TRelativeEncoder &encoder, TController &controller, - PidSettings pidSettings, TAbsoluteEncoder *absEncoder, - units::revolutions_per_minute_t maxRpm) +PidMotorController< + TMotor, TController, TRelativeEncoder, TAbsoluteEncoder, + TPidConfig>::PidMotorController(std::string name, TMotor &motor, + TRelativeEncoder &encoder, + TController &controller, + PidSettings pidSettings, + TAbsoluteEncoder *absEncoder, + units::revolutions_per_minute_t maxRpm) : IPidMotorController(name), m_motor{motor}, m_controller{controller}, m_encoder{encoder}, m_absEncoder{absEncoder}, m_settings{pidSettings}, m_pidController{ frc::PIDController{pidSettings.p, pidSettings.i, pidSettings.d}}, m_maxRpm{maxRpm} { - + // Doing it here so the PID controllers themselves get updated UpdatePidSettings(pidSettings); } template -void PidMotorController::Set(units::volt_t volts) { +void PidMotorController::Set(units::volt_t volts) { frc::SmartDashboard::PutNumber(m_name + " Commanded volts", volts.value()); m_motor.SetVoltage(volts); } template -void PidMotorController::Set(double percentage) { +void PidMotorController::Set(double percentage) { m_motor.Set(percentage); } template -void PidMotorController::SetPidTolerance(double tolerance) { +void PidMotorController::SetPidTolerance(double tolerance) { m_pidController.SetTolerance(tolerance); } template -void PidMotorController::Update() { +void PidMotorController::Update() { if (m_absolutePositionEnabled) { // ConsoleWriter.logVerbose( // m_name, @@ -70,8 +73,8 @@ void PidMotorController void PidMotorController< - TMotor, TController, TRelativeEncoder, - TAbsoluteEncoder, TPidConfig>::RunWithVelocity(units::revolutions_per_minute_t rpm) { + TMotor, TController, TRelativeEncoder, TAbsoluteEncoder, + TPidConfig>::RunWithVelocity(units::revolutions_per_minute_t rpm) { m_absolutePositionEnabled = false; frc::SmartDashboard::PutNumber(m_name + "commanded rpm", rpm.value()); m_controller.SetReference(rpm.value(), @@ -80,8 +83,8 @@ void PidMotorController< template -void PidMotorController::RunWithVelocity(double percentage) { +void PidMotorController::RunWithVelocity(double percentage) { if (abs(percentage) > 1.0) { ConsoleWriter.logError("PidMotorController", "Incorrect percentages for motor %s: Value=%.4f ", @@ -94,8 +97,8 @@ void PidMotorController -void PidMotorController::RunToPosition(double position) { +void PidMotorController::RunToPosition(double position) { ConsoleWriter.logVerbose(m_name + " Target position", position); Stop(); m_pidController.Reset(); @@ -106,8 +109,8 @@ void PidMotorController std::optional -PidMotorController::GetAbsoluteEncoderPosition() { +PidMotorController::GetAbsoluteEncoderPosition() { if (m_absEncoder) { return m_absEncoder->GetPosition(); } @@ -117,8 +120,8 @@ PidMotorController -void PidMotorController::Stop() { +void PidMotorController::Stop() { m_absolutePositionEnabled = false; m_motor.Set(0); } @@ -126,8 +129,8 @@ void PidMotorController -void PidMotorController::UpdatePidSettings(PidSettings settings) { +void PidMotorController::UpdatePidSettings(PidSettings settings) { bool changed = false; @@ -168,8 +171,9 @@ void PidMotorController #include -#include +#include #include -#include #include -#include #include +#include +#include #include #include @@ -121,8 +121,9 @@ class PidMotorController : public IPidMotorController { m_config.encoder.PositionConversionFactor(factor); m_config.encoder.VelocityConversionFactor(factor); - m_motor.Configure(m_config, rev::spark::SparkBase::ResetMode::kNoResetSafeParameters, - rev::spark::SparkBase::PersistMode::kPersistParameters); + m_motor.Configure(m_config, + rev::spark::SparkBase::ResetMode::kNoResetSafeParameters, + rev::spark::SparkBase::PersistMode::kPersistParameters); } /** @@ -137,8 +138,9 @@ class PidMotorController : public IPidMotorController { m_config.absoluteEncoder.PositionConversionFactor(factor); m_config.absoluteEncoder.VelocityConversionFactor(factor); - m_motor.Configure(m_config, rev::spark::SparkBase::ResetMode::kNoResetSafeParameters, - rev::spark::SparkBase::PersistMode::kPersistParameters); + m_motor.Configure( + m_config, rev::spark::SparkBase::ResetMode::kNoResetSafeParameters, + rev::spark::SparkBase::PersistMode::kPersistParameters); } } @@ -183,7 +185,8 @@ class PidMotorControllerTuner { explicit PidMotorControllerTuner( // TODO, use template type for config PidMotorController &controller) + TAbsoluteEncoder, rev::spark::SparkFlexConfig> + &controller) : m_controller{controller} { frc::SmartDashboard::PutNumber(m_controller.m_name + " P Gain", m_controller.GetPidSettings().p); @@ -219,14 +222,13 @@ class PidMotorControllerTuner { } private: - PidMotorController - &m_controller; + PidMotorController &m_controller; }; - - class RevPidMotorController - : public PidMotorController {}; + : public PidMotorController< + rev::spark::SparkMax, rev::spark::SparkClosedLoopController, + rev::spark::SparkRelativeEncoder, rev::spark::SparkAbsoluteEncoder, + rev::spark::SparkBaseConfig> {}; } // namespace subzero \ No newline at end of file diff --git a/lib/include/subzero/singleaxis/BaseSingleAxisSubsystem.h b/lib/include/subzero/singleaxis/BaseSingleAxisSubsystem.h index 2aaf620..2f7e5a5 100644 --- a/lib/include/subzero/singleaxis/BaseSingleAxisSubsystem.h +++ b/lib/include/subzero/singleaxis/BaseSingleAxisSubsystem.h @@ -9,9 +9,9 @@ #include #include #include +#include #include #include -#include #include #include #include From f57a4f7fa2530a045ba568e2dd8168c11a13d0e1 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 31 Jan 2025 19:50:10 -0600 Subject: [PATCH 06/12] Remove autofactory method implementations in header file --- lib/include/subzero/autonomous/AutoFactory.h | 39 ++------------------ 1 file changed, 3 insertions(+), 36 deletions(-) diff --git a/lib/include/subzero/autonomous/AutoFactory.h b/lib/include/subzero/autonomous/AutoFactory.h index bd4abf1..461ca83 100644 --- a/lib/include/subzero/autonomous/AutoFactory.h +++ b/lib/include/subzero/autonomous/AutoFactory.h @@ -34,31 +34,9 @@ template class AutoFactory { private: const std::map &m_autos; - bool AutoFileExists(const std::string fileName) { - const std::string filePath = frc::filesystem::GetDeployDirectory() + - "/pathplanner/autos/" + fileName + ".auto"; + bool AutoFileExists(const std::string fileName); - // Use wpi::MemoryBuffer::GetFile and handle the wpi::expected result - auto fileBufferResult = wpi::MemoryBuffer::GetFile(filePath); - - if (!fileBufferResult) { - // fileBufferResult is an error; the file does not exist - return false; - } - - // fileBufferResult contains a valid std::unique_ptr - return true; - } - - frc2::CommandPtr PathPlannerPathFromName(const std::string autoName) { - if (!AutoFileExists(autoName)) { - ConsoleWriter.logError("Auto Factory", - "AUTO '%s' DOES NOT EXIST HELP US EVAN", - autoName.c_str()); - return EmptyCommand().ToPtr(); - } - return pathplanner::PathPlannerAuto(autoName).ToPtr(); - } + frc2::CommandPtr PathPlannerPathFromName(const std::string autoName); public: /** @@ -67,18 +45,7 @@ template class AutoFactory { * @param type * @return frc2::CommandPtr The schedulable auto command */ - frc2::CommandPtr GetAuto(T type) { - if (!m_autos.contains(type)) { - ConsoleWriter.logWarning( - "Auto Factory", - "Auto type %d does not exist, defaulting to empty " - "auto", - static_cast(type)); - return EmptyCommand().ToPtr(); - } - - return PathPlannerPathFromName(m_autos.at(type)); - } + frc2::CommandPtr GetAuto(T type); }; } // namespace subzero From 84cc6416120b163ec1043dbc13a8129679f62f64 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 31 Jan 2025 19:57:44 -0600 Subject: [PATCH 07/12] Add template type for pid config in `PidMotorControllerTuner` --- lib/include/subzero/motor/PidMotorController.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/lib/include/subzero/motor/PidMotorController.h b/lib/include/subzero/motor/PidMotorController.h index b25e1d4..c4881fd 100644 --- a/lib/include/subzero/motor/PidMotorController.h +++ b/lib/include/subzero/motor/PidMotorController.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -159,8 +160,7 @@ class PidMotorController : public IPidMotorController { TController &m_controller; TRelativeEncoder &m_encoder; TAbsoluteEncoder *m_absEncoder; - // TODO, use template type for config - rev::spark::SparkFlexConfig m_config; + TPidConfig m_config; PidSettings m_settings; frc::PIDController m_pidController; bool m_absolutePositionEnabled = false; @@ -179,13 +179,12 @@ class PidMotorController : public IPidMotorController { * @tparam TAbsoluteEncoder */ template + typename TAbsoluteEncoder, typename TPidConfig> class PidMotorControllerTuner { public: explicit PidMotorControllerTuner( - // TODO, use template type for config PidMotorController + TAbsoluteEncoder, TPidConfig> &controller) : m_controller{controller} { frc::SmartDashboard::PutNumber(m_controller.m_name + " P Gain", @@ -230,5 +229,5 @@ class RevPidMotorController : public PidMotorController< rev::spark::SparkMax, rev::spark::SparkClosedLoopController, rev::spark::SparkRelativeEncoder, rev::spark::SparkAbsoluteEncoder, - rev::spark::SparkBaseConfig> {}; + rev::spark::SparkMaxConfig> {}; } // namespace subzero \ No newline at end of file From 7efa68e23d1521576369baa850b2ee7d81698424 Mon Sep 17 00:00:00 2001 From: Lint Action Date: Sat, 1 Feb 2025 01:58:15 +0000 Subject: [PATCH 08/12] Fix code style issues with clang_format --- lib/include/subzero/motor/PidMotorController.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/lib/include/subzero/motor/PidMotorController.h b/lib/include/subzero/motor/PidMotorController.h index c4881fd..7da2dfd 100644 --- a/lib/include/subzero/motor/PidMotorController.h +++ b/lib/include/subzero/motor/PidMotorController.h @@ -5,10 +5,10 @@ #include #include #include -#include #include #include #include +#include #include #include @@ -184,8 +184,7 @@ class PidMotorControllerTuner { public: explicit PidMotorControllerTuner( PidMotorController - &controller) + TAbsoluteEncoder, TPidConfig> &controller) : m_controller{controller} { frc::SmartDashboard::PutNumber(m_controller.m_name + " P Gain", m_controller.GetPidSettings().p); From 3372753254e5a39e024b0d9feb051faead8b01ba Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 3 Feb 2025 16:07:53 -0600 Subject: [PATCH 09/12] Remove solved TODOs --- lib/include/subzero/motor/PidMotorController.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/lib/include/subzero/motor/PidMotorController.h b/lib/include/subzero/motor/PidMotorController.h index 7da2dfd..491eb28 100644 --- a/lib/include/subzero/motor/PidMotorController.h +++ b/lib/include/subzero/motor/PidMotorController.h @@ -117,7 +117,6 @@ class PidMotorController : public IPidMotorController { * * @param factor */ - // TODO: find missing SetPositionConversionFactor() method inline void SetEncoderConversionFactor(double factor) override { m_config.encoder.PositionConversionFactor(factor); m_config.encoder.VelocityConversionFactor(factor); @@ -133,7 +132,6 @@ class PidMotorController : public IPidMotorController { * * @param factor */ - // TODO: find missing SetPositionConversionFactor() method inline void SetAbsoluteEncoderConversionFactor(double factor) override { if (m_absEncoder) { m_config.absoluteEncoder.PositionConversionFactor(factor); From 64a021909d5dd9b0b47fc8f6d0c12ad8a27183e5 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 3 Feb 2025 16:41:38 -0600 Subject: [PATCH 10/12] Remove `TODO` in PID --- lib/cpp/subzero/motor/PidMotorController.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/lib/cpp/subzero/motor/PidMotorController.cpp b/lib/cpp/subzero/motor/PidMotorController.cpp index 361bfd5..40b2fd3 100644 --- a/lib/cpp/subzero/motor/PidMotorController.cpp +++ b/lib/cpp/subzero/motor/PidMotorController.cpp @@ -126,7 +126,6 @@ void PidMotorController void PidMotorController Date: Mon, 3 Feb 2025 19:26:07 -0600 Subject: [PATCH 11/12] Add default behavior to pit settings object --- lib/cpp/subzero/motor/PidMotorController.cpp | 20 ++++++++++++------- .../subzero/motor/IPidMotorController.h | 1 + .../subzero/motor/PidMotorController.h | 4 +++- 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/lib/cpp/subzero/motor/PidMotorController.cpp b/lib/cpp/subzero/motor/PidMotorController.cpp index 40b2fd3..3f28003 100644 --- a/lib/cpp/subzero/motor/PidMotorController.cpp +++ b/lib/cpp/subzero/motor/PidMotorController.cpp @@ -18,7 +18,7 @@ PidMotorController< m_encoder{encoder}, m_absEncoder{absEncoder}, m_settings{pidSettings}, m_pidController{ frc::PIDController{pidSettings.p, pidSettings.i, pidSettings.d}}, - m_maxRpm{maxRpm} { + m_maxRpm{maxRpm}, m_isInitialized{false} { // Doing it here so the PID controllers themselves get updated UpdatePidSettings(pidSettings); @@ -133,7 +133,7 @@ void PidMotorController` instead of a raw pointer explicit PidMotorController(std::string name, TMotor &motor, TRelativeEncoder &encoder, - TController &controller, PidSettings pidSettings, + TController &controller, + PidSettings pidSettings, TAbsoluteEncoder *absEncoder, units::revolutions_per_minute_t maxRpm); @@ -164,6 +165,7 @@ class PidMotorController : public IPidMotorController { bool m_absolutePositionEnabled = false; double m_absoluteTarget = 0; const units::revolutions_per_minute_t m_maxRpm; + bool m_isInitialized; }; // TODO: Move to its own file and make it work with IPidMotorController From a9ff92b3a84e8d3e6d588a1d2a547aac033a92e5 Mon Sep 17 00:00:00 2001 From: Lint Action Date: Tue, 4 Feb 2025 01:26:42 +0000 Subject: [PATCH 12/12] Fix code style issues with clang_format --- lib/cpp/subzero/motor/PidMotorController.cpp | 7 +++++-- lib/include/subzero/motor/PidMotorController.h | 3 +-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/lib/cpp/subzero/motor/PidMotorController.cpp b/lib/cpp/subzero/motor/PidMotorController.cpp index 3f28003..fd2bfa8 100644 --- a/lib/cpp/subzero/motor/PidMotorController.cpp +++ b/lib/cpp/subzero/motor/PidMotorController.cpp @@ -169,8 +169,11 @@ void PidMotorController` instead of a raw pointer explicit PidMotorController(std::string name, TMotor &motor, TRelativeEncoder &encoder, - TController &controller, - PidSettings pidSettings, + TController &controller, PidSettings pidSettings, TAbsoluteEncoder *absEncoder, units::revolutions_per_minute_t maxRpm);