Diff: main_processing/strategy/strategy.cpp

Revision:

14:b510adcb6065

Parent:

13:b20921316f3c

Child:

15:17502a27a60b

--- a/main_processing/strategy/strategy.cpp	Fri Mar 11 00:39:21 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Fri Mar 11 04:17:00 2016 +0000
@@ -728,61 +728,7 @@
     }
     return;
 }
-double LineJudgeX(double x){
-    uint8_t LineState;
-    uint8_t LnRaw;
-    uint8_t LnHold;
-    
-    LnRaw = LineRaw;
-    LnHold = LineHold;
-    
-    //line
-    LnRaw = LineRaw;
-    LnHold = LineHold;
-    /*
-    if((LineRaw==0)){
-        if((data.ping[L_PING]>60)&&(data.ping[R_PING]>60)){
-            LineLiberate();
-        }
-    }*/
-    
-    
-    
-    if(LnHold==7){
-        if(LnRaw>0){
-            //場外間際
-            LineState = 3;
-        }
-        else{//LnRaw==0
-            //powerVoid & restore
-            //場外
-            LineState = 4;
-        }
-    }
-    else if(LnHold>0){
-        if(LnRaw>0){
-            //踏んでるけどまだ出てない
-            LineState = 4;
-        }
-        else{//LnRaw==0
-            //線をまたいでいるか，中途半端に線を踏んだあと復帰したか
-            LineState = 4;
-            //ping&reset
-            //timeout&reset
-        }
-    }
-    //else if(LnHold==0){...maxpower
-    
-    if(x>=0){
-        if(data.ping[R_PING]<WhiteToWall[X_PING]) return LineDecline[LineState];
-        else return 1;
-    }
-    else{
-        if(data.ping[L_PING]<WhiteToWall[X_PING]) return LineDecline[LineState];
-        else return 1;
-    }
-}
-void LineJudgeSlow(double *x, double *y){
+void LineJudgeSlow(double pow_x, double pow_y, double *x, double *y){
     uint8_t LineState;
     
     //line
@@ -807,7 +753,6 @@
         else{//data.lnRaw==0
             //線をまたいでいるか，中途半端に線を踏んだあと復帰したか...減衰
             LineState = 1;
-            //if((data.ping[L_PING]>40)&&(data.ping[R_PING]>40)) LineLiberate();
             //ping&reset
             //timeout&reset
         }
@@ -818,7 +763,7 @@
     }
     
     
-    if(*x>=0){
+    if(pow_x>=0){
         if(data.ping[R_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
         else *x=1;
     }
@@ -827,7 +772,7 @@
         else *x=1;
     }
     
-    if(*y>=0){
+    if(pow_y>=0){
         if(data.ping[F_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
         else *y=1;
     }
@@ -937,8 +882,35 @@
     }
 }
 void LineJudgeReset(void){
-    uint8_t LineState;
+    //uint8_t LineCorner[4];
     
+    if((data.lnRaw==0)&&(data.lnHold==7)){
+        if(data.FieldState==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])
+            ){
+                LineLiberate();
+                data.FieldState=LINE_OUTSIDE;
+            }
+        }
+        else{//data.FieldState==LINE_OUTSIDE
+            if(
+                (data.ping[L_PING]>=WhiteToWall[X_PING])&&
+                (data.ping[R_PING]>=WhiteToWall[X_PING])
+            ){
+                LineLiberate();
+                data.FieldState=LINE_OUTSIDE;
+            }
+        }
+    }
+    /*
     //line
     if(data.lnHold==7){
         if(data.lnRaw>0){
@@ -946,57 +918,66 @@
         }
         else{//data.lnRaw==0
             //場外...出力完全無効+反発
+            
+            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.ping[L_PING]<OutToWall[X_PING])||
-                (data.ping[R_PING]<OutToWall[X_PING])||
-                (data.ping[F_PING]<OutToWall[Y_PING])||
-                (data.ping[B_PING]<OutToWall[Y_PING])
+                (data.lnCorner[L_LINE])||
+                (data.lnCorner[R_LINE])||
+                (data.lnCorner[F_LINE])||
+                (data.lnCorner[B_LINE])
             ){
-                LineLiberate();
+                //LineLiberate();
                 data.FieldState=LINE_OUTSIDE;
             }
             if(
                 (data.ping[L_PING]>=WhiteToWall[X_PING])&&
                 (data.ping[R_PING]>=WhiteToWall[X_PING])
             ){
-                LineLiberate();
-                data.FieldState=LINE_INSIDE;
+                //LineLiberate();
+                //data.FieldState=LINE_INSIDE;
             }
+            //LineLiberate();
+            //data.FieldState=!data.FieldState;
         }
     }
     else if(data.lnHold>0){
         if(data.lnRaw>0){
             //踏んでるけどまだ出てない...減衰
-            //timeout&reset...detach
         }
         else{//data.lnRaw==0
             //線をまたいでいるか，中途半端に線を踏んだあと復帰したか...減衰
-            //if((data.ping[L_PING]>40)&&(data.ping[R_PING]>40)) LineLiberate();
-            //ping&reset
-            //timeout&reset
+            //if((data.ping[L_PING]>WhiteToWall[X_PING])&&(data.ping[R_PING]>WhiteToWall[X_PING])) LineLiberate();
         }
     }
     //else if(data.lnHold==0){...maxpower
-    
-    if((data.ping[R_PING]>WhiteToWall[X_PING])&&(data.ping[L_PING]>WhiteToWall[X_PING])) LineLiberate();
+    */
 }
 void modeAttack4(void){
     double ir_x_dir, ir_y_dir;
     double ir_x_turn, ir_y_turn;
     double ir_x, ir_y;
     
-    double pow_x, pow_y;
+    double LineSlowPower[2];
+    double LineReturnPower[2];
+    
+    //double pow_x, pow_y;
     uint8_t ir_pow;
     int vx,vy,vs;
+    //static uint8_t data.kkk;
     
     
     if(sys.KickOffFlag==1){
         
-        sys.IrBlind=0;
+        sys.IrBlind=1;
         sys.LineBlind=0;
         sys.PingBlind=0;
         
-        data.FieldState=LINE_INSIDE;
+        data.FieldState=LINE_OUTSIDE;
+        data.kkk = LINE_INSIDE;
+        LineLiberate();
         
         sys.KickOffFlag=0;
     }
@@ -1027,15 +1008,80 @@
     }
     if(sys.IrBlind==1) data.irNotice=IR_NONE;
     
-    
-    
-    
-    
     ir_x = (ir_x_dir + ir_x_turn);
     ir_y = (ir_y_dir + ir_y_turn);
     
-    vx = ir_pow*ir_x;
-    vy = ir_pow*ir_y;
+    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.FieldState = LINE_INSIDE;
+    }
+    else{
+        //LineJudgeReset();
+        if((data.lnRaw==0)&&(data.lnHold==7)){
+            if(data.kkk==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])
+                ){
+                    //LineLiberate();
+                    data.FieldState=LINE_OUTSIDE;
+                }
+                data.FieldState=LINE_OUTSIDE;
+                data.kkk = LINE_OUTSIDE;
+                LineLiberate();
+            }
+            else if(data.kkk==LINE_OUTSIDE){//data.FieldState==LINE_OUTSIDE
+                if(
+                    (data.ping[L_PING]>=WhiteToWall[X_PING])&&
+                    (data.ping[R_PING]>=WhiteToWall[X_PING])
+                ){
+                    //LineLiberate();
+                    data.FieldState=LINE_OUTSIDE;
+                }
+                //data.FieldState=LINE_OUTSIDE;
+                data.kkk = LINE_INSIDE;
+                LineLiberate();
+            }
+        }
+        //LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
+        //LineJudgeReturn(ir_x, ir_y, &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.kkk==LINE_OUTSIDE) LED = 0x9;
+    if(data.kkk==LINE_INSIDE) LED = 0x6;
+    
+    
+    //if(data.FieldState==LINE_OUTSIDE) LED = 0x9;
+    //if(data.FieldState==LINE_INSIDE) LED = 0x6;
+    //else LED = 0xA;
+    //LED = LineHold;
+    
+    vx = (ir_pow*ir_x)*data.lnStop[X_LINE]*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
+    vy = (ir_pow*ir_y)*data.lnStop[Y_LINE]*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
     vs = cmps_set.OutputPID;
     move(
         vx,
@@ -1060,14 +1106,14 @@
     }
     move(0,0,cmps_set.OutputPID);
     if(sys.MotorFlag==1){
-        LED[0] = 1;
-        LED[1] = 0;
+        //LED[0] = 1;
+        //LED[1] = 0;
         tx_motor();
         sys.MotorFlag=0;
     }
     else{
-        LED[0] = 0;
-        LED[1] = 1;
+        //LED[0] = 0;
+        //LED[1] = 1;
     }
     if(sys.stopflag==1){
         //停止処理