![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 28:8ac6c3c1e643
- Parent:
- 27:769cb5a7ea37
- Child:
- 29:b413b0bb07a1
diff -r 769cb5a7ea37 -r 8ac6c3c1e643 main_processing/strategy/strategy.cpp --- a/main_processing/strategy/strategy.cpp Sun Jan 31 15:46:19 2016 +0000 +++ b/main_processing/strategy/strategy.cpp Mon Feb 01 02:05:44 2016 +0000 @@ -3,13 +3,90 @@ //Atk void modeAttack0(void){ + double ir_x, ir_y; int vx,vy,vs; + uint8_t LineDir[4]; uint8_t LineStop[2]; - static uint8_t moveLnFlag[4]={1, 1, 1, 1}; - static int moveLnlog[4]; - //int LineForce[2]; - //int Return; - //int line + 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; + } + + 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]; + + + + IrRange[A_SPOT] = (data.irPosition==19)||(data.irPosition==8)||(data.irPosition==9); + IrRange[B_SPOT] = (data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12); + IrRange[C_SPOT] = (data.irPosition==13)||(data.irPosition==14)||(data.irPosition==15); + IrRange[AB_SPOT] = (data.irPosition==16)||(data.irPosition==17)||(data.irPosition==18); + + LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1))); + LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1))); + 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 = ir_x*(!(LineBind[A_SPOT]==1))*(!(LineBind[B_SPOT]==1))*(!(LineBind[C_SPOT]==1))*(!(LineBind[AB_SPOT]==1)); + vy = ir_y*(!(LineBind[A_SPOT]==1))*(!(LineBind[B_SPOT]==1))*(!(LineBind[C_SPOT]==1))*(!(LineBind[AB_SPOT]==1));; + vs = data.OutputPID; + + move( + vx*LineStop[X_AXIS] + (LINE_RF)*(vx!=0)*(LineDir[C_SPOT]==0) + (-LINE_RF)*(vx!=0)*(LineDir[AB_SPOT]==0), + vy*LineStop[Y_AXIS] + (-LINE_RF)*(vy!=0)*(LineDir[A_SPOT]==0) + (LINE_RF)*(vy!=0)*(LineDir[B_SPOT]==0), + vs + ); + + if(data.MotorFlag==1){ + tx_motor(); + data.MotorFlag=0; + } + return; +} +void modeAttack1(void){ + return; +} +void modeAttack2(void){ + return; +} +void modeAttack3(void){ + 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; @@ -18,16 +95,105 @@ PidUpdate(); data.PidFlag=0; } - //move(30,30,data.OutputPID); - //move(30,0,data.OutputPID); - /*move( - ir_move_val[data.irNotice][data.irPosition][IR_X], - ir_move_val[data.irNotice][data.irPosition][IR_Y], - data.OutputPID - );*/ + + 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; @@ -54,9 +220,6 @@ 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))); @@ -65,72 +228,32 @@ 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; -} -void modeAttack1(void){ return; } -void modeAttack2(void){ - return; -} -void modeAttack3(void){ - return; -} -void modeAttack4(void){ - return; -} -void modeAttack5(void){ +void modeDebug4(void){//solenoid + if(data.KickFlag==1){ + DriveSolenoid(); + } return; } -//Debug -void modeDebug0(void){ - return; -} -void modeDebug1(void){ - return; -} -void modeDebug2(void){ +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; } -void modeDebug3(void){ - return; -} -void modeDebug4(void){ - return; -} -void modeDebug5(void){ - return; -}