CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@32:367b16d69a32, 2016-03-24 (annotated)

Committer:
lilac0112_1
Date:
Thu Mar 24 04:54:39 2016 +0000
Revision:
32:367b16d69a32
Parent:
31:745a775cfc20
Child:
33:aa115c30892e
turn start are renewed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:ea35c18c85fc 1 #include "mbed.h"
lilac0112_1 0:ea35c18c85fc 2 #include "extern.h"
lilac0112_1 0:ea35c18c85fc 3
lilac0112_1 0:ea35c18c85fc 4 //Atk
lilac0112_1 0:ea35c18c85fc 5 void modeAttack4(void){
lilac0112_1 24:34ef6379b0df 6 //irの値に影響するモーター補正値
lilac0112_1 25:a7460e23e02e 7 //uint8_t ir_pow;
lilac0112_1 25:a7460e23e02e 8 uint8_t ir_pow_x, ir_pow_y;
lilac0112_1 10:6df631c39f9b 9 double ir_x_dir, ir_y_dir;
lilac0112_1 11:3efae754e6ef 10 double ir_x_turn, ir_y_turn;
lilac0112_1 11:3efae754e6ef 11 double ir_x, ir_y;
lilac0112_1 24:34ef6379b0df 12 //lineの値に影響するモーター補正値
lilac0112_1 14:b510adcb6065 13 double LineSlowPower[2];
lilac0112_1 14:b510adcb6065 14 double LineReturnPower[2];
lilac0112_1 24:34ef6379b0df 15 //モーターの出力値
lilac0112_1 10:6df631c39f9b 16 int vx,vy,vs;
lilac0112_1 11:3efae754e6ef 17
lilac0112_1 24:34ef6379b0df 18 ////初期値を決める等
lilac0112_1 10:6df631c39f9b 19 if(sys.KickOffFlag==1){
lilac0112_1 30:5998ba42237e 20 /*
lilac0112_1 24:34ef6379b0df 21 ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
lilac0112_1 27:825c6835e3db 22 sys.IrBlind=0;
lilac0112_1 10:6df631c39f9b 23 sys.LineBlind=0;
lilac0112_1 10:6df631c39f9b 24 sys.PingBlind=0;
lilac0112_1 10:6df631c39f9b 25
lilac0112_1 26:6ca88eeaa2b4 26 sys.HomeBlind=1;
lilac0112_1 26:6ca88eeaa2b4 27 sys.DriBlind=1;
lilac0112_1 30:5998ba42237e 28 //defence
lilac0112_1 30:5998ba42237e 29 sys.DefenceFlag=0;
lilac0112_1 30:5998ba42237e 30 */
lilac0112_1 30:5998ba42237e 31
lilac0112_1 30:5998ba42237e 32
lilac0112_1 24:34ef6379b0df 33 //Kick
lilac0112_1 24:34ef6379b0df 34 sys.KickStopFlag=0;
lilac0112_1 24:34ef6379b0df 35 //Ir
lilac0112_1 18:3a42a931c95a 36 sys.ir_pow_table = 0;
lilac0112_1 24:34ef6379b0df 37 //Line
lilac0112_1 30:5998ba42237e 38 //data.lnCorner[L_LINE]=data.lnCorner[R_LINE]=data.lnCorner[F_LINE]=data.lnCorner[B_LINE]=LINE_EMPTY;
lilac0112_1 18:3a42a931c95a 39 data.lnRepeat = 0;
lilac0112_1 24:34ef6379b0df 40 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 41 data.lnStayNow[X_LINE]=data.lnStayNow[Y_LINE]=0;
lilac0112_1 18:3a42a931c95a 42 data.lnStop[X_LINE]=data.lnStop[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 43
lilac0112_1 30:5998ba42237e 44 //data.FieldSpot = LINE_INSIDE;
lilac0112_1 30:5998ba42237e 45 //LineLiberate();
lilac0112_1 30:5998ba42237e 46 //LineRankClear();
lilac0112_1 27:825c6835e3db 47
lilac0112_1 27:825c6835e3db 48 LineRawRankClear();
lilac0112_1 30:5998ba42237e 49
lilac0112_1 30:5998ba42237e 50 //data.lnRawMemory[A_SPOT]=0;
lilac0112_1 30:5998ba42237e 51 //data.lnRawMemory[B_SPOT]=0;
lilac0112_1 30:5998ba42237e 52 //data.lnRawMemory[C_SPOT]=0;
lilac0112_1 27:825c6835e3db 53
lilac0112_1 27:825c6835e3db 54 data.lnRawReturn=0;
lilac0112_1 27:825c6835e3db 55
lilac0112_1 27:825c6835e3db 56 LineRawLogReset();
lilac0112_1 25:a7460e23e02e 57 //backhome
lilac0112_1 25:a7460e23e02e 58 sys.HomeStayFlag[X_PING]=0;
lilac0112_1 25:a7460e23e02e 59 sys.HomeStayFlag[Y_PING]=0;
lilac0112_1 24:34ef6379b0df 60 //pid
lilac0112_1 24:34ef6379b0df 61 sys.TurnStopFlag=0;
lilac0112_1 25:a7460e23e02e 62 cmps_set.AtkDeg=0;
lilac0112_1 25:a7460e23e02e 63 cmps_set.HoldDeg=0;
lilac0112_1 30:5998ba42237e 64 cmps_set.GoalDeg=0;
lilac0112_1 24:34ef6379b0df 65 //ドリブラー
lilac0112_1 25:a7460e23e02e 66 sys.BallHoldJudgeFlag=0;
lilac0112_1 25:a7460e23e02e 67 sys.BallHoldFlag=0;
lilac0112_1 19:967207de919d 68
lilac0112_1 24:34ef6379b0df 69 //初期値設定の終了
lilac0112_1 10:6df631c39f9b 70 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 71 }
lilac0112_1 24:34ef6379b0df 72 ////DataRetrieve
lilac0112_1 19:967207de919d 73 if(sys.InfoFlag==1){ReadInfo();sys.InfoFlag=0;}
lilac0112_1 11:3efae754e6ef 74 data.lnRaw = LineRaw;
lilac0112_1 11:3efae754e6ef 75 data.lnHold = LineHold;
lilac0112_1 21:378470320524 76 data.ball = ReadBall();
lilac0112_1 19:967207de919d 77
lilac0112_1 24:34ef6379b0df 78 //ボールがなければ自分のゴールに戻る
lilac0112_1 24:34ef6379b0df 79 if(data.irNotice==IR_NONE){
lilac0112_1 24:34ef6379b0df 80 sys.BackHomeFlag=(sys.HomeBlind==0);
lilac0112_1 24:34ef6379b0df 81 }
lilac0112_1 24:34ef6379b0df 82 else{
lilac0112_1 24:34ef6379b0df 83 sys.BackHomeFlag=0;
lilac0112_1 25:a7460e23e02e 84 sys.HomeStayFlag[X_PING]=0;
lilac0112_1 25:a7460e23e02e 85 sys.HomeStayFlag[Y_PING]=0;
lilac0112_1 24:34ef6379b0df 86 }
lilac0112_1 24:34ef6379b0df 87 //回り込みの値を代入
lilac0112_1 30:5998ba42237e 88 if(sys.DefenceFlag==1){
lilac0112_1 30:5998ba42237e 89
lilac0112_1 30:5998ba42237e 90 if((data.irNotice==IR_NONE)||(data.irNotice==IR_FAR)){
lilac0112_1 30:5998ba42237e 91
lilac0112_1 30:5998ba42237e 92 if((data.ping[L_PING]>=50)&&(data.ping[R_PING]>=50)){
lilac0112_1 30:5998ba42237e 93 ir_x_dir = ir_move_val[5][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 30:5998ba42237e 94 ir_x_turn = ir_move_val[5][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 30:5998ba42237e 95
lilac0112_1 30:5998ba42237e 96 if(data.ping[B_PING]>=10){
lilac0112_1 30:5998ba42237e 97 ir_y_dir = -1;
lilac0112_1 30:5998ba42237e 98 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 99 }
lilac0112_1 30:5998ba42237e 100 else{
lilac0112_1 30:5998ba42237e 101 ir_y_dir = 0;
lilac0112_1 30:5998ba42237e 102 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 103 }
lilac0112_1 30:5998ba42237e 104 }
lilac0112_1 30:5998ba42237e 105 else if((data.ping[L_PING]>=50)&&(data.ping[R_PING]<50)){
lilac0112_1 30:5998ba42237e 106 ir_x_dir = -1;
lilac0112_1 30:5998ba42237e 107 ir_x_turn = 0;
lilac0112_1 30:5998ba42237e 108
lilac0112_1 30:5998ba42237e 109 if(data.ping[B_PING]>=45){
lilac0112_1 30:5998ba42237e 110 ir_y_dir = -1;
lilac0112_1 30:5998ba42237e 111 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 112 }
lilac0112_1 30:5998ba42237e 113 else{
lilac0112_1 30:5998ba42237e 114 ir_y_dir = 0;
lilac0112_1 30:5998ba42237e 115 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 116 }
lilac0112_1 30:5998ba42237e 117 }
lilac0112_1 30:5998ba42237e 118 else if((data.ping[L_PING]<50)&&(data.ping[R_PING]>=50)){
lilac0112_1 30:5998ba42237e 119 ir_x_dir = 1;
lilac0112_1 30:5998ba42237e 120 ir_x_turn = 0;
lilac0112_1 30:5998ba42237e 121
lilac0112_1 30:5998ba42237e 122 if(data.ping[B_PING]>=45){
lilac0112_1 30:5998ba42237e 123 ir_y_dir = -1;
lilac0112_1 30:5998ba42237e 124 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 125 }
lilac0112_1 30:5998ba42237e 126 else{
lilac0112_1 30:5998ba42237e 127 ir_y_dir = 0;
lilac0112_1 30:5998ba42237e 128 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 129 }
lilac0112_1 30:5998ba42237e 130 }
lilac0112_1 30:5998ba42237e 131 else if((data.ping[L_PING]<50)&&(data.ping[R_PING]<50)){
lilac0112_1 30:5998ba42237e 132 ir_x_dir = 0;
lilac0112_1 30:5998ba42237e 133 ir_x_turn = 0;
lilac0112_1 30:5998ba42237e 134
lilac0112_1 30:5998ba42237e 135 if(data.ping[B_PING]>=45){
lilac0112_1 30:5998ba42237e 136 ir_y_dir = -1;
lilac0112_1 30:5998ba42237e 137 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 138 }
lilac0112_1 30:5998ba42237e 139 else{
lilac0112_1 30:5998ba42237e 140 ir_y_dir = 0;
lilac0112_1 30:5998ba42237e 141 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 142 }
lilac0112_1 30:5998ba42237e 143 }
lilac0112_1 30:5998ba42237e 144 }
lilac0112_1 30:5998ba42237e 145 else{
lilac0112_1 30:5998ba42237e 146
lilac0112_1 30:5998ba42237e 147 ir_x_dir = ir_move_val[5][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 30:5998ba42237e 148 ir_x_turn = ir_move_val[5][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 30:5998ba42237e 149
lilac0112_1 30:5998ba42237e 150 if((data.ping[B_PING]>=5)&&(data.ping[R_PING]>=50)&&(data.ping[L_PING]>=50)){
lilac0112_1 30:5998ba42237e 151 ir_y_dir = -1;
lilac0112_1 30:5998ba42237e 152 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 153 }
lilac0112_1 30:5998ba42237e 154 else{
lilac0112_1 30:5998ba42237e 155 ir_y_dir = 0;
lilac0112_1 30:5998ba42237e 156 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 157 }
lilac0112_1 30:5998ba42237e 158 }
lilac0112_1 30:5998ba42237e 159 /*
lilac0112_1 30:5998ba42237e 160 if((data.ping[L_PING]>=50)&&(data.ping[R_PING]>=50)&&(data.ping[B_PING]>10)){
lilac0112_1 30:5998ba42237e 161 ir_y_dir = -1;
lilac0112_1 30:5998ba42237e 162 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 163
lilac0112_1 30:5998ba42237e 164 if(data.ping[B_PING]>=10){
lilac0112_1 30:5998ba42237e 165 ir_y_dir = -1;
lilac0112_1 30:5998ba42237e 166 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 167 }
lilac0112_1 30:5998ba42237e 168 else{
lilac0112_1 30:5998ba42237e 169 ir_y_dir = 0;
lilac0112_1 30:5998ba42237e 170 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 171 }
lilac0112_1 30:5998ba42237e 172
lilac0112_1 30:5998ba42237e 173 ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 30:5998ba42237e 174 ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 30:5998ba42237e 175 }
lilac0112_1 30:5998ba42237e 176 else{
lilac0112_1 30:5998ba42237e 177
lilac0112_1 30:5998ba42237e 178 }
lilac0112_1 30:5998ba42237e 179 */
lilac0112_1 30:5998ba42237e 180 /*
lilac0112_1 30:5998ba42237e 181 if((data.ping[B_PING]>=45)&&(data.irNotice!=IR_NONE)&&(15<=data.irPosition)&&(data.irPosition<=19)){
lilac0112_1 30:5998ba42237e 182 //sys.ir_pow_table=1;
lilac0112_1 30:5998ba42237e 183 ir_x_dir = 0;
lilac0112_1 30:5998ba42237e 184 ir_y_dir = -1;
lilac0112_1 30:5998ba42237e 185 ir_x_turn = 0;
lilac0112_1 30:5998ba42237e 186 ir_y_turn = 0;
lilac0112_1 30:5998ba42237e 187 }
lilac0112_1 30:5998ba42237e 188 else{
lilac0112_1 30:5998ba42237e 189 if(data.irValPhase[IR_SHORT]>=DIS_4){
lilac0112_1 30:5998ba42237e 190 sys.ir_pow_table=2;//直進
lilac0112_1 30:5998ba42237e 191 ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 30:5998ba42237e 192 ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 30:5998ba42237e 193 ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 30:5998ba42237e 194 ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 30:5998ba42237e 195 }
lilac0112_1 30:5998ba42237e 196 else{
lilac0112_1 30:5998ba42237e 197 if(data.irValPhase[IR_SHORT]<=DIS_1){
lilac0112_1 30:5998ba42237e 198 sys.ir_pow_table=0;//3
lilac0112_1 30:5998ba42237e 199 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 30:5998ba42237e 200 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 30:5998ba42237e 201 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 30:5998ba42237e 202 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 30:5998ba42237e 203 }
lilac0112_1 30:5998ba42237e 204 else{
lilac0112_1 30:5998ba42237e 205 sys.ir_pow_table=0;
lilac0112_1 30:5998ba42237e 206 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 30:5998ba42237e 207 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 30:5998ba42237e 208 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 30:5998ba42237e 209 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 30:5998ba42237e 210 }
lilac0112_1 30:5998ba42237e 211 }
lilac0112_1 30:5998ba42237e 212 }*/
lilac0112_1 24:34ef6379b0df 213 }
lilac0112_1 24:34ef6379b0df 214 else{
lilac0112_1 30:5998ba42237e 215 if((data.ping[B_PING]>40)&&(0)){
lilac0112_1 30:5998ba42237e 216 sys.ir_pow_table=0;
lilac0112_1 30:5998ba42237e 217 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 30:5998ba42237e 218 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 30:5998ba42237e 219 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 30:5998ba42237e 220 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 29:e8bafe48aa90 221 }
lilac0112_1 29:e8bafe48aa90 222 else{
lilac0112_1 30:5998ba42237e 223 if((data.ping[B_PING]>60)&&(data.irValPhase[IR_SHORT]>=DIS_4)){
lilac0112_1 30:5998ba42237e 224 sys.ir_pow_table=2;//直進
lilac0112_1 30:5998ba42237e 225 ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 30:5998ba42237e 226 ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 30:5998ba42237e 227 ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 30:5998ba42237e 228 ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 29:e8bafe48aa90 229 }
lilac0112_1 29:e8bafe48aa90 230 else{
lilac0112_1 30:5998ba42237e 231 if(data.irValPhase[IR_SHORT]<=DIS_1){
lilac0112_1 30:5998ba42237e 232 sys.ir_pow_table=0;//3
lilac0112_1 30:5998ba42237e 233 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 30:5998ba42237e 234 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 30:5998ba42237e 235 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 30:5998ba42237e 236 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 30:5998ba42237e 237 }
lilac0112_1 30:5998ba42237e 238 else{
lilac0112_1 30:5998ba42237e 239 sys.ir_pow_table=0;
lilac0112_1 30:5998ba42237e 240 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 30:5998ba42237e 241 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 30:5998ba42237e 242 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 30:5998ba42237e 243 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 30:5998ba42237e 244 }
lilac0112_1 29:e8bafe48aa90 245 }
lilac0112_1 29:e8bafe48aa90 246 }
lilac0112_1 32:367b16d69a32 247 if((sys.HomeBlind==0)){
lilac0112_1 32:367b16d69a32 248 if(((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER))&&(data.ping[B_PING]<60)&&
lilac0112_1 32:367b16d69a32 249 (
lilac0112_1 32:367b16d69a32 250 (data.irPosition==11)||
lilac0112_1 32:367b16d69a32 251 (data.irPosition==(ir_posi_s[(11-8+24+1)%12]))||
lilac0112_1 32:367b16d69a32 252 (data.irPosition==(ir_posi_s[(11-8+24-1)%12]))||
lilac0112_1 32:367b16d69a32 253 (data.irPosition==(ir_posi_s[(11-8+24+2)%12]))||
lilac0112_1 32:367b16d69a32 254 (data.irPosition==(ir_posi_s[(11-8+24-2)%12]))||
lilac0112_1 32:367b16d69a32 255 (data.irPosition==(ir_posi_s[(11-8+24+3)%12]))||
lilac0112_1 32:367b16d69a32 256 (data.irPosition==(ir_posi_s[(11-8+24-3)%12]))
lilac0112_1 32:367b16d69a32 257 )
lilac0112_1 32:367b16d69a32 258 ){
lilac0112_1 32:367b16d69a32 259 sys.ir_pow_table=2;//直進
lilac0112_1 32:367b16d69a32 260 ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 32:367b16d69a32 261 ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 32:367b16d69a32 262 ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 32:367b16d69a32 263 ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 32:367b16d69a32 264 }
lilac0112_1 32:367b16d69a32 265 }
lilac0112_1 24:34ef6379b0df 266 }
lilac0112_1 30:5998ba42237e 267
lilac0112_1 24:34ef6379b0df 268 //Irの検出値によって出力を調整
lilac0112_1 24:34ef6379b0df 269 if(data.irNotice==IR_CLOSER){
lilac0112_1 25:a7460e23e02e 270 //ir_pow = sys.s_pow;
lilac0112_1 25:a7460e23e02e 271 ir_pow_x = ir_pow_y = sys.s_pow;
lilac0112_1 24:34ef6379b0df 272 }
lilac0112_1 24:34ef6379b0df 273 else if(data.irNotice==IR_CLOSE){
lilac0112_1 25:a7460e23e02e 274 //ir_pow = sys.m_pow;
lilac0112_1 25:a7460e23e02e 275 ir_pow_x = ir_pow_y = sys.m_pow;
lilac0112_1 24:34ef6379b0df 276 }
lilac0112_1 24:34ef6379b0df 277 else if(data.irNotice==IR_FAR){
lilac0112_1 25:a7460e23e02e 278 //ir_pow = sys.l_pow;
lilac0112_1 25:a7460e23e02e 279 ir_pow_x = ir_pow_y = sys.l_pow;
lilac0112_1 24:34ef6379b0df 280 }
lilac0112_1 24:34ef6379b0df 281 else{//data.irNotice==IR_NONE
lilac0112_1 25:a7460e23e02e 282 //ir_pow = 0;
lilac0112_1 25:a7460e23e02e 283 ir_pow_x = ir_pow_y = 0;
lilac0112_1 24:34ef6379b0df 284 }
lilac0112_1 30:5998ba42237e 285
lilac0112_1 30:5998ba42237e 286 if(sys.DefenceFlag==1){
lilac0112_1 30:5998ba42237e 287 ir_pow_x = 30;
lilac0112_1 30:5998ba42237e 288 ir_pow_y = 30;
lilac0112_1 30:5998ba42237e 289 if(data.ping[L_PING]<=60){
lilac0112_1 30:5998ba42237e 290 ir_pow_y = 30;
lilac0112_1 30:5998ba42237e 291 }
lilac0112_1 30:5998ba42237e 292 if(data.ping[L_PING]<=40){
lilac0112_1 30:5998ba42237e 293 ir_pow_y = 25;
lilac0112_1 30:5998ba42237e 294 }
lilac0112_1 30:5998ba42237e 295 if(data.ping[L_PING]<=30){
lilac0112_1 30:5998ba42237e 296 ir_pow_y = 20;
lilac0112_1 30:5998ba42237e 297 }
lilac0112_1 30:5998ba42237e 298 if(data.ping[L_PING]<15){
lilac0112_1 30:5998ba42237e 299 ir_pow_y = 15;
lilac0112_1 30:5998ba42237e 300 }
lilac0112_1 30:5998ba42237e 301 if(data.ping[L_PING]<10){
lilac0112_1 30:5998ba42237e 302 ir_pow_y = 10;
lilac0112_1 30:5998ba42237e 303 }
lilac0112_1 30:5998ba42237e 304 }
lilac0112_1 24:34ef6379b0df 305 ////ドリブラー
lilac0112_1 24:34ef6379b0df 306 //ホールド判定
lilac0112_1 25:a7460e23e02e 307 JudgeBallHolding();
lilac0112_1 25:a7460e23e02e 308 //ドリブラー駆動
lilac0112_1 32:367b16d69a32 309 if((sys.TurnDriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){
lilac0112_1 32:367b16d69a32 310 if(sys.TurnHoldBlind==0){
lilac0112_1 32:367b16d69a32 311 if(
lilac0112_1 32:367b16d69a32 312 ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))||
lilac0112_1 32:367b16d69a32 313 ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))
lilac0112_1 32:367b16d69a32 314 ){
lilac0112_1 32:367b16d69a32 315 cmps_set.GoalDeg=0;
lilac0112_1 32:367b16d69a32 316 }
lilac0112_1 32:367b16d69a32 317 else if((data.lnRawOrderLog1[0]==A_SPOT)&&(1)){
lilac0112_1 32:367b16d69a32 318 cmps_set.GoalDeg=-30;
lilac0112_1 32:367b16d69a32 319 }
lilac0112_1 32:367b16d69a32 320 else if((data.lnRawOrderLog1[0]==B_SPOT)&&(1)){
lilac0112_1 32:367b16d69a32 321 cmps_set.GoalDeg=30;
lilac0112_1 32:367b16d69a32 322 }
lilac0112_1 32:367b16d69a32 323 else{
lilac0112_1 32:367b16d69a32 324 cmps_set.GoalDeg=0;
lilac0112_1 32:367b16d69a32 325 }
lilac0112_1 30:5998ba42237e 326 }
lilac0112_1 30:5998ba42237e 327 }
lilac0112_1 25:a7460e23e02e 328 if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 25:a7460e23e02e 329 sys.DribbleFlag=1;
lilac0112_1 30:5998ba42237e 330 if((sys.BallHoldFlag==1)||(data.ball==1)){
lilac0112_1 25:a7460e23e02e 331 //ir_pow=20;
lilac0112_1 25:a7460e23e02e 332 ir_pow_x = ir_pow_y = 20;
lilac0112_1 19:967207de919d 333 }
lilac0112_1 19:967207de919d 334 }
lilac0112_1 19:967207de919d 335 else{
lilac0112_1 19:967207de919d 336 sys.DribbleFlag=0;
lilac0112_1 25:a7460e23e02e 337 cmps_set.HoldDeg=0;
lilac0112_1 19:967207de919d 338 }
lilac0112_1 19:967207de919d 339
lilac0112_1 24:34ef6379b0df 340 ////Kick
lilac0112_1 25:a7460e23e02e 341 /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 24:34ef6379b0df 342 DriveTurn();
lilac0112_1 25:a7460e23e02e 343 }*/
lilac0112_1 25:a7460e23e02e 344 /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 25:a7460e23e02e 345 DriveTurn();
lilac0112_1 25:a7460e23e02e 346 DriveSolenoid();
lilac0112_1 25:a7460e23e02e 347 }*/
lilac0112_1 30:5998ba42237e 348 if(
lilac0112_1 30:5998ba42237e 349 /*(sys.BallHoldFlag==1)&&
lilac0112_1 30:5998ba42237e 350 (data.ball==1)&&
lilac0112_1 30:5998ba42237e 351 (data.irNotice==IR_CLOSER)&&
lilac0112_1 30:5998ba42237e 352 ((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))&&
lilac0112_1 30:5998ba42237e 353 (data.ping[B_PING]>100)&&
lilac0112_1 30:5998ba42237e 354 (
lilac0112_1 30:5998ba42237e 355 ((data.ping[L_PING]<60)&&(data.ping[R_PING]>60))||
lilac0112_1 30:5998ba42237e 356 ((data.ping[L_PING]>60)&&(data.ping[R_PING]<60))||
lilac0112_1 30:5998ba42237e 357 (data.lnRawOrderLog1[0]==A_SPOT)||
lilac0112_1 30:5998ba42237e 358 (data.lnRawOrder[0]==A_SPOT)||
lilac0112_1 30:5998ba42237e 359 (data.lnRawOrderLog1[0]==B_SPOT)||
lilac0112_1 30:5998ba42237e 360 (data.lnRawOrder[0]==B_SPOT)
lilac0112_1 30:5998ba42237e 361 )&&
lilac0112_1 30:5998ba42237e 362 (data.lnRaw==0)*/
lilac0112_1 30:5998ba42237e 363 (sys.DriBlind==0)&&
lilac0112_1 30:5998ba42237e 364 (data.ping[R_PING]<30)&&
lilac0112_1 30:5998ba42237e 365 (data.ping[L_PING]<30)
lilac0112_1 30:5998ba42237e 366 ){
lilac0112_1 32:367b16d69a32 367 //DriveTurn();
lilac0112_1 32:367b16d69a32 368 //DriveSolenoid();
lilac0112_1 30:5998ba42237e 369 }
lilac0112_1 19:967207de919d 370
lilac0112_1 24:34ef6379b0df 371 //Irの細かい補正値の処理．基本的にLineの補正値を演算する処理が始まる前にIrのモーター出力値補正をすべて終わらせる．
lilac0112_1 25:a7460e23e02e 372 //if(sys.IrBlind==1) ir_pow=0;
lilac0112_1 25:a7460e23e02e 373 if(sys.IrBlind==1) ir_pow_x=ir_pow_y=0;
lilac0112_1 28:f246a5fbf6a5 374 if((data.ping[R_PING]<data.ping[L_PING])&&(data.irPosition==17)&&(data.irNotice==IR_CLOSER)){
lilac0112_1 18:3a42a931c95a 375 ir_x_turn = -ir_x_turn;
lilac0112_1 18:3a42a931c95a 376 ir_y_turn = -ir_y_turn;
lilac0112_1 18:3a42a931c95a 377 }
lilac0112_1 25:a7460e23e02e 378
lilac0112_1 26:6ca88eeaa2b4 379 if((sys.BackHomeFlag==1)&&(sys.HomeBlind==0)){
lilac0112_1 25:a7460e23e02e 380 ir_pow_x = ir_pow_y = 25;
lilac0112_1 25:a7460e23e02e 381 //x
lilac0112_1 25:a7460e23e02e 382 if((abs(data.ping[L_PING]-data.ping[R_PING])>20)&&(sys.HomeStayFlag[X_PING]==0)){
lilac0112_1 25:a7460e23e02e 383 if(data.ping[L_PING]>data.ping[R_PING]){
lilac0112_1 25:a7460e23e02e 384 ir_x = -1;
lilac0112_1 25:a7460e23e02e 385 }
lilac0112_1 25:a7460e23e02e 386 else{
lilac0112_1 25:a7460e23e02e 387 ir_x = 1;
lilac0112_1 25:a7460e23e02e 388 }
lilac0112_1 25:a7460e23e02e 389 if((data.ping[L_PING]<WhiteToWallPlus[X_PING])&&(data.ping[R_PING]<WhiteToWallPlus[X_PING])){
lilac0112_1 25:a7460e23e02e 390 ir_x = 0;
lilac0112_1 25:a7460e23e02e 391 }
lilac0112_1 25:a7460e23e02e 392 }
lilac0112_1 25:a7460e23e02e 393 else{
lilac0112_1 25:a7460e23e02e 394 ir_x = 0;
lilac0112_1 25:a7460e23e02e 395 //sys.HomeStayFlag[X_PING]=1;
lilac0112_1 25:a7460e23e02e 396 }
lilac0112_1 25:a7460e23e02e 397 //y
lilac0112_1 31:745a775cfc20 398 if((data.ping[B_PING]>60)&&(1)&&(sys.HomeStayFlag[Y_PING]==0)){
lilac0112_1 25:a7460e23e02e 399 ir_y = -1;
lilac0112_1 25:a7460e23e02e 400 }
lilac0112_1 25:a7460e23e02e 401 else{
lilac0112_1 32:367b16d69a32 402 if((data.ping[B_PING]>40)&&(data.ping[R_PING]>55)&&(data.ping[L_PING]>55)){
lilac0112_1 32:367b16d69a32 403 ir_y = -1;
lilac0112_1 32:367b16d69a32 404 ir_pow_y = 15;
lilac0112_1 32:367b16d69a32 405 }
lilac0112_1 32:367b16d69a32 406 else{
lilac0112_1 32:367b16d69a32 407 ir_y = 0;
lilac0112_1 32:367b16d69a32 408 }
lilac0112_1 25:a7460e23e02e 409 //sys.HomeStayFlag[Y_PING]=1;
lilac0112_1 25:a7460e23e02e 410 }
lilac0112_1 25:a7460e23e02e 411 }
lilac0112_1 25:a7460e23e02e 412 else{
lilac0112_1 25:a7460e23e02e 413 ir_x = (ir_x_dir + ir_x_turn);
lilac0112_1 25:a7460e23e02e 414 ir_y = (ir_y_dir + ir_y_turn);
lilac0112_1 25:a7460e23e02e 415 }
lilac0112_1 31:745a775cfc20 416 if((sys.HomeBlind==0)&&(data.irNotice==IR_FAR)){
lilac0112_1 31:745a775cfc20 417 ir_pow_x = ir_pow_y = 25;
lilac0112_1 31:745a775cfc20 418 if(ir_y>0) ir_y=0;
lilac0112_1 31:745a775cfc20 419 if((data.ping[B_PING]>60)&&(1)){
lilac0112_1 31:745a775cfc20 420 ir_y = -1;
lilac0112_1 32:367b16d69a32 421 ir_pow_y = 15;
lilac0112_1 31:745a775cfc20 422 }
lilac0112_1 31:745a775cfc20 423 }
lilac0112_1 30:5998ba42237e 424 if(
lilac0112_1 30:5998ba42237e 425 (sys.DefenceFlag==1)&&
lilac0112_1 30:5998ba42237e 426 (ir_y>0)&&
lilac0112_1 31:745a775cfc20 427 (data.ping[B_PING]>60)&&
lilac0112_1 30:5998ba42237e 428 (data.ping[B_PING]<225)&&
lilac0112_1 30:5998ba42237e 429 (sys.BackHomeFlag==0)
lilac0112_1 30:5998ba42237e 430 ){
lilac0112_1 30:5998ba42237e 431 ir_pow_y=0;
lilac0112_1 30:5998ba42237e 432 }
lilac0112_1 24:34ef6379b0df 433 ////Lineセンサーの値を評価しモーターの出力補正値を演算
lilac0112_1 14:b510adcb6065 434 if(sys.LineBlind==1){
lilac0112_1 14:b510adcb6065 435
lilac0112_1 14:b510adcb6065 436 LineSlowPower[X_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 437 LineSlowPower[Y_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 438
lilac0112_1 14:b510adcb6065 439 LineReturnPower[X_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 440 LineReturnPower[Y_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 441
lilac0112_1 14:b510adcb6065 442 data.lnStop[X_LINE] = 1;
lilac0112_1 14:b510adcb6065 443 data.lnStop[Y_LINE] = 1;
lilac0112_1 14:b510adcb6065 444
lilac0112_1 15:17502a27a60b 445 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 446 }
lilac0112_1 14:b510adcb6065 447 else{
lilac0112_1 28:f246a5fbf6a5 448 /*
lilac0112_1 18:3a42a931c95a 449 LineJudgeReset(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 18:3a42a931c95a 450 LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 28:f246a5fbf6a5 451 LineJudgeReturn(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
lilac0112_1 28:f246a5fbf6a5 452 */
lilac0112_1 28:f246a5fbf6a5 453 LineJudgeReset2(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 28:f246a5fbf6a5 454 LineJudgeSlow2(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE], &ir_pow_x, &ir_pow_y);
lilac0112_1 27:825c6835e3db 455 LineJudgeReturn2(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
lilac0112_1 14:b510adcb6065 456 }
lilac0112_1 24:34ef6379b0df 457 ////LEDデバッグ
lilac0112_1 19:967207de919d 458 //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
lilac0112_1 19:967207de919d 459 //if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
lilac0112_1 27:825c6835e3db 460 //LED = ((data.lnRawMemory[0]!=0)<<2) | ((data.lnRawMemory[1]!=0)<<1) | ((data.lnRawMemory[2]!=0)<<0);
lilac0112_1 30:5998ba42237e 461 //LED = ((data.lnRawOrder[0]!=LINE_EMPTY)<<2) | ((data.lnRawOrder[1]!=LINE_EMPTY)<<1) | ((data.lnRawOrder[2]!=LINE_EMPTY)<<0);
lilac0112_1 27:825c6835e3db 462 //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
lilac0112_1 19:967207de919d 463
lilac0112_1 25:a7460e23e02e 464 //LED = sys.BallHoldFlag*15;
lilac0112_1 32:367b16d69a32 465 if(sys.BallHoldFlag==1) LED=15;
lilac0112_1 32:367b16d69a32 466 else LED=10;
lilac0112_1 19:967207de919d 467
lilac0112_1 32:367b16d69a32 468 //if(data.lnRepeat>=1) LED=15;
lilac0112_1 32:367b16d69a32 469 //else LED=10;
lilac0112_1 30:5998ba42237e 470
lilac0112_1 21:378470320524 471 //LED = 0xFF*(data.ping[B_PING]<30);
lilac0112_1 14:b510adcb6065 472
lilac0112_1 16:4fadb7a87497 473 //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
lilac0112_1 14:b510adcb6065 474
lilac0112_1 14:b510adcb6065 475 //else LED = 0xA;
lilac0112_1 14:b510adcb6065 476 //LED = LineHold;
lilac0112_1 14:b510adcb6065 477
lilac0112_1 24:34ef6379b0df 478
lilac0112_1 24:34ef6379b0df 479 ////最終的なモーターの出力を演算
lilac0112_1 32:367b16d69a32 480 vx = (ir_pow_x*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[X_LINE];
lilac0112_1 32:367b16d69a32 481 vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[Y_LINE];
lilac0112_1 10:6df631c39f9b 482 vs = cmps_set.OutputPID;
lilac0112_1 10:6df631c39f9b 483 move(
lilac0112_1 10:6df631c39f9b 484 vx,
lilac0112_1 10:6df631c39f9b 485 vy,
lilac0112_1 10:6df631c39f9b 486 vs
lilac0112_1 10:6df631c39f9b 487 );
lilac0112_1 24:34ef6379b0df 488 //モーターに信号を出力
lilac0112_1 10:6df631c39f9b 489 if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;}
lilac0112_1 24:34ef6379b0df 490
lilac0112_1 0:ea35c18c85fc 491 if(sys.stopflag==1){
lilac0112_1 24:34ef6379b0df 492 //コマンドモードに戻る際の処理
lilac0112_1 0:ea35c18c85fc 493 }
lilac0112_1 0:ea35c18c85fc 494 return;
lilac0112_1 0:ea35c18c85fc 495 }
lilac0112_1 30:5998ba42237e 496
lilac0112_1 0:ea35c18c85fc 497 void modeAttack5(void){
lilac0112_1 30:5998ba42237e 498 ////初期値を決める等
lilac0112_1 30:5998ba42237e 499 if(sys.KickOffFlag==1){
lilac0112_1 30:5998ba42237e 500
lilac0112_1 30:5998ba42237e 501 ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
lilac0112_1 30:5998ba42237e 502 sys.IrBlind=0;
lilac0112_1 30:5998ba42237e 503 sys.LineBlind=0;
lilac0112_1 30:5998ba42237e 504 sys.PingBlind=0;
lilac0112_1 30:5998ba42237e 505
lilac0112_1 30:5998ba42237e 506 sys.HomeBlind=1;
lilac0112_1 30:5998ba42237e 507 sys.DriBlind=1;
lilac0112_1 30:5998ba42237e 508 //defence
lilac0112_1 30:5998ba42237e 509 sys.DefenceFlag=1;
lilac0112_1 30:5998ba42237e 510
lilac0112_1 30:5998ba42237e 511
lilac0112_1 30:5998ba42237e 512 //初期値設定の終了
lilac0112_1 30:5998ba42237e 513 //sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 514 }
lilac0112_1 30:5998ba42237e 515 modeAttack4();
lilac0112_1 0:ea35c18c85fc 516 }