Diff: kontrolPID.cpp

Revision:

2:b37bdc0b8677

Parent:

0:a400d726fdc6

Child:

3:8607359f4813

diff -r 0305c2cd17d2 -r b37bdc0b8677 kontrolPID.cpp
--- a/kontrolPID.cpp	Mon Oct 30 13:17:45 2017 +0000
+++ b/kontrolPID.cpp	Mon Nov 06 14:52:38 2017 +0000
@@ -1,19 +1,21 @@
 #include "mbed.h"
 #include "kontrolPID.h"
- float _kP, _kI, _kD;
- int _interval;
- short _limit_Min, _limit_Max;
- float _current_Error, _sum_Error, _delta_Error, _previous_Error;
- float _controller_Output;
-kontrolPID::kontrolPID(float kP, float kI, float kD,
-                        int interval, 
-                        short limit_Min, short limit_Max){
+float _kP, _kI, _kD;
+float _last_output;
+int _interval;
+short _limit_Min, _limit_Max;
+float _current_Error, _sum_Error, _delta_Error, _previous_Error_1, _previous_Error_2;
+float _controller_Output;
+kontrolPID::kontrolPID( int interval, 
+                        short limit_Min, short limit_Max,
+                        float kP, float kI, float kD){
     _kP = kP;
     _kI = kI;
     _kD = kD;
     _interval = interval;
     _limit_Min = limit_Min;
     _limit_Max = limit_Max;
+    _last_output = 0;
 }
  
 void kontrolPID::resetPID(){
@@ -23,11 +25,13 @@
 }
  
 float kontrolPID::hitungPID(float _process_Value, float _set_Point){
+    float a = _kP + _kI*_interval/2 + _kD/_interval;
+    float b = -1*_kP + _kI*_interval/2 - 2*_kD/_interval;
+    float c = _kD/_interval;
+    
     _current_Error = _set_Point - _process_Value;
-    _delta_Error = _current_Error - _previous_Error;
-    _sum_Error = _current_Error + _previous_Error;
     
-    _controller_Output = _kP * _current_Error + _kI * _sum_Error * _interval + _kD * _delta_Error / _interval;
+    _controller_Output = _last_output + a * _current_Error + b * _previous_Error_1 + c * _previous_Error_2;
     
     if (_limit_Max < _controller_Output){
         _controller_Output = _limit_Max;
@@ -36,7 +40,9 @@
         _controller_Output = _limit_Min;
     }
     
-    _previous_Error = _current_Error;
+    _last_output = _controller_Output;
+    _previous_Error_2 = _previous_Error_1;
+    _previous_Error_1 = _current_Error;
     
     return _controller_Output;
 }