ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
main_processing/strategy/strategy.cpp
- Committer:
- lilac0112_1
- Date:
- 2016-03-11
- Revision:
- 13:b20921316f3c
- Parent:
- 12:bee8f883c33a
- Child:
- 14:b510adcb6065
File content as of revision 13:b20921316f3c:
#include "mbed.h" #include "extern.h" //Atk void modeAttack0(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(sys.IrFlag==1){ ReadIr(); sys.IrFlag=0; } if(sys.PidFlag==1){ Line_ticker.detach(); PidUpdate(); Line_ticker.attach(&ReadLine, 0.005); sys.PidFlag=0; } ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X]; ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y]; if(data.irPosition<8){ ir_x *= (double)(sys.l_pow); ir_y *= (double)(sys.l_pow); } else{ ir_x *= (double)(sys.s_pow); ir_y *= (double)(sys.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]; //Ir 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(sys.MotorFlag==1){ tx_motor(); sys.MotorFlag=0; } return; } uint8_t HmcResetFlag; void HmcReset(void){ hmc_reset=HMC_RST; wait_us(100); hmc_reset=HMC_RUN; } uint8_t LineReverseFlag; void LineReverse(void){ LineReverseFlag=0; } uint8_t LineSign[3]; uint8_t LineFirst[2]; uint8_t LinePriority[2];//0を後に,1を優先 void LineClear_A(void){LineSign[A_SPOT]=0;data.lnFlag[A_SPOT]=0;} void LineClear_B(void){LineSign[B_SPOT]=0;data.lnFlag[B_SPOT]=0;} void LineClear_C(void){LineSign[C_SPOT]=0;data.lnFlag[C_SPOT]=0;} void LineCall_A(void){ // LineSign[A_SPOT] = 1; if(LineSign[B_SPOT]==0){ LineFirst[X_AXIS] = A_SPOT; } if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){ if(LineSign[C_SPOT]==0){ LineFirst[Y_AXIS] = AB_SPOT; } } // if((Line[A_SPOT].read()==1)||(1)) data.lnFlag[A_SPOT]=1; // Line_timeout[A_SPOT].attach(&LineClear_A, LINE_DELAY); } void LineCall_B(void){ // LineSign[B_SPOT] = 1; if(LineSign[A_SPOT]==0){ LineFirst[X_AXIS] = B_SPOT; } if((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1)){ if(LineSign[C_SPOT]==0){ LineFirst[Y_AXIS] = AB_SPOT; } } // if((Line[B_SPOT].read()==1)||(1)) data.lnFlag[B_SPOT]=1; // Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY); } void LineCall_C(void){ // LineSign[C_SPOT] = 1; if(!((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))){ LineFirst[Y_AXIS] = C_SPOT; } // if((Line[C_SPOT].read()==1)||(1)) data.lnFlag[C_SPOT]=1; // Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY); } 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 LinePulse[4]; uint8_t static LineBind[4]; if(sys.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; sys.KickOffFlag=0; } if(sys.IrFlag==1){ ReadIr(); sys.IrFlag=0; } if(sys.PidFlag==1){ //Line_ticker.detach(); PidUpdate(); //Line_ticker.attach(&ReadLine, 0.005); sys.PidFlag=0; } ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X]; ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y]; if(data.irPosition<8){ ir_x *= sys.l_pow; ir_y *= sys.l_pow; } else{ ir_x *= sys.s_pow; ir_y *= sys.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))&&(1))); LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))&&(1))); LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1 ))&&(1))); LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))&&(1))); LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT]; LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[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)); LinePulse[A_SPOT] = ((IrRange[A_SPOT]==1)&&(LineDir[A_SPOT]==0)); LinePulse[B_SPOT] = ((IrRange[B_SPOT]==1)&&(LineDir[B_SPOT]==0)); LinePulse[C_SPOT] = ((IrRange[C_SPOT]==1)&&(LineDir[C_SPOT]==0)); LinePulse[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&(LineDir[AB_SPOT]==0)); LineBind[A_SPOT] = ((LinePulse[A_SPOT])||((IrRange[A_SPOT]==1)&&(LineBind[A_SPOT]==1)))&&(LineBind[B_SPOT]==0); LineBind[B_SPOT] = ((LinePulse[B_SPOT])||((IrRange[B_SPOT]==1)&&(LineBind[B_SPOT]==1)))&&(LineBind[A_SPOT]==0); LineBind[C_SPOT] = ((LinePulse[C_SPOT])||((IrRange[C_SPOT]==1)&&(LineBind[C_SPOT]==1)))&&(LineBind[AB_SPOT]==0); LineBind[AB_SPOT] = ((LinePulse[AB_SPOT])||((IrRange[AB_SPOT]==1)&&(LineBind[AB_SPOT]==1)))&&(LineBind[C_SPOT]==0); /* 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 = 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 = cmps_set.OutputPID; if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){ if(LineRaw>0){ vx=(LINE_RF*2)*((-1)*IrRange[A_SPOT] + IrRange[B_SPOT]); 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, LINE_DELAY); Line_timeout[B_SPOT].attach(&LineClear_B, LINE_DELAY); Line_timeout[C_SPOT].attach(&LineClear_C, LINE_DELAY);*/ } else{ vx=0; vy=0; } } if(LineRaw>0){ 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, vy, vs ); if(sys.MotorFlag==1){ tx_motor(); sys.MotorFlag=0; } return; } void modeAttack2(void){ double ir_x, ir_y; int vx,vy,vs; /*int LineForce[2]; uint8_t LineDir[4]; uint8_t LineOn[4]; uint8_t LineReturn[4]; uint8_t LineStop[2]; uint8_t IrRange[4]; uint8_t static LineBind[4];*/ //buint8_t static spi_count; if(sys.KickOffFlag==1){ sys.MotorFlag=0; sys.IrFlag=0; /*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; sys.UswFlag = 0; //spi_count=0; */ hmc_reset=HMC_RUN; sys.KickFlag = 0; sys.KickOffFlag=0; //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート } if(sys.IrFlag==1){ //LED[0] = 0; //LED[1] = 1; /*spi_count++; if(spi_count%10 == 0){ ReadPing(); } else{ ReadIr(); } if(spi_count==20) spi_count=0; */ ReadIr(); sys.IrFlag=0; } if(sys.PidFlag==1){ PidUpdate(); sys.PidFlag=0; } /*if(sys.UswFlag==1){ ReadPing(); sys.UswFlag=0; }*/ /* if(HmcResetFlag==1){ HmcReset(); HmcResetFlag=0; } */ //ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X]; //ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y]; ir_x = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN]; ir_y = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]+ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN]; if(data.irPosition<8){ ir_x *= sys.l_pow; ir_y *= sys.l_pow; } else{ ir_x *= sys.s_pow; ir_y *= sys.s_pow; } //Lineを考慮していないIrのみの値 vx = ir_x; vy = ir_y; /* if((data.irPosition==10)&&(vy>0)){ vy += 0;//前進加速 } if((data.irPosition==11)&&(vy>0)){ vy += 0;//前進加速 if(sys.KickFlag==1){ DriveSolenoid(); } } if((data.irPosition==12)&&(vy>0)){ vy += 0;//前進加速 } if((data.irPosition==1)&&(vy>0)){ vy += 0;//前進加速 } if((data.irPosition==2)&&(vy>0)){ vy += 0;//前進加速 } if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){ vx *= -1.0;//背後回り込みの左右判断 }*/ /* if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){ vx = vx*(0.75); vy = vy*(0.75); } */ //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);*/ /* LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT); LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT); LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT); LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[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));*/ /* //none if(data.irNotice==IR_NONE){ IrRange[A_SPOT] = 0; IrRange[B_SPOT] = 0; IrRange[C_SPOT] = 0; IrRange[AB_SPOT] = 0; } //白線を踏み始めた方向とボールの方向が一致.(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(LineRaw>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; } //内側に向かう力を加える. LineReturn[A_SPOT] = (LineOn[A_SPOT]==1); if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){ if(LineOn[C_SPOT]==0){ LineReturn[A_SPOT]=0; } else{ LineReturn[A_SPOT]=1; } } LineReturn[B_SPOT] = (LineOn[B_SPOT]==1); if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){ if(LineOn[C_SPOT]==0){ LineReturn[B_SPOT]=0; } else{ LineReturn[B_SPOT]=1; } } LineReturn[C_SPOT] = (LineOn[C_SPOT]==1); if(LineReturn[C_SPOT]==1){ LineReturn[A_SPOT]=0; LineReturn[B_SPOT]=0; } LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1); LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) + (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0) *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) + (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0) *(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)); LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[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 = cmps_set.OutputPID; move( vx, vy, vs ); /*move( 0, 0, 10 );*/ if(sys.MotorFlag==1){ tx_motor(); sys.MotorFlag=0; } return; } void modeAttack3(void){ double ir_x, ir_y; int vx,vy,vs, LineForce[2]; uint8_t LineDir[4]; uint8_t LineOn[4]; uint8_t LineReturn[4]; uint8_t LineStop[2]; uint8_t IrRange[4]; uint8_t static LineBind[4]; //buint8_t static spi_count; if(sys.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; LinePriority[X_AXIS]=0; LinePriority[Y_AXIS]=0; HmcResetFlag = 0; sys.UswFlag = 0; //spi_count=0; hmc_reset=HMC_RUN; sys.KickFlag = 0; sys.KickOffFlag=0; //while((Sw[2].read()==1)&&(Sw[3].read()==1));//押して離すとスタート } if(sys.IrFlag==1){ /*spi_count++; if(spi_count%10 == 0){ ReadPing(); } else{ ReadIr(); } if(spi_count==20) spi_count=0; */ ReadIr(); sys.IrFlag=0; } if(sys.PidFlag==1){ PidUpdate(); sys.PidFlag=0; } if(sys.UswFlag==1){ ReadPing(); sys.UswFlag=0; } /* if(HmcResetFlag==1){ HmcReset(); HmcResetFlag=0; } */ ir_x = ir_move_val_old[data.irNotice][data.irPosition][IR_X]; ir_y = ir_move_val_old[data.irNotice][data.irPosition][IR_Y]; if(data.irPosition<8){ ir_x *= sys.l_pow; ir_y *= sys.l_pow; } else{ ir_x *= sys.s_pow; ir_y *= sys.s_pow; } //Lineを考慮していないIrのみの値 vx = ir_x; vy = ir_y; if((data.irPosition==10)&&(vy>0)){ vy += 15;//前進加速 } if((data.irPosition==11)&&(vy>0)){ vy += 15;//前進加速 if(sys.KickFlag==1){ DriveSolenoid(); } } if((data.irPosition==12)&&(vy>0)){ vy += 15;//前進加速 } if((data.irPosition==1)&&(vy>0)){ vy += 25;//前進加速 } if((data.irPosition==2)&&(vy>0)){ vy += 25;//前進加速 } if((data.irPosition==17)&&(data.ping[L_PING]>data.ping[R_PING])){ vx *= -1.0;//背後回り込みの左右判断 } /* if((cmps_set.InputPID<(REFERENCE-30))||(cmps_set.InputPID>(REFERENCE+30))){ vx = vx*(0.75); vy = vy*(0.75); } */ //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);*/ LineDir[A_SPOT] = (vx>0)&&((LineSign[A_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == A_SPOT); LineDir[B_SPOT] = (vx<0)&&((LineSign[B_SPOT]==1)&&(1 ))&&(LineFirst[X_AXIS] == B_SPOT); LineDir[C_SPOT] = (vy<0)&&((LineSign[C_SPOT]==1)&&(1 ))&&(LineFirst[Y_AXIS] == C_SPOT); LineDir[AB_SPOT]= (vy>0)&&((LineSign[A_SPOT]==1)&&(LineSign[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));*/ //none if(data.irNotice==IR_NONE){ IrRange[A_SPOT] = 0; IrRange[B_SPOT] = 0; IrRange[C_SPOT] = 0; IrRange[AB_SPOT] = 0; } //白線を踏み始めた方向とボールの方向が一致.(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] = 1;//(LineBind[A_SPOT]==0)*(LineBind[B_SPOT]==0); LineStop[Y_AXIS] = 1;//(LineBind[C_SPOT]==0)*(LineBind[AB_SPOT]==0); //白線踏んでる if(LineRaw>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))){ if(LinePriority[Y_AXIS]==0){ LinePriority[X_AXIS]=1; LinePriority[Y_AXIS]=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))){ if(LinePriority[X_AXIS]==0){ LinePriority[X_AXIS]=0; LinePriority[Y_AXIS]=1; } if((LinePriority[X_AXIS]==1)&&(LineOn[AB_SPOT]==1)){ LinePriority[X_AXIS]=0; LinePriority[Y_AXIS]=1; } vy=0; } //内側に向かう力を加える. LineReturn[A_SPOT] = (LineOn[A_SPOT]==1); /*if((LineReturn[A_SPOT]==1)&&(LineOn[AB_SPOT]==1)){ if(LineOn[C_SPOT]==0){ LineReturn[A_SPOT]=0; } else{ LineReturn[A_SPOT]=1; } }*/ LineReturn[B_SPOT] = (LineOn[B_SPOT]==1); /*if((LineReturn[B_SPOT]==1)&&(LineOn[AB_SPOT]==1)){ if(LineOn[C_SPOT]==0){ LineReturn[B_SPOT]=0; } else{ LineReturn[B_SPOT]=1; } }*/ LineReturn[C_SPOT] = (LineOn[C_SPOT]==1); /*if(LineReturn[C_SPOT]==1){ LineReturn[A_SPOT]=0; LineReturn[B_SPOT]=0; }*/ LineReturn[AB_SPOT] = (LineOn[AB_SPOT]==1); LineForce[X_AXIS] = (LINE_RF*2)*(LinePriority[X_AXIS])*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) + (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)) + (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(( 1)*(data.ping[L_PING]<40) + (-1)*(data.ping[R_PING]<40)); LineForce[Y_AXIS] = (LINE_RF*2)*(LinePriority[Y_AXIS])*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[AB_SPOT]==1)); /* LineForce[X_AXIS] = (LINE_RF*2)*((-1)*(LineReturn[A_SPOT]==1) + ( 1)*(LineReturn[B_SPOT]==1)) + (LINE_RF*2)*(LineReturn[AB_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0) *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)) + (LINE_RF*2)*(LineReturn[C_SPOT]==1)*(LineReturn[A_SPOT]==0)*(LineReturn[B_SPOT]==0) *(( 1)*(data.ping[L_PING]<30) + (-1)*(data.ping[R_PING]<30)); LineForce[Y_AXIS] = (LINE_RF*2)*(( 1)*(LineReturn[C_SPOT]==1) + (-1)*(LineReturn[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; LinePriority[X_AXIS]=0; LinePriority[Y_AXIS]=0; } vx = vx*LineStop[X_AXIS] + LineForce[X_AXIS]; vy = vy*LineStop[Y_AXIS] + LineForce[Y_AXIS]; vs = cmps_set.OutputPID; move( vx, vy, vs ); /*move( 10, 0, 0 );*/ if(sys.MotorFlag==1){ tx_motor(); sys.MotorFlag=0; } if(sys.stopflag==1){ //停止処理 } return; } double LineJudgeX(double x){ uint8_t LineState; uint8_t LnRaw; uint8_t LnHold; LnRaw = LineRaw; LnHold = LineHold; //line LnRaw = LineRaw; LnHold = LineHold; /* if((LineRaw==0)){ if((data.ping[L_PING]>60)&&(data.ping[R_PING]>60)){ LineLiberate(); } }*/ if(LnHold==7){ if(LnRaw>0){ //場外間際 LineState = 3; } else{//LnRaw==0 //powerVoid & restore //場外 LineState = 4; } } else if(LnHold>0){ if(LnRaw>0){ //踏んでるけどまだ出てない LineState = 4; } else{//LnRaw==0 //線をまたいでいるか,中途半端に線を踏んだあと復帰したか LineState = 4; //ping&reset //timeout&reset } } //else if(LnHold==0){...maxpower if(x>=0){ if(data.ping[R_PING]<WhiteToWall[X_PING]) return LineDecline[LineState]; else return 1; } else{ if(data.ping[L_PING]<WhiteToWall[X_PING]) return LineDecline[LineState]; else return 1; } } void LineJudgeSlow(double *x, double *y){ uint8_t LineState; //line LineState = 0; if(data.lnHold==7){ if(data.lnRaw>0){ //場外間際...減衰 LineState = 3; } else{//data.lnRaw==0 //場外...出力完全無効+反発 //powerVoid & restore LineState = 4; } } else if(data.lnHold>0){ if(data.lnRaw>0){ //踏んでるけどまだ出てない...減衰 LineState = 2; //timeout&reset...detach } else{//data.lnRaw==0 //線をまたいでいるか,中途半端に線を踏んだあと復帰したか...減衰 LineState = 1; //if((data.ping[L_PING]>40)&&(data.ping[R_PING]>40)) LineLiberate(); //ping&reset //timeout&reset } } //else if(data.lnHold==0){...maxpower if(data.FieldState==LINE_OUTSIDE){ LineState = 4; } if(*x>=0){ if(data.ping[R_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState]; else *x=1; } else{ if(data.ping[L_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState]; else *x=1; } if(*y>=0){ if(data.ping[F_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState]; else *y=1; } else{ if(data.ping[B_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState]; else *y=1; } } void LineJudgeReturn(double pow_x, double pow_y, double *x, double *y){ const int8_t static LineReturn[5] = {0, 0, 0, 0, 10}; uint8_t LinePingState[4]; //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される. //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される. //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない.ボールを追う力は場内に出るまで作用する. //×ラインのほぼ場外では常時ラインから離れる力が優先される. //※場外判定を行うには再び場内に戻る必要がある. if(data.FieldState==LINE_OUTSIDE){ LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWall[X_PING]); LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWall[X_PING]); LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWall[Y_PING]); LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWall[Y_PING]); //line間際の復帰力以外の力を作用させるか否か data.lnStop[X_LINE]=1; data.lnStop[Y_LINE]=1; //x if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){ if(pow_x>0){ *x = -pow_x*(1-0.75); } else{ *x = pow_x*(1-0.75); } data.lnStop[X_LINE]=1; } if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){ if(pow_x<0){ *x = 0; data.lnStop[X_LINE]=1; } else{ *x = -LineReturn[4]; data.lnStop[X_LINE]=0; } } if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){ if(pow_x>0){ *x = 0; data.lnStop[X_LINE]=1; } else{ *x = LineReturn[4]; data.lnStop[X_LINE]=0; } } if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){ *x = 0; data.lnStop[X_LINE]=0; } //y if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==0)){ if(pow_y>0){ *y = -pow_y*(1-0.75); } else{ *y = pow_y*(1-0.75); } data.lnStop[Y_LINE]=1; } if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){ if(pow_y<0){ *y = 0; data.lnStop[Y_LINE]=1; } else{ *y = -LineReturn[4]; data.lnStop[Y_LINE]=0; } } if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){ if(pow_y>0){ *y = 0; data.lnStop[Y_LINE]=1; } else{ *y = LineReturn[4]; data.lnStop[Y_LINE]=0; } } if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){ *y = 0; data.lnStop[Y_LINE]=0; } } else{//data.FieldState==LINE_INSIDE *x = 0; *y = 0; //line間際の復帰力以外の力を作用させるか否か data.lnStop[X_LINE]=1; data.lnStop[Y_LINE]=1; } } void LineJudgeReset(void){ uint8_t LineState; //line if(data.lnHold==7){ if(data.lnRaw>0){ //場外間際...減衰 } else{//data.lnRaw==0 //場外...出力完全無効+反発 if( (data.ping[L_PING]<OutToWall[X_PING])|| (data.ping[R_PING]<OutToWall[X_PING])|| (data.ping[F_PING]<OutToWall[Y_PING])|| (data.ping[B_PING]<OutToWall[Y_PING]) ){ LineLiberate(); data.FieldState=LINE_OUTSIDE; } if( (data.ping[L_PING]>=WhiteToWall[X_PING])&& (data.ping[R_PING]>=WhiteToWall[X_PING]) ){ LineLiberate(); data.FieldState=LINE_INSIDE; } } } else if(data.lnHold>0){ if(data.lnRaw>0){ //踏んでるけどまだ出てない...減衰 //timeout&reset...detach } else{//data.lnRaw==0 //線をまたいでいるか,中途半端に線を踏んだあと復帰したか...減衰 //if((data.ping[L_PING]>40)&&(data.ping[R_PING]>40)) LineLiberate(); //ping&reset //timeout&reset } } //else if(data.lnHold==0){...maxpower if((data.ping[R_PING]>WhiteToWall[X_PING])&&(data.ping[L_PING]>WhiteToWall[X_PING])) LineLiberate(); } void modeAttack4(void){ double ir_x_dir, ir_y_dir; double ir_x_turn, ir_y_turn; double ir_x, ir_y; double pow_x, pow_y; uint8_t ir_pow; int vx,vy,vs; if(sys.KickOffFlag==1){ sys.IrBlind=0; sys.LineBlind=0; sys.PingBlind=0; data.FieldState=LINE_INSIDE; sys.KickOffFlag=0; } //data if(sys.IrFlag==1){ReadIr();sys.IrFlag=0;} if(sys.PidFlag==1){PidUpdate();sys.PidFlag=0;} if(sys.UswFlag==1){ReadPing();sys.UswFlag=0;} if(sys.UswFlag2==1){ReadPing2();sys.UswFlag2=0;} //if(sys.KickFlag==1){DriveSolenoid();} data.lnRaw = LineRaw; data.lnHold = LineHold; ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR]; ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR]; ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN]; ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN]; if(data.irNotice==IR_CLOSER){ ir_pow = sys.s_pow; } else if(data.irNotice==IR_CLOSE){ ir_pow = sys.m_pow; } else if(data.irNotice==IR_FAR){ ir_pow = sys.l_pow; } else{//data.irNotice==IR_NONE ir_pow = 0; } if(sys.IrBlind==1) data.irNotice=IR_NONE; ir_x = (ir_x_dir + ir_x_turn); ir_y = (ir_y_dir + ir_y_turn); vx = ir_pow*ir_x; vy = ir_pow*ir_y; vs = cmps_set.OutputPID; move( vx, vy, vs ); if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;} if(sys.stopflag==1){ //停止処理 } return; } void modeAttack5(void){ if(sys.IrFlag==1){ ReadIr(); sys.IrFlag=0; } if(sys.PidFlag==1){ PidUpdate(); sys.PidFlag=0; } move(0,0,cmps_set.OutputPID); if(sys.MotorFlag==1){ LED[0] = 1; LED[1] = 0; tx_motor(); sys.MotorFlag=0; } else{ LED[0] = 0; LED[1] = 1; } if(sys.stopflag==1){ //停止処理 } return; }