CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

ジャパンオープン用のメインプログラム

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@31:745a775cfc20, 2016-03-23 (annotated)

Committer:
lilac0112_1
Date:
Wed Mar 23 13:01:43 2016 +0000
Revision:
31:745a775cfc20
Parent:
30:5998ba42237e
Child:
32:367b16d69a32
Libero and atk

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 24:34ef6379b0df 247 }
lilac0112_1 30:5998ba42237e 248
lilac0112_1 24:34ef6379b0df 249 //Irの検出値によって出力を調整
lilac0112_1 24:34ef6379b0df 250 if(data.irNotice==IR_CLOSER){
lilac0112_1 25:a7460e23e02e 251 //ir_pow = sys.s_pow;
lilac0112_1 25:a7460e23e02e 252 ir_pow_x = ir_pow_y = sys.s_pow;
lilac0112_1 24:34ef6379b0df 253 }
lilac0112_1 24:34ef6379b0df 254 else if(data.irNotice==IR_CLOSE){
lilac0112_1 25:a7460e23e02e 255 //ir_pow = sys.m_pow;
lilac0112_1 25:a7460e23e02e 256 ir_pow_x = ir_pow_y = sys.m_pow;
lilac0112_1 24:34ef6379b0df 257 }
lilac0112_1 24:34ef6379b0df 258 else if(data.irNotice==IR_FAR){
lilac0112_1 25:a7460e23e02e 259 //ir_pow = sys.l_pow;
lilac0112_1 25:a7460e23e02e 260 ir_pow_x = ir_pow_y = sys.l_pow;
lilac0112_1 24:34ef6379b0df 261 }
lilac0112_1 24:34ef6379b0df 262 else{//data.irNotice==IR_NONE
lilac0112_1 25:a7460e23e02e 263 //ir_pow = 0;
lilac0112_1 25:a7460e23e02e 264 ir_pow_x = ir_pow_y = 0;
lilac0112_1 24:34ef6379b0df 265 }
lilac0112_1 30:5998ba42237e 266
lilac0112_1 30:5998ba42237e 267 if(sys.DefenceFlag==1){
lilac0112_1 30:5998ba42237e 268 ir_pow_x = 30;
lilac0112_1 30:5998ba42237e 269 ir_pow_y = 30;
lilac0112_1 30:5998ba42237e 270 if(data.ping[L_PING]<=60){
lilac0112_1 30:5998ba42237e 271 ir_pow_y = 30;
lilac0112_1 30:5998ba42237e 272 }
lilac0112_1 30:5998ba42237e 273 if(data.ping[L_PING]<=40){
lilac0112_1 30:5998ba42237e 274 ir_pow_y = 25;
lilac0112_1 30:5998ba42237e 275 }
lilac0112_1 30:5998ba42237e 276 if(data.ping[L_PING]<=30){
lilac0112_1 30:5998ba42237e 277 ir_pow_y = 20;
lilac0112_1 30:5998ba42237e 278 }
lilac0112_1 30:5998ba42237e 279 if(data.ping[L_PING]<15){
lilac0112_1 30:5998ba42237e 280 ir_pow_y = 15;
lilac0112_1 30:5998ba42237e 281 }
lilac0112_1 30:5998ba42237e 282 if(data.ping[L_PING]<10){
lilac0112_1 30:5998ba42237e 283 ir_pow_y = 10;
lilac0112_1 30:5998ba42237e 284 }
lilac0112_1 30:5998ba42237e 285 }
lilac0112_1 24:34ef6379b0df 286 ////ドリブラー
lilac0112_1 24:34ef6379b0df 287 //ホールド判定
lilac0112_1 25:a7460e23e02e 288 JudgeBallHolding();
lilac0112_1 25:a7460e23e02e 289 //ドリブラー駆動
lilac0112_1 30:5998ba42237e 290 if((sys.DriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){
lilac0112_1 30:5998ba42237e 291 if(
lilac0112_1 30:5998ba42237e 292 ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))||
lilac0112_1 30:5998ba42237e 293 ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))
lilac0112_1 30:5998ba42237e 294 ){
lilac0112_1 30:5998ba42237e 295 cmps_set.GoalDeg=0;
lilac0112_1 30:5998ba42237e 296 }
lilac0112_1 30:5998ba42237e 297 else if((data.lnRawOrderLog1[0]==A_SPOT)&&(1)){
lilac0112_1 30:5998ba42237e 298 cmps_set.GoalDeg=-30;
lilac0112_1 30:5998ba42237e 299 }
lilac0112_1 30:5998ba42237e 300 else if((data.lnRawOrderLog1[0]==B_SPOT)&&(1)){
lilac0112_1 30:5998ba42237e 301 cmps_set.GoalDeg=30;
lilac0112_1 30:5998ba42237e 302 }
lilac0112_1 30:5998ba42237e 303 else{
lilac0112_1 30:5998ba42237e 304 cmps_set.GoalDeg=0;
lilac0112_1 30:5998ba42237e 305 }
lilac0112_1 30:5998ba42237e 306 }
lilac0112_1 25:a7460e23e02e 307 if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 25:a7460e23e02e 308 sys.DribbleFlag=1;
lilac0112_1 30:5998ba42237e 309 if((sys.BallHoldFlag==1)||(data.ball==1)){
lilac0112_1 25:a7460e23e02e 310 //ir_pow=20;
lilac0112_1 25:a7460e23e02e 311 ir_pow_x = ir_pow_y = 20;
lilac0112_1 19:967207de919d 312 }
lilac0112_1 19:967207de919d 313 }
lilac0112_1 19:967207de919d 314 else{
lilac0112_1 19:967207de919d 315 sys.DribbleFlag=0;
lilac0112_1 25:a7460e23e02e 316 cmps_set.HoldDeg=0;
lilac0112_1 19:967207de919d 317 }
lilac0112_1 19:967207de919d 318
lilac0112_1 24:34ef6379b0df 319 ////Kick
lilac0112_1 25:a7460e23e02e 320 /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 24:34ef6379b0df 321 DriveTurn();
lilac0112_1 25:a7460e23e02e 322 }*/
lilac0112_1 25:a7460e23e02e 323 /*if((data.ball==1)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 25:a7460e23e02e 324 DriveTurn();
lilac0112_1 25:a7460e23e02e 325 DriveSolenoid();
lilac0112_1 25:a7460e23e02e 326 }*/
lilac0112_1 30:5998ba42237e 327 if(
lilac0112_1 30:5998ba42237e 328 /*(sys.BallHoldFlag==1)&&
lilac0112_1 30:5998ba42237e 329 (data.ball==1)&&
lilac0112_1 30:5998ba42237e 330 (data.irNotice==IR_CLOSER)&&
lilac0112_1 30:5998ba42237e 331 ((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))&&
lilac0112_1 30:5998ba42237e 332 (data.ping[B_PING]>100)&&
lilac0112_1 30:5998ba42237e 333 (
lilac0112_1 30:5998ba42237e 334 ((data.ping[L_PING]<60)&&(data.ping[R_PING]>60))||
lilac0112_1 30:5998ba42237e 335 ((data.ping[L_PING]>60)&&(data.ping[R_PING]<60))||
lilac0112_1 30:5998ba42237e 336 (data.lnRawOrderLog1[0]==A_SPOT)||
lilac0112_1 30:5998ba42237e 337 (data.lnRawOrder[0]==A_SPOT)||
lilac0112_1 30:5998ba42237e 338 (data.lnRawOrderLog1[0]==B_SPOT)||
lilac0112_1 30:5998ba42237e 339 (data.lnRawOrder[0]==B_SPOT)
lilac0112_1 30:5998ba42237e 340 )&&
lilac0112_1 30:5998ba42237e 341 (data.lnRaw==0)*/
lilac0112_1 30:5998ba42237e 342 (sys.DriBlind==0)&&
lilac0112_1 30:5998ba42237e 343 (data.ping[R_PING]<30)&&
lilac0112_1 30:5998ba42237e 344 (data.ping[L_PING]<30)
lilac0112_1 30:5998ba42237e 345 ){
lilac0112_1 30:5998ba42237e 346 DriveTurn();
lilac0112_1 30:5998ba42237e 347 DriveSolenoid();
lilac0112_1 30:5998ba42237e 348 }
lilac0112_1 19:967207de919d 349
lilac0112_1 24:34ef6379b0df 350 //Irの細かい補正値の処理．基本的にLineの補正値を演算する処理が始まる前にIrのモーター出力値補正をすべて終わらせる．
lilac0112_1 25:a7460e23e02e 351 //if(sys.IrBlind==1) ir_pow=0;
lilac0112_1 25:a7460e23e02e 352 if(sys.IrBlind==1) ir_pow_x=ir_pow_y=0;
lilac0112_1 28:f246a5fbf6a5 353 if((data.ping[R_PING]<data.ping[L_PING])&&(data.irPosition==17)&&(data.irNotice==IR_CLOSER)){
lilac0112_1 18:3a42a931c95a 354 ir_x_turn = -ir_x_turn;
lilac0112_1 18:3a42a931c95a 355 ir_y_turn = -ir_y_turn;
lilac0112_1 18:3a42a931c95a 356 }
lilac0112_1 25:a7460e23e02e 357
lilac0112_1 26:6ca88eeaa2b4 358 if((sys.BackHomeFlag==1)&&(sys.HomeBlind==0)){
lilac0112_1 25:a7460e23e02e 359 ir_pow_x = ir_pow_y = 25;
lilac0112_1 25:a7460e23e02e 360 //x
lilac0112_1 25:a7460e23e02e 361 if((abs(data.ping[L_PING]-data.ping[R_PING])>20)&&(sys.HomeStayFlag[X_PING]==0)){
lilac0112_1 25:a7460e23e02e 362 if(data.ping[L_PING]>data.ping[R_PING]){
lilac0112_1 25:a7460e23e02e 363 ir_x = -1;
lilac0112_1 25:a7460e23e02e 364 }
lilac0112_1 25:a7460e23e02e 365 else{
lilac0112_1 25:a7460e23e02e 366 ir_x = 1;
lilac0112_1 25:a7460e23e02e 367 }
lilac0112_1 25:a7460e23e02e 368 if((data.ping[L_PING]<WhiteToWallPlus[X_PING])&&(data.ping[R_PING]<WhiteToWallPlus[X_PING])){
lilac0112_1 25:a7460e23e02e 369 ir_x = 0;
lilac0112_1 25:a7460e23e02e 370 }
lilac0112_1 25:a7460e23e02e 371 }
lilac0112_1 25:a7460e23e02e 372 else{
lilac0112_1 25:a7460e23e02e 373 ir_x = 0;
lilac0112_1 25:a7460e23e02e 374 //sys.HomeStayFlag[X_PING]=1;
lilac0112_1 25:a7460e23e02e 375 }
lilac0112_1 25:a7460e23e02e 376 //y
lilac0112_1 31:745a775cfc20 377 if((data.ping[B_PING]>60)&&(1)&&(sys.HomeStayFlag[Y_PING]==0)){
lilac0112_1 25:a7460e23e02e 378 ir_y = -1;
lilac0112_1 25:a7460e23e02e 379 }
lilac0112_1 25:a7460e23e02e 380 else{
lilac0112_1 25:a7460e23e02e 381 ir_y = 0;
lilac0112_1 25:a7460e23e02e 382 //sys.HomeStayFlag[Y_PING]=1;
lilac0112_1 25:a7460e23e02e 383 }
lilac0112_1 25:a7460e23e02e 384 }
lilac0112_1 25:a7460e23e02e 385 else{
lilac0112_1 25:a7460e23e02e 386 ir_x = (ir_x_dir + ir_x_turn);
lilac0112_1 25:a7460e23e02e 387 ir_y = (ir_y_dir + ir_y_turn);
lilac0112_1 25:a7460e23e02e 388 }
lilac0112_1 31:745a775cfc20 389 if((sys.HomeBlind==0)&&(data.irNotice==IR_FAR)){
lilac0112_1 31:745a775cfc20 390 ir_pow_x = ir_pow_y = 25;
lilac0112_1 31:745a775cfc20 391 if(ir_y>0) ir_y=0;
lilac0112_1 31:745a775cfc20 392 if((data.ping[B_PING]>60)&&(1)){
lilac0112_1 31:745a775cfc20 393 ir_y = -1;
lilac0112_1 31:745a775cfc20 394 }
lilac0112_1 31:745a775cfc20 395 }
lilac0112_1 30:5998ba42237e 396 if(
lilac0112_1 30:5998ba42237e 397 (sys.DefenceFlag==1)&&
lilac0112_1 30:5998ba42237e 398 (ir_y>0)&&
lilac0112_1 31:745a775cfc20 399 (data.ping[B_PING]>60)&&
lilac0112_1 30:5998ba42237e 400 (data.ping[B_PING]<225)&&
lilac0112_1 30:5998ba42237e 401 (sys.BackHomeFlag==0)
lilac0112_1 30:5998ba42237e 402 ){
lilac0112_1 30:5998ba42237e 403 ir_pow_y=0;
lilac0112_1 30:5998ba42237e 404 }
lilac0112_1 24:34ef6379b0df 405 ////Lineセンサーの値を評価しモーターの出力補正値を演算
lilac0112_1 14:b510adcb6065 406 if(sys.LineBlind==1){
lilac0112_1 14:b510adcb6065 407
lilac0112_1 14:b510adcb6065 408 LineSlowPower[X_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 409 LineSlowPower[Y_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 410
lilac0112_1 14:b510adcb6065 411 LineReturnPower[X_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 412 LineReturnPower[Y_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 413
lilac0112_1 14:b510adcb6065 414 data.lnStop[X_LINE] = 1;
lilac0112_1 14:b510adcb6065 415 data.lnStop[Y_LINE] = 1;
lilac0112_1 14:b510adcb6065 416
lilac0112_1 15:17502a27a60b 417 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 418 }
lilac0112_1 14:b510adcb6065 419 else{
lilac0112_1 28:f246a5fbf6a5 420 /*
lilac0112_1 18:3a42a931c95a 421 LineJudgeReset(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 18:3a42a931c95a 422 LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 28:f246a5fbf6a5 423 LineJudgeReturn(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
lilac0112_1 28:f246a5fbf6a5 424 */
lilac0112_1 28:f246a5fbf6a5 425 LineJudgeReset2(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 28:f246a5fbf6a5 426 LineJudgeSlow2(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE], &ir_pow_x, &ir_pow_y);
lilac0112_1 27:825c6835e3db 427 LineJudgeReturn2(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
lilac0112_1 14:b510adcb6065 428 }
lilac0112_1 24:34ef6379b0df 429 ////LEDデバッグ
lilac0112_1 19:967207de919d 430 //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
lilac0112_1 19:967207de919d 431 //if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
lilac0112_1 27:825c6835e3db 432 //LED = ((data.lnRawMemory[0]!=0)<<2) | ((data.lnRawMemory[1]!=0)<<1) | ((data.lnRawMemory[2]!=0)<<0);
lilac0112_1 30:5998ba42237e 433 //LED = ((data.lnRawOrder[0]!=LINE_EMPTY)<<2) | ((data.lnRawOrder[1]!=LINE_EMPTY)<<1) | ((data.lnRawOrder[2]!=LINE_EMPTY)<<0);
lilac0112_1 27:825c6835e3db 434 //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
lilac0112_1 19:967207de919d 435
lilac0112_1 25:a7460e23e02e 436 //LED = sys.BallHoldFlag*15;
lilac0112_1 27:825c6835e3db 437 //if(sys.BallHoldFlag==1) LED=15;
lilac0112_1 27:825c6835e3db 438 //else LED=10;
lilac0112_1 19:967207de919d 439
lilac0112_1 30:5998ba42237e 440 if(data.lnRepeat>=1) LED=15;
lilac0112_1 30:5998ba42237e 441 else LED=10;
lilac0112_1 30:5998ba42237e 442
lilac0112_1 21:378470320524 443 //LED = 0xFF*(data.ping[B_PING]<30);
lilac0112_1 14:b510adcb6065 444
lilac0112_1 16:4fadb7a87497 445 //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
lilac0112_1 14:b510adcb6065 446
lilac0112_1 14:b510adcb6065 447 //else LED = 0xA;
lilac0112_1 14:b510adcb6065 448 //LED = LineHold;
lilac0112_1 14:b510adcb6065 449
lilac0112_1 24:34ef6379b0df 450
lilac0112_1 24:34ef6379b0df 451 ////最終的なモーターの出力を演算
lilac0112_1 25:a7460e23e02e 452 vx = (ir_pow_x*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
lilac0112_1 25:a7460e23e02e 453 vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
lilac0112_1 10:6df631c39f9b 454 vs = cmps_set.OutputPID;
lilac0112_1 10:6df631c39f9b 455 move(
lilac0112_1 10:6df631c39f9b 456 vx,
lilac0112_1 10:6df631c39f9b 457 vy,
lilac0112_1 10:6df631c39f9b 458 vs
lilac0112_1 10:6df631c39f9b 459 );
lilac0112_1 24:34ef6379b0df 460 //モーターに信号を出力
lilac0112_1 10:6df631c39f9b 461 if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;}
lilac0112_1 24:34ef6379b0df 462
lilac0112_1 0:ea35c18c85fc 463 if(sys.stopflag==1){
lilac0112_1 24:34ef6379b0df 464 //コマンドモードに戻る際の処理
lilac0112_1 0:ea35c18c85fc 465 }
lilac0112_1 0:ea35c18c85fc 466 return;
lilac0112_1 0:ea35c18c85fc 467 }
lilac0112_1 30:5998ba42237e 468
lilac0112_1 0:ea35c18c85fc 469 void modeAttack5(void){
lilac0112_1 30:5998ba42237e 470 ////初期値を決める等
lilac0112_1 30:5998ba42237e 471 if(sys.KickOffFlag==1){
lilac0112_1 30:5998ba42237e 472
lilac0112_1 30:5998ba42237e 473 ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
lilac0112_1 30:5998ba42237e 474 sys.IrBlind=0;
lilac0112_1 30:5998ba42237e 475 sys.LineBlind=0;
lilac0112_1 30:5998ba42237e 476 sys.PingBlind=0;
lilac0112_1 30:5998ba42237e 477
lilac0112_1 30:5998ba42237e 478 sys.HomeBlind=1;
lilac0112_1 30:5998ba42237e 479 sys.DriBlind=1;
lilac0112_1 30:5998ba42237e 480 //defence
lilac0112_1 30:5998ba42237e 481 sys.DefenceFlag=1;
lilac0112_1 30:5998ba42237e 482
lilac0112_1 30:5998ba42237e 483
lilac0112_1 30:5998ba42237e 484 //初期値設定の終了
lilac0112_1 30:5998ba42237e 485 //sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 486 }
lilac0112_1 30:5998ba42237e 487 modeAttack4();
lilac0112_1 0:ea35c18c85fc 488 }