Diff: main_processing/strategy/strategy.cpp

Revision:

35:a0421268ee95

Parent:

34:69bdf29a4442

Child:

36:50092d488f78

--- a/main_processing/strategy/strategy.cpp	Tue Feb 02 10:31:05 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Tue Feb 02 11:28:15 2016 +0000
@@ -60,18 +60,26 @@
     }
     return;
 }
+
+
+uint8_t LineReverseFlag;
+void LineReverse(void){
+    LineReverseFlag=0;
+}
 void modeAttack1(void){
     double ir_x, ir_y;
     int vx,vy,vs;
     uint8_t LineDir[4];
     uint8_t LineStop[2];
     uint8_t IrRange[4];
+    uint8_t LinePulse[4];
     uint8_t static LineBind[4];
     if(data.KickOffFlag==1){
         LineBind[0]=0;
         LineBind[1]=0;
         LineBind[2]=0;
         LineBind[3]=0;
+        LineReverseFlag=0;
         data.KickOffFlag=0;
     }
     if(data.IrFlag==1){
@@ -99,10 +107,10 @@
     vx = ir_x;
     vy = ir_y;
     //Line検出方向を調べる
-    LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))));
-    LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))));
-    LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
-    LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))));
+    LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))&&(1)));
+    LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))&&(1)));
+    LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1                      ))&&(1)));
+    LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(1)));
     
     LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
     LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
@@ -114,23 +122,36 @@
     IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
     IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||((                 1)&&(data.irPosition<=3));
     
+    
+    LinePulse[A_SPOT] = ((IrRange[A_SPOT]==1)&&(LineDir[A_SPOT]==0));
+    LinePulse[B_SPOT] = ((IrRange[B_SPOT]==1)&&(LineDir[B_SPOT]==0));
+    LinePulse[C_SPOT] = ((IrRange[C_SPOT]==1)&&(LineDir[C_SPOT]==0));
+    LinePulse[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&(LineDir[AB_SPOT]==0));
+    
+    LineBind[A_SPOT] = ((LinePulse[A_SPOT])||((IrRange[A_SPOT]==1)&&(LineBind[A_SPOT]==1)))&&(LineBind[B_SPOT]==0);
+    LineBind[B_SPOT] = ((LinePulse[B_SPOT])||((IrRange[B_SPOT]==1)&&(LineBind[B_SPOT]==1)))&&(LineBind[A_SPOT]==0);
+    LineBind[C_SPOT] = ((LinePulse[C_SPOT])||((IrRange[C_SPOT]==1)&&(LineBind[C_SPOT]==1)))&&(LineBind[AB_SPOT]==0);
+    LineBind[AB_SPOT] = ((LinePulse[AB_SPOT])||((IrRange[AB_SPOT]==1)&&(LineBind[AB_SPOT]==1)))&&(LineBind[C_SPOT]==0);
+    /*
     LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1)));
     LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1)));
     LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1)));
     LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1)));
-    
+    */
     
     if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){
-        vx=0;
-        vy=0;
-    }
-    if(data.IrFlag==1){
-        ReadIr();
-        data.IrFlag=0;
-    }
-    if(data.PidFlag==1){
-        PidUpdate();
-        data.PidFlag=0;
+        if(((LinePulse[A_SPOT]==1)||(LinePulse[B_SPOT]==1)||(LinePulse[C_SPOT]==1)||(LinePulse[AB_SPOT]==1)||1)&&(LineReverseFlag==0)){
+            LineReverseFlag=1;
+            Line_timeout.attach(&LineReverse, 1.0);
+        }
+        if(LineReverseFlag==1){
+            vx=(LINE_RF*2)*((-1)*LineBind[A_SPOT] +      LineBind[B_SPOT]);
+            vy=(LINE_RF*2)*(     LineBind[C_SPOT] + (-1)*LineBind[AB_SPOT]);
+        }
+        else{
+            vx=0;
+            vy=0;
+        }
     }
     vs = data.OutputPID;
     move(