CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@19:967207de919d, 2016-03-16 (annotated)

Committer:
lilac0112_1
Date:
Wed Mar 16 02:58:31 2016 +0000
Revision:
19:967207de919d
Parent:
18:3a42a931c95a
Child:
20:d69ce8f47f1d
TICK DecLine

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 18:3a42a931c95a 5
lilac0112_1 14:b510adcb6065 6 void LineJudgeSlow(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 11:3efae754e6ef 7 uint8_t LineState;
lilac0112_1 16:4fadb7a87497 8 uint8_t LineSum;
lilac0112_1 11:3efae754e6ef 9
lilac0112_1 11:3efae754e6ef 10 //line
lilac0112_1 11:3efae754e6ef 11 LineState = 0;
lilac0112_1 16:4fadb7a87497 12
lilac0112_1 16:4fadb7a87497 13 LineSum = ((data.lnHold>>2)&0x1)+((data.lnHold>>1)&0x1)+((data.lnHold>>0)&0x1);
lilac0112_1 16:4fadb7a87497 14 if((LineSum==3)||(data.FieldSpot==LINE_OUTSIDE)){
lilac0112_1 16:4fadb7a87497 15 LineState = 3;
lilac0112_1 16:4fadb7a87497 16 }
lilac0112_1 16:4fadb7a87497 17 else if(LineSum==2){
lilac0112_1 16:4fadb7a87497 18 LineState = 2;
lilac0112_1 16:4fadb7a87497 19 }
lilac0112_1 16:4fadb7a87497 20 else if(LineSum==1){
lilac0112_1 16:4fadb7a87497 21 LineState = 1;
lilac0112_1 16:4fadb7a87497 22 }
lilac0112_1 16:4fadb7a87497 23 else if(LineSum==0){
lilac0112_1 16:4fadb7a87497 24 LineState = 0;
lilac0112_1 16:4fadb7a87497 25 }
lilac0112_1 13:b20921316f3c 26
lilac0112_1 11:3efae754e6ef 27
lilac0112_1 14:b510adcb6065 28 if(pow_x>=0){
lilac0112_1 11:3efae754e6ef 29 if(data.ping[R_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 30 else *x=1;
lilac0112_1 11:3efae754e6ef 31 }
lilac0112_1 11:3efae754e6ef 32 else{
lilac0112_1 11:3efae754e6ef 33 if(data.ping[L_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 34 else *x=1;
lilac0112_1 11:3efae754e6ef 35 }
lilac0112_1 11:3efae754e6ef 36
lilac0112_1 14:b510adcb6065 37 if(pow_y>=0){
lilac0112_1 11:3efae754e6ef 38 if(data.ping[F_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 39 else *y=1;
lilac0112_1 11:3efae754e6ef 40 }
lilac0112_1 11:3efae754e6ef 41 else{
lilac0112_1 11:3efae754e6ef 42 if(data.ping[B_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
lilac0112_1 11:3efae754e6ef 43 else *y=1;
lilac0112_1 11:3efae754e6ef 44 }
lilac0112_1 11:3efae754e6ef 45 }
lilac0112_1 13:b20921316f3c 46 void LineJudgeReturn(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 17:cc862ecf9812 47 const int8_t static LineReturn[5] = {0, 0, 0, 0, 20};
lilac0112_1 12:bee8f883c33a 48 uint8_t LinePingState[4];
lilac0112_1 12:bee8f883c33a 49 //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
lilac0112_1 12:bee8f883c33a 50 //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
lilac0112_1 12:bee8f883c33a 51 //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
lilac0112_1 12:bee8f883c33a 52 //×ラインのほぼ場外では常時ラインから離れる力が優先される．
lilac0112_1 12:bee8f883c33a 53 //※場外判定を行うには再び場内に戻る必要がある．
lilac0112_1 12:bee8f883c33a 54
lilac0112_1 17:cc862ecf9812 55 if(data.FieldSpot==LINE_OUTSIDE){
lilac0112_1 12:bee8f883c33a 56
lilac0112_1 12:bee8f883c33a 57 LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWall[X_PING]);
lilac0112_1 12:bee8f883c33a 58 LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWall[X_PING]);
lilac0112_1 12:bee8f883c33a 59 LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWall[Y_PING]);
lilac0112_1 12:bee8f883c33a 60 LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWall[Y_PING]);
lilac0112_1 12:bee8f883c33a 61
lilac0112_1 12:bee8f883c33a 62 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 12:bee8f883c33a 63 data.lnStop[X_LINE]=1;
lilac0112_1 12:bee8f883c33a 64 data.lnStop[Y_LINE]=1;
lilac0112_1 17:cc862ecf9812 65
lilac0112_1 17:cc862ecf9812 66 data.ReturnDir[X_LINE]=LINE_EMPTY;
lilac0112_1 17:cc862ecf9812 67 data.ReturnDir[Y_LINE]=LINE_EMPTY;
lilac0112_1 17:cc862ecf9812 68
lilac0112_1 12:bee8f883c33a 69 //x
lilac0112_1 17:cc862ecf9812 70 if(data.ReturnDir[X_LINE]==L_LINE){
lilac0112_1 13:b20921316f3c 71 if(pow_x<0){
lilac0112_1 13:b20921316f3c 72 *x = 0;
lilac0112_1 13:b20921316f3c 73 data.lnStop[X_LINE]=1;
lilac0112_1 13:b20921316f3c 74 }
lilac0112_1 13:b20921316f3c 75 else{
lilac0112_1 13:b20921316f3c 76 *x = -LineReturn[4];
lilac0112_1 13:b20921316f3c 77 data.lnStop[X_LINE]=0;
lilac0112_1 13:b20921316f3c 78 }
lilac0112_1 12:bee8f883c33a 79 }
lilac0112_1 17:cc862ecf9812 80 else if(data.ReturnDir[X_LINE]==R_LINE){
lilac0112_1 13:b20921316f3c 81 if(pow_x>0){
lilac0112_1 13:b20921316f3c 82 *x = 0;
lilac0112_1 13:b20921316f3c 83 data.lnStop[X_LINE]=1;
lilac0112_1 13:b20921316f3c 84 }
lilac0112_1 13:b20921316f3c 85 else{
lilac0112_1 13:b20921316f3c 86 *x = LineReturn[4];
lilac0112_1 13:b20921316f3c 87 data.lnStop[X_LINE]=0;
lilac0112_1 13:b20921316f3c 88 }
lilac0112_1 12:bee8f883c33a 89 }
lilac0112_1 17:cc862ecf9812 90 else if(data.ReturnDir[X_LINE]==LINE_EMPTY){
lilac0112_1 17:cc862ecf9812 91 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){
lilac0112_1 17:cc862ecf9812 92 /*if(pow_x>0){
lilac0112_1 17:cc862ecf9812 93 *x = -pow_x*(1-0.75);
lilac0112_1 17:cc862ecf9812 94 }
lilac0112_1 17:cc862ecf9812 95 else{
lilac0112_1 17:cc862ecf9812 96 *x = pow_x*(1-0.75);
lilac0112_1 17:cc862ecf9812 97 }*/
lilac0112_1 17:cc862ecf9812 98 data.lnStop[X_LINE]=1;
lilac0112_1 17:cc862ecf9812 99 }
lilac0112_1 17:cc862ecf9812 100 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
lilac0112_1 17:cc862ecf9812 101
lilac0112_1 17:cc862ecf9812 102 data.ReturnDir[X_LINE]=L_LINE;
lilac0112_1 17:cc862ecf9812 103
lilac0112_1 18:3a42a931c95a 104 /*if(pow_x<0){
lilac0112_1 17:cc862ecf9812 105 *x = 0;
lilac0112_1 17:cc862ecf9812 106 data.lnStop[X_LINE]=1;
lilac0112_1 17:cc862ecf9812 107 }
lilac0112_1 17:cc862ecf9812 108 else{
lilac0112_1 17:cc862ecf9812 109 *x = -LineReturn[4];
lilac0112_1 17:cc862ecf9812 110 data.lnStop[X_LINE]=0;
lilac0112_1 18:3a42a931c95a 111 }*/
lilac0112_1 18:3a42a931c95a 112 *x = -LineReturn[4];
lilac0112_1 18:3a42a931c95a 113 data.lnStop[X_LINE]=0;
lilac0112_1 17:cc862ecf9812 114 }
lilac0112_1 17:cc862ecf9812 115 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
lilac0112_1 17:cc862ecf9812 116
lilac0112_1 17:cc862ecf9812 117 data.ReturnDir[X_LINE]=R_LINE;
lilac0112_1 17:cc862ecf9812 118
lilac0112_1 18:3a42a931c95a 119 /*if(pow_x>0){
lilac0112_1 17:cc862ecf9812 120 *x = 0;
lilac0112_1 17:cc862ecf9812 121 data.lnStop[X_LINE]=1;
lilac0112_1 17:cc862ecf9812 122 }
lilac0112_1 17:cc862ecf9812 123 else{
lilac0112_1 17:cc862ecf9812 124 *x = LineReturn[4];
lilac0112_1 17:cc862ecf9812 125 data.lnStop[X_LINE]=0;
lilac0112_1 18:3a42a931c95a 126 }*/
lilac0112_1 18:3a42a931c95a 127 *x = LineReturn[4];
lilac0112_1 18:3a42a931c95a 128 data.lnStop[X_LINE]=0;
lilac0112_1 17:cc862ecf9812 129 }
lilac0112_1 17:cc862ecf9812 130 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
lilac0112_1 17:cc862ecf9812 131 *x = 0;
lilac0112_1 17:cc862ecf9812 132 data.lnStop[X_LINE]=0;
lilac0112_1 17:cc862ecf9812 133 }
lilac0112_1 12:bee8f883c33a 134 }
lilac0112_1 12:bee8f883c33a 135 //y
lilac0112_1 17:cc862ecf9812 136 if(data.ReturnDir[Y_LINE]==F_LINE){
lilac0112_1 13:b20921316f3c 137 if(pow_y>0){
lilac0112_1 17:cc862ecf9812 138 *y = 0;
lilac0112_1 17:cc862ecf9812 139 data.lnStop[Y_LINE]=1;
lilac0112_1 13:b20921316f3c 140 }
lilac0112_1 13:b20921316f3c 141 else{
lilac0112_1 17:cc862ecf9812 142 *y = LineReturn[4];
lilac0112_1 17:cc862ecf9812 143 data.lnStop[Y_LINE]=0;
lilac0112_1 13:b20921316f3c 144 }
lilac0112_1 12:bee8f883c33a 145 }
lilac0112_1 17:cc862ecf9812 146 else if(data.ReturnDir[Y_LINE]==B_LINE){
lilac0112_1 13:b20921316f3c 147 if(pow_y<0){
lilac0112_1 13:b20921316f3c 148 *y = 0;
lilac0112_1 13:b20921316f3c 149 data.lnStop[Y_LINE]=1;
lilac0112_1 13:b20921316f3c 150 }
lilac0112_1 13:b20921316f3c 151 else{
lilac0112_1 13:b20921316f3c 152 *y = -LineReturn[4];
lilac0112_1 13:b20921316f3c 153 data.lnStop[Y_LINE]=0;
lilac0112_1 13:b20921316f3c 154 }
lilac0112_1 12:bee8f883c33a 155 }
lilac0112_1 17:cc862ecf9812 156 else if(data.ReturnDir[Y_LINE]==LINE_EMPTY){
lilac0112_1 17:cc862ecf9812 157 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==0)){
lilac0112_1 17:cc862ecf9812 158 /*if(pow_y>0){
lilac0112_1 17:cc862ecf9812 159 *y = -pow_y*(1-0.75);
lilac0112_1 17:cc862ecf9812 160 }
lilac0112_1 17:cc862ecf9812 161 else{
lilac0112_1 17:cc862ecf9812 162 *y = pow_y*(1-0.75);
lilac0112_1 17:cc862ecf9812 163 }*/
lilac0112_1 17:cc862ecf9812 164 data.lnStop[Y_LINE]=1;
lilac0112_1 13:b20921316f3c 165 }
lilac0112_1 17:cc862ecf9812 166 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){
lilac0112_1 17:cc862ecf9812 167
lilac0112_1 17:cc862ecf9812 168 data.ReturnDir[Y_LINE]=B_LINE;
lilac0112_1 17:cc862ecf9812 169
lilac0112_1 17:cc862ecf9812 170 if(pow_y<0){
lilac0112_1 17:cc862ecf9812 171 *y = 0;
lilac0112_1 17:cc862ecf9812 172 data.lnStop[Y_LINE]=1;
lilac0112_1 17:cc862ecf9812 173 }
lilac0112_1 17:cc862ecf9812 174 else{
lilac0112_1 17:cc862ecf9812 175 *y = -LineReturn[4];
lilac0112_1 17:cc862ecf9812 176 data.lnStop[Y_LINE]=0;
lilac0112_1 17:cc862ecf9812 177 }
lilac0112_1 17:cc862ecf9812 178 }
lilac0112_1 17:cc862ecf9812 179 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){
lilac0112_1 17:cc862ecf9812 180
lilac0112_1 17:cc862ecf9812 181 data.ReturnDir[Y_LINE]=F_LINE;
lilac0112_1 17:cc862ecf9812 182
lilac0112_1 17:cc862ecf9812 183 if(pow_y>0){
lilac0112_1 17:cc862ecf9812 184 *y = 0;
lilac0112_1 17:cc862ecf9812 185 data.lnStop[Y_LINE]=1;
lilac0112_1 17:cc862ecf9812 186 }
lilac0112_1 17:cc862ecf9812 187 else{
lilac0112_1 17:cc862ecf9812 188 *y = LineReturn[4];
lilac0112_1 17:cc862ecf9812 189 data.lnStop[Y_LINE]=0;
lilac0112_1 17:cc862ecf9812 190 }
lilac0112_1 17:cc862ecf9812 191 }
lilac0112_1 17:cc862ecf9812 192 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){
lilac0112_1 17:cc862ecf9812 193 *y = 0;
lilac0112_1 17:cc862ecf9812 194 data.lnStop[Y_LINE]=0;
lilac0112_1 13:b20921316f3c 195 }
lilac0112_1 12:bee8f883c33a 196 }
lilac0112_1 17:cc862ecf9812 197 //none
lilac0112_1 17:cc862ecf9812 198 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)&&(LinePingState[F_PING]==1)&&(LinePingState[B_PING]==1)){
lilac0112_1 17:cc862ecf9812 199 //turn!
lilac0112_1 12:bee8f883c33a 200 }
lilac0112_1 13:b20921316f3c 201
lilac0112_1 17:cc862ecf9812 202 }
lilac0112_1 17:cc862ecf9812 203 else{//data.FieldSpot==LINE_INSIDE
lilac0112_1 17:cc862ecf9812 204
lilac0112_1 17:cc862ecf9812 205 data.ReturnDir[X_LINE]=LINE_EMPTY;
lilac0112_1 17:cc862ecf9812 206 data.ReturnDir[Y_LINE]=LINE_EMPTY;
lilac0112_1 13:b20921316f3c 207
lilac0112_1 12:bee8f883c33a 208 *x = 0;
lilac0112_1 12:bee8f883c33a 209 *y = 0;
lilac0112_1 12:bee8f883c33a 210
lilac0112_1 12:bee8f883c33a 211 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 12:bee8f883c33a 212 data.lnStop[X_LINE]=1;
lilac0112_1 12:bee8f883c33a 213 data.lnStop[Y_LINE]=1;
lilac0112_1 12:bee8f883c33a 214 }
lilac0112_1 12:bee8f883c33a 215 }
lilac0112_1 18:3a42a931c95a 216 void LineJudgeReset(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 18:3a42a931c95a 217 //static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
lilac0112_1 18:3a42a931c95a 218 //static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
lilac0112_1 18:3a42a931c95a 219
lilac0112_1 18:3a42a931c95a 220 if((/*data.lnRaw==0*/1)&&(data.lnHold==7)){
lilac0112_1 16:4fadb7a87497 221 if(data.FieldSpot==LINE_INSIDE){
lilac0112_1 18:3a42a931c95a 222
lilac0112_1 14:b510adcb6065 223 data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]);
lilac0112_1 14:b510adcb6065 224 data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]);
lilac0112_1 14:b510adcb6065 225 data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]);
lilac0112_1 14:b510adcb6065 226 data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]);
lilac0112_1 14:b510adcb6065 227 if(
lilac0112_1 14:b510adcb6065 228 (data.lnCorner[L_LINE])||
lilac0112_1 14:b510adcb6065 229 (data.lnCorner[R_LINE])||
lilac0112_1 14:b510adcb6065 230 (data.lnCorner[F_LINE])||
lilac0112_1 14:b510adcb6065 231 (data.lnCorner[B_LINE])
lilac0112_1 14:b510adcb6065 232 ){
lilac0112_1 18:3a42a931c95a 233
lilac0112_1 16:4fadb7a87497 234 data.NonWall[L_LINE] = (data.ping[L_PING]>WhiteToWall[X_PING]);
lilac0112_1 16:4fadb7a87497 235 data.NonWall[R_LINE] = (data.ping[R_PING]>WhiteToWall[X_PING]);
lilac0112_1 16:4fadb7a87497 236 data.NonWall[F_LINE] = (data.ping[F_PING]>WhiteToWall[Y_PING]);
lilac0112_1 16:4fadb7a87497 237 data.NonWall[B_LINE] = (data.ping[B_PING]>WhiteToWall[Y_PING]);
lilac0112_1 18:3a42a931c95a 238
lilac0112_1 18:3a42a931c95a 239
lilac0112_1 18:3a42a931c95a 240
lilac0112_1 18:3a42a931c95a 241 /*LastLineCorner[L_LINE]=NewLineCorner[L_LINE];
lilac0112_1 18:3a42a931c95a 242 LastLineCorner[R_LINE]=NewLineCorner[R_LINE];
lilac0112_1 18:3a42a931c95a 243 LastLineCorner[F_LINE]=NewLineCorner[F_LINE];
lilac0112_1 18:3a42a931c95a 244 LastLineCorner[B_LINE]=NewLineCorner[B_LINE];
lilac0112_1 18:3a42a931c95a 245
lilac0112_1 18:3a42a931c95a 246 NewLineCorner[L_LINE]=data.lnCorner[L_LINE];
lilac0112_1 18:3a42a931c95a 247 NewLineCorner[R_LINE]=data.lnCorner[R_LINE];
lilac0112_1 18:3a42a931c95a 248 NewLineCorner[F_LINE]=data.lnCorner[F_LINE];
lilac0112_1 18:3a42a931c95a 249 NewLineCorner[B_LINE]=data.lnCorner[B_LINE];
lilac0112_1 18:3a42a931c95a 250
lilac0112_1 18:3a42a931c95a 251 if(
lilac0112_1 18:3a42a931c95a 252 (LastLineCorner[L_LINE]==NewLineCorner[L_LINE])&&
lilac0112_1 18:3a42a931c95a 253 (LastLineCorner[R_LINE]==NewLineCorner[R_LINE])&&
lilac0112_1 18:3a42a931c95a 254 (LastLineCorner[F_LINE]==NewLineCorner[F_LINE])&&
lilac0112_1 18:3a42a931c95a 255 (LastLineCorner[B_LINE]==NewLineCorner[B_LINE])
lilac0112_1 18:3a42a931c95a 256 ){
lilac0112_1 18:3a42a931c95a 257 data.lnRepeat++;
lilac0112_1 18:3a42a931c95a 258 }
lilac0112_1 18:3a42a931c95a 259 else{
lilac0112_1 18:3a42a931c95a 260 data.lnRepeat=0;
lilac0112_1 18:3a42a931c95a 261 }*/
lilac0112_1 18:3a42a931c95a 262
lilac0112_1 16:4fadb7a87497 263 data.FieldSpot = LINE_OUTSIDE;
lilac0112_1 14:b510adcb6065 264 LineLiberate();
lilac0112_1 14:b510adcb6065 265 }
lilac0112_1 14:b510adcb6065 266 }
lilac0112_1 16:4fadb7a87497 267 else if(data.FieldSpot==LINE_OUTSIDE){
lilac0112_1 18:3a42a931c95a 268 if(data.lnRaw==0){
lilac0112_1 18:3a42a931c95a 269 data.FieldSpot = LINE_INSIDE;
lilac0112_1 18:3a42a931c95a 270 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 18:3a42a931c95a 271 LineLiberate();
lilac0112_1 18:3a42a931c95a 272 LineRankClear();
lilac0112_1 18:3a42a931c95a 273 }
lilac0112_1 14:b510adcb6065 274 }
lilac0112_1 14:b510adcb6065 275 }
lilac0112_1 16:4fadb7a87497 276 if(data.FieldSpot == LINE_OUTSIDE){
lilac0112_1 16:4fadb7a87497 277 if(
lilac0112_1 16:4fadb7a87497 278 (
lilac0112_1 16:4fadb7a87497 279 (data.ping[L_PING]>=WhiteToWall[X_PING])||
lilac0112_1 16:4fadb7a87497 280 (data.NonWall[L_LINE]==1)||
lilac0112_1 16:4fadb7a87497 281 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 16:4fadb7a87497 282 //(data.lnOrder[0]==A_SPOT)
lilac0112_1 16:4fadb7a87497 283 )&&
lilac0112_1 16:4fadb7a87497 284 (
lilac0112_1 16:4fadb7a87497 285 (data.ping[R_PING]>=WhiteToWall[X_PING])||
lilac0112_1 16:4fadb7a87497 286 (data.NonWall[R_LINE]==1)||
lilac0112_1 16:4fadb7a87497 287 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 16:4fadb7a87497 288 //(data.lnOrder[0]==B_SPOT)
lilac0112_1 16:4fadb7a87497 289 )&&
lilac0112_1 16:4fadb7a87497 290 (
lilac0112_1 16:4fadb7a87497 291 (data.ping[F_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 16:4fadb7a87497 292 (data.NonWall[F_LINE]==1)||
lilac0112_1 16:4fadb7a87497 293 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
lilac0112_1 16:4fadb7a87497 294 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
lilac0112_1 16:4fadb7a87497 295 )&&
lilac0112_1 16:4fadb7a87497 296 ((data.ping[B_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 16:4fadb7a87497 297 (data.NonWall[B_LINE]==1)||
lilac0112_1 16:4fadb7a87497 298 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
lilac0112_1 16:4fadb7a87497 299 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
lilac0112_1 16:4fadb7a87497 300 )
lilac0112_1 16:4fadb7a87497 301 ){
lilac0112_1 16:4fadb7a87497 302 data.FieldSpot = LINE_INSIDE;
lilac0112_1 16:4fadb7a87497 303 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 16:4fadb7a87497 304 LineLiberate();
lilac0112_1 16:4fadb7a87497 305 LineRankClear();
lilac0112_1 13:b20921316f3c 306 }
lilac0112_1 16:4fadb7a87497 307 }
lilac0112_1 16:4fadb7a87497 308 if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){
lilac0112_1 16:4fadb7a87497 309 if(
lilac0112_1 16:4fadb7a87497 310 (
lilac0112_1 16:4fadb7a87497 311 (data.ping[L_PING]>=WhiteToWall[X_PING])||
lilac0112_1 16:4fadb7a87497 312 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 16:4fadb7a87497 313 //(data.lnOrder[0]==A_SPOT)
lilac0112_1 16:4fadb7a87497 314 )&&
lilac0112_1 16:4fadb7a87497 315 (
lilac0112_1 16:4fadb7a87497 316 (data.ping[R_PING]>=WhiteToWall[X_PING])||
lilac0112_1 16:4fadb7a87497 317 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 16:4fadb7a87497 318 //(data.lnOrder[0]==B_SPOT)
lilac0112_1 16:4fadb7a87497 319 )&&
lilac0112_1 16:4fadb7a87497 320 (
lilac0112_1 16:4fadb7a87497 321 (data.ping[F_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 16:4fadb7a87497 322 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
lilac0112_1 16:4fadb7a87497 323 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
lilac0112_1 16:4fadb7a87497 324 )&&
lilac0112_1 16:4fadb7a87497 325 ((data.ping[B_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 16:4fadb7a87497 326 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
lilac0112_1 16:4fadb7a87497 327 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
lilac0112_1 16:4fadb7a87497 328 )
lilac0112_1 18:3a42a931c95a 329 /*||
lilac0112_1 18:3a42a931c95a 330 (
lilac0112_1 18:3a42a931c95a 331 (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
lilac0112_1 18:3a42a931c95a 332 (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
lilac0112_1 18:3a42a931c95a 333 )*/
lilac0112_1 16:4fadb7a87497 334 ){
lilac0112_1 16:4fadb7a87497 335 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 16:4fadb7a87497 336 LineLiberate();
lilac0112_1 13:b20921316f3c 337 }
lilac0112_1 13:b20921316f3c 338 }
lilac0112_1 18:3a42a931c95a 339 /*
lilac0112_1 18:3a42a931c95a 340 if(
lilac0112_1 18:3a42a931c95a 341 (data.irNotice==IR_NONE)||
lilac0112_1 18:3a42a931c95a 342 (data.irNotice==IR_FAR)||
lilac0112_1 18:3a42a931c95a 343 (
lilac0112_1 18:3a42a931c95a 344 (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
lilac0112_1 18:3a42a931c95a 345 (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
lilac0112_1 18:3a42a931c95a 346 )
lilac0112_1 18:3a42a931c95a 347 ){
lilac0112_1 18:3a42a931c95a 348 data.lnRepeat = 0;
lilac0112_1 18:3a42a931c95a 349
lilac0112_1 18:3a42a931c95a 350 data.lnCorner[L_LINE]=LINE_EMPTY;
lilac0112_1 18:3a42a931c95a 351 data.lnCorner[R_LINE]=LINE_EMPTY;
lilac0112_1 18:3a42a931c95a 352 data.lnCorner[F_LINE]=LINE_EMPTY;
lilac0112_1 18:3a42a931c95a 353 data.lnCorner[B_LINE]=LINE_EMPTY;
lilac0112_1 18:3a42a931c95a 354 }
lilac0112_1 18:3a42a931c95a 355 data.lnRepeat=0;
lilac0112_1 18:3a42a931c95a 356 if(data.lnRepeat>0){
lilac0112_1 18:3a42a931c95a 357 if(
lilac0112_1 18:3a42a931c95a 358 ((pow_x>=0)&&(data.lnCorner[R_LINE]))||
lilac0112_1 18:3a42a931c95a 359 ((pow_x<0)&&(data.lnCorner[L_LINE]))
lilac0112_1 18:3a42a931c95a 360 ){
lilac0112_1 18:3a42a931c95a 361 data.lnStay[X_LINE]=0;
lilac0112_1 18:3a42a931c95a 362 }
lilac0112_1 18:3a42a931c95a 363 else{
lilac0112_1 18:3a42a931c95a 364 data.lnStay[X_LINE]=1;
lilac0112_1 18:3a42a931c95a 365 }
lilac0112_1 18:3a42a931c95a 366 if(
lilac0112_1 18:3a42a931c95a 367 ((pow_y>=0)&&(data.lnCorner[F_LINE]))||
lilac0112_1 18:3a42a931c95a 368 ((pow_y<0)&&(data.lnCorner[B_LINE]))
lilac0112_1 18:3a42a931c95a 369 ){
lilac0112_1 18:3a42a931c95a 370 data.lnStay[Y_LINE]=0;
lilac0112_1 18:3a42a931c95a 371 }
lilac0112_1 18:3a42a931c95a 372 else{
lilac0112_1 18:3a42a931c95a 373 data.lnStay[Y_LINE]=1;
lilac0112_1 18:3a42a931c95a 374 }
lilac0112_1 18:3a42a931c95a 375 }
lilac0112_1 18:3a42a931c95a 376 else{
lilac0112_1 18:3a42a931c95a 377 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
lilac0112_1 18:3a42a931c95a 378 }*/
lilac0112_1 18:3a42a931c95a 379
lilac0112_1 12:bee8f883c33a 380 }
lilac0112_1 0:ea35c18c85fc 381 void modeAttack4(void){
lilac0112_1 10:6df631c39f9b 382 double ir_x_dir, ir_y_dir;
lilac0112_1 11:3efae754e6ef 383 double ir_x_turn, ir_y_turn;
lilac0112_1 11:3efae754e6ef 384 double ir_x, ir_y;
lilac0112_1 13:b20921316f3c 385
lilac0112_1 14:b510adcb6065 386 double LineSlowPower[2];
lilac0112_1 14:b510adcb6065 387 double LineReturnPower[2];
lilac0112_1 14:b510adcb6065 388
lilac0112_1 14:b510adcb6065 389 //double pow_x, pow_y;
lilac0112_1 10:6df631c39f9b 390 uint8_t ir_pow;
lilac0112_1 10:6df631c39f9b 391 int vx,vy,vs;
lilac0112_1 15:17502a27a60b 392 //static uint8_t data.FieldSpot;
lilac0112_1 11:3efae754e6ef 393
lilac0112_1 11:3efae754e6ef 394
lilac0112_1 10:6df631c39f9b 395 if(sys.KickOffFlag==1){
lilac0112_1 10:6df631c39f9b 396
lilac0112_1 18:3a42a931c95a 397 sys.IrBlind=0;
lilac0112_1 10:6df631c39f9b 398 sys.LineBlind=0;
lilac0112_1 10:6df631c39f9b 399 sys.PingBlind=0;
lilac0112_1 10:6df631c39f9b 400
lilac0112_1 19:967207de919d 401 sys.HomeBlind=1;
lilac0112_1 19:967207de919d 402 sys.DriBlind=1;
lilac0112_1 19:967207de919d 403
lilac0112_1 19:967207de919d 404
lilac0112_1 18:3a42a931c95a 405 sys.ir_pow_table = 0;
lilac0112_1 18:3a42a931c95a 406
lilac0112_1 18:3a42a931c95a 407 data.lnCorner[L_LINE]=data.lnCorner[R_LINE]=data.lnCorner[F_LINE]=data.lnCorner[B_LINE]=LINE_EMPTY;
lilac0112_1 18:3a42a931c95a 408 data.lnRepeat = 0;
lilac0112_1 18:3a42a931c95a 409
lilac0112_1 18:3a42a931c95a 410 //data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
lilac0112_1 18:3a42a931c95a 411 data.lnStop[X_LINE]=data.lnStop[Y_LINE]=1;
lilac0112_1 18:3a42a931c95a 412
lilac0112_1 15:17502a27a60b 413 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 414 LineLiberate();
lilac0112_1 16:4fadb7a87497 415 LineRankClear();
lilac0112_1 13:b20921316f3c 416
lilac0112_1 19:967207de919d 417 cmps_set.AtkDeg=0;
lilac0112_1 19:967207de919d 418
lilac0112_1 19:967207de919d 419 //sys.BallHoldJudgeFlag=0;
lilac0112_1 19:967207de919d 420 //sys.BallHoldFlag=0;
lilac0112_1 19:967207de919d 421
lilac0112_1 19:967207de919d 422
lilac0112_1 10:6df631c39f9b 423 sys.KickOffFlag=0;
lilac0112_1 0:ea35c18c85fc 424 }
lilac0112_1 10:6df631c39f9b 425 //data
lilac0112_1 19:967207de919d 426 /*if(sys.IrFlag==1){ReadIr();sys.IrFlag=0;}
lilac0112_1 10:6df631c39f9b 427 if(sys.PidFlag==1){PidUpdate();sys.PidFlag=0;}
lilac0112_1 10:6df631c39f9b 428 if(sys.UswFlag==1){ReadPing();sys.UswFlag=0;}
lilac0112_1 19:967207de919d 429 if(sys.UswFlag2==1){ReadPing2();sys.UswFlag2=0;}*/
lilac0112_1 19:967207de919d 430 if(sys.InfoFlag==1){ReadInfo();sys.InfoFlag=0;}
lilac0112_1 19:967207de919d 431
lilac0112_1 10:6df631c39f9b 432 //if(sys.KickFlag==1){DriveSolenoid();}
lilac0112_1 11:3efae754e6ef 433 data.lnRaw = LineRaw;
lilac0112_1 11:3efae754e6ef 434 data.lnHold = LineHold;
lilac0112_1 19:967207de919d 435 data.ball = (BallCheckerA.read_u16()<20000);
lilac0112_1 19:967207de919d 436
lilac0112_1 19:967207de919d 437 /*
lilac0112_1 19:967207de919d 438 if(sys.DriBlind==0){
lilac0112_1 19:967207de919d 439 if((data.ball==1)&&(sys.BallHoldJudgeFlag==0)){
lilac0112_1 19:967207de919d 440 sys.BallHoldJudgeFlag=1;
lilac0112_1 19:967207de919d 441 Ball_catch.attach(&AdmitBallHold, 1.0);
lilac0112_1 19:967207de919d 442 }
lilac0112_1 19:967207de919d 443
lilac0112_1 19:967207de919d 444 if((data.ball==0)&&(sys.BallHoldFlag==1)&&(sys.BallHoldGiveUpFlag==0)){
lilac0112_1 19:967207de919d 445 sys.BallHoldGiveUpFlag=1;
lilac0112_1 19:967207de919d 446 Ball_leave.attach(&GiveUpBallHold, 1.0);
lilac0112_1 19:967207de919d 447 }
lilac0112_1 19:967207de919d 448 if((data.ball==1)&&(sys.BallHoldFlag==1)){
lilac0112_1 19:967207de919d 449 sys.BallHoldGiveUpFlag=0;
lilac0112_1 19:967207de919d 450 Ball_leave.detach();
lilac0112_1 19:967207de919d 451 }
lilac0112_1 19:967207de919d 452
lilac0112_1 19:967207de919d 453 if((data.ping[B_PING]<30)&&(sys.BallHoldFlag==1)){
lilac0112_1 19:967207de919d 454 cmps_set.AtkDeg = 150;
lilac0112_1 19:967207de919d 455 Kick_now.attach(&DriveSolenoid, .125);
lilac0112_1 19:967207de919d 456 Front_now.attach(&FaceToFront, .75);
lilac0112_1 19:967207de919d 457
lilac0112_1 19:967207de919d 458 sys.BallHoldJudgeFlag=0;
lilac0112_1 19:967207de919d 459 sys.BallHoldFlag=0;
lilac0112_1 19:967207de919d 460 Ball_catch.detach();
lilac0112_1 19:967207de919d 461 Ball_leave.detach();
lilac0112_1 19:967207de919d 462 }
lilac0112_1 19:967207de919d 463 }
lilac0112_1 19:967207de919d 464 else{
lilac0112_1 19:967207de919d 465 sys.BallHoldFlag=0;
lilac0112_1 19:967207de919d 466 }*/
lilac0112_1 19:967207de919d 467
lilac0112_1 19:967207de919d 468 if(data.irNotice==IR_NONE){
lilac0112_1 19:967207de919d 469 sys.BackHomeFlag=(sys.HomeBlind==0);
lilac0112_1 19:967207de919d 470 }
lilac0112_1 19:967207de919d 471 else{
lilac0112_1 19:967207de919d 472 sys.BackHomeFlag=0;
lilac0112_1 19:967207de919d 473 }
lilac0112_1 10:6df631c39f9b 474
lilac0112_1 18:3a42a931c95a 475 if(data.ping[B_PING]<=30){
lilac0112_1 18:3a42a931c95a 476 sys.ir_pow_table=1;
lilac0112_1 18:3a42a931c95a 477 ir_x_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 18:3a42a931c95a 478 ir_y_dir = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 18:3a42a931c95a 479 ir_x_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 18:3a42a931c95a 480 ir_y_turn = ir_move_val[1][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 18:3a42a931c95a 481 }
lilac0112_1 18:3a42a931c95a 482 else{
lilac0112_1 18:3a42a931c95a 483 sys.ir_pow_table=0;
lilac0112_1 18:3a42a931c95a 484 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 18:3a42a931c95a 485 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 18:3a42a931c95a 486 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 18:3a42a931c95a 487 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 18:3a42a931c95a 488 }
lilac0112_1 18:3a42a931c95a 489 /*
lilac0112_1 18:3a42a931c95a 490 ir_x_dir = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 18:3a42a931c95a 491 ir_y_dir = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 18:3a42a931c95a 492 ir_x_turn = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 18:3a42a931c95a 493 ir_y_turn = ir_move_val[sys.ir_pow_table][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 18:3a42a931c95a 494 */
lilac0112_1 18:3a42a931c95a 495 /*
lilac0112_1 10:6df631c39f9b 496 ir_x_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_X_DIR];
lilac0112_1 10:6df631c39f9b 497 ir_y_dir = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_DIR];
lilac0112_1 10:6df631c39f9b 498 ir_x_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_X_TURN];
lilac0112_1 10:6df631c39f9b 499 ir_y_turn = ir_move_val[0][data.irNotice][data.irPosition][IR_Y_TURN];
lilac0112_1 18:3a42a931c95a 500 */
lilac0112_1 18:3a42a931c95a 501
lilac0112_1 10:6df631c39f9b 502 if(data.irNotice==IR_CLOSER){
lilac0112_1 10:6df631c39f9b 503 ir_pow = sys.s_pow;
lilac0112_1 10:6df631c39f9b 504 }
lilac0112_1 10:6df631c39f9b 505 else if(data.irNotice==IR_CLOSE){
lilac0112_1 10:6df631c39f9b 506 ir_pow = sys.m_pow;
lilac0112_1 10:6df631c39f9b 507 }
lilac0112_1 10:6df631c39f9b 508 else if(data.irNotice==IR_FAR){
lilac0112_1 10:6df631c39f9b 509 ir_pow = sys.l_pow;
lilac0112_1 10:6df631c39f9b 510 }
lilac0112_1 10:6df631c39f9b 511 else{//data.irNotice==IR_NONE
lilac0112_1 10:6df631c39f9b 512 ir_pow = 0;
lilac0112_1 10:6df631c39f9b 513 }
lilac0112_1 18:3a42a931c95a 514
lilac0112_1 19:967207de919d 515 if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
lilac0112_1 19:967207de919d 516 sys.DribbleFlag=1;
lilac0112_1 19:967207de919d 517 ir_pow=20;
lilac0112_1 19:967207de919d 518 /*if(cmps_set.AtkDeg==0){
lilac0112_1 19:967207de919d 519 if(data.irPosition==10) cmps_set.AtkDeg=30;
lilac0112_1 19:967207de919d 520 if(data.irPosition==11) cmps_set.AtkDeg=0;
lilac0112_1 19:967207de919d 521 if(data.irPosition==12) cmps_set.AtkDeg=-30;
lilac0112_1 19:967207de919d 522 }
lilac0112_1 19:967207de919d 523 else if(cmps_set.AtkDeg>0){
lilac0112_1 19:967207de919d 524 if(data.irPosition==10) cmps_set.AtkDeg=0;
lilac0112_1 19:967207de919d 525 if(data.irPosition==11) cmps_set.AtkDeg=30;
lilac0112_1 19:967207de919d 526 if(data.irPosition==12) cmps_set.AtkDeg=0;
lilac0112_1 19:967207de919d 527
lilac0112_1 19:967207de919d 528 }
lilac0112_1 19:967207de919d 529 else if(cmps_set.AtkDeg<0){
lilac0112_1 19:967207de919d 530 if(data.irPosition==10) cmps_set.AtkDeg=0;
lilac0112_1 19:967207de919d 531 if(data.irPosition==11) cmps_set.AtkDeg=-30;
lilac0112_1 19:967207de919d 532 if(data.irPosition==12) cmps_set.AtkDeg=0;
lilac0112_1 19:967207de919d 533 }*/
lilac0112_1 19:967207de919d 534 }
lilac0112_1 19:967207de919d 535 else{
lilac0112_1 19:967207de919d 536 sys.DribbleFlag=0;
lilac0112_1 19:967207de919d 537 cmps_set.AtkDeg=0;
lilac0112_1 19:967207de919d 538
lilac0112_1 19:967207de919d 539 /*sys.BallHoldJudgeFlag=0;
lilac0112_1 19:967207de919d 540 sys.BallHoldFlag=0;
lilac0112_1 19:967207de919d 541 Ball_catch.detach();
lilac0112_1 19:967207de919d 542 Ball_leave.detach();*/
lilac0112_1 19:967207de919d 543 }
lilac0112_1 19:967207de919d 544
lilac0112_1 19:967207de919d 545 if(sys.IrBlind==1) ir_pow=0;
lilac0112_1 19:967207de919d 546
lilac0112_1 18:3a42a931c95a 547
lilac0112_1 18:3a42a931c95a 548 if((data.ping[R_PING]<data.ping[L_PING])&&(data.irPosition==17)){
lilac0112_1 18:3a42a931c95a 549 ir_x_turn = -ir_x_turn;
lilac0112_1 18:3a42a931c95a 550 ir_y_turn = -ir_y_turn;
lilac0112_1 18:3a42a931c95a 551 }
lilac0112_1 10:6df631c39f9b 552
lilac0112_1 11:3efae754e6ef 553 ir_x = (ir_x_dir + ir_x_turn);
lilac0112_1 11:3efae754e6ef 554 ir_y = (ir_y_dir + ir_y_turn);
lilac0112_1 11:3efae754e6ef 555
lilac0112_1 14:b510adcb6065 556 if(sys.LineBlind==1){
lilac0112_1 14:b510adcb6065 557
lilac0112_1 14:b510adcb6065 558 LineSlowPower[X_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 559 LineSlowPower[Y_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 560
lilac0112_1 14:b510adcb6065 561 LineReturnPower[X_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 562 LineReturnPower[Y_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 563
lilac0112_1 14:b510adcb6065 564 data.lnStop[X_LINE] = 1;
lilac0112_1 14:b510adcb6065 565 data.lnStop[Y_LINE] = 1;
lilac0112_1 14:b510adcb6065 566
lilac0112_1 15:17502a27a60b 567 data.FieldSpot = LINE_INSIDE;
lilac0112_1 14:b510adcb6065 568 }
lilac0112_1 14:b510adcb6065 569 else{
lilac0112_1 18:3a42a931c95a 570 LineJudgeReset(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 16:4fadb7a87497 571
lilac0112_1 18:3a42a931c95a 572 LineJudgeSlow(ir_x, ir_y, &LineSlowPower[X_LINE], &LineSlowPower[Y_LINE]);
lilac0112_1 18:3a42a931c95a 573 LineJudgeReturn(ir_x*LineSlowPower[X_LINE], ir_y*LineSlowPower[Y_LINE], &LineReturnPower[X_LINE], &LineReturnPower[Y_LINE]);
lilac0112_1 18:3a42a931c95a 574 /*
lilac0112_1 14:b510adcb6065 575 LineSlowPower[X_LINE] = 1.0;
lilac0112_1 14:b510adcb6065 576 LineSlowPower[Y_LINE] = 1.0;
lilac0112_1 18:3a42a931c95a 577
lilac0112_1 14:b510adcb6065 578 LineReturnPower[X_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 579 LineReturnPower[Y_LINE] = 0.0;
lilac0112_1 14:b510adcb6065 580
lilac0112_1 14:b510adcb6065 581 data.lnStop[X_LINE] = 1;
lilac0112_1 14:b510adcb6065 582 data.lnStop[Y_LINE] = 1;
lilac0112_1 17:cc862ecf9812 583 */
lilac0112_1 14:b510adcb6065 584
lilac0112_1 14:b510adcb6065 585 }
lilac0112_1 19:967207de919d 586 //if(data.FieldSpot==LINE_OUTSIDE) LED = 0x9;
lilac0112_1 19:967207de919d 587 //if(data.FieldSpot==LINE_INSIDE) LED = 0x6;
lilac0112_1 19:967207de919d 588
lilac0112_1 19:967207de919d 589 //LED = 0xFF*sys.BallHoldFlag;
lilac0112_1 19:967207de919d 590
lilac0112_1 19:967207de919d 591 LED = 0xFF*(data.ping[B_PING]<30);
lilac0112_1 14:b510adcb6065 592
lilac0112_1 16:4fadb7a87497 593 //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
lilac0112_1 14:b510adcb6065 594
lilac0112_1 14:b510adcb6065 595 //else LED = 0xA;
lilac0112_1 14:b510adcb6065 596 //LED = LineHold;
lilac0112_1 14:b510adcb6065 597
lilac0112_1 18:3a42a931c95a 598 vx = (ir_pow*ir_x)*data.lnStop[X_LINE]*(/*data.lnStay[X_LINE]*/1)*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
lilac0112_1 18:3a42a931c95a 599 vy = (ir_pow*ir_y)*data.lnStop[Y_LINE]*(/*data.lnStay[X_LINE]*/1)*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
lilac0112_1 10:6df631c39f9b 600 vs = cmps_set.OutputPID;
lilac0112_1 10:6df631c39f9b 601 move(
lilac0112_1 10:6df631c39f9b 602 vx,
lilac0112_1 10:6df631c39f9b 603 vy,
lilac0112_1 10:6df631c39f9b 604 vs
lilac0112_1 10:6df631c39f9b 605 );
lilac0112_1 10:6df631c39f9b 606 if(sys.MotorFlag==1){tx_motor();sys.MotorFlag=0;}
lilac0112_1 0:ea35c18c85fc 607 if(sys.stopflag==1){
lilac0112_1 10:6df631c39f9b 608
lilac0112_1 0:ea35c18c85fc 609 //停止処理
lilac0112_1 0:ea35c18c85fc 610 }
lilac0112_1 0:ea35c18c85fc 611 return;
lilac0112_1 0:ea35c18c85fc 612 }
lilac0112_1 0:ea35c18c85fc 613 void modeAttack5(void){
lilac0112_1 0:ea35c18c85fc 614 if(sys.IrFlag==1){
lilac0112_1 0:ea35c18c85fc 615 ReadIr();
lilac0112_1 0:ea35c18c85fc 616 sys.IrFlag=0;
lilac0112_1 0:ea35c18c85fc 617 }
lilac0112_1 0:ea35c18c85fc 618 if(sys.PidFlag==1){
lilac0112_1 0:ea35c18c85fc 619 PidUpdate();
lilac0112_1 0:ea35c18c85fc 620 sys.PidFlag=0;
lilac0112_1 0:ea35c18c85fc 621 }
lilac0112_1 10:6df631c39f9b 622 move(0,0,cmps_set.OutputPID);
lilac0112_1 0:ea35c18c85fc 623 if(sys.MotorFlag==1){
lilac0112_1 14:b510adcb6065 624 //LED[0] = 1;
lilac0112_1 14:b510adcb6065 625 //LED[1] = 0;
lilac0112_1 0:ea35c18c85fc 626 tx_motor();
lilac0112_1 0:ea35c18c85fc 627 sys.MotorFlag=0;
lilac0112_1 0:ea35c18c85fc 628 }
lilac0112_1 5:5ff3a7d5d8c2 629 else{
lilac0112_1 14:b510adcb6065 630 //LED[0] = 0;
lilac0112_1 14:b510adcb6065 631 //LED[1] = 1;
lilac0112_1 5:5ff3a7d5d8c2 632 }
lilac0112_1 0:ea35c18c85fc 633 if(sys.stopflag==1){
lilac0112_1 0:ea35c18c85fc 634 //停止処理
lilac0112_1 0:ea35c18c85fc 635 }
lilac0112_1 0:ea35c18c85fc 636 return;
lilac0112_1 0:ea35c18c85fc 637 }