Diff: main.cpp

Revision:

7:01bb0b0a98f1

Parent:

5:09afcbe0c18f

--- a/main.cpp	Fri Mar 13 01:14:38 2015 +0000
+++ b/main.cpp	Sat Mar 14 00:47:16 2015 +0000
@@ -94,7 +94,7 @@
     }
     
     
-    if(!WaitFlag){
+    if((!WaitFlag)&&(!(last_deg == Degree ))){
         SetDegree = Degree * 11/9;
         if(SetDegree > 110){
             SetDegree = 110;
@@ -369,7 +369,7 @@
     return;
 
 }
-    
+/*
 void GoHome()//Ball is not found.
 {
     int pingRl =0;
@@ -388,20 +388,62 @@
         return;
     }
     if(PingData[1] < 52){
-        PowLev = (52 - PingData[1])*1.3;
-        move(PowLev,PowLev,0,90);
+        PowLev = (52 - PingData[1])*1.35;
+        move(PowLev,PowLev,0,45);
         return ;
     }else if (PingData[3] < 52){
         PowLev = (52 - PingData[3])*1.35;
-        move(PowLev,PowLev,0,-90);
+        move(PowLev,PowLev,0,-45);
         return ;
     }
 
     move(0,0,0,0);//stop
 
+}*/
+void GoHome()//Ball is not found.
+{
+    int pingRl =0;
+    int PowLev;
+    if(PingData[2] >=70){//後ろからの距離60cm
+        move(-25,-25,0,0);
+        return ;
+    }
+    
+    
+    pingRl = PingData[1]+PingData[3];
+    if(pingRl <120){
+        if(PingData[2]<30){
+            if(PingData[1]<30){
+                move(15,15,0,30);
+            }else if(PingData[3]<30){
+                move(15,15,0,-30);
+            }
+        }
+        move(0,0,0,0);//stop
+        return;
+    }
+    if(PingData[1] < 65){
+        PowLev = (52 - PingData[1])*1.35;
+        move(-PowLev,-PowLev,0,-20);
+        return ;
+    }else if (PingData[3] < 65){
+        PowLev = (52 - PingData[3])*1.35;
+        move(-PowLev,-PowLev,0,20);
+        return ;
+    }
+    if(PingData[2]<40){
+        move(0,0,0,0);//stop
+    }else{
+        move(-15,-15,0,0);//stop
+    
+    }
+    
+    
+    
 }
 
 
+
 void PidUpdate()
 {   
 
@@ -644,11 +686,12 @@
                 move(0,0,0,0);
                 StopCt = 0;
                 while(LinePing){
-                    if(StopCt>30){
+                    if(StopCt>10){
                         GoHome();
                         wait_ms(200);
                     }
                     StopCt++;
+                    
                     Led[1] = Led[2] = Led[3] = 1;  
                     //Receive();
                     LineData = (~Line+0x00) & 0x0F;