ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
main_processing/strategy/LineProcess.cpp@38:67bc78f3c0ab, 2016-03-27 (annotated)
- Committer:
- lilac0112_1
- Date:
- Sun Mar 27 13:04:39 2016 +0000
- Revision:
- 38:67bc78f3c0ab
- Parent:
- 34:1c86c1299ea4
JapanSoccerOpen2016 CatPot Program(main)
Who changed what in which revision?
User | Revision | Line number | New 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 | 38:67bc78f3c0ab | 349 | //JSO2 |
lilac0112_1 | 38:67bc78f3c0ab | 350 | static uint8_t lnRawReturn1 = 0; |
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 | //※場外判定を行うには再び場内に戻る必要がある. |
lilac0112_1 | 27:825c6835e3db | 356 | |
lilac0112_1 | 27:825c6835e3db | 357 | data.FieldSpot=LINE_INSIDE; |
lilac0112_1 | 27:825c6835e3db | 358 | |
lilac0112_1 | 27:825c6835e3db | 359 | if(data.FieldSpot==LINE_OUTSIDE){ |
lilac0112_1 | 27:825c6835e3db | 360 | } |
lilac0112_1 | 27:825c6835e3db | 361 | else{//data.FieldSpot==LINE_INSIDE |
lilac0112_1 | 27:825c6835e3db | 362 | |
lilac0112_1 | 27:825c6835e3db | 363 | data.ReturnDir[X_LINE]=LINE_EMPTY; |
lilac0112_1 | 27:825c6835e3db | 364 | data.ReturnDir[Y_LINE]=LINE_EMPTY; |
lilac0112_1 | 27:825c6835e3db | 365 | |
lilac0112_1 | 27:825c6835e3db | 366 | *x = 0; |
lilac0112_1 | 27:825c6835e3db | 367 | *y = 0; |
lilac0112_1 | 27:825c6835e3db | 368 | |
lilac0112_1 | 27:825c6835e3db | 369 | //line間際の復帰力以外の力を作用させるか否か |
lilac0112_1 | 27:825c6835e3db | 370 | data.lnStop[X_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 371 | data.lnStop[Y_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 372 | |
lilac0112_1 | 27:825c6835e3db | 373 | if(//y |
lilac0112_1 | 27:825c6835e3db | 374 | ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==B_SPOT)&&(1))|| |
lilac0112_1 | 27:825c6835e3db | 375 | ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==A_SPOT)&&(1)) |
lilac0112_1 | 27:825c6835e3db | 376 | ){ |
lilac0112_1 | 27:825c6835e3db | 377 | data.lnStop[Y_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 378 | *y = -LineReturn[4]; |
lilac0112_1 | 38:67bc78f3c0ab | 379 | //JSO1 |
lilac0112_1 | 38:67bc78f3c0ab | 380 | if( |
lilac0112_1 | 38:67bc78f3c0ab | 381 | (data.ping[F_PING]<45)&& |
lilac0112_1 | 38:67bc78f3c0ab | 382 | (data.lnRawReturn==1)//JSO2 |
lilac0112_1 | 38:67bc78f3c0ab | 383 | ){ |
lilac0112_1 | 38:67bc78f3c0ab | 384 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 385 | } |
lilac0112_1 | 38:67bc78f3c0ab | 386 | //JSO2 |
lilac0112_1 | 38:67bc78f3c0ab | 387 | if( |
lilac0112_1 | 38:67bc78f3c0ab | 388 | ( |
lilac0112_1 | 38:67bc78f3c0ab | 389 | ((Line[A_SPOT].read()==1)&&(Line[B_SPOT].read()==0))|| |
lilac0112_1 | 38:67bc78f3c0ab | 390 | ((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0)) |
lilac0112_1 | 38:67bc78f3c0ab | 391 | )&& |
lilac0112_1 | 38:67bc78f3c0ab | 392 | (data.ping[F_PING]>45)&& |
lilac0112_1 | 38:67bc78f3c0ab | 393 | (data.ping[F_PING]<200)&& |
lilac0112_1 | 38:67bc78f3c0ab | 394 | (data.ping[R_PING]<25)&& |
lilac0112_1 | 38:67bc78f3c0ab | 395 | (data.ping[L_PING]>40) |
lilac0112_1 | 38:67bc78f3c0ab | 396 | ){ |
lilac0112_1 | 38:67bc78f3c0ab | 397 | *x = -LineReturn[4]; |
lilac0112_1 | 38:67bc78f3c0ab | 398 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 399 | } |
lilac0112_1 | 38:67bc78f3c0ab | 400 | if( |
lilac0112_1 | 38:67bc78f3c0ab | 401 | ( |
lilac0112_1 | 38:67bc78f3c0ab | 402 | ((Line[A_SPOT].read()==0)&&(Line[B_SPOT].read()==1))|| |
lilac0112_1 | 38:67bc78f3c0ab | 403 | ((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1)) |
lilac0112_1 | 38:67bc78f3c0ab | 404 | )&& |
lilac0112_1 | 38:67bc78f3c0ab | 405 | (data.ping[F_PING]>45)&& |
lilac0112_1 | 38:67bc78f3c0ab | 406 | (data.ping[F_PING]<200)&& |
lilac0112_1 | 38:67bc78f3c0ab | 407 | (data.ping[L_PING]<25)&& |
lilac0112_1 | 38:67bc78f3c0ab | 408 | (data.ping[R_PING]>40) |
lilac0112_1 | 38:67bc78f3c0ab | 409 | ){ |
lilac0112_1 | 38:67bc78f3c0ab | 410 | *x = LineReturn[4]; |
lilac0112_1 | 38:67bc78f3c0ab | 411 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 412 | } |
lilac0112_1 | 27:825c6835e3db | 413 | } |
lilac0112_1 | 27:825c6835e3db | 414 | else if( |
lilac0112_1 | 27:825c6835e3db | 415 | ((data.lnRawOrder[0]==C_SPOT)&&(1)&&(1)) |
lilac0112_1 | 27:825c6835e3db | 416 | ){ |
lilac0112_1 | 27:825c6835e3db | 417 | data.lnStop[Y_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 418 | *y = LineReturn[4]; |
lilac0112_1 | 38:67bc78f3c0ab | 419 | |
lilac0112_1 | 38:67bc78f3c0ab | 420 | //JSO1 |
lilac0112_1 | 38:67bc78f3c0ab | 421 | if( |
lilac0112_1 | 38:67bc78f3c0ab | 422 | (data.ping[B_PING]<45)&& |
lilac0112_1 | 38:67bc78f3c0ab | 423 | (data.lnRawReturn==1) |
lilac0112_1 | 38:67bc78f3c0ab | 424 | ){ |
lilac0112_1 | 38:67bc78f3c0ab | 425 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 426 | } |
lilac0112_1 | 27:825c6835e3db | 427 | } |
lilac0112_1 | 27:825c6835e3db | 428 | else if(//x |
lilac0112_1 | 27:825c6835e3db | 429 | ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1)) |
lilac0112_1 | 27:825c6835e3db | 430 | ){ |
lilac0112_1 | 27:825c6835e3db | 431 | *x = -LineReturn[4]; |
lilac0112_1 | 27:825c6835e3db | 432 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 433 | } |
lilac0112_1 | 27:825c6835e3db | 434 | else if( |
lilac0112_1 | 27:825c6835e3db | 435 | ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1)) |
lilac0112_1 | 27:825c6835e3db | 436 | ){ |
lilac0112_1 | 27:825c6835e3db | 437 | *x = LineReturn[4]; |
lilac0112_1 | 27:825c6835e3db | 438 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 439 | } |
lilac0112_1 | 27:825c6835e3db | 440 | |
lilac0112_1 | 27:825c6835e3db | 441 | if( |
lilac0112_1 | 27:825c6835e3db | 442 | ((*x)!=0)|| |
lilac0112_1 | 27:825c6835e3db | 443 | ((*y)!=0) |
lilac0112_1 | 27:825c6835e3db | 444 | ){ |
lilac0112_1 | 27:825c6835e3db | 445 | |
lilac0112_1 | 27:825c6835e3db | 446 | data.lnRawReturn=1; |
lilac0112_1 | 27:825c6835e3db | 447 | |
lilac0112_1 | 27:825c6835e3db | 448 | LinePingState[L_PING]=(data.ping[L_PING]<WhiteToWallPlus[X_PING]); |
lilac0112_1 | 27:825c6835e3db | 449 | LinePingState[R_PING]=(data.ping[R_PING]<WhiteToWallPlus[X_PING]); |
lilac0112_1 | 27:825c6835e3db | 450 | LinePingState[F_PING]=(data.ping[F_PING]<WhiteToWallPlus[Y_PING]); |
lilac0112_1 | 27:825c6835e3db | 451 | LinePingState[B_PING]=(data.ping[B_PING]<WhiteToWallPlus[Y_PING]); |
lilac0112_1 | 27:825c6835e3db | 452 | //x |
lilac0112_1 | 27:825c6835e3db | 453 | if(data.lnStop[X_LINE]==1){ |
lilac0112_1 | 27:825c6835e3db | 454 | if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){ |
lilac0112_1 | 27:825c6835e3db | 455 | *x = -LineReturn[4]; |
lilac0112_1 | 27:825c6835e3db | 456 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 457 | } |
lilac0112_1 | 27:825c6835e3db | 458 | if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){ |
lilac0112_1 | 27:825c6835e3db | 459 | *x = LineReturn[4]; |
lilac0112_1 | 27:825c6835e3db | 460 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 461 | } |
lilac0112_1 | 27:825c6835e3db | 462 | if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==1)){ |
lilac0112_1 | 27:825c6835e3db | 463 | *x = 0; |
lilac0112_1 | 27:825c6835e3db | 464 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 465 | } |
lilac0112_1 | 27:825c6835e3db | 466 | } |
lilac0112_1 | 27:825c6835e3db | 467 | //y |
lilac0112_1 | 27:825c6835e3db | 468 | if(data.lnStop[Y_LINE]==1){ |
lilac0112_1 | 27:825c6835e3db | 469 | if((LinePingState[B_PING]==0)&&(LinePingState[F_PING]==1)){ |
lilac0112_1 | 27:825c6835e3db | 470 | if(pow_y<0){ |
lilac0112_1 | 27:825c6835e3db | 471 | *y = 0; |
lilac0112_1 | 27:825c6835e3db | 472 | data.lnStop[Y_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 473 | } |
lilac0112_1 | 27:825c6835e3db | 474 | else{ |
lilac0112_1 | 27:825c6835e3db | 475 | *y = -LineReturn[4]; |
lilac0112_1 | 27:825c6835e3db | 476 | data.lnStop[Y_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 477 | } |
lilac0112_1 | 27:825c6835e3db | 478 | } |
lilac0112_1 | 38:67bc78f3c0ab | 479 | //JSO2 |
lilac0112_1 | 38:67bc78f3c0ab | 480 | if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)&&(data.lnRawReturn==1)){ |
lilac0112_1 | 38:67bc78f3c0ab | 481 | *y = -LineReturn[4]; |
lilac0112_1 | 38:67bc78f3c0ab | 482 | data.lnStop[Y_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 483 | } |
lilac0112_1 | 38:67bc78f3c0ab | 484 | |
lilac0112_1 | 27:825c6835e3db | 485 | if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==0)){ |
lilac0112_1 | 27:825c6835e3db | 486 | if(pow_y>0){ |
lilac0112_1 | 27:825c6835e3db | 487 | *y = 0; |
lilac0112_1 | 27:825c6835e3db | 488 | data.lnStop[Y_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 489 | } |
lilac0112_1 | 27:825c6835e3db | 490 | else{ |
lilac0112_1 | 27:825c6835e3db | 491 | *y = LineReturn[4]; |
lilac0112_1 | 27:825c6835e3db | 492 | data.lnStop[Y_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 493 | } |
lilac0112_1 | 27:825c6835e3db | 494 | } |
lilac0112_1 | 27:825c6835e3db | 495 | if((LinePingState[B_PING]==1)&&(LinePingState[F_PING]==1)){ |
lilac0112_1 | 27:825c6835e3db | 496 | *y = 0; |
lilac0112_1 | 27:825c6835e3db | 497 | data.lnStop[Y_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 498 | } |
lilac0112_1 | 27:825c6835e3db | 499 | } |
lilac0112_1 | 27:825c6835e3db | 500 | } |
lilac0112_1 | 27:825c6835e3db | 501 | else{ |
lilac0112_1 | 27:825c6835e3db | 502 | data.lnRawReturn=0; |
lilac0112_1 | 38:67bc78f3c0ab | 503 | |
lilac0112_1 | 38:67bc78f3c0ab | 504 | //JSO3 |
lilac0112_1 | 38:67bc78f3c0ab | 505 | if((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==LINE_EMPTY)&&(pow_x>0)){ |
lilac0112_1 | 38:67bc78f3c0ab | 506 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 507 | } |
lilac0112_1 | 38:67bc78f3c0ab | 508 | if((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==LINE_EMPTY)&&(pow_x<0)){ |
lilac0112_1 | 38:67bc78f3c0ab | 509 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 510 | } |
lilac0112_1 | 38:67bc78f3c0ab | 511 | //JSO3 |
lilac0112_1 | 38:67bc78f3c0ab | 512 | if((Line[C_SPOT].read()==1)&&(1)&&(pow_y>0)){ |
lilac0112_1 | 38:67bc78f3c0ab | 513 | data.lnStop[Y_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 514 | } |
lilac0112_1 | 38:67bc78f3c0ab | 515 | } |
lilac0112_1 | 38:67bc78f3c0ab | 516 | //JSO3 |
lilac0112_1 | 38:67bc78f3c0ab | 517 | if((Line[C_SPOT].read()==1)&&(1)&&(pow_y>0)){ |
lilac0112_1 | 38:67bc78f3c0ab | 518 | data.lnStop[Y_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 519 | } |
lilac0112_1 | 38:67bc78f3c0ab | 520 | //JSO1 |
lilac0112_1 | 38:67bc78f3c0ab | 521 | if(data.lnRawReturn==1){ |
lilac0112_1 | 38:67bc78f3c0ab | 522 | //JSO2 |
lilac0112_1 | 38:67bc78f3c0ab | 523 | if((lnRawReturn1==0)){ |
lilac0112_1 | 38:67bc78f3c0ab | 524 | LineLiberate(); |
lilac0112_1 | 38:67bc78f3c0ab | 525 | lnRawReturn1=1; |
lilac0112_1 | 38:67bc78f3c0ab | 526 | } |
lilac0112_1 | 38:67bc78f3c0ab | 527 | |
lilac0112_1 | 38:67bc78f3c0ab | 528 | if( |
lilac0112_1 | 38:67bc78f3c0ab | 529 | ( |
lilac0112_1 | 38:67bc78f3c0ab | 530 | ((data.lnRawOrder[0]==A_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1))|| |
lilac0112_1 | 38:67bc78f3c0ab | 531 | ((data.lnRawOrder[0]==B_SPOT)&&(data.lnRawOrder[1]==C_SPOT)&&(1)) |
lilac0112_1 | 38:67bc78f3c0ab | 532 | )&& |
lilac0112_1 | 38:67bc78f3c0ab | 533 | //((Line[A_SPOT].read()==1)&&(Line[B_SPOT].read()==1))&&//JSO1 |
lilac0112_1 | 38:67bc78f3c0ab | 534 | ((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1))&&//JSO2 |
lilac0112_1 | 38:67bc78f3c0ab | 535 | ((data.ping[F_PING]<45)&&(1))&&//JSO2 |
lilac0112_1 | 38:67bc78f3c0ab | 536 | //(!((data.ping[B_PING]<40)&&(data.ping[B_PING]>25)))&&//JSO2 |
lilac0112_1 | 38:67bc78f3c0ab | 537 | ((data.lnRawOrder[0]!=C_SPOT)&&(data.lnRawOrderLog1[0]!=C_SPOT)&&(data.lnRawOrderLog2[0]!=C_SPOT)) |
lilac0112_1 | 38:67bc78f3c0ab | 538 | |
lilac0112_1 | 38:67bc78f3c0ab | 539 | ){ |
lilac0112_1 | 38:67bc78f3c0ab | 540 | *y = -LineReturn[4]; |
lilac0112_1 | 38:67bc78f3c0ab | 541 | data.lnStop[Y_LINE]=0;//JSO2 |
lilac0112_1 | 38:67bc78f3c0ab | 542 | } |
lilac0112_1 | 38:67bc78f3c0ab | 543 | } |
lilac0112_1 | 38:67bc78f3c0ab | 544 | else{ |
lilac0112_1 | 38:67bc78f3c0ab | 545 | lnRawReturn1=0; |
lilac0112_1 | 38:67bc78f3c0ab | 546 | |
lilac0112_1 | 38:67bc78f3c0ab | 547 | |
lilac0112_1 | 38:67bc78f3c0ab | 548 | if((LinePingState[L_PING]==0)&&(LinePingState[R_PING]==1)){ |
lilac0112_1 | 38:67bc78f3c0ab | 549 | *x = -LineReturn[4]; |
lilac0112_1 | 38:67bc78f3c0ab | 550 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 551 | } |
lilac0112_1 | 38:67bc78f3c0ab | 552 | if((LinePingState[L_PING]==1)&&(LinePingState[R_PING]==0)){ |
lilac0112_1 | 38:67bc78f3c0ab | 553 | *x = LineReturn[4]; |
lilac0112_1 | 38:67bc78f3c0ab | 554 | data.lnStop[X_LINE]=0; |
lilac0112_1 | 38:67bc78f3c0ab | 555 | } |
lilac0112_1 | 38:67bc78f3c0ab | 556 | |
lilac0112_1 | 27:825c6835e3db | 557 | } |
lilac0112_1 | 27:825c6835e3db | 558 | |
lilac0112_1 | 25:a7460e23e02e | 559 | } |
lilac0112_1 | 25:a7460e23e02e | 560 | } |
lilac0112_1 | 25:a7460e23e02e | 561 | void LineJudgeReset(double pow_x, double pow_y, double *x, double *y){ |
lilac0112_1 | 25:a7460e23e02e | 562 | //static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY}; |
lilac0112_1 | 25:a7460e23e02e | 563 | //static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY}; |
lilac0112_1 | 25:a7460e23e02e | 564 | static uint8_t NewLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY}; |
lilac0112_1 | 25:a7460e23e02e | 565 | static uint8_t LastLineCorner[4]={LINE_EMPTY, LINE_EMPTY, LINE_EMPTY, LINE_EMPTY}; |
lilac0112_1 | 25:a7460e23e02e | 566 | |
lilac0112_1 | 25:a7460e23e02e | 567 | if((/*data.lnRaw==0*/1)&&(data.lnHold==7)){ |
lilac0112_1 | 25:a7460e23e02e | 568 | if(data.FieldSpot==LINE_INSIDE){ |
lilac0112_1 | 25:a7460e23e02e | 569 | |
lilac0112_1 | 25:a7460e23e02e | 570 | data.lnCorner[L_LINE] = (data.ping[L_PING]<OutToWall[X_PING]); |
lilac0112_1 | 25:a7460e23e02e | 571 | data.lnCorner[R_LINE] = (data.ping[R_PING]<OutToWall[X_PING]); |
lilac0112_1 | 25:a7460e23e02e | 572 | data.lnCorner[F_LINE] = (data.ping[F_PING]<OutToWall[Y_PING]); |
lilac0112_1 | 25:a7460e23e02e | 573 | data.lnCorner[B_LINE] = (data.ping[B_PING]<OutToWall[Y_PING]); |
lilac0112_1 | 25:a7460e23e02e | 574 | if( |
lilac0112_1 | 25:a7460e23e02e | 575 | (data.lnCorner[L_LINE])|| |
lilac0112_1 | 25:a7460e23e02e | 576 | (data.lnCorner[R_LINE])|| |
lilac0112_1 | 25:a7460e23e02e | 577 | (data.lnCorner[F_LINE])|| |
lilac0112_1 | 25:a7460e23e02e | 578 | (data.lnCorner[B_LINE]) |
lilac0112_1 | 25:a7460e23e02e | 579 | ){ |
lilac0112_1 | 25:a7460e23e02e | 580 | |
lilac0112_1 | 25:a7460e23e02e | 581 | data.NonWall[L_LINE] = (data.ping[L_PING]>WhiteToWall[X_PING]); |
lilac0112_1 | 25:a7460e23e02e | 582 | data.NonWall[R_LINE] = (data.ping[R_PING]>WhiteToWall[X_PING]); |
lilac0112_1 | 25:a7460e23e02e | 583 | data.NonWall[F_LINE] = (data.ping[F_PING]>WhiteToWall[Y_PING]); |
lilac0112_1 | 25:a7460e23e02e | 584 | data.NonWall[B_LINE] = (data.ping[B_PING]>WhiteToWall[Y_PING]); |
lilac0112_1 | 25:a7460e23e02e | 585 | |
lilac0112_1 | 25:a7460e23e02e | 586 | |
lilac0112_1 | 25:a7460e23e02e | 587 | |
lilac0112_1 | 25:a7460e23e02e | 588 | ///* |
lilac0112_1 | 25:a7460e23e02e | 589 | LastLineCorner[L_LINE]=NewLineCorner[L_LINE]; |
lilac0112_1 | 25:a7460e23e02e | 590 | LastLineCorner[R_LINE]=NewLineCorner[R_LINE]; |
lilac0112_1 | 25:a7460e23e02e | 591 | LastLineCorner[F_LINE]=NewLineCorner[F_LINE]; |
lilac0112_1 | 25:a7460e23e02e | 592 | LastLineCorner[B_LINE]=NewLineCorner[B_LINE]; |
lilac0112_1 | 25:a7460e23e02e | 593 | |
lilac0112_1 | 25:a7460e23e02e | 594 | NewLineCorner[L_LINE]=data.lnCorner[L_LINE]; |
lilac0112_1 | 25:a7460e23e02e | 595 | NewLineCorner[R_LINE]=data.lnCorner[R_LINE]; |
lilac0112_1 | 25:a7460e23e02e | 596 | NewLineCorner[F_LINE]=data.lnCorner[F_LINE]; |
lilac0112_1 | 25:a7460e23e02e | 597 | NewLineCorner[B_LINE]=data.lnCorner[B_LINE]; |
lilac0112_1 | 25:a7460e23e02e | 598 | |
lilac0112_1 | 25:a7460e23e02e | 599 | if( |
lilac0112_1 | 25:a7460e23e02e | 600 | (LastLineCorner[L_LINE]==NewLineCorner[L_LINE])&& |
lilac0112_1 | 25:a7460e23e02e | 601 | (LastLineCorner[R_LINE]==NewLineCorner[R_LINE])&& |
lilac0112_1 | 25:a7460e23e02e | 602 | (LastLineCorner[F_LINE]==NewLineCorner[F_LINE])&& |
lilac0112_1 | 25:a7460e23e02e | 603 | (LastLineCorner[B_LINE]==NewLineCorner[B_LINE]) |
lilac0112_1 | 25:a7460e23e02e | 604 | ){ |
lilac0112_1 | 25:a7460e23e02e | 605 | data.lnRepeat++; |
lilac0112_1 | 25:a7460e23e02e | 606 | } |
lilac0112_1 | 25:a7460e23e02e | 607 | else{ |
lilac0112_1 | 25:a7460e23e02e | 608 | data.lnRepeat=0; |
lilac0112_1 | 25:a7460e23e02e | 609 | } |
lilac0112_1 | 25:a7460e23e02e | 610 | //*/ |
lilac0112_1 | 25:a7460e23e02e | 611 | |
lilac0112_1 | 25:a7460e23e02e | 612 | data.FieldSpot = LINE_OUTSIDE; |
lilac0112_1 | 25:a7460e23e02e | 613 | LineLiberate(); |
lilac0112_1 | 25:a7460e23e02e | 614 | } |
lilac0112_1 | 25:a7460e23e02e | 615 | } |
lilac0112_1 | 25:a7460e23e02e | 616 | else if(data.FieldSpot==LINE_OUTSIDE){ |
lilac0112_1 | 25:a7460e23e02e | 617 | if(data.lnRaw==0){ |
lilac0112_1 | 25:a7460e23e02e | 618 | data.FieldSpot = LINE_INSIDE; |
lilac0112_1 | 25:a7460e23e02e | 619 | data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; |
lilac0112_1 | 25:a7460e23e02e | 620 | LineLiberate(); |
lilac0112_1 | 25:a7460e23e02e | 621 | LineRankClear(); |
lilac0112_1 | 25:a7460e23e02e | 622 | } |
lilac0112_1 | 25:a7460e23e02e | 623 | } |
lilac0112_1 | 25:a7460e23e02e | 624 | } |
lilac0112_1 | 25:a7460e23e02e | 625 | if(data.FieldSpot == LINE_OUTSIDE){ |
lilac0112_1 | 25:a7460e23e02e | 626 | if( |
lilac0112_1 | 25:a7460e23e02e | 627 | ( |
lilac0112_1 | 25:a7460e23e02e | 628 | (data.ping[L_PING]>=WhiteToWall[X_PING])|| |
lilac0112_1 | 25:a7460e23e02e | 629 | (data.NonWall[L_LINE]==1)|| |
lilac0112_1 | 25:a7460e23e02e | 630 | ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)) |
lilac0112_1 | 25:a7460e23e02e | 631 | //(data.lnOrder[0]==A_SPOT) |
lilac0112_1 | 25:a7460e23e02e | 632 | )&& |
lilac0112_1 | 25:a7460e23e02e | 633 | ( |
lilac0112_1 | 25:a7460e23e02e | 634 | (data.ping[R_PING]>=WhiteToWall[X_PING])|| |
lilac0112_1 | 25:a7460e23e02e | 635 | (data.NonWall[R_LINE]==1)|| |
lilac0112_1 | 25:a7460e23e02e | 636 | ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)) |
lilac0112_1 | 25:a7460e23e02e | 637 | //(data.lnOrder[0]==B_SPOT) |
lilac0112_1 | 25:a7460e23e02e | 638 | )&& |
lilac0112_1 | 25:a7460e23e02e | 639 | ( |
lilac0112_1 | 25:a7460e23e02e | 640 | (data.ping[F_PING]>=WhiteToWall[Y_PING])|| |
lilac0112_1 | 25:a7460e23e02e | 641 | (data.NonWall[F_LINE]==1)|| |
lilac0112_1 | 25:a7460e23e02e | 642 | ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))|| |
lilac0112_1 | 25:a7460e23e02e | 643 | ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)) |
lilac0112_1 | 25:a7460e23e02e | 644 | )&& |
lilac0112_1 | 25:a7460e23e02e | 645 | ((data.ping[B_PING]>=WhiteToWall[Y_PING])|| |
lilac0112_1 | 25:a7460e23e02e | 646 | (data.NonWall[B_LINE]==1)|| |
lilac0112_1 | 25:a7460e23e02e | 647 | ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))|| |
lilac0112_1 | 25:a7460e23e02e | 648 | ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)) |
lilac0112_1 | 25:a7460e23e02e | 649 | ) |
lilac0112_1 | 25:a7460e23e02e | 650 | ){ |
lilac0112_1 | 25:a7460e23e02e | 651 | data.FieldSpot = LINE_INSIDE; |
lilac0112_1 | 25:a7460e23e02e | 652 | data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; |
lilac0112_1 | 25:a7460e23e02e | 653 | LineLiberate(); |
lilac0112_1 | 25:a7460e23e02e | 654 | LineRankClear(); |
lilac0112_1 | 25:a7460e23e02e | 655 | } |
lilac0112_1 | 25:a7460e23e02e | 656 | } |
lilac0112_1 | 25:a7460e23e02e | 657 | if((data.FieldSpot == LINE_INSIDE)&&(0<data.lnHold)&&(data.lnHold<7)&&(data.lnRaw==0)){ |
lilac0112_1 | 25:a7460e23e02e | 658 | if( |
lilac0112_1 | 25:a7460e23e02e | 659 | ( |
lilac0112_1 | 25:a7460e23e02e | 660 | ( |
lilac0112_1 | 25:a7460e23e02e | 661 | (data.ping[L_PING]>=WhiteToWall[X_PING])|| |
lilac0112_1 | 25:a7460e23e02e | 662 | ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==C_SPOT)) |
lilac0112_1 | 25:a7460e23e02e | 663 | //(data.lnOrder[0]==A_SPOT) |
lilac0112_1 | 25:a7460e23e02e | 664 | )&& |
lilac0112_1 | 25:a7460e23e02e | 665 | ( |
lilac0112_1 | 25:a7460e23e02e | 666 | (data.ping[R_PING]>=WhiteToWall[X_PING])|| |
lilac0112_1 | 25:a7460e23e02e | 667 | ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==C_SPOT)) |
lilac0112_1 | 25:a7460e23e02e | 668 | //(data.lnOrder[0]==B_SPOT) |
lilac0112_1 | 25:a7460e23e02e | 669 | )&& |
lilac0112_1 | 25:a7460e23e02e | 670 | ( |
lilac0112_1 | 25:a7460e23e02e | 671 | (data.ping[F_PING]>=WhiteToWall[Y_PING])|| |
lilac0112_1 | 25:a7460e23e02e | 672 | ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==A_SPOT))|| |
lilac0112_1 | 25:a7460e23e02e | 673 | ((data.lnOrder[0]==C_SPOT)&&(data.lnOrder[1]==B_SPOT)) |
lilac0112_1 | 25:a7460e23e02e | 674 | )&& |
lilac0112_1 | 25:a7460e23e02e | 675 | ( |
lilac0112_1 | 25:a7460e23e02e | 676 | (data.ping[B_PING]>=WhiteToWall[Y_PING])|| |
lilac0112_1 | 25:a7460e23e02e | 677 | ((data.lnOrder[0]==A_SPOT)&&(data.lnOrder[1]==B_SPOT))|| |
lilac0112_1 | 25:a7460e23e02e | 678 | ((data.lnOrder[0]==B_SPOT)&&(data.lnOrder[1]==A_SPOT)) |
lilac0112_1 | 25:a7460e23e02e | 679 | ) |
lilac0112_1 | 25:a7460e23e02e | 680 | ) |
lilac0112_1 | 25:a7460e23e02e | 681 | || |
lilac0112_1 | 25:a7460e23e02e | 682 | ( |
lilac0112_1 | 25:a7460e23e02e | 683 | (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&& |
lilac0112_1 | 25:a7460e23e02e | 684 | (data.ping[R_PING]>=GoalEdgeToWall[X_PING]) |
lilac0112_1 | 25:a7460e23e02e | 685 | ) |
lilac0112_1 | 25:a7460e23e02e | 686 | ){ |
lilac0112_1 | 25:a7460e23e02e | 687 | data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; |
lilac0112_1 | 25:a7460e23e02e | 688 | LineLiberate(); |
lilac0112_1 | 25:a7460e23e02e | 689 | } |
lilac0112_1 | 25:a7460e23e02e | 690 | } |
lilac0112_1 | 25:a7460e23e02e | 691 | ///* |
lilac0112_1 | 25:a7460e23e02e | 692 | if( |
lilac0112_1 | 25:a7460e23e02e | 693 | (data.irNotice==IR_NONE)|| |
lilac0112_1 | 25:a7460e23e02e | 694 | (data.irNotice==IR_FAR)|| |
lilac0112_1 | 25:a7460e23e02e | 695 | ( |
lilac0112_1 | 25:a7460e23e02e | 696 | (data.ping[L_PING]>=GoalEdgeToWall[X_PING])&& |
lilac0112_1 | 25:a7460e23e02e | 697 | (data.ping[R_PING]>=GoalEdgeToWall[X_PING]) |
lilac0112_1 | 25:a7460e23e02e | 698 | ) |
lilac0112_1 | 25:a7460e23e02e | 699 | ){ |
lilac0112_1 | 25:a7460e23e02e | 700 | data.lnRepeat = 0; |
lilac0112_1 | 25:a7460e23e02e | 701 | |
lilac0112_1 | 25:a7460e23e02e | 702 | NewLineCorner[L_LINE]=LINE_EMPTY; |
lilac0112_1 | 25:a7460e23e02e | 703 | NewLineCorner[R_LINE]=LINE_EMPTY; |
lilac0112_1 | 25:a7460e23e02e | 704 | NewLineCorner[F_LINE]=LINE_EMPTY; |
lilac0112_1 | 25:a7460e23e02e | 705 | NewLineCorner[B_LINE]=LINE_EMPTY; |
lilac0112_1 | 25:a7460e23e02e | 706 | |
lilac0112_1 | 25:a7460e23e02e | 707 | LastLineCorner[L_LINE]=LINE_EMPTY; |
lilac0112_1 | 25:a7460e23e02e | 708 | LastLineCorner[R_LINE]=LINE_EMPTY; |
lilac0112_1 | 25:a7460e23e02e | 709 | LastLineCorner[F_LINE]=LINE_EMPTY; |
lilac0112_1 | 25:a7460e23e02e | 710 | LastLineCorner[B_LINE]=LINE_EMPTY; |
lilac0112_1 | 25:a7460e23e02e | 711 | } |
lilac0112_1 | 25:a7460e23e02e | 712 | //data.lnRepeat=0; |
lilac0112_1 | 25:a7460e23e02e | 713 | if((data.lnRepeat>0)&&(data.FieldSpot == LINE_INSIDE)){ |
lilac0112_1 | 25:a7460e23e02e | 714 | //x |
lilac0112_1 | 25:a7460e23e02e | 715 | if( |
lilac0112_1 | 25:a7460e23e02e | 716 | ((pow_x>=0)&&(NewLineCorner[R_LINE]))|| |
lilac0112_1 | 25:a7460e23e02e | 717 | ((pow_x<0)&&(NewLineCorner[L_LINE])) |
lilac0112_1 | 25:a7460e23e02e | 718 | ){ |
lilac0112_1 | 25:a7460e23e02e | 719 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 25:a7460e23e02e | 720 | } |
lilac0112_1 | 25:a7460e23e02e | 721 | else{ |
lilac0112_1 | 25:a7460e23e02e | 722 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 25:a7460e23e02e | 723 | } |
lilac0112_1 | 25:a7460e23e02e | 724 | //y |
lilac0112_1 | 25:a7460e23e02e | 725 | if( |
lilac0112_1 | 25:a7460e23e02e | 726 | ((pow_y>=0)&&(NewLineCorner[F_LINE]))|| |
lilac0112_1 | 25:a7460e23e02e | 727 | ((pow_y<0)&&(NewLineCorner[B_LINE])) |
lilac0112_1 | 25:a7460e23e02e | 728 | ){ |
lilac0112_1 | 25:a7460e23e02e | 729 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 25:a7460e23e02e | 730 | } |
lilac0112_1 | 25:a7460e23e02e | 731 | else{ |
lilac0112_1 | 25:a7460e23e02e | 732 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 25:a7460e23e02e | 733 | } |
lilac0112_1 | 25:a7460e23e02e | 734 | } |
lilac0112_1 | 25:a7460e23e02e | 735 | else{ |
lilac0112_1 | 25:a7460e23e02e | 736 | data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1; |
lilac0112_1 | 25:a7460e23e02e | 737 | } |
lilac0112_1 | 25:a7460e23e02e | 738 | //*/ |
lilac0112_1 | 25:a7460e23e02e | 739 | |
lilac0112_1 | 25:a7460e23e02e | 740 | } |
lilac0112_1 | 27:825c6835e3db | 741 | void LineJudgeReset2(double pow_x, double pow_y, double *x, double *y){ |
lilac0112_1 | 30:5998ba42237e | 742 | |
lilac0112_1 | 30:5998ba42237e | 743 | if(data.lnRepeat==0){ |
lilac0112_1 | 30:5998ba42237e | 744 | data.lnStayNow[X_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 745 | data.lnStayNow[Y_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 746 | } |
lilac0112_1 | 30:5998ba42237e | 747 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 748 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 749 | |
lilac0112_1 | 30:5998ba42237e | 750 | if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){ |
lilac0112_1 | 27:825c6835e3db | 751 | data.lnRepeat=1; |
lilac0112_1 | 27:825c6835e3db | 752 | } |
lilac0112_1 | 27:825c6835e3db | 753 | |
lilac0112_1 | 27:825c6835e3db | 754 | if( |
lilac0112_1 | 27:825c6835e3db | 755 | (data.irNotice==IR_NONE)|| |
lilac0112_1 | 30:5998ba42237e | 756 | //(data.irNotice==IR_FAR)|| |
lilac0112_1 | 30:5998ba42237e | 757 | //(data.irLastPosition<=7)|| |
lilac0112_1 | 27:825c6835e3db | 758 | ( |
lilac0112_1 | 30:5998ba42237e | 759 | (!( |
lilac0112_1 | 30:5998ba42237e | 760 | (data.irPosition==data.irLastPosition)|| |
lilac0112_1 | 30:5998ba42237e | 761 | (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))|| |
lilac0112_1 | 30:5998ba42237e | 762 | (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12]))|| |
lilac0112_1 | 30:5998ba42237e | 763 | (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))|| |
lilac0112_1 | 33:aa115c30892e | 764 | (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12])) |
lilac0112_1 | 33:aa115c30892e | 765 | //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))&&(cmps_set.GoalDeg==0))|| |
lilac0112_1 | 33:aa115c30892e | 766 | //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0)) |
lilac0112_1 | 30:5998ba42237e | 767 | ))&&( |
lilac0112_1 | 30:5998ba42237e | 768 | (data.irLastNotice==IR_CLOSE)|| |
lilac0112_1 | 30:5998ba42237e | 769 | (data.irLastNotice==IR_CLOSER) |
lilac0112_1 | 30:5998ba42237e | 770 | ) |
lilac0112_1 | 27:825c6835e3db | 771 | ) |
lilac0112_1 | 27:825c6835e3db | 772 | ){ |
lilac0112_1 | 27:825c6835e3db | 773 | data.lnRepeat = 0; |
lilac0112_1 | 27:825c6835e3db | 774 | |
lilac0112_1 | 27:825c6835e3db | 775 | LineRawLogReset(); |
lilac0112_1 | 27:825c6835e3db | 776 | } |
lilac0112_1 | 27:825c6835e3db | 777 | //data.lnRepeat=0; |
lilac0112_1 | 28:f246a5fbf6a5 | 778 | //if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){ |
lilac0112_1 | 30:5998ba42237e | 779 | if( |
lilac0112_1 | 30:5998ba42237e | 780 | (data.lnRepeat>=1)&& |
lilac0112_1 | 30:5998ba42237e | 781 | (data.lnRawOrder[0]==LINE_EMPTY)/*&& |
lilac0112_1 | 30:5998ba42237e | 782 | ( |
lilac0112_1 | 30:5998ba42237e | 783 | ( |
lilac0112_1 | 30:5998ba42237e | 784 | (data.irPosition==data.irLastPosition)|| |
lilac0112_1 | 30:5998ba42237e | 785 | (data.irPosition==(data.irLastPosition+1))|| |
lilac0112_1 | 30:5998ba42237e | 786 | (data.irPosition==(data.irLastPosition-1))|| |
lilac0112_1 | 30:5998ba42237e | 787 | (data.irPosition==(data.irLastPosition+11))|| |
lilac0112_1 | 30:5998ba42237e | 788 | (data.irPosition==(data.irLastPosition-11)) |
lilac0112_1 | 30:5998ba42237e | 789 | )&& |
lilac0112_1 | 30:5998ba42237e | 790 | ( |
lilac0112_1 | 30:5998ba42237e | 791 | (data.irNotice==IR_CLOSE)|| |
lilac0112_1 | 30:5998ba42237e | 792 | (data.irNotice==IR_CLOSER) |
lilac0112_1 | 30:5998ba42237e | 793 | ) |
lilac0112_1 | 30:5998ba42237e | 794 | )*/ |
lilac0112_1 | 30:5998ba42237e | 795 | ){ |
lilac0112_1 | 27:825c6835e3db | 796 | //y |
lilac0112_1 | 27:825c6835e3db | 797 | if( |
lilac0112_1 | 27:825c6835e3db | 798 | ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))|| |
lilac0112_1 | 30:5998ba42237e | 799 | ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1))) |
lilac0112_1 | 30:5998ba42237e | 800 | ){ |
lilac0112_1 | 30:5998ba42237e | 801 | if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){ |
lilac0112_1 | 30:5998ba42237e | 802 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 803 | } |
lilac0112_1 | 30:5998ba42237e | 804 | else{ |
lilac0112_1 | 30:5998ba42237e | 805 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 806 | } |
lilac0112_1 | 30:5998ba42237e | 807 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 808 | data.lnStayNow[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 809 | } |
lilac0112_1 | 30:5998ba42237e | 810 | else{ |
lilac0112_1 | 30:5998ba42237e | 811 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 812 | } |
lilac0112_1 | 30:5998ba42237e | 813 | if( |
lilac0112_1 | 27:825c6835e3db | 814 | ((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1))) |
lilac0112_1 | 27:825c6835e3db | 815 | ){ |
lilac0112_1 | 30:5998ba42237e | 816 | if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){ |
lilac0112_1 | 30:5998ba42237e | 817 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 818 | } |
lilac0112_1 | 30:5998ba42237e | 819 | else{ |
lilac0112_1 | 30:5998ba42237e | 820 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 821 | } |
lilac0112_1 | 27:825c6835e3db | 822 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 823 | data.lnStayNow[Y_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 824 | } |
lilac0112_1 | 27:825c6835e3db | 825 | else{ |
lilac0112_1 | 27:825c6835e3db | 826 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 827 | } |
lilac0112_1 | 27:825c6835e3db | 828 | //x |
lilac0112_1 | 27:825c6835e3db | 829 | if( |
lilac0112_1 | 30:5998ba42237e | 830 | ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1))) |
lilac0112_1 | 30:5998ba42237e | 831 | ){ |
lilac0112_1 | 30:5998ba42237e | 832 | if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){ |
lilac0112_1 | 30:5998ba42237e | 833 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 834 | } |
lilac0112_1 | 30:5998ba42237e | 835 | else{ |
lilac0112_1 | 30:5998ba42237e | 836 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 837 | } |
lilac0112_1 | 30:5998ba42237e | 838 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 839 | data.lnStayNow[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 840 | } |
lilac0112_1 | 30:5998ba42237e | 841 | else{ |
lilac0112_1 | 30:5998ba42237e | 842 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 843 | } |
lilac0112_1 | 30:5998ba42237e | 844 | if( |
lilac0112_1 | 27:825c6835e3db | 845 | ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1))) |
lilac0112_1 | 27:825c6835e3db | 846 | ){ |
lilac0112_1 | 30:5998ba42237e | 847 | if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){ |
lilac0112_1 | 30:5998ba42237e | 848 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 849 | } |
lilac0112_1 | 30:5998ba42237e | 850 | else{ |
lilac0112_1 | 30:5998ba42237e | 851 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 852 | } |
lilac0112_1 | 27:825c6835e3db | 853 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 854 | data.lnStayNow[X_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 855 | } |
lilac0112_1 | 27:825c6835e3db | 856 | else{ |
lilac0112_1 | 27:825c6835e3db | 857 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 858 | } |
lilac0112_1 | 30:5998ba42237e | 859 | if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){ |
lilac0112_1 | 30:5998ba42237e | 860 | //y2 |
lilac0112_1 | 30:5998ba42237e | 861 | if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){ |
lilac0112_1 | 30:5998ba42237e | 862 | if( |
lilac0112_1 | 30:5998ba42237e | 863 | ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| |
lilac0112_1 | 30:5998ba42237e | 864 | ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| |
lilac0112_1 | 30:5998ba42237e | 865 | ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==18)||(data.irPosition==19))) |
lilac0112_1 | 30:5998ba42237e | 866 | ){ |
lilac0112_1 | 30:5998ba42237e | 867 | if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){ |
lilac0112_1 | 30:5998ba42237e | 868 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 869 | } |
lilac0112_1 | 30:5998ba42237e | 870 | else{ |
lilac0112_1 | 30:5998ba42237e | 871 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 872 | } |
lilac0112_1 | 30:5998ba42237e | 873 | if(data.lnRepeat>=LINE_REPEAT){ |
lilac0112_1 | 30:5998ba42237e | 874 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 875 | } |
lilac0112_1 | 30:5998ba42237e | 876 | data.lnStayNow[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 877 | } |
lilac0112_1 | 30:5998ba42237e | 878 | else{ |
lilac0112_1 | 30:5998ba42237e | 879 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 880 | } |
lilac0112_1 | 30:5998ba42237e | 881 | } |
lilac0112_1 | 30:5998ba42237e | 882 | if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){ |
lilac0112_1 | 30:5998ba42237e | 883 | if( |
lilac0112_1 | 30:5998ba42237e | 884 | ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))|| |
lilac0112_1 | 30:5998ba42237e | 885 | ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))|| |
lilac0112_1 | 30:5998ba42237e | 886 | ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==15)||(data.irPosition==16))) |
lilac0112_1 | 30:5998ba42237e | 887 | ){ |
lilac0112_1 | 30:5998ba42237e | 888 | if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){ |
lilac0112_1 | 30:5998ba42237e | 889 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 890 | } |
lilac0112_1 | 30:5998ba42237e | 891 | else{ |
lilac0112_1 | 30:5998ba42237e | 892 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 893 | } |
lilac0112_1 | 30:5998ba42237e | 894 | if(data.lnRepeat>=LINE_REPEAT){ |
lilac0112_1 | 30:5998ba42237e | 895 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 896 | } |
lilac0112_1 | 30:5998ba42237e | 897 | data.lnStayNow[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 898 | } |
lilac0112_1 | 30:5998ba42237e | 899 | else{ |
lilac0112_1 | 30:5998ba42237e | 900 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 901 | } |
lilac0112_1 | 30:5998ba42237e | 902 | } |
lilac0112_1 | 30:5998ba42237e | 903 | //x2 |
lilac0112_1 | 30:5998ba42237e | 904 | if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){ |
lilac0112_1 | 30:5998ba42237e | 905 | if( |
lilac0112_1 | 30:5998ba42237e | 906 | ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| |
lilac0112_1 | 30:5998ba42237e | 907 | ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13))) |
lilac0112_1 | 30:5998ba42237e | 908 | ){ |
lilac0112_1 | 30:5998ba42237e | 909 | if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){ |
lilac0112_1 | 30:5998ba42237e | 910 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 911 | } |
lilac0112_1 | 30:5998ba42237e | 912 | else{ |
lilac0112_1 | 30:5998ba42237e | 913 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 914 | } |
lilac0112_1 | 30:5998ba42237e | 915 | if(data.lnRepeat>=LINE_REPEAT){ |
lilac0112_1 | 30:5998ba42237e | 916 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 917 | } |
lilac0112_1 | 30:5998ba42237e | 918 | data.lnStayNow[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 919 | } |
lilac0112_1 | 30:5998ba42237e | 920 | else{ |
lilac0112_1 | 30:5998ba42237e | 921 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 922 | } |
lilac0112_1 | 30:5998ba42237e | 923 | |
lilac0112_1 | 30:5998ba42237e | 924 | if( |
lilac0112_1 | 30:5998ba42237e | 925 | ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))|| |
lilac0112_1 | 30:5998ba42237e | 926 | ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16))) |
lilac0112_1 | 30:5998ba42237e | 927 | ){ |
lilac0112_1 | 30:5998ba42237e | 928 | if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){ |
lilac0112_1 | 30:5998ba42237e | 929 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 930 | } |
lilac0112_1 | 30:5998ba42237e | 931 | else{ |
lilac0112_1 | 30:5998ba42237e | 932 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 933 | } |
lilac0112_1 | 30:5998ba42237e | 934 | if(data.lnRepeat>=LINE_REPEAT){ |
lilac0112_1 | 30:5998ba42237e | 935 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 936 | } |
lilac0112_1 | 30:5998ba42237e | 937 | data.lnStayNow[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 938 | } |
lilac0112_1 | 30:5998ba42237e | 939 | else{ |
lilac0112_1 | 30:5998ba42237e | 940 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 30:5998ba42237e | 941 | } |
lilac0112_1 | 30:5998ba42237e | 942 | } |
lilac0112_1 | 30:5998ba42237e | 943 | if( |
lilac0112_1 | 30:5998ba42237e | 944 | (data.lnRepeat>=2) |
lilac0112_1 | 30:5998ba42237e | 945 | ){ |
lilac0112_1 | 30:5998ba42237e | 946 | if( |
lilac0112_1 | 30:5998ba42237e | 947 | ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))|| |
lilac0112_1 | 30:5998ba42237e | 948 | ((pow_y<0)&&((data.ping[B_PING]<40)&&(1))) |
lilac0112_1 | 30:5998ba42237e | 949 | ){ |
lilac0112_1 | 30:5998ba42237e | 950 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 951 | } |
lilac0112_1 | 30:5998ba42237e | 952 | if( |
lilac0112_1 | 30:5998ba42237e | 953 | ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))|| |
lilac0112_1 | 30:5998ba42237e | 954 | ((pow_x<0)&&((data.ping[L_PING]<40)&&(1))) |
lilac0112_1 | 30:5998ba42237e | 955 | ){ |
lilac0112_1 | 30:5998ba42237e | 956 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 30:5998ba42237e | 957 | } |
lilac0112_1 | 30:5998ba42237e | 958 | } |
lilac0112_1 | 30:5998ba42237e | 959 | } |
lilac0112_1 | 30:5998ba42237e | 960 | |
lilac0112_1 | 30:5998ba42237e | 961 | |
lilac0112_1 | 27:825c6835e3db | 962 | } |
lilac0112_1 | 27:825c6835e3db | 963 | else{ |
lilac0112_1 | 27:825c6835e3db | 964 | data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 965 | } |
lilac0112_1 | 27:825c6835e3db | 966 | |
lilac0112_1 | 27:825c6835e3db | 967 | } |
lilac0112_1 | 27:825c6835e3db | 968 | void LineJudgeReset3(double pow_x, double pow_y, double *x, double *y){ |
lilac0112_1 | 27:825c6835e3db | 969 | |
lilac0112_1 | 34:1c86c1299ea4 | 970 | if(data.lnRepeat==0){ |
lilac0112_1 | 34:1c86c1299ea4 | 971 | data.lnStayNow[X_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 972 | data.lnStayNow[Y_LINE]=0; |
lilac0112_1 | 27:825c6835e3db | 973 | } |
lilac0112_1 | 34:1c86c1299ea4 | 974 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 975 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 976 | |
lilac0112_1 | 34:1c86c1299ea4 | 977 | if((data.irNotice==IR_FAR)&&(data.lnRepeat>1)){ |
lilac0112_1 | 34:1c86c1299ea4 | 978 | data.lnRepeat=1; |
lilac0112_1 | 27:825c6835e3db | 979 | } |
lilac0112_1 | 34:1c86c1299ea4 | 980 | |
lilac0112_1 | 27:825c6835e3db | 981 | if( |
lilac0112_1 | 27:825c6835e3db | 982 | (data.irNotice==IR_NONE)|| |
lilac0112_1 | 27:825c6835e3db | 983 | (data.irNotice==IR_FAR)|| |
lilac0112_1 | 34:1c86c1299ea4 | 984 | //(data.irLastPosition<=7)|| |
lilac0112_1 | 27:825c6835e3db | 985 | ( |
lilac0112_1 | 34:1c86c1299ea4 | 986 | (!( |
lilac0112_1 | 34:1c86c1299ea4 | 987 | (data.irPosition==data.irLastPosition)|| |
lilac0112_1 | 34:1c86c1299ea4 | 988 | (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+1)%12]))|| |
lilac0112_1 | 34:1c86c1299ea4 | 989 | (data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-1)%12])) |
lilac0112_1 | 34:1c86c1299ea4 | 990 | //(data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+2)%12]))|| |
lilac0112_1 | 34:1c86c1299ea4 | 991 | //(data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-2)%12])) |
lilac0112_1 | 34:1c86c1299ea4 | 992 | //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24+3)%12]))&&(cmps_set.GoalDeg==0))|| |
lilac0112_1 | 34:1c86c1299ea4 | 993 | //((data.irPosition==(ir_posi_s[(data.irLastPosition-8+24-3)%12]))&&(cmps_set.GoalDeg==0)) |
lilac0112_1 | 34:1c86c1299ea4 | 994 | ))&&( |
lilac0112_1 | 34:1c86c1299ea4 | 995 | (data.irLastNotice==IR_CLOSE)|| |
lilac0112_1 | 34:1c86c1299ea4 | 996 | (data.irLastNotice==IR_CLOSER) |
lilac0112_1 | 34:1c86c1299ea4 | 997 | ) |
lilac0112_1 | 27:825c6835e3db | 998 | ) |
lilac0112_1 | 27:825c6835e3db | 999 | ){ |
lilac0112_1 | 27:825c6835e3db | 1000 | data.lnRepeat = 0; |
lilac0112_1 | 27:825c6835e3db | 1001 | |
lilac0112_1 | 34:1c86c1299ea4 | 1002 | LineRawLogReset(); |
lilac0112_1 | 27:825c6835e3db | 1003 | } |
lilac0112_1 | 27:825c6835e3db | 1004 | //data.lnRepeat=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1005 | //if((data.lnRepeat==1)&&((data.lnRawOrder[0]==LINE_EMPTY)&&(data.lnRawOrder[1]==LINE_EMPTY)&&(data.lnRawOrder[2]==LINE_EMPTY))){ |
lilac0112_1 | 34:1c86c1299ea4 | 1006 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1007 | (data.lnRepeat>=1)&& |
lilac0112_1 | 34:1c86c1299ea4 | 1008 | (data.lnRawOrder[0]==LINE_EMPTY)/*&& |
lilac0112_1 | 34:1c86c1299ea4 | 1009 | ( |
lilac0112_1 | 34:1c86c1299ea4 | 1010 | ( |
lilac0112_1 | 34:1c86c1299ea4 | 1011 | (data.irPosition==data.irLastPosition)|| |
lilac0112_1 | 34:1c86c1299ea4 | 1012 | (data.irPosition==(data.irLastPosition+1))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1013 | (data.irPosition==(data.irLastPosition-1))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1014 | (data.irPosition==(data.irLastPosition+11))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1015 | (data.irPosition==(data.irLastPosition-11)) |
lilac0112_1 | 34:1c86c1299ea4 | 1016 | )&& |
lilac0112_1 | 34:1c86c1299ea4 | 1017 | ( |
lilac0112_1 | 34:1c86c1299ea4 | 1018 | (data.irNotice==IR_CLOSE)|| |
lilac0112_1 | 34:1c86c1299ea4 | 1019 | (data.irNotice==IR_CLOSER) |
lilac0112_1 | 34:1c86c1299ea4 | 1020 | ) |
lilac0112_1 | 34:1c86c1299ea4 | 1021 | )*/ |
lilac0112_1 | 34:1c86c1299ea4 | 1022 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1023 | //y |
lilac0112_1 | 34:1c86c1299ea4 | 1024 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1025 | ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT)&&(1)))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1026 | ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT)&&(1))) |
lilac0112_1 | 34:1c86c1299ea4 | 1027 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1028 | if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){ |
lilac0112_1 | 34:1c86c1299ea4 | 1029 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1030 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1031 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1032 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1033 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1034 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1035 | data.lnStayNow[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1036 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1037 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1038 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1039 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1040 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1041 | ((pow_y<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1)&&(1))) |
lilac0112_1 | 34:1c86c1299ea4 | 1042 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1043 | if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){ |
lilac0112_1 | 34:1c86c1299ea4 | 1044 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1045 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1046 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1047 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1048 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1049 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1050 | data.lnStayNow[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1051 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1052 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1053 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1054 | } |
lilac0112_1 | 27:825c6835e3db | 1055 | //x |
lilac0112_1 | 27:825c6835e3db | 1056 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1057 | ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1))) |
lilac0112_1 | 27:825c6835e3db | 1058 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1059 | if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){ |
lilac0112_1 | 34:1c86c1299ea4 | 1060 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1061 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1062 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1063 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1064 | } |
lilac0112_1 | 27:825c6835e3db | 1065 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1066 | data.lnStayNow[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1067 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1068 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1069 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1070 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1071 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1072 | ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT)&&(1))) |
lilac0112_1 | 34:1c86c1299ea4 | 1073 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1074 | if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){ |
lilac0112_1 | 34:1c86c1299ea4 | 1075 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1076 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1077 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1078 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1079 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1080 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1081 | data.lnStayNow[X_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 1082 | } |
lilac0112_1 | 27:825c6835e3db | 1083 | else{ |
lilac0112_1 | 27:825c6835e3db | 1084 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 1085 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1086 | if((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER)){ |
lilac0112_1 | 34:1c86c1299ea4 | 1087 | //y2 |
lilac0112_1 | 34:1c86c1299ea4 | 1088 | if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){ |
lilac0112_1 | 34:1c86c1299ea4 | 1089 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1090 | ((pow_x>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1091 | ((pow_x>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1092 | ((pow_x>0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==18)||(data.irPosition==19))) |
lilac0112_1 | 34:1c86c1299ea4 | 1093 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1094 | if(data.ping[R_PING]<WhiteToWallPlus[X_PING]){ |
lilac0112_1 | 34:1c86c1299ea4 | 1095 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1096 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1097 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1098 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1099 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1100 | if(data.lnRepeat>=LINE_REPEAT){ |
lilac0112_1 | 34:1c86c1299ea4 | 1101 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1102 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1103 | data.lnStayNow[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1104 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1105 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1106 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1107 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1108 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1109 | if((data.lnStay[X_LINE]==1)&&(data.lnStay[Y_LINE]==0)){ |
lilac0112_1 | 34:1c86c1299ea4 | 1110 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1111 | ((pow_x<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1112 | ((pow_x<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))&&((data.irPosition==12)||(data.irPosition==13)))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1113 | ((pow_x<0)&&((data.lnRawOrderLog1[0]==C_SPOT)&&(1 ))&&((data.irPosition==15)||(data.irPosition==16))) |
lilac0112_1 | 34:1c86c1299ea4 | 1114 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1115 | if(data.ping[L_PING]<WhiteToWallPlus[X_PING]){ |
lilac0112_1 | 34:1c86c1299ea4 | 1116 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1117 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1118 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1119 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1120 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1121 | if(data.lnRepeat>=LINE_REPEAT){ |
lilac0112_1 | 34:1c86c1299ea4 | 1122 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1123 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1124 | data.lnStayNow[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1125 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1126 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1127 | data.lnStay[X_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1128 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1129 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1130 | //x2 |
lilac0112_1 | 34:1c86c1299ea4 | 1131 | if((data.lnStay[X_LINE]==0)&&(data.lnStay[Y_LINE]==1)){ |
lilac0112_1 | 34:1c86c1299ea4 | 1132 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1133 | ((pow_y>0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition== 9)||(data.irPosition==10)))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1134 | ((pow_y>0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==12)||(data.irPosition==13))) |
lilac0112_1 | 34:1c86c1299ea4 | 1135 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1136 | if(data.ping[F_PING]<WhiteToWallPlus[Y_PING]){ |
lilac0112_1 | 34:1c86c1299ea4 | 1137 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1138 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1139 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1140 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1141 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1142 | if(data.lnRepeat>=LINE_REPEAT){ |
lilac0112_1 | 34:1c86c1299ea4 | 1143 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1144 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1145 | data.lnStayNow[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1146 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1147 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1148 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1149 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1150 | |
lilac0112_1 | 34:1c86c1299ea4 | 1151 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1152 | ((pow_y<0)&&((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==18)||(data.irPosition==19)))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1153 | ((pow_y<0)&&((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==C_SPOT))&&((data.irPosition==15)||(data.irPosition==16))) |
lilac0112_1 | 34:1c86c1299ea4 | 1154 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1155 | if(data.ping[B_PING]<WhiteToWallPlus[Y_PING]){ |
lilac0112_1 | 34:1c86c1299ea4 | 1156 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1157 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1158 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1159 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1160 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1161 | if(data.lnRepeat>=LINE_REPEAT){ |
lilac0112_1 | 34:1c86c1299ea4 | 1162 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1163 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1164 | data.lnStayNow[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1165 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1166 | else{ |
lilac0112_1 | 34:1c86c1299ea4 | 1167 | data.lnStay[Y_LINE]=1; |
lilac0112_1 | 34:1c86c1299ea4 | 1168 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1169 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1170 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1171 | (data.lnRepeat>=2) |
lilac0112_1 | 34:1c86c1299ea4 | 1172 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1173 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1174 | ((pow_y>0)&&((data.ping[F_PING]<40)&&(1)))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1175 | ((pow_y<0)&&((data.ping[B_PING]<40)&&(1))) |
lilac0112_1 | 34:1c86c1299ea4 | 1176 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1177 | data.lnStay[Y_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1178 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1179 | if( |
lilac0112_1 | 34:1c86c1299ea4 | 1180 | ((pow_x>0)&&((data.ping[R_PING]<40)&&(1)))|| |
lilac0112_1 | 34:1c86c1299ea4 | 1181 | ((pow_x<0)&&((data.ping[L_PING]<40)&&(1))) |
lilac0112_1 | 34:1c86c1299ea4 | 1182 | ){ |
lilac0112_1 | 34:1c86c1299ea4 | 1183 | data.lnStay[X_LINE]=0; |
lilac0112_1 | 34:1c86c1299ea4 | 1184 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1185 | } |
lilac0112_1 | 27:825c6835e3db | 1186 | } |
lilac0112_1 | 34:1c86c1299ea4 | 1187 | |
lilac0112_1 | 34:1c86c1299ea4 | 1188 | |
lilac0112_1 | 27:825c6835e3db | 1189 | } |
lilac0112_1 | 27:825c6835e3db | 1190 | else{ |
lilac0112_1 | 27:825c6835e3db | 1191 | data.lnStay[X_LINE]=data.lnStay[Y_LINE]=1; |
lilac0112_1 | 27:825c6835e3db | 1192 | } |
lilac0112_1 | 27:825c6835e3db | 1193 | |
lilac0112_1 | 27:825c6835e3db | 1194 | } |
lilac0112_1 | 27:825c6835e3db | 1195 | void JudgeInSide(void){ |
lilac0112_1 | 27:825c6835e3db | 1196 | data.FieldSpot = LINE_INSIDE; |
lilac0112_1 | 27:825c6835e3db | 1197 | data.NonWall[L_LINE] = data.NonWall[R_LINE] = data.NonWall[F_LINE] = data.NonWall[B_LINE] = 0; |
lilac0112_1 | 27:825c6835e3db | 1198 | LineLiberate(); |
lilac0112_1 | 27:825c6835e3db | 1199 | LineRankClear(); |
lilac0112_1 | 27:825c6835e3db | 1200 | //Line_home.detach(); |
lilac0112_1 | 27:825c6835e3db | 1201 | } |