File content as of revision 25:a7460e23e02e:

#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=0;
        sys.DriBlind=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.lnStop[X_LINE]=data.lnStop[Y_LINE]=1;
        data.FieldSpot = LINE_INSIDE;
        LineLiberate();
        LineRankClear();
        //backhome
        sys.HomeStayFlag[X_PING]=0;
        sys.HomeStayFlag[Y_PING]=0;
        //pid
        sys.TurnStopFlag=0;
        cmps_set.AtkDeg=0;
        cmps_set.HoldDeg=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(data.ping[B_PING]<=30){
        sys.ir_pow_table=1;
        ir_x_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_X_DIR];
        ir_y_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_DIR];
        ir_x_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_X_TURN];
        ir_y_turn = ir_move_val[1][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;
    }
    ////ドリブラー
    //ホールド判定
    JudgeBallHolding();
    //ドリブラー駆動
    if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
        sys.DribbleFlag=1;
        if(sys.BallHoldFlag==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();
    }*/
    
    //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)){
        ir_x_turn = -ir_x_turn;
        ir_y_turn = -ir_y_turn;
    }
    
    if((sys.BackHomeFlag==1)&&(sys.HomeBlind==0)||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]>40)&&(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((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){
        if(
            (
                (data.ping[L_PING]<WhiteToWallPlus[X_PING])&&
                (data.ping[R_PING]>WhiteToWallPlus[X_PING])&&
                (ir_x>0)
            )||
            (
                (data.ping[L_PING]>WhiteToWallPlus[X_PING])&&
                (data.ping[R_PING]<WhiteToWallPlus[X_PING])&&
                (ir_x<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]);
    }
    ////LEDデバッグ
    //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
    //if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
    
    //LED = sys.BallHoldFlag*15;
    if(sys.BallHoldFlag==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.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;
}