![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
main_processing/strategy/strategy.cpp
- Committer:
- lilac0112_1
- Date:
- 2016-02-04
- Revision:
- 40:f68474b1d5e7
- Parent:
- 38:a7eb15b1f813
- Child:
- 41:3cdcdec9c885
File content as of revision 40:f68474b1d5e7:
#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(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 *= (double)(data.l_pow); ir_y *= (double)(data.l_pow); } else{ ir_x *= (double)(data.s_pow); ir_y *= (double)(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]; //Ir /* //ForLineAll data.motorlog[X_AXIS] *= LineStop[X_AXIS]; data.motorlog[Y_AXIS] *= LineStop[Y_AXIS]; data.motorlog[X_AXIS] += vx; data.motorlog[Y_AXIS] += vy; */ 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; } uint8_t LineReverseFlag; void LineReverse(void){ LineReverseFlag=0; } uint8_t LineSign[3]; uint8_t LineFirst[2]; void LineClear_A(void){LineSign[A_SPOT] = 0;} void LineClear_B(void){LineSign[B_SPOT] = 0;} void LineClear_C(void){LineSign[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; } } Line_timeout[A_SPOT].attach(&LineClear_A, 0.5); } 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; } } Line_timeout[B_SPOT].attach(&LineClear_B, 0.5); } void LineCall_C(void){ LineSign[C_SPOT] = 1; if(!((LineSign[A_SPOT]==1)&&(LineSign[B_SPOT]==1))){ LineFirst[Y_AXIS] = C_SPOT; } Line_timeout[C_SPOT].attach(&LineClear_C, 0.5); } 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(data.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.KickOffFlag=0; } 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))&&(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 = data.OutputPID; if((LineBind[A_SPOT]==1)||(LineBind[B_SPOT]==1)||(LineBind[C_SPOT]==1)||(LineBind[AB_SPOT]==1)){ 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]); /*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, 0.5); Line_timeout[B_SPOT].attach(&LineClear_B, 0.5); Line_timeout[C_SPOT].attach(&LineClear_C, 0.5);*/ } else{ vx=0; vy=0; } } move( vx, vy, vs ); if(data.MotorFlag==1){ tx_motor(); data.MotorFlag=0; } return; } void modeAttack2(void){ if(data.IrFlag==1){ ReadIr(); data.IrFlag=0; } if(data.PidFlag==1){ PidUpdate(); data.PidFlag=0; } move(0,0,10); if(data.MotorFlag==1){ tx_motor(); data.MotorFlag=0; } return; } void modeAttack3(void){ if(data.KickFlag==1){ DriveSolenoid(); } 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; } 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; } 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; 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)&&(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 ); if(data.MotorFlag==1){ tx_motor(); data.MotorFlag=0; } return; } void modeDebug4(void){//solenoid if(data.KickFlag==1){ DriveSolenoid(); } return; } 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; }