File content as of revision 20:d69ce8f47f1d:

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

//Atk

void LineJudgeSlow(double pow_x, double pow_y, double *x, double *y){
    uint8_t LineState;
    uint8_t LineSum;
    
    //line
    LineState = 0;
    
    LineSum = ((data.lnHold>>2)&0x1)+((data.lnHold>>1)&0x1)+((data.lnHold>>0)&0x1);
    if((LineSum==3)||(data.FieldSpot==LINE_OUTSIDE)){
        LineState = 3;
    }
    else if(LineSum==2){
        LineState = 2;
    }
    else if(LineSum==1){
        LineState = 1;
    }
    else if(LineSum==0){
        LineState = 0;
    }
    
    
    if(pow_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(pow_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, double *x, double *y){
    const int8_t static LineReturn[5] = {0, 0, 0, 0, 20};
    uint8_t LinePingState[4];
    //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
    //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
    //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
    //×ラインのほぼ場外では常時ラインから離れる力が優先される．
    //※場外判定を行うには再び場内に戻る必要がある．
    
    if(data.FieldSpot==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;
        
        data.ReturnDir[X_LINE]=LINE_EMPTY;
        data.ReturnDir[Y_LINE]=LINE_EMPTY;
        
        //x
        if(data.ReturnDir[X_LINE]==L_LINE){
            if(pow_x<0){
              *x = 0;
              data.lnStop[X_LINE]=1;
            }
            else{
              *x = -LineReturn[4];
              data.lnStop[X_LINE]=0;
            }
        }
        else if(data.ReturnDir[X_LINE]==R_LINE){
            if(pow_x>0){
              *x = 0;
              data.lnStop[X_LINE]=1;
            }
            else{
              *x = LineReturn[4];
              data.lnStop[X_LINE]=0;
            }
        }
        else if(data.ReturnDir[X_LINE]==LINE_EMPTY){
            if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){
                /*if(pow_x>0){
                  *x = -pow_x*(1-0.75);
                }
                else{
                  *x = pow_x*(1-0.75);
                }*/
                data.lnStop[X_LINE]=1;
            }
            if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
                
                data.ReturnDir[X_LINE]=L_LINE;
                
                /*if(pow_x<0){
                  *x = 0;
                  data.lnStop[X_LINE]=1;
                }
                else{
                  *x = -LineReturn[4];
                  data.lnStop[X_LINE]=0;
                }*/
                *x = -LineReturn[4];
                data.lnStop[X_LINE]=0;
            }
            if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
                
                data.ReturnDir[X_LINE]=R_LINE;
                
                /*if(pow_x>0){
                  *x = 0;
                  data.lnStop[X_LINE]=1;
                }
                else{
                  *x = LineReturn[4];
                  data.lnStop[X_LINE]=0;
                }*/
                *x = LineReturn[4];
                data.lnStop[X_LINE]=0;
            }
            if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
                *x = 0;
                data.lnStop[X_LINE]=0;
            }
        }
        //y
        if(data.ReturnDir[Y_LINE]==F_LINE){
            if(pow_y>0){
              *y = 0;
              data.lnStop[Y_LINE]=1;
            }
            else{
              *y = LineReturn[4];
              data.lnStop[Y_LINE]=0;
            }
        }
        else if(data.ReturnDir[Y_LINE]==B_LINE){
            if(pow_y<0){
              *y = 0;
              data.lnStop[Y_LINE]=1;
            }
            else{
              *y = -LineReturn[4];
              data.lnStop[Y_LINE]=0;
            }
        }
        else if(data.ReturnDir[Y_LINE]==LINE_EMPTY){
            if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==0)){
                /*if(pow_y>0){
                  *y = -pow_y*(1-0.75);
                }
                else{
                  *y = pow_y*(1-0.75);
                }*/
                data.lnStop[Y_LINE]=1;
            }
            if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){
                
                data.ReturnDir[Y_LINE]=B_LINE;
                
                if(pow_y<0){
                  *y = 0;
                  data.lnStop[Y_LINE]=1;
                }
                else{
                  *y = -LineReturn[4];
                  data.lnStop[Y_LINE]=0;
                }
            }
            if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){
                
                data.ReturnDir[Y_LINE]=F_LINE;
                
                if(pow_y>0){
                  *y = 0;
                  data.lnStop[Y_LINE]=1;
                }
                else{
                  *y = LineReturn[4];
                  data.lnStop[Y_LINE]=0;
                }
            }
            if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){
                *y = 0;
                data.lnStop[Y_LINE]=0;
            }
        }
        //none
        if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)&&(LinePingState[F_PING]==1)&&(LinePingState[B_PING]==1)){
            //turn!
        }
        
    }
    else{//data.FieldSpot==LINE_INSIDE
    
        data.ReturnDir[X_LINE]=LINE_EMPTY;
        data.ReturnDir[Y_LINE]=LINE_EMPTY;
        
        *x = 0;
        *y = 0;
        
        //line間際の復帰力以外の力を作用させるか否か
        data.lnStop[X_LINE]=1;
        data.lnStop[Y_LINE]=1;
    }
}
void LineJudgeReset(double pow_x, double pow_y, double *x, double *y){
    //static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
    //static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
    
    if((/*data.lnRaw==0*/1)&&(data.lnHold==7)){
        if(data.FieldSpot==LINE_INSIDE){
            
            data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]);
            data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]);
            data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]);
            data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]);
            if(
                (data.lnCorner[L_LINE])||
                (data.lnCorner[R_LINE])||
                (data.lnCorner[F_LINE])||
                (data.lnCorner[B_LINE])
            ){
                
                data.NonWall[L_LINE] = (data.ping[L_PING]>WhiteToWall[X_PING]);
                data.NonWall[R_LINE] = (data.ping[R_PING]>WhiteToWall[X_PING]);
                data.NonWall[F_LINE] = (data.ping[F_PING]>WhiteToWall[Y_PING]);
                data.NonWall[B_LINE] = (data.ping[B_PING]>WhiteToWall[Y_PING]);
                
                
                
                /*LastLineCorner[L_LINE]=NewLineCorner[L_LINE];
                LastLineCorner[R_LINE]=NewLineCorner[R_LINE];
                LastLineCorner[F_LINE]=NewLineCorner[F_LINE];
                LastLineCorner[B_LINE]=NewLineCorner[B_LINE];
                
                NewLineCorner[L_LINE]=data.lnCorner[L_LINE];
                NewLineCorner[R_LINE]=data.lnCorner[R_LINE];
                NewLineCorner[F_LINE]=data.lnCorner[F_LINE];
                NewLineCorner[B_LINE]=data.lnCorner[B_LINE];
                
                if(
                    (LastLineCorner[L_LINE]==NewLineCorner[L_LINE])&&
                    (LastLineCorner[R_LINE]==NewLineCorner[R_LINE])&&
                    (LastLineCorner[F_LINE]==NewLineCorner[F_LINE])&&
                    (LastLineCorner[B_LINE]==NewLineCorner[B_LINE])
                ){
                    data.lnRepeat++;
                }
                else{
                    data.lnRepeat=0;
                }*/
                
                data.FieldSpot = LINE_OUTSIDE;
                LineLiberate();
            }
        }
        else if(data.FieldSpot==LINE_OUTSIDE){
            if(data.lnRaw==0){
                data.FieldSpot = LINE_INSIDE;
                data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
                LineLiberate();
                LineRankClear();
            }
        }
    }
    if(data.FieldSpot == LINE_OUTSIDE){
        if(
            (
                (data.ping[L_PING]>=WhiteToWall[X_PING])||
                (data.NonWall[L_LINE]==1)||
                ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
                //(data.lnOrder[0]==A_SPOT)
            )&&
            (
                (data.ping[R_PING]>=WhiteToWall[X_PING])||
                (data.NonWall[R_LINE]==1)||
                ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
                //(data.lnOrder[0]==B_SPOT)
            )&&
            (
                (data.ping[F_PING]>=WhiteToWall[Y_PING])||
                (data.NonWall[F_LINE]==1)||
                ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
                ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
            )&&
                ((data.ping[B_PING]>=WhiteToWall[Y_PING])||
                (data.NonWall[B_LINE]==1)||
                ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
                ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
            )
        ){
            data.FieldSpot = LINE_INSIDE;
            data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
            LineLiberate();
            LineRankClear();
        }
    }
    if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){
        if(
            (
                (data.ping[L_PING]>=WhiteToWall[X_PING])||
                ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
                //(data.lnOrder[0]==A_SPOT)
            )&&
            (
                (data.ping[R_PING]>=WhiteToWall[X_PING])||
                ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
                //(data.lnOrder[0]==B_SPOT)
            )&&
            (
                (data.ping[F_PING]>=WhiteToWall[Y_PING])||
                ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
                ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
            )&&
                ((data.ping[B_PING]>=WhiteToWall[Y_PING])||
                ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
                ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
            )
            /*||
            (
                (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
                (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
            )*/
        ){
            data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
            LineLiberate();
        }
    }
    /*
    if(
        (data.irNotice==IR_NONE)||
        (data.irNotice==IR_FAR)||
        (
            (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
            (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
        )
    ){
        data.lnRepeat = 0;
        
        data.lnCorner[L_LINE]=LINE_EMPTY;
        data.lnCorner[R_LINE]=LINE_EMPTY;
        data.lnCorner[F_LINE]=LINE_EMPTY;
        data.lnCorner[B_LINE]=LINE_EMPTY;
    }
    data.lnRepeat=0;
    if(data.lnRepeat>0){
        if(
            ((pow_x>=0)&&(data.lnCorner[R_LINE]))||
            ((pow_x<0)&&(data.lnCorner[L_LINE]))
        ){
            data.lnStay[X_LINE]=0;
        }
        else{
            data.lnStay[X_LINE]=1;
        }
        if(
            ((pow_y>=0)&&(data.lnCorner[F_LINE]))||
            ((pow_y<0)&&(data.lnCorner[B_LINE]))
        ){
            data.lnStay[Y_LINE]=0;
        }
        else{
            data.lnStay[Y_LINE]=1;
        }
    }
    else{
        data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
    }*/
    
}
void modeAttack4(void){
    double ir_x_dir, ir_y_dir;
    double ir_x_turn, ir_y_turn;
    double ir_x, ir_y;
    
    double LineSlowPower[2];
    double LineReturnPower[2];
    
    //double pow_x, pow_y;
    uint8_t ir_pow;
    int vx,vy,vs;
    //static uint8_t data.FieldSpot;
    
    
    if(sys.KickOffFlag==1){
        
        sys.IrBlind=0;
        sys.LineBlind=0;
        sys.PingBlind=0;
        
        sys.HomeBlind=1;
        sys.DriBlind=0;
        
        
        sys.ir_pow_table = 0;
        
        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();
        
        cmps_set.AtkDeg=0;
        
        //sys.BallHoldJudgeFlag=0;
        //sys.BallHoldFlag=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.InfoFlag==1){ReadInfo();sys.InfoFlag=0;}
    
    //if(sys.KickFlag==1){DriveSolenoid();}
    data.lnRaw = LineRaw;
    data.lnHold = LineHold;
    data.ball = (BallCheckerA.read_u16()<20000);
    
    /*
    if(sys.DriBlind==0){
        if((data.ball==1)&&(sys.BallHoldJudgeFlag==0)){
            sys.BallHoldJudgeFlag=1;
            Ball_catch.attach(&AdmitBallHold, 1.0);
        }
        
        if((data.ball==0)&&(sys.BallHoldFlag==1)&&(sys.BallHoldGiveUpFlag==0)){
            sys.BallHoldGiveUpFlag=1;
            Ball_leave.attach(&GiveUpBallHold, 1.0);
        }
        if((data.ball==1)&&(sys.BallHoldFlag==1)){
            sys.BallHoldGiveUpFlag=0;
            Ball_leave.detach();
        }
        
        if((data.ping[B_PING]<30)&&(sys.BallHoldFlag==1)){
            cmps_set.AtkDeg = 150;
            Kick_now.attach(&DriveSolenoid, .125);
            Front_now.attach(&FaceToFront, .75);
            
            sys.BallHoldJudgeFlag=0;
            sys.BallHoldFlag=0;
            Ball_catch.detach();
            Ball_leave.detach();
        }
    }
    else{
        sys.BallHoldFlag=0;
    }*/
    
    if(data.irNotice==IR_NONE){
        sys.BackHomeFlag=(sys.HomeBlind==0);
    }
    else{
        sys.BackHomeFlag=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_x_dir = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_X_DIR];
    ir_y_dir = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_Y_DIR];
    ir_x_turn = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_X_TURN];
    ir_y_turn = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_Y_TURN];
    */
    /*
    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.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
        sys.DribbleFlag=1;
        ir_pow=20;
        /*if(cmps_set.AtkDeg==0){
            if(data.irPosition==10) cmps_set.AtkDeg=30;
            if(data.irPosition==11) cmps_set.AtkDeg=0;
            if(data.irPosition==12) cmps_set.AtkDeg=-30;
        }
        else if(cmps_set.AtkDeg>0){
            if(data.irPosition==10) cmps_set.AtkDeg=0;
            if(data.irPosition==11) cmps_set.AtkDeg=30;
            if(data.irPosition==12) cmps_set.AtkDeg=0;
            
        }
        else if(cmps_set.AtkDeg<0){
            if(data.irPosition==10) cmps_set.AtkDeg=0;
            if(data.irPosition==11) cmps_set.AtkDeg=-30;
            if(data.irPosition==12) cmps_set.AtkDeg=0;
        }*/
    }
    else{
        sys.DribbleFlag=0;
        cmps_set.AtkDeg=0;
        
        /*sys.BallHoldJudgeFlag=0;
        sys.BallHoldFlag=0;
        Ball_catch.detach();
        Ball_leave.detach();*/
    }
    
    if(sys.IrBlind==1) ir_pow=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;
    }
    
    ir_x = (ir_x_dir + ir_x_turn);
    ir_y = (ir_y_dir + ir_y_turn);
    
    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]);
        /*
        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;
        */
        
    }
    //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
    //if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
    
    //LED = 0xFF*sys.BallHoldFlag;
    
    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*ir_x)*data.lnStop[X_LINE]*(/*data.lnStay[X_LINE]*/1)*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
    vy = (ir_pow*ir_y)*data.lnStop[Y_LINE]*(/*data.lnStay[X_LINE]*/1)*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;
}