Diff: PID.cpp

Revision:

0:9c49bdc7e402

Child:

6:688449345fff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Wed Nov 22 02:18:07 2017 +0000
@@ -0,0 +1,49 @@
+
+#include "PID.h"
+
+//led controlled
+const double Kp =.00001;
+const double Ki = 0;
+const double Kd = 0.1;
+const double decay=1;
+
+double total_error=0;
+double prev_error=0;
+
+//error for sharp turns or forward movement
+double PID()
+{
+    double error = 0;
+    error= signed( counterM1- counterM2);
+    double P = Kp * error;
+    total_error = (total_error+error);
+    double I = Ki * total_error;
+    total_error /= decay;
+
+    double D = Kd * (error - prev_error);
+    prev_error = error;
+    return P + I+ D;
+}
+
+//encoder controlled
+const double Kp_e =1;
+const double Ki_e = 0;
+const double Kd_e = 0.1;
+const double decay_e=1;
+
+double total_error_e=0;
+double prev_error_e=0;
+
+
+double PID_e(){
+    double error = 0;
+    error= signed ( LL_t.read_u16()-RR_t.read_u16());
+    double P = Kp_e * error;
+    total_error = (total_error+error);
+    double I = Ki_e * total_error;
+    total_error /= decay_e;
+    double D = Kd_e * (error - prev_error);
+    prev_error = error;
+    return P + I+ D;
+    }
+    
\ No newline at end of file