![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 37:ae5661306900
- Parent:
- 36:50092d488f78
- Child:
- 38:a7eb15b1f813
diff -r 50092d488f78 -r ae5661306900 main_processing/strategy/strategy.cpp --- a/main_processing/strategy/strategy.cpp Wed Feb 03 07:46:32 2016 +0000 +++ b/main_processing/strategy/strategy.cpp Wed Feb 03 09:14:49 2016 +0000 @@ -147,25 +147,35 @@ LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1))); LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1))); */ - + vx = vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)); + vy = vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)); + vs = data.OutputPID; if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){ - if(((LinePulse[A_SPOT]==1)||(LinePulse[B_SPOT]==1)||(LinePulse[C_SPOT]==1)||(LinePulse[AB_SPOT]==1)||1)&&(LineReverseFlag==0)){ + /*if(((LinePulse[A_SPOT]==1)||(LinePulse[B_SPOT]==1)||(LinePulse[C_SPOT]==1)||(LinePulse[AB_SPOT]==1))&&(LineReverseFlag==0)){ LineReverseFlag=1; - Line_timeout.attach(&LineReverse, 1.0); + Line_timeout.attach(&LineReverse, 0.5); } if(LineReverseFlag==1){ - vx=(LINE_RF*2)*((-1)*LineBind[A_SPOT] + LineBind[B_SPOT]); - vy=(LINE_RF*2)*( LineBind[C_SPOT] + (-1)*LineBind[AB_SPOT]); + vx+=(LINE_RF*2)*((-1)*LineBind[A_SPOT] + LineBind[B_SPOT]); + vy+=(LINE_RF*2)*( LineBind[C_SPOT] + (-1)*LineBind[AB_SPOT]); + } + else{ + vx=0; + vy=0; + }*/ + if(RawLineSignal>0){ + vx=(LINE_RF*2)*((-1)*IrRange[A_SPOT] + IrRange[B_SPOT]); + vy=(LINE_RF*2)*( IrRange[C_SPOT] + (-1)*IrRange[AB_SPOT]); } else{ vx=0; vy=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)), + vx, + vy, vs ); /*