CatPot 2015-2016
ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 30:5998ba42237e
- Parent:
- 29:e8bafe48aa90
- Child:
- 31:745a775cfc20
--- a/main_processing/strategy/strategy.cpp Tue Mar 22 07:01:10 2016 +0000
+++ b/main_processing/strategy/strategy.cpp Wed Mar 23 11:25:22 2016 +0000
@@ -17,6 +17,7 @@
////初期値を決める等
if(sys.KickOffFlag==1){
+ /*
///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
sys.IrBlind=0;
sys.LineBlind=0;
@@ -24,23 +25,31 @@
sys.HomeBlind=1;
sys.DriBlind=1;
+ //defence
+ sys.DefenceFlag=0;
+ */
+
+
//Kick
sys.KickStopFlag=0;
//Ir
sys.ir_pow_table = 0;
//Line
- data.lnCorner[L_LINE]=data.lnCorner[R_LINE]=data.lnCorner[F_LINE]=data.lnCorner[B_LINE]=LINE_EMPTY;
+ //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.lnStayNow[X_LINE]=data.lnStayNow[Y_LINE]=0;
data.lnStop[X_LINE]=data.lnStop[Y_LINE]=1;
- data.FieldSpot = LINE_INSIDE;
- LineLiberate();
- LineRankClear();
+
+ //data.FieldSpot = LINE_INSIDE;
+ //LineLiberate();
+ //LineRankClear();
LineRawRankClear();
- data.lnRawMemory[A_SPOT]=0;
- data.lnRawMemory[B_SPOT]=0;
- data.lnRawMemory[C_SPOT]=0;
+
+ //data.lnRawMemory[A_SPOT]=0;
+ //data.lnRawMemory[B_SPOT]=0;
+ //data.lnRawMemory[C_SPOT]=0;
data.lnRawReturn=0;
@@ -52,6 +61,7 @@
sys.TurnStopFlag=0;
cmps_set.AtkDeg=0;
cmps_set.HoldDeg=0;
+ cmps_set.GoalDeg=0;
//ドリブラー
sys.BallHoldJudgeFlag=0;
sys.BallHoldFlag=0;
@@ -75,38 +85,167 @@
sys.HomeStayFlag[Y_PING]=0;
}
//回り込みの値を代入
- if((data.ping[B_PING]<=30)&&0){
- 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];
+ if(sys.DefenceFlag==1){
+
+ if((data.irNotice==IR_NONE)||(data.irNotice==IR_FAR)){
+
+ if((data.ping[L_PING]>=50)&&(data.ping[R_PING]>=50)){
+ ir_x_dir = ir_move_val[5][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_x_turn = ir_move_val[5][data.irNotice][data.irPosition][IR_X_TURN];
+
+ if(data.ping[B_PING]>=10){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+ }
+ else if((data.ping[L_PING]>=50)&&(data.ping[R_PING]<50)){
+ ir_x_dir = -1;
+ ir_x_turn = 0;
+
+ if(data.ping[B_PING]>=45){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+ }
+ else if((data.ping[L_PING]<50)&&(data.ping[R_PING]>=50)){
+ ir_x_dir = 1;
+ ir_x_turn = 0;
+
+ if(data.ping[B_PING]>=45){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+ }
+ else if((data.ping[L_PING]<50)&&(data.ping[R_PING]<50)){
+ ir_x_dir = 0;
+ ir_x_turn = 0;
+
+ if(data.ping[B_PING]>=45){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+ }
+ }
+ else{
+
+ ir_x_dir = ir_move_val[5][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_x_turn = ir_move_val[5][data.irNotice][data.irPosition][IR_X_TURN];
+
+ if((data.ping[B_PING]>=5)&&(data.ping[R_PING]>=50)&&(data.ping[L_PING]>=50)){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+ }
+ /*
+ if((data.ping[L_PING]>=50)&&(data.ping[R_PING]>=50)&&(data.ping[B_PING]>10)){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+
+ if(data.ping[B_PING]>=10){
+ ir_y_dir = -1;
+ ir_y_turn = 0;
+ }
+ else{
+ ir_y_dir = 0;
+ ir_y_turn = 0;
+ }
+
+ ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
+ }
+ else{
+
+ }
+ */
+ /*
+ if((data.ping[B_PING]>=45)&&(data.irNotice!=IR_NONE)&&(15<=data.irPosition)&&(data.irPosition<=19)){
+ //sys.ir_pow_table=1;
+ ir_x_dir = 0;
+ ir_y_dir = -1;
+ ir_x_turn = 0;
+ ir_y_turn = 0;
+ }
+ else{
+ if(data.irValPhase[IR_SHORT]>=DIS_4){
+ sys.ir_pow_table=2;//直進
+ ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
+ ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
+ }
+ else{
+ if(data.irValPhase[IR_SHORT]<=DIS_1){
+ sys.ir_pow_table=0;//3
+ 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];
+ }
+ 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];
+ }
+ }
+ }*/
}
else{
- if(data.irValPhase[IR_SHORT]>=DIS_4){
- sys.ir_pow_table=2;
- ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
- ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
- ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
- ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
+ if((data.ping[B_PING]>40)&&(0)){
+ 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];
}
else{
- if(data.irValPhase[IR_SHORT]<=DIS_1){
- 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];
+ if((data.ping[B_PING]>60)&&(data.irValPhase[IR_SHORT]>=DIS_4)){
+ sys.ir_pow_table=2;//直進
+ ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
+ ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
+ ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
+ ir_y_turn = ir_move_val[2][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];
+ if(data.irValPhase[IR_SHORT]<=DIS_1){
+ sys.ir_pow_table=0;//3
+ 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];
+ }
+ 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の検出値によって出力を調整
if(data.irNotice==IR_CLOSER){
//ir_pow = sys.s_pow;
@@ -124,13 +263,50 @@
//ir_pow = 0;
ir_pow_x = ir_pow_y = 0;
}
+
+ if(sys.DefenceFlag==1){
+ ir_pow_x = 30;
+ ir_pow_y = 30;
+ if(data.ping[L_PING]<=60){
+ ir_pow_y = 30;
+ }
+ if(data.ping[L_PING]<=40){
+ ir_pow_y = 25;
+ }
+ if(data.ping[L_PING]<=30){
+ ir_pow_y = 20;
+ }
+ if(data.ping[L_PING]<15){
+ ir_pow_y = 15;
+ }
+ if(data.ping[L_PING]<10){
+ ir_pow_y = 10;
+ }
+ }
////ドリブラー
//ホールド判定
JudgeBallHolding();
//ドリブラー駆動
+ if((sys.DriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){
+ if(
+ ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))||
+ ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))
+ ){
+ cmps_set.GoalDeg=0;
+ }
+ else if((data.lnRawOrderLog1[0]==A_SPOT)&&(1)){
+ cmps_set.GoalDeg=-30;
+ }
+ else if((data.lnRawOrderLog1[0]==B_SPOT)&&(1)){
+ cmps_set.GoalDeg=30;
+ }
+ else{
+ cmps_set.GoalDeg=0;
+ }
+ }
if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
sys.DribbleFlag=1;
- if(sys.BallHoldFlag==1){
+ if((sys.BallHoldFlag==1)||(data.ball==1)){
//ir_pow=20;
ir_pow_x = ir_pow_y = 20;
}
@@ -148,6 +324,28 @@
DriveTurn();
DriveSolenoid();
}*/
+ if(
+ /*(sys.BallHoldFlag==1)&&
+ (data.ball==1)&&
+ (data.irNotice==IR_CLOSER)&&
+ ((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))&&
+ (data.ping[B_PING]>100)&&
+ (
+ ((data.ping[L_PING]<60)&&(data.ping[R_PING]>60))||
+ ((data.ping[L_PING]>60)&&(data.ping[R_PING]<60))||
+ (data.lnRawOrderLog1[0]==A_SPOT)||
+ (data.lnRawOrder[0]==A_SPOT)||
+ (data.lnRawOrderLog1[0]==B_SPOT)||
+ (data.lnRawOrder[0]==B_SPOT)
+ )&&
+ (data.lnRaw==0)*/
+ (sys.DriBlind==0)&&
+ (data.ping[R_PING]<30)&&
+ (data.ping[L_PING]<30)
+ ){
+ DriveTurn();
+ DriveSolenoid();
+ }
//Irの細かい補正値の処理．基本的にLineの補正値を演算する処理が始まる前にIrのモーター出力値補正をすべて終わらせる．
//if(sys.IrBlind==1) ir_pow=0;
@@ -188,24 +386,15 @@
ir_x = (ir_x_dir + ir_x_turn);
ir_y = (ir_y_dir + ir_y_turn);
}
-
- ////超音波による減速
- /*if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){
- if(
- (
- (data.ping[L_PING]<WhiteToWallPlus[X_PING])&&
- (data.ping[R_PING]>WhiteToWallPlus[X_PING])&&
- (ir_x>0)
- )||
- (
- (data.ping[L_PING]>WhiteToWallPlus[X_PING])&&
- (data.ping[R_PING]<WhiteToWallPlus[X_PING])&&
- (ir_x<0)
- )
- ){
-
- }
- }*/
+ if(
+ (sys.DefenceFlag==1)&&
+ (ir_y>0)&&
+ (data.ping[B_PING]>40)&&
+ (data.ping[B_PING]<225)&&
+ (sys.BackHomeFlag==0)
+ ){
+ ir_pow_y=0;
+ }
////Lineセンサーの値を評価しモーターの出力補正値を演算
if(sys.LineBlind==1){
@@ -234,13 +423,16 @@
//if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
//if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
//LED = ((data.lnRawMemory[0]!=0)<<2) | ((data.lnRawMemory[1]!=0)<<1) | ((data.lnRawMemory[2]!=0)<<0);
- LED = ((data.lnRawOrder[0]!=LINE_EMPTY)<<2) | ((data.lnRawOrder[1]!=LINE_EMPTY)<<1) | ((data.lnRawOrder[2]!=LINE_EMPTY)<<0);
+ //LED = ((data.lnRawOrder[0]!=LINE_EMPTY)<<2) | ((data.lnRawOrder[1]!=LINE_EMPTY)<<1) | ((data.lnRawOrder[2]!=LINE_EMPTY)<<0);
//LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
//LED = sys.BallHoldFlag*15;
//if(sys.BallHoldFlag==1) LED=15;
//else LED=10;
+ if(data.lnRepeat>=1) LED=15;
+ else LED=10;
+
//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);
@@ -266,28 +458,24 @@
}
return;
}
+
void modeAttack5(void){
- /*if(sys.IrFlag==1){
- ReadIr();
- sys.IrFlag=0;
+ ////初期値を決める等
+ if(sys.KickOffFlag==1){
+
+ ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
+ sys.IrBlind=0;
+ sys.LineBlind=0;
+ sys.PingBlind=0;
+
+ sys.HomeBlind=1;
+ sys.DriBlind=1;
+ //defence
+ sys.DefenceFlag=1;
+
+
+ //初期値設定の終了
+ //sys.KickOffFlag=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;
+ modeAttack4();
}