Diff: Distance.h

Revision:

1:60c79e942c98

Parent:

0:4ca3e247b86a

--- a/Distance.h	Fri Jun 08 12:29:16 2018 +0000
+++ b/Distance.h	Fri Jun 08 22:22:10 2018 +0000
@@ -5,42 +5,69 @@
 SRF05 Usound1(p26,p15);
 HCSR04 Usound2(p25,p16);
 Ticker t1;
+
+extern Motor M1;
+extern Motor M2;
+
 int n=0;
+int turns=0;
+void enable1();
+void enable2();
+void turnR()
+{
+    M1=1;M2=-1; //右转90
+    wait(0.75);   
+    }
+void turnL()
+{
+    M1=-1;M2=1; //左转90
+    wait(0.75);
+    }
 void distancedetectL()
 {
     float u= Usound1.read();
     if(7<=u<=20)  {
         n+=1;
-        if (n>=3)
-        {
-            //turnL();
+        if (n>=3) {
+            turnL();
             change(90);
-            //driveit();
+            driveit();
             n=0;
-            }
+            enable2();
         }
     }
+}
 void distancedetectR()
 {
     float u= Usound2.read_cm();
-    if(7<=u<=20)  {
+    if(30<=u)  {
         n+=1;
-        if (n>=3)
-        {
-            //turnL();
-            change(90);
-            //driveit();
-            n=0;
+        if (n>=3) {
+            turns+=1;
+            turnL();
+            if(turns>=3) {
+                t1.detach();
+                M1=0.5;M2=0.5;
+                wait(3);
+                M1=0;M2=0;
+            } else {
+                turnR();
+                change(-90);
+                driveit();
+                n=0;
+                enable1();
             }
         }
     }
+}
 void enable1()
 {
-   t1.detach();
-   t1.attach(&distancedetectR,0.4);
-    }  
+   M1=0;M2=0;
+   /* t1.detach();
+    t1.attach(&distancedetectR,0.4);*/
+}
 void enable2()
 {
-   t1.detach();
-   t1.attach(&distancedetectL,0.4);
-    }    
\ No newline at end of file
+    t1.detach();
+    t1.attach(&distancedetectL,0.4);
+}
\ No newline at end of file