Diff: main_processing/strategy/strategy.cpp

Revision:

31:0b0f64831771

Parent:

30:26070ba1f21f

Child:

34:69bdf29a4442

--- a/main_processing/strategy/strategy.cpp	Mon Feb 01 11:43:58 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Tue Feb 02 04:07:06 2016 +0000
@@ -7,6 +7,8 @@
     int vx,vy,vs;
     uint8_t LineDir[4];
     uint8_t LineStop[2];
+    //uint8_t IrRange[4];
+    //uint8_t LineBind[4];
     if(data.IrFlag==1){
         ReadIr();
         data.IrFlag=0;
@@ -39,7 +41,7 @@
     LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
     LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
     
-    vs = data.OutputPID;
+    //Ir
     /*
     //ForLineAll
     data.motorlog[X_AXIS] *= LineStop[X_AXIS];
@@ -59,6 +61,63 @@
     return;
 }
 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 LineBind[4];
+    if(data.IrFlag==1){
+        ReadIr();
+        data.IrFlag=0;
+    }
+    if(data.PidFlag==1){
+        Line_ticker.detach();
+        PidUpdate();
+        Line_ticker.attach(&ReadLine, 0.005);
+        data.PidFlag=0;
+    }
+    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;
+    }
+    else{
+        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))));
+    LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))));
+    
+    LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
+    LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
+    
+    if(data.IrFlag==1){
+        ReadIr();
+        data.IrFlag=0;
+    }
+    if(data.PidFlag==1){
+        PidUpdate();
+        data.PidFlag=0;
+    }
+    vs = data.OutputPID;
+    move(
+        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
+    );
+    if(data.MotorFlag==1){
+        tx_motor();
+        data.MotorFlag=0;
+    }
     return;
 }
 void modeAttack2(void){