Diff: main_processing/strategy/strategy.cpp

Revision:

11:3efae754e6ef

Parent:

10:6df631c39f9b

Child:

12:bee8f883c33a

--- a/main_processing/strategy/strategy.cpp	Thu Mar 10 03:03:50 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Thu Mar 10 08:26:33 2016 +0000
@@ -728,12 +728,124 @@
     }
     return;
 }
+double LineJudgeX(double x){
+    const double static LineDecline[5] = {1, .75, .5, .375, 0};
+    const uint8_t static WhiteToWall = 30;
+    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) return LineDecline[LineState];
+        else return 1;
+    }
+    else{
+        if(data.ping[L_PING]<WhiteToWall) return LineDecline[LineState];
+        else return 1;
+    }
+}
+void LineJudgeSlow(double *x, double *y){
+    const double static LineDecline[5] = {1, .75, .5, .375, 0};
+    const uint8_t static WhiteToWall[2] = {30, 30};
+    uint8_t LineState;
+    
+    //line
+    LineState = 0;
+    if(data.lnHold==7){
+        if(data.lnRaw>0){
+            //場外間際...減衰
+            LineState = 3;
+        }
+        else{//data.lnRaw==0
+            //場外...出力完全無効+反発
+            //powerVoid & restore
+            LineState = 4;
+        }
+    }
+    else if(data.lnHold>0){
+        if(data.lnRaw>0){
+            //踏んでるけどまだ出てない...減衰
+            LineState = 2;
+            //timeout&reset...detach
+        }
+        else{//data.lnRaw==0
+            //線をまたいでいるか，中途半端に線を踏んだあと復帰したか...減衰
+            LineState = 1;
+            //if((data.ping[L_PING]>40)&&(data.ping[R_PING]>40)) LineLiberate();
+            //ping&reset
+            //timeout&reset
+        }
+    }
+    //else if(data.lnHold==0){...maxpower
+    
+    if(*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(*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(int8_t *x, int8_t *y);
+void LineJudgeReset(void);
 void modeAttack4(void){
     double ir_x_dir, ir_y_dir;
-    double ir_x_turn;
-    double ir_y_turn;
+    double ir_x_turn, ir_y_turn;
+    double ir_x, ir_y;
     uint8_t ir_pow;
     int vx,vy,vs;
+    
+    
     if(sys.KickOffFlag==1){
         
         sys.IrBlind=0;
@@ -748,6 +860,8 @@
     if(sys.UswFlag==1){ReadPing();sys.UswFlag=0;}
     if(sys.UswFlag2==1){ReadPing2();sys.UswFlag2=0;}
     //if(sys.KickFlag==1){DriveSolenoid();}
+    data.lnRaw = LineRaw;
+    data.lnHold = LineHold;
     
     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];
@@ -767,11 +881,15 @@
     }
     if(sys.IrBlind==1) data.irNotice=IR_NONE;
     
-    //if((LineHold==7) 
+    
+    
     
     
-    vx = ir_pow*(ir_x_dir + ir_x_turn);
-    vy = ir_pow*(ir_y_dir + ir_y_turn);
+    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;
     vs = cmps_set.OutputPID;
     move(
         vx,