taka yamanouchi / PID

pid

PID.cpp@2:9dd174ee5ffc, 2019-11-19 (annotated)

Committer:
sink
Date:
Tue Nov 19 09:42:59 2019 +0000
Revision:
2:9dd174ee5ffc
Parent:
1:09b63bc8f46a 
?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sink 0:88340ec1ec48 1 #include "PID.h"
sink 0:88340ec1ec48 2
sink 2:9dd174ee5ffc 3 PID::PID(double _INT_TIME){
sink 2:9dd174ee5ffc 4 _time = _INT_TIME;
sink 2:9dd174ee5ffc 5 _pre_err = 0;
sink 0:88340ec1ec48 6 reset();
sink 0:88340ec1ec48 7 }
sink 0:88340ec1ec48 8
sink 2:9dd174ee5ffc 9 void PID::set(double _P, double _I, double _D){
sink 2:9dd174ee5ffc 10 _p = _P;
sink 2:9dd174ee5ffc 11 _i = _I;
sink 2:9dd174ee5ffc 12 _d = _D;
sink 0:88340ec1ec48 13 }
sink 0:88340ec1ec48 14
sink 1:09b63bc8f46a 15 double PID::con (double _error){
sink 1:09b63bc8f46a 16 double _result = 0;
sink 0:88340ec1ec48 17
sink 1:09b63bc8f46a 18 _result += _error * _p;
sink 0:88340ec1ec48 19
sink 1:09b63bc8f46a 20 _integ += (_error + _pre_err) / 2.0 * _time;
sink 1:09b63bc8f46a 21 _result += _integ * _i;
sink 0:88340ec1ec48 22
sink 1:09b63bc8f46a 23 _result += (_error - _pre_err) / _time * _d;
sink 0:88340ec1ec48 24
sink 1:09b63bc8f46a 25 _pre_err = _error;
sink 0:88340ec1ec48 26
sink 1:09b63bc8f46a 27 return _result;
sink 0:88340ec1ec48 28 }
sink 0:88340ec1ec48 29
sink 0:88340ec1ec48 30 void PID::reset(){
sink 1:09b63bc8f46a 31 _p = 0.0;
sink 1:09b63bc8f46a 32 _i = 0.0;
sink 1:09b63bc8f46a 33 _d = 0.0;
sink 1:09b63bc8f46a 34 _integ = 0.0;
sink 2:9dd174ee5ffc 35 }
sink 2:9dd174ee5ffc 36
sink 2:9dd174ee5ffc 37 void PID::Ireset(){
sink 2:9dd174ee5ffc 38 _integ = 0.0;
sink 0:88340ec1ec48 39 }