ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
main_processing/strategy_parts/input.cpp@19:967207de919d, 2016-03-16 (annotated)
- Committer:
- lilac0112_1
- Date:
- Wed Mar 16 02:58:31 2016 +0000
- Revision:
- 19:967207de919d
- Parent:
- 16:4fadb7a87497
- Child:
- 20:d69ce8f47f1d
TICK DecLine
Who changed what in which revision?
User | Revision | Line number | New 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 | 16:4fadb7a87497 | 62 | if((LineHold==1)||(LineHold==2)||(LineHold==4)){ |
lilac0112_1 | 16:4fadb7a87497 | 63 | if(LineHolding[A_SPOT].read()==1){ |
lilac0112_1 | 16:4fadb7a87497 | 64 | data.lnOrder[0] = A_SPOT; |
lilac0112_1 | 16:4fadb7a87497 | 65 | } |
lilac0112_1 | 16:4fadb7a87497 | 66 | else if(LineHolding[B_SPOT].read()==1){ |
lilac0112_1 | 16:4fadb7a87497 | 67 | data.lnOrder[0] = B_SPOT; |
lilac0112_1 | 16:4fadb7a87497 | 68 | } |
lilac0112_1 | 16:4fadb7a87497 | 69 | else if(LineHolding[C_SPOT].read()==1){ |
lilac0112_1 | 16:4fadb7a87497 | 70 | data.lnOrder[0] = C_SPOT; |
lilac0112_1 | 16:4fadb7a87497 | 71 | } |
lilac0112_1 | 16:4fadb7a87497 | 72 | } |
lilac0112_1 | 16:4fadb7a87497 | 73 | } |
lilac0112_1 | 0:ea35c18c85fc | 74 | //ball |
lilac0112_1 | 0:ea35c18c85fc | 75 | uint8_t ReadBall(void){//1or0 |
lilac0112_1 | 2:635947de1583 | 76 | data.ball = !BallChecker; |
lilac0112_1 | 2:635947de1583 | 77 | return (data.ball>0); |
lilac0112_1 | 0:ea35c18c85fc | 78 | } |
lilac0112_1 | 0:ea35c18c85fc | 79 | void ReadGyro(void){ |
lilac0112_1 | 0:ea35c18c85fc | 80 | //Data can be got by interrupt. |
lilac0112_1 | 0:ea35c18c85fc | 81 | } |
lilac0112_1 | 0:ea35c18c85fc | 82 | void ReadCmps(void){ |
lilac0112_1 | 0:ea35c18c85fc | 83 | cmps_set.cmps = hmc.sample()/10.0; |
lilac0112_1 | 0:ea35c18c85fc | 84 | } |
lilac0112_1 | 10:6df631c39f9b | 85 | void ValidPing(void){ |
lilac0112_1 | 10:6df631c39f9b | 86 | if(sys.UswFlag==0){ |
lilac0112_1 | 10:6df631c39f9b | 87 | sys.UswFlag=1; |
lilac0112_1 | 10:6df631c39f9b | 88 | } |
lilac0112_1 | 10:6df631c39f9b | 89 | } |
lilac0112_1 | 10:6df631c39f9b | 90 | void ValidPing2(void){ |
lilac0112_1 | 10:6df631c39f9b | 91 | if(sys.UswFlag2==0){ |
lilac0112_1 | 10:6df631c39f9b | 92 | sys.UswFlag2=1; |
lilac0112_1 | 10:6df631c39f9b | 93 | } |
lilac0112_1 | 10:6df631c39f9b | 94 | } |
lilac0112_1 | 0:ea35c18c85fc | 95 | void ReadPing(void){ |
lilac0112_1 | 0:ea35c18c85fc | 96 | uint16_t spi_data; |
lilac0112_1 | 0:ea35c18c85fc | 97 | //Ultra Sonic Wave |
lilac0112_1 | 0:ea35c18c85fc | 98 | spi_ss[1]=0; |
lilac0112_1 | 0:ea35c18c85fc | 99 | wait_us(200); |
lilac0112_1 | 0:ea35c18c85fc | 100 | |
lilac0112_1 | 0:ea35c18c85fc | 101 | spi_data = spi.write(0xABCD); |
lilac0112_1 | 0:ea35c18c85fc | 102 | |
lilac0112_1 | 0:ea35c18c85fc | 103 | wait_us(200); |
lilac0112_1 | 0:ea35c18c85fc | 104 | spi_ss[1]=1; |
lilac0112_1 | 0:ea35c18c85fc | 105 | |
lilac0112_1 | 2:635947de1583 | 106 | data.ping[0] = (spi_data&0x00FF)>>0; |
lilac0112_1 | 2:635947de1583 | 107 | data.ping[1] = (spi_data&0xFF00)>>8; |
lilac0112_1 | 0:ea35c18c85fc | 108 | } |
lilac0112_1 | 10:6df631c39f9b | 109 | void ReadPing2(void){ |
lilac0112_1 | 0:ea35c18c85fc | 110 | uint16_t spi_data; |
lilac0112_1 | 0:ea35c18c85fc | 111 | //Ultra Sonic Wave |
lilac0112_1 | 0:ea35c18c85fc | 112 | spi_ss[0]=0; |
lilac0112_1 | 0:ea35c18c85fc | 113 | wait_us(200); |
lilac0112_1 | 0:ea35c18c85fc | 114 | |
lilac0112_1 | 0:ea35c18c85fc | 115 | spi_data = spi.write(0xABCD); |
lilac0112_1 | 0:ea35c18c85fc | 116 | |
lilac0112_1 | 0:ea35c18c85fc | 117 | wait_us(200); |
lilac0112_1 | 0:ea35c18c85fc | 118 | spi_ss[0]=1; |
lilac0112_1 | 0:ea35c18c85fc | 119 | |
lilac0112_1 | 2:635947de1583 | 120 | data.ping[2] = (spi_data&0x00FF)>>0; |
lilac0112_1 | 2:635947de1583 | 121 | data.ping[3] = (spi_data&0xFF00)>>8; |
lilac0112_1 | 0:ea35c18c85fc | 122 | } |
lilac0112_1 | 0:ea35c18c85fc | 123 | void ValidIr(void){ |
lilac0112_1 | 0:ea35c18c85fc | 124 | if(sys.IrFlag==0){ |
lilac0112_1 | 0:ea35c18c85fc | 125 | sys.IrFlag=1; |
lilac0112_1 | 0:ea35c18c85fc | 126 | } |
lilac0112_1 | 0:ea35c18c85fc | 127 | } |
lilac0112_1 | 0:ea35c18c85fc | 128 | void ReadIr(void){ |
lilac0112_1 | 0:ea35c18c85fc | 129 | uint16_t spi_data; |
lilac0112_1 | 0:ea35c18c85fc | 130 | |
lilac0112_1 | 0:ea35c18c85fc | 131 | //Ir |
lilac0112_1 | 0:ea35c18c85fc | 132 | spi_ss[3]=0; |
lilac0112_1 | 0:ea35c18c85fc | 133 | wait_us(200); |
lilac0112_1 | 0:ea35c18c85fc | 134 | |
lilac0112_1 | 0:ea35c18c85fc | 135 | spi_data = spi.write(0xABCD); |
lilac0112_1 | 0:ea35c18c85fc | 136 | |
lilac0112_1 | 0:ea35c18c85fc | 137 | wait_us(200); |
lilac0112_1 | 0:ea35c18c85fc | 138 | spi_ss[3]=1; |
lilac0112_1 | 2:635947de1583 | 139 | /* |
lilac0112_1 | 1:3e013ae7900d | 140 | //key/phaseL/phaseS/long/short/ |
lilac0112_1 | 1:3e013ae7900d | 141 | // 2/ 3/ 3/ 4/ 4/ |
lilac0112_1 | 2:635947de1583 | 142 | data.irKey = (spi_data&0xC000)>>14;//1100000000000000 |
lilac0112_1 | 2:635947de1583 | 143 | data.irValPhase[IR_LONG] = (spi_data&0x3800)>>11;//0011100000000000 |
lilac0112_1 | 2:635947de1583 | 144 | data.irValPhase[IR_SHORT] = (spi_data&0x0700)>>8;///0000011100000000 |
lilac0112_1 | 2:635947de1583 | 145 | data.irSpot[IR_LONG] = (spi_data&0x00F0)>>4;///0000000011110000 |
lilac0112_1 | 2:635947de1583 | 146 | data.irSpot[IR_SHORT] = (spi_data&0x000F)>>0;///0000000000001111 |
lilac0112_1 | 2:635947de1583 | 147 | */ |
lilac0112_1 | 0:ea35c18c85fc | 148 | |
lilac0112_1 | 2:635947de1583 | 149 | //key/phaseL/phaseS/diffL/position/ |
lilac0112_1 | 2:635947de1583 | 150 | // 2/ 3/ 3/ 3/ 5/ |
lilac0112_1 | 2:635947de1583 | 151 | data.irKey = (spi_data&0xC000)>>14;//1100000000000000 |
lilac0112_1 | 2:635947de1583 | 152 | data.irValPhase[IR_LONG] = (spi_data&0x3800)>>11;//0011100000000000 |
lilac0112_1 | 2:635947de1583 | 153 | data.irValPhase[IR_SHORT] = (spi_data&0x0700)>>8;///0000011100000000 |
lilac0112_1 | 2:635947de1583 | 154 | data.irDif[IR_LONG] = (spi_data&0x00E0)>>5;///0000000011100000 |
lilac0112_1 | 2:635947de1583 | 155 | data.irPosition = (spi_data&0x001F)>>0;///0000000000011111 |
lilac0112_1 | 1:3e013ae7900d | 156 | |
lilac0112_1 | 2:635947de1583 | 157 | if(data.irKey!=0x2) data.irNotice=IR_NONE; |
lilac0112_1 | 2:635947de1583 | 158 | |
lilac0112_1 | 2:635947de1583 | 159 | |
lilac0112_1 | 2:635947de1583 | 160 | if(data.irValPhase[IR_SHORT]>=DIS_7){ |
lilac0112_1 | 10:6df631c39f9b | 161 | if(data.irValPhase[IR_LONG]>=DIS_5){ |
lilac0112_1 | 2:635947de1583 | 162 | data.irNotice=IR_NONE; |
lilac0112_1 | 1:3e013ae7900d | 163 | } |
lilac0112_1 | 1:3e013ae7900d | 164 | else{ |
lilac0112_1 | 2:635947de1583 | 165 | if(data.irDif[IR_LONG]<=DIS_6){ |
lilac0112_1 | 2:635947de1583 | 166 | data.irNotice=IR_NONE; |
lilac0112_1 | 1:3e013ae7900d | 167 | } |
lilac0112_1 | 1:3e013ae7900d | 168 | else{ |
lilac0112_1 | 2:635947de1583 | 169 | data.irNotice=IR_FAR; |
lilac0112_1 | 1:3e013ae7900d | 170 | } |
lilac0112_1 | 1:3e013ae7900d | 171 | } |
lilac0112_1 | 1:3e013ae7900d | 172 | } |
lilac0112_1 | 2:635947de1583 | 173 | else{ |
lilac0112_1 | 2:635947de1583 | 174 | if(data.irValPhase[IR_SHORT]>=DIS_3){ |
lilac0112_1 | 2:635947de1583 | 175 | data.irNotice = IR_CLOSE; |
lilac0112_1 | 2:635947de1583 | 176 | } |
lilac0112_1 | 2:635947de1583 | 177 | else{ |
lilac0112_1 | 2:635947de1583 | 178 | data.irNotice = IR_CLOSER; |
lilac0112_1 | 2:635947de1583 | 179 | } |
lilac0112_1 | 2:635947de1583 | 180 | } |
lilac0112_1 | 1:3e013ae7900d | 181 | |
lilac0112_1 | 2:635947de1583 | 182 | /*if(data.irValPhase[IR_LONG]>=DIS_7){ |
lilac0112_1 | 2:635947de1583 | 183 | data.irNotice=IR_NONE; |
lilac0112_1 | 2:635947de1583 | 184 | } |
lilac0112_1 | 2:635947de1583 | 185 | else{ |
lilac0112_1 | 2:635947de1583 | 186 | if(data.irValPhase[IR_SHORT]>=DIS_7){ |
lilac0112_1 | 2:635947de1583 | 187 | data.irNotice=IR_FAR; |
lilac0112_1 | 2:635947de1583 | 188 | } |
lilac0112_1 | 2:635947de1583 | 189 | else{ |
lilac0112_1 | 2:635947de1583 | 190 | if(data.irValPhase[IR_LONG]>=DIS_4){ |
lilac0112_1 | 2:635947de1583 | 191 | data.irNotice = IR_CLOSE; |
lilac0112_1 | 2:635947de1583 | 192 | } |
lilac0112_1 | 2:635947de1583 | 193 | else{ |
lilac0112_1 | 2:635947de1583 | 194 | data.irNotice = IR_CLOSER; |
lilac0112_1 | 2:635947de1583 | 195 | } |
lilac0112_1 | 2:635947de1583 | 196 | } |
lilac0112_1 | 2:635947de1583 | 197 | }*/ |
lilac0112_1 | 2:635947de1583 | 198 | /* |
lilac0112_1 | 2:635947de1583 | 199 | if(data.irNotice == IR_NONE) data.irPosition=11; |
lilac0112_1 | 2:635947de1583 | 200 | if(data.irNotice == IR_FAR) data.irPosition=data.irSpot[IR_LONG]-1; |
lilac0112_1 | 2:635947de1583 | 201 | if(data.irNotice == IR_CLOSE) data.irPosition=data.irSpot[IR_SHORT]+7; |
lilac0112_1 | 2:635947de1583 | 202 | if(data.irNotice == IR_CLOSER) data.irPosition=data.irSpot[IR_SHORT]+7; |
lilac0112_1 | 2:635947de1583 | 203 | */ |
lilac0112_1 | 5:5ff3a7d5d8c2 | 204 | //LED[1] = LED[0]; |
lilac0112_1 | 5:5ff3a7d5d8c2 | 205 | //LED[0] = !LED[0]; |
lilac0112_1 | 0:ea35c18c85fc | 206 | } |
lilac0112_1 | 19:967207de919d | 207 | //info |
lilac0112_1 | 19:967207de919d | 208 | void ValidInfo(void){ |
lilac0112_1 | 19:967207de919d | 209 | if(sys.InfoFlag==0){ |
lilac0112_1 | 19:967207de919d | 210 | sys.InfoFlag=1; |
lilac0112_1 | 19:967207de919d | 211 | } |
lilac0112_1 | 19:967207de919d | 212 | } |
lilac0112_1 | 0:ea35c18c85fc | 213 | void ReadInfo(void){ |
lilac0112_1 | 19:967207de919d | 214 | ReadIr(); |
lilac0112_1 | 19:967207de919d | 215 | PidUpdate(); |
lilac0112_1 | 19:967207de919d | 216 | ReadPing(); |
lilac0112_1 | 19:967207de919d | 217 | ReadPing2(); |
lilac0112_1 | 0:ea35c18c85fc | 218 | } |