![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 43:f11f68918299
- Parent:
- 42:02aaa806d929
- Child:
- 45:c23f25c00d0d
diff -r 02aaa806d929 -r f11f68918299 main_processing/strategy/strategy.cpp --- a/main_processing/strategy/strategy.cpp Thu Feb 04 20:45:41 2016 +0000 +++ b/main_processing/strategy/strategy.cpp Fri Feb 05 11:01:03 2016 +0000 @@ -253,9 +253,11 @@ 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]; + uint8_t static spi_count; if(data.KickOffFlag==1){ LineBind[0]=0; LineBind[1]=0; @@ -273,10 +275,20 @@ HmcResetFlag = 0; PingFlag = 0; + spi_count=0; data.KickOffFlag=0; } 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; } @@ -284,11 +296,11 @@ PidUpdate(); data.PidFlag=0; } - /* if(PingFlag==1){ ReadPing(); PingFlag=0; } + /* if(HmcResetFlag==1){ HmcReset(); HmcResetFlag=0; @@ -312,14 +324,14 @@ if((data.irPosition==11)&&(vy>0)){ vy += 10;//前進加速 } - if((data.irPosition==17)&&(data.motorlog[X_AXIS]>0)){ + /*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;//背後回り込みの左右判断 } - */ + //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); @@ -341,14 +353,21 @@ 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] = (LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0); - LineStop[Y_AXIS] = (LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0); + 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){ @@ -369,8 +388,39 @@ vy=0; } //内側に向かう力を加える. - LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineOn[A_SPOT]==1) + ( 1)*(LineOn[B_SPOT]==1)); - LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineOn[C_SPOT]==1) + (-1)*(LineOn[AB_SPOT]==1)); + 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)*((-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);