taka yamanouchi / PID

PID.cpp

Committer:
sink
Date:
2019-07-23
Revision:
1:09b63bc8f46a
Parent:
0:88340ec1ec48
Child:
2:9dd174ee5ffc

File content as of revision 1:09b63bc8f46a:

#include "PID.h"

PID::PID(double INT_TIME){
    _time = INT_TIME;
    reset();
}

void PID::set(double P, double I, double D){
    _p = P;
    _i = I;
    _d = D;
}

double PID::con (double _error){
    static double _pre_err = 0.0;
    double _result = 0;
    
    _result += _error * _p;
    
    _integ += (_error + _pre_err) / 2.0 * _time;
    _result += _integ * _i;
    
    _result += (_error - _pre_err) / _time * _d;
    
    _pre_err = _error;
    
    return _result;
}
    
void PID::reset(){
    _p = 0.0;
    _i = 0.0;
    _d = 0.0;
    _integ = 0.0;
}