Diff: main_processing/strategy/strategy.cpp

Revision:

29:b413b0bb07a1

Parent:

28:8ac6c3c1e643

Child:

30:26070ba1f21f

--- a/main_processing/strategy/strategy.cpp	Mon Feb 01 02:05:44 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Mon Feb 01 11:25:32 2016 +0000
@@ -7,8 +7,7 @@
     int vx,vy,vs;
     uint8_t LineDir[4];
     uint8_t LineStop[2];
-    uint8_t IrRange[4];
-    uint8_t LineBind[4];
+    //uint8_t IrRange[4];
     if(data.IrFlag==1){
         ReadIr();
         data.IrFlag=0;
@@ -29,7 +28,10 @@
         ir_x *= data.s_pow;
         ir_y *= data.s_pow;
     }
-    
+    //Lineを考慮していないIrのみの値
+    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))));
@@ -39,24 +41,55 @@
     LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
     
     
-    
+    /*
     IrRange[A_SPOT] = (data.irPosition==19)||(data.irPosition==8)||(data.irPosition==9);
-    IrRange[B_SPOT] = (data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12);
-    IrRange[C_SPOT] = (data.irPosition==13)||(data.irPosition==14)||(data.irPosition==15);
-    IrRange[AB_SPOT] = (data.irPosition==16)||(data.irPosition==17)||(data.irPosition==18);
+    IrRange[B_SPOT] = (data.irPosition==13)||(data.irPosition==14)||(data.irPosition==15);
+    IrRange[C_SPOT] = (data.irPosition==16)||(data.irPosition==17)||(data.irPosition==18);
+    IrRange[AB_SPOT] = (data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12);
+    */
+    /*
+    IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
+                    ||((6<=data.irPosition)&&(data.irPosition<=7))||((                   1)&&(data.irPosition<=1));
+    IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
+    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));
+    
+    data.LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(data.LineBind[A_SPOT]==1)));
+    data.LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(data.LineBind[B_SPOT]==1)));
+    data.LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(data.LineBind[C_SPOT]==1)));
+    data.LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(data.LineBind[AB_SPOT]==1)));
+    */
     
-    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)));
-    
-    vx = ir_x*(!(LineBind[A_SPOT]==1))*(!(LineBind[B_SPOT]==1))*(!(LineBind[C_SPOT]==1))*(!(LineBind[AB_SPOT]==1));
-    vy = ir_y*(!(LineBind[A_SPOT]==1))*(!(LineBind[B_SPOT]==1))*(!(LineBind[C_SPOT]==1))*(!(LineBind[AB_SPOT]==1));;
+    /*
+    if(data.LineBind[A_SPOT]||data.LineBind[B_SPOT]||data.LineBind[C_SPOT]||data.LineBind[AB_SPOT]){
+        //Line_timeout.attach();
+        vx=(LINE_RF*2)*((-1)*data.LineBind[A_SPOT] +      data.LineBind[B_SPOT]);
+        vy=(LINE_RF*2)*(     data.LineBind[C_SPOT] + (-1)*data.LineBind[AB_SPOT]);
+        if((LineDir[A_SPOT]==0)||(LineDir[B_SPOT]==0)||(LineDir[C_SPOT]==0)||(LineDir[AB_SPOT]==0)){
+            vx=(LINE_RF*2)*(-data.LineBind[A_SPOT] + data.LineBind[B_SPOT]);
+            vy=(LINE_RF*2)*(data.LineBind[C_SPOT] - data.LineBind[AB_SPOT]);
+        }
+        else{
+            vx=0;
+            vy=0;
+        }
+    }*/
+    /*if(data.LineBind[A_SPOT]||data.LineBind[B_SPOT]||data.LineBind[C_SPOT]||data.LineBind[AB_SPOT]){
+        vx=(LINE_RF*2)*((-1.0)*data.LineBind[A_SPOT]*(LineDir[A_SPOT]==0) +        data.LineBind[B_SPOT]*(LineDir[B_SPOT]==0));
+        vy=(LINE_RF*2)*(       data.LineBind[C_SPOT]*(LineDir[C_SPOT]==0) + (-1.0)*data.LineBind[AB_SPOT]*(LineDir[AB_SPOT]==0));
+        
+    }*/
     vs = data.OutputPID;
-    
+    /*
+    //ForLineAll
+    data.motorlog[X_AXIS] *= LineStop[X_AXIS];
+    data.motorlog[Y_AXIS] *= LineStop[Y_AXIS];
+    data.motorlog[X_AXIS] += vx;
+    data.motorlog[Y_AXIS] += vy;
+    */
     move(
-        vx*LineStop[X_AXIS] + (LINE_RF)*(vx!=0)*(LineDir[C_SPOT]==0) + (-LINE_RF)*(vx!=0)*(LineDir[AB_SPOT]==0),
-        vy*LineStop[Y_AXIS] + (-LINE_RF)*(vy!=0)*(LineDir[A_SPOT]==0) + (LINE_RF)*(vy!=0)*(LineDir[B_SPOT]==0),
+        vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)),
+        vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)),
         vs
     );