Diff: main_processing/strategy/strategy.cpp

Revision:

34:69bdf29a4442

Parent:

31:0b0f64831771

Child:

35:a0421268ee95

--- a/main_processing/strategy/strategy.cpp	Tue Feb 02 04:21:09 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Tue Feb 02 10:31:05 2016 +0000
@@ -22,12 +22,12 @@
     ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
     ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
     if(data.irPosition<8){
-        ir_x *= data.l_pow;
-        ir_y *= data.l_pow;
+        ir_x *= (double)(data.l_pow);
+        ir_y *= (double)(data.l_pow);
     }
     else{
-        ir_x *= data.s_pow;
-        ir_y *= data.s_pow;
+        ir_x *= (double)(data.s_pow);
+        ir_y *= (double)(data.s_pow);
     }
     //Lineを考慮していないIrのみの値
     vx = ir_x;
@@ -65,8 +65,15 @@
     int vx,vy,vs;
     uint8_t LineDir[4];
     uint8_t LineStop[2];
-    //uint8_t IrRange[4];
-    //uint8_t LineBind[4];
+    uint8_t IrRange[4];
+    uint8_t static LineBind[4];
+    if(data.KickOffFlag==1){
+        LineBind[0]=0;
+        LineBind[1]=0;
+        LineBind[2]=0;
+        LineBind[3]=0;
+        data.KickOffFlag=0;
+    }
     if(data.IrFlag==1){
         ReadIr();
         data.IrFlag=0;
@@ -100,6 +107,23 @@
     LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
     LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
     
+    //Ir
+    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));
+    
+    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;
@@ -121,9 +145,26 @@
     return;
 }
 void modeAttack2(void){
+    if(data.IrFlag==1){
+        ReadIr();
+        data.IrFlag=0;
+    }
+    if(data.PidFlag==1){
+        PidUpdate();
+        data.PidFlag=0;
+    }
+    move(0,0,10);
+    if(data.MotorFlag==1){
+        tx_motor();
+        data.MotorFlag=0;
+    }
+    
     return;
 }
 void modeAttack3(void){
+    if(data.KickFlag==1){
+        DriveSolenoid();
+    }
     return;
 }
 void modeAttack4(void){