Revision 4:79adbd22ee27, committed 2016-12-10

Comitter:

brian12858

Date:

Sat Dec 10 11:21:42 2016 +0000

Parent:

3:6484736ff515

Commit message:

almost finish, front ultrasonic sensor not in use

Changed in this revision

diff -r 6484736ff515 -r 79adbd22ee27 SRF05.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SRF05.lib	Sat Dec 10 11:21:42 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/simon/code/SRF05/#e758665e072c

diff -r 6484736ff515 -r 79adbd22ee27 main.cpp
--- a/main.cpp	Fri Dec 09 11:55:50 2016 +0000
+++ b/main.cpp	Sat Dec 10 11:21:42 2016 +0000
@@ -2,6 +2,7 @@
 #include <string>
 #include <cstring>
 //#include "rtos.h"
+#include "SRF05.h"
 #include "motordriver.h"
 Serial bt(p28,p27);//tx,rx
 Serial TAG(p9,p10);//tx,rx
@@ -25,11 +26,73 @@
 
 Motor L(p21, p23, p22, 1); // pwm, fwd, rev, can brake ,right
 Motor R(p26, p24, p25, 1); // pwm, fwd, rev, can brake ,left
+const double speed=0.55;
+const double speed_R = 0.85 * speed;
+const double speed_L = 1 * speed;
+SRF05 srfF(p11, p12);//Trigger,echo
+SRF05 srfL(p13, p14);
+SRF05 srfR(p7, p8);
 
+int auto_move() {
+        //int     d=40; 
+        double sp_R=0;
+        double sp_L=0;
+        
+       
+        double  r=srfR.read();
+        double  l=srfL.read();
+        double  w=(r+l)/2;
+        
+        pc.printf("r = %f l=%f\n\r",r,l);
+         /*while(1){
+          
+         printf("Distancef = %.1f\n",srfF.read());
+         printf("DistanceR = %.1f\n",srfR.read());
+         printf("DistanceL = %.1f\n",srfL.read());              
+            }*/     
 
+//==============go straight beside the wall========================//
+
+        sp_R=speed_R*l/w;
+        sp_L=speed_L*r/w;
+        R.speed(sp_R),L.speed(sp_L);
+//=======================================================//        
+        
+//==================avoid obstacle=======================//
+/**********************************************************     
+        if(srfF.read()<=80)
+        {
+            R.speed(-0.6),L.speed(-0.7);//<80 lower speed            
+                
+            if(srfF.read()<=20)
+            {   R.stop(-0.5),L.stop(-0.5);
+                
+                if(srfR.read()>=80)
+                {
+                    wait(0.2);
+                    R.speed(-0.6),L.speed(-0.9);//turn right
+                    if(srfF.read()>=80)
+                    {
+                    R.speed(-0.9),L.speed(-1);//forward
+                    }
+                }
+                else
+                {
+                    wait(0.2);
+                    R.speed(-0.7),L.speed(-0.5);//turn left
+                    if(srfF.read()>=80)
+                    {
+                    R.speed(-0.9),L.speed(-1);//forward
+                    }                    
+                }
+            }
+         }  
+ //**********************************************************/
+//============================================================//                
+}
 void move(char in){
     char ch[1]={'s'};
-    ch[0] = in;
+    ch[0] = in;//in;
    // while(1) {
         //pc.printf("ch[0]= %s\n\r",ch);
      /*   if(bt.readable ()){
@@ -39,11 +102,11 @@
         switch(ch[0]){
         case 'w':
             //前進
-            R.speed(0.7),L.speed(1);
+            R.speed(speed_R),L.speed(speed_L);
         break;   
         case 'a':
             //左轉
-            R.speed(0.7),L.speed(0);
+            R.speed(speed_R),L.speed(0);
         break;
         case 's':
             //停住
@@ -51,24 +114,23 @@
         break;
         case 'd':
             //右轉
-            R.speed(0),L.speed(0.711qqqa);
+            R.speed(0),L.speed(speed_L);
         break;
         case 'x':
             //backward
-            R.speed(-0.7),L.speed(-1);
+            R.speed(-speed_R),L.speed(-speed_L);
         break;
         case 'm':
             //自走車
+            pc.printf("auto move");
+            auto_move();
         break;
        }  
-   // }
-
-
-    
-    
+   // }   
 }
 //std::string.
-int main() {
+int main() {    
+    
     bt.baud(38400);
     pc.baud(9600);
     TAG.baud(115200);
@@ -81,7 +143,7 @@
     //thread.set_priority(osPriorityLow);
     
     while(1) {
-   
+
     
         // Read a line from the large rx buffer from rx interrupt routine
         read_line();
@@ -94,17 +156,17 @@
         case '0':
             flag1=true;
             tagStr1=&rx_line[2];
-            pc.printf("string1 = %s\n\r",tagStr1);
+            //pc.printf("string1 = %s\n\r",tagStr1);
             break;
         case '1':
             flag2=true;
             tagStr2=&rx_line[2];
-            pc.printf("string2 = %s\n\r",tagStr2);
+            //pc.printf("string2 = %s\n\r",tagStr2);
             break;
         case '2':
             flag3=true;
             tagStr3=&rx_line[2];
-            pc.printf("string3 = %s\n\r",tagStr3);
+            //pc.printf("string3 = %s\n\r",tagStr3);
             break;
         }
         if(flag3==true&&flag2==true&&flag1==true){
@@ -113,7 +175,6 @@
             bt.printf("%s\n",str);
         }
         
-        
         if(bt.readable())
             ch=bt.getc();
         move(ch);