![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 27:769cb5a7ea37
- Parent:
- 26:fbb03281fc7d
- Child:
- 28:8ac6c3c1e643
diff -r fbb03281fc7d -r 769cb5a7ea37 main_processing/strategy/strategy.cpp --- a/main_processing/strategy/strategy.cpp Sun Jan 31 06:53:54 2016 +0000 +++ b/main_processing/strategy/strategy.cpp Sun Jan 31 15:46:19 2016 +0000 @@ -3,11 +3,21 @@ //Atk void modeAttack0(void){ - + int vx,vy,vs; + uint8_t LineStop[2]; + static uint8_t moveLnFlag[4]={1, 1, 1, 1}; + static int moveLnlog[4]; + //int LineForce[2]; + //int Return; + //int line if(data.IrFlag==1){ ReadIr(); data.IrFlag=0; } + if(data.PidFlag==1){ + PidUpdate(); + data.PidFlag=0; + } //move(30,30,data.OutputPID); //move(30,0,data.OutputPID); /*move( @@ -15,9 +25,78 @@ ir_move_val[data.irNotice][data.irPosition][IR_Y], data.OutputPID );*/ - move( + vx = ir_move_val[data.irNotice][data.irPosition][IR_X]; + vy = ir_move_val[data.irNotice][data.irPosition][IR_Y]; + vs = data.OutputPID; + + 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)&&((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)))); + + 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 ); + /* + 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 + );*/ + + /* + move( + 0,//vx*LineStop[X_AXIS] + LineForce[X_AXIS], + 0,//vy*LineStop[Y_AXIS] + LineForce[Y_AXIS], + vs + ); + */ + if(data.MotorFlag==1){ + tx_motor(); + data.MotorFlag=0; + } //move(0,0,data.OutputPID); return; }