pid.h

Go to the documentation of this file.

/**
 * @file pid.h
 * @brief A control loop that utilizes the proportional, integral, and derivative values of error to
 * get an output value
 *
 * This is a conventional PID controller using the Feedback class as an interface.
 * It calculates an output using the proportional, integral, and derivative values of the error.
 * It can decide whether or not it has reached it's target state based on how long the error has
 * remained inside the deadband
 */
 
#pragma once
 
#include "core/math/controls/feedback.h"
#include "core/math/units.h"
 
using namespace units::literals;
 
/*
 * @param T the data type for states
 * @param U the data type for output
 *
 * Feedback child class to create PID control loops
 */
template <typename T, typename U>
class PID : public Feedback<T, U> {
 public:
  using kPType = units::Divided<U, T>;
  using kIType = units::Multiplied<kPType, units::Time>;
  using kDType = units::Divided<kPType, units::Time>;
 
  /**
   * @brief PID controller object constructor
   *
   * @param kP the proportional constant for the controller
   * @param kI the integral constant for the controller
   * @param kD the derivative constant for the controller
   * @param deadband the deadband where the loop decides it
   * is on target
   * @param on_target_time the time where the loops decides
   * it is on target
   */
  PID(kPType kP, kIType kI, kDType kD, T deadband, units::Time on_target_time)
      : kP_(kP), kI_(kI), kD_(kD), deadband_(deadband), on_target_time_(on_target_time) {
    reset();
  }
 
  /// Resets the internal state of the PID controller
  void reset() override {
    this->accum_error_ = T();
    this->last_error_ = T();
    this->output_ = U();
  }
 
  /**
   * @brief Updates the control loop output
   *
   * @param state the current state the PID controller is at
   * @param reference the target state for the PID controller
   * @param dt the timestep since the last time this method was called
   * @return the controller output
   */
  U update(const double& state, const double& reference, const units::Time& dt = 10_ms) override {
    this->state_ = state;
    this->reference_ = reference;
 
    const T current_error = this->reference_ - this->state_;
    const T delta_error = current_error - this->last_error_;
 
    accum_error_ += current_error;
 
    const U p_out = kP_ * current_error;
 
    const U i_out = kI_ * accum_error_;
 
    const U d_out = kP_ * delta_error;
 
    this->last_error_ = current_error;
 
    if (std::abs(current_error) < deadband_) {
      last_target_time_ += dt;
    } else {
      last_target_time_ = 0_ms;
    }
 
    if (last_target_time_ > on_target_time_) {
      on_target_ = true;
    } else {
      on_target_ = false;
    }
 
    this->output_ = p_out + i_out + d_out;
 
    this->output_ = clamp(this->output_, this->output_min_, this->output_max_);
 
    return this->output_;
  }
 
  /// Gets the on target state of the PID controller
  bool get_on_target() { return on_target_; }
 
  kPType kP_;
  kIType kI_;
  kDType kD_;
  T deadband_;
  units::Time on_target_time_;
 
 private:
  bool on_target_;
  units::Time last_target_time_;
  T accum_error_;
};
 
using AngularPositionPID = PID<units::Angle, units::Voltage>;
using AngularVelocityPID = PID<units::AngularVelocity, units::Voltage>;
using LinearPositionPID = PID<units::Length, units::Voltage>;
using LinearVelocityPID = PID<units::Velocity, units::Voltage>;