Diff: main.cpp

Revision:

6:2c9cb40ada59

Parent:

5:c8e4d5cdc353

Child:

7:81f57b67dff8

--- a/main.cpp	Fri Mar 13 03:17:44 2015 +0000
+++ b/main.cpp	Sat Mar 14 01:06:13 2015 +0000
@@ -70,21 +70,55 @@
 void Interval(void){
     WaitFlag=0;
 }
+void Interval3(void){
+    CompassFlag=0;
+    //HomeFlag=0;
+}
+void Interval2(void){
+    
+    CompassFlag=1;
+    //CompassFlag=0;
+    DTimer.attach(Interval3, 1.25);
+    
+}
+
 void move(int vr,int vl, double vs ,int Degree){
     double pwm[4] = {0};
     uint8_t i = 0;
-    double CompassVias=15.0;
-    double CompassDif;
+    
+    int dec = 0;
+    static double CompassVias=15+dec;
+    
+   // bool BackFlag;
+    
+    //BackFlag=0;
+    //if((vr==vrl[6])&&(Degree==IrServo[6])&&(PingData[2]>=100))BackFlag=1;
+    
+    if(IrNum==12){
+        if(vr<0){
+            vr=vr-(-0);
+            vl=vr-(-0);
+        }
+        else{
+            vr=vr-0;
+            vl=vr-0;
+        }
+    }
+    
+    //CompassFlag=0;
     
     vr=vr*( 1.0);
     vl=vl*(-1.0);
     Degree = Degree*(-1.0);
     
     CompassDif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE;
-    if(/*abs(CompassDif)<CompassVias*/1){
+    if(
+        (abs(CompassDif)<CompassVias            )||
+        CompassFlag/*((BackFlag==1)&&(abs(CompassDif)<45)    )*/  
+    ){
         
-        
-        CompassVias=15.0;
+        CompassVias=15+dec;
+        if(IrNum==12) CompassVias=15+dec;
         
         pwm[0] = LastPwm[0] = vr;
         pwm[1] = LastPwm[1] =0;
@@ -97,17 +131,17 @@
     }
     else{
         
-        CompassVias=10.0;
+        CompassVias=10+dec;
         
         if(CompassDif<0){
             //vs = vs - (20-CompassVias);
             vs = -(15+(abs(CompassDif))*0.1);
-            if( abs(CompassDif) >= 100) vs = -(20+(100)*0.1);
+            if( abs(CompassDif) >= 100) vs = -(15+(100)*0.1);
         }
         else{
             //vs = vs + (20-CompassVias);
             vs = (15+(abs(CompassDif))*0.1);
-            if( abs(CompassDif) >= 100) vs = (20+(100)*0.1);
+            if( abs(CompassDif) >= 100) vs = (15+(100)*0.1);
         }
         
         pwm[0] = vs;
@@ -116,8 +150,9 @@
         pwm[3] =  vs*(-1)*(-1);
         
         Degree = 0.0;
+        CTimer.attach(Interval2, 0.2);
     }
-    if(Loop==1){
+    if(Loop|0){
         pwm[0] = 0;
         pwm[1] = 0;
         pwm[2] = 0;
@@ -142,6 +177,7 @@
     if(WaitFlag==1) Degree = LastDegree;
     
     
+    
     SetDegree = LastDegree = Degree;
     S555.position(SetDegree*11.0/9.0);
     
@@ -432,14 +468,13 @@
     
     if(PingData[2] <=10) State[8]=1;
     
-    if((PingData[2] >=15)&&(PingData[2] <=25)) State[9]=1;
+    if((PingData[2] >=10)&&(PingData[2] <=30)) State[9]=1;
     
-    if(PingData[2] <15) State[10]=1;
+    if(PingData[2] <10) State[10]=1;
     
     if(PingData[2] >25) State[11]=1;
     
     
-    
     if(PingData[0]>PingData[1]) State[2]=1;
     
     if( 
@@ -449,7 +484,7 @@
     
     if(
         (abs(    int(    int(PingData[0])    -   int(PingData[1])    )           )      )
-        <= 10
+        <= 25
     )   State[4]=1;
     
     
@@ -459,30 +494,41 @@
         return ;   
     }*/
     
+    
+    
+    
     if(State[4]){
         
-        if(State[9]){
+        
+        if(State[10]){
             moving(12);
             return ;
         }
-        if(State[10]){
-            moving(0);
-            return ;
-        }
         if(State[11]){
             moving(6);
             return ;
         }
-        
-
+        if(State[9]){
+            moving(12);
+            HomeFlag=1;
+            return ;
+        }
+        else{
+            moving(12);
+            HomeFlag=1;
+            return ;
+        }
         
     }else{
+        
         if(State[2]){
             moving(9);
+            //wait(0.5);
             return ;
         }
         else{
             moving(3);
+            //wait(0.5);
             return ;
         }
     }
@@ -567,7 +613,7 @@
     pid.setMode(AUTO_MODE);                         //pid sed def
     pid.setSetPoint(REFERENCE);                     //pid sed def
     
-    for(int i=0; i<15; i++){
+    for(int i=0; i<5; i++){
         CompassDef = (Compass / 10);
         wait_ms(10);
     }
@@ -719,6 +765,11 @@
     
     while(1){
         
+        if(IrNum!=12){
+            HomeFlag=0;
+        }
+        //if(HomeFlag)
+        
         /*if(
             (
                 (
@@ -733,6 +784,8 @@
             continue;
         }*/
         
+        
+        
         LineData = (~Line+0x00) & 0x0F;
         
         for(int i=0; i<10; i++)  LineState[i]=0;
@@ -764,85 +817,73 @@
                 
             }
             move(LastPwm[0]*(-1),LastPwm[3]*(-1)*(-1),0,LastDeg);*/
-            wait(0.5);
+            //wait(0.5);
             continue;
             
-        }else{
-            if(PingData[0]<=10) LineState[2]=1;
-            if(PingData[1]<=10) LineState[3]=1;
-            if(PingData[2]<=10) LineState[4]=1;
+        }
+        if(
+                (!(
+                    (abs(    int(    int(PingData[0])    -   int(PingData[1])    )           )      )
+                    <= 30
+                ))   
+           ){
             
-            switch(LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07){
-                
-                case 0:
-                    break;
-                case 1:
-                    moving(3);
-                    break;
-                case 2:
-                    moving(9);
-                    break;
-                case 3:
-                    moving(6);
-                    break;
-                case 4:
-                    moving(0);
-                    break;
-                case 5:
-                    moving(1);
-                    break;
-                case 6:
-                    moving(11);
-                    break;
-                case 7:
-                    moving(0);
-                    break;
-                default :
-                    break;
-                
+            if(PingData[0]>PingData[1]){
+                moving(9);
+                //wait(0.5);
+                continue;
+            }
+            else{
+                moving(3);
+                //wait(0.5);
+                continue;
             }
             
-            if((LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07)!=0){
-                wait(0.2);
+        }
+        if(
+                ((
+                    (abs(    int(    int(PingData[0])    -   int(PingData[1])    )           )      )
+                    <= 30
+                ))   &&
+                (
+                    PingData[2]>15
+                
+                )
+           ){
+            
+                moving(6);
+                //wait(0.5);
                 continue;
-            }
+            
         }
         
         
         
-        if(PingData[2]>=90){
+        switch(IrNum){
             
-            moving(6);
-            wait(0.2);
-            continue;
-        }
-        
-    
-        switch(/*IrNum*/12){
-            case 0:
             case 1:
             case 2:
             case 3:
-                moving(IrNum);
-                break;
             case 4:
             case 5:
-                moving(IrNum+Gap);
+                moving(3);
                 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);
+                moving(9);
                 break;
+                
+                
+            case 6:
+                if(PingData[0]>PingData[1]) moving(7);
+                else                        moving(5);
+                break;
+                
+            case 0:
             case 12:
                 GoHome();
                 break;
@@ -851,5 +892,11 @@
         }
         
         
+        
+        
+    
+        
+        
+        
     }
-}
+}
\ No newline at end of file