ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
main_processing/strategy/strategy.cpp
- Committer:
- lilac0112_1
- Date:
- 2016-03-16
- Revision:
- 23:df68f1a2c226
- Parent:
- 21:378470320524
- Child:
- 24:34ef6379b0df
File content as of revision 23:df68f1a2c226:
#include "mbed.h" #include "extern.h" //Atk void LineJudgeSlow(double pow_x, double pow_y, double *x, double *y){ uint8_t LineState; uint8_t LineSum; //line LineState = 0; LineSum = ((data.lnHold>>2)&0x1)+((data.lnHold>>1)&0x1)+((data.lnHold>>0)&0x1); if((LineSum==3)||(data.FieldSpot==LINE_OUTSIDE)){ LineState = 3; } else if(LineSum==2){ LineState = 2; } else if(LineSum==1){ LineState = 1; } else if(LineSum==0){ LineState = 0; } if(pow_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(pow_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, 20}; uint8_t LinePingState[4]; //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される. //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される. //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない.ボールを追う力は場内に出るまで作用する. //×ラインのほぼ場外では常時ラインから離れる力が優先される. //※場外判定を行うには再び場内に戻る必要がある. if(data.FieldSpot==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; data.ReturnDir[X_LINE]=LINE_EMPTY; data.ReturnDir[Y_LINE]=LINE_EMPTY; //x if(data.ReturnDir[X_LINE]==L_LINE){ if(pow_x<0){ *x = 0; data.lnStop[X_LINE]=1; } else{ *x = -LineReturn[4]; data.lnStop[X_LINE]=0; } } else if(data.ReturnDir[X_LINE]==R_LINE){ if(pow_x>0){ *x = 0; data.lnStop[X_LINE]=1; } else{ *x = LineReturn[4]; data.lnStop[X_LINE]=0; } } else if(data.ReturnDir[X_LINE]==LINE_EMPTY){ 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)){ data.ReturnDir[X_LINE]=L_LINE; /*if(pow_x<0){ *x = 0; data.lnStop[X_LINE]=1; } else{ *x = -LineReturn[4]; data.lnStop[X_LINE]=0; }*/ *x = -LineReturn[4]; data.lnStop[X_LINE]=0; } if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){ data.ReturnDir[X_LINE]=R_LINE; /*if(pow_x>0){ *x = 0; data.lnStop[X_LINE]=1; } else{ *x = LineReturn[4]; data.lnStop[X_LINE]=0; }*/ *x = LineReturn[4]; data.lnStop[X_LINE]=0; } if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){ if( (data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)&&(data.lnOrder[2]==B_SPOT) ){ *x = -LineReturn[4]; data.lnStop[X_LINE]=0; } else if( (data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)&&(data.lnOrder[2]==A_SPOT) ){ *x = LineReturn[4]; data.lnStop[X_LINE]=0; } else{ *x = 0; data.lnStop[X_LINE]=0; } } } //y if(data.ReturnDir[Y_LINE]==F_LINE){ if(pow_y>0){ *y = 0; data.lnStop[Y_LINE]=1; } else{ *y = LineReturn[4]; data.lnStop[Y_LINE]=0; } } else if(data.ReturnDir[Y_LINE]==B_LINE){ if(pow_y<0){ *y = 0; data.lnStop[Y_LINE]=1; } else{ *y = -LineReturn[4]; data.lnStop[Y_LINE]=0; } } else if(data.ReturnDir[Y_LINE]==LINE_EMPTY){ 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)){ data.ReturnDir[Y_LINE]=B_LINE; 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)){ data.ReturnDir[Y_LINE]=F_LINE; 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)){ if( (data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT)&&(data.lnOrder[2]==B_SPOT)|| (data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)&&(data.lnOrder[2]==A_SPOT) ){ *y = LineReturn[4]; data.lnStop[Y_LINE]=0; } else if( (data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT)&&(data.lnOrder[2]==C_SPOT)|| (data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)&&(data.lnOrder[2]==C_SPOT) ){ *y = -LineReturn[4]; data.lnStop[Y_LINE]=0; } else{ *y = 0; data.lnStop[Y_LINE]=0; } } } //none if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)&&(LinePingState[F_PING]==1)&&(LinePingState[B_PING]==1)){ //turn! } } else{//data.FieldSpot==LINE_INSIDE data.ReturnDir[X_LINE]=LINE_EMPTY; data.ReturnDir[Y_LINE]=LINE_EMPTY; *x = 0; *y = 0; //line間際の復帰力以外の力を作用させるか否か data.lnStop[X_LINE]=1; data.lnStop[Y_LINE]=1; } } void LineJudgeReset(double pow_x, double pow_y, double *x, double *y){ //static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY}; //static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY}; if((/*data.lnRaw==0*/1)&&(data.lnHold==7)){ if(data.FieldSpot==LINE_INSIDE){ data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]); data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]); data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]); data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]); if( (data.lnCorner[L_LINE])|| (data.lnCorner[R_LINE])|| (data.lnCorner[F_LINE])|| (data.lnCorner[B_LINE]) ){ data.NonWall[L_LINE] = (data.ping[L_PING]>WhiteToWall[X_PING]); data.NonWall[R_LINE] = (data.ping[R_PING]>WhiteToWall[X_PING]); data.NonWall[F_LINE] = (data.ping[F_PING]>WhiteToWall[Y_PING]); data.NonWall[B_LINE] = (data.ping[B_PING]>WhiteToWall[Y_PING]); /*LastLineCorner[L_LINE]=NewLineCorner[L_LINE]; LastLineCorner[R_LINE]=NewLineCorner[R_LINE]; LastLineCorner[F_LINE]=NewLineCorner[F_LINE]; LastLineCorner[B_LINE]=NewLineCorner[B_LINE]; NewLineCorner[L_LINE]=data.lnCorner[L_LINE]; NewLineCorner[R_LINE]=data.lnCorner[R_LINE]; NewLineCorner[F_LINE]=data.lnCorner[F_LINE]; NewLineCorner[B_LINE]=data.lnCorner[B_LINE]; if( (LastLineCorner[L_LINE]==NewLineCorner[L_LINE])&& (LastLineCorner[R_LINE]==NewLineCorner[R_LINE])&& (LastLineCorner[F_LINE]==NewLineCorner[F_LINE])&& (LastLineCorner[B_LINE]==NewLineCorner[B_LINE]) ){ data.lnRepeat++; } else{ data.lnRepeat=0; }*/ data.FieldSpot = LINE_OUTSIDE; LineLiberate(); } } else if(data.FieldSpot==LINE_OUTSIDE){ if(data.lnRaw==0){ data.FieldSpot = LINE_INSIDE; data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; LineLiberate(); LineRankClear(); } } } if(data.FieldSpot == LINE_OUTSIDE){ if( ( (data.ping[L_PING]>=WhiteToWall[X_PING])|| (data.NonWall[L_LINE]==1)|| ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)) //(data.lnOrder[0]==A_SPOT) )&& ( (data.ping[R_PING]>=WhiteToWall[X_PING])|| (data.NonWall[R_LINE]==1)|| ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)) //(data.lnOrder[0]==B_SPOT) )&& ( (data.ping[F_PING]>=WhiteToWall[Y_PING])|| (data.NonWall[F_LINE]==1)|| ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))|| ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)) )&& ((data.ping[B_PING]>=WhiteToWall[Y_PING])|| (data.NonWall[B_LINE]==1)|| ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))|| ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)) ) ){ data.FieldSpot = LINE_INSIDE; data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; LineLiberate(); LineRankClear(); } } if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){ if( ( (data.ping[L_PING]>=WhiteToWall[X_PING])|| ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)) //(data.lnOrder[0]==A_SPOT) )&& ( (data.ping[R_PING]>=WhiteToWall[X_PING])|| ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)) //(data.lnOrder[0]==B_SPOT) )&& ( (data.ping[F_PING]>=WhiteToWall[Y_PING])|| ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))|| ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)) )&& ((data.ping[B_PING]>=WhiteToWall[Y_PING])|| ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))|| ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)) ) /*|| ( (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&& (data.ping[R_PING]>=GoalEdgeToWall[X_PING]) )*/ ){ data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; LineLiberate(); } } /* if( (data.irNotice==IR_NONE)|| (data.irNotice==IR_FAR)|| ( (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&& (data.ping[R_PING]>=GoalEdgeToWall[X_PING]) ) ){ data.lnRepeat = 0; data.lnCorner[L_LINE]=LINE_EMPTY; data.lnCorner[R_LINE]=LINE_EMPTY; data.lnCorner[F_LINE]=LINE_EMPTY; data.lnCorner[B_LINE]=LINE_EMPTY; } data.lnRepeat=0; if(data.lnRepeat>0){ if( ((pow_x>=0)&&(data.lnCorner[R_LINE]))|| ((pow_x<0)&&(data.lnCorner[L_LINE])) ){ data.lnStay[X_LINE]=0; } else{ data.lnStay[X_LINE]=1; } if( ((pow_y>=0)&&(data.lnCorner[F_LINE]))|| ((pow_y<0)&&(data.lnCorner[B_LINE])) ){ data.lnStay[Y_LINE]=0; } else{ data.lnStay[Y_LINE]=1; } } else{ data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1; }*/ } void modeAttack4(void){ double ir_x_dir, ir_y_dir; double ir_x_turn, ir_y_turn; double ir_x, ir_y; double LineSlowPower[2]; double LineReturnPower[2]; //double pow_x, pow_y; uint8_t ir_pow; int vx,vy,vs; //static uint8_t data.FieldSpot; if(sys.KickOffFlag==1){ sys.IrBlind=0; sys.LineBlind=0; sys.PingBlind=0; sys.HomeBlind=1; sys.DriBlind=1; sys.ir_pow_table = 0; data.lnCorner[L_LINE]=data.lnCorner[R_LINE]=data.lnCorner[F_LINE]=data.lnCorner[B_LINE]=LINE_EMPTY; data.lnRepeat = 0; //data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1; data.lnStop[X_LINE]=data.lnStop[Y_LINE]=1; data.FieldSpot = LINE_INSIDE; LineLiberate(); LineRankClear(); cmps_set.AtkDeg=0; //sys.BallHoldJudgeFlag=0; //sys.BallHoldFlag=0; sys.KickOffFlag=0; } //data if(sys.InfoFlag==1){ReadInfo();sys.InfoFlag=0;} data.lnRaw = LineRaw; data.lnHold = LineHold; data.ball = ReadBall(); /* if(sys.DriBlind==0){ if((data.ball==1)&&(sys.BallHoldJudgeFlag==0)){ sys.BallHoldJudgeFlag=1; Ball_catch.attach(&AdmitBallHold, 1.0); } if((data.ball==0)&&(sys.BallHoldFlag==1)&&(sys.BallHoldGiveUpFlag==0)){ sys.BallHoldGiveUpFlag=1; Ball_leave.attach(&GiveUpBallHold, 1.0); } if((data.ball==1)&&(sys.BallHoldFlag==1)){ sys.BallHoldGiveUpFlag=0; Ball_leave.detach(); } if((data.ping[B_PING]<30)&&(sys.BallHoldFlag==1)){ cmps_set.AtkDeg = 150; Kick_now.attach(&DriveSolenoid, .125); Front_now.attach(&FaceToFront, .75); sys.BallHoldJudgeFlag=0; sys.BallHoldFlag=0; Ball_catch.detach(); Ball_leave.detach(); } } else{ sys.BallHoldFlag=0; }*/ if(data.irNotice==IR_NONE){ sys.BackHomeFlag=(sys.HomeBlind==0); } else{ sys.BackHomeFlag=0; } if(data.ping[B_PING]<=30){ sys.ir_pow_table=1; ir_x_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_X_DIR]; ir_y_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_DIR]; ir_x_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_X_TURN]; ir_y_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_TURN]; } else{ sys.ir_pow_table=0; 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]; } /* ir_x_dir = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_X_DIR]; ir_y_dir = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_Y_DIR]; ir_x_turn = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_X_TURN]; ir_y_turn = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_Y_TURN]; */ /* 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.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&(data.irValPhase[IR_SHORT]==DIS_0)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){ sys.DribbleFlag=1; //if(sys.BallHoldFlag==0){ ir_pow=20; //} /*if(cmps_set.AtkDeg==0){ if(data.irPosition==10) cmps_set.AtkDeg=30; if(data.irPosition==11) cmps_set.AtkDeg=0; if(data.irPosition==12) cmps_set.AtkDeg=-30; } else if(cmps_set.AtkDeg>0){ if(data.irPosition==10) cmps_set.AtkDeg=0; if(data.irPosition==11) cmps_set.AtkDeg=30; if(data.irPosition==12) cmps_set.AtkDeg=0; } else if(cmps_set.AtkDeg<0){ if(data.irPosition==10) cmps_set.AtkDeg=0; if(data.irPosition==11) cmps_set.AtkDeg=-30; if(data.irPosition==12) cmps_set.AtkDeg=0; }*/ } else{ sys.DribbleFlag=0; cmps_set.AtkDeg=0; /*sys.BallHoldJudgeFlag=0; sys.BallHoldFlag=0; Ball_catch.detach(); Ball_leave.detach();*/ } if(sys.IrBlind==1) ir_pow=0; if((data.ping[R_PING]<data.ping[L_PING])&&(data.irPosition==17)){ ir_x_turn = -ir_x_turn; ir_y_turn = -ir_y_turn; } ir_x = (ir_x_dir + ir_x_turn); ir_y = (ir_y_dir + ir_y_turn); if(sys.LineBlind==1){ LineSlowPower[X_LINE] = 1.0; LineSlowPower[Y_LINE] = 1.0; LineReturnPower[X_LINE] = 0.0; LineReturnPower[Y_LINE] = 0.0; data.lnStop[X_LINE] = 1; data.lnStop[Y_LINE] = 1; data.FieldSpot = LINE_INSIDE; } else{ LineJudgeReset(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]); LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]); LineJudgeReturn(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]); /* LineSlowPower[X_LINE] = 1.0; LineSlowPower[Y_LINE] = 1.0; LineReturnPower[X_LINE] = 0.0; LineReturnPower[Y_LINE] = 0.0; data.lnStop[X_LINE] = 1; data.lnStop[Y_LINE] = 1; */ } //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9; //if(data.FieldSpot==LINE_INSIDE) LED = 0x6; //LED = 0xFF*sys.BallHoldFlag; //LED = 0xFF*(data.ping[B_PING]<30); //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0); //else LED = 0xA; //LED = LineHold; vx = (ir_pow*ir_x)*data.lnStop[X_LINE]*(/*data.lnStay[X_LINE]*/1)*LineSlowPower[X_LINE] + LineReturnPower[X_LINE]; vy = (ir_pow*ir_y)*data.lnStop[Y_LINE]*(/*data.lnStay[X_LINE]*/1)*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE]; 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; }