CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v20_T_Main

せとうちオープン用のメインプログラム

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@0:b910276f9da2, 2016-05-01 (annotated)

Committer:
lilac0112_1
Date:
Sun May 01 06:30:37 2016 +0000
Revision:
0:b910276f9da2
from 2v10

Who changed what in which revision?

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