Diff: PID.cpp

Revision:

9:76e4808df4cb

Parent:

7:edd065946e9b

--- a/PID.cpp	Wed Nov 22 06:58:41 2017 +0000
+++ b/PID.cpp	Fri Dec 01 21:20:29 2017 +0000
@@ -15,8 +15,8 @@
 {
     double error = 0;
     error= signed( counterM1- counterM2);
-    pc.printf("counter1: %6.6f",counterM1);
-    pc.printf("counter2: %6.6f",counterM2);
+   /* pc.printf("counter1: %6.6f",counterM1);
+    pc.printf("counter2: %6.6f",counterM2);*/
     double P = Kp * error;
     total_error = (total_error+error);
     double I = Ki * total_error;
@@ -28,9 +28,9 @@
 }
 
 //error for led
-const double Kp_e =.00001;
+const double Kp_e =.0000001;
 const double Ki_e = 0;
-const double Kd_e = 0;
+const double Kd_e = 0.0000; // 0.0001
 const double decay_e=1;
 
 double total_error_e=0;
@@ -51,10 +51,12 @@
     double I = Ki_e * total_error;
     total_error /= decay_e;
     double D = Kd_e * (error - prev_error);
+     // pc.printf("kd error: %8.8f\n", Kd_e * (error - prev_error));
     prev_error = error;
+   //pc.printf("total error %8.8f\n",  P + I +D);
     return (P + I+ D);
 }
-
+/*
 //opening on right side
 bool opening_r(double threshold_p, double threshold_d)
 {
@@ -69,8 +71,8 @@
 //wall in fron the car
 bool wall_front(double threshold_r, double threshold_l)
 {
-    return threshold_r < R_t.read_u(16) && threshold_l < L_t.read_u(16);
-}
+    return threshold_r < R_t.read_u16() && threshold_l < L_t.read_u16();
+}*/