Diff: PID/PID.cpp

Revision:

2:03e5f7ab473f

Parent:

1:5e2b81f2d0b4

Child:

5:06e978fd147a

--- a/PID/PID.cpp	Sun Sep 08 20:53:33 2013 +0000
+++ b/PID/PID.cpp	Mon Sep 09 20:01:13 2013 +0000
@@ -1,7 +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;
     Integrate = true;
@@ -12,40 +12,34 @@
     dtTimer.start();
 }
 
-float PID::compute(float SetPoint, float ProcessValue)
-{
+void PID::compute(float DesiredAngle, float Angle, float Gyro_data) {
     // 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 =  ProcessValue - SetPoint;
+    float Error =  Angle - DesiredAngle;
     
     // Integral
     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;
     
     // Final Formula
-    float Result = P * Error + I * Integral + D * Derivative;
-    
-    PreviousError = Error;
-    
-    return Result;
+    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