Diff: main.cpp

Revision:

1:66d0178e8f40

Parent:

0:9acdbbfa4eb2

Child:

2:35bc9b7f5756

--- a/main.cpp	Wed Apr 17 14:45:09 2019 +0000
+++ b/main.cpp	Tue Apr 23 15:10:54 2019 +0000
@@ -1,4 +1,4 @@
-//Satre Gagnon, Ewing, Batten 
+//Satre, Gagnon, Ewing, Batten 
 //Tach Initial 
 
 #include "mbed.h"
@@ -10,14 +10,17 @@
 
 //speed
 float wL, wR;
+float x = 0.2, y = 0.2, dist, realdist;
  
 ContinuousServo left(p23);
 ContinuousServo right(p26);
+AnalogIn sonar(p19); //range sensor 9.8 mV/in
  
 //encoders
 Tach tleft(p17,64);
 Tach tright(p13,64);
- 
+
+        
 int main()
 { 
     
@@ -25,14 +28,36 @@
     
     while(1) {
         
-        left.speed(0.5);
-        right.speed(-0.5); //right negative to go forward
+        left.speed(x);
+        right.speed(-y); //right negative to go forward
+        x = x;
+        y = y;
         
         wL = tleft.getSpeed(); //speed of left wheel (rev/sec)
         wR = tright.getSpeed(); //speed of right wheel (rev/sec)
+        dist = sonar.read();
+        realdist = dist/0.00277;
+            
+        if (wL<wR) {
+            x = x+0.03;
+            y = y; }
+        else if (wR<wL) {
+            x = x;
+            y = y+0.03;}
+        else if (wR == wL) {
+            x = x;
+            y = y;}
+        else if ((x || y) > 0.2) {
+            x = 0.2;
+            y = y;}
+            
+        if (realdist <= 8) {
+            left.stop();
+            right.stop();
+            }   
+        pc.printf("%f, %f\n %f\n %f %f\n", wL, wR, realdist, x, y);
         
-        pc.printf("%f, %f\n", wL, wR);
-        wait(1);
+        wait(.2);
      
 }  //while
 }  //main
\ No newline at end of file