CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:ea35c18c85fc 1 #include "mbed.h"
lilac0112_1 0:ea35c18c85fc 2 #include "extern.h"
lilac0112_1 0:ea35c18c85fc 3
lilac0112_1 0:ea35c18c85fc 4 //line
lilac0112_1 0:ea35c18c85fc 5 void ReadLine(void){
lilac0112_1 0:ea35c18c85fc 6 static uint8_t line_buf[3]={0,0,0};
lilac0112_1 0:ea35c18c85fc 7 wait_us(100);
lilac0112_1 0:ea35c18c85fc 8 if(Line[0].read()==1) line_buf[0] = 100;
lilac0112_1 2:635947de1583 9 if(line_buf[0]==0) data.lnFlag[0]=0;
lilac0112_1 2:635947de1583 10 else data.lnFlag[0]=1;
lilac0112_1 0:ea35c18c85fc 11 if(line_buf[0]>0) line_buf[0]--;
lilac0112_1 0:ea35c18c85fc 12
lilac0112_1 0:ea35c18c85fc 13 if(Line[1].read()==1) line_buf[1] = 100;
lilac0112_1 2:635947de1583 14 if(line_buf[1]==0) data.lnFlag[1]=0;
lilac0112_1 2:635947de1583 15 else data.lnFlag[1]=1;
lilac0112_1 0:ea35c18c85fc 16 if(line_buf[1]>0) line_buf[1]--;
lilac0112_1 0:ea35c18c85fc 17
lilac0112_1 0:ea35c18c85fc 18 if(Line[2].read()==1) line_buf[2] = 100;
lilac0112_1 2:635947de1583 19 if(line_buf[2]==0) data.lnFlag[2]=0;
lilac0112_1 2:635947de1583 20 else data.lnFlag[2]=1;
lilac0112_1 0:ea35c18c85fc 21 if(line_buf[2]>0) line_buf[2]--;
lilac0112_1 0:ea35c18c85fc 22 }
lilac0112_1 16:4fadb7a87497 23 //line_hold
lilac0112_1 16:4fadb7a87497 24 void LineRanking_A(void){
lilac0112_1 16:4fadb7a87497 25 if(data.lnOrder[0]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 26 data.lnOrder[0]=A_SPOT;
lilac0112_1 16:4fadb7a87497 27 }
lilac0112_1 16:4fadb7a87497 28 else if(data.lnOrder[1]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 29 data.lnOrder[1]=A_SPOT;
lilac0112_1 16:4fadb7a87497 30 }
lilac0112_1 16:4fadb7a87497 31 else if(data.lnOrder[2]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 32 data.lnOrder[2]=A_SPOT;
lilac0112_1 16:4fadb7a87497 33 }
lilac0112_1 16:4fadb7a87497 34 }
lilac0112_1 16:4fadb7a87497 35 void LineRanking_B(void){
lilac0112_1 16:4fadb7a87497 36 if(data.lnOrder[0]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 37 data.lnOrder[0]=B_SPOT;
lilac0112_1 16:4fadb7a87497 38 }
lilac0112_1 16:4fadb7a87497 39 else if(data.lnOrder[1]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 40 data.lnOrder[1]=B_SPOT;
lilac0112_1 16:4fadb7a87497 41 }
lilac0112_1 16:4fadb7a87497 42 else if(data.lnOrder[2]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 43 data.lnOrder[2]=B_SPOT;
lilac0112_1 16:4fadb7a87497 44 }
lilac0112_1 16:4fadb7a87497 45 }
lilac0112_1 16:4fadb7a87497 46 void LineRanking_C(void){
lilac0112_1 16:4fadb7a87497 47 if(data.lnOrder[0]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 48 data.lnOrder[0]=C_SPOT;
lilac0112_1 16:4fadb7a87497 49 }
lilac0112_1 16:4fadb7a87497 50 else if(data.lnOrder[1]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 51 data.lnOrder[1]=C_SPOT;
lilac0112_1 16:4fadb7a87497 52 }
lilac0112_1 16:4fadb7a87497 53 else if(data.lnOrder[2]==LINE_EMPTY){
lilac0112_1 16:4fadb7a87497 54 data.lnOrder[2]=C_SPOT;
lilac0112_1 16:4fadb7a87497 55 }
lilac0112_1 16:4fadb7a87497 56 }
lilac0112_1 16:4fadb7a87497 57 void LineRankClear(void){
lilac0112_1 16:4fadb7a87497 58 data.lnOrder[0]=LINE_EMPTY;
lilac0112_1 16:4fadb7a87497 59 data.lnOrder[1]=LINE_EMPTY;
lilac0112_1 16:4fadb7a87497 60 data.lnOrder[2]=LINE_EMPTY;
lilac0112_1 16:4fadb7a87497 61
lilac0112_1 22:b7720bba2ca6 62 if((0<LineHold)&&(LineHold<7)){
lilac0112_1 22:b7720bba2ca6 63 if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==1)){
lilac0112_1 22:b7720bba2ca6 64 data.lnOrder[0] = C_SPOT;
lilac0112_1 22:b7720bba2ca6 65 data.lnOrder[1] = LINE_EMPTY;
lilac0112_1 16:4fadb7a87497 66 }
lilac0112_1 22:b7720bba2ca6 67 else if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)){
lilac0112_1 16:4fadb7a87497 68 data.lnOrder[0] = B_SPOT;
lilac0112_1 22:b7720bba2ca6 69 data.lnOrder[1] = LINE_EMPTY;
lilac0112_1 16:4fadb7a87497 70 }
lilac0112_1 22:b7720bba2ca6 71 else if((LineHolding[A_SPOT].read()==0)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==1)){
lilac0112_1 16:4fadb7a87497 72 data.lnOrder[0] = C_SPOT;
lilac0112_1 22:b7720bba2ca6 73 data.lnOrder[1] = B_SPOT;
lilac0112_1 16:4fadb7a87497 74 }
lilac0112_1 22:b7720bba2ca6 75 else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==0)){
lilac0112_1 22:b7720bba2ca6 76 data.lnOrder[0] = A_SPOT;
lilac0112_1 22:b7720bba2ca6 77 data.lnOrder[1] = LINE_EMPTY;
lilac0112_1 22:b7720bba2ca6 78 }
lilac0112_1 22:b7720bba2ca6 79 else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==0)&&(LineHolding[C_SPOT].read()==1)){
lilac0112_1 22:b7720bba2ca6 80 data.lnOrder[0] = C_SPOT;
lilac0112_1 22:b7720bba2ca6 81 data.lnOrder[1] = A_SPOT;
lilac0112_1 22:b7720bba2ca6 82 }
lilac0112_1 22:b7720bba2ca6 83 else if((LineHolding[A_SPOT].read()==1)&&(LineHolding[B_SPOT].read()==1)&&(LineHolding[C_SPOT].read()==0)){
lilac0112_1 21:378470320524 84 data.lnOrder[0] = A_SPOT;
lilac0112_1 21:378470320524 85 data.lnOrder[1] = B_SPOT;
lilac0112_1 21:378470320524 86 }
lilac0112_1 16:4fadb7a87497 87 }
lilac0112_1 16:4fadb7a87497 88 }
lilac0112_1 27:825c6835e3db 89 //line raw
lilac0112_1 27:825c6835e3db 90 /*
lilac0112_1 27:825c6835e3db 91 void LineRawCall_A(void){data.lnRawMemory[A_SPOT]=1;Line_timeout[A_SPOT].attach(&LineRawClear_A, LINE_DELAY);}
lilac0112_1 27:825c6835e3db 92 void LineRawCall_B(void){data.lnRawMemory[B_SPOT]=1;Line_timeout[B_SPOT].attach(&LineRawClear_B, LINE_DELAY);}
lilac0112_1 27:825c6835e3db 93 void LineRawCall_C(void){data.lnRawMemory[C_SPOT]=1;Line_timeout[C_SPOT].attach(&LineRawClear_C, LINE_DELAY);}
lilac0112_1 27:825c6835e3db 94 void LineRawClear_A(void){if(Line[A_SPOT].read()==1){LineRawCall_A();}else{data.lnRawMemory[A_SPOT]=0;}}
lilac0112_1 27:825c6835e3db 95 void LineRawClear_B(void){if(Line[B_SPOT].read()==1){LineRawCall_B();}else{data.lnRawMemory[B_SPOT]=0;}}
lilac0112_1 27:825c6835e3db 96 void LineRawClear_C(void){if(Line[C_SPOT].read()==1){LineRawCall_C();}else{data.lnRawMemory[C_SPOT]=0;}}
lilac0112_1 27:825c6835e3db 97 */
lilac0112_1 27:825c6835e3db 98 void LineRawCall_ALL(void){
lilac0112_1 27:825c6835e3db 99 if(data.lnRawReturn==1){
lilac0112_1 27:825c6835e3db 100 if(
lilac0112_1 27:825c6835e3db 101 (data.lnRawOrder[0]==A_SPOT)||
lilac0112_1 27:825c6835e3db 102 (data.lnRawOrder[0]==B_SPOT)||
lilac0112_1 27:825c6835e3db 103 (data.lnRawOrder[0]==C_SPOT)
lilac0112_1 27:825c6835e3db 104 ){
lilac0112_1 27:825c6835e3db 105 if(
lilac0112_1 29:e8bafe48aa90 106 (
lilac0112_1 29:e8bafe48aa90 107 (data.lnRawOrder[0]!=LINE_EMPTY)&&
lilac0112_1 29:e8bafe48aa90 108 (data.lnRawOrder[1]!=LINE_EMPTY)&&
lilac0112_1 29:e8bafe48aa90 109 ((Line[data.lnRawOrder[0]].read()==1)||(data.lnRawOrder[0]==data.lnRawLastRise))
lilac0112_1 29:e8bafe48aa90 110 )||
lilac0112_1 29:e8bafe48aa90 111 (data.lnRawOrder[0]==C_SPOT)
lilac0112_1 27:825c6835e3db 112 ){
lilac0112_1 27:825c6835e3db 113 Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_3);
lilac0112_1 27:825c6835e3db 114 }
lilac0112_1 27:825c6835e3db 115 else{
lilac0112_1 27:825c6835e3db 116 Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_2);
lilac0112_1 27:825c6835e3db 117 }
lilac0112_1 27:825c6835e3db 118 }
lilac0112_1 27:825c6835e3db 119 else{
lilac0112_1 27:825c6835e3db 120 Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_2);
lilac0112_1 27:825c6835e3db 121 }
lilac0112_1 27:825c6835e3db 122 }
lilac0112_1 27:825c6835e3db 123 else{
lilac0112_1 27:825c6835e3db 124 Line_reset.attach(&LineRawClear_ALL, LINE_DELAY_1);
lilac0112_1 27:825c6835e3db 125 }
lilac0112_1 27:825c6835e3db 126 }
lilac0112_1 27:825c6835e3db 127 void LineRawClear_ALL(void){
lilac0112_1 27:825c6835e3db 128 if(LineRaw>0){
lilac0112_1 27:825c6835e3db 129 LineRawCall_ALL();
lilac0112_1 27:825c6835e3db 130 }
lilac0112_1 27:825c6835e3db 131 else if(LineRaw==0){
lilac0112_1 27:825c6835e3db 132 LineRawRankClear();
lilac0112_1 27:825c6835e3db 133 }
lilac0112_1 27:825c6835e3db 134 }
lilac0112_1 27:825c6835e3db 135 void LineRawRankClear(void){
lilac0112_1 27:825c6835e3db 136
lilac0112_1 38:67bc78f3c0ab 137 //JSO2
lilac0112_1 38:67bc78f3c0ab 138 LineLiberate();
lilac0112_1 38:67bc78f3c0ab 139
lilac0112_1 27:825c6835e3db 140 data.lnRawOrderLog2[0]=data.lnRawOrderLog1[0];
lilac0112_1 27:825c6835e3db 141 data.lnRawOrderLog2[1]=data.lnRawOrderLog1[1];
lilac0112_1 27:825c6835e3db 142 data.lnRawOrderLog2[2]=data.lnRawOrderLog1[2];
lilac0112_1 27:825c6835e3db 143
lilac0112_1 27:825c6835e3db 144 data.lnRawOrderLog1[0]=data.lnRawOrder[0];
lilac0112_1 27:825c6835e3db 145 data.lnRawOrderLog1[1]=data.lnRawOrder[1];
lilac0112_1 27:825c6835e3db 146 data.lnRawOrderLog1[2]=data.lnRawOrder[2];
lilac0112_1 27:825c6835e3db 147
lilac0112_1 27:825c6835e3db 148 data.lnRawOrder[0]=LINE_EMPTY;
lilac0112_1 27:825c6835e3db 149 data.lnRawOrder[1]=LINE_EMPTY;
lilac0112_1 27:825c6835e3db 150 data.lnRawOrder[2]=LINE_EMPTY;
lilac0112_1 27:825c6835e3db 151
lilac0112_1 30:5998ba42237e 152 if(
lilac0112_1 30:5998ba42237e 153 (data.lnRawOrderLog2[0]==data.lnRawOrderLog1[0])&&
lilac0112_1 30:5998ba42237e 154 //(data.lnRawOrderLog2[1]==data.lnRawOrderLog1[1])&&
lilac0112_1 30:5998ba42237e 155 //(data.lnRawOrderLog2[2]==data.lnRawOrderLog1[2])
lilac0112_1 30:5998ba42237e 156 (data.lnRawOrderLog2[0]!=LINE_EMPTY)
lilac0112_1 30:5998ba42237e 157 //(data.lnRawOrderLog2[1]!=LINE_EMPTY)
lilac0112_1 30:5998ba42237e 158 ){
lilac0112_1 30:5998ba42237e 159 if(data.lnRepeat>100) data.lnRepeat=100;
lilac0112_1 30:5998ba42237e 160 data.lnRepeat++;
lilac0112_1 30:5998ba42237e 161
lilac0112_1 30:5998ba42237e 162 data.irLastNotice = data.irNotice;
lilac0112_1 30:5998ba42237e 163 data.irLastPosition = data.irPosition;
lilac0112_1 30:5998ba42237e 164 }
lilac0112_1 30:5998ba42237e 165 else{
lilac0112_1 30:5998ba42237e 166 data.lnRepeat=0;
lilac0112_1 30:5998ba42237e 167 }
lilac0112_1 30:5998ba42237e 168
lilac0112_1 30:5998ba42237e 169
lilac0112_1 27:825c6835e3db 170 data.lnRawRise[A_SPOT]=0;
lilac0112_1 27:825c6835e3db 171 data.lnRawRise[B_SPOT]=0;
lilac0112_1 27:825c6835e3db 172 data.lnRawRise[C_SPOT]=0;
lilac0112_1 27:825c6835e3db 173 }
lilac0112_1 27:825c6835e3db 174 void LineRawLogReset(void){
lilac0112_1 30:5998ba42237e 175 /*
lilac0112_1 30:5998ba42237e 176 if(
lilac0112_1 30:5998ba42237e 177 (!(
lilac0112_1 30:5998ba42237e 178 (data.irPosition==data.irLastPosition)||
lilac0112_1 30:5998ba42237e 179 (data.irPosition==(data.irLastPosition+1))||
lilac0112_1 30:5998ba42237e 180 (data.irPosition==(data.irLastPosition-1))||
lilac0112_1 30:5998ba42237e 181 (data.irPosition==(data.irLastPosition+11))||
lilac0112_1 30:5998ba42237e 182 (data.irPosition==(data.irLastPosition-11))
lilac0112_1 30:5998ba42237e 183 ))&&
lilac0112_1 30:5998ba42237e 184 (
lilac0112_1 30:5998ba42237e 185 (data.irNotice==IR_CLOSE)||
lilac0112_1 30:5998ba42237e 186 (data.irNotice==IR_CLOSER)
lilac0112_1 30:5998ba42237e 187 )&&
lilac0112_1 30:5998ba42237e 188 (
lilac0112_1 30:5998ba42237e 189 (data.irLastPosition>7)
lilac0112_1 30:5998ba42237e 190 )
lilac0112_1 30:5998ba42237e 191 ){
lilac0112_1 30:5998ba42237e 192 data.lnRawOrderLog2[0]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 193 data.lnRawOrderLog2[1]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 194 data.lnRawOrderLog2[2]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 195 }
lilac0112_1 30:5998ba42237e 196 else{
lilac0112_1 30:5998ba42237e 197 data.lnRawOrderLog2[0]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 198 data.lnRawOrderLog2[1]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 199 data.lnRawOrderLog2[2]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 200
lilac0112_1 30:5998ba42237e 201 data.lnRawOrderLog1[0]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 202 data.lnRawOrderLog1[1]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 203 data.lnRawOrderLog1[2]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 204 }*/
lilac0112_1 30:5998ba42237e 205 if(data.irNotice==IR_NONE){
lilac0112_1 30:5998ba42237e 206 data.lnRawOrderLog2[0]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 207 data.lnRawOrderLog2[1]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 208 data.lnRawOrderLog2[2]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 209
lilac0112_1 30:5998ba42237e 210 data.lnRawOrderLog1[0]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 211 data.lnRawOrderLog1[1]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 212 data.lnRawOrderLog1[2]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 213 }
lilac0112_1 30:5998ba42237e 214 else{
lilac0112_1 30:5998ba42237e 215 data.lnRawOrderLog2[0]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 216 data.lnRawOrderLog2[1]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 217 data.lnRawOrderLog2[2]=LINE_EMPTY;
lilac0112_1 30:5998ba42237e 218 }
lilac0112_1 27:825c6835e3db 219
lilac0112_1 27:825c6835e3db 220 }
lilac0112_1 27:825c6835e3db 221 void LineRawRanking_A(void){
lilac0112_1 27:825c6835e3db 222 data.lnRawLastRise=A_SPOT;
lilac0112_1 27:825c6835e3db 223 LineRawCall_ALL();
lilac0112_1 27:825c6835e3db 224 if(data.lnRawRise[A_SPOT]==1) return;
lilac0112_1 27:825c6835e3db 225
lilac0112_1 27:825c6835e3db 226 if((data.lnRawOrder[0]==LINE_EMPTY)||(data.lnRawOrder[0]==A_SPOT)){
lilac0112_1 27:825c6835e3db 227 data.lnRawOrder[0]=A_SPOT;
lilac0112_1 30:5998ba42237e 228 if((sys.DefenceFlag==1)&&(data.lnRawOrder[1]==LINE_EMPTY)){
lilac0112_1 30:5998ba42237e 229 data.lnRawOrder[1]=C_SPOT;
lilac0112_1 30:5998ba42237e 230 }
lilac0112_1 27:825c6835e3db 231 }
lilac0112_1 27:825c6835e3db 232 else if((data.lnRawOrder[1]==LINE_EMPTY)||(data.lnRawOrder[1]==A_SPOT)){
lilac0112_1 27:825c6835e3db 233 data.lnRawOrder[1]=A_SPOT;
lilac0112_1 27:825c6835e3db 234 }
lilac0112_1 27:825c6835e3db 235 else if((data.lnRawOrder[2]==LINE_EMPTY)||(data.lnRawOrder[2]==A_SPOT)){
lilac0112_1 27:825c6835e3db 236 data.lnRawOrder[2]=A_SPOT;
lilac0112_1 27:825c6835e3db 237 }
lilac0112_1 27:825c6835e3db 238 data.lnRawRise[A_SPOT]=1;
lilac0112_1 27:825c6835e3db 239 }
lilac0112_1 27:825c6835e3db 240 void LineRawRanking_B(void){
lilac0112_1 27:825c6835e3db 241 data.lnRawLastRise=B_SPOT;
lilac0112_1 27:825c6835e3db 242 LineRawCall_ALL();
lilac0112_1 27:825c6835e3db 243 if(data.lnRawRise[B_SPOT]==1) return;
lilac0112_1 27:825c6835e3db 244
lilac0112_1 27:825c6835e3db 245 if((data.lnRawOrder[0]==LINE_EMPTY)||(data.lnRawOrder[0]==B_SPOT)){
lilac0112_1 27:825c6835e3db 246 data.lnRawOrder[0]=B_SPOT;
lilac0112_1 30:5998ba42237e 247 if((sys.DefenceFlag==1)&&(data.lnRawOrder[1]==LINE_EMPTY)){
lilac0112_1 30:5998ba42237e 248 data.lnRawOrder[1]=C_SPOT;
lilac0112_1 30:5998ba42237e 249 }
lilac0112_1 27:825c6835e3db 250 }
lilac0112_1 27:825c6835e3db 251 else if((data.lnRawOrder[1]==LINE_EMPTY)||(data.lnRawOrder[1]==B_SPOT)){
lilac0112_1 27:825c6835e3db 252 data.lnRawOrder[1]=B_SPOT;
lilac0112_1 27:825c6835e3db 253 }
lilac0112_1 27:825c6835e3db 254 else if((data.lnRawOrder[2]==LINE_EMPTY)||(data.lnRawOrder[2]==B_SPOT)){
lilac0112_1 27:825c6835e3db 255 data.lnRawOrder[2]=B_SPOT;
lilac0112_1 27:825c6835e3db 256 }
lilac0112_1 27:825c6835e3db 257 data.lnRawRise[B_SPOT]=1;
lilac0112_1 27:825c6835e3db 258 }
lilac0112_1 27:825c6835e3db 259 void LineRawRanking_C(void){
lilac0112_1 27:825c6835e3db 260 data.lnRawLastRise=C_SPOT;
lilac0112_1 27:825c6835e3db 261 LineRawCall_ALL();
lilac0112_1 27:825c6835e3db 262 if(data.lnRawRise[C_SPOT]==1) return;
lilac0112_1 27:825c6835e3db 263
lilac0112_1 27:825c6835e3db 264 if((data.lnRawOrder[0]==LINE_EMPTY)||(data.lnRawOrder[0]==C_SPOT)){
lilac0112_1 27:825c6835e3db 265 data.lnRawOrder[0]=C_SPOT;
lilac0112_1 27:825c6835e3db 266 }
lilac0112_1 27:825c6835e3db 267 else if((data.lnRawOrder[1]==LINE_EMPTY)||(data.lnRawOrder[1]==C_SPOT)){
lilac0112_1 27:825c6835e3db 268 data.lnRawOrder[1]=C_SPOT;
lilac0112_1 27:825c6835e3db 269 }
lilac0112_1 27:825c6835e3db 270 else if((data.lnRawOrder[2]==LINE_EMPTY)||(data.lnRawOrder[2]==C_SPOT)){
lilac0112_1 27:825c6835e3db 271 data.lnRawOrder[2]=C_SPOT;
lilac0112_1 27:825c6835e3db 272 }
lilac0112_1 27:825c6835e3db 273 data.lnRawRise[C_SPOT]=1;
lilac0112_1 27:825c6835e3db 274 }
lilac0112_1 0:ea35c18c85fc 275 //ball
lilac0112_1 0:ea35c18c85fc 276 uint8_t ReadBall(void){//1or0
lilac0112_1 33:aa115c30892e 277 return (BallChecker.read()==0);
lilac0112_1 33:aa115c30892e 278 //return (BallCheckerA.read_u16()<40000);
lilac0112_1 0:ea35c18c85fc 279 }
lilac0112_1 25:a7460e23e02e 280 void JudgeBallHolding(void){
lilac0112_1 25:a7460e23e02e 281 //ホールド判定
lilac0112_1 25:a7460e23e02e 282 if(sys.DriBlind==0){
lilac0112_1 25:a7460e23e02e 283 if(
lilac0112_1 25:a7460e23e02e 284 ((data.ball==0)&&(sys.BallHoldFlag==0))||
lilac0112_1 25:a7460e23e02e 285 ((data.ball==1)&&(sys.BallHoldFlag==1))
lilac0112_1 25:a7460e23e02e 286 ){
lilac0112_1 25:a7460e23e02e 287 sys.BallHoldJudgeFlag=0;
lilac0112_1 25:a7460e23e02e 288 Ball_judge.detach();
lilac0112_1 25:a7460e23e02e 289 }
lilac0112_1 25:a7460e23e02e 290 if(
lilac0112_1 25:a7460e23e02e 291 ((data.ball==0)&&(sys.BallHoldFlag==1))||
lilac0112_1 25:a7460e23e02e 292 ((data.ball==1)&&(sys.BallHoldFlag==0))
lilac0112_1 25:a7460e23e02e 293 ){
lilac0112_1 25:a7460e23e02e 294 if(sys.BallHoldJudgeFlag==0){
lilac0112_1 25:a7460e23e02e 295 sys.BallHoldJudgeFlag=1;
lilac0112_1 30:5998ba42237e 296 Ball_judge.attach(&JudgeBallHold, .25);
lilac0112_1 25:a7460e23e02e 297 }
lilac0112_1 25:a7460e23e02e 298 }
lilac0112_1 25:a7460e23e02e 299 /*
lilac0112_1 25:a7460e23e02e 300 if(
lilac0112_1 25:a7460e23e02e 301 !(
lilac0112_1 25:a7460e23e02e 302 (data.irNotice==IR_CLOSER)&&
lilac0112_1 25:a7460e23e02e 303 ((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))
lilac0112_1 25:a7460e23e02e 304 )
lilac0112_1 25:a7460e23e02e 305 ){
lilac0112_1 25:a7460e23e02e 306 sys.BallHoldJudgeFlag=0;
lilac0112_1 25:a7460e23e02e 307 sys.BallHoldFlag=0;
lilac0112_1 25:a7460e23e02e 308 Ball_judge.detach();
lilac0112_1 25:a7460e23e02e 309 }
lilac0112_1 25:a7460e23e02e 310 */
lilac0112_1 25:a7460e23e02e 311 }
lilac0112_1 25:a7460e23e02e 312 else{
lilac0112_1 25:a7460e23e02e 313 sys.BallHoldFlag=0;
lilac0112_1 25:a7460e23e02e 314 }
lilac0112_1 25:a7460e23e02e 315 }
lilac0112_1 25:a7460e23e02e 316 //readsensor
lilac0112_1 0:ea35c18c85fc 317 void ReadCmps(void){
lilac0112_1 0:ea35c18c85fc 318 cmps_set.cmps = hmc.sample()/10.0;
lilac0112_1 0:ea35c18c85fc 319 }
lilac0112_1 0:ea35c18c85fc 320 void ReadPing(void){
lilac0112_1 0:ea35c18c85fc 321 uint16_t spi_data;
lilac0112_1 0:ea35c18c85fc 322 //Ultra Sonic Wave
lilac0112_1 0:ea35c18c85fc 323 spi_ss[1]=0;
lilac0112_1 0:ea35c18c85fc 324 wait_us(200);
lilac0112_1 0:ea35c18c85fc 325
lilac0112_1 0:ea35c18c85fc 326 spi_data = spi.write(0xABCD);
lilac0112_1 0:ea35c18c85fc 327
lilac0112_1 0:ea35c18c85fc 328 wait_us(200);
lilac0112_1 0:ea35c18c85fc 329 spi_ss[1]=1;
lilac0112_1 0:ea35c18c85fc 330
lilac0112_1 2:635947de1583 331 data.ping[0] = (spi_data&0x00FF)>>0;
lilac0112_1 2:635947de1583 332 data.ping[1] = (spi_data&0xFF00)>>8;
lilac0112_1 0:ea35c18c85fc 333 }
lilac0112_1 10:6df631c39f9b 334 void ReadPing2(void){
lilac0112_1 0:ea35c18c85fc 335 uint16_t spi_data;
lilac0112_1 0:ea35c18c85fc 336 //Ultra Sonic Wave
lilac0112_1 0:ea35c18c85fc 337 spi_ss[0]=0;
lilac0112_1 0:ea35c18c85fc 338 wait_us(200);
lilac0112_1 0:ea35c18c85fc 339
lilac0112_1 0:ea35c18c85fc 340 spi_data = spi.write(0xABCD);
lilac0112_1 0:ea35c18c85fc 341
lilac0112_1 0:ea35c18c85fc 342 wait_us(200);
lilac0112_1 0:ea35c18c85fc 343 spi_ss[0]=1;
lilac0112_1 0:ea35c18c85fc 344
lilac0112_1 2:635947de1583 345 data.ping[2] = (spi_data&0x00FF)>>0;
lilac0112_1 2:635947de1583 346 data.ping[3] = (spi_data&0xFF00)>>8;
lilac0112_1 0:ea35c18c85fc 347 }
lilac0112_1 0:ea35c18c85fc 348 void ReadIr(void){
lilac0112_1 0:ea35c18c85fc 349 uint16_t spi_data;
lilac0112_1 0:ea35c18c85fc 350
lilac0112_1 0:ea35c18c85fc 351 //Ir
lilac0112_1 0:ea35c18c85fc 352 spi_ss[3]=0;
lilac0112_1 0:ea35c18c85fc 353 wait_us(200);
lilac0112_1 0:ea35c18c85fc 354
lilac0112_1 0:ea35c18c85fc 355 spi_data = spi.write(0xABCD);
lilac0112_1 0:ea35c18c85fc 356
lilac0112_1 0:ea35c18c85fc 357 wait_us(200);
lilac0112_1 0:ea35c18c85fc 358 spi_ss[3]=1;
lilac0112_1 2:635947de1583 359 /*
lilac0112_1 1:3e013ae7900d 360 //key/phaseL/phaseS/long/short/
lilac0112_1 1:3e013ae7900d 361 // 2/ 3/ 3/ 4/ 4/
lilac0112_1 2:635947de1583 362 data.irKey = (spi_data&0xC000)>>14;//1100000000000000
lilac0112_1 2:635947de1583 363 data.irValPhase[IR_LONG] = (spi_data&0x3800)>>11;//0011100000000000
lilac0112_1 2:635947de1583 364 data.irValPhase[IR_SHORT] = (spi_data&0x0700)>>8;///0000011100000000
lilac0112_1 2:635947de1583 365 data.irSpot[IR_LONG] = (spi_data&0x00F0)>>4;///0000000011110000
lilac0112_1 2:635947de1583 366 data.irSpot[IR_SHORT] = (spi_data&0x000F)>>0;///0000000000001111
lilac0112_1 2:635947de1583 367 */
lilac0112_1 0:ea35c18c85fc 368
lilac0112_1 2:635947de1583 369 //key/phaseL/phaseS/diffL/position/
lilac0112_1 2:635947de1583 370 // 2/ 3/ 3/ 3/ 5/
lilac0112_1 2:635947de1583 371 data.irKey = (spi_data&0xC000)>>14;//1100000000000000
lilac0112_1 2:635947de1583 372 data.irValPhase[IR_LONG] = (spi_data&0x3800)>>11;//0011100000000000
lilac0112_1 2:635947de1583 373 data.irValPhase[IR_SHORT] = (spi_data&0x0700)>>8;///0000011100000000
lilac0112_1 2:635947de1583 374 data.irDif[IR_LONG] = (spi_data&0x00E0)>>5;///0000000011100000
lilac0112_1 2:635947de1583 375 data.irPosition = (spi_data&0x001F)>>0;///0000000000011111
lilac0112_1 1:3e013ae7900d 376
lilac0112_1 2:635947de1583 377 if(data.irKey!=0x2) data.irNotice=IR_NONE;
lilac0112_1 2:635947de1583 378
lilac0112_1 2:635947de1583 379
lilac0112_1 2:635947de1583 380 if(data.irValPhase[IR_SHORT]>=DIS_7){
lilac0112_1 27:825c6835e3db 381 if(data.irValPhase[IR_LONG]>=DIS_7){
lilac0112_1 2:635947de1583 382 data.irNotice=IR_NONE;
lilac0112_1 1:3e013ae7900d 383 }
lilac0112_1 1:3e013ae7900d 384 else{
lilac0112_1 27:825c6835e3db 385 if(data.irDif[IR_LONG]<=DIS_0){
lilac0112_1 2:635947de1583 386 data.irNotice=IR_NONE;
lilac0112_1 1:3e013ae7900d 387 }
lilac0112_1 1:3e013ae7900d 388 else{
lilac0112_1 2:635947de1583 389 data.irNotice=IR_FAR;
lilac0112_1 1:3e013ae7900d 390 }
lilac0112_1 1:3e013ae7900d 391 }
lilac0112_1 1:3e013ae7900d 392 }
lilac0112_1 2:635947de1583 393 else{
lilac0112_1 29:e8bafe48aa90 394 if(data.irValPhase[IR_SHORT]>=DIS_3){
lilac0112_1 2:635947de1583 395 data.irNotice = IR_CLOSE;
lilac0112_1 2:635947de1583 396 }
lilac0112_1 2:635947de1583 397 else{
lilac0112_1 2:635947de1583 398 data.irNotice = IR_CLOSER;
lilac0112_1 2:635947de1583 399 }
lilac0112_1 2:635947de1583 400 }
lilac0112_1 1:3e013ae7900d 401
lilac0112_1 2:635947de1583 402 /*if(data.irValPhase[IR_LONG]>=DIS_7){
lilac0112_1 2:635947de1583 403 data.irNotice=IR_NONE;
lilac0112_1 2:635947de1583 404 }
lilac0112_1 2:635947de1583 405 else{
lilac0112_1 2:635947de1583 406 if(data.irValPhase[IR_SHORT]>=DIS_7){
lilac0112_1 2:635947de1583 407 data.irNotice=IR_FAR;
lilac0112_1 2:635947de1583 408 }
lilac0112_1 2:635947de1583 409 else{
lilac0112_1 2:635947de1583 410 if(data.irValPhase[IR_LONG]>=DIS_4){
lilac0112_1 2:635947de1583 411 data.irNotice = IR_CLOSE;
lilac0112_1 2:635947de1583 412 }
lilac0112_1 2:635947de1583 413 else{
lilac0112_1 2:635947de1583 414 data.irNotice = IR_CLOSER;
lilac0112_1 2:635947de1583 415 }
lilac0112_1 2:635947de1583 416 }
lilac0112_1 2:635947de1583 417 }*/
lilac0112_1 2:635947de1583 418 /*
lilac0112_1 2:635947de1583 419 if(data.irNotice == IR_NONE) data.irPosition=11;
lilac0112_1 2:635947de1583 420 if(data.irNotice == IR_FAR) data.irPosition=data.irSpot[IR_LONG]-1;
lilac0112_1 2:635947de1583 421 if(data.irNotice == IR_CLOSE) data.irPosition=data.irSpot[IR_SHORT]+7;
lilac0112_1 2:635947de1583 422 if(data.irNotice == IR_CLOSER) data.irPosition=data.irSpot[IR_SHORT]+7;
lilac0112_1 2:635947de1583 423 */
lilac0112_1 5:5ff3a7d5d8c2 424 //LED[1] = LED[0];
lilac0112_1 5:5ff3a7d5d8c2 425 //LED[0] = !LED[0];
lilac0112_1 0:ea35c18c85fc 426 }
lilac0112_1 19:967207de919d 427 //info
lilac0112_1 19:967207de919d 428 void ValidInfo(void){
lilac0112_1 21:378470320524 429 //LED=0xA;
lilac0112_1 19:967207de919d 430 if(sys.InfoFlag==0){
lilac0112_1 19:967207de919d 431 sys.InfoFlag=1;
lilac0112_1 19:967207de919d 432 }
lilac0112_1 19:967207de919d 433 }
lilac0112_1 0:ea35c18c85fc 434 void ReadInfo(void){
lilac0112_1 19:967207de919d 435 ReadIr();
lilac0112_1 19:967207de919d 436 PidUpdate();
lilac0112_1 19:967207de919d 437 ReadPing();
lilac0112_1 19:967207de919d 438 ReadPing2();
lilac0112_1 0:ea35c18c85fc 439 }