![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 42:02aaa806d929
- Parent:
- 41:3cdcdec9c885
- Child:
- 43:f11f68918299
diff -r 3cdcdec9c885 -r 02aaa806d929 main_processing/strategy/strategy.cpp --- a/main_processing/strategy/strategy.cpp Thu Feb 04 11:43:53 2016 +0000 +++ b/main_processing/strategy/strategy.cpp Thu Feb 04 20:45:41 2016 +0000 @@ -60,8 +60,18 @@ } return; } - - +uint8_t HmcResetFlag; +void HmcReset(void){ + RN42_Reset=1; + wait_us(100); + RN42_Reset=0; +} +uint8_t PingFlag; +void ValidPing(void){ + if(PingFlag==0){ + PingFlag=1; + } +} uint8_t LineReverseFlag; void LineReverse(void){ LineReverseFlag=0; @@ -85,7 +95,7 @@ // if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1; // - Line_timeout[A_SPOT].attach(&LineClear_A, 0.5); + Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY); } void LineCall_B(void){ // @@ -101,7 +111,7 @@ // if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1; // - Line_timeout[B_SPOT].attach(&LineClear_B, 0.5); + Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY); } void LineCall_C(void){ // @@ -112,7 +122,7 @@ // if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1; // - Line_timeout[C_SPOT].attach(&LineClear_C, 0.5); + Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY); } void modeAttack1(void){ double ir_x, ir_y; @@ -213,9 +223,9 @@ vy=(LINE_RF*2)*( IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]); /*vx=(LINE_RF*2)*((-1)*(LineFirst[X_AXIS] == A_SPOT) + (LineFirst[X_AXIS] == B_SPOT)); vy=(LINE_RF*2)*( (LineFirst[Y_AXIS] == C_SPOT) + (-1)*(LineFirst[Y_AXIS] == AB_SPOT)); - Line_timeout[A_SPOT].attach(&LineClear_A, 0.5); - Line_timeout[B_SPOT].attach(&LineClear_B, 0.5); - Line_timeout[C_SPOT].attach(&LineClear_C, 0.5);*/ + 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{ vx=0; @@ -223,9 +233,9 @@ } } if(RawLineSignal>0){ - Line_timeout[A_SPOT].attach(&LineClear_A, 0.5); - Line_timeout[B_SPOT].attach(&LineClear_B, 0.5); - Line_timeout[C_SPOT].attach(&LineClear_C, 0.5); + 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); } move( vx, @@ -239,6 +249,163 @@ return; } void modeAttack2(void){ + double ir_x, ir_y; + int vx,vy,vs, LineForce[2]; + uint8_t LineDir[4]; + uint8_t LineOn[4]; + uint8_t LineStop[2]; + uint8_t IrRange[4]; + uint8_t static LineBind[4]; + 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; + + HmcResetFlag = 0; + PingFlag = 0; + + data.KickOffFlag=0; + } + if(data.IrFlag==1){ + 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==11)&&(vy>0)){ + vy += 10;//前進加速 + } + 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); + 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); + + //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)); + //白線を踏み始めた方向とボールの方向が一致.(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); + + //白線踏んでる + 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))){ + 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))){ + 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)); + 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; + } + + vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS]; + vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS]; + vs = data.OutputPID; + move( + vx, + vy, + vs + ); + data.motorlog[X_AXIS] += vx; + data.motorlog[Y_AXIS] += vy; + if(data.MotorFlag==1){ + tx_motor(); + data.MotorFlag=0; + } + return; +} +void modeAttack3(void){ + if(data.KickFlag==1){ + DriveSolenoid(); + } + return; +} +void modeAttack4(void){ + return; +} +void modeAttack5(void){ if(data.IrFlag==1){ ReadIr(); data.IrFlag=0; @@ -255,191 +422,3 @@ return; } -void modeAttack3(void){ - if(data.KickFlag==1){ - DriveSolenoid(); - } - return; -} -void modeAttack4(void){ - return; -} -void modeAttack5(void){ - return; -} -//Debug -void modeDebug0(void){ - return; -} -void modeDebug1(void){//ChaseOnly NonLine -int vx,vy,vs; - if(data.IrFlag==1){ - ReadIr(); - data.IrFlag=0; - } - if(data.PidFlag==1){ - PidUpdate(); - data.PidFlag=0; - } - - vx = ir_move_val[data.irNotice][data.irPosition][IR_X]; - vy = ir_move_val[data.irNotice][data.irPosition][IR_Y]; - vs = data.OutputPID; - if(data.irPosition<8){ - vx *= data.l_pow; - vy *= data.l_pow; - } - else{ - vx *= data.s_pow; - vy *= data.s_pow; - } - - - move(vx, vy, vs); - - if(data.MotorFlag==1){ - tx_motor(); - data.MotorFlag=0; - } - return; -} -void modeDebug2(void){//LineRestoringForce - int vx,vy,vs; - uint8_t LineStop[2]; - int LineForce[2]; - if(data.IrFlag==1){ - ReadIr(); - data.IrFlag=0; - } - if(data.PidFlag==1){ - PidUpdate(); - data.PidFlag=0; - } - vx = ir_move_val[data.irNotice][data.irPosition][IR_X]; - vy = ir_move_val[data.irNotice][data.irPosition][IR_Y]; - vs = data.OutputPID; - if(data.irPosition<8){ - vx *= data.l_pow; - vy *= data.l_pow; - } - else{ - vx *= data.s_pow; - vy *= data.s_pow; - } - - - LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))); - LineStop[Y_AXIS] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))))*(!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))); - - LineForce[X_AXIS] = LineForce[Y_AXIS] = 0; - if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){ - LineForce[X_AXIS] = -LINE_RF; - } - if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){ - LineForce[X_AXIS] = LINE_RF; - } - if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){ - LineForce[Y_AXIS] = -LINE_RF; - } - if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){ - LineForce[Y_AXIS] = LINE_RF; - } - - move( - vx*LineStop[X_AXIS] + LineForce[X_AXIS], - vy*LineStop[Y_AXIS] + LineForce[Y_AXIS], - vs - ); - if(data.MotorFlag==1){ - tx_motor(); - data.MotorFlag=0; - } - return; -} -void modeDebug3(void){//movesum - int vx,vy,vs; - uint8_t LineStop[2]; - static uint8_t moveLnFlag[4]={1, 1, 1, 1}; - static int moveLnlog[4]; - if(data.IrFlag==1){ - ReadIr(); - data.IrFlag=0; - } - if(data.PidFlag==1){ - PidUpdate(); - data.PidFlag=0; - } - vx = ir_move_val[data.irNotice][data.irPosition][IR_X]; - vy = ir_move_val[data.irNotice][data.irPosition][IR_Y]; - vs = data.OutputPID; - if(data.irPosition<8){ - vx *= data.l_pow; - vy *= data.l_pow; - } - else{ - vx *= data.s_pow; - vy *= data.s_pow; - } - - data.motorlog[X_AXIS] += vx; - data.motorlog[Y_AXIS] += vy; - - if((data.lnFlag[A_SPOT]==1)&&(1)){ - moveLnFlag[A_SPOT]=0; - moveLnlog[A_SPOT] = data.motorlog[X_AXIS]; - } - if((data.lnFlag[B_SPOT]==1)&&(1)){ - moveLnFlag[B_SPOT]=0; - moveLnlog[B_SPOT] = data.motorlog[X_AXIS]; - } - if((data.lnFlag[C_SPOT]==1)&&(1)){ - moveLnFlag[C_SPOT]=0; - moveLnlog[C_SPOT] = data.motorlog[Y_AXIS]; - } - if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){ - moveLnFlag[AB_SPOT]=0; - moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS]; - } - - if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1; - if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1; - if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1; - if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1; - - LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0))); - LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0))); - - move( - vx*LineStop[X_AXIS], - vy*LineStop[Y_AXIS], - vs - ); - if(data.MotorFlag==1){ - tx_motor(); - data.MotorFlag=0; - } - return; -} -void modeDebug4(void){//solenoid - if(data.KickFlag==1){ - DriveSolenoid(); - } - return; -} -void modeDebug5(void){//cmpsTest - if(data.IrFlag==1){ - ReadIr(); - data.IrFlag=0; - } - if(data.PidFlag==1){ - PidUpdate(); - data.PidFlag=0; - } - move(0,0,data.OutputPID); - if(data.MotorFlag==1){ - tx_motor(); - data.MotorFlag=0; - } - - return; -}