Revision 45:c23f25c00d0d, committed 2016-02-25

Comitter:

lilac0112_1

Date:

Thu Feb 25 06:12:47 2016 +0000

Parent:

44:a793dd45f84c

Commit message:

Hokushinetsu(20:1)

Changed in this revision

diff -r a793dd45f84c -r c23f25c00d0d main_processing/setup_command_active/active.cpp
--- a/main_processing/setup_command_active/active.cpp	Fri Feb 05 14:04:07 2016 +0000
+++ b/main_processing/setup_command_active/active.cpp	Thu Feb 25 06:12:47 2016 +0000
@@ -8,11 +8,14 @@
     };
     void (*StrategyFunction[STRATEGY_NUM])(void) = {
         modeAttack2,
+        //modeAttack3,
+        //modeAttack4,
         modeAttack1,
         modeAttack2,
         modeAttack3,
         modeAttack4,
         modeAttack5,
+        
         modeDebug0,
         modeDebug1,
         modeDebug2,

diff -r a793dd45f84c -r c23f25c00d0d main_processing/setup_command_active/command.cpp
--- a/main_processing/setup_command_active/command.cpp	Fri Feb 05 14:04:07 2016 +0000
+++ b/main_processing/setup_command_active/command.cpp	Thu Feb 25 06:12:47 2016 +0000
@@ -163,17 +163,35 @@
 }
 uint8_t LoopFunction8(uint8_t x){
     char buf[0x10];
-    if(x==1) data.s_pow = 15;
-    if(x==2) data.s_pow = 25;
-    if(x==3) data.s_pow = 35;
+    if(x==1){
+        data.s_pow =  20;
+        data.l_pow =  20;
+    }
+    if(x==2){
+        data.s_pow =  30;
+        data.l_pow =  30;
+    }
+    if(x==3){
+        data.s_pow =  40;
+        data.l_pow =  40;
+    }
     sprintf(buf, "TuningS_power");
     Lcd.locate(0, 1);Lcd.print(buf);return 0;
 }
 uint8_t LoopFunction9(uint8_t x){
     char buf[0x10];
-    if(x==1) data.l_pow = 15;
-    if(x==2) data.l_pow = 25;
-    if(x==3) data.l_pow = 35;
+    if(x==1){
+        data.s_pow =  20;
+        data.l_pow =  30;
+    }
+    if(x==2){
+        data.s_pow =  25;
+        data.l_pow =  35;
+    }
+    if(x==3){
+        data.s_pow =  35;
+        data.l_pow =  45;
+    }
     sprintf(buf, "TuningL_power");
     Lcd.locate(0, 1);Lcd.print(buf);return 0;
 }

diff -r a793dd45f84c -r c23f25c00d0d main_processing/setup_command_active/setup.cpp
--- a/main_processing/setup_command_active/setup.cpp	Fri Feb 05 14:04:07 2016 +0000
+++ b/main_processing/setup_command_active/setup.cpp	Thu Feb 25 06:12:47 2016 +0000
@@ -77,7 +77,7 @@
     Line[A_SPOT].rise(&LineCall_A);
     Line[B_SPOT].rise(&LineCall_B);
     Line[C_SPOT].rise(&LineCall_C);
-    //Solenoid_ticker.attach(&AvailableSolenoid, 3.0);
+    Solenoid_ticker.attach(&AvailableSolenoid, 6.0);
     Ir_ticker.attach(&ValidIr, 0.050);
     Ping_ticker.attach(&ValidPing, 0.050);
     //Hmc_ticker.attach(&HmcReset, 1.0);
@@ -88,17 +88,18 @@
 void StopProcess(void){//コマンドに戻るときの処理
     pidupdate.detach();
     Motor_ticker.detach();
-    //Line_ticker.detach();
-    //Solenoid_ticker.detach();
+    ////Line_ticker.detach();
+    Solenoid_ticker.detach();
     Ir_ticker.detach();
     Ping_ticker.detach();
-    //Hmc_ticker.detach();
-    //DriveSolenoid();
+    ////Hmc_ticker.detach();
     motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
     move(0,0,0);
     data.OutputPID=0;
     data.InputPID=REFERENCE;
     //solenoid
+    kicker=1;
+    wait(0.25);
     kicker=0;
     
     __disable_irq(); // 禁止

diff -r a793dd45f84c -r c23f25c00d0d main_processing/strategy/strategy.cpp
--- a/main_processing/strategy/strategy.cpp	Fri Feb 05 14:04:07 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Thu Feb 25 06:12:47 2016 +0000
@@ -78,6 +78,7 @@
 }
 uint8_t LineSign[3];
 uint8_t LineFirst[2];
+uint8_t LinePriority[2];//0を後に，1を優先
 void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;}
 void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;}
 void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;}
@@ -257,7 +258,7 @@
     uint8_t LineStop[2];
     uint8_t IrRange[4];
     uint8_t static LineBind[4];
-    uint8_t static spi_count;
+    //buint8_t static spi_count;
     if(data.KickOffFlag==1){
         LineBind[0]=0;
         LineBind[1]=0;
@@ -275,9 +276,13 @@
         
         HmcResetFlag = 0;
         PingFlag = 0;
-        spi_count=0;
+        //spi_count=0;
+        
+        RN42_Reset=0;
+        data.KickFlag = 0;
         
         data.KickOffFlag=0;
+        //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
     }
     if(data.IrFlag==1){
         /*spi_count++;
@@ -321,8 +326,23 @@
     vx = ir_x;
     vy = ir_y;
     
+    if((data.irPosition==10)&&(vy>0)){
+        vy += 0;//前進加速
+    }
     if((data.irPosition==11)&&(vy>0)){
-        vy += 10;//前進加速
+        vy += 0;//前進加速
+        if(data.KickFlag==1){
+            DriveSolenoid();
+        }
+    }
+    if((data.irPosition==12)&&(vy>0)){
+        vy += 0;//前進加速
+    }
+    if((data.irPosition==1)&&(vy>0)){
+        vy += 0;//前進加速
+    }
+    if((data.irPosition==2)&&(vy>0)){
+        vy += 0;//前進加速
     }
     /*if((data.irPosition==17)&&(data.motorlog[X_AXIS]>0)){
         vx *= -1.0;//背後回り込みの左右判断
@@ -331,28 +351,38 @@
     if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
         vx *= -1.0;//背後回り込みの左右判断
     }
-    
+    /*
+    if((data.InputPID<(REFERENCE-30))||(data.InputPID>(REFERENCE+30))){
+        vx = vx*(0.75);
+        vy = vy*(0.75);
+    }
+    */
     //Lineを踏み始めた方向を調べる
-    LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1                     ))&&(LineFirst[X_AXIS] == A_SPOT);
+    /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1                     ))&&(LineFirst[X_AXIS] == A_SPOT);
     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[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);
+    LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
     
     //Irボールの方向
     //strict
-    /*
+    
     IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
                     ||((6<=data.irPosition)&&(data.irPosition<=7))||((                   1)&&(data.irPosition<=1));
     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))
+    /*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));
+    IrRange[AB_SPOT] = ((10<=data.irPosition)&&(data.irPosition<=12))||((                 1)&&(data.irPosition<=3));*/
     //none
     if(data.irNotice==IR_NONE){
         IrRange[A_SPOT]  = 0;
@@ -366,8 +396,8 @@
     LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
     LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
     
-    LineStop[X_AXIS] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
-    LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
+    LineStop[X_AXIS] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
+    LineStop[Y_AXIS] = (LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
     
     //白線踏んでる
     if(RawLineSignal>0){
@@ -416,9 +446,9 @@
         
         LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) + 
                             (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
-                            *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) +
+                            *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
                             (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
-                            *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30));
+                            *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
         LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
         
         Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
@@ -438,6 +468,11 @@
         vy,
         vs
     );
+    /*move(
+        0,
+        0,
+        10
+    );*/
     data.motorlog[X_AXIS] += vx;
     data.motorlog[Y_AXIS] += vy;
     if(data.MotorFlag==1){
@@ -447,12 +482,265 @@
     return;
 }
 void modeAttack3(void){
-    if(data.KickFlag==1){
-        DriveSolenoid();
+    double ir_x, ir_y;
+    int vx,vy,vs, 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];
+    //buint8_t static spi_count;
+    if(data.KickOffFlag==1){
+        LineBind[0]=0;
+        LineBind[1]=0;
+        LineBind[2]=0;
+        LineBind[3]=0;
+        LineReverseFlag=0;
+        
+        LineSign[A_SPOT]=0;
+        LineSign[B_SPOT]=0;
+        LineSign[C_SPOT]=0;
+        
+        data.lnFlag[A_SPOT]=0;
+        data.lnFlag[B_SPOT]=0;
+        data.lnFlag[C_SPOT]=0;
+        
+        LinePriority[X_AXIS]=0;
+        LinePriority[Y_AXIS]=0;
+        
+        HmcResetFlag = 0;
+        PingFlag = 0;
+        //spi_count=0;
+        
+        RN42_Reset=0;
+        data.KickFlag = 0;
+        
+        data.KickOffFlag=0;
+        //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート
+    }
+    if(data.IrFlag==1){
+        /*spi_count++;
+        if(spi_count%10 == 0){
+            ReadPing();
+        }
+        else{
+            ReadIr();
+        }
+        if(spi_count==20) spi_count=0;
+        */
+        ReadIr();
+        data.IrFlag=0;
+    }
+    if(data.PidFlag==1){
+        PidUpdate();
+        data.PidFlag=0;
+    }
+    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];
+    if(data.irPosition<8){
+        ir_x *= data.l_pow;
+        ir_y *= data.l_pow;
+    }
+    else{
+        ir_x *= data.s_pow;
+        ir_y *= data.s_pow;
+    }
+    
+    //Lineを考慮していないIrのみの値
+    vx = ir_x;
+    vy = ir_y;
+    
+    if((data.irPosition==10)&&(vy>0)){
+        vy += 15;//前進加速
+    }
+    if((data.irPosition==11)&&(vy>0)){
+        vy += 15;//前進加速
+        if(data.KickFlag==1){
+            DriveSolenoid();
+        }
+    }
+    if((data.irPosition==12)&&(vy>0)){
+        vy += 15;//前進加速
+    }
+    if((data.irPosition==1)&&(vy>0)){
+        vy += 25;//前進加速
+    }
+    if((data.irPosition==2)&&(vy>0)){
+        vy += 25;//前進加速
+    }
+    /*if((data.irPosition==17)&&(data.motorlog[X_AXIS]>0)){
+        vx *= -1.0;//背後回り込みの左右判断
+    }*/
+    
+    if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){
+        vx *= -1.0;//背後回り込みの左右判断
+    }
+    /*
+    if((data.InputPID<(REFERENCE-30))||(data.InputPID>(REFERENCE+30))){
+        vx = vx*(0.75);
+        vy = vy*(0.75);
+    }
+    */
+    //Lineを踏み始めた方向を調べる
+    /*LineDir[A_SPOT] = (vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1                     ))&&(LineFirst[X_AXIS] == A_SPOT);
+    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);
+    LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS] == AB_SPOT);
+    
+    //Irボールの方向
+    //strict
+    
+    IrRange[A_SPOT] = ((18<=data.irPosition)&&(data.irPosition<=19))||((8<=data.irPosition)&&(data.irPosition<=10))
+                    ||((6<=data.irPosition)&&(data.irPosition<=7))||((                   1)&&(data.irPosition<=1));
+    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;
+        IrRange[B_SPOT]  = 0;
+        IrRange[C_SPOT]  = 0;
+        IrRange[AB_SPOT] = 0;
+    }
+    //白線を踏み始めた方向とボールの方向が一致．(SelfHold)
+    LineBind[A_SPOT] = (IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==1)||(LineBind[A_SPOT]==1));
+    LineBind[B_SPOT] = (IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==1)||(LineBind[B_SPOT]==1));
+    LineBind[C_SPOT] = (IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==1)||(LineBind[C_SPOT]==1));
+    LineBind[AB_SPOT] = (IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==1)||(LineBind[AB_SPOT]==1));
+    
+    LineStop[X_AXIS] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0);
+    LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0);
+    
+    //白線踏んでる
+    if(RawLineSignal>0){
+        LineOn[A_SPOT]  = (LineSign[A_SPOT]==1)                         &&(LineFirst[X_AXIS]==A_SPOT);
+        LineOn[B_SPOT]  = (LineSign[B_SPOT]==1)                         &&(LineFirst[X_AXIS]==B_SPOT);
+        LineOn[C_SPOT]  = (LineSign[C_SPOT]==1)                         &&(LineFirst[Y_AXIS]==C_SPOT);
+        LineOn[AB_SPOT] = ((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))&&(LineFirst[Y_AXIS]==AB_SPOT);
+        //外側に向かう力を消す．
+        //x
+        if(((LineOn[A_SPOT]==1)&&(vx>0))||((LineOn[B_SPOT]==1)&&(vx<0))){
+            
+            if(LinePriority[Y_AXIS]==0){
+                LinePriority[X_AXIS]=1;
+                LinePriority[Y_AXIS]=0;
+            }
+            vx=0;
+            //yの力を加える．
+            if(vy>0){vy += 10;}
+            if(vy<0){vy -= 10;}
+        }
+        //y
+        if(((LineOn[C_SPOT]==1)&&(vy<0))||((LineOn[AB_SPOT]==1)&&(vy>0))){
+            if(LinePriority[X_AXIS]==0){
+                LinePriority[X_AXIS]=0;
+                LinePriority[Y_AXIS]=1;
+            }
+            if((LinePriority[X_AXIS]==1)&&(LineOn[AB_SPOT]==1)){
+                LinePriority[X_AXIS]=0;
+                LinePriority[Y_AXIS]=1;
+            }
+            vy=0;
+        }
+        //内側に向かう力を加える．
+        LineReturn[A_SPOT] = (LineOn[A_SPOT]==1);
+        /*if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
+            if(LineOn[C_SPOT]==0){
+                LineReturn[A_SPOT]=0;
+            }
+            else{
+                LineReturn[A_SPOT]=1;
+            }
+        }*/
+        
+        LineReturn[B_SPOT] = (LineOn[B_SPOT]==1);
+        /*if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){
+            if(LineOn[C_SPOT]==0){
+                LineReturn[B_SPOT]=0;
+            }
+            else{
+                LineReturn[B_SPOT]=1;
+            }
+        }*/
+        LineReturn[C_SPOT] = (LineOn[C_SPOT]==1);
+        /*if(LineReturn[C_SPOT]==1){
+            LineReturn[A_SPOT]=0;
+            LineReturn[B_SPOT]=0;
+        }*/
+        LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1);
+        
+        LineForce[X_AXIS] = (LINE_RF*2)*(LinePriority[X_AXIS])*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) +
+                            (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) +
+                            (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40));
+        LineForce[Y_AXIS] = (LINE_RF*2)*(LinePriority[Y_AXIS])*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
+        /*
+        LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) + 
+                            (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
+                            *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) +
+                            (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0)
+                            *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30));
+        LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1));
+        */
+        Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY);
+        Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY);
+        Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);
+    }
+    else{
+        LineForce[X_AXIS] = 0;
+        LineForce[Y_AXIS] = 0;
+        
+        LinePriority[X_AXIS]=0;
+        LinePriority[Y_AXIS]=0;
+    }
+    
+    vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS];
+    vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS];
+    vs = data.OutputPID;
+    move(
+        vx,
+        vy,
+        vs
+    );
+    /*move(
+        10,
+        0,
+        0
+    );*/
+    data.motorlog[X_AXIS] += vx;
+    data.motorlog[Y_AXIS] += vy;
+    if(data.MotorFlag==1){
+        tx_motor();
+        data.MotorFlag=0;
     }
     return;
 }
 void modeAttack4(void){
+    if(data.KickFlag==1){
+        DriveSolenoid();
+    }
     return;
 }
 void modeAttack5(void){

diff -r a793dd45f84c -r c23f25c00d0d main_processing/strategy/strategy2.cpp
--- a/main_processing/strategy/strategy2.cpp	Fri Feb 05 14:04:07 2016 +0000
+++ b/main_processing/strategy/strategy2.cpp	Thu Feb 25 06:12:47 2016 +0000
@@ -169,7 +169,9 @@
         PidUpdate();
         data.PidFlag=0;
     }
-    move(0,0,data.OutputPID);
+    move(0,0,10);
+    LED[0]=1;
+    LED[3]=1;
     if(data.MotorFlag==1){
         tx_motor();
         data.MotorFlag=0;

diff -r a793dd45f84c -r c23f25c00d0d main_processing/strategy_parts/output.cpp
--- a/main_processing/strategy_parts/output.cpp	Fri Feb 05 14:04:07 2016 +0000
+++ b/main_processing/strategy_parts/output.cpp	Thu Feb 25 06:12:47 2016 +0000
@@ -24,7 +24,7 @@
     }
 }
 void tx_motor(){//モーターへの送信
-    array2(-speed[1],-speed[0],speed[2],speed[3]);
+    array2(speed[1],-speed[0],-speed[2],speed[3]);//右後左無
     motor.printf("%s",StringFIN.c_str());
 }
 void move(int vx, int vy, int vs){//三輪オムニの移動（基本の形）
@@ -73,7 +73,7 @@
 void DriveSolenoid(void){
     data.KickFlag = 0;
     kicker=1;
-    Solenoid_timeout.attach(&SolenoidSignal, .99);
+    Solenoid_timeout.attach(&SolenoidSignal, .5);
 }
 void SolenoidSignal(void){
     kicker=0;

diff -r a793dd45f84c -r c23f25c00d0d setting/LcdConfig.h
--- a/setting/LcdConfig.h	Fri Feb 05 14:04:07 2016 +0000
+++ b/setting/LcdConfig.h	Thu Feb 25 06:12:47 2016 +0000
@@ -11,8 +11,8 @@
     {{"ClbEnt"},     {"Enter "},      {0       },      {0       }},//5
     {{"ClbExt"},     {"Exit  "},      {0       },      {0       }},//6
     {{"CpsRst"},     {"CpsRst"},      {0       },      {0       }},//7
-    {{"S_POW "},     {"15    "},      {"25    "},      {"35    "}},//8
-    {{"L_POW "},     {"15    "},      {"25    "},      {"35    "}},//9
+    {{"IrPow0"},     {"S20L20"},      {"S30L30"},      {"S40L40"}},//8
+    {{"IrPow1"},     {"S20L30"},      {"S25L35"},      {"S35L45"}},//9
     {{"Reset "},     {"ByeBye"},      {0       },      {0       }},//A
     {{"Test0 "},     {"Debug0"},      {"Debug1"},      {"Debug2"}},//B
     {{"Test1 "},     {"Debug3"},      {"Debug4"},      {"Debug5"}},//C