PidMotorController.h

Go to the documentation of this file.

#pragma once
 
#include <frc/controller/PIDController.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <rev/ClosedLoopSlot.h>
#include <rev/SparkBase.h>
#include <rev/SparkClosedLoopController.h>
#include <rev/SparkFlex.h>
#include <rev/SparkMax.h>
#include <rev/config/SparkFlexConfig.h>
#include <rev/config/SparkMaxConfig.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
 
#include <string>
 
#include "subzero/logging/ConsoleLogger.h"
#include "subzero/motor/IPidMotorController.h"
 
namespace subzero {
template <typename TMotor, typename TController, typename TRelativeEncoder,
          typename TAbsoluteEncoder, typename TPidConfig>
class PidMotorController : public IPidMotorController {
public:
 
  // TODO: make `absEncoder` an `std::optional<>` instead of a raw pointer
  explicit PidMotorController(std::string name, TMotor &motor,
                              TRelativeEncoder &encoder,
                              TController &controller, PidSettings pidSettings,
                              TAbsoluteEncoder *absEncoder,
                              units::revolutions_per_minute_t maxRpm);

  void Set(double percentage) override;

  void Set(units::volt_t volts) override;

  void SetPidTolerance(double tolerance) override;

  void Update() override;

  void RunWithVelocity(units::revolutions_per_minute_t rpm) override;

  void RunWithVelocity(double percentage) override;

  void RunToPosition(double position) override;
 
  inline virtual void ResetEncoder() override {
    m_encoder.SetPosition(0);
    ConsoleWriter.logInfo(m_name + " PID Controller", "Reset encoder%s", "");
  }
 
  inline double GetEncoderPosition() override {
    return m_encoder.GetPosition();
  }
 
  std::optional<double> GetAbsoluteEncoderPosition() override;

  inline void SetEncoderConversionFactor(double factor) override {
    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);
  }

  inline void SetAbsoluteEncoderConversionFactor(double factor) override {
    if (m_absEncoder) {
      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);
    }
  }

  void Stop() override;
 
  inline const PidSettings &GetPidSettings() override { return m_settings; }
 
  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
template <typename TMotor, typename TController, typename TRelativeEncoder,
          typename TAbsoluteEncoder, typename TPidConfig>
class PidMotorControllerTuner {
public:
  explicit PidMotorControllerTuner(
      PidMotorController<TMotor, TController, TRelativeEncoder,
                         TAbsoluteEncoder, TPidConfig> &controller)
      : m_controller{controller} {
 
    frc::SmartDashboard::PutNumber(m_controller.m_name + " P Gain",
                                   m_controller.GetPidSettings().p);
    frc::SmartDashboard::PutNumber(m_controller.m_name + " I Gain",
                                   m_controller.GetPidSettings().i);
    frc::SmartDashboard::PutNumber(m_controller.m_name + " D Gain",
                                   m_controller.GetPidSettings().d);
    frc::SmartDashboard::PutNumber(m_controller.m_name + " IZone",
                                   m_controller.GetPidSettings().iZone);
    frc::SmartDashboard::PutNumber(m_controller.m_name + " Feed Forward",
                                   m_controller.GetPidSettings().ff);
  }

 
  void UpdateFromShuffleboard() {
    double tP = frc::SmartDashboard::GetNumber(m_controller.m_name + " P Gain",
                                               m_controller.GetPidSettings().p);
    double tI = frc::SmartDashboard::GetNumber(m_controller.m_name + " I Gain",
                                               m_controller.GetPidSettings().i);
    double tD = frc::SmartDashboard::GetNumber(m_controller.m_name + " D Gain",
                                               m_controller.GetPidSettings().d);
    double tIZone = frc::SmartDashboard::GetNumber(
        m_controller.m_name + " IZone", m_controller.GetPidSettings().iZone);
    double tFeedForward =
        frc::SmartDashboard::GetNumber(m_controller.m_name + " Feed Forward",
                                       m_controller.GetPidSettings().ff);
 
    m_controller.UpdatePidSettings(
        {.p = tP,
         .i = tI,
         .d = tD,
         .iZone = tIZone,
         .ff = tFeedForward,
         .isIdleModeBrake = m_controller.GetPidSettings().isIdleModeBrake});
  }
 
private:
  PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder,
                     TPidConfig> &m_controller;
};
 
class RevPidMotorController
    : public PidMotorController<
          rev::spark::SparkMax, rev::spark::SparkClosedLoopController,
          rev::spark::SparkRelativeEncoder, rev::spark::SparkAbsoluteEncoder,
          rev::spark::SparkMaxConfig> {};
} // namespace subzero