CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp

Committer:
lilac0112_1
Date:
2016-03-10
Revision:
12:bee8f883c33a
Parent:
11:3efae754e6ef
Child:
13:b20921316f3c

File content as of revision 12:bee8f883c33a:

#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;
void HmcReset(void){
    hmc_reset=HMC_RST;
    wait_us(100);
    hmc_reset=HMC_RUN;
}
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;
}
double LineJudgeX(double x){
    const double static LineDecline[5] = {1, .75, .5, .375, 0};
    const uint8_t static WhiteToWall = 30;
    uint8_t LineState;
    uint8_t LnRaw;
    uint8_t LnHold;
    
    LnRaw = LineRaw;
    LnHold = LineHold;
    
    //line
    LnRaw = LineRaw;
    LnHold = LineHold;
    /*
    if((LineRaw==0)){
        if((data.ping[L_PING]>60)&&(data.ping[R_PING]>60)){
            LineLiberate();
        }
    }*/
    
    
    
    if(LnHold==7){
        if(LnRaw>0){
            //場外間際
            LineState = 3;
        }
        else{//LnRaw==0
            //powerVoid & restore
            //場外
            LineState = 4;
        }
    }
    else if(LnHold>0){
        if(LnRaw>0){
            //踏んでるけどまだ出てない
            LineState = 4;
        }
        else{//LnRaw==0
            //線をまたいでいるか，中途半端に線を踏んだあと復帰したか
            LineState = 4;
            //ping&reset
            //timeout&reset
        }
    }
    //else if(LnHold==0){...maxpower
    
    if(x>=0){
        if(data.ping[R_PING]<WhiteToWall) return LineDecline[LineState];
        else return 1;
    }
    else{
        if(data.ping[L_PING]<WhiteToWall) return LineDecline[LineState];
        else return 1;
    }
}
void LineJudgeSlow(double *x, double *y){
    const double static LineDecline[5] = {1, .75, .5, .375, 0};
    const uint8_t static WhiteToWall[2] = {30, 30};
    uint8_t LineState;
    
    //line
    LineState = 0;
    if(data.lnHold==7){
        if(data.lnRaw>0){
            //場外間際...減衰
            LineState = 3;
        }
        else{//data.lnRaw==0
            //場外...出力完全無効+反発
            //powerVoid & restore
            LineState = 4;
        }
    }
    else if(data.lnHold>0){
        if(data.lnRaw>0){
            //踏んでるけどまだ出てない...減衰
            LineState = 2;
            //timeout&reset...detach
        }
        else{//data.lnRaw==0
            //線をまたいでいるか，中途半端に線を踏んだあと復帰したか...減衰
            LineState = 1;
            //if((data.ping[L_PING]>40)&&(data.ping[R_PING]>40)) LineLiberate();
            //ping&reset
            //timeout&reset
        }
    }
    //else if(data.lnHold==0){...maxpower
    
    if(*x>=0){
        if(data.ping[R_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
        else *x=1;
    }
    else{
        if(data.ping[L_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
        else *x=1;
    }
    
    if(*y>=0){
        if(data.ping[F_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
        else *y=1;
    }
    else{
        if(data.ping[B_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
        else *y=1;
    }
}
void LineJudgeReturn(double pow_x, double pow_y, int8_t *x, int8_t *y){
    const int8_t static LineReturn[5] = {0, 0, 0, 0, 10};
    const uint8_t static WhiteToWall[2] = {30, 30};
    uint8_t LinePingState[4];
    //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
    //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
    //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
    //×ラインのほぼ場外では常時ラインから離れる力が優先される．
    //※場外判定を行うには再び場内に戻る必要がある．
    
    
    
    if(data.FieldState==LINE_OUTSIDE){
        
        LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWall[X_PING]);
        LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWall[X_PING]);
        LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWall[Y_PING]);
        LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWall[Y_PING]);
        
        //line間際の復帰力以外の力を作用させるか否か
        data.lnStop[X_LINE]=1;
        data.lnStop[Y_LINE]=1;
        //x
        if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){
            if(data.ping[L_PING]>data.ping[R_PING]) *x = -LineReturn[4];
            if(data.ping[L_PING]==data.ping[R_PING]) *x = 0;
            if(data.ping[L_PING]<data.ping[R_PING]) *x = LineReturn[4];
        }
        if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
            if(pow_x<0){*x = 0;}
            else{*x = -LineReturn[4];data.lnStop[X_LINE]=0;}
        }
        if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
            if(pow_x>0){*x = 0;}
            else{*x = -LineReturn[4];data.lnStop[X_LINE]=0;}
        }
        if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
            *x = 0;
        }
        
        //y
        if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){
            if(data.ping[L_PING]>data.ping[R_PING]) *x = -LineReturn[4];
            if(data.ping[L_PING]==data.ping[R_PING]) *x = 0;
            if(data.ping[L_PING]<data.ping[R_PING]) *x = LineReturn[4];
        }
        if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
            if(pow_x<0){*x = 0;}
            else{*x = -LineReturn[4];data.lnStop[X_LINE]=0;}
        }
        if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
            if(pow_x>0){*x = 0;}
            else{*x = -LineReturn[4];data.lnStop[X_LINE]=0;}
        }
        if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
            *x = 0;
        }
    }
    else{//data.FieldState==LINE_INSIDE
        *x = 0;
        *y = 0;
        
        //line間際の復帰力以外の力を作用させるか否か
        data.lnStop[X_LINE]=1;
        data.lnStop[Y_LINE]=1;
    }
}
void LineJudgeReset(void){
    return;
}
void modeAttack4(void){
    double ir_x_dir, ir_y_dir;
    double ir_x_turn, ir_y_turn;
    double ir_x, ir_y;
    uint8_t ir_pow;
    int vx,vy,vs;
    
    
    if(sys.KickOffFlag==1){
        
        sys.IrBlind=0;
        sys.LineBlind=0;
        sys.PingBlind=0;
        
        sys.KickOffFlag=0;
    }
    //data
    if(sys.IrFlag==1){ReadIr();sys.IrFlag=0;}
    if(sys.PidFlag==1){PidUpdate();sys.PidFlag=0;}
    if(sys.UswFlag==1){ReadPing();sys.UswFlag=0;}
    if(sys.UswFlag2==1){ReadPing2();sys.UswFlag2=0;}
    //if(sys.KickFlag==1){DriveSolenoid();}
    data.lnRaw = LineRaw;
    data.lnHold = LineHold;
    
    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(data.irNotice==IR_CLOSER){
        ir_pow = sys.s_pow;
    }
    else if(data.irNotice==IR_CLOSE){
        ir_pow = sys.m_pow;
    }
    else if(data.irNotice==IR_FAR){
        ir_pow = sys.l_pow;
    }
    else{//data.irNotice==IR_NONE
        ir_pow = 0;
    }
    if(sys.IrBlind==1) data.irNotice=IR_NONE;
    
    
    
    
    
    ir_x = (ir_x_dir + ir_x_turn);
    ir_y = (ir_y_dir + ir_y_turn);
    
    vx = ir_pow*ir_x;
    vy = ir_pow*ir_y;
    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.IrFlag==1){
        ReadIr();
        sys.IrFlag=0;
    }
    if(sys.PidFlag==1){
        PidUpdate();
        sys.PidFlag=0;
    }
    move(0,0,cmps_set.OutputPID);
    if(sys.MotorFlag==1){
        LED[0] = 1;
        LED[1] = 0;
        tx_motor();
        sys.MotorFlag=0;
    }
    else{
        LED[0] = 0;
        LED[1] = 1;
    }
    if(sys.stopflag==1){
        //停止処理
    }
    return;
}