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