Diff: main.cpp

Revision:

2:b8e0c824a562

Parent:

1:be893bda5f00

diff -r be893bda5f00 -r b8e0c824a562 main.cpp
--- a/main.cpp	Wed Apr 25 15:18:05 2018 +0000
+++ b/main.cpp	Mon Apr 30 23:42:07 2018 +0000
@@ -25,34 +25,45 @@
 
 int main() {
     while(1) {
-        float check;
-        check = sonar;
-        //son[0] = sonar;
-        //son[1] = sonar;
-        //son[2] = sonar;
-        //son[3] = sonar;
-        //son[4] = sonar;
-        //son[5] = sonar;
-        //float check;
-        //check = son[0] + son[1] + son[2] + son[3]+son[4]+son[5];
-        //check = check/6;
-        check = check*3.3;
-        check = check/0.0098;
-        pc.printf("%f\n",check);
-        distance = 7;//inches;
+        float son[6];
+        for (int count = 0;count<6;count++){
+            son[count] = sonar;
+            wait(0.01);
+            //pc.printf("%f\n",son[count]);
+        }
+        
+        float a;
+        int i;
+        int j;
+        int n = 6;
+        for (i = 0; i < n; i++) 
+        {
+            for (j = i + 1; j < n; j++)
+            {
+                if (son[i] > son[j]) 
+                {
+                    a =  son[i];
+                    son[i] = son[j];
+                    son[j] = a;
+                }
+            }
+        }
+        pc.printf("%f\n",son[2]);
+        distance = 10;//inches;
+        distance = 0.003*distance - 0.0057;
         wait(0.05);
-        if (check > distance){
-        
-                speedL = tLeft.getSpeed();
-                speedR = tRight.getSpeed();
-                l = PIpwmL(0.3, speedL);
-                r = PIpwmR(0.3, speedR); 
+        pc.printf("%f\n",son);
+        if (son[2] > 0.038){
+            speedL = tLeft.getSpeed();
+            speedR = tRight.getSpeed();
+            l = PIpwmL(0.3, speedL);
+            r = PIpwmR(0.3, speedR); 
 
-                left.speed(l);
-                right.speed(-r);
-        
-                pc.printf("%f,%f\n",speedL,speedR);
-                wait(0.05);
+            left.speed(l);
+            right.speed(-r);
+
+            wait(0.05);
+              
         }
         else {
             left.stop();
@@ -67,7 +78,7 @@
     float sampling_per = 0.05;    
     float errorL = desired_speed - speed;
     integral_errorL += (errorL*sampling_per);   
-    float left = (0.07*integral_errorL) + (0.08*errorL) + 0.485;   
+    float left = (0.07*integral_errorL) + (0.08*errorL) + 0.48;   
     return left;
     }
 float PIpwmR(float desired_speed,float speed)