Diff: main_processing/strategy/strategy.cpp

Revision:

5:5ff3a7d5d8c2

Parent:

2:635947de1583

Child:

10:6df631c39f9b

--- a/main_processing/strategy/strategy.cpp	Fri Mar 04 03:39:29 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Fri Mar 04 09:01:20 2016 +0000
@@ -19,8 +19,8 @@
         Line_ticker.attach(&ReadLine, 0.005);
         sys.PidFlag=0;
     }
-    ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
-    ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
+    ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
     if(data.irPosition<8){
         ir_x *= (double)(sys.l_pow);
         ir_y *= (double)(sys.l_pow);
@@ -153,8 +153,8 @@
         //Line_ticker.attach(&ReadLine, 0.005);
         sys.PidFlag=0;
     }
-    ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
-    ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
+    ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
     if(data.irPosition<8){
         ir_x *= sys.l_pow;
         ir_y *= sys.l_pow;
@@ -244,16 +244,20 @@
 }
 void modeAttack2(void){
     double ir_x, ir_y;
-    int vx,vy,vs, LineForce[2];
+    int vx,vy,vs;
+    /*int LineForce[2];
     uint8_t LineDir[4];
     uint8_t LineOn[4];
     uint8_t LineReturn[4];
     uint8_t LineStop[2];
     uint8_t IrRange[4];
-    uint8_t static LineBind[4];
+    uint8_t static LineBind[4];*/
     //buint8_t static spi_count;
     if(sys.KickOffFlag==1){
-        LineBind[0]=0;
+        
+        sys.MotorFlag=0;
+        sys.IrFlag=0;
+        /*LineBind[0]=0;
         LineBind[1]=0;
         LineBind[2]=0;
         LineBind[3]=0;
@@ -270,7 +274,7 @@
         HmcResetFlag = 0;
         PingFlag = 0;
         //spi_count=0;
-        
+        */
         hmc_reset=0;
         sys.KickFlag = 0;
         
@@ -278,6 +282,8 @@
         //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
     }
     if(sys.IrFlag==1){
+        //LED[0] = 0;
+        //LED[1] = 1;
         /*spi_count++;
         if(spi_count%10 == 0){
             ReadPing();
@@ -294,18 +300,21 @@
         PidUpdate();
         sys.PidFlag=0;
     }
-    if(PingFlag==1){
+    /*if(PingFlag==1){
         ReadPing();
         PingFlag=0;
-    }
+    }*/
     /*
     if(HmcResetFlag==1){
         HmcReset();
         HmcResetFlag=0;
     }
     */
-    ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
-    ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
+    //ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    //ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
+    ir_x = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
+    ir_y = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
+    
     if(data.irPosition<8){
         ir_x *= sys.l_pow;
         ir_y *= sys.l_pow;
@@ -318,7 +327,7 @@
     //Lineを考慮していないIrのみの値
     vx = ir_x;
     vy = ir_y;
-    
+    /*
     if((data.irPosition==10)&&(vy>0)){
         vy += 0;//前進加速
     }
@@ -340,7 +349,7 @@
     
     if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
         vx *= -1.0;//背後回り込みの左右判断
-    }
+    }*/
     /*
     if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){
         vx = vx*(0.75);
@@ -352,7 +361,7 @@
     LineDir[B_SPOT] = (vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1                     ))&&(LineFirst[X_AXIS] == B_SPOT);
     LineDir[C_SPOT] = (vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1                     ))&&(LineFirst[Y_AXIS] == C_SPOT);
     LineDir[AB_SPOT]= (vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);*/
-    
+    /*
     LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1                  ))&&(LineFirst[X_AXIS] == A_SPOT);
     LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1                  ))&&(LineFirst[X_AXIS] == B_SPOT);
     LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1                  ))&&(LineFirst[Y_AXIS] == C_SPOT);
@@ -366,13 +375,14 @@
     IrRange[B_SPOT] = ((12<=data.irPosition)&&(data.irPosition<=16))||((2<=data.irPosition)&&(data.irPosition<=5));
     IrRange[C_SPOT] = ((15<=data.irPosition)&&(data.irPosition<=19))||((4<=data.irPosition)&&(data.irPosition<=7));
     IrRange[AB_SPOT] = ((9<=data.irPosition)&&(data.irPosition<=13))||((                 1)&&(data.irPosition<=3));
-    
+    */
     //sweet
     /*IrRange[A_SPOT] = ((19<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=9))
                     ||((6<=data.irPosition)&&(data.irPosition<=7))||((                   1)&&(data.irPosition<=1));
     IrRange[B_SPOT] = ((13<=data.irPosition)&&(data.irPosition<=15))||((2<=data.irPosition)&&(data.irPosition<=5));
     IrRange[C_SPOT] = ((16<=data.irPosition)&&(data.irPosition<=18))||((4<=data.irPosition)&&(data.irPosition<=7));
     IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||((                 1)&&(data.irPosition<=3));*/
+    /*
     //none
     if(data.irNotice==IR_NONE){
         IrRange[A_SPOT]  = 0;
@@ -449,9 +459,9 @@
         LineForce[X_AXIS] = 0;
         LineForce[Y_AXIS] = 0;
     }
-    
-    vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
-    vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
+    */
+    //vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
+    //vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
     vs = cmps_set.OutputPID;
     move(
         vx,
@@ -534,8 +544,8 @@
         HmcResetFlag=0;
     }
     */
-    ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
-    ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
+    ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X];
+    ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y];
     if(data.irPosition<8){
         ir_x *= sys.l_pow;
         ir_y *= sys.l_pow;
@@ -744,10 +754,15 @@
     }
     move(0,0,10);
     if(sys.MotorFlag==1){
+        LED[0] = 1;
+        LED[1] = 0;
         tx_motor();
         sys.MotorFlag=0;
     }
-    
+    else{
+        LED[0] = 0;
+        LED[1] = 1;
+    }
     if(sys.stopflag==1){
         //停止処理
     }