File content as of revision 0:b910276f9da2:

#include "mbed.h"
#include "extern.h"

//Atk
void modeAttack4(void){
    //irの値に影響するモーター補正値
    //uint8_t ir_pow;
    uint8_t ir_pow_x, ir_pow_y;
    double ir_x_dir, ir_y_dir;
    double ir_x_turn, ir_y_turn;
    double ir_x, ir_y;
    //lineの値に影響するモーター補正値
    double LineSlowPower[2];
    double LineReturnPower[2];
    
    //モーターの出力値
    int vx,vy,vs;
    
    ////初期値を決める等
    if(sys.KickOffFlag==1){
        /*
        ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
        sys.IrBlind=0;
        sys.LineBlind=0;
        sys.PingBlind=0;
        
        sys.HomeBlind=1;
        sys.DriBlind=1;
        //defence
        sys.DefenceFlag=0;
        */
        
        
        //Kick
        sys.KickStopFlag=0;
        //Ir
        sys.ir_pow_table = 0;
        //Line
        //data.lnCorner[L_LINE]=data.lnCorner[R_LINE]=data.lnCorner[F_LINE]=data.lnCorner[B_LINE]=LINE_EMPTY;
        data.lnRepeat = 0;
        data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
        data.lnStayNow[X_LINE]=data.lnStayNow[Y_LINE]=0;
        data.lnStop[X_LINE]=data.lnStop[Y_LINE]=1;
        
        //data.FieldSpot = LINE_INSIDE;
        //LineLiberate();
        //LineRankClear();
        
        //JSO2
        LineLiberate();
        
        LineRawRankClear();
        
        //data.lnRawMemory[A_SPOT]=0;
        //data.lnRawMemory[B_SPOT]=0;
        //data.lnRawMemory[C_SPOT]=0;
        
        data.lnRawReturn=0;
        
        LineRawLogReset();
        //backhome
        sys.HomeStayFlag[X_PING]=0;
        sys.HomeStayFlag[Y_PING]=0;
        //pid
        sys.TurnStopFlag=0;
        cmps_set.AtkDeg=0;
        cmps_set.HoldDeg=0;
        cmps_set.GoalDeg=0;
        //ドリブラー
        sys.BallHoldJudgeFlag=0;
        sys.BallHoldFlag=0;
        
        //初期値設定の終了
        sys.KickOffFlag=0;
    }
    ////DataRetrieve
    if(sys.InfoFlag==1){ReadInfo();sys.InfoFlag=0;}
    data.lnRaw = LineRaw;
    data.lnHold = LineHold;
    data.ball = ReadBall();
    
    //ボールがなければ自分のゴールに戻る
    if(data.irNotice==IR_NONE){
        
        cmps_set.GoalDeg=0;
        sys.BackHomeFlag=(sys.HomeBlind==0);
    }
    else{
        sys.BackHomeFlag=0;
        sys.HomeStayFlag[X_PING]=0;
        sys.HomeStayFlag[Y_PING]=0;
    }
    //回り込みの値を代入
    if(sys.DefenceFlag==1){
        
        if((data.irNotice==IR_NONE)||(data.irNotice==IR_FAR)){
            
            if((data.ping[L_PING]>=50)&&(data.ping[R_PING]>=50)){
                ir_x_dir = ir_move_val[5][data.irNotice][data.irPosition][IR_X_DIR];
                ir_x_turn = ir_move_val[5][data.irNotice][data.irPosition][IR_X_TURN];
                
                if(data.ping[B_PING]>=10){
                    ir_y_dir = -1;
                    ir_y_turn = 0;
                }
                else{
                    ir_y_dir = 0;
                    ir_y_turn = 0;
                }
            }
            else if((data.ping[L_PING]>=50)&&(data.ping[R_PING]<50)){
                ir_x_dir = -1;
                ir_x_turn = 0;
                
                if(data.ping[B_PING]>=45){
                    ir_y_dir = -1;
                    ir_y_turn = 0;
                }
                else{
                    ir_y_dir = 0;
                    ir_y_turn = 0;
                }
            }
            else if((data.ping[L_PING]<50)&&(data.ping[R_PING]>=50)){
                ir_x_dir = 1;
                ir_x_turn = 0;
                
                if(data.ping[B_PING]>=45){
                    ir_y_dir = -1;
                    ir_y_turn = 0;
                }
                else{
                    ir_y_dir = 0;
                    ir_y_turn = 0;
                }
            }
            else if((data.ping[L_PING]<50)&&(data.ping[R_PING]<50)){
                ir_x_dir = 0;
                ir_x_turn = 0;
                
                if(data.ping[B_PING]>=45){
                    ir_y_dir = -1;
                    ir_y_turn = 0;
                }
                else{
                    ir_y_dir = 0;
                    ir_y_turn = 0;
                }
            }
        }
        else{
            
            ir_x_dir = ir_move_val[5][data.irNotice][data.irPosition][IR_X_DIR];
            ir_x_turn = ir_move_val[5][data.irNotice][data.irPosition][IR_X_TURN];
            
            if((data.ping[B_PING]>=5)&&(data.ping[R_PING]>=50)&&(data.ping[L_PING]>=50)){
                ir_y_dir = -1;
                ir_y_turn = 0;
            }
            else{
                ir_y_dir = 0;
                ir_y_turn = 0;
            }
        }
        /*
        if((data.ping[L_PING]>=50)&&(data.ping[R_PING]>=50)&&(data.ping[B_PING]>10)){
            ir_y_dir = -1;
            ir_y_turn = 0;
            
            if(data.ping[B_PING]>=10){
                ir_y_dir = -1;
                ir_y_turn = 0;
            }
            else{
                ir_y_dir = 0;
                ir_y_turn = 0;
            }
            
            ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
            ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
        }
        else{
            
        }
        */
        /*
        if((data.ping[B_PING]>=45)&&(data.irNotice!=IR_NONE)&&(15<=data.irPosition)&&(data.irPosition<=19)){
            //sys.ir_pow_table=1;
            ir_x_dir = 0;
            ir_y_dir = -1;
            ir_x_turn = 0;
            ir_y_turn = 0;
        }
        else{
            if(data.irValPhase[IR_SHORT]>=DIS_4){
                sys.ir_pow_table=2;//直進
                ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
                ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
                ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
                ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
            }
            else{
                if(data.irValPhase[IR_SHORT]<=DIS_1){
                    sys.ir_pow_table=0;//3
                    ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
                    ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
                    ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
                    ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
                }
                else{
                    sys.ir_pow_table=0;
                    ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
                    ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
                    ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
                    ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
                }
            }
        }*/
    }
    else{
        if((data.ping[B_PING]>40)&&(0)){
            sys.ir_pow_table=0;
            ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
            ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
            ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
            ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
        }
        else{
            if((data.ping[B_PING]>60)&&(data.irValPhase[IR_SHORT]>=DIS_4)){
                sys.ir_pow_table=2;//直進
                ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
                ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
                ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
                ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
            }
            else{
                if(data.irValPhase[IR_SHORT]<=DIS_1){
                    sys.ir_pow_table=0;//3
                    ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
                    ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
                    ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
                    ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
                }
                else{
                    sys.ir_pow_table=0;
                    ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
                    ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
                    ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
                    ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
                }
            }
        }
        if((sys.HomeBlind==0)){
            if(((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER))&&(data.ping[B_PING]<60)&&
                (
                    (data.irPosition==11)||
                    (data.irPosition==(ir_posi_s[(11-8+24+1)%12]))||
                    (data.irPosition==(ir_posi_s[(11-8+24-1)%12]))||
                    (data.irPosition==(ir_posi_s[(11-8+24+2)%12]))||
                    (data.irPosition==(ir_posi_s[(11-8+24-2)%12]))||
                    (data.irPosition==(ir_posi_s[(11-8+24+3)%12]))||
                    (data.irPosition==(ir_posi_s[(11-8+24-3)%12]))
                )
            ){
                sys.ir_pow_table=2;//直進
                ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
                ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
                ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
                ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
            }
        }
    }
    
    //Irの検出値によって出力を調整
    if(data.irNotice==IR_CLOSER){
        //ir_pow = sys.s_pow;
        ir_pow_x = ir_pow_y = sys.s_pow;
    }
    else if(data.irNotice==IR_CLOSE){
        //ir_pow = sys.m_pow;
        ir_pow_x = ir_pow_y = sys.m_pow;
    }
    else if(data.irNotice==IR_FAR){
        //ir_pow = sys.l_pow;
        ir_pow_x = ir_pow_y = sys.l_pow;
    }
    else{//data.irNotice==IR_NONE
        //ir_pow = 0;
        ir_pow_x = ir_pow_y = 0;
    }
    
    if(sys.DefenceFlag==1){
        ir_pow_x = 30;
        ir_pow_y = 30;
        if(data.ping[L_PING]<=60){
            ir_pow_y = 30;
        }
        if(data.ping[L_PING]<=40){
            ir_pow_y = 25;
        }
        if(data.ping[L_PING]<=30){
            ir_pow_y = 20;
        }
        if(data.ping[L_PING]<15){
            ir_pow_y = 15;
        }
        if(data.ping[L_PING]<10){
            ir_pow_y = 10;
        }
    }
    ////ドリブラーdribbler
    //ホールド判定hold
    JudgeBallHolding();
    //JudgeBallHold();
    //sys.BallHoldFlag = data.ball;
    //ドリブラー駆動
    /*if((sys.TurnDriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){
        if(sys.TurnDriBlind==0){
            if(
                ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))||
                ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))
            ){
                cmps_set.GoalDeg=0;
            }
            else if((data.lnRawOrderLog1[0]==A_SPOT)&&(1)){
                cmps_set.GoalDeg=-30;
            }
            else if((data.lnRawOrderLog1[0]==B_SPOT)&&(1)){
                cmps_set.GoalDeg=30;
            }
            else{
                cmps_set.GoalDeg=0;
            }
        }
    }*/
    if((sys.TurnDriBlind==0)&&(/*data.lnRaw==0*/1)&&((data.irNotice==IR_CLOSER)||(data.irNotice==IR_CLOSE))&&(/*data.ping[B_PING]>90*/1)&&
    (
        (data.irPosition==11)||
        (data.irPosition==(ir_posi_s[(11-8+24+1)%12]))||
        (data.irPosition==(ir_posi_s[(11-8+24-1)%12]))||
        (data.irPosition==(ir_posi_s[(11-8+24+2)%12]))||
        (data.irPosition==(ir_posi_s[(11-8+24-2)%12]))
    )
    
    ){
        if(sys.TurnDriBlind==0){
            if(
                ((data.ping[L_PING]<40)&&(data.ping[R_PING]>70))
            ){
                cmps_set.GoalDeg=60;
            }
            else if(
                ((data.ping[R_PING]<40)&&(data.ping[L_PING]>70))
            ){
                cmps_set.GoalDeg=-60;
            }
            else{
                cmps_set.GoalDeg=0;
            }
        }
    }
    if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
        sys.DribbleFlag=1;
        if((sys.BallHoldFlag==1)||(data.ball==1)){
            //ir_pow=20;
            ir_pow_x = ir_pow_y = 20;
        }
    }
    else{
        sys.DribbleFlag=0;
        cmps_set.HoldDeg=0;
    }
    
    ////Kick
    /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
        DriveTurn();
    }*/
    /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
        DriveTurn();
        DriveSolenoid();
    }*/
    if(
        /*(sys.BallHoldFlag==1)&&
        (data.ball==1)&&
        (data.irNotice==IR_CLOSER)&&
        ((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))&&
        (data.ping[B_PING]>100)&&
        (
            ((data.ping[L_PING]<60)&&(data.ping[R_PING]>60))||
            ((data.ping[L_PING]>60)&&(data.ping[R_PING]<60))||
            (data.lnRawOrderLog1[0]==A_SPOT)||
            (data.lnRawOrder[0]==A_SPOT)||
            (data.lnRawOrderLog1[0]==B_SPOT)||
            (data.lnRawOrder[0]==B_SPOT)
        )&&
        (data.lnRaw==0)*/
        (sys.DriBlind==0)&&
        (data.ping[R_PING]<30)&&
        (data.ping[L_PING]<30)
    ){
        //DriveTurn();
        //DriveSolenoid();
    }
    
    //Irの細かい補正値の処理．基本的にLineの補正値を演算する処理が始まる前にIrのモーター出力値補正をすべて終わらせる．
    //if(sys.IrBlind==1) ir_pow=0;
    if(sys.IrBlind==1) ir_pow_x=ir_pow_y=0;
    if((data.ping[R_PING]<data.ping[L_PING])&&(data.irPosition==17)&&(data.irNotice==IR_CLOSER)){
        ir_x_turn = -ir_x_turn;
        ir_y_turn = -ir_y_turn;
    }
    
    if((sys.BackHomeFlag==1)&&(sys.HomeBlind==0)){
        ir_pow_x = ir_pow_y = 25;
        //x
        if((abs(data.ping[L_PING]-data.ping[R_PING])>20)&&(sys.HomeStayFlag[X_PING]==0)){
            if(data.ping[L_PING]>data.ping[R_PING]){
                ir_x = -1;
            }
            else{
                ir_x = 1;
            }
            if((data.ping[L_PING]<WhiteToWallPlus[X_PING])&&(data.ping[R_PING]<WhiteToWallPlus[X_PING])){
                ir_x = 0;
            }
        }
        else{
            ir_x = 0;
            //
            sys.HomeStayFlag[X_PING]=1;
        }
        //y
        if((data.ping[B_PING]>60)&&(1)&&(sys.HomeStayFlag[Y_PING]==0)){
            ir_y = -1;
            /*if((abs(data.ping[L_PING]-data.ping[R_PING])>30)&&(1)){
                ir_pow_y = 15;
            }*/
        }
        else{
            ir_y = 0;
            /*
            if((1)&&(data.ping[B_PING]>60)&&(data.ping[R_PING]>55)&&(data.ping[L_PING]>55)){
                ir_y = -1;
                ir_pow_y = 15;
            }
            else{
                ir_y = 0;
            }
            */
            //
            sys.HomeStayFlag[Y_PING]=1;
        }
    }
    else{
        ir_x = (ir_x_dir + ir_x_turn);
        ir_y = (ir_y_dir + ir_y_turn);
    }
    if((sys.HomeBlind==0)&&(data.irNotice==IR_FAR)){
        if(ir_y>0){
            ir_y=0;
            ir_pow_x = ir_pow_y = 25;
            
            if((data.ping[B_PING]>50)&&(1)){
                ir_y = -1;
                //ir_pow_y = 15;
            }
            
        }
        //ir_pow_x = ir_pow_y = 25;
        //if(ir_y>0) ir_y=0;
        /*if((data.ping[B_PING]>40)&&(1)){
            ir_y = -1;
            //ir_pow_y = 15;
        }*/
    }
    if(
        (sys.DefenceFlag==1)&&
        (ir_y>0)&&
        (data.ping[B_PING]>60)&&
        (data.ping[B_PING]<225)&&
        (sys.BackHomeFlag==0)
    ){
        ir_pow_y=0;
    }
    ///dribbler
    if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&(/*sys.BallHoldFlag==1*/data.ball==1)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
        
        if((1/*data.ping[L_PING]<60*/)&&(data.ping[R_PING]>90)){
            //ir_x = 1;
        }
        else{
            if(data.lnRawOrderLog1[0]==B_SPOT){
                 //ir_x = 1;
            }
        }
        if((1/*data.ping[R_PING]<60*/)&&(data.ping[L_PING]>90)){
            //ir_x = -1;
        }
        else{
            if(data.lnRawOrderLog1[0]==A_SPOT){
                 //ir_x = -1;
            }
        }
        if((data.ping[F_PING]<50)&&(data.ping[B_PING]>100)){
            //ir_y = -1;
        }
        if(
            ((data.ping[L_PING]<55)&&(data.ping[R_PING]<55))||
            ((data.ping[L_PING]>55)&&(data.ping[R_PING]>55))
        ){
            //DriveSolenoid();
        }
        //x
        if((abs(data.ping[L_PING]-data.ping[R_PING])>40)&&(data.lnRawOrderLog1[0]==A_SPOT)){
            ir_x = -1;
        }
        else if((abs(data.ping[L_PING]-data.ping[R_PING])>40)&&(data.lnRawOrderLog1[0]==B_SPOT)){
            ir_x = 1;
        }
        else if((abs(data.ping[L_PING]-data.ping[R_PING])>10)&&((data.ping[L_PING]+data.ping[R_PING])>80)){
            if(data.ping[L_PING]>data.ping[R_PING]){
                ir_x = -1;
            }
            else{
                ir_x = 1;
            }
        }
        else{
            if((abs(data.ping[L_PING]-data.ping[R_PING])>10)&&(1)){
                if(data.ping[L_PING]>data.ping[R_PING]){
                    ir_x = -1;
                }
                else{
                    ir_x = 1;
                }
            }
            else{
                ir_x = 0;
            }
            
            //ir_x = 0;
            DriveSolenoid();
        }
        //y
        if((data.ping[F_PING]<45)&&(abs(data.ping[L_PING]-data.ping[R_PING])>50)){
            //ir_y = 0.75;
        }
        //kick
        if((abs(data.ping[L_PING]-data.ping[R_PING])<30)&&(1)){
            DriveSolenoid();
        }
    }
    ////Lineセンサーの値を評価しモーターの出力補正値を演算
    if(sys.LineBlind==1){
        
        LineSlowPower[X_LINE] = 1.0;
        LineSlowPower[Y_LINE] = 1.0;
        
        LineReturnPower[X_LINE] = 0.0;
        LineReturnPower[Y_LINE] = 0.0;
        
        data.lnStop[X_LINE] = 1;
        data.lnStop[Y_LINE] = 1;
        
        data.FieldSpot = LINE_INSIDE;
    }
    else{
        /*
        LineJudgeReset(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
        LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
        LineJudgeReturn(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
        */
        LineJudgeReset3(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
        LineJudgeSlow2(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE], &ir_pow_x, &ir_pow_y);
        LineJudgeReturn2(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
    }
    ////LEDデバッグ
    //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
    //if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
    //LED = ((data.lnRawMemory[0]!=0)<<2) | ((data.lnRawMemory[1]!=0)<<1) | ((data.lnRawMemory[2]!=0)<<0);
    //LED = ((data.lnRawOrder[0]!=LINE_EMPTY)<<2) | ((data.lnRawOrder[1]!=LINE_EMPTY)<<1) | ((data.lnRawOrder[2]!=LINE_EMPTY)<<0);
    //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
    
    //LED = sys.BallHoldFlag*15;
    if(sys.BallHoldFlag==0) LED=15;
    else LED=10;
    /*
    if((abs(data.ping[L_PING]-data.ping[R_PING])>10)&&((data.ping[L_PING]+data.ping[R_PING])>80)){
        if(data.ping[L_PING]>data.ping[R_PING]){
            LED=9;
        }
        else{
            LED=6;
        }
    }
    else{
        LED=15;
    }*/
    
    //if(data.ball==1) LED=15;
    //else LED=10;
    
    //if(data.lnRepeat>=1) LED=15;
    //else LED=10;
    
    //LED = 0xFF*(data.ping[B_PING]<30);
    
    //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
    
    //else LED = 0xA;
    //LED = LineHold;
    
    
    ////最終的なモーターの出力を演算
    vx = (ir_pow_x*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[X_LINE];
    vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[Y_LINE];
    vs = cmps_set.OutputPID;
    move(
        vx,
        vy,
        vs
    );
    //モーターに信号を出力
    if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;}
    
    if(sys.stopflag==1){
        //コマンドモードに戻る際の処理
    }
    return;
}

void modeAttack5(void){
    ////初期値を決める等
    if(sys.KickOffFlag==1){
        ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
        sys.IrBlind=0;
        sys.LineBlind=0;
        sys.PingBlind=0;
        
        sys.HomeBlind=1;
        sys.DriBlind=0;
        sys.DriMotorBlind=0;
        sys.TurnAtkBlind=1;
        sys.TurnDriBlind=1;
        sys.TurnHoldBlind=1;
        sys.KickBlind=0;
        //defence
        sys.DefenceFlag=0;
        //初期値設定の終了
        //sys.KickOffFlag=0;
    }
    modeAttack4();
}