#include "pid.h"

Pid::Pid(void)
    {
        this->iState=0;
        this->dState=0;
        this->iGain=0;
        this->pGain=0;
        this->dGain=0;
    }

Pid::Pid(float& iGain,unsigned short& pGain,unsigned short& dGain)
    {
        this->iGain=iGain;
        this->pGain=pGain;
        this->dGain=dGain;
    }

Pid::~Pid(void)
    {
        
    }
    
float Pid::UpdatePID(unsigned short& error, unsigned short& position)
    {
    
    unsigned short     pTerm;
    unsigned short    dTerm;
    float    iTerm;
    pTerm=this->pGain*error;    //Вычисление пропорциональной состовляющей
    this->iState+=error;    //Вычисление интегральной состовляющей с учетом приделов
    
    if(this->iState > this->iMax)
    {
        this->iState=this->iMax;
    }
    else if(this->iState < this->iMin)
    {
        this->iState=this->iMin;
    }

    iTerm=this->iGain*this->iState;    //Расчет интегральной состовляющей
    dTerm=this->dGain*(position - this->dState);//расчет. диф. состовляющей
    this->dState=position;
    return (pTerm+iTerm+dTerm);
    }

#ifndef PID_DEF_H
#define PID_DEF_H

#include "stdafx.h"
#include <float.h>
class Pid
{
  private:
    unsigned short     dState;        //Последняя входная позиция
    unsigned short    iState;        //Сосотояние интегратора
    unsigned short     iMax,iMin;    //Max & Min состояние интегратора
    
    float        iGain;        //интегральная компонента
    unsigned short    pGain;        //пропорциональная компонента
    unsigned short    dGain;        //производная компонента
  
  public:
    Pid(void);
    Pid(float& iGain,unsigned short& pGain,unsigned short& dGain);
    ~Pid(void);
    float UpdatePID(unsigned short& error, unsigned short& position);
};

#endif //PID_DEF_H