Diff: PID/PID.cpp

Revision:

9:2779500685cb

Child:

10:12f9371f3e0f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID.cpp	Sun May 15 16:15:44 2016 +0000
@@ -0,0 +1,37 @@
+#include "PID.h"
+
+PIDControll::PIDControll() { 
+    //variabelen instellen als we ons PIDControll object aanmaken
+    Kp = 10;
+    Ki = 0.0001; 
+    Kd = 5;
+    set_point = 0;         // camera hoek
+    proportional = 0;
+    last_proportional =0;
+    integral = 0;
+    derivative = 0;
+    errorPID = 0;  
+    dir = 0;  
+}
+
+void PIDControll::PIDaanpassing(float angle){
+    proportional = set_point - angle;        
+    integral += proportional; 
+    derivative = proportional - last_proportional;
+    last_proportional = proportional;
+    
+    errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative); 
+    (errorPID > 0)?(dir = 1):(dir = 0);
+    
+    // errorPID is restricted between -400 and 400 
+    if(errorPID > 400)
+        errorPID = 400;
+    else if(errorPID < -400)
+        errorPID = -400;   
+   
+   stop = 0;   
+   delay = 0.1/abs(errorPID);      // speed should be proportional to error, therefore time delay
+                                     //between steps should be inverse proportional to error.
+   if (abs(errorPID)< Kp/2) stop = 1;  // 0.5 deg noise margin
+         
+}
\ No newline at end of file