CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

Important changes to repositories hosted on mbed.com

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

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

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/LineProcess.cpp@34:1c86c1299ea4, 2016-03-24 (annotated)

Committer:
lilac0112_1
Date:
Thu Mar 24 15:33:54 2016 +0000
Revision:
34:1c86c1299ea4
Parent:
33:aa115c30892e
Child:
38:67bc78f3c0ab
OK?It can be shipped; red......libero; green...atkOnly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 25:a7460e23e02e 1 #include "mbed.h"
lilac0112_1 25:a7460e23e02e 2 #include "extern.h"
lilac0112_1 25:a7460e23e02e 3
lilac0112_1 25:a7460e23e02e 4 void LineJudgeSlow(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 25:a7460e23e02e 5 uint8_t LineState;
lilac0112_1 25:a7460e23e02e 6 uint8_t LineSum;
lilac0112_1 25:a7460e23e02e 7
lilac0112_1 25:a7460e23e02e 8 //line
lilac0112_1 25:a7460e23e02e 9 LineState = 0;
lilac0112_1 25:a7460e23e02e 10
lilac0112_1 25:a7460e23e02e 11 LineSum = ((data.lnHold>>2)&0x1)+((data.lnHold>>1)&0x1)+((data.lnHold>>0)&0x1);
lilac0112_1 25:a7460e23e02e 12 if((LineSum==3)||(data.FieldSpot==LINE_OUTSIDE)){
lilac0112_1 25:a7460e23e02e 13 LineState = 3;
lilac0112_1 25:a7460e23e02e 14 }
lilac0112_1 25:a7460e23e02e 15 else if(LineSum==2){
lilac0112_1 25:a7460e23e02e 16 LineState = 2;
lilac0112_1 25:a7460e23e02e 17 }
lilac0112_1 25:a7460e23e02e 18 else if(LineSum==1){
lilac0112_1 25:a7460e23e02e 19 LineState = 1;
lilac0112_1 25:a7460e23e02e 20 }
lilac0112_1 25:a7460e23e02e 21 else if(LineSum==0){
lilac0112_1 25:a7460e23e02e 22 LineState = 0;
lilac0112_1 25:a7460e23e02e 23 }
lilac0112_1 25:a7460e23e02e 24
lilac0112_1 25:a7460e23e02e 25
lilac0112_1 25:a7460e23e02e 26 if(pow_x>=0){
lilac0112_1 25:a7460e23e02e 27 if(data.ping[R_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
lilac0112_1 25:a7460e23e02e 28 else *x=1;
lilac0112_1 25:a7460e23e02e 29 }
lilac0112_1 25:a7460e23e02e 30 else{
lilac0112_1 25:a7460e23e02e 31 if(data.ping[L_PING]<WhiteToWall[X_PING]) (*x)=LineDecline[LineState];
lilac0112_1 25:a7460e23e02e 32 else *x=1;
lilac0112_1 25:a7460e23e02e 33 }
lilac0112_1 25:a7460e23e02e 34
lilac0112_1 25:a7460e23e02e 35 if(pow_y>=0){
lilac0112_1 25:a7460e23e02e 36 if(data.ping[F_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
lilac0112_1 25:a7460e23e02e 37 else *y=1;
lilac0112_1 25:a7460e23e02e 38 }
lilac0112_1 25:a7460e23e02e 39 else{
lilac0112_1 25:a7460e23e02e 40 if(data.ping[B_PING]<WhiteToWall[Y_PING]) (*y)=LineDecline[LineState];
lilac0112_1 25:a7460e23e02e 41 else *y=1;
lilac0112_1 25:a7460e23e02e 42 }
lilac0112_1 25:a7460e23e02e 43 }
lilac0112_1 27:825c6835e3db 44 void LineJudgeSlow2(double pow_x, double pow_y, double *x, double *y, uint8_t *px, uint8_t *py){
lilac0112_1 27:825c6835e3db 45 uint8_t LineState;
lilac0112_1 27:825c6835e3db 46 uint8_t LineSum;
lilac0112_1 27:825c6835e3db 47
lilac0112_1 27:825c6835e3db 48 //line
lilac0112_1 27:825c6835e3db 49 LineState = 0;
lilac0112_1 27:825c6835e3db 50
lilac0112_1 27:825c6835e3db 51 LineSum = (data.lnRawOrder[0]!=LINE_EMPTY)+(data.lnRawOrder[1]!=LINE_EMPTY)+(data.lnRawOrder[2]!=LINE_EMPTY);
lilac0112_1 27:825c6835e3db 52 if(LineSum==3){
lilac0112_1 27:825c6835e3db 53 LineState = 3;
lilac0112_1 27:825c6835e3db 54 }
lilac0112_1 27:825c6835e3db 55 else if(LineSum==2){
lilac0112_1 27:825c6835e3db 56 LineState = 2;
lilac0112_1 27:825c6835e3db 57 }
lilac0112_1 27:825c6835e3db 58 else if(LineSum==1){
lilac0112_1 27:825c6835e3db 59 LineState = 1;
lilac0112_1 27:825c6835e3db 60 }
lilac0112_1 27:825c6835e3db 61 else if(LineSum==0){
lilac0112_1 27:825c6835e3db 62 LineState = 0;
lilac0112_1 27:825c6835e3db 63 }
lilac0112_1 27:825c6835e3db 64
lilac0112_1 27:825c6835e3db 65 if(LineSum>0){
lilac0112_1 27:825c6835e3db 66 if(pow_x>=0){
lilac0112_1 27:825c6835e3db 67 if((data.ping[R_PING]<WhiteToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
lilac0112_1 27:825c6835e3db 68 }
lilac0112_1 27:825c6835e3db 69 else{
lilac0112_1 27:825c6835e3db 70 if((data.ping[L_PING]<WhiteToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
lilac0112_1 27:825c6835e3db 71 }
lilac0112_1 27:825c6835e3db 72
lilac0112_1 27:825c6835e3db 73 if(pow_y>=0){
lilac0112_1 27:825c6835e3db 74 if((data.ping[F_PING]<WhiteToWall[Y_PING])&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
lilac0112_1 27:825c6835e3db 75 }
lilac0112_1 27:825c6835e3db 76 else{
lilac0112_1 27:825c6835e3db 77 if((data.ping[B_PING]<WhiteToWall[Y_PING])&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
lilac0112_1 27:825c6835e3db 78 }
lilac0112_1 27:825c6835e3db 79 }
lilac0112_1 27:825c6835e3db 80 else if(LineSum==0){
lilac0112_1 30:5998ba42237e 81 ////超音波による減速
lilac0112_1 27:825c6835e3db 82 if(pow_x>=0){
lilac0112_1 27:825c6835e3db 83 if((data.ping[R_PING]<GoalEdgeToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
lilac0112_1 27:825c6835e3db 84 }
lilac0112_1 27:825c6835e3db 85 else{
lilac0112_1 27:825c6835e3db 86 if((data.ping[L_PING]<GoalEdgeToWall[X_PING])&&((*px)>LineDecrease[LineState])) (*px)=LineDecrease[LineState];
lilac0112_1 27:825c6835e3db 87 }
lilac0112_1 30:5998ba42237e 88
lilac0112_1 30:5998ba42237e 89 if(pow_y>=0){
lilac0112_1 30:5998ba42237e 90 //if((data.ping[F_PING]<15)&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
lilac0112_1 30:5998ba42237e 91 }
lilac0112_1 30:5998ba42237e 92 else{
lilac0112_1 30:5998ba42237e 93 if((data.ping[B_PING]<15)&&((*py)>LineDecrease[LineState])) (*py)=LineDecrease[LineState];
lilac0112_1 30:5998ba42237e 94 }
lilac0112_1 30:5998ba42237e 95 }
lilac0112_1 30:5998ba42237e 96
lilac0112_1 30:5998ba42237e 97 if((data.lnStayNow[X_LINE]==1)&&((*px)>15)){
lilac0112_1 30:5998ba42237e 98 (*px)=15;
lilac0112_1 30:5998ba42237e 99 }
lilac0112_1 30:5998ba42237e 100 if((data.lnStayNow[Y_LINE]==1)&&((*py)>15)){
lilac0112_1 30:5998ba42237e 101 (*py)=15;
lilac0112_1 30:5998ba42237e 102 }
lilac0112_1 30:5998ba42237e 103
lilac0112_1 30:5998ba42237e 104 ////超音波による減速
lilac0112_1 30:5998ba42237e 105 if(pow_x>=0){
lilac0112_1 30:5998ba42237e 106 if((data.ping[R_PING]<GoalEdgeToWall[X_PING])&&((*px)>20)){
lilac0112_1 30:5998ba42237e 107 (*px)=20;
lilac0112_1 30:5998ba42237e 108 if((*py)>30) (*py)=30;
lilac0112_1 30:5998ba42237e 109 }
lilac0112_1 30:5998ba42237e 110 }
lilac0112_1 30:5998ba42237e 111 else{
lilac0112_1 30:5998ba42237e 112 if((data.ping[L_PING]<GoalEdgeToWall[X_PING])&&((*px)>20)){
lilac0112_1 30:5998ba42237e 113 (*px)=20;
lilac0112_1 30:5998ba42237e 114 if((*py)>30) (*py)=30;
lilac0112_1 30:5998ba42237e 115 }
lilac0112_1 30:5998ba42237e 116 }
lilac0112_1 30:5998ba42237e 117
lilac0112_1 30:5998ba42237e 118 if(pow_y>=0){
lilac0112_1 30:5998ba42237e 119 if((data.ping[F_PING]<WhiteToWallPlus[Y_PING])&&((*py)>25)) (*py)=25;
lilac0112_1 30:5998ba42237e 120 }
lilac0112_1 30:5998ba42237e 121 else{
lilac0112_1 30:5998ba42237e 122 if((data.ping[B_PING]<WhiteToWallPlus[Y_PING])&&((*py)>25)) (*py)=25;
lilac0112_1 27:825c6835e3db 123 }
lilac0112_1 27:825c6835e3db 124
lilac0112_1 27:825c6835e3db 125 *x=1;
lilac0112_1 27:825c6835e3db 126 *y=1;
lilac0112_1 27:825c6835e3db 127 }
lilac0112_1 25:a7460e23e02e 128 void LineJudgeReturn(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 25:a7460e23e02e 129 const int8_t static LineReturn[5] = {0, 0, 0, 0, 20};
lilac0112_1 25:a7460e23e02e 130 uint8_t LinePingState[4];
lilac0112_1 25:a7460e23e02e 131 //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
lilac0112_1 25:a7460e23e02e 132 //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
lilac0112_1 25:a7460e23e02e 133 //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
lilac0112_1 25:a7460e23e02e 134 //×ラインのほぼ場外では常時ラインから離れる力が優先される．
lilac0112_1 25:a7460e23e02e 135 //※場外判定を行うには再び場内に戻る必要がある．
lilac0112_1 25:a7460e23e02e 136
lilac0112_1 25:a7460e23e02e 137 if(data.FieldSpot==LINE_OUTSIDE){
lilac0112_1 25:a7460e23e02e 138
lilac0112_1 25:a7460e23e02e 139 LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWall[X_PING]);
lilac0112_1 25:a7460e23e02e 140 LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWall[X_PING]);
lilac0112_1 25:a7460e23e02e 141 LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWall[Y_PING]);
lilac0112_1 25:a7460e23e02e 142 LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWall[Y_PING]);
lilac0112_1 25:a7460e23e02e 143
lilac0112_1 25:a7460e23e02e 144 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 25:a7460e23e02e 145 data.lnStop[X_LINE]=1;
lilac0112_1 25:a7460e23e02e 146 data.lnStop[Y_LINE]=1;
lilac0112_1 25:a7460e23e02e 147
lilac0112_1 25:a7460e23e02e 148 data.ReturnDir[X_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 149 data.ReturnDir[Y_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 150
lilac0112_1 25:a7460e23e02e 151 //x
lilac0112_1 25:a7460e23e02e 152 if(data.ReturnDir[X_LINE]==L_LINE){
lilac0112_1 25:a7460e23e02e 153 if(pow_x<0){
lilac0112_1 25:a7460e23e02e 154 *x = 0;
lilac0112_1 25:a7460e23e02e 155 data.lnStop[X_LINE]=1;
lilac0112_1 25:a7460e23e02e 156 }
lilac0112_1 25:a7460e23e02e 157 else{
lilac0112_1 25:a7460e23e02e 158 *x = -LineReturn[4];
lilac0112_1 25:a7460e23e02e 159 data.lnStop[X_LINE]=0;
lilac0112_1 25:a7460e23e02e 160 }
lilac0112_1 25:a7460e23e02e 161 }
lilac0112_1 25:a7460e23e02e 162 else if(data.ReturnDir[X_LINE]==R_LINE){
lilac0112_1 25:a7460e23e02e 163 if(pow_x>0){
lilac0112_1 25:a7460e23e02e 164 *x = 0;
lilac0112_1 25:a7460e23e02e 165 data.lnStop[X_LINE]=1;
lilac0112_1 25:a7460e23e02e 166 }
lilac0112_1 25:a7460e23e02e 167 else{
lilac0112_1 25:a7460e23e02e 168 *x = LineReturn[4];
lilac0112_1 25:a7460e23e02e 169 data.lnStop[X_LINE]=0;
lilac0112_1 25:a7460e23e02e 170 }
lilac0112_1 25:a7460e23e02e 171 }
lilac0112_1 25:a7460e23e02e 172 else if(data.ReturnDir[X_LINE]==LINE_EMPTY){
lilac0112_1 25:a7460e23e02e 173 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==0)){
lilac0112_1 25:a7460e23e02e 174 /*if(pow_x>0){
lilac0112_1 25:a7460e23e02e 175 *x = -pow_x*(1-0.75);
lilac0112_1 25:a7460e23e02e 176 }
lilac0112_1 25:a7460e23e02e 177 else{
lilac0112_1 25:a7460e23e02e 178 *x = pow_x*(1-0.75);
lilac0112_1 25:a7460e23e02e 179 }*/
lilac0112_1 25:a7460e23e02e 180 data.lnStop[X_LINE]=1;
lilac0112_1 25:a7460e23e02e 181 }
lilac0112_1 25:a7460e23e02e 182 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
lilac0112_1 25:a7460e23e02e 183
lilac0112_1 25:a7460e23e02e 184 data.ReturnDir[X_LINE]=L_LINE;
lilac0112_1 25:a7460e23e02e 185
lilac0112_1 25:a7460e23e02e 186 /*if(pow_x<0){
lilac0112_1 25:a7460e23e02e 187 *x = 0;
lilac0112_1 25:a7460e23e02e 188 data.lnStop[X_LINE]=1;
lilac0112_1 25:a7460e23e02e 189 }
lilac0112_1 25:a7460e23e02e 190 else{
lilac0112_1 25:a7460e23e02e 191 *x = -LineReturn[4];
lilac0112_1 25:a7460e23e02e 192 data.lnStop[X_LINE]=0;
lilac0112_1 25:a7460e23e02e 193 }*/
lilac0112_1 25:a7460e23e02e 194 *x = -LineReturn[4];
lilac0112_1 25:a7460e23e02e 195 data.lnStop[X_LINE]=0;
lilac0112_1 25:a7460e23e02e 196 }
lilac0112_1 25:a7460e23e02e 197 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
lilac0112_1 25:a7460e23e02e 198
lilac0112_1 25:a7460e23e02e 199 data.ReturnDir[X_LINE]=R_LINE;
lilac0112_1 25:a7460e23e02e 200
lilac0112_1 25:a7460e23e02e 201 /*if(pow_x>0){
lilac0112_1 25:a7460e23e02e 202 *x = 0;
lilac0112_1 25:a7460e23e02e 203 data.lnStop[X_LINE]=1;
lilac0112_1 25:a7460e23e02e 204 }
lilac0112_1 25:a7460e23e02e 205 else{
lilac0112_1 25:a7460e23e02e 206 *x = LineReturn[4];
lilac0112_1 25:a7460e23e02e 207 data.lnStop[X_LINE]=0;
lilac0112_1 25:a7460e23e02e 208 }*/
lilac0112_1 25:a7460e23e02e 209 *x = LineReturn[4];
lilac0112_1 25:a7460e23e02e 210 data.lnStop[X_LINE]=0;
lilac0112_1 25:a7460e23e02e 211 }
lilac0112_1 25:a7460e23e02e 212 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
lilac0112_1 25:a7460e23e02e 213
lilac0112_1 25:a7460e23e02e 214 if(
lilac0112_1 25:a7460e23e02e 215 (data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)&&(data.lnOrder[2]==B_SPOT)
lilac0112_1 25:a7460e23e02e 216 ){
lilac0112_1 25:a7460e23e02e 217 *x = -LineReturn[4];
lilac0112_1 25:a7460e23e02e 218 data.lnStop[X_LINE]=0;
lilac0112_1 25:a7460e23e02e 219 }
lilac0112_1 25:a7460e23e02e 220 else if(
lilac0112_1 25:a7460e23e02e 221 (data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)&&(data.lnOrder[2]==A_SPOT)
lilac0112_1 25:a7460e23e02e 222 ){
lilac0112_1 25:a7460e23e02e 223 *x = LineReturn[4];
lilac0112_1 25:a7460e23e02e 224 data.lnStop[X_LINE]=0;
lilac0112_1 25:a7460e23e02e 225 }
lilac0112_1 25:a7460e23e02e 226 else{
lilac0112_1 25:a7460e23e02e 227 *x = 0;
lilac0112_1 25:a7460e23e02e 228 data.lnStop[X_LINE]=0;
lilac0112_1 25:a7460e23e02e 229 }
lilac0112_1 25:a7460e23e02e 230 }
lilac0112_1 25:a7460e23e02e 231 }
lilac0112_1 25:a7460e23e02e 232 //y
lilac0112_1 25:a7460e23e02e 233 if(data.ReturnDir[Y_LINE]==F_LINE){
lilac0112_1 25:a7460e23e02e 234 if(pow_y>0){
lilac0112_1 25:a7460e23e02e 235 *y = 0;
lilac0112_1 25:a7460e23e02e 236 data.lnStop[Y_LINE]=1;
lilac0112_1 25:a7460e23e02e 237 }
lilac0112_1 25:a7460e23e02e 238 else{
lilac0112_1 25:a7460e23e02e 239 *y = LineReturn[4];
lilac0112_1 25:a7460e23e02e 240 data.lnStop[Y_LINE]=0;
lilac0112_1 25:a7460e23e02e 241 }
lilac0112_1 25:a7460e23e02e 242 }
lilac0112_1 25:a7460e23e02e 243 else if(data.ReturnDir[Y_LINE]==B_LINE){
lilac0112_1 25:a7460e23e02e 244 if(pow_y<0){
lilac0112_1 25:a7460e23e02e 245 *y = 0;
lilac0112_1 25:a7460e23e02e 246 data.lnStop[Y_LINE]=1;
lilac0112_1 25:a7460e23e02e 247 }
lilac0112_1 25:a7460e23e02e 248 else{
lilac0112_1 25:a7460e23e02e 249 *y = -LineReturn[4];
lilac0112_1 25:a7460e23e02e 250 data.lnStop[Y_LINE]=0;
lilac0112_1 25:a7460e23e02e 251 }
lilac0112_1 25:a7460e23e02e 252 }
lilac0112_1 25:a7460e23e02e 253 else if(data.ReturnDir[Y_LINE]==LINE_EMPTY){
lilac0112_1 25:a7460e23e02e 254 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==0)){
lilac0112_1 25:a7460e23e02e 255 /*if(pow_y>0){
lilac0112_1 25:a7460e23e02e 256 *y = -pow_y*(1-0.75);
lilac0112_1 25:a7460e23e02e 257 }
lilac0112_1 25:a7460e23e02e 258 else{
lilac0112_1 25:a7460e23e02e 259 *y = pow_y*(1-0.75);
lilac0112_1 25:a7460e23e02e 260 }*/
lilac0112_1 25:a7460e23e02e 261 data.lnStop[Y_LINE]=1;
lilac0112_1 25:a7460e23e02e 262 }
lilac0112_1 25:a7460e23e02e 263 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){
lilac0112_1 25:a7460e23e02e 264
lilac0112_1 25:a7460e23e02e 265 data.ReturnDir[Y_LINE]=B_LINE;
lilac0112_1 25:a7460e23e02e 266
lilac0112_1 25:a7460e23e02e 267 if(pow_y<0){
lilac0112_1 25:a7460e23e02e 268 *y = 0;
lilac0112_1 25:a7460e23e02e 269 data.lnStop[Y_LINE]=1;
lilac0112_1 25:a7460e23e02e 270 }
lilac0112_1 25:a7460e23e02e 271 else{
lilac0112_1 25:a7460e23e02e 272 *y = -LineReturn[4];
lilac0112_1 25:a7460e23e02e 273 data.lnStop[Y_LINE]=0;
lilac0112_1 25:a7460e23e02e 274 }
lilac0112_1 25:a7460e23e02e 275 }
lilac0112_1 25:a7460e23e02e 276 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){
lilac0112_1 25:a7460e23e02e 277
lilac0112_1 25:a7460e23e02e 278 data.ReturnDir[Y_LINE]=F_LINE;
lilac0112_1 25:a7460e23e02e 279
lilac0112_1 25:a7460e23e02e 280 if(pow_y>0){
lilac0112_1 25:a7460e23e02e 281 *y = 0;
lilac0112_1 25:a7460e23e02e 282 data.lnStop[Y_LINE]=1;
lilac0112_1 25:a7460e23e02e 283 }
lilac0112_1 25:a7460e23e02e 284 else{
lilac0112_1 25:a7460e23e02e 285 *y = LineReturn[4];
lilac0112_1 25:a7460e23e02e 286 data.lnStop[Y_LINE]=0;
lilac0112_1 25:a7460e23e02e 287 }
lilac0112_1 25:a7460e23e02e 288 }
lilac0112_1 25:a7460e23e02e 289 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){
lilac0112_1 25:a7460e23e02e 290 if(
lilac0112_1 25:a7460e23e02e 291 (data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT)&&(data.lnOrder[2]==B_SPOT)||
lilac0112_1 25:a7460e23e02e 292 (data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)&&(data.lnOrder[2]==A_SPOT)
lilac0112_1 25:a7460e23e02e 293 ){
lilac0112_1 25:a7460e23e02e 294 *y = LineReturn[4];
lilac0112_1 25:a7460e23e02e 295 data.lnStop[Y_LINE]=0;
lilac0112_1 25:a7460e23e02e 296 }
lilac0112_1 25:a7460e23e02e 297 else if(
lilac0112_1 25:a7460e23e02e 298 (data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT)&&(data.lnOrder[2]==C_SPOT)||
lilac0112_1 25:a7460e23e02e 299 (data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)&&(data.lnOrder[2]==C_SPOT)
lilac0112_1 25:a7460e23e02e 300 ){
lilac0112_1 25:a7460e23e02e 301 *y = -LineReturn[4];
lilac0112_1 25:a7460e23e02e 302 data.lnStop[Y_LINE]=0;
lilac0112_1 25:a7460e23e02e 303 }
lilac0112_1 25:a7460e23e02e 304 else{
lilac0112_1 25:a7460e23e02e 305 *y = 0;
lilac0112_1 25:a7460e23e02e 306 data.lnStop[Y_LINE]=0;
lilac0112_1 25:a7460e23e02e 307 }
lilac0112_1 25:a7460e23e02e 308 }
lilac0112_1 25:a7460e23e02e 309 }
lilac0112_1 25:a7460e23e02e 310 //none
lilac0112_1 25:a7460e23e02e 311 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)&&(LinePingState[F_PING]==1)&&(LinePingState[B_PING]==1)){
lilac0112_1 25:a7460e23e02e 312 //turn!
lilac0112_1 25:a7460e23e02e 313 }
lilac0112_1 25:a7460e23e02e 314
lilac0112_1 25:a7460e23e02e 315 }
lilac0112_1 25:a7460e23e02e 316 else{//data.FieldSpot==LINE_INSIDE
lilac0112_1 25:a7460e23e02e 317
lilac0112_1 25:a7460e23e02e 318 data.ReturnDir[X_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 319 data.ReturnDir[Y_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 320
lilac0112_1 25:a7460e23e02e 321 *x = 0;
lilac0112_1 25:a7460e23e02e 322 *y = 0;
lilac0112_1 25:a7460e23e02e 323
lilac0112_1 25:a7460e23e02e 324 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 25:a7460e23e02e 325 data.lnStop[X_LINE]=1;
lilac0112_1 25:a7460e23e02e 326 data.lnStop[Y_LINE]=1;
lilac0112_1 27:825c6835e3db 327 /*
lilac0112_1 27:825c6835e3db 328 if(
lilac0112_1 27:825c6835e3db 329 (data.lnRawMemory[A_SPOT]==1)&&
lilac0112_1 27:825c6835e3db 330 (data.lnRawMemory[B_SPOT]==1)&&
lilac0112_1 27:825c6835e3db 331 (data.lnRawMemory[C_SPOT]==0)
lilac0112_1 27:825c6835e3db 332 ){
lilac0112_1 27:825c6835e3db 333 data.lnStop[Y_LINE]=0;
lilac0112_1 27:825c6835e3db 334 *y = -LineReturn[4];
lilac0112_1 27:825c6835e3db 335 }
lilac0112_1 27:825c6835e3db 336 else if(
lilac0112_1 27:825c6835e3db 337 (data.lnRawMemory[A_SPOT]==0)&&
lilac0112_1 27:825c6835e3db 338 (data.lnRawMemory[B_SPOT]==0)&&
lilac0112_1 27:825c6835e3db 339 (data.lnRawMemory[C_SPOT]==1)
lilac0112_1 27:825c6835e3db 340 ){
lilac0112_1 27:825c6835e3db 341 data.lnStop[Y_LINE]=0;
lilac0112_1 27:825c6835e3db 342 *y = LineReturn[4];
lilac0112_1 27:825c6835e3db 343 }*/
lilac0112_1 27:825c6835e3db 344 }
lilac0112_1 27:825c6835e3db 345 }
lilac0112_1 27:825c6835e3db 346 void LineJudgeReturn2(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 27:825c6835e3db 347 const int8_t static LineReturn[5] = {0, 0, 0, 0, 20};
lilac0112_1 27:825c6835e3db 348 uint8_t LinePingState[4];
lilac0112_1 27:825c6835e3db 349 //◎ボールを追う力とラインから離れる力の向きが違うならばラインから離れる力が優先される．
lilac0112_1 27:825c6835e3db 350 //◎ボールを追う力とラインから離れる力の向きが同じならばボールを追う力が優先される．
lilac0112_1 27:825c6835e3db 351 //◎ラインセンサ全てが場外になるまではボールを追う力は作用しない．ボールを追う力は場内に出るまで作用する．
lilac0112_1 27:825c6835e3db 352 //×ラインのほぼ場外では常時ラインから離れる力が優先される．
lilac0112_1 27:825c6835e3db 353 //※場外判定を行うには再び場内に戻る必要がある．
lilac0112_1 27:825c6835e3db 354
lilac0112_1 27:825c6835e3db 355 data.FieldSpot=LINE_INSIDE;
lilac0112_1 27:825c6835e3db 356
lilac0112_1 27:825c6835e3db 357 if(data.FieldSpot==LINE_OUTSIDE){
lilac0112_1 27:825c6835e3db 358 }
lilac0112_1 27:825c6835e3db 359 else{//data.FieldSpot==LINE_INSIDE
lilac0112_1 27:825c6835e3db 360
lilac0112_1 27:825c6835e3db 361 data.ReturnDir[X_LINE]=LINE_EMPTY;
lilac0112_1 27:825c6835e3db 362 data.ReturnDir[Y_LINE]=LINE_EMPTY;
lilac0112_1 27:825c6835e3db 363
lilac0112_1 27:825c6835e3db 364 *x = 0;
lilac0112_1 27:825c6835e3db 365 *y = 0;
lilac0112_1 27:825c6835e3db 366
lilac0112_1 27:825c6835e3db 367 //line間際の復帰力以外の力を作用させるか否か
lilac0112_1 27:825c6835e3db 368 data.lnStop[X_LINE]=1;
lilac0112_1 27:825c6835e3db 369 data.lnStop[Y_LINE]=1;
lilac0112_1 27:825c6835e3db 370
lilac0112_1 27:825c6835e3db 371 if(//y
lilac0112_1 27:825c6835e3db 372 ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==B_SPOT)&&(1))||
lilac0112_1 27:825c6835e3db 373 ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==A_SPOT)&&(1))
lilac0112_1 27:825c6835e3db 374 ){
lilac0112_1 27:825c6835e3db 375 data.lnStop[Y_LINE]=0;
lilac0112_1 27:825c6835e3db 376 *y = -LineReturn[4];
lilac0112_1 27:825c6835e3db 377 }
lilac0112_1 27:825c6835e3db 378 else if(
lilac0112_1 27:825c6835e3db 379 ((data.lnRawOrder[0]==C_SPOT)&&(1)&&(1))
lilac0112_1 27:825c6835e3db 380 ){
lilac0112_1 27:825c6835e3db 381 data.lnStop[Y_LINE]=0;
lilac0112_1 27:825c6835e3db 382 *y = LineReturn[4];
lilac0112_1 27:825c6835e3db 383 }
lilac0112_1 27:825c6835e3db 384 else if(//x
lilac0112_1 27:825c6835e3db 385 ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))
lilac0112_1 27:825c6835e3db 386 ){
lilac0112_1 27:825c6835e3db 387 *x = -LineReturn[4];
lilac0112_1 27:825c6835e3db 388 data.lnStop[X_LINE]=0;
lilac0112_1 27:825c6835e3db 389 }
lilac0112_1 27:825c6835e3db 390 else if(
lilac0112_1 27:825c6835e3db 391 ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))
lilac0112_1 27:825c6835e3db 392 ){
lilac0112_1 27:825c6835e3db 393 *x = LineReturn[4];
lilac0112_1 27:825c6835e3db 394 data.lnStop[X_LINE]=0;
lilac0112_1 27:825c6835e3db 395 }
lilac0112_1 27:825c6835e3db 396
lilac0112_1 27:825c6835e3db 397 if(
lilac0112_1 27:825c6835e3db 398 ((*x)!=0)||
lilac0112_1 27:825c6835e3db 399 ((*y)!=0)
lilac0112_1 27:825c6835e3db 400 ){
lilac0112_1 27:825c6835e3db 401
lilac0112_1 27:825c6835e3db 402 data.lnRawReturn=1;
lilac0112_1 27:825c6835e3db 403
lilac0112_1 27:825c6835e3db 404 LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWallPlus[X_PING]);
lilac0112_1 27:825c6835e3db 405 LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWallPlus[X_PING]);
lilac0112_1 27:825c6835e3db 406 LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWallPlus[Y_PING]);
lilac0112_1 27:825c6835e3db 407 LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWallPlus[Y_PING]);
lilac0112_1 27:825c6835e3db 408 //x
lilac0112_1 27:825c6835e3db 409 if(data.lnStop[X_LINE]==1){
lilac0112_1 27:825c6835e3db 410 if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){
lilac0112_1 27:825c6835e3db 411 *x = -LineReturn[4];
lilac0112_1 27:825c6835e3db 412 data.lnStop[X_LINE]=0;
lilac0112_1 27:825c6835e3db 413 }
lilac0112_1 27:825c6835e3db 414 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){
lilac0112_1 27:825c6835e3db 415 *x = LineReturn[4];
lilac0112_1 27:825c6835e3db 416 data.lnStop[X_LINE]=0;
lilac0112_1 27:825c6835e3db 417 }
lilac0112_1 27:825c6835e3db 418 if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){
lilac0112_1 27:825c6835e3db 419 *x = 0;
lilac0112_1 27:825c6835e3db 420 data.lnStop[X_LINE]=0;
lilac0112_1 27:825c6835e3db 421 }
lilac0112_1 27:825c6835e3db 422 }
lilac0112_1 27:825c6835e3db 423 //y
lilac0112_1 27:825c6835e3db 424 if(data.lnStop[Y_LINE]==1){
lilac0112_1 27:825c6835e3db 425 if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){
lilac0112_1 27:825c6835e3db 426 if(pow_y<0){
lilac0112_1 27:825c6835e3db 427 *y = 0;
lilac0112_1 27:825c6835e3db 428 data.lnStop[Y_LINE]=1;
lilac0112_1 27:825c6835e3db 429 }
lilac0112_1 27:825c6835e3db 430 else{
lilac0112_1 27:825c6835e3db 431 *y = -LineReturn[4];
lilac0112_1 27:825c6835e3db 432 data.lnStop[Y_LINE]=0;
lilac0112_1 27:825c6835e3db 433 }
lilac0112_1 27:825c6835e3db 434 }
lilac0112_1 27:825c6835e3db 435 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){
lilac0112_1 27:825c6835e3db 436 if(pow_y>0){
lilac0112_1 27:825c6835e3db 437 *y = 0;
lilac0112_1 27:825c6835e3db 438 data.lnStop[Y_LINE]=1;
lilac0112_1 27:825c6835e3db 439 }
lilac0112_1 27:825c6835e3db 440 else{
lilac0112_1 27:825c6835e3db 441 *y = LineReturn[4];
lilac0112_1 27:825c6835e3db 442 data.lnStop[Y_LINE]=0;
lilac0112_1 27:825c6835e3db 443 }
lilac0112_1 27:825c6835e3db 444 }
lilac0112_1 27:825c6835e3db 445 if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){
lilac0112_1 27:825c6835e3db 446 *y = 0;
lilac0112_1 27:825c6835e3db 447 data.lnStop[Y_LINE]=0;
lilac0112_1 27:825c6835e3db 448 }
lilac0112_1 27:825c6835e3db 449 }
lilac0112_1 27:825c6835e3db 450 }
lilac0112_1 27:825c6835e3db 451 else{
lilac0112_1 27:825c6835e3db 452 data.lnRawReturn=0;
lilac0112_1 27:825c6835e3db 453 }
lilac0112_1 27:825c6835e3db 454
lilac0112_1 25:a7460e23e02e 455 }
lilac0112_1 25:a7460e23e02e 456 }
lilac0112_1 25:a7460e23e02e 457 void LineJudgeReset(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 25:a7460e23e02e 458 //static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
lilac0112_1 25:a7460e23e02e 459 //static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
lilac0112_1 25:a7460e23e02e 460 static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
lilac0112_1 25:a7460e23e02e 461 static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY};
lilac0112_1 25:a7460e23e02e 462
lilac0112_1 25:a7460e23e02e 463 if((/*data.lnRaw==0*/1)&&(data.lnHold==7)){
lilac0112_1 25:a7460e23e02e 464 if(data.FieldSpot==LINE_INSIDE){
lilac0112_1 25:a7460e23e02e 465
lilac0112_1 25:a7460e23e02e 466 data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]);
lilac0112_1 25:a7460e23e02e 467 data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]);
lilac0112_1 25:a7460e23e02e 468 data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]);
lilac0112_1 25:a7460e23e02e 469 data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]);
lilac0112_1 25:a7460e23e02e 470 if(
lilac0112_1 25:a7460e23e02e 471 (data.lnCorner[L_LINE])||
lilac0112_1 25:a7460e23e02e 472 (data.lnCorner[R_LINE])||
lilac0112_1 25:a7460e23e02e 473 (data.lnCorner[F_LINE])||
lilac0112_1 25:a7460e23e02e 474 (data.lnCorner[B_LINE])
lilac0112_1 25:a7460e23e02e 475 ){
lilac0112_1 25:a7460e23e02e 476
lilac0112_1 25:a7460e23e02e 477 data.NonWall[L_LINE] = (data.ping[L_PING]>WhiteToWall[X_PING]);
lilac0112_1 25:a7460e23e02e 478 data.NonWall[R_LINE] = (data.ping[R_PING]>WhiteToWall[X_PING]);
lilac0112_1 25:a7460e23e02e 479 data.NonWall[F_LINE] = (data.ping[F_PING]>WhiteToWall[Y_PING]);
lilac0112_1 25:a7460e23e02e 480 data.NonWall[B_LINE] = (data.ping[B_PING]>WhiteToWall[Y_PING]);
lilac0112_1 25:a7460e23e02e 481
lilac0112_1 25:a7460e23e02e 482
lilac0112_1 25:a7460e23e02e 483
lilac0112_1 25:a7460e23e02e 484 ///*
lilac0112_1 25:a7460e23e02e 485 LastLineCorner[L_LINE]=NewLineCorner[L_LINE];
lilac0112_1 25:a7460e23e02e 486 LastLineCorner[R_LINE]=NewLineCorner[R_LINE];
lilac0112_1 25:a7460e23e02e 487 LastLineCorner[F_LINE]=NewLineCorner[F_LINE];
lilac0112_1 25:a7460e23e02e 488 LastLineCorner[B_LINE]=NewLineCorner[B_LINE];
lilac0112_1 25:a7460e23e02e 489
lilac0112_1 25:a7460e23e02e 490 NewLineCorner[L_LINE]=data.lnCorner[L_LINE];
lilac0112_1 25:a7460e23e02e 491 NewLineCorner[R_LINE]=data.lnCorner[R_LINE];
lilac0112_1 25:a7460e23e02e 492 NewLineCorner[F_LINE]=data.lnCorner[F_LINE];
lilac0112_1 25:a7460e23e02e 493 NewLineCorner[B_LINE]=data.lnCorner[B_LINE];
lilac0112_1 25:a7460e23e02e 494
lilac0112_1 25:a7460e23e02e 495 if(
lilac0112_1 25:a7460e23e02e 496 (LastLineCorner[L_LINE]==NewLineCorner[L_LINE])&&
lilac0112_1 25:a7460e23e02e 497 (LastLineCorner[R_LINE]==NewLineCorner[R_LINE])&&
lilac0112_1 25:a7460e23e02e 498 (LastLineCorner[F_LINE]==NewLineCorner[F_LINE])&&
lilac0112_1 25:a7460e23e02e 499 (LastLineCorner[B_LINE]==NewLineCorner[B_LINE])
lilac0112_1 25:a7460e23e02e 500 ){
lilac0112_1 25:a7460e23e02e 501 data.lnRepeat++;
lilac0112_1 25:a7460e23e02e 502 }
lilac0112_1 25:a7460e23e02e 503 else{
lilac0112_1 25:a7460e23e02e 504 data.lnRepeat=0;
lilac0112_1 25:a7460e23e02e 505 }
lilac0112_1 25:a7460e23e02e 506 //*/
lilac0112_1 25:a7460e23e02e 507
lilac0112_1 25:a7460e23e02e 508 data.FieldSpot = LINE_OUTSIDE;
lilac0112_1 25:a7460e23e02e 509 LineLiberate();
lilac0112_1 25:a7460e23e02e 510 }
lilac0112_1 25:a7460e23e02e 511 }
lilac0112_1 25:a7460e23e02e 512 else if(data.FieldSpot==LINE_OUTSIDE){
lilac0112_1 25:a7460e23e02e 513 if(data.lnRaw==0){
lilac0112_1 25:a7460e23e02e 514 data.FieldSpot = LINE_INSIDE;
lilac0112_1 25:a7460e23e02e 515 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 25:a7460e23e02e 516 LineLiberate();
lilac0112_1 25:a7460e23e02e 517 LineRankClear();
lilac0112_1 25:a7460e23e02e 518 }
lilac0112_1 25:a7460e23e02e 519 }
lilac0112_1 25:a7460e23e02e 520 }
lilac0112_1 25:a7460e23e02e 521 if(data.FieldSpot == LINE_OUTSIDE){
lilac0112_1 25:a7460e23e02e 522 if(
lilac0112_1 25:a7460e23e02e 523 (
lilac0112_1 25:a7460e23e02e 524 (data.ping[L_PING]>=WhiteToWall[X_PING])||
lilac0112_1 25:a7460e23e02e 525 (data.NonWall[L_LINE]==1)||
lilac0112_1 25:a7460e23e02e 526 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 25:a7460e23e02e 527 //(data.lnOrder[0]==A_SPOT)
lilac0112_1 25:a7460e23e02e 528 )&&
lilac0112_1 25:a7460e23e02e 529 (
lilac0112_1 25:a7460e23e02e 530 (data.ping[R_PING]>=WhiteToWall[X_PING])||
lilac0112_1 25:a7460e23e02e 531 (data.NonWall[R_LINE]==1)||
lilac0112_1 25:a7460e23e02e 532 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 25:a7460e23e02e 533 //(data.lnOrder[0]==B_SPOT)
lilac0112_1 25:a7460e23e02e 534 )&&
lilac0112_1 25:a7460e23e02e 535 (
lilac0112_1 25:a7460e23e02e 536 (data.ping[F_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 25:a7460e23e02e 537 (data.NonWall[F_LINE]==1)||
lilac0112_1 25:a7460e23e02e 538 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
lilac0112_1 25:a7460e23e02e 539 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
lilac0112_1 25:a7460e23e02e 540 )&&
lilac0112_1 25:a7460e23e02e 541 ((data.ping[B_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 25:a7460e23e02e 542 (data.NonWall[B_LINE]==1)||
lilac0112_1 25:a7460e23e02e 543 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
lilac0112_1 25:a7460e23e02e 544 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
lilac0112_1 25:a7460e23e02e 545 )
lilac0112_1 25:a7460e23e02e 546 ){
lilac0112_1 25:a7460e23e02e 547 data.FieldSpot = LINE_INSIDE;
lilac0112_1 25:a7460e23e02e 548 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 25:a7460e23e02e 549 LineLiberate();
lilac0112_1 25:a7460e23e02e 550 LineRankClear();
lilac0112_1 25:a7460e23e02e 551 }
lilac0112_1 25:a7460e23e02e 552 }
lilac0112_1 25:a7460e23e02e 553 if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){
lilac0112_1 25:a7460e23e02e 554 if(
lilac0112_1 25:a7460e23e02e 555 (
lilac0112_1 25:a7460e23e02e 556 (
lilac0112_1 25:a7460e23e02e 557 (data.ping[L_PING]>=WhiteToWall[X_PING])||
lilac0112_1 25:a7460e23e02e 558 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 25:a7460e23e02e 559 //(data.lnOrder[0]==A_SPOT)
lilac0112_1 25:a7460e23e02e 560 )&&
lilac0112_1 25:a7460e23e02e 561 (
lilac0112_1 25:a7460e23e02e 562 (data.ping[R_PING]>=WhiteToWall[X_PING])||
lilac0112_1 25:a7460e23e02e 563 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT))
lilac0112_1 25:a7460e23e02e 564 //(data.lnOrder[0]==B_SPOT)
lilac0112_1 25:a7460e23e02e 565 )&&
lilac0112_1 25:a7460e23e02e 566 (
lilac0112_1 25:a7460e23e02e 567 (data.ping[F_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 25:a7460e23e02e 568 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))||
lilac0112_1 25:a7460e23e02e 569 ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT))
lilac0112_1 25:a7460e23e02e 570 )&&
lilac0112_1 25:a7460e23e02e 571 (
lilac0112_1 25:a7460e23e02e 572 (data.ping[B_PING]>=WhiteToWall[Y_PING])||
lilac0112_1 25:a7460e23e02e 573 ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))||
lilac0112_1 25:a7460e23e02e 574 ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT))
lilac0112_1 25:a7460e23e02e 575 )
lilac0112_1 25:a7460e23e02e 576 )
lilac0112_1 25:a7460e23e02e 577 ||
lilac0112_1 25:a7460e23e02e 578 (
lilac0112_1 25:a7460e23e02e 579 (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
lilac0112_1 25:a7460e23e02e 580 (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
lilac0112_1 25:a7460e23e02e 581 )
lilac0112_1 25:a7460e23e02e 582 ){
lilac0112_1 25:a7460e23e02e 583 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 25:a7460e23e02e 584 LineLiberate();
lilac0112_1 25:a7460e23e02e 585 }
lilac0112_1 25:a7460e23e02e 586 }
lilac0112_1 25:a7460e23e02e 587 ///*
lilac0112_1 25:a7460e23e02e 588 if(
lilac0112_1 25:a7460e23e02e 589 (data.irNotice==IR_NONE)||
lilac0112_1 25:a7460e23e02e 590 (data.irNotice==IR_FAR)||
lilac0112_1 25:a7460e23e02e 591 (
lilac0112_1 25:a7460e23e02e 592 (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&&
lilac0112_1 25:a7460e23e02e 593 (data.ping[R_PING]>=GoalEdgeToWall[X_PING])
lilac0112_1 25:a7460e23e02e 594 )
lilac0112_1 25:a7460e23e02e 595 ){
lilac0112_1 25:a7460e23e02e 596 data.lnRepeat = 0;
lilac0112_1 25:a7460e23e02e 597
lilac0112_1 25:a7460e23e02e 598 NewLineCorner[L_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 599 NewLineCorner[R_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 600 NewLineCorner[F_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 601 NewLineCorner[B_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 602
lilac0112_1 25:a7460e23e02e 603 LastLineCorner[L_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 604 LastLineCorner[R_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 605 LastLineCorner[F_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 606 LastLineCorner[B_LINE]=LINE_EMPTY;
lilac0112_1 25:a7460e23e02e 607 }
lilac0112_1 25:a7460e23e02e 608 //data.lnRepeat=0;
lilac0112_1 25:a7460e23e02e 609 if((data.lnRepeat>0)&&(data.FieldSpot == LINE_INSIDE)){
lilac0112_1 25:a7460e23e02e 610 //x
lilac0112_1 25:a7460e23e02e 611 if(
lilac0112_1 25:a7460e23e02e 612 ((pow_x>=0)&&(NewLineCorner[R_LINE]))||
lilac0112_1 25:a7460e23e02e 613 ((pow_x<0)&&(NewLineCorner[L_LINE]))
lilac0112_1 25:a7460e23e02e 614 ){
lilac0112_1 25:a7460e23e02e 615 data.lnStay[X_LINE]=0;
lilac0112_1 25:a7460e23e02e 616 }
lilac0112_1 25:a7460e23e02e 617 else{
lilac0112_1 25:a7460e23e02e 618 data.lnStay[X_LINE]=1;
lilac0112_1 25:a7460e23e02e 619 }
lilac0112_1 25:a7460e23e02e 620 //y
lilac0112_1 25:a7460e23e02e 621 if(
lilac0112_1 25:a7460e23e02e 622 ((pow_y>=0)&&(NewLineCorner[F_LINE]))||
lilac0112_1 25:a7460e23e02e 623 ((pow_y<0)&&(NewLineCorner[B_LINE]))
lilac0112_1 25:a7460e23e02e 624 ){
lilac0112_1 25:a7460e23e02e 625 data.lnStay[Y_LINE]=0;
lilac0112_1 25:a7460e23e02e 626 }
lilac0112_1 25:a7460e23e02e 627 else{
lilac0112_1 25:a7460e23e02e 628 data.lnStay[Y_LINE]=1;
lilac0112_1 25:a7460e23e02e 629 }
lilac0112_1 25:a7460e23e02e 630 }
lilac0112_1 25:a7460e23e02e 631 else{
lilac0112_1 25:a7460e23e02e 632 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
lilac0112_1 25:a7460e23e02e 633 }
lilac0112_1 25:a7460e23e02e 634 //*/
lilac0112_1 25:a7460e23e02e 635
lilac0112_1 25:a7460e23e02e 636 }
lilac0112_1 27:825c6835e3db 637 void LineJudgeReset2(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 30:5998ba42237e 638
lilac0112_1 30:5998ba42237e 639 if(data.lnRepeat==0){
lilac0112_1 30:5998ba42237e 640 data.lnStayNow[X_LINE]=0;
lilac0112_1 30:5998ba42237e 641 data.lnStayNow[Y_LINE]=0;
lilac0112_1 30:5998ba42237e 642 }
lilac0112_1 30:5998ba42237e 643 data.lnStay[X_LINE]=1;
lilac0112_1 30:5998ba42237e 644 data.lnStay[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 645
lilac0112_1 30:5998ba42237e 646 if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){
lilac0112_1 27:825c6835e3db 647 data.lnRepeat=1;
lilac0112_1 27:825c6835e3db 648 }
lilac0112_1 27:825c6835e3db 649
lilac0112_1 27:825c6835e3db 650 if(
lilac0112_1 27:825c6835e3db 651 (data.irNotice==IR_NONE)||
lilac0112_1 30:5998ba42237e 652 //(data.irNotice==IR_FAR)||
lilac0112_1 30:5998ba42237e 653 //(data.irLastPosition<=7)||
lilac0112_1 27:825c6835e3db 654 (
lilac0112_1 30:5998ba42237e 655 (!(
lilac0112_1 30:5998ba42237e 656 (data.irPosition==data.irLastPosition)||
lilac0112_1 30:5998ba42237e 657 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))||
lilac0112_1 30:5998ba42237e 658 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12]))||
lilac0112_1 30:5998ba42237e 659 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))||
lilac0112_1 33:aa115c30892e 660 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12]))
lilac0112_1 33:aa115c30892e 661 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))&&(cmps_set.GoalDeg==0))||
lilac0112_1 33:aa115c30892e 662 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0))
lilac0112_1 30:5998ba42237e 663 ))&&(
lilac0112_1 30:5998ba42237e 664 (data.irLastNotice==IR_CLOSE)||
lilac0112_1 30:5998ba42237e 665 (data.irLastNotice==IR_CLOSER)
lilac0112_1 30:5998ba42237e 666 )
lilac0112_1 27:825c6835e3db 667 )
lilac0112_1 27:825c6835e3db 668 ){
lilac0112_1 27:825c6835e3db 669 data.lnRepeat = 0;
lilac0112_1 27:825c6835e3db 670
lilac0112_1 27:825c6835e3db 671 LineRawLogReset();
lilac0112_1 27:825c6835e3db 672 }
lilac0112_1 27:825c6835e3db 673 //data.lnRepeat=0;
lilac0112_1 28:f246a5fbf6a5 674 //if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){
lilac0112_1 30:5998ba42237e 675 if(
lilac0112_1 30:5998ba42237e 676 (data.lnRepeat>=1)&&
lilac0112_1 30:5998ba42237e 677 (data.lnRawOrder[0]==LINE_EMPTY)/*&&
lilac0112_1 30:5998ba42237e 678 (
lilac0112_1 30:5998ba42237e 679 (
lilac0112_1 30:5998ba42237e 680 (data.irPosition==data.irLastPosition)||
lilac0112_1 30:5998ba42237e 681 (data.irPosition==(data.irLastPosition+1))||
lilac0112_1 30:5998ba42237e 682 (data.irPosition==(data.irLastPosition-1))||
lilac0112_1 30:5998ba42237e 683 (data.irPosition==(data.irLastPosition+11))||
lilac0112_1 30:5998ba42237e 684 (data.irPosition==(data.irLastPosition-11))
lilac0112_1 30:5998ba42237e 685 )&&
lilac0112_1 30:5998ba42237e 686 (
lilac0112_1 30:5998ba42237e 687 (data.irNotice==IR_CLOSE)||
lilac0112_1 30:5998ba42237e 688 (data.irNotice==IR_CLOSER)
lilac0112_1 30:5998ba42237e 689 )
lilac0112_1 30:5998ba42237e 690 )*/
lilac0112_1 30:5998ba42237e 691 ){
lilac0112_1 27:825c6835e3db 692 //y
lilac0112_1 27:825c6835e3db 693 if(
lilac0112_1 27:825c6835e3db 694 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))||
lilac0112_1 30:5998ba42237e 695 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1)))
lilac0112_1 30:5998ba42237e 696 ){
lilac0112_1 30:5998ba42237e 697 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 30:5998ba42237e 698 data.lnStay[Y_LINE]=0;
lilac0112_1 30:5998ba42237e 699 }
lilac0112_1 30:5998ba42237e 700 else{
lilac0112_1 30:5998ba42237e 701 data.lnStay[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 702 }
lilac0112_1 30:5998ba42237e 703 data.lnStay[Y_LINE]=0;
lilac0112_1 30:5998ba42237e 704 data.lnStayNow[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 705 }
lilac0112_1 30:5998ba42237e 706 else{
lilac0112_1 30:5998ba42237e 707 data.lnStay[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 708 }
lilac0112_1 30:5998ba42237e 709 if(
lilac0112_1 27:825c6835e3db 710 ((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1)))
lilac0112_1 27:825c6835e3db 711 ){
lilac0112_1 30:5998ba42237e 712 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 30:5998ba42237e 713 data.lnStay[Y_LINE]=0;
lilac0112_1 30:5998ba42237e 714 }
lilac0112_1 30:5998ba42237e 715 else{
lilac0112_1 30:5998ba42237e 716 data.lnStay[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 717 }
lilac0112_1 27:825c6835e3db 718 data.lnStay[Y_LINE]=0;
lilac0112_1 30:5998ba42237e 719 data.lnStayNow[Y_LINE]=1;
lilac0112_1 27:825c6835e3db 720 }
lilac0112_1 27:825c6835e3db 721 else{
lilac0112_1 27:825c6835e3db 722 data.lnStay[Y_LINE]=1;
lilac0112_1 27:825c6835e3db 723 }
lilac0112_1 27:825c6835e3db 724 //x
lilac0112_1 27:825c6835e3db 725 if(
lilac0112_1 30:5998ba42237e 726 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
lilac0112_1 30:5998ba42237e 727 ){
lilac0112_1 30:5998ba42237e 728 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 30:5998ba42237e 729 data.lnStay[X_LINE]=0;
lilac0112_1 30:5998ba42237e 730 }
lilac0112_1 30:5998ba42237e 731 else{
lilac0112_1 30:5998ba42237e 732 data.lnStay[X_LINE]=1;
lilac0112_1 30:5998ba42237e 733 }
lilac0112_1 30:5998ba42237e 734 data.lnStay[X_LINE]=0;
lilac0112_1 30:5998ba42237e 735 data.lnStayNow[X_LINE]=1;
lilac0112_1 30:5998ba42237e 736 }
lilac0112_1 30:5998ba42237e 737 else{
lilac0112_1 30:5998ba42237e 738 data.lnStay[X_LINE]=1;
lilac0112_1 30:5998ba42237e 739 }
lilac0112_1 30:5998ba42237e 740 if(
lilac0112_1 27:825c6835e3db 741 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
lilac0112_1 27:825c6835e3db 742 ){
lilac0112_1 30:5998ba42237e 743 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 30:5998ba42237e 744 data.lnStay[X_LINE]=0;
lilac0112_1 30:5998ba42237e 745 }
lilac0112_1 30:5998ba42237e 746 else{
lilac0112_1 30:5998ba42237e 747 data.lnStay[X_LINE]=1;
lilac0112_1 30:5998ba42237e 748 }
lilac0112_1 27:825c6835e3db 749 data.lnStay[X_LINE]=0;
lilac0112_1 30:5998ba42237e 750 data.lnStayNow[X_LINE]=1;
lilac0112_1 27:825c6835e3db 751 }
lilac0112_1 27:825c6835e3db 752 else{
lilac0112_1 27:825c6835e3db 753 data.lnStay[X_LINE]=1;
lilac0112_1 27:825c6835e3db 754 }
lilac0112_1 30:5998ba42237e 755 if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){
lilac0112_1 30:5998ba42237e 756 //y2
lilac0112_1 30:5998ba42237e 757 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
lilac0112_1 30:5998ba42237e 758 if(
lilac0112_1 30:5998ba42237e 759 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 30:5998ba42237e 760 ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 30:5998ba42237e 761 ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==18)||(data.irPosition==19)))
lilac0112_1 30:5998ba42237e 762 ){
lilac0112_1 30:5998ba42237e 763 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 30:5998ba42237e 764 data.lnStay[X_LINE]=0;
lilac0112_1 30:5998ba42237e 765 }
lilac0112_1 30:5998ba42237e 766 else{
lilac0112_1 30:5998ba42237e 767 data.lnStay[X_LINE]=1;
lilac0112_1 30:5998ba42237e 768 }
lilac0112_1 30:5998ba42237e 769 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 30:5998ba42237e 770 data.lnStay[X_LINE]=0;
lilac0112_1 30:5998ba42237e 771 }
lilac0112_1 30:5998ba42237e 772 data.lnStayNow[X_LINE]=1;
lilac0112_1 30:5998ba42237e 773 }
lilac0112_1 30:5998ba42237e 774 else{
lilac0112_1 30:5998ba42237e 775 data.lnStay[X_LINE]=1;
lilac0112_1 30:5998ba42237e 776 }
lilac0112_1 30:5998ba42237e 777 }
lilac0112_1 30:5998ba42237e 778 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
lilac0112_1 30:5998ba42237e 779 if(
lilac0112_1 30:5998ba42237e 780 ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
lilac0112_1 30:5998ba42237e 781 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
lilac0112_1 30:5998ba42237e 782 ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==15)||(data.irPosition==16)))
lilac0112_1 30:5998ba42237e 783 ){
lilac0112_1 30:5998ba42237e 784 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 30:5998ba42237e 785 data.lnStay[X_LINE]=0;
lilac0112_1 30:5998ba42237e 786 }
lilac0112_1 30:5998ba42237e 787 else{
lilac0112_1 30:5998ba42237e 788 data.lnStay[X_LINE]=1;
lilac0112_1 30:5998ba42237e 789 }
lilac0112_1 30:5998ba42237e 790 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 30:5998ba42237e 791 data.lnStay[X_LINE]=0;
lilac0112_1 30:5998ba42237e 792 }
lilac0112_1 30:5998ba42237e 793 data.lnStayNow[X_LINE]=1;
lilac0112_1 30:5998ba42237e 794 }
lilac0112_1 30:5998ba42237e 795 else{
lilac0112_1 30:5998ba42237e 796 data.lnStay[X_LINE]=1;
lilac0112_1 30:5998ba42237e 797 }
lilac0112_1 30:5998ba42237e 798 }
lilac0112_1 30:5998ba42237e 799 //x2
lilac0112_1 30:5998ba42237e 800 if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){
lilac0112_1 30:5998ba42237e 801 if(
lilac0112_1 30:5998ba42237e 802 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 30:5998ba42237e 803 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))
lilac0112_1 30:5998ba42237e 804 ){
lilac0112_1 30:5998ba42237e 805 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 30:5998ba42237e 806 data.lnStay[Y_LINE]=0;
lilac0112_1 30:5998ba42237e 807 }
lilac0112_1 30:5998ba42237e 808 else{
lilac0112_1 30:5998ba42237e 809 data.lnStay[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 810 }
lilac0112_1 30:5998ba42237e 811 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 30:5998ba42237e 812 data.lnStay[Y_LINE]=0;
lilac0112_1 30:5998ba42237e 813 }
lilac0112_1 30:5998ba42237e 814 data.lnStayNow[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 815 }
lilac0112_1 30:5998ba42237e 816 else{
lilac0112_1 30:5998ba42237e 817 data.lnStay[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 818 }
lilac0112_1 30:5998ba42237e 819
lilac0112_1 30:5998ba42237e 820 if(
lilac0112_1 30:5998ba42237e 821 ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))||
lilac0112_1 30:5998ba42237e 822 ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16)))
lilac0112_1 30:5998ba42237e 823 ){
lilac0112_1 30:5998ba42237e 824 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 30:5998ba42237e 825 data.lnStay[Y_LINE]=0;
lilac0112_1 30:5998ba42237e 826 }
lilac0112_1 30:5998ba42237e 827 else{
lilac0112_1 30:5998ba42237e 828 data.lnStay[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 829 }
lilac0112_1 30:5998ba42237e 830 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 30:5998ba42237e 831 data.lnStay[Y_LINE]=0;
lilac0112_1 30:5998ba42237e 832 }
lilac0112_1 30:5998ba42237e 833 data.lnStayNow[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 834 }
lilac0112_1 30:5998ba42237e 835 else{
lilac0112_1 30:5998ba42237e 836 data.lnStay[Y_LINE]=1;
lilac0112_1 30:5998ba42237e 837 }
lilac0112_1 30:5998ba42237e 838 }
lilac0112_1 30:5998ba42237e 839 if(
lilac0112_1 30:5998ba42237e 840 (data.lnRepeat>=2)
lilac0112_1 30:5998ba42237e 841 ){
lilac0112_1 30:5998ba42237e 842 if(
lilac0112_1 30:5998ba42237e 843 ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))||
lilac0112_1 30:5998ba42237e 844 ((pow_y<0)&&((data.ping[B_PING]<40)&&(1)))
lilac0112_1 30:5998ba42237e 845 ){
lilac0112_1 30:5998ba42237e 846 data.lnStay[Y_LINE]=0;
lilac0112_1 30:5998ba42237e 847 }
lilac0112_1 30:5998ba42237e 848 if(
lilac0112_1 30:5998ba42237e 849 ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))||
lilac0112_1 30:5998ba42237e 850 ((pow_x<0)&&((data.ping[L_PING]<40)&&(1)))
lilac0112_1 30:5998ba42237e 851 ){
lilac0112_1 30:5998ba42237e 852 data.lnStay[X_LINE]=0;
lilac0112_1 30:5998ba42237e 853 }
lilac0112_1 30:5998ba42237e 854 }
lilac0112_1 30:5998ba42237e 855 }
lilac0112_1 30:5998ba42237e 856
lilac0112_1 30:5998ba42237e 857
lilac0112_1 27:825c6835e3db 858 }
lilac0112_1 27:825c6835e3db 859 else{
lilac0112_1 27:825c6835e3db 860 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
lilac0112_1 27:825c6835e3db 861 }
lilac0112_1 27:825c6835e3db 862
lilac0112_1 27:825c6835e3db 863 }
lilac0112_1 27:825c6835e3db 864 void LineJudgeReset3(double pow_x, double pow_y, double *x, double *y){
lilac0112_1 27:825c6835e3db 865
lilac0112_1 34:1c86c1299ea4 866 if(data.lnRepeat==0){
lilac0112_1 34:1c86c1299ea4 867 data.lnStayNow[X_LINE]=0;
lilac0112_1 34:1c86c1299ea4 868 data.lnStayNow[Y_LINE]=0;
lilac0112_1 27:825c6835e3db 869 }
lilac0112_1 34:1c86c1299ea4 870 data.lnStay[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 871 data.lnStay[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 872
lilac0112_1 34:1c86c1299ea4 873 if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){
lilac0112_1 34:1c86c1299ea4 874 data.lnRepeat=1;
lilac0112_1 27:825c6835e3db 875 }
lilac0112_1 34:1c86c1299ea4 876
lilac0112_1 27:825c6835e3db 877 if(
lilac0112_1 27:825c6835e3db 878 (data.irNotice==IR_NONE)||
lilac0112_1 27:825c6835e3db 879 (data.irNotice==IR_FAR)||
lilac0112_1 34:1c86c1299ea4 880 //(data.irLastPosition<=7)||
lilac0112_1 27:825c6835e3db 881 (
lilac0112_1 34:1c86c1299ea4 882 (!(
lilac0112_1 34:1c86c1299ea4 883 (data.irPosition==data.irLastPosition)||
lilac0112_1 34:1c86c1299ea4 884 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))||
lilac0112_1 34:1c86c1299ea4 885 (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12]))
lilac0112_1 34:1c86c1299ea4 886 //(data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))||
lilac0112_1 34:1c86c1299ea4 887 //(data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12]))
lilac0112_1 34:1c86c1299ea4 888 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))&&(cmps_set.GoalDeg==0))||
lilac0112_1 34:1c86c1299ea4 889 //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0))
lilac0112_1 34:1c86c1299ea4 890 ))&&(
lilac0112_1 34:1c86c1299ea4 891 (data.irLastNotice==IR_CLOSE)||
lilac0112_1 34:1c86c1299ea4 892 (data.irLastNotice==IR_CLOSER)
lilac0112_1 34:1c86c1299ea4 893 )
lilac0112_1 27:825c6835e3db 894 )
lilac0112_1 27:825c6835e3db 895 ){
lilac0112_1 27:825c6835e3db 896 data.lnRepeat = 0;
lilac0112_1 27:825c6835e3db 897
lilac0112_1 34:1c86c1299ea4 898 LineRawLogReset();
lilac0112_1 27:825c6835e3db 899 }
lilac0112_1 27:825c6835e3db 900 //data.lnRepeat=0;
lilac0112_1 34:1c86c1299ea4 901 //if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){
lilac0112_1 34:1c86c1299ea4 902 if(
lilac0112_1 34:1c86c1299ea4 903 (data.lnRepeat>=1)&&
lilac0112_1 34:1c86c1299ea4 904 (data.lnRawOrder[0]==LINE_EMPTY)/*&&
lilac0112_1 34:1c86c1299ea4 905 (
lilac0112_1 34:1c86c1299ea4 906 (
lilac0112_1 34:1c86c1299ea4 907 (data.irPosition==data.irLastPosition)||
lilac0112_1 34:1c86c1299ea4 908 (data.irPosition==(data.irLastPosition+1))||
lilac0112_1 34:1c86c1299ea4 909 (data.irPosition==(data.irLastPosition-1))||
lilac0112_1 34:1c86c1299ea4 910 (data.irPosition==(data.irLastPosition+11))||
lilac0112_1 34:1c86c1299ea4 911 (data.irPosition==(data.irLastPosition-11))
lilac0112_1 34:1c86c1299ea4 912 )&&
lilac0112_1 34:1c86c1299ea4 913 (
lilac0112_1 34:1c86c1299ea4 914 (data.irNotice==IR_CLOSE)||
lilac0112_1 34:1c86c1299ea4 915 (data.irNotice==IR_CLOSER)
lilac0112_1 34:1c86c1299ea4 916 )
lilac0112_1 34:1c86c1299ea4 917 )*/
lilac0112_1 34:1c86c1299ea4 918 ){
lilac0112_1 34:1c86c1299ea4 919 //y
lilac0112_1 34:1c86c1299ea4 920 if(
lilac0112_1 34:1c86c1299ea4 921 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))||
lilac0112_1 34:1c86c1299ea4 922 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1)))
lilac0112_1 34:1c86c1299ea4 923 ){
lilac0112_1 34:1c86c1299ea4 924 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 34:1c86c1299ea4 925 data.lnStay[Y_LINE]=0;
lilac0112_1 34:1c86c1299ea4 926 }
lilac0112_1 34:1c86c1299ea4 927 else{
lilac0112_1 34:1c86c1299ea4 928 data.lnStay[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 929 }
lilac0112_1 34:1c86c1299ea4 930 data.lnStay[Y_LINE]=0;
lilac0112_1 34:1c86c1299ea4 931 data.lnStayNow[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 932 }
lilac0112_1 34:1c86c1299ea4 933 else{
lilac0112_1 34:1c86c1299ea4 934 data.lnStay[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 935 }
lilac0112_1 34:1c86c1299ea4 936 if(
lilac0112_1 34:1c86c1299ea4 937 ((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1)))
lilac0112_1 34:1c86c1299ea4 938 ){
lilac0112_1 34:1c86c1299ea4 939 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 34:1c86c1299ea4 940 data.lnStay[Y_LINE]=0;
lilac0112_1 34:1c86c1299ea4 941 }
lilac0112_1 34:1c86c1299ea4 942 else{
lilac0112_1 34:1c86c1299ea4 943 data.lnStay[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 944 }
lilac0112_1 34:1c86c1299ea4 945 data.lnStay[Y_LINE]=0;
lilac0112_1 34:1c86c1299ea4 946 data.lnStayNow[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 947 }
lilac0112_1 34:1c86c1299ea4 948 else{
lilac0112_1 34:1c86c1299ea4 949 data.lnStay[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 950 }
lilac0112_1 27:825c6835e3db 951 //x
lilac0112_1 27:825c6835e3db 952 if(
lilac0112_1 34:1c86c1299ea4 953 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
lilac0112_1 27:825c6835e3db 954 ){
lilac0112_1 34:1c86c1299ea4 955 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 34:1c86c1299ea4 956 data.lnStay[X_LINE]=0;
lilac0112_1 34:1c86c1299ea4 957 }
lilac0112_1 34:1c86c1299ea4 958 else{
lilac0112_1 34:1c86c1299ea4 959 data.lnStay[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 960 }
lilac0112_1 27:825c6835e3db 961 data.lnStay[X_LINE]=0;
lilac0112_1 34:1c86c1299ea4 962 data.lnStayNow[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 963 }
lilac0112_1 34:1c86c1299ea4 964 else{
lilac0112_1 34:1c86c1299ea4 965 data.lnStay[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 966 }
lilac0112_1 34:1c86c1299ea4 967 if(
lilac0112_1 34:1c86c1299ea4 968 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1)))
lilac0112_1 34:1c86c1299ea4 969 ){
lilac0112_1 34:1c86c1299ea4 970 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 34:1c86c1299ea4 971 data.lnStay[X_LINE]=0;
lilac0112_1 34:1c86c1299ea4 972 }
lilac0112_1 34:1c86c1299ea4 973 else{
lilac0112_1 34:1c86c1299ea4 974 data.lnStay[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 975 }
lilac0112_1 34:1c86c1299ea4 976 data.lnStay[X_LINE]=0;
lilac0112_1 34:1c86c1299ea4 977 data.lnStayNow[X_LINE]=1;
lilac0112_1 27:825c6835e3db 978 }
lilac0112_1 27:825c6835e3db 979 else{
lilac0112_1 27:825c6835e3db 980 data.lnStay[X_LINE]=1;
lilac0112_1 27:825c6835e3db 981 }
lilac0112_1 34:1c86c1299ea4 982 if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){
lilac0112_1 34:1c86c1299ea4 983 //y2
lilac0112_1 34:1c86c1299ea4 984 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
lilac0112_1 34:1c86c1299ea4 985 if(
lilac0112_1 34:1c86c1299ea4 986 ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 34:1c86c1299ea4 987 ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 34:1c86c1299ea4 988 ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==18)||(data.irPosition==19)))
lilac0112_1 34:1c86c1299ea4 989 ){
lilac0112_1 34:1c86c1299ea4 990 if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 34:1c86c1299ea4 991 data.lnStay[X_LINE]=0;
lilac0112_1 34:1c86c1299ea4 992 }
lilac0112_1 34:1c86c1299ea4 993 else{
lilac0112_1 34:1c86c1299ea4 994 data.lnStay[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 995 }
lilac0112_1 34:1c86c1299ea4 996 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 34:1c86c1299ea4 997 data.lnStay[X_LINE]=0;
lilac0112_1 34:1c86c1299ea4 998 }
lilac0112_1 34:1c86c1299ea4 999 data.lnStayNow[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1000 }
lilac0112_1 34:1c86c1299ea4 1001 else{
lilac0112_1 34:1c86c1299ea4 1002 data.lnStay[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1003 }
lilac0112_1 34:1c86c1299ea4 1004 }
lilac0112_1 34:1c86c1299ea4 1005 if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){
lilac0112_1 34:1c86c1299ea4 1006 if(
lilac0112_1 34:1c86c1299ea4 1007 ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
lilac0112_1 34:1c86c1299ea4 1008 ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))||
lilac0112_1 34:1c86c1299ea4 1009 ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==15)||(data.irPosition==16)))
lilac0112_1 34:1c86c1299ea4 1010 ){
lilac0112_1 34:1c86c1299ea4 1011 if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){
lilac0112_1 34:1c86c1299ea4 1012 data.lnStay[X_LINE]=0;
lilac0112_1 34:1c86c1299ea4 1013 }
lilac0112_1 34:1c86c1299ea4 1014 else{
lilac0112_1 34:1c86c1299ea4 1015 data.lnStay[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1016 }
lilac0112_1 34:1c86c1299ea4 1017 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 34:1c86c1299ea4 1018 data.lnStay[X_LINE]=0;
lilac0112_1 34:1c86c1299ea4 1019 }
lilac0112_1 34:1c86c1299ea4 1020 data.lnStayNow[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1021 }
lilac0112_1 34:1c86c1299ea4 1022 else{
lilac0112_1 34:1c86c1299ea4 1023 data.lnStay[X_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1024 }
lilac0112_1 34:1c86c1299ea4 1025 }
lilac0112_1 34:1c86c1299ea4 1026 //x2
lilac0112_1 34:1c86c1299ea4 1027 if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){
lilac0112_1 34:1c86c1299ea4 1028 if(
lilac0112_1 34:1c86c1299ea4 1029 ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))||
lilac0112_1 34:1c86c1299ea4 1030 ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))
lilac0112_1 34:1c86c1299ea4 1031 ){
lilac0112_1 34:1c86c1299ea4 1032 if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 34:1c86c1299ea4 1033 data.lnStay[Y_LINE]=0;
lilac0112_1 34:1c86c1299ea4 1034 }
lilac0112_1 34:1c86c1299ea4 1035 else{
lilac0112_1 34:1c86c1299ea4 1036 data.lnStay[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1037 }
lilac0112_1 34:1c86c1299ea4 1038 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 34:1c86c1299ea4 1039 data.lnStay[Y_LINE]=0;
lilac0112_1 34:1c86c1299ea4 1040 }
lilac0112_1 34:1c86c1299ea4 1041 data.lnStayNow[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1042 }
lilac0112_1 34:1c86c1299ea4 1043 else{
lilac0112_1 34:1c86c1299ea4 1044 data.lnStay[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1045 }
lilac0112_1 34:1c86c1299ea4 1046
lilac0112_1 34:1c86c1299ea4 1047 if(
lilac0112_1 34:1c86c1299ea4 1048 ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))||
lilac0112_1 34:1c86c1299ea4 1049 ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16)))
lilac0112_1 34:1c86c1299ea4 1050 ){
lilac0112_1 34:1c86c1299ea4 1051 if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){
lilac0112_1 34:1c86c1299ea4 1052 data.lnStay[Y_LINE]=0;
lilac0112_1 34:1c86c1299ea4 1053 }
lilac0112_1 34:1c86c1299ea4 1054 else{
lilac0112_1 34:1c86c1299ea4 1055 data.lnStay[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1056 }
lilac0112_1 34:1c86c1299ea4 1057 if(data.lnRepeat>=LINE_REPEAT){
lilac0112_1 34:1c86c1299ea4 1058 data.lnStay[Y_LINE]=0;
lilac0112_1 34:1c86c1299ea4 1059 }
lilac0112_1 34:1c86c1299ea4 1060 data.lnStayNow[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1061 }
lilac0112_1 34:1c86c1299ea4 1062 else{
lilac0112_1 34:1c86c1299ea4 1063 data.lnStay[Y_LINE]=1;
lilac0112_1 34:1c86c1299ea4 1064 }
lilac0112_1 34:1c86c1299ea4 1065 }
lilac0112_1 34:1c86c1299ea4 1066 if(
lilac0112_1 34:1c86c1299ea4 1067 (data.lnRepeat>=2)
lilac0112_1 34:1c86c1299ea4 1068 ){
lilac0112_1 34:1c86c1299ea4 1069 if(
lilac0112_1 34:1c86c1299ea4 1070 ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))||
lilac0112_1 34:1c86c1299ea4 1071 ((pow_y<0)&&((data.ping[B_PING]<40)&&(1)))
lilac0112_1 34:1c86c1299ea4 1072 ){
lilac0112_1 34:1c86c1299ea4 1073 data.lnStay[Y_LINE]=0;
lilac0112_1 34:1c86c1299ea4 1074 }
lilac0112_1 34:1c86c1299ea4 1075 if(
lilac0112_1 34:1c86c1299ea4 1076 ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))||
lilac0112_1 34:1c86c1299ea4 1077 ((pow_x<0)&&((data.ping[L_PING]<40)&&(1)))
lilac0112_1 34:1c86c1299ea4 1078 ){
lilac0112_1 34:1c86c1299ea4 1079 data.lnStay[X_LINE]=0;
lilac0112_1 34:1c86c1299ea4 1080 }
lilac0112_1 34:1c86c1299ea4 1081 }
lilac0112_1 27:825c6835e3db 1082 }
lilac0112_1 34:1c86c1299ea4 1083
lilac0112_1 34:1c86c1299ea4 1084
lilac0112_1 27:825c6835e3db 1085 }
lilac0112_1 27:825c6835e3db 1086 else{
lilac0112_1 27:825c6835e3db 1087 data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1;
lilac0112_1 27:825c6835e3db 1088 }
lilac0112_1 27:825c6835e3db 1089
lilac0112_1 27:825c6835e3db 1090 }
lilac0112_1 27:825c6835e3db 1091 void JudgeInSide(void){
lilac0112_1 27:825c6835e3db 1092 data.FieldSpot = LINE_INSIDE;
lilac0112_1 27:825c6835e3db 1093 data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0;
lilac0112_1 27:825c6835e3db 1094 LineLiberate();
lilac0112_1 27:825c6835e3db 1095 LineRankClear();
lilac0112_1 27:825c6835e3db 1096 //Line_home.detach();
lilac0112_1 27:825c6835e3db 1097 }