Diff: main_processing/strategy/strategy.cpp

Revision:

12:bee8f883c33a

Parent:

11:3efae754e6ef

Child:

13:b20921316f3c

--- a/main_processing/strategy/strategy.cpp	Thu Mar 10 08:26:33 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Thu Mar 10 14:08:09 2016 +0000
@@ -836,8 +836,76 @@
         else *y=1;
     }
 }
-void LineJudgeReturn(int8_t *x, int8_t *y);
-void LineJudgeReset(void);
+void LineJudgeReturn(double pow_x, double pow_y, int8_t *x, int8_t *y){
+    const int8_t static LineReturn[5] = {0, 0, 0, 0, 10};
+    const uint8_t static WhiteToWall[2] = {30, 30};
+    uint8_t LinePingState[4];
+    //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
+    //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
+    //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
+    //×ラインのほぼ場外では常時ラインから離れる力が優先される．
+    //※場外判定を行うには再び場内に戻る必要がある．
+    
+    
+    
+    if(data.FieldState==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;
+        //x
+        if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){
+            if(data.ping[L_PING]>data.ping[R_PING]) *x = -LineReturn[4];
+            if(data.ping[L_PING]==data.ping[R_PING]) *x = 0;
+            if(data.ping[L_PING]<data.ping[R_PING]) *x = LineReturn[4];
+        }
+        if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
+            if(pow_x<0){*x = 0;}
+            else{*x = -LineReturn[4];data.lnStop[X_LINE]=0;}
+        }
+        if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
+            if(pow_x>0){*x = 0;}
+            else{*x = -LineReturn[4];data.lnStop[X_LINE]=0;}
+        }
+        if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
+            *x = 0;
+        }
+        
+        //y
+        if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){
+            if(data.ping[L_PING]>data.ping[R_PING]) *x = -LineReturn[4];
+            if(data.ping[L_PING]==data.ping[R_PING]) *x = 0;
+            if(data.ping[L_PING]<data.ping[R_PING]) *x = LineReturn[4];
+        }
+        if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
+            if(pow_x<0){*x = 0;}
+            else{*x = -LineReturn[4];data.lnStop[X_LINE]=0;}
+        }
+        if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
+            if(pow_x>0){*x = 0;}
+            else{*x = -LineReturn[4];data.lnStop[X_LINE]=0;}
+        }
+        if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
+            *x = 0;
+        }
+    }
+    else{//data.FieldState==LINE_INSIDE
+        *x = 0;
+        *y = 0;
+        
+        //line間際の復帰力以外の力を作用させるか否か
+        data.lnStop[X_LINE]=1;
+        data.lnStop[Y_LINE]=1;
+    }
+}
+void LineJudgeReset(void){
+    return;
+}
 void modeAttack4(void){
     double ir_x_dir, ir_y_dir;
     double ir_x_turn, ir_y_turn;