PID class

#ifndef PID_H_
#define PID_H_

  #include "AVRBase.h"

  class PID : public AVRBase {
    public:

      PID();
      ~PID();

      void SetPID(float fP, float fI, float fD) { m_fP = fP; m_fI = fI; m_fD = fD; }

      void SetSetpoint(float fInput)            { m_fSetpoint = fInput; }
      void SetCurrent(float fInput)             { m_fCurrent = fInput; }

      void SetConstraints(float fMin, float fMax ) { m_fMinConstraint = fMin; m_fMaxConstraint = fMax; }

      float Output() { Update(); return m_fOutput; }

      void Update();

    private:

      float m_fP, m_fI, m_fD;

      float m_fDt;

      float m_fPrevError;
      float m_fError;

      float m_fIntegral;

      float m_fSetpoint;
      float m_fCurrent;

      float m_fOutput;
      float m_fMinConstraint;
      float m_fMaxConstraint;

  };

#endif

PID::PID() {
  m_fP = 1;
  m_fI = 1;
  m_fD = 1;

  m_fError = 0;
  m_fPrevError = 0;

  m_fIntegral = 0;

  m_fMinConstraint = 0;
  m_fMaxConstraint = 1;
}

PID::~PID() {

}

void PID::Update() {
  m_fOutput = 0;

  m_fError = m_fCurrent - m_fSetpoint;

  // P
  if (m_fP != 0) {
    m_fOutput = m_fP * m_fError;
  }
  // I
  if (m_fI != 0) {
    m_fIntegral += (m_fError * m_fDt * m_fI);
    m_fOutput += m_fIntegral;
  }
  // D
  if (m_fD != 0) {
    m_fOutput += -1.0 * ((m_fPrevError - m_fError) / m_fDt) * m_fD;
  }

  if (m_fOutput < m_fMinConstraint) {
    m_fOutput = m_fMinConstraint;
  }
  if (m_fOutput > m_fMaxConstraint) {
    m_fOutput = m_fMaxConstraint;
  }
}