PidMotorController.h

Go to the documentation of this file.

#pragma once
 
#include <frc/controller/PIDController.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <rev/CANSparkBase.h>
#include <rev/CANSparkMax.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>
class PidMotorController : public IPidMotorController {
public:
  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_encoder.SetPositionConversionFactor(factor);
  }
 
  inline void SetAbsoluteEncoderConversionFactor(double factor) override {
    if (m_absEncoder) {
      m_absEncoder->SetPositionConversionFactor(factor);
    }
  }
 
  void Stop() override;
 
  inline const PidSettings &GetPidSettings() override { return m_settings; }
 
  void UpdatePidSettings(PidSettings settings) override;
 
protected:
  TMotor &m_motor;
  TController &m_controller;
  TRelativeEncoder &m_encoder;
  TAbsoluteEncoder *m_absEncoder;
  PidSettings m_settings;
  frc::PIDController m_pidController;
  bool m_absolutePositionEnabled = false;
  double m_absoluteTarget = 0;
  const units::revolutions_per_minute_t m_maxRpm;
};
 
// TODO: Move to its own file and make it work with IPidMotorController
template <typename TMotor, typename TController, typename TRelativeEncoder,
          typename TAbsoluteEncoder>
class PidMotorControllerTuner {
public:
  explicit PidMotorControllerTuner(
      PidMotorController<TMotor, TController, TRelativeEncoder,
                         TAbsoluteEncoder> &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});
  }
 
private:
  PidMotorController<TMotor, TController, TRelativeEncoder, TAbsoluteEncoder>
      &m_controller;
};
 
class RevPidMotorController
    : public PidMotorController<rev::CANSparkMax, rev::SparkPIDController,
                                rev::SparkRelativeEncoder,
                                rev::SparkAbsoluteEncoder> {};
} // namespace subzero