File content as of revision 18:3a42a931c95a:

#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;
}