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 d310f03..fd2bfa8 100644 --- a/lib/cpp/subzero/motor/PidMotorController.cpp +++ b/lib/cpp/subzero/motor/PidMotorController.cpp @@ -5,47 +5,51 @@ 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) + typename TAbsoluteEncoder, typename TPidConfig> +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} { + m_maxRpm{maxRpm}, m_isInitialized{false} { + // Doing it here so the PID controllers themselves get updated UpdatePidSettings(pidSettings); } template -void PidMotorController::Set(units::volt_t volts) { + typename TAbsoluteEncoder, typename TPidConfig> +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) { + typename TAbsoluteEncoder, typename TPidConfig> +void PidMotorController::Set(double percentage) { m_motor.Set(percentage); } template -void PidMotorController::SetPidTolerance(double tolerance) { + typename TAbsoluteEncoder, typename TPidConfig> +void PidMotorController::SetPidTolerance(double tolerance) { m_pidController.SetTolerance(tolerance); } template -void PidMotorController::Update() { + typename TAbsoluteEncoder, typename TPidConfig> +void PidMotorController::Update() { if (m_absolutePositionEnabled) { // ConsoleWriter.logVerbose( // m_name, @@ -67,10 +71,10 @@ void PidMotorController + typename TAbsoluteEncoder, typename TPidConfig> void PidMotorController< - TMotor, TController, TRelativeEncoder, - TAbsoluteEncoder>::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(), @@ -78,9 +82,9 @@ void PidMotorController< } template -void PidMotorController::RunWithVelocity(double percentage) { + typename TAbsoluteEncoder, typename TPidConfig> +void PidMotorController::RunWithVelocity(double percentage) { if (abs(percentage) > 1.0) { ConsoleWriter.logError("PidMotorController", "Incorrect percentages for motor %s: Value=%.4f ", @@ -92,9 +96,9 @@ void PidMotorController -void PidMotorController::RunToPosition(double position) { + typename TAbsoluteEncoder, typename TPidConfig> +void PidMotorController::RunToPosition(double position) { ConsoleWriter.logVerbose(m_name + " Target position", position); Stop(); m_pidController.Reset(); @@ -103,10 +107,10 @@ void PidMotorController + typename TAbsoluteEncoder, typename TPidConfig> std::optional -PidMotorController::GetAbsoluteEncoderPosition() { +PidMotorController::GetAbsoluteEncoderPosition() { if (m_absEncoder) { return m_absEncoder->GetPosition(); } @@ -115,46 +119,69 @@ PidMotorController -void PidMotorController::Stop() { + typename TAbsoluteEncoder, typename TPidConfig> +void PidMotorController::Stop() { m_absolutePositionEnabled = false; m_motor.Set(0); } template -void PidMotorController::UpdatePidSettings(PidSettings - settings) { - if (settings.p != m_settings.p) { + typename TAbsoluteEncoder, typename TPidConfig> +void PidMotorController::UpdatePidSettings(PidSettings settings) { + + bool changed = false; + + if (settings.p != m_settings.p || !m_isInitialized) { ConsoleWriter.logInfo("PidMotorController", "Setting P to %.6f for %s", settings.p, m_name.c_str()); - m_controller.SetP(settings.p); + + m_config.closedLoop.P(settings.p); + changed = true; } - if (settings.i != m_settings.i) { + if (settings.i != m_settings.i || !m_isInitialized) { ConsoleWriter.logInfo("PidMotorController", "Setting I to %.6f for %s", settings.i, m_name.c_str()); - m_controller.SetI(settings.i); + m_config.closedLoop.I(settings.i); + changed = true; } - if (settings.d != m_settings.d) { + if (settings.d != m_settings.d || !m_isInitialized) { ConsoleWriter.logInfo("PidMotorController", "Setting D to %.6f for %s", settings.d, m_name.c_str()); - m_controller.SetD(settings.d); + m_config.closedLoop.D(settings.d); + changed = true; } - if (settings.iZone != m_settings.iZone) { + if (settings.iZone != m_settings.iZone || !m_isInitialized) { ConsoleWriter.logInfo("PidMotorController", "Setting IZone to %.6f for %s", settings.iZone, m_name.c_str()); - m_controller.SetIZone(settings.iZone); + m_config.closedLoop.IZone(settings.iZone); + changed = true; } - if (settings.ff != m_settings.ff) { + if (settings.ff != m_settings.ff || !m_isInitialized) { ConsoleWriter.logInfo("PidMotorController", "Setting FF to %.6f for %s", settings.ff, m_name.c_str()); - m_controller.SetFF(settings.ff); + m_config.closedLoop.VelocityFF(settings.ff); + changed = true; + } + + if (settings.isIdleModeBrake != m_settings.isIdleModeBrake || + !m_isInitialized) { + m_config.SetIdleMode(m_settings.isIdleModeBrake + ? rev::spark::SparkBaseConfig::IdleMode::kBrake + : rev::spark::SparkBaseConfig::IdleMode::kCoast); + changed = true; + } + + if (changed || !m_isInitialized) { + m_motor.Configure(m_config, + rev::spark::SparkBase::ResetMode::kNoResetSafeParameters, + rev::spark::SparkBase::PersistMode::kPersistParameters); + m_isInitialized = true; } m_settings = settings; 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 diff --git a/lib/include/subzero/motor/IPidMotorController.h b/lib/include/subzero/motor/IPidMotorController.h index 3971245..719bccd 100644 --- a/lib/include/subzero/motor/IPidMotorController.h +++ b/lib/include/subzero/motor/IPidMotorController.h @@ -7,8 +7,11 @@ #include namespace subzero { + +// TODO: Add slot support inside of PidSettings struct struct PidSettings { double p, i, d, iZone, ff; + bool isIdleModeBrake; }; class IPidMotorController { diff --git a/lib/include/subzero/motor/PidMotorController.h b/lib/include/subzero/motor/PidMotorController.h index 7dcf831..19313f1 100644 --- a/lib/include/subzero/motor/PidMotorController.h +++ b/lib/include/subzero/motor/PidMotorController.h @@ -2,7 +2,13 @@ #include #include +#include +#include +#include +#include #include +#include +#include #include #include @@ -23,7 +29,7 @@ namespace subzero { * @tparam TAbsoluteEncoder */ template + typename TAbsoluteEncoder, typename TPidConfig> class PidMotorController : public IPidMotorController { public: /** @@ -38,6 +44,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, @@ -110,7 +118,12 @@ class PidMotorController : public IPidMotorController { * @param factor */ 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); } /** @@ -121,7 +134,12 @@ class PidMotorController : public IPidMotorController { */ 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); } } @@ -133,18 +151,20 @@ 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; + TPidConfig m_config; PidSettings m_settings; frc::PIDController m_pidController; 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 @@ -158,12 +178,12 @@ class PidMotorController : public IPidMotorController { * @tparam TAbsoluteEncoder */ template + typename TAbsoluteEncoder, typename TPidConfig> 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); @@ -199,13 +219,13 @@ class PidMotorControllerTuner { } private: - PidMotorController - &m_controller; + PidMotorController &m_controller; }; class RevPidMotorController : public PidMotorController< rev::spark::SparkMax, rev::spark::SparkClosedLoopController, - rev::spark::SparkRelativeEncoder, rev::spark::SparkAbsoluteEncoder> { -}; + rev::spark::SparkRelativeEncoder, rev::spark::SparkAbsoluteEncoder, + rev::spark::SparkMaxConfig> {}; } // 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