MiUP_Reglator PID

//Regulator PID
//Funkcja w jezyku C

/******Biblioteki******/
#include <reg1210.h>

/******Definicje******/
#define KP 0.1              // wzmocnienie czlonu proporcjonalnego
#define KI 0.01             // wzmocnienie czlonu calkujacego
#define KD 0.0              // wzmocnienie czlonu rozniczkujacego
#define DELTA_T 0.02        // krok calkowania i rozniczkowania
#define MAX_OUT 100         //ograniczenie maksymalnej wartosci wyjscia
#define MIN_OUT -100        //ograniczenie minimalnej wartosci wyjscia

/******Zmienne globalne*******/
float wynik_poprz;  // wynik calkowania
float error_poprz;  // poprzednia wartosc uchybu

/*******Dzialanie czlonu calkujacego******
0. 0+C
1. f(t1) * delta_t + A(t) = A(t1)
2. f(t2) * delta_t + A(t1) = A(t2)
/*******/

/************************************************************/
/*                                                          */
/*  integral(float error)                                   */
/*  funkcja calkujaca                                       */
/*                                                          */
/************************************************************/

float calka(float error){
    float wynik;
    /*
    static float wynik_poprz;
    //zmienna statyczna majaca zarezerwowane miejsce w pamieci
    //zle rozwiazanie poniewaz przy pierwszym wykonaniu petli program nie zna jej wartosci
    */
    wynik = error * DELTA_T + wynik_poprz;
    wynik_poprz = wynik;
    return wynik;
}

/******Dzialanie czlonu rózniczkujacego******
pochodna = (f(t)-f(t-delta_t))/delta_t
/*******/

/************************************************************/
/*                                                          */
/*  derivative(float error)                                 */
/*  funkcja rozniczkowania                                  */
/*                                                          */
/************************************************************/

float pochodna(float error){
    float wynik;
    wynik = (error - error_poprz)/DELTA_T;
    error_poprz = error; // nadpisanie poprzedniego wyniku;
    return wynik;
}

float PID(float error){
    float y = KP * error + KI * calka(error) + KD * pochodna(error);
    return y;
}

void delay(){
    unsigned int i;
    for(i=0;i<30000;i++)
    {
    }
}

/******Funkcja glowna******/

void main(){
    //Blok inicjalizacji
    error_poprz = 0;
    wynik_poprz = 0;

    // Petla glowna
    while(1){
        delay();
        PID(5);
    }

}

/******Koniec funkcji glownej******/