Diff: main_processing/strategy/strategy.cpp

Revision:

24:34ef6379b0df

Parent:

23:df68f1a2c226

Child:

25:a7460e23e02e

--- a/main_processing/strategy/strategy.cpp	Wed Mar 16 12:25:48 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Thu Mar 17 02:49:45 2016 +0000
@@ -247,6 +247,8 @@
 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){
@@ -269,7 +271,8 @@
                 
                 
                 
-                /*LastLineCorner[L_LINE]=NewLineCorner[L_LINE];
+                ///*
+                LastLineCorner[L_LINE]=NewLineCorner[L_LINE];
                 LastLineCorner[R_LINE]=NewLineCorner[R_LINE];
                 LastLineCorner[F_LINE]=NewLineCorner[F_LINE];
                 LastLineCorner[B_LINE]=NewLineCorner[B_LINE];
@@ -289,7 +292,8 @@
                 }
                 else{
                     data.lnRepeat=0;
-                }*/
+                }
+                //*/
                 
                 data.FieldSpot = LINE_OUTSIDE;
                 LineLiberate();
@@ -339,35 +343,38 @@
     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]>=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)||
@@ -378,25 +385,32 @@
     ){
         data.lnRepeat = 0;
         
-        data.lnCorner[L_LINE]=LINE_EMPTY;
-        data.lnCorner[R_LINE]=LINE_EMPTY;
-        data.lnCorner[F_LINE]=LINE_EMPTY;
-        data.lnCorner[B_LINE]=LINE_EMPTY;
+        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.lnRepeat=0;
+    if((data.lnRepeat>0)&&(data.FieldSpot == LINE_INSIDE)){
+        //x
         if(
-            ((pow_x>=0)&&(data.lnCorner[R_LINE]))||
-            ((pow_x<0)&&(data.lnCorner[L_LINE]))
+            ((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)&&(data.lnCorner[F_LINE]))||
-            ((pow_y<0)&&(data.lnCorner[B_LINE]))
+            ((pow_y>=0)&&(NewLineCorner[F_LINE]))||
+            ((pow_y<0)&&(NewLineCorner[B_LINE]))
         ){
             data.lnStay[Y_LINE]=0;
         }
@@ -406,60 +420,96 @@
     }
     else{
         data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
-    }*/
+    }
+    //*/
     
 }
 void modeAttack4(void){
+    //irの値に影響するモーター補正値
+    uint8_t ir_pow;
     double ir_x_dir, ir_y_dir;
     double ir_x_turn, ir_y_turn;
     double ir_x, ir_y;
-    
+    //lineの値に影響するモーター補正値
     double LineSlowPower[2];
     double LineReturnPower[2];
-    
-    //double pow_x, pow_y;
-    uint8_t ir_pow;
+    //モーターの出力値
     int vx,vy,vs;
-    //static uint8_t data.FieldSpot;
     
-    
+    ////初期値を決める等
     if(sys.KickOffFlag==1){
-        
-        sys.IrBlind=0;
+        ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
+        sys.IrBlind=1;
         sys.LineBlind=0;
         sys.PingBlind=0;
         
         sys.HomeBlind=1;
         sys.DriBlind=1;
-        
-        
+        //Kick
+        sys.KickStopFlag=0;
+        //Ir
         sys.ir_pow_table = 0;
-        
+        //Line
         data.lnCorner[L_LINE]=data.lnCorner[R_LINE]=data.lnCorner[F_LINE]=data.lnCorner[B_LINE]=LINE_EMPTY;
         data.lnRepeat = 0;
-        
-        //data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
+        data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
         data.lnStop[X_LINE]=data.lnStop[Y_LINE]=1;
-        
         data.FieldSpot = LINE_INSIDE;
         LineLiberate();
         LineRankClear();
-        
-        cmps_set.AtkDeg=0;
-        
+        //pid
+        sys.TurnStopFlag=0;
+        cmps_set.AtkDeg=30;
+        //ドリブラー
         //sys.BallHoldJudgeFlag=0;
         //sys.BallHoldFlag=0;
         
-        
+        //初期値設定の終了
         sys.KickOffFlag=0;
     }
-    //data
+    ////DataRetrieve
     if(sys.InfoFlag==1){ReadInfo();sys.InfoFlag=0;}
-    
     data.lnRaw = LineRaw;
     data.lnHold = LineHold;
     data.ball = ReadBall();
     
+    //ボールがなければ自分のゴールに戻る
+    if(data.irNotice==IR_NONE){
+        sys.BackHomeFlag=(sys.HomeBlind==0);
+    }
+    else{
+        sys.BackHomeFlag=0;
+    }
+    //回り込みの値を代入
+    if(data.ping[B_PING]<=30){
+        sys.ir_pow_table=1;
+        ir_x_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_X_DIR];
+        ir_y_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_DIR];
+        ir_x_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_X_TURN];
+        ir_y_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_TURN];
+    }
+    else{
+        sys.ir_pow_table=0;
+        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];
+        ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
+        ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
+    }
+    //Irの検出値によって出力を調整
+    if(data.irNotice==IR_CLOSER){
+        ir_pow = sys.s_pow;
+    }
+    else if(data.irNotice==IR_CLOSE){
+        ir_pow = sys.m_pow;
+    }
+    else if(data.irNotice==IR_FAR){
+        ir_pow = sys.l_pow;
+    }
+    else{//data.irNotice==IR_NONE
+        ir_pow = 0;
+    }
+    ////ドリブラー
+    //ホールド判定
     /*
     if(sys.DriBlind==0){
         if((data.ball==1)&&(sys.BallHoldJudgeFlag==0)){
@@ -490,53 +540,7 @@
     else{
         sys.BallHoldFlag=0;
     }*/
-    
-    if(data.irNotice==IR_NONE){
-        sys.BackHomeFlag=(sys.HomeBlind==0);
-    }
-    else{
-        sys.BackHomeFlag=0;
-    }
-    
-    if(data.ping[B_PING]<=30){
-        sys.ir_pow_table=1;
-        ir_x_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_X_DIR];
-        ir_y_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_DIR];
-        ir_x_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_X_TURN];
-        ir_y_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_TURN];
-    }
-    else{
-        sys.ir_pow_table=0;
-        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];
-        ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
-        ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
-    }
-    /*
-    ir_x_dir = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_X_DIR];
-    ir_y_dir = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_Y_DIR];
-    ir_x_turn = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_X_TURN];
-    ir_y_turn = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_Y_TURN];
-    */
-    /*
-    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];
-    ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
-    ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
-    */
-    
-    if(data.irNotice==IR_CLOSER){
-        ir_pow = sys.s_pow;
-    }
-    else if(data.irNotice==IR_CLOSE){
-        ir_pow = sys.m_pow;
-    }
-    else if(data.irNotice==IR_FAR){
-        ir_pow = sys.l_pow;
-    }
-    else{//data.irNotice==IR_NONE
-        ir_pow = 0;
-    }
+    //ドリブラー駆動
     if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&(data.irValPhase[IR_SHORT]==DIS_0)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
         sys.DribbleFlag=1;
         //if(sys.BallHoldFlag==0){
@@ -569,17 +573,20 @@
         Ball_leave.detach();*/
     }
     
-    if(sys.IrBlind==1) ir_pow=0;
+    ////Kick
+    if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
+        DriveTurn();
+    }
     
-    
+    //Irの細かい補正値の処理．基本的にLineの補正値を演算する処理が始まる前にIrのモーター出力値補正をすべて終わらせる．
+    if(sys.IrBlind==1) ir_pow=0;
     if((data.ping[R_PING]<data.ping[L_PING])&&(data.irPosition==17)){
         ir_x_turn = -ir_x_turn;
         ir_y_turn = -ir_y_turn;
     }
-    
     ir_x = (ir_x_dir + ir_x_turn);
     ir_y = (ir_y_dir + ir_y_turn);
-    
+    ////Lineセンサーの値を評価しモーターの出力補正値を演算
     if(sys.LineBlind==1){
         
         LineSlowPower[X_LINE] = 1.0;
@@ -595,21 +602,10 @@
     }
     else{
         LineJudgeReset(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
-        
         LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
         LineJudgeReturn(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &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;
-        */
-        
     }
+    ////LEDデバッグ
     //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
     //if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
     
@@ -622,18 +618,21 @@
     //else LED = 0xA;
     //LED = LineHold;
     
-    vx = (ir_pow*ir_x)*data.lnStop[X_LINE]*(/*data.lnStay[X_LINE]*/1)*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
-    vy = (ir_pow*ir_y)*data.lnStop[Y_LINE]*(/*data.lnStay[X_LINE]*/1)*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
+    
+    ////最終的なモーターの出力を演算
+    vx = (ir_pow*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
+    vy = (ir_pow*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
     vs = cmps_set.OutputPID;
     move(
         vx,
         vy,
         vs
     );
+    //モーターに信号を出力
     if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;}
+    
     if(sys.stopflag==1){
-        
-        //停止処理
+        //コマンドモードに戻る際の処理
     }
     return;
 }