Diff: main.cpp

Revision:

5:c8e4d5cdc353

Parent:

4:7c488c059498

Child:

6:2c9cb40ada59

--- a/main.cpp	Fri Mar 13 02:10:37 2015 +0000
+++ b/main.cpp	Fri Mar 13 03:17:44 2015 +0000
@@ -81,7 +81,7 @@
     Degree = Degree*(-1.0);
     
     CompassDif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE;
-    if(abs(CompassDif)<CompassVias){
+    if(/*abs(CompassDif)<CompassVias*/1){
         
         
         CompassVias=15.0;
@@ -415,20 +415,6 @@
     6
     */
     
-    if(
-        (
-            (
-                (
-                    (Compass / 10) + 540 - CompassDef
-                ) % 360
-            ) - REFERENCE
-        ) >10.0
-    ){
-        
-        move(0,0,0,0);     
-        return;
-    }
-    
     
     for(int i=0; i<20; i++)  State[i]=0;
     
@@ -442,13 +428,15 @@
     
     if(PingData[2] <=40+10) State[6]=1;
     
-    if(PingData[2] >=10) State[7]=1;
+    if(PingData[2] >=30) State[7]=1;
     
-    if(PingData[2] <=5) State[8]=1;
+    if(PingData[2] <=10) State[8]=1;
     
-    if(PingData[2] >=25-5) State[9]=1;
+    if((PingData[2] >=15)&&(PingData[2] <=25)) State[9]=1;
     
-    if(PingData[2] <=5+5) State[10]=1;
+    if(PingData[2] <15) State[10]=1;
+    
+    if(PingData[2] >25) State[11]=1;
     
     
     
@@ -472,50 +460,29 @@
     }*/
     
     if(State[4]){
-            
-        if(State[3]){
-            
-            if((State[7])==(State[8])){
-                
-                move(0,0,CompassPID,LastDegree);
-                return ;
-            }
-            if(State[7]){
-                
-                move(vrl[6],vrl[6],CompassPID,IrServo[6]);
-                return ;   
-            }
-            if(State[8]){
-                
-                move(vrl[0],vrl[0],CompassPID,IrServo[0]);
-                return ;   
-            }
+        
+        if(State[9]){
+            moving(12);
+            return ;
+        }
+        if(State[10]){
+            moving(0);
+            return ;
+        }
+        if(State[11]){
+            moving(6);
+            return ;
         }
         
-        
-        if((State[0])==(State[1])){
-            
-            move(0,0,CompassPID,LastDegree);
-            return ;
-        }
-        if(State[0]){
-            
-            move(vrl[6],vrl[6],CompassPID,IrServo[6]);
-            return ;   
-        }
-        if(State[1]){
-            
-            move(vrl[0],vrl[0],CompassPID,IrServo[0]);
-            return ;   
-        }
+
         
     }else{
         if(State[2]){
-            move(vrl[9],vrl[9],CompassPID,IrServo[9]);
+            moving(9);
             return ;
         }
         else{
-            move(vrl[3],vrl[3],CompassPID,IrServo[3]);
+            moving(3);
             return ;
         }
     }
@@ -728,7 +695,7 @@
     //int addDegree = 0;//追加角
     
     /**/
-    //int Gap=2;
+    int Gap=2;
     bool LineState[10]={0};
     
     
@@ -752,19 +719,19 @@
     
     while(1){
         
-        if(
+        /*if(
             (
                 (
                     (
                         (Compass / 10) + 540 - CompassDef
                     ) % 360
                 ) - REFERENCE
-            ) >15.0
+            ) >=15
         ){
             
             move(0,0,0,0);     
             continue;
-        }
+        }*/
         
         LineData = (~Line+0x00) & 0x0F;
         
@@ -773,7 +740,7 @@
         if(LineData!=0){
             
             if(PingData[0]>PingData[1]) LineState[0]=1;
-            if(PingData[2]<=20)         LineState[1]=1;
+            if(PingData[2]<=30)         LineState[1]=1;
             
             switch(LinkBit(0,0,0,0,0,0,LineState[1],LineState[0])&0x03){
                 
@@ -801,9 +768,9 @@
             continue;
             
         }else{
-            if(PingData[0]<=5) LineState[2]=1;
-            if(PingData[1]<=5) LineState[3]=1;
-            if(PingData[2]<=5) LineState[4]=1;
+            if(PingData[0]<=10) LineState[2]=1;
+            if(PingData[1]<=10) LineState[3]=1;
+            if(PingData[2]<=10) LineState[4]=1;
             
             switch(LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07){
                 
@@ -849,47 +816,40 @@
             wait(0.2);
             continue;
         }
-        else{
-            
-            moving(12);
-            continue;
+        
+    
+        switch(/*IrNum*/12){
+            case 0:
+            case 1:
+            case 2:
+            case 3:
+                moving(IrNum);
+                break;
+            case 4:
+            case 5:
+                moving(IrNum+Gap);
+                break;
+            case 6:
+                if(PingData[0]>PingData[1]) moving(IrNum+Gap);
+                else moving(IrNum-Gap);
+                
+                break;
+            case 7:
+            case 8:
+                moving(IrNum-Gap);
+                break;
+            case 9:
+            case 10:
+            case 11:
+                moving(IrNum);
+                break;
+            case 12:
+                GoHome();
+                break;
+            default:
+                break;
         }
         
+        
     }
-    /*
-    Gap=2;
-    
-    switch(IrNum){
-        case 0:
-        case 1:
-        case 2:
-        case 3:
-            moving(IrNum);
-            break;
-        case 4:
-        case 5:
-            moving(IrNum+Gap);
-            break;
-        case 6:
-            if(PingData[0]>PingData[1]) moving(IrNum+Gap);
-            else moving(IrNum-Gap);
-            
-            break;
-        case 7:
-        case 8:
-            moving(IrNum-Gap);
-            break;
-        case 9:
-        case 10:
-        case 11:
-            moving(IrNum);
-            break;
-        case 12:
-            GoHome();
-            break;
-            
-    }
-    */
-    
-    
 }