Diff: main.cpp

Revision:

1:be893bda5f00

Parent:

0:a0013f51f232

Child:

2:b8e0c824a562

--- a/main.cpp	Mon Apr 23 15:15:49 2018 +0000
+++ b/main.cpp	Wed Apr 25 15:18:05 2018 +0000
@@ -10,16 +10,72 @@
 AnalogIn sonar(p19);
 float distance;
 
+float l;
+float r;
+float speedL;
+float speedR;
+float errorL;
+float errorR;
+float sampling_per;
+
+Serial pc(USBTX,USBRX);
+
+float PIpwmL(float desired_speed,float speed); 
+float PIpwmR(float desired_speed,float speed); 
+
 int main() {
     while(1) {
-        distance = //inches;
-        distance = //conversion to analog value;
-        if (sonar > distance){
-            left.speed();
-            right.speed();
-            }
+        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;
+        wait(0.05);
+        if (check > distance){
+        
+                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);
+        }
         else {
+            left.stop();
+            right.stop();
             break;
             }
     }
 }
+float PIpwmL(float desired_speed,float speed)
+{
+    float integral_errorL = 0.0;  
+    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;   
+    return left;
+    }
+float PIpwmR(float desired_speed,float speed)
+{
+        float integral_errorR = 0.0;
+        float sampling_per = 0.05;
+        float errorR = desired_speed - speed;
+        integral_errorR += (errorR*sampling_per);
+        float right = (0.07*integral_errorR) + (0.08*errorR) + 0.25;
+        return right;
+    }