Diff: main_processing/strategy/strategy.cpp

Revision:

32:367b16d69a32

Parent:

31:745a775cfc20

Child:

33:aa115c30892e

--- a/main_processing/strategy/strategy.cpp	Wed Mar 23 13:01:43 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Thu Mar 24 04:54:39 2016 +0000
@@ -244,6 +244,25 @@
                 }
             }
         }
+        if((sys.HomeBlind==0)){
+            if(((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER))&&(data.ping[B_PING]<60)&&
+                (
+                    (data.irPosition==11)||
+                    (data.irPosition==(ir_posi_s[(11-8+24+1)%12]))||
+                    (data.irPosition==(ir_posi_s[(11-8+24-1)%12]))||
+                    (data.irPosition==(ir_posi_s[(11-8+24+2)%12]))||
+                    (data.irPosition==(ir_posi_s[(11-8+24-2)%12]))||
+                    (data.irPosition==(ir_posi_s[(11-8+24+3)%12]))||
+                    (data.irPosition==(ir_posi_s[(11-8+24-3)%12]))
+                )
+            ){
+                sys.ir_pow_table=2;//直進
+                ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
+                ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
+                ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
+                ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
+            }
+        }
     }
     
     //Irの検出値によって出力を調整
@@ -287,21 +306,23 @@
     //ホールド判定
     JudgeBallHolding();
     //ドリブラー駆動
-    if((sys.DriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){
-        if(
-            ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))||
-            ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))
-        ){
-            cmps_set.GoalDeg=0;
-        }
-        else if((data.lnRawOrderLog1[0]==A_SPOT)&&(1)){
-            cmps_set.GoalDeg=-30;
-        }
-        else if((data.lnRawOrderLog1[0]==B_SPOT)&&(1)){
-            cmps_set.GoalDeg=30;
-        }
-        else{
-            cmps_set.GoalDeg=0;
+    if((sys.TurnDriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){
+        if(sys.TurnHoldBlind==0){
+            if(
+                ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))||
+                ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))
+            ){
+                cmps_set.GoalDeg=0;
+            }
+            else if((data.lnRawOrderLog1[0]==A_SPOT)&&(1)){
+                cmps_set.GoalDeg=-30;
+            }
+            else if((data.lnRawOrderLog1[0]==B_SPOT)&&(1)){
+                cmps_set.GoalDeg=30;
+            }
+            else{
+                cmps_set.GoalDeg=0;
+            }
         }
     }
     if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
@@ -343,8 +364,8 @@
         (data.ping[R_PING]<30)&&
         (data.ping[L_PING]<30)
     ){
-        DriveTurn();
-        DriveSolenoid();
+        //DriveTurn();
+        //DriveSolenoid();
     }
     
     //Irの細かい補正値の処理．基本的にLineの補正値を演算する処理が始まる前にIrのモーター出力値補正をすべて終わらせる．
@@ -378,7 +399,13 @@
             ir_y = -1;
         }
         else{
-            ir_y = 0;
+            if((data.ping[B_PING]>40)&&(data.ping[R_PING]>55)&&(data.ping[L_PING]>55)){
+                ir_y = -1;
+                ir_pow_y = 15;
+            }
+            else{
+                ir_y = 0;
+            }
             //sys.HomeStayFlag[Y_PING]=1;
         }
     }
@@ -391,6 +418,7 @@
         if(ir_y>0) ir_y=0;
         if((data.ping[B_PING]>60)&&(1)){
             ir_y = -1;
+            ir_pow_y = 15;
         }
     }
     if(
@@ -434,11 +462,11 @@
     //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
     
     //LED = sys.BallHoldFlag*15;
-    //if(sys.BallHoldFlag==1) LED=15;
-    //else LED=10;
+    if(sys.BallHoldFlag==1) LED=15;
+    else LED=10;
     
-    if(data.lnRepeat>=1) LED=15;
-    else LED=10;
+    //if(data.lnRepeat>=1) LED=15;
+    //else LED=10;
     
     //LED = 0xFF*(data.ping[B_PING]<30);
     
@@ -449,8 +477,8 @@
     
     
     ////最終的なモーターの出力を演算
-    vx = (ir_pow_x*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
-    vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
+    vx = (ir_pow_x*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[X_LINE];
+    vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[Y_LINE];
     vs = cmps_set.OutputPID;
     move(
         vx,