Diff: main_processing/strategy/old_strategy.cpp

Revision:

18:3a42a931c95a

Child:

21:378470320524

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_processing/strategy/old_strategy.cpp	Tue Mar 15 12:03:51 2016 +0000
@@ -0,0 +1,884 @@
+#include "mbed.h"
+#include "extern.h"
+
+//Atk
+void modeAttack0(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(sys.IrFlag==1){
+        ReadIr();
+        sys.IrFlag=0;
+    }
+    if(sys.PidFlag==1){
+        Line_ticker.detach();
+        PidUpdate();
+        Line_ticker.attach(&ReadLine, 0.005);
+        sys.PidFlag=0;
+    }
+    ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
+    if(data.irPosition<8){
+        ir_x *= (double)(sys.l_pow);
+        ir_y *= (double)(sys.l_pow);
+    }
+    else{
+        ir_x *= (double)(sys.s_pow);
+        ir_y *= (double)(sys.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];
+    
+    //Ir
+    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(sys.MotorFlag==1){
+        tx_motor();
+        sys.MotorFlag=0;
+    }
+    return;
+}
+uint8_t HmcResetFlag;
+uint8_t LineReverseFlag;
+void LineReverse(void){
+    LineReverseFlag=0;
+}
+uint8_t LineSign[3];
+uint8_t LineFirst[2];
+uint8_t LinePriority[2];//0を後に，1を優先
+void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;}
+void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;}
+void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;}
+void LineCall_A(void){
+    //
+    LineSign[A_SPOT] = 1;
+    if(LineSign[B_SPOT]==0){
+        LineFirst[X_AXIS] = A_SPOT;
+    }
+    if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
+        if(LineSign[C_SPOT]==0){
+            LineFirst[Y_AXIS] = AB_SPOT;
+        }
+    }
+    //
+    if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1;
+    //
+    Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
+}
+void LineCall_B(void){
+    //
+    LineSign[B_SPOT] = 1;
+    if(LineSign[A_SPOT]==0){
+        LineFirst[X_AXIS] = B_SPOT;
+    }
+    if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){
+        if(LineSign[C_SPOT]==0){
+            LineFirst[Y_AXIS] = AB_SPOT;
+        }
+    }
+    //
+    if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1;
+    //
+    Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
+}
+void LineCall_C(void){
+    //
+    LineSign[C_SPOT] = 1;
+    if(!((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))){
+        LineFirst[Y_AXIS] = C_SPOT;
+    }
+    //
+    if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1;
+    //
+    Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
+}
+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(sys.KickOffFlag==1){
+        LineBind[0]=0;
+        LineBind[1]=0;
+        LineBind[2]=0;
+        LineBind[3]=0;
+        LineReverseFlag=0;
+        
+        LineSign[A_SPOT]=0;
+        LineSign[B_SPOT]=0;
+        LineSign[C_SPOT]=0;
+        
+        data.lnFlag[A_SPOT]=0;
+        data.lnFlag[B_SPOT]=0;
+        data.lnFlag[C_SPOT]=0;
+        
+        sys.KickOffFlag=0;
+    }
+    if(sys.IrFlag==1){
+        ReadIr();
+        sys.IrFlag=0;
+    }
+    if(sys.PidFlag==1){
+        //Line_ticker.detach();
+        PidUpdate();
+        //Line_ticker.attach(&ReadLine, 0.005);
+        sys.PidFlag=0;
+    }
+    ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
+    if(data.irPosition<8){
+        ir_x *= sys.l_pow;
+        ir_y *= sys.l_pow;
+    }
+    else{
+        ir_x *= sys.s_pow;
+        ir_y *= sys.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))&&(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];
+    
+    //Ir
+    //strict
+    /*
+    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));
+    */
+    //sweet
+    IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
+                    ||((6<=data.irPosition)&&(data.irPosition<=7))||((                   1)&&(data.irPosition<=1));
+    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));
+    
+    
+    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)));
+    */
+    vx = vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0));
+    vy = vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0));
+    vs = cmps_set.OutputPID;
+    if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){
+        if(LineRaw>0){
+            vx=(LINE_RF*2)*((-1)*IrRange[A_SPOT] +      IrRange[B_SPOT]);
+            vy=(LINE_RF*2)*(     IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]);
+            /*vx=(LINE_RF*2)*((-1)*(LineFirst[X_AXIS] == A_SPOT) +      (LineFirst[X_AXIS] == B_SPOT));
+            vy=(LINE_RF*2)*(     (LineFirst[Y_AXIS] == C_SPOT) + (-1)*(LineFirst[Y_AXIS] == AB_SPOT));
+            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);*/
+        }
+        else{
+            vx=0;
+            vy=0;
+        }
+    }
+    if(LineRaw>0){
+        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);
+    }
+    move(
+        vx,
+        vy,
+        vs
+    );
+    if(sys.MotorFlag==1){
+        tx_motor();
+        sys.MotorFlag=0;
+    }
+    return;
+}
+void modeAttack2(void){
+    double ir_x, ir_y;
+    int vx,vy,vs;
+    /*int 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];*/
+    //buint8_t static spi_count;
+    if(sys.KickOffFlag==1){
+        
+        sys.MotorFlag=0;
+        sys.IrFlag=0;
+        /*LineBind[0]=0;
+        LineBind[1]=0;
+        LineBind[2]=0;
+        LineBind[3]=0;
+        LineReverseFlag=0;
+        
+        LineSign[A_SPOT]=0;
+        LineSign[B_SPOT]=0;
+        LineSign[C_SPOT]=0;
+        
+        data.lnFlag[A_SPOT]=0;
+        data.lnFlag[B_SPOT]=0;
+        data.lnFlag[C_SPOT]=0;
+        
+        HmcResetFlag = 0;
+        sys.UswFlag = 0;
+        //spi_count=0;
+        */
+        hmc_reset=HMC_RUN;
+        sys.KickFlag = 0;
+        
+        sys.KickOffFlag=0;
+        //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
+    }
+    if(sys.IrFlag==1){
+        //LED[0] = 0;
+        //LED[1] = 1;
+        /*spi_count++;
+        if(spi_count%10 == 0){
+            ReadPing();
+        }
+        else{
+            ReadIr();
+        }
+        if(spi_count==20) spi_count=0;
+        */
+        ReadIr();
+        sys.IrFlag=0;
+    }
+    if(sys.PidFlag==1){
+        PidUpdate();
+        sys.PidFlag=0;
+    }
+    /*if(sys.UswFlag==1){
+        ReadPing();
+        sys.UswFlag=0;
+    }*/
+    /*
+    if(HmcResetFlag==1){
+        HmcReset();
+        HmcResetFlag=0;
+    }
+    */
+    //ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    //ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
+    ir_x = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
+    ir_y = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
+    
+    if(data.irPosition<8){
+        ir_x *= sys.l_pow;
+        ir_y *= sys.l_pow;
+    }
+    else{
+        ir_x *= sys.s_pow;
+        ir_y *= sys.s_pow;
+    }
+    
+    //Lineを考慮していないIrのみの値
+    vx = ir_x;
+    vy = ir_y;
+    /*
+    if((data.irPosition==10)&&(vy>0)){
+        vy += 0;//前進加速
+    }
+    if((data.irPosition==11)&&(vy>0)){
+        vy += 0;//前進加速
+        if(sys.KickFlag==1){
+            DriveSolenoid();
+        }
+    }
+    if((data.irPosition==12)&&(vy>0)){
+        vy += 0;//前進加速
+    }
+    if((data.irPosition==1)&&(vy>0)){
+        vy += 0;//前進加速
+    }
+    if((data.irPosition==2)&&(vy>0)){
+        vy += 0;//前進加速
+    }
+    
+    if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
+        vx *= -1.0;//背後回り込みの左右判断
+    }*/
+    /*
+    if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
+        vx = vx*(0.75);
+        vy = vy*(0.75);
+    }
+    */
+    //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);
+    LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1                     ))&&(LineFirst[Y_AXIS] == C_SPOT);
+    LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
+    /*
+    LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1                  ))&&(LineFirst[X_AXIS] == A_SPOT);
+    LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1                  ))&&(LineFirst[X_AXIS] == B_SPOT);
+    LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1                  ))&&(LineFirst[Y_AXIS] == C_SPOT);
+    LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
+    
+    //Irボールの方向
+    //strict
+    
+    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));
+    */
+    //sweet
+    /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
+                    ||((6<=data.irPosition)&&(data.irPosition<=7))||((                   1)&&(data.irPosition<=1));
+    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);
+    
+    //白線踏んでる
+    if(LineRaw>0){
+        LineOn[A_SPOT]  = (LineSign[A_SPOT]==1)                         &&(LineFirst[X_AXIS]==A_SPOT);
+        LineOn[B_SPOT]  = (LineSign[B_SPOT]==1)                         &&(LineFirst[X_AXIS]==B_SPOT);
+        LineOn[C_SPOT]  = (LineSign[C_SPOT]==1)                         &&(LineFirst[Y_AXIS]==C_SPOT);
+        LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
+        //外側に向かう力を消す．
+        //x
+        if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
+            vx=0;
+            //yの力を加える．
+            if(vy>0){vy += 10;}
+            if(vy<0){vy -= 10;}
+        }
+        //y
+        if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
+            vy=0;
+        }
+        //内側に向かう力を加える．
+        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]<40) + (-1)*(data.ping[R_PING]<40)) +
+                            (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
+                            *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
+        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);
+    }
+    else{
+        LineForce[X_AXIS] = 0;
+        LineForce[Y_AXIS] = 0;
+    }
+    */
+    //vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
+    //vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
+    vs = cmps_set.OutputPID;
+    move(
+        vx,
+        vy,
+        vs
+    );
+    /*move(
+        0,
+        0,
+        10
+    );*/
+    if(sys.MotorFlag==1){
+        tx_motor();
+        sys.MotorFlag=0;
+    }
+    return;
+}
+void modeAttack3(void){
+    double ir_x, ir_y;
+    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];
+    //buint8_t static spi_count;
+    if(sys.KickOffFlag==1){
+        LineBind[0]=0;
+        LineBind[1]=0;
+        LineBind[2]=0;
+        LineBind[3]=0;
+        LineReverseFlag=0;
+        
+        LineSign[A_SPOT]=0;
+        LineSign[B_SPOT]=0;
+        LineSign[C_SPOT]=0;
+        
+        data.lnFlag[A_SPOT]=0;
+        data.lnFlag[B_SPOT]=0;
+        data.lnFlag[C_SPOT]=0;
+        
+        LinePriority[X_AXIS]=0;
+        LinePriority[Y_AXIS]=0;
+        
+        HmcResetFlag = 0;
+        sys.UswFlag = 0;
+        //spi_count=0;
+        
+        hmc_reset=HMC_RUN;
+        sys.KickFlag = 0;
+        
+        sys.KickOffFlag=0;
+        //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
+    }
+    if(sys.IrFlag==1){
+        /*spi_count++;
+        if(spi_count%10 == 0){
+            ReadPing();
+        }
+        else{
+            ReadIr();
+        }
+        if(spi_count==20) spi_count=0;
+        */
+        ReadIr();
+        sys.IrFlag=0;
+    }
+    if(sys.PidFlag==1){
+        PidUpdate();
+        sys.PidFlag=0;
+    }
+    if(sys.UswFlag==1){
+        ReadPing();
+        sys.UswFlag=0;
+    }
+    /*
+    if(HmcResetFlag==1){
+        HmcReset();
+        HmcResetFlag=0;
+    }
+    */
+    ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
+    if(data.irPosition<8){
+        ir_x *= sys.l_pow;
+        ir_y *= sys.l_pow;
+    }
+    else{
+        ir_x *= sys.s_pow;
+        ir_y *= sys.s_pow;
+    }
+    
+    //Lineを考慮していないIrのみの値
+    vx = ir_x;
+    vy = ir_y;
+    
+    if((data.irPosition==10)&&(vy>0)){
+        vy += 15;//前進加速
+    }
+    if((data.irPosition==11)&&(vy>0)){
+        vy += 15;//前進加速
+        if(sys.KickFlag==1){
+            DriveSolenoid();
+        }
+    }
+    if((data.irPosition==12)&&(vy>0)){
+        vy += 15;//前進加速
+    }
+    if((data.irPosition==1)&&(vy>0)){
+        vy += 25;//前進加速
+    }
+    if((data.irPosition==2)&&(vy>0)){
+        vy += 25;//前進加速
+    }
+    
+    
+    if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
+        vx *= -1.0;//背後回り込みの左右判断
+    }
+    /*
+    if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
+        vx = vx*(0.75);
+        vy = vy*(0.75);
+    }
+    */
+    //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);
+    LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1                     ))&&(LineFirst[Y_AXIS] == C_SPOT);
+    LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
+    
+    LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1                  ))&&(LineFirst[X_AXIS] == A_SPOT);
+    LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1                  ))&&(LineFirst[X_AXIS] == B_SPOT);
+    LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1                  ))&&(LineFirst[Y_AXIS] == C_SPOT);
+    LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
+    
+    //Irボールの方向
+    //strict
+    
+    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));
+    
+    //sweet
+    /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
+                    ||((6<=data.irPosition)&&(data.irPosition<=7))||((                   1)&&(data.irPosition<=1));
+    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] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
+    LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
+    
+    //白線踏んでる
+    if(LineRaw>0){
+        LineOn[A_SPOT]  = (LineSign[A_SPOT]==1)                         &&(LineFirst[X_AXIS]==A_SPOT);
+        LineOn[B_SPOT]  = (LineSign[B_SPOT]==1)                         &&(LineFirst[X_AXIS]==B_SPOT);
+        LineOn[C_SPOT]  = (LineSign[C_SPOT]==1)                         &&(LineFirst[Y_AXIS]==C_SPOT);
+        LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
+        //外側に向かう力を消す．
+        //x
+        if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
+            
+            if(LinePriority[Y_AXIS]==0){
+                LinePriority[X_AXIS]=1;
+                LinePriority[Y_AXIS]=0;
+            }
+            vx=0;
+            //yの力を加える．
+            if(vy>0){vy += 10;}
+            if(vy<0){vy -= 10;}
+        }
+        //y
+        if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
+            if(LinePriority[X_AXIS]==0){
+                LinePriority[X_AXIS]=0;
+                LinePriority[Y_AXIS]=1;
+            }
+            if((LinePriority[X_AXIS]==1)&&(LineOn[AB_SPOT]==1)){
+                LinePriority[X_AXIS]=0;
+                LinePriority[Y_AXIS]=1;
+            }
+            vy=0;
+        }
+        //内側に向かう力を加える．
+        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)*(LinePriority[X_AXIS])*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
+                            (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
+                            (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
+        LineForce[Y_AXIS] = (LINE_RF*2)*(LinePriority[Y_AXIS])*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[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);
+    }
+    else{
+        LineForce[X_AXIS] = 0;
+        LineForce[Y_AXIS] = 0;
+        
+        LinePriority[X_AXIS]=0;
+        LinePriority[Y_AXIS]=0;
+    }
+    
+    vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
+    vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
+    vs = cmps_set.OutputPID;
+    move(
+        vx,
+        vy,
+        vs
+    );
+    /*move(
+        10,
+        0,
+        0
+    );*/
+    if(sys.MotorFlag==1){
+        tx_motor();
+        sys.MotorFlag=0;
+    }
+    if(sys.stopflag==1){
+        //停止処理
+    }
+    return;
+}
+
+//Debug
+void modeDebug0(void){
+    return;
+}
+void modeDebug1(void){//ChaseOnly NonLine
+int vx,vy,vs;
+    if(sys.IrFlag==1){
+        ReadIr();
+        sys.IrFlag=0;
+    }
+    if(sys.PidFlag==1){
+        PidUpdate();
+        sys.PidFlag=0;
+    }
+    
+    vx = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    vy = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
+    vs = cmps_set.OutputPID;
+    if(data.irPosition<8){
+        vx *= sys.l_pow;
+        vy *= sys.l_pow;
+    }
+    else{
+        vx *= sys.s_pow;
+        vy *= sys.s_pow;
+    }
+    
+    
+    move(vx, vy, vs);
+    
+    if(sys.MotorFlag==1){
+        tx_motor();
+        sys.MotorFlag=0;
+    }
+    return;
+}
+void modeDebug2(void){//LineRestoringForce
+    int vx,vy,vs;
+    uint8_t LineStop[2];
+    int LineForce[2];
+    if(sys.IrFlag==1){
+        ReadIr();
+        sys.IrFlag=0;
+    }
+    if(sys.PidFlag==1){
+        PidUpdate();
+        sys.PidFlag=0;
+    }
+    vx = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    vy = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
+    vs = cmps_set.OutputPID;
+    if(data.irPosition<8){
+        vx *= sys.l_pow;
+        vy *= sys.l_pow;
+    }
+    else{
+        vx *= sys.s_pow;
+        vy *= sys.s_pow;
+    }
+    
+    
+    LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
+    LineStop[Y_AXIS] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))))*(!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
+    
+    LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
+    if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
+        LineForce[X_AXIS] = -LINE_RF;
+    }
+    if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
+        LineForce[X_AXIS] = LINE_RF;
+    }
+    if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
+        LineForce[Y_AXIS] = -LINE_RF;
+    }
+    if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
+        LineForce[Y_AXIS] = LINE_RF;
+    }
+    
+    move(
+        vx*LineStop[X_AXIS] + LineForce[X_AXIS],
+        vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
+        vs
+    );
+    if(sys.MotorFlag==1){
+        tx_motor();
+        sys.MotorFlag=0;
+    }
+    return;
+}
+void modeDebug3(void){//movesum
+    int vx,vy,vs;
+    uint8_t LineStop[2];
+    static uint8_t moveLnFlag[4]={1, 1, 1, 1};
+    //static int moveLnlog[4];
+    if(sys.IrFlag==1){
+        ReadIr();
+        sys.IrFlag=0;
+    }
+    if(sys.PidFlag==1){
+        PidUpdate();
+        sys.PidFlag=0;
+    }
+    vx = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    vy = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
+    vs = cmps_set.OutputPID;
+    if(data.irPosition<8){
+        vx *= sys.l_pow;
+        vy *= sys.l_pow;
+    }
+    else{
+        vx *= sys.s_pow;
+        vy *= sys.s_pow;
+    }
+    
+    LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
+    LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
+    
+    move(
+        vx*LineStop[X_AXIS],
+        vy*LineStop[Y_AXIS],
+        vs
+    );
+    if(sys.MotorFlag==1){
+        tx_motor();
+        sys.MotorFlag=0;
+    }
+    return;
+}
+void modeDebug4(void){//solenoid
+    if(sys.KickFlag==1){
+        DriveSolenoid();
+    }
+    return;
+}
+void modeDebug5(void){//cmpsTest
+    if(sys.IrFlag==1){
+        ReadIr();
+        sys.IrFlag=0;
+    }
+    if(sys.PidFlag==1){
+        PidUpdate();
+        sys.PidFlag=0;
+    }
+    wait(2);
+    kicker = 1;
+    wait(.5);
+    kicker = 0;
+    wait(.5);
+    //move(0,0,10);
+    //LED[0]=1;
+    //LED[3]=1;
+    if(sys.MotorFlag==1){
+        tx_motor();
+        sys.MotorFlag=0;
+    }
+    
+    return;
+}
\ No newline at end of file