Diff: main_processing/strategy/strategy.cpp

Revision:

43:f11f68918299

Parent:

42:02aaa806d929

Child:

45:c23f25c00d0d

--- a/main_processing/strategy/strategy.cpp	Thu Feb 04 20:45:41 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Fri Feb 05 11:01:03 2016 +0000
@@ -253,9 +253,11 @@
     int vx,vy,vs, LineForce[2];
     uint8_t LineDir[4];
     uint8_t LineOn[4];
+    uint8_t LineReturn[4];
     uint8_t LineStop[2];
     uint8_t IrRange[4];
     uint8_t static LineBind[4];
+    uint8_t static spi_count;
     if(data.KickOffFlag==1){
         LineBind[0]=0;
         LineBind[1]=0;
@@ -273,10 +275,20 @@
         
         HmcResetFlag = 0;
         PingFlag = 0;
+        spi_count=0;
         
         data.KickOffFlag=0;
     }
     if(data.IrFlag==1){
+        /*spi_count++;
+        if(spi_count%10 == 0){
+            ReadPing();
+        }
+        else{
+            ReadIr();
+        }
+        if(spi_count==20) spi_count=0;
+        */
         ReadIr();
         data.IrFlag=0;
     }
@@ -284,11 +296,11 @@
         PidUpdate();
         data.PidFlag=0;
     }
-    /*
     if(PingFlag==1){
         ReadPing();
         PingFlag=0;
     }
+    /*
     if(HmcResetFlag==1){
         HmcReset();
         HmcResetFlag=0;
@@ -312,14 +324,14 @@
     if((data.irPosition==11)&&(vy>0)){
         vy += 10;//前進加速
     }
-    if((data.irPosition==17)&&(data.motorlog[X_AXIS]>0)){
+    /*if((data.irPosition==17)&&(data.motorlog[X_AXIS]>0)){
         vx *= -1.0;//背後回り込みの左右判断
-    }
-    /*
+    }*/
+    
     if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
         vx *= -1.0;//背後回り込みの左右判断
     }
-    */
+    
     //Lineを踏み始めた方向を調べる
     LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1                     ))&&(LineFirst[X_AXIS] == A_SPOT);
     LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1                     ))&&(LineFirst[X_AXIS] == B_SPOT);
@@ -341,14 +353,21 @@
     IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
     IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
     IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||((                 1)&&(data.irPosition<=3));
+    //none
+    if(data.irNotice==IR_NONE){
+        IrRange[A_SPOT]  = 0;
+        IrRange[B_SPOT]  = 0;
+        IrRange[C_SPOT]  = 0;
+        IrRange[AB_SPOT] = 0;
+    }
     //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
     LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
     LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
     LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
     LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
     
-    LineStop[X_AXIS] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
-    LineStop[Y_AXIS] = (LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
+    LineStop[X_AXIS] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
+    LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
     
     //白線踏んでる
     if(RawLineSignal>0){
@@ -369,8 +388,39 @@
             vy=0;
         }
         //内側に向かう力を加える．
-        LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineOn[A_SPOT]==1) + ( 1)*(LineOn[B_SPOT]==1));
-        LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineOn[C_SPOT]==1) + (-1)*(LineOn[AB_SPOT]==1));
+        LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
+        if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
+            if(LineOn[C_SPOT]==0){
+                LineReturn[A_SPOT]=0;
+            }
+            else{
+                LineReturn[A_SPOT]=1;
+            }
+        }
+        
+        LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
+        if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
+            if(LineOn[C_SPOT]==0){
+                LineReturn[B_SPOT]=0;
+            }
+            else{
+                LineReturn[B_SPOT]=1;
+            }
+        }
+        LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
+        if(LineReturn[C_SPOT]==1){
+            LineReturn[A_SPOT]=0;
+            LineReturn[B_SPOT]=0;
+        }
+        LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
+        
+        LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) + 
+                            (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
+                            *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) +
+                            (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
+                            *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30));
+        LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
+        
         Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
         Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
         Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);