Diff: PID/PID.cpp

Revision:

7:ac2895479e34

Parent:

5:06e978fd147a

--- a/PID/PID.cpp	Sun Jul 06 09:25:09 2014 +0000
+++ b/PID/PID.cpp	Sat Jul 12 12:21:11 2014 +0000
@@ -1,6 +1,7 @@
 #include "PID.h"
 
-PID::PID(float P, float I, float D, float Integral_Max) {
+PID::PID(float P, float I, float D, float Integral_Max)
+{
     Value = 0;
     Integral = 0;
     LastTime = 0;
@@ -12,34 +13,37 @@
     dtTimer.start();
 }
 
-void PID::compute(float DesiredAngle, float Angle, float Gyro_data) {
+float PID::compute(float SetPoint, float ProcessValue)
+{
     // meassure dt
     float dt = dtTimer.read() - LastTime;    // time in us since last loop
     LastTime = dtTimer.read();                   // set new time for next measurement
     
-    // Derivative (most important for QC stability and Gyro only because very sensitive!)
-    float Derivative = Gyro_data;
-    //PreviousGyro_data = Gyro_data;
-    
     // Proportional
-    float Error =  Angle - DesiredAngle;
+    float Error =  ProcessValue - SetPoint;
     
     // Integral
-    if (dt > 2 || !Integrate) // TODO: 2 secs is the maximal time between two computations
+    if (dt > 2 || !Integrate) // Todo: 2 secs is the maximal time between two computations
         Integral = 0;
     else if (abs(Integral + Error) <= Integral_Max)
         Integral += Error * dt;
+        
+    // Derivative
+    float Derivative = (Error - PreviousError) / dt;
+    PreviousError = Error;
     
     // Final Formula
     Value = P * Error + I * Integral + D * Derivative;
 }
 
-void PID::setPID(float P, float I, float D) {
+void PID::setPID(float P, float I, float D)
+{
     PID::P = P;
     PID::I = I;
     PID::D = D;
 }
 
-void PID::setIntegrate(bool Integrate) {
+void PID::setIntegrate(bool Integrate)
+{
     PID::Integrate = Integrate;
 }
\ No newline at end of file