Diff: main_processing/strategy/LineProcess.cpp

Revision:

25:a7460e23e02e

Child:

27:825c6835e3db

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_processing/strategy/LineProcess.cpp	Thu Mar 17 13:00:24 2016 +0000
@@ -0,0 +1,424 @@
+#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 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;
+    }
+}
+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;
+    }
+    //*/
+    
+}