![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 31:0b0f64831771
- Parent:
- 30:26070ba1f21f
- Child:
- 34:69bdf29a4442
diff -r 26070ba1f21f -r 0b0f64831771 main_processing/strategy/strategy.cpp --- a/main_processing/strategy/strategy.cpp Mon Feb 01 11:43:58 2016 +0000 +++ b/main_processing/strategy/strategy.cpp Tue Feb 02 04:07:06 2016 +0000 @@ -7,6 +7,8 @@ int vx,vy,vs; uint8_t LineDir[4]; uint8_t LineStop[2]; + //uint8_t IrRange[4]; + //uint8_t LineBind[4]; if(data.IrFlag==1){ ReadIr(); data.IrFlag=0; @@ -39,7 +41,7 @@ LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT]; LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT]; - vs = data.OutputPID; + //Ir /* //ForLineAll data.motorlog[X_AXIS] *= LineStop[X_AXIS]; @@ -59,6 +61,63 @@ return; } void modeAttack1(void){ + double ir_x, ir_y; + int vx,vy,vs; + uint8_t LineDir[4]; + uint8_t LineStop[2]; + //uint8_t IrRange[4]; + //uint8_t LineBind[4]; + if(data.IrFlag==1){ + ReadIr(); + data.IrFlag=0; + } + if(data.PidFlag==1){ + Line_ticker.detach(); + PidUpdate(); + Line_ticker.attach(&ReadLine, 0.005); + data.PidFlag=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; + //Line検出方向を調べる + LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0)))); + LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1)))); + LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))); + LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))); + + LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT]; + LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT]; + + if(data.IrFlag==1){ + ReadIr(); + data.IrFlag=0; + } + if(data.PidFlag==1){ + PidUpdate(); + data.PidFlag=0; + } + vs = data.OutputPID; + move( + vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)), + vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)), + vs + ); + if(data.MotorFlag==1){ + tx_motor(); + data.MotorFlag=0; + } return; } void modeAttack2(void){