Diff: PID.cpp

Revision:

7:edd065946e9b

Parent:

6:688449345fff

Child:

8:45797dcd8b66

Child:

9:76e4808df4cb

--- a/PID.cpp	Wed Nov 22 05:56:05 2017 +0000
+++ b/PID.cpp	Wed Nov 22 06:58:41 2017 +0000
@@ -10,13 +10,13 @@
 double total_error=0;
 double prev_error=0;
 
-//error for sharp turns or forward movement
+//error based on encoders
 double PID()
 {
     double error = 0;
     error= signed( counterM1- counterM2);
     pc.printf("counter1: %6.6f",counterM1);
-    pc.printf("counter2: %6.6f",counterM2);   
+    pc.printf("counter2: %6.6f",counterM2);
     double P = Kp * error;
     total_error = (total_error+error);
     double I = Ki * total_error;
@@ -27,7 +27,7 @@
     return P + I+ D;
 }
 
-//encoder controlled
+//error for led
 const double Kp_e =.00001;
 const double Ki_e = 0;
 const double Kd_e = 0;
@@ -37,12 +37,13 @@
 double prev_error_e=0;
 
 
-double PID_e(){
+double PID_e()
+{
     double error = 0;
     error= signed ( LL_t.read_u16()-RR_t.read_u16());
-    
-   // pc.printf("ledL: %d\n",LL_t.read_u16());
- //   pc.printf("LedR: %d\n",RR_t.read_u16()); 
+
+    // pc.printf("ledL: %d\n",LL_t.read_u16());
+//   pc.printf("LedR: %d\n",RR_t.read_u16());
     // pc.printf("kp error: %8.8f\n",(signed ( LL_t.read_u16()-RR_t.read_u16()))*Kp_e);
 
     double P = Kp_e * error;
@@ -51,6 +52,25 @@
     total_error /= decay_e;
     double D = Kd_e * (error - prev_error);
     prev_error = error;
-    return P + I+ D;
-    }
-    
\ No newline at end of file
+    return (P + I+ D);
+}
+
+//opening on right side
+bool opening_r(double threshold_p, double threshold_d)
+{
+    return threshold_p < LL_t.read_u16 && prev_error > threshold;
+}
+
+bool opening_l(double threshold_p, double threshold_d)
+{
+    return threshold_p < LL_t.read_u16) && prev_error > threshold;
+}
+
+//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);
+}
+
+
+