File content as of revision 0:b910276f9da2:

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

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 LineJudgeSlow2(double pow_x, double pow_y, double *x, double *y, uint8_t *px, uint8_t *py){
    uint8_t LineState;
    uint8_t LineSum;
    
    //line
    LineState = 0;
    
    LineSum = (data.lnRawOrder[0]!=LINE_EMPTY)+(data.lnRawOrder[1]!=LINE_EMPTY)+(data.lnRawOrder[2]!=LINE_EMPTY);
    if(LineSum==3){
        LineState = 3;
    }
    else if(LineSum==2){
        LineState = 2;
    }
    else if(LineSum==1){
        LineState = 1;
    }
    else if(LineSum==0){
        LineState = 0;
    }
    
    if(LineSum>0){
        if(pow_x>=0){
            if((data.ping[R_PING]<WhiteToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
        }
        else{
            if((data.ping[L_PING]<WhiteToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
        }
        
        if(pow_y>=0){
            if((data.ping[F_PING]<WhiteToWall[Y_PING])&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
        }
        else{
            if((data.ping[B_PING]<WhiteToWall[Y_PING])&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
        }
    }
    else if(LineSum==0){
        ////超音波による減速
        if(pow_x>=0){
            if((data.ping[R_PING]<GoalEdgeToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
        }
        else{
            if((data.ping[L_PING]<GoalEdgeToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
        }
        
        if(pow_y>=0){
            //if((data.ping[F_PING]<15)&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
        }
        else{
            if((data.ping[B_PING]<15)&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
        }
    }
    
    if((data.lnStayNow[X_LINE]==1)&&((*px)>15)){
        (*px)=15;
    }
    if((data.lnStayNow[Y_LINE]==1)&&((*py)>15)){
        (*py)=15;
    }
    
    ////超音波による減速
    if(pow_x>=0){
        if((data.ping[R_PING]<GoalEdgeToWall[X_PING])&&((*px)>20)){
            (*px)=20;
            if((*py)>30) (*py)=30;
        }
    }
    else{
        if((data.ping[L_PING]<GoalEdgeToWall[X_PING])&&((*px)>20)){
            (*px)=20;
            if((*py)>30) (*py)=30;
        }
    }
    
    if(pow_y>=0){
        if((data.ping[F_PING]<WhiteToWallPlus[Y_PING])&&((*py)>25)) (*py)=25;
    }
    else{
        if((data.ping[B_PING]<WhiteToWallPlus[Y_PING])&&((*py)>25)) (*py)=25;
    }
    
    *x=1;
    *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)){
                
                if(
                    (data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)&&(data.lnOrder[2]==B_SPOT)
                ){
                    *x = -LineReturn[4];
                    data.lnStop[X_LINE]=0;
                }
                else if(
                    (data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)&&(data.lnOrder[2]==A_SPOT)
                ){
                    *x = LineReturn[4];
                    data.lnStop[X_LINE]=0;
                }
                else{
                    *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)){
                if(
                    (data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT)&&(data.lnOrder[2]==B_SPOT)||
                    (data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)&&(data.lnOrder[2]==A_SPOT)
                ){
                    *y = LineReturn[4];
                    data.lnStop[Y_LINE]=0;
                }
                else if(
                    (data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT)&&(data.lnOrder[2]==C_SPOT)||
                    (data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)&&(data.lnOrder[2]==C_SPOT)
                ){
                    *y = -LineReturn[4];
                    data.lnStop[Y_LINE]=0;
                }
                else{
                    *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;
       /* 
        if(
            (data.lnRawMemory[A_SPOT]==1)&&
            (data.lnRawMemory[B_SPOT]==1)&&
            (data.lnRawMemory[C_SPOT]==0)
        ){
            data.lnStop[Y_LINE]=0;
            *y = -LineReturn[4];
        }
        else if(
            (data.lnRawMemory[A_SPOT]==0)&&
            (data.lnRawMemory[B_SPOT]==0)&&
            (data.lnRawMemory[C_SPOT]==1)
        ){
            data.lnStop[Y_LINE]=0;
            *y = LineReturn[4];
        }*/
    }
}
void LineJudgeReturn2(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];
    //JSO2
    static uint8_t lnRawReturn1 = 0;
    //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
    //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
    //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
    //×ラインのほぼ場外では常時ラインから離れる力が優先される．
    //※場外判定を行うには再び場内に戻る必要がある．
    
    data.FieldSpot=LINE_INSIDE;
    
    if(data.FieldSpot==LINE_OUTSIDE){
    }
    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;
        
        if(//y
            ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==B_SPOT)&&(1))||
            ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==A_SPOT)&&(1))
        ){
            data.lnStop[Y_LINE]=0;
            *y = -LineReturn[4];
            //JSO1
            if(
                (data.ping[F_PING]<45)&&
                (data.lnRawReturn==1)//JSO2
            ){
                data.lnStop[X_LINE]=0;
            }
            //JSO2
            if(
                (
                    ((Line[A_SPOT].read()==1)&&(Line[B_SPOT].read()==0))||
                    ((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0))
                )&&
                (data.ping[F_PING]>45)&&
                (data.ping[F_PING]<200)&&
                (data.ping[R_PING]<25)&&
                (data.ping[L_PING]>40)
            ){
                *x = -LineReturn[4];
                data.lnStop[X_LINE]=0;
            }
            if(
                (
                    ((Line[A_SPOT].read()==0)&&(Line[B_SPOT].read()==1))||
                    ((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1))
                )&&
                (data.ping[F_PING]>45)&&
                (data.ping[F_PING]<200)&&
                (data.ping[L_PING]<25)&&
                (data.ping[R_PING]>40)
            ){
                *x = LineReturn[4];
                data.lnStop[X_LINE]=0;
            }
        }
        else if(
            ((data.lnRawOrder[0]==C_SPOT)&&(1)&&(1))
        ){
            data.lnStop[Y_LINE]=0;
            *y = LineReturn[4];
            
            //JSO1
            if(
                (data.ping[B_PING]<45)&&
                (data.lnRawReturn==1)
            ){
                data.lnStop[X_LINE]=0;
            }
        }
        else if(//x
            ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))
        ){
            *x = -LineReturn[4];
            data.lnStop[X_LINE]=0;
        }
        else if(
            ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))
        ){
            *x = LineReturn[4];
            data.lnStop[X_LINE]=0;
        }
        
        if(
            ((*x)!=0)||
            ((*y)!=0)
        ){
            
            data.lnRawReturn=1;
            
            LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWallPlus[X_PING]);
            LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWallPlus[X_PING]);
            LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWallPlus[Y_PING]);
            LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWallPlus[Y_PING]);
            //x
            if(data.lnStop[X_LINE]==1){
                if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
                    *x = -LineReturn[4];
                    data.lnStop[X_LINE]=0;
                }
                if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==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.lnStop[Y_LINE]==1){
                if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){
                    if(pow_y<0){
                      *y = 0;
                      data.lnStop[Y_LINE]=1;
                    }
                    else{
                      *y = -LineReturn[4];
                      data.lnStop[Y_LINE]=0;
                    }
                }
                //JSO2
                if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)&&(data.lnRawReturn==1)){
                    *y = -LineReturn[4];
                    data.lnStop[Y_LINE]=0;
                }
                
                if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){
                    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;
                }
            }
        }
        else{
            data.lnRawReturn=0;
            
            //JSO3
            if((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==LINE_EMPTY)&&(pow_x>0)){
                data.lnStop[X_LINE]=0;
            }
            if((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==LINE_EMPTY)&&(pow_x<0)){
                data.lnStop[X_LINE]=0;
            }
            //JSO3
            if((Line[C_SPOT].read()==1)&&(1)&&(pow_y>0)){
                data.lnStop[Y_LINE]=0;
            }
        }
        //JSO3
        if((Line[C_SPOT].read()==1)&&(1)&&(pow_y>0)){
            data.lnStop[Y_LINE]=0;
        }
        //JSO1
        if(data.lnRawReturn==1){
            //JSO2
            if((lnRawReturn1==0)){
                LineLiberate();
                lnRawReturn1=1;
            }
            
            if(
                (
                    ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))||
                    ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))
                )&&
                //((Line[A_SPOT].read()==1)&&(Line[B_SPOT].read()==1))&&//JSO1
                ((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1))&&//JSO2
                ((data.ping[F_PING]<45)&&(1))&&//JSO2
                //(!((data.ping[B_PING]<40)&&(data.ping[B_PING]>25)))&&//JSO2
                ((data.lnRawOrder[0]!=C_SPOT)&&(data.lnRawOrderLog1[0]!=C_SPOT)&&(data.lnRawOrderLog2[0]!=C_SPOT))
                
            ){
                *y = -LineReturn[4];
                data.lnStop[Y_LINE]=0;//JSO2
            }
        }
        else{
            lnRawReturn1=0;
            
            
            if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
                *x = -LineReturn[4];
                data.lnStop[X_LINE]=0;
            }
            if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
                *x = LineReturn[4];
                data.lnStop[X_LINE]=0;
            }
            
        }
        
    }
}
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};
    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;
        
        NewLineCorner[L_LINE]=LINE_EMPTY;
        NewLineCorner[R_LINE]=LINE_EMPTY;
        NewLineCorner[F_LINE]=LINE_EMPTY;
        NewLineCorner[B_LINE]=LINE_EMPTY;
        
        LastLineCorner[L_LINE]=LINE_EMPTY;
        LastLineCorner[R_LINE]=LINE_EMPTY;
        LastLineCorner[F_LINE]=LINE_EMPTY;
        LastLineCorner[B_LINE]=LINE_EMPTY;
    }
    //data.lnRepeat=0;
    if((data.lnRepeat>0)&&(data.FieldSpot == LINE_INSIDE)){
        //x
        if(
            ((pow_x>=0)&&(NewLineCorner[R_LINE]))||
            ((pow_x<0)&&(NewLineCorner[L_LINE]))
        ){
            data.lnStay[X_LINE]=0;
        }
        else{
            data.lnStay[X_LINE]=1;
        }
        //y
        if(
            ((pow_y>=0)&&(NewLineCorner[F_LINE]))||
            ((pow_y<0)&&(NewLineCorner[B_LINE]))
        ){
            data.lnStay[Y_LINE]=0;
        }
        else{
            data.lnStay[Y_LINE]=1;
        }
    }
    else{
        data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
    }
    //*/
    
}
void LineJudgeReset2(double pow_x, double pow_y, double *x, double *y){
    
    if(data.lnRepeat==0){
        data.lnStayNow[X_LINE]=0;
        data.lnStayNow[Y_LINE]=0;
    }
    data.lnStay[X_LINE]=1;
    data.lnStay[Y_LINE]=1;
    
    if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){
        data.lnRepeat=1;
    }
    
    if(
        (data.irNotice==IR_NONE)||
        //(data.irNotice==IR_FAR)||
        //(data.irLastPosition<=7)||
        (
            (!(
                (data.irPosition==data.irLastPosition)||
                (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))||
                (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12]))||
                (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))||
                (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12]))
                //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))&&(cmps_set.GoalDeg==0))||
                //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0))
            ))&&(
                (data.irLastNotice==IR_CLOSE)||
                (data.irLastNotice==IR_CLOSER)
            )
        )
    ){
        data.lnRepeat = 0;
        
        LineRawLogReset();
    }
    //data.lnRepeat=0;
    //if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){
    if(
        (data.lnRepeat>=1)&&
        (data.lnRawOrder[0]==LINE_EMPTY)/*&&
        (
            (
                (data.irPosition==data.irLastPosition)||
                (data.irPosition==(data.irLastPosition+1))||
                (data.irPosition==(data.irLastPosition-1))||
                (data.irPosition==(data.irLastPosition+11))||
                (data.irPosition==(data.irLastPosition-11))
            )&&
            (
                (data.irNotice==IR_CLOSE)||
                (data.irNotice==IR_CLOSER)
            )
        )*/
    ){
        //y
        if(
            ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))||
            ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1)))
        ){
            if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
                data.lnStay[Y_LINE]=0;
            }
            else{
                data.lnStay[Y_LINE]=1;
            }
            data.lnStay[Y_LINE]=0;
            data.lnStayNow[Y_LINE]=1;
        }
        else{
            data.lnStay[Y_LINE]=1;
        }
        if(
            ((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1)))
        ){
            if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
                data.lnStay[Y_LINE]=0;
            }
            else{
                data.lnStay[Y_LINE]=1;
            }
            data.lnStay[Y_LINE]=0;
            data.lnStayNow[Y_LINE]=1;
        }
        else{
            data.lnStay[Y_LINE]=1;
        }
        //x
        if(
            ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
        ){
            if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
                data.lnStay[X_LINE]=0;
            }
            else{
                data.lnStay[X_LINE]=1;
            }
            data.lnStay[X_LINE]=0;
            data.lnStayNow[X_LINE]=1;
        }
        else{
            data.lnStay[X_LINE]=1;
        }
        if(
            ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
        ){
            if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
                data.lnStay[X_LINE]=0;
            }
            else{
                data.lnStay[X_LINE]=1;
            }
            data.lnStay[X_LINE]=0;
            data.lnStayNow[X_LINE]=1;
        }
        else{
            data.lnStay[X_LINE]=1;
        }
        if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){
            //y2
            if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
                if( 
                    ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
                    ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
                    ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1                             ))&&((data.irPosition==18)||(data.irPosition==19)))
                ){
                    if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
                        data.lnStay[X_LINE]=0;
                    }
                    else{
                        data.lnStay[X_LINE]=1;
                    }
                    if(data.lnRepeat>=LINE_REPEAT){
                        data.lnStay[X_LINE]=0;
                    }
                    data.lnStayNow[X_LINE]=1;
                }
                else{
                    data.lnStay[X_LINE]=1;
                }
            }
            if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
                if( 
                    ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
                    ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
                    ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1                             ))&&((data.irPosition==15)||(data.irPosition==16)))
                ){
                    if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
                        data.lnStay[X_LINE]=0;
                    }
                    else{
                        data.lnStay[X_LINE]=1;
                    }
                    if(data.lnRepeat>=LINE_REPEAT){
                        data.lnStay[X_LINE]=0;
                    }
                    data.lnStayNow[X_LINE]=1;
                }
                else{
                    data.lnStay[X_LINE]=1;
                }
            }
            //x2
            if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){
                if(
                    ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
                    ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))
                ){
                    if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
                        data.lnStay[Y_LINE]=0;
                    }
                    else{
                        data.lnStay[Y_LINE]=1;
                    }
                    if(data.lnRepeat>=LINE_REPEAT){
                        data.lnStay[Y_LINE]=0;
                    }
                    data.lnStayNow[Y_LINE]=1;
                }
                else{
                    data.lnStay[Y_LINE]=1;
                }
                
                if(
                    ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))||
                    ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16)))
                ){
                    if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
                        data.lnStay[Y_LINE]=0;
                    }
                    else{
                        data.lnStay[Y_LINE]=1;
                    }
                    if(data.lnRepeat>=LINE_REPEAT){
                        data.lnStay[Y_LINE]=0;
                    }
                    data.lnStayNow[Y_LINE]=1;
                }
                else{
                    data.lnStay[Y_LINE]=1;
                }
            }
            if(
                (data.lnRepeat>=2)
            ){
                if(
                    ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))||
                    ((pow_y<0)&&((data.ping[B_PING]<40)&&(1)))
                ){
                    data.lnStay[Y_LINE]=0;
                }
                if(
                    ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))||
                    ((pow_x<0)&&((data.ping[L_PING]<40)&&(1)))
                ){
                    data.lnStay[X_LINE]=0;
                }
            }
        }
        
        
    }
    else{
        data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
    }
    
}
void LineJudgeReset3(double pow_x, double pow_y, double *x, double *y){
    
    if(data.lnRepeat==0){
        data.lnStayNow[X_LINE]=0;
        data.lnStayNow[Y_LINE]=0;
    }
    data.lnStay[X_LINE]=1;
    data.lnStay[Y_LINE]=1;
    
    if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){
        data.lnRepeat=1;
    }
    
    if(
        (data.irNotice==IR_NONE)||
        (data.irNotice==IR_FAR)||
        //(data.irLastPosition<=7)||
        (
            (!(
                (data.irPosition==data.irLastPosition)||
                (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))||
                (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12]))
                //(data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))||
                //(data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12]))
                //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))&&(cmps_set.GoalDeg==0))||
                //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0))
            ))&&(
                (data.irLastNotice==IR_CLOSE)||
                (data.irLastNotice==IR_CLOSER)
            )
        )
    ){
        data.lnRepeat = 0;
        
        LineRawLogReset();
    }
    //data.lnRepeat=0;
    //if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){
    if(
        (data.lnRepeat>=1)&&
        (data.lnRawOrder[0]==LINE_EMPTY)/*&&
        (
            (
                (data.irPosition==data.irLastPosition)||
                (data.irPosition==(data.irLastPosition+1))||
                (data.irPosition==(data.irLastPosition-1))||
                (data.irPosition==(data.irLastPosition+11))||
                (data.irPosition==(data.irLastPosition-11))
            )&&
            (
                (data.irNotice==IR_CLOSE)||
                (data.irNotice==IR_CLOSER)
            )
        )*/
    ){
        //y
        if(
            ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))||
            ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1)))
        ){
            if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
                data.lnStay[Y_LINE]=0;
            }
            else{
                data.lnStay[Y_LINE]=1;
            }
            data.lnStay[Y_LINE]=0;
            data.lnStayNow[Y_LINE]=1;
        }
        else{
            data.lnStay[Y_LINE]=1;
        }
        if(
            ((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1)))
        ){
            if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
                data.lnStay[Y_LINE]=0;
            }
            else{
                data.lnStay[Y_LINE]=1;
            }
            data.lnStay[Y_LINE]=0;
            data.lnStayNow[Y_LINE]=1;
        }
        else{
            data.lnStay[Y_LINE]=1;
        }
        //x
        if(
            ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
        ){
            if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
                data.lnStay[X_LINE]=0;
            }
            else{
                data.lnStay[X_LINE]=1;
            }
            data.lnStay[X_LINE]=0;
            data.lnStayNow[X_LINE]=1;
        }
        else{
            data.lnStay[X_LINE]=1;
        }
        if(
            ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
        ){
            if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
                data.lnStay[X_LINE]=0;
            }
            else{
                data.lnStay[X_LINE]=1;
            }
            data.lnStay[X_LINE]=0;
            data.lnStayNow[X_LINE]=1;
        }
        else{
            data.lnStay[X_LINE]=1;
        }
        if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){
            //y2
            if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
                if( 
                    ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
                    ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
                    ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1                             ))&&((data.irPosition==18)||(data.irPosition==19)))
                ){
                    if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
                        data.lnStay[X_LINE]=0;
                    }
                    else{
                        data.lnStay[X_LINE]=1;
                    }
                    if(data.lnRepeat>=LINE_REPEAT){
                        data.lnStay[X_LINE]=0;
                    }
                    data.lnStayNow[X_LINE]=1;
                }
                else{
                    data.lnStay[X_LINE]=1;
                }
            }
            if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
                if( 
                    ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
                    ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
                    ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1                             ))&&((data.irPosition==15)||(data.irPosition==16)))
                ){
                    if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
                        data.lnStay[X_LINE]=0;
                    }
                    else{
                        data.lnStay[X_LINE]=1;
                    }
                    if(data.lnRepeat>=LINE_REPEAT){
                        data.lnStay[X_LINE]=0;
                    }
                    data.lnStayNow[X_LINE]=1;
                }
                else{
                    data.lnStay[X_LINE]=1;
                }
            }
            //x2
            if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){
                if(
                    ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
                    ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))
                ){
                    if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
                        data.lnStay[Y_LINE]=0;
                    }
                    else{
                        data.lnStay[Y_LINE]=1;
                    }
                    if(data.lnRepeat>=LINE_REPEAT){
                        data.lnStay[Y_LINE]=0;
                    }
                    data.lnStayNow[Y_LINE]=1;
                }
                else{
                    data.lnStay[Y_LINE]=1;
                }
                
                if(
                    ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))||
                    ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16)))
                ){
                    if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
                        data.lnStay[Y_LINE]=0;
                    }
                    else{
                        data.lnStay[Y_LINE]=1;
                    }
                    if(data.lnRepeat>=LINE_REPEAT){
                        data.lnStay[Y_LINE]=0;
                    }
                    data.lnStayNow[Y_LINE]=1;
                }
                else{
                    data.lnStay[Y_LINE]=1;
                }
            }
            if(
                (data.lnRepeat>=2)
            ){
                if(
                    ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))||
                    ((pow_y<0)&&((data.ping[B_PING]<40)&&(1)))
                ){
                    data.lnStay[Y_LINE]=0;
                }
                if(
                    ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))||
                    ((pow_x<0)&&((data.ping[L_PING]<40)&&(1)))
                ){
                    data.lnStay[X_LINE]=0;
                }
            }
        }
        
        
    }
    else{
        data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
    }
    
}
void JudgeInSide(void){
    data.FieldSpot = LINE_INSIDE;
    data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
    LineLiberate();
    LineRankClear();
    //Line_home.detach();
}