Supakorn Meelarp
/
8B_Bike_Light_Back
Bike Light Back Side
main.cpp@0:b5bafad435c8, 2015-12-09 (annotated)
- Committer:
- kornvarrel
- Date:
- Wed Dec 09 10:15:06 2015 +0000
- Revision:
- 0:b5bafad435c8
Bike Light Back Side
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kornvarrel | 0:b5bafad435c8 | 1 | #include "mbed.h" |
kornvarrel | 0:b5bafad435c8 | 2 | #include "hcsr04.h" |
kornvarrel | 0:b5bafad435c8 | 3 | #include "math.h" |
kornvarrel | 0:b5bafad435c8 | 4 | |
kornvarrel | 0:b5bafad435c8 | 5 | Serial pc(D8, D2); |
kornvarrel | 0:b5bafad435c8 | 6 | |
kornvarrel | 0:b5bafad435c8 | 7 | DigitalIn digi(D6); |
kornvarrel | 0:b5bafad435c8 | 8 | DigitalOut led(LED1); |
kornvarrel | 0:b5bafad435c8 | 9 | |
kornvarrel | 0:b5bafad435c8 | 10 | //D12 TRIGGER D9 ECHO |
kornvarrel | 0:b5bafad435c8 | 11 | HCSR04 sensor(D12, D9); |
kornvarrel | 0:b5bafad435c8 | 12 | |
kornvarrel | 0:b5bafad435c8 | 13 | Timer t; |
kornvarrel | 0:b5bafad435c8 | 14 | |
kornvarrel | 0:b5bafad435c8 | 15 | SPI max72_spi(D11,NC,D13); |
kornvarrel | 0:b5bafad435c8 | 16 | DigitalOut load(D10); |
kornvarrel | 0:b5bafad435c8 | 17 | |
kornvarrel | 0:b5bafad435c8 | 18 | int maxInUse = 1; |
kornvarrel | 0:b5bafad435c8 | 19 | |
kornvarrel | 0:b5bafad435c8 | 20 | #define max7219_reg_noop 0x00 |
kornvarrel | 0:b5bafad435c8 | 21 | #define max7219_reg_digit0 0x01 |
kornvarrel | 0:b5bafad435c8 | 22 | #define max7219_reg_digit1 0x02 |
kornvarrel | 0:b5bafad435c8 | 23 | #define max7219_reg_digit2 0x03 |
kornvarrel | 0:b5bafad435c8 | 24 | #define max7219_reg_digit3 0x04 |
kornvarrel | 0:b5bafad435c8 | 25 | #define max7219_reg_digit4 0x05 |
kornvarrel | 0:b5bafad435c8 | 26 | #define max7219_reg_digit5 0x06 |
kornvarrel | 0:b5bafad435c8 | 27 | #define max7219_reg_digit6 0x07 |
kornvarrel | 0:b5bafad435c8 | 28 | #define max7219_reg_digit7 0x08 |
kornvarrel | 0:b5bafad435c8 | 29 | #define max7219_reg_decodeMode 0x09 |
kornvarrel | 0:b5bafad435c8 | 30 | #define max7219_reg_intensity 0x0a |
kornvarrel | 0:b5bafad435c8 | 31 | #define max7219_reg_scanLimit 0x0b |
kornvarrel | 0:b5bafad435c8 | 32 | #define max7219_reg_shutdown 0x0c |
kornvarrel | 0:b5bafad435c8 | 33 | #define max7219_reg_displayTest 0x0f |
kornvarrel | 0:b5bafad435c8 | 34 | |
kornvarrel | 0:b5bafad435c8 | 35 | #define MHZ 1000000 |
kornvarrel | 0:b5bafad435c8 | 36 | |
kornvarrel | 0:b5bafad435c8 | 37 | void maxSin(int reg,int col) |
kornvarrel | 0:b5bafad435c8 | 38 | { |
kornvarrel | 0:b5bafad435c8 | 39 | load = 0; |
kornvarrel | 0:b5bafad435c8 | 40 | max72_spi.write(reg); |
kornvarrel | 0:b5bafad435c8 | 41 | max72_spi.write(col); |
kornvarrel | 0:b5bafad435c8 | 42 | load =1; |
kornvarrel | 0:b5bafad435c8 | 43 | } |
kornvarrel | 0:b5bafad435c8 | 44 | void maxall(int reg,int col) |
kornvarrel | 0:b5bafad435c8 | 45 | { |
kornvarrel | 0:b5bafad435c8 | 46 | load = 0; |
kornvarrel | 0:b5bafad435c8 | 47 | for(int c=1; c<=maxInUse; c++) { |
kornvarrel | 0:b5bafad435c8 | 48 | max72_spi.write(reg); |
kornvarrel | 0:b5bafad435c8 | 49 | max72_spi.write(col); |
kornvarrel | 0:b5bafad435c8 | 50 | } |
kornvarrel | 0:b5bafad435c8 | 51 | load =1; |
kornvarrel | 0:b5bafad435c8 | 52 | } |
kornvarrel | 0:b5bafad435c8 | 53 | /*void maxOne(int NU,int reg,int col) |
kornvarrel | 0:b5bafad435c8 | 54 | { |
kornvarrel | 0:b5bafad435c8 | 55 | int c=0; |
kornvarrel | 0:b5bafad435c8 | 56 | load = 0; |
kornvarrel | 0:b5bafad435c8 | 57 | for(c=maxInUse; c>NU; c--) { |
kornvarrel | 0:b5bafad435c8 | 58 | max72_spi.write(0); |
kornvarrel | 0:b5bafad435c8 | 59 | max72_spi.write(0); |
kornvarrel | 0:b5bafad435c8 | 60 | } |
kornvarrel | 0:b5bafad435c8 | 61 | max72_spi.write(reg); |
kornvarrel | 0:b5bafad435c8 | 62 | max72_spi.write(col); |
kornvarrel | 0:b5bafad435c8 | 63 | for(c=NU-1; c>=1; c--) { |
kornvarrel | 0:b5bafad435c8 | 64 | max72_spi.write(0); |
kornvarrel | 0:b5bafad435c8 | 65 | max72_spi.write(0); |
kornvarrel | 0:b5bafad435c8 | 66 | } |
kornvarrel | 0:b5bafad435c8 | 67 | load = 1; |
kornvarrel | 0:b5bafad435c8 | 68 | }*/ |
kornvarrel | 0:b5bafad435c8 | 69 | void Set() |
kornvarrel | 0:b5bafad435c8 | 70 | { |
kornvarrel | 0:b5bafad435c8 | 71 | max72_spi.format(8,0); |
kornvarrel | 0:b5bafad435c8 | 72 | |
kornvarrel | 0:b5bafad435c8 | 73 | maxall(max7219_reg_scanLimit,0x07); |
kornvarrel | 0:b5bafad435c8 | 74 | maxall(max7219_reg_decodeMode,0x00); |
kornvarrel | 0:b5bafad435c8 | 75 | maxall(max7219_reg_shutdown,0x01); |
kornvarrel | 0:b5bafad435c8 | 76 | maxall(max7219_reg_displayTest,0x00); |
kornvarrel | 0:b5bafad435c8 | 77 | for(int i=1; i<=8; i++) { |
kornvarrel | 0:b5bafad435c8 | 78 | maxall(i,0); |
kornvarrel | 0:b5bafad435c8 | 79 | } |
kornvarrel | 0:b5bafad435c8 | 80 | maxall(max7219_reg_intensity,0x0f&0x0f); |
kornvarrel | 0:b5bafad435c8 | 81 | |
kornvarrel | 0:b5bafad435c8 | 82 | } |
kornvarrel | 0:b5bafad435c8 | 83 | void loop() |
kornvarrel | 0:b5bafad435c8 | 84 | { |
kornvarrel | 0:b5bafad435c8 | 85 | int i,j,k,f,n; |
kornvarrel | 0:b5bafad435c8 | 86 | double z=2; |
kornvarrel | 0:b5bafad435c8 | 87 | |
kornvarrel | 0:b5bafad435c8 | 88 | int q,w; |
kornvarrel | 0:b5bafad435c8 | 89 | int num1,num2,num3,num4; |
kornvarrel | 0:b5bafad435c8 | 90 | |
kornvarrel | 0:b5bafad435c8 | 91 | int hallState = 0; |
kornvarrel | 0:b5bafad435c8 | 92 | int count = 0; |
kornvarrel | 0:b5bafad435c8 | 93 | int speed; |
kornvarrel | 0:b5bafad435c8 | 94 | float rad = 1; |
kornvarrel | 0:b5bafad435c8 | 95 | int dis = 0; |
kornvarrel | 0:b5bafad435c8 | 96 | int di = 0; |
kornvarrel | 0:b5bafad435c8 | 97 | |
kornvarrel | 0:b5bafad435c8 | 98 | t.start(); |
kornvarrel | 0:b5bafad435c8 | 99 | |
kornvarrel | 0:b5bafad435c8 | 100 | while(1) { |
kornvarrel | 0:b5bafad435c8 | 101 | |
kornvarrel | 0:b5bafad435c8 | 102 | |
kornvarrel | 0:b5bafad435c8 | 103 | if(pc.readable()) { |
kornvarrel | 0:b5bafad435c8 | 104 | num1 = pc.getc(); |
kornvarrel | 0:b5bafad435c8 | 105 | num1 = pc.getc(); |
kornvarrel | 0:b5bafad435c8 | 106 | num2 = pc.getc(); |
kornvarrel | 0:b5bafad435c8 | 107 | num3 = pc.getc(); |
kornvarrel | 0:b5bafad435c8 | 108 | num4 = pc.getc(); |
kornvarrel | 0:b5bafad435c8 | 109 | |
kornvarrel | 0:b5bafad435c8 | 110 | //printf("%c%c%c%c\n",num1,num2,num3,num4); |
kornvarrel | 0:b5bafad435c8 | 111 | |
kornvarrel | 0:b5bafad435c8 | 112 | if(num1 == '#' && num4 == '$') { |
kornvarrel | 0:b5bafad435c8 | 113 | if(num2 == 'T') { |
kornvarrel | 0:b5bafad435c8 | 114 | if(num3 == 'R') { |
kornvarrel | 0:b5bafad435c8 | 115 | printf("%c%c%c%c\n",num1,num2,num3,num4); |
kornvarrel | 0:b5bafad435c8 | 116 | //wait_ms(20); |
kornvarrel | 0:b5bafad435c8 | 117 | for(j=8; j>=1; j--) { |
kornvarrel | 0:b5bafad435c8 | 118 | for(n=8; n>=1; n--) { |
kornvarrel | 0:b5bafad435c8 | 119 | q = (pow(z,j+1)); |
kornvarrel | 0:b5bafad435c8 | 120 | w = (pow(z,j)); |
kornvarrel | 0:b5bafad435c8 | 121 | i = (pow(z,j+2)); |
kornvarrel | 0:b5bafad435c8 | 122 | k = (pow(z,j-1)); |
kornvarrel | 0:b5bafad435c8 | 123 | maxSin(1,w); |
kornvarrel | 0:b5bafad435c8 | 124 | maxSin(2,k); |
kornvarrel | 0:b5bafad435c8 | 125 | maxSin(3,i); |
kornvarrel | 0:b5bafad435c8 | 126 | maxSin(4,w); |
kornvarrel | 0:b5bafad435c8 | 127 | maxSin(5,i); |
kornvarrel | 0:b5bafad435c8 | 128 | maxSin(6,q); |
kornvarrel | 0:b5bafad435c8 | 129 | maxSin(7,q); |
kornvarrel | 0:b5bafad435c8 | 130 | maxSin(8,k); |
kornvarrel | 0:b5bafad435c8 | 131 | wait_ms(5); |
kornvarrel | 0:b5bafad435c8 | 132 | f++; |
kornvarrel | 0:b5bafad435c8 | 133 | if(f>8) { |
kornvarrel | 0:b5bafad435c8 | 134 | f=0; |
kornvarrel | 0:b5bafad435c8 | 135 | } |
kornvarrel | 0:b5bafad435c8 | 136 | } |
kornvarrel | 0:b5bafad435c8 | 137 | } |
kornvarrel | 0:b5bafad435c8 | 138 | } |
kornvarrel | 0:b5bafad435c8 | 139 | //TURN LEFT |
kornvarrel | 0:b5bafad435c8 | 140 | if(num3 == 'L') { |
kornvarrel | 0:b5bafad435c8 | 141 | printf("%c%c%c%c\n",num1,num2,num3,num4); |
kornvarrel | 0:b5bafad435c8 | 142 | //wait_ms(20); |
kornvarrel | 0:b5bafad435c8 | 143 | for(j=1; j<=8; j++) { |
kornvarrel | 0:b5bafad435c8 | 144 | for(n=1; n<=8; n++) { |
kornvarrel | 0:b5bafad435c8 | 145 | q = (pow(z,j-3)); |
kornvarrel | 0:b5bafad435c8 | 146 | w = (pow(z,j-2)); |
kornvarrel | 0:b5bafad435c8 | 147 | i = (pow(z,j-4)); |
kornvarrel | 0:b5bafad435c8 | 148 | k = (pow(z,j-1)); |
kornvarrel | 0:b5bafad435c8 | 149 | maxSin(1,w); |
kornvarrel | 0:b5bafad435c8 | 150 | maxSin(2,k); |
kornvarrel | 0:b5bafad435c8 | 151 | maxSin(3,i); |
kornvarrel | 0:b5bafad435c8 | 152 | maxSin(4,w); |
kornvarrel | 0:b5bafad435c8 | 153 | maxSin(5,i); |
kornvarrel | 0:b5bafad435c8 | 154 | maxSin(6,q); |
kornvarrel | 0:b5bafad435c8 | 155 | maxSin(7,q); |
kornvarrel | 0:b5bafad435c8 | 156 | maxSin(8,k); |
kornvarrel | 0:b5bafad435c8 | 157 | wait_ms(5); |
kornvarrel | 0:b5bafad435c8 | 158 | f++; |
kornvarrel | 0:b5bafad435c8 | 159 | if(f>8) { |
kornvarrel | 0:b5bafad435c8 | 160 | f=0; |
kornvarrel | 0:b5bafad435c8 | 161 | } |
kornvarrel | 0:b5bafad435c8 | 162 | } |
kornvarrel | 0:b5bafad435c8 | 163 | } |
kornvarrel | 0:b5bafad435c8 | 164 | } |
kornvarrel | 0:b5bafad435c8 | 165 | } |
kornvarrel | 0:b5bafad435c8 | 166 | |
kornvarrel | 0:b5bafad435c8 | 167 | } |
kornvarrel | 0:b5bafad435c8 | 168 | |
kornvarrel | 0:b5bafad435c8 | 169 | if(num2 == 'B' && num3 == 'R') { |
kornvarrel | 0:b5bafad435c8 | 170 | printf("%c%c%c%c\n",num1,num2,num3,num4); |
kornvarrel | 0:b5bafad435c8 | 171 | //wait_ms(20); |
kornvarrel | 0:b5bafad435c8 | 172 | |
kornvarrel | 0:b5bafad435c8 | 173 | maxSin(1,255); |
kornvarrel | 0:b5bafad435c8 | 174 | maxSin(2,255); |
kornvarrel | 0:b5bafad435c8 | 175 | maxSin(3,255); |
kornvarrel | 0:b5bafad435c8 | 176 | maxSin(4,255); |
kornvarrel | 0:b5bafad435c8 | 177 | maxSin(5,255); |
kornvarrel | 0:b5bafad435c8 | 178 | maxSin(6,255); |
kornvarrel | 0:b5bafad435c8 | 179 | maxSin(7,255); |
kornvarrel | 0:b5bafad435c8 | 180 | maxSin(8,255); |
kornvarrel | 0:b5bafad435c8 | 181 | |
kornvarrel | 0:b5bafad435c8 | 182 | } |
kornvarrel | 0:b5bafad435c8 | 183 | |
kornvarrel | 0:b5bafad435c8 | 184 | /*if(num2 == 'E' && num3 == 'M') { |
kornvarrel | 0:b5bafad435c8 | 185 | printf("%c%c%c%c\n",num1,num2,num3,num4); |
kornvarrel | 0:b5bafad435c8 | 186 | //wait_ms(20); |
kornvarrel | 0:b5bafad435c8 | 187 | |
kornvarrel | 0:b5bafad435c8 | 188 | maxSin(1,66); |
kornvarrel | 0:b5bafad435c8 | 189 | maxSin(2,66); |
kornvarrel | 0:b5bafad435c8 | 190 | maxSin(4,36); |
kornvarrel | 0:b5bafad435c8 | 191 | maxSin(6,36); |
kornvarrel | 0:b5bafad435c8 | 192 | maxSin(8,255); |
kornvarrel | 0:b5bafad435c8 | 193 | wait(0.10); |
kornvarrel | 0:b5bafad435c8 | 194 | maxSin(1,0); |
kornvarrel | 0:b5bafad435c8 | 195 | maxSin(2,0); |
kornvarrel | 0:b5bafad435c8 | 196 | maxSin(4,0); |
kornvarrel | 0:b5bafad435c8 | 197 | maxSin(6,0); |
kornvarrel | 0:b5bafad435c8 | 198 | maxSin(8,0); |
kornvarrel | 0:b5bafad435c8 | 199 | wait(0.10); |
kornvarrel | 0:b5bafad435c8 | 200 | }*/ |
kornvarrel | 0:b5bafad435c8 | 201 | |
kornvarrel | 0:b5bafad435c8 | 202 | |
kornvarrel | 0:b5bafad435c8 | 203 | |
kornvarrel | 0:b5bafad435c8 | 204 | //if(num2 == 'M' && num3 == 'D') { |
kornvarrel | 0:b5bafad435c8 | 205 | //printf("%c%c%c%c\n",num1,num2,num3,num4); |
kornvarrel | 0:b5bafad435c8 | 206 | //wait_ms(20); |
kornvarrel | 0:b5bafad435c8 | 207 | else { |
kornvarrel | 0:b5bafad435c8 | 208 | |
kornvarrel | 0:b5bafad435c8 | 209 | //printf("%c%c%c%c\n",num1,num2,num3,num4); |
kornvarrel | 0:b5bafad435c8 | 210 | //wait_ms(20); |
kornvarrel | 0:b5bafad435c8 | 211 | |
kornvarrel | 0:b5bafad435c8 | 212 | maxSin(1,0); |
kornvarrel | 0:b5bafad435c8 | 213 | maxSin(2,0); |
kornvarrel | 0:b5bafad435c8 | 214 | maxSin(3,0); |
kornvarrel | 0:b5bafad435c8 | 215 | maxSin(4,0); |
kornvarrel | 0:b5bafad435c8 | 216 | maxSin(5,0); |
kornvarrel | 0:b5bafad435c8 | 217 | maxSin(6,0); |
kornvarrel | 0:b5bafad435c8 | 218 | maxSin(7,0); |
kornvarrel | 0:b5bafad435c8 | 219 | maxSin(8,0); |
kornvarrel | 0:b5bafad435c8 | 220 | |
kornvarrel | 0:b5bafad435c8 | 221 | |
kornvarrel | 0:b5bafad435c8 | 222 | } |
kornvarrel | 0:b5bafad435c8 | 223 | } |
kornvarrel | 0:b5bafad435c8 | 224 | |
kornvarrel | 0:b5bafad435c8 | 225 | hallState = digi.read(); |
kornvarrel | 0:b5bafad435c8 | 226 | |
kornvarrel | 0:b5bafad435c8 | 227 | if(t.read()<=1) { |
kornvarrel | 0:b5bafad435c8 | 228 | |
kornvarrel | 0:b5bafad435c8 | 229 | if (hallState == 0) { |
kornvarrel | 0:b5bafad435c8 | 230 | // turn LED on: |
kornvarrel | 0:b5bafad435c8 | 231 | led = 1; |
kornvarrel | 0:b5bafad435c8 | 232 | //wait_ms(5); |
kornvarrel | 0:b5bafad435c8 | 233 | //printf("%d\n",hallState); |
kornvarrel | 0:b5bafad435c8 | 234 | //wait(0.5); |
kornvarrel | 0:b5bafad435c8 | 235 | count++; |
kornvarrel | 0:b5bafad435c8 | 236 | ++dis; |
kornvarrel | 0:b5bafad435c8 | 237 | //pc.printf("%d\n",count); |
kornvarrel | 0:b5bafad435c8 | 238 | //wait_ms(50); |
kornvarrel | 0:b5bafad435c8 | 239 | |
kornvarrel | 0:b5bafad435c8 | 240 | } else { |
kornvarrel | 0:b5bafad435c8 | 241 | // turn LED off: |
kornvarrel | 0:b5bafad435c8 | 242 | led = 0; |
kornvarrel | 0:b5bafad435c8 | 243 | //wait_ms(5); |
kornvarrel | 0:b5bafad435c8 | 244 | //printf("%d\n",hallState); |
kornvarrel | 0:b5bafad435c8 | 245 | //wait(0.5); |
kornvarrel | 0:b5bafad435c8 | 246 | } |
kornvarrel | 0:b5bafad435c8 | 247 | } else { |
kornvarrel | 0:b5bafad435c8 | 248 | |
kornvarrel | 0:b5bafad435c8 | 249 | di = (dis*rad); |
kornvarrel | 0:b5bafad435c8 | 250 | |
kornvarrel | 0:b5bafad435c8 | 251 | speed = count*rad; |
kornvarrel | 0:b5bafad435c8 | 252 | speed = speed*(3.6); |
kornvarrel | 0:b5bafad435c8 | 253 | //speed = 79.2; |
kornvarrel | 0:b5bafad435c8 | 254 | |
kornvarrel | 0:b5bafad435c8 | 255 | //printf("%d\n",speed); |
kornvarrel | 0:b5bafad435c8 | 256 | //pc.printf("#S%02d$",speed); |
kornvarrel | 0:b5bafad435c8 | 257 | |
kornvarrel | 0:b5bafad435c8 | 258 | long distance = sensor.distance(); |
kornvarrel | 0:b5bafad435c8 | 259 | |
kornvarrel | 0:b5bafad435c8 | 260 | pc.printf("#"); |
kornvarrel | 0:b5bafad435c8 | 261 | //printf("#"); |
kornvarrel | 0:b5bafad435c8 | 262 | if(distance >= 570 && distance < 620) { |
kornvarrel | 0:b5bafad435c8 | 263 | pc.printf("#D00S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 264 | //printf("#D00$"); |
kornvarrel | 0:b5bafad435c8 | 265 | //printf("#D00S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 266 | } else if(distance >= 520 && distance < 570) { |
kornvarrel | 0:b5bafad435c8 | 267 | pc.printf("#D01S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 268 | //printf("#D01$"); |
kornvarrel | 0:b5bafad435c8 | 269 | //printf("#D01S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 270 | } else if(distance >= 470 && distance < 520) { |
kornvarrel | 0:b5bafad435c8 | 271 | pc.printf("#D02S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 272 | //printf("#D02$"); |
kornvarrel | 0:b5bafad435c8 | 273 | //printf("#D02S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 274 | } else if(distance >= 420 && distance < 470) { |
kornvarrel | 0:b5bafad435c8 | 275 | pc.printf("#D03S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 276 | //printf("#D03$"); |
kornvarrel | 0:b5bafad435c8 | 277 | printf("#D03S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 278 | } else if(distance >= 370 && distance < 420) { |
kornvarrel | 0:b5bafad435c8 | 279 | pc.printf("#D04S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 280 | //printf("#D04$"); |
kornvarrel | 0:b5bafad435c8 | 281 | //printf("#D04S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 282 | } else if(distance >= 220 && distance < 370) { |
kornvarrel | 0:b5bafad435c8 | 283 | pc.printf("#D05S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 284 | //printf("#D05$"); |
kornvarrel | 0:b5bafad435c8 | 285 | //printf("#D05S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 286 | } else if(distance >= 170 && distance < 220) { |
kornvarrel | 0:b5bafad435c8 | 287 | pc.printf("#D06S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 288 | //printf("#D06$"); |
kornvarrel | 0:b5bafad435c8 | 289 | //printf("#D06S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 290 | } else if(distance >= 120 && distance < 170) { |
kornvarrel | 0:b5bafad435c8 | 291 | pc.printf("#D07S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 292 | //printf("#D07$"); |
kornvarrel | 0:b5bafad435c8 | 293 | //printf("#D07S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 294 | } else if(distance >= 70 && distance < 120 ) { |
kornvarrel | 0:b5bafad435c8 | 295 | pc.printf("#D08S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 296 | //printf("#D08$"); |
kornvarrel | 0:b5bafad435c8 | 297 | //printf("#D08S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 298 | } else if(distance <= 70) { |
kornvarrel | 0:b5bafad435c8 | 299 | pc.printf("#D09S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 300 | //printf("#D09S%02dK%03d$",speed,di); |
kornvarrel | 0:b5bafad435c8 | 301 | //printf("#D09$"); |
kornvarrel | 0:b5bafad435c8 | 302 | } //else { |
kornvarrel | 0:b5bafad435c8 | 303 | // pc.printf("%d cm",distance); |
kornvarrel | 0:b5bafad435c8 | 304 | //} |
kornvarrel | 0:b5bafad435c8 | 305 | |
kornvarrel | 0:b5bafad435c8 | 306 | count = 0; |
kornvarrel | 0:b5bafad435c8 | 307 | t.reset(); |
kornvarrel | 0:b5bafad435c8 | 308 | } |
kornvarrel | 0:b5bafad435c8 | 309 | } |
kornvarrel | 0:b5bafad435c8 | 310 | } |
kornvarrel | 0:b5bafad435c8 | 311 | |
kornvarrel | 0:b5bafad435c8 | 312 | int main() |
kornvarrel | 0:b5bafad435c8 | 313 | { |
kornvarrel | 0:b5bafad435c8 | 314 | Set(); |
kornvarrel | 0:b5bafad435c8 | 315 | loop(); |
kornvarrel | 0:b5bafad435c8 | 316 | } |