CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@38:67bc78f3c0ab, 2016-03-27 (annotated)

Committer:
lilac0112_1
Date:
Sun Mar 27 13:04:39 2016 +0000
Revision:
38:67bc78f3c0ab
Parent:
35:eb1854273e55 
JapanSoccerOpen2016 CatPot Program(main)

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