File content as of revision 31:745a775cfc20:

#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();
        
        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){
        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];
                }
            }
        }
    }
    
    //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;
        }
    }
    ////ドリブラー
    //ホールド判定
    JudgeBallHolding();
    //ドリブラー駆動
    if((sys.DriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){
        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.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;
        }
        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)){
        ir_pow_x = ir_pow_y = 25;
        if(ir_y>0) ir_y=0;
        if((data.ping[B_PING]>60)&&(1)){
            ir_y = -1;
        }
    }
    if(
        (sys.DefenceFlag==1)&&
        (ir_y>0)&&
        (data.ping[B_PING]>60)&&
        (data.ping[B_PING]<225)&&
        (sys.BackHomeFlag==0)
    ){
        ir_pow_y=0;
    }
    ////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]);
        */
        LineJudgeReset2(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==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] + LineReturnPower[X_LINE];
    vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE] + 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=1;
        //defence
        sys.DefenceFlag=1;
        
        
        //初期値設定の終了
        //sys.KickOffFlag=0;
    }
    modeAttack4();
}