Test
Dependencies: SRF02 SRF05 mbed
Fork of Prosjekt_2 by
main.cpp@24:06d3a7b85602, 2016-04-30 (annotated)
- Committer:
- leonardene
- Date:
- Sat Apr 30 10:58:53 2016 +0000
- Revision:
- 24:06d3a7b85602
- Parent:
- 23:366bfbc66e51
- Child:
- 25:a35c991995ce
Endringer.; med Y og N til ? styre hele greia.; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
leonardene | 0:bf27900bdc22 | 1 | #include "mbed.h" |
leonardene | 1:f3c6c8f735a5 | 2 | #include "SRF05.h" |
leonardene | 7:112b745b2b9e | 3 | #include "SRF02.h" |
leonardene | 1:f3c6c8f735a5 | 4 | |
leonardene | 1:f3c6c8f735a5 | 5 | Serial pc(USBTX, USBRX); |
leonardene | 1:f3c6c8f735a5 | 6 | Serial bt(p13, p14); |
leonardene | 0:bf27900bdc22 | 7 | |
leonardene | 1:f3c6c8f735a5 | 8 | DigitalOut led (LED1); |
leonardene | 7:112b745b2b9e | 9 | SRF05 srf05(p7, p8); |
leonardene | 8:63f1033aaea2 | 10 | SRF02 srf02(p9, p10); |
leonardene | 1:f3c6c8f735a5 | 11 | |
leonardene | 4:690abfca1f4d | 12 | char input(); |
leonardene | 4:690abfca1f4d | 13 | void avstand(); |
leonardene | 1:f3c6c8f735a5 | 14 | void detektor(float); |
leonardene | 12:4cefe940afbc | 15 | void hastighet(); |
leonardene | 17:5d08825028b0 | 16 | void triangulering(); |
leonardene | 17:5d08825028b0 | 17 | |
leonardene | 1:f3c6c8f735a5 | 18 | int main() |
leonardene | 1:f3c6c8f735a5 | 19 | { |
leonardene | 1:f3c6c8f735a5 | 20 | float y; |
leonardene | 0:bf27900bdc22 | 21 | while(1) { |
leonardene | 11:cc2d82380198 | 22 | char kom1 [] = {"\n\nMenu\n1)\tMeassure Distance\n2)\tMotion Detection\n3)\tMeassure Velocity\n4)\tPlacement\n"}; |
leonardene | 9:4b38e90b8f10 | 23 | pc.printf("%s", kom1); |
leonardene | 9:4b38e90b8f10 | 24 | bt.printf("%s", kom1); |
leonardene | 4:690abfca1f4d | 25 | while (pc.readable() == 0 && bt.readable() == 0) {} |
leonardene | 4:690abfca1f4d | 26 | switch (input()) { |
leonardene | 1:f3c6c8f735a5 | 27 | case('1'): { |
leonardene | 23:366bfbc66e51 | 28 | char kom2 [] = {"\nPress 'y' to meassure distance\nPress 'n' to return to the menu\nThe bargraph will light up according to how close the object is\n1 LED equals 35cm\n"}; |
leonardene | 9:4b38e90b8f10 | 29 | pc.printf("%s", kom2); |
leonardene | 9:4b38e90b8f10 | 30 | bt.printf("%s", kom2); |
leonardene | 4:690abfca1f4d | 31 | avstand(); |
leonardene | 1:f3c6c8f735a5 | 32 | break; |
leonardene | 1:f3c6c8f735a5 | 33 | } |
leonardene | 1:f3c6c8f735a5 | 34 | case('2'): { |
leonardene | 9:4b38e90b8f10 | 35 | char kom3 [] = {"\nLights will flash when a motion is detected\nPress 'n' to return to the menu\n"}; |
leonardene | 9:4b38e90b8f10 | 36 | pc.printf("%s", kom3); |
leonardene | 9:4b38e90b8f10 | 37 | bt.printf("%s", kom3); |
leonardene | 7:112b745b2b9e | 38 | y = srf05.read(); |
leonardene | 2:d8543bd496a0 | 39 | detektor(y); |
leonardene | 1:f3c6c8f735a5 | 40 | break; |
leonardene | 1:f3c6c8f735a5 | 41 | } |
leonardene | 1:f3c6c8f735a5 | 42 | case('3'): { |
leonardene | 11:cc2d82380198 | 43 | char kom4 [] = {"\nThis feature meassures the velocity of any movement towards Ultra-Sonic Ranger SRF05\nPress 'n' to return to the menu\n"}; |
leonardene | 9:4b38e90b8f10 | 44 | pc.printf("%s", kom4); |
leonardene | 9:4b38e90b8f10 | 45 | bt.printf("%s", kom4); |
leonardene | 12:4cefe940afbc | 46 | hastighet(); |
leonardene | 1:f3c6c8f735a5 | 47 | break; |
leonardene | 2:d8543bd496a0 | 48 | } |
leonardene | 7:112b745b2b9e | 49 | case('4'): { |
leonardene | 23:366bfbc66e51 | 50 | char kom5 [] = {"\nThis feature returns the positon of an object in a 2D-table\nEach 'O' represents 10square centimeters and 'X' represents the detected object\nPlace componentes according to the datasheet\n"}; |
leonardene | 9:4b38e90b8f10 | 51 | pc.printf("%s", kom5); |
leonardene | 9:4b38e90b8f10 | 52 | bt.printf("%s", kom5); |
leonardene | 14:215006ed25d8 | 53 | triangulering(); |
leonardene | 14:215006ed25d8 | 54 | break; |
leonardene | 22:0606a03a5b6a | 55 | } |
leonardene | 11:cc2d82380198 | 56 | default: |
leonardene | 11:cc2d82380198 | 57 | char kom6 [] = {"\nERROR: Invalid value\n"}; |
leonardene | 11:cc2d82380198 | 58 | pc.printf("%s", kom6); |
leonardene | 11:cc2d82380198 | 59 | bt.printf("%s", kom6); |
leonardene | 11:cc2d82380198 | 60 | } |
leonardene | 11:cc2d82380198 | 61 | } |
leonardene | 9:4b38e90b8f10 | 62 | } |
leonardene | 5:7209741a08bc | 63 | |
leonardene | 5:7209741a08bc | 64 | /* Char Input() er en funksjon som avgjør om input kommer fra bluetooth eller pc. Deretter returnerer den input verdien. */ |
leonardene | 5:7209741a08bc | 65 | |
leonardene | 4:690abfca1f4d | 66 | char input() |
leonardene | 4:690abfca1f4d | 67 | { |
leonardene | 4:690abfca1f4d | 68 | if(pc.readable()) |
leonardene | 4:690abfca1f4d | 69 | return getchar(); |
leonardene | 4:690abfca1f4d | 70 | else if (bt.readable()) |
leonardene | 4:690abfca1f4d | 71 | return bt.getc(); |
leonardene | 4:690abfca1f4d | 72 | else |
leonardene | 4:690abfca1f4d | 73 | return 1; |
leonardene | 0:bf27900bdc22 | 74 | } |
leonardene | 1:f3c6c8f735a5 | 75 | |
leonardene | 5:7209741a08bc | 76 | /* Void avstand() er en funksjon som måler avstand til en gjenstand innenfor rekkevidden til ultralyd sensoren. Avstanden sendes til pc & bluetooth i cm. */ |
leonardene | 5:7209741a08bc | 77 | |
leonardene | 4:690abfca1f4d | 78 | void avstand() |
leonardene | 4:690abfca1f4d | 79 | { |
leonardene | 9:4b38e90b8f10 | 80 | DigitalOut bar[] = {NC, p21, p22, p23, p24, p25, p26, p27, p28, p29, p30}; |
leonardene | 4:690abfca1f4d | 81 | char a; |
leonardene | 14:215006ed25d8 | 82 | float z, y; |
leonardene | 4:690abfca1f4d | 83 | int n = 0; |
leonardene | 1:f3c6c8f735a5 | 84 | |
leonardene | 9:4b38e90b8f10 | 85 | while (a != 'n') { |
leonardene | 4:690abfca1f4d | 86 | while (pc.readable() == 0 && bt.readable() == 0) {} |
leonardene | 4:690abfca1f4d | 87 | a = input(); |
leonardene | 4:690abfca1f4d | 88 | if (a == 'y') { |
leonardene | 7:112b745b2b9e | 89 | y = srf05.read(); |
leonardene | 5:7209741a08bc | 90 | if (y > 350) { |
leonardene | 9:4b38e90b8f10 | 91 | char kom1 [] = {"\nDetection area is exceeded (350cm)\n"}; |
leonardene | 9:4b38e90b8f10 | 92 | pc.printf("%s", kom1); |
leonardene | 9:4b38e90b8f10 | 93 | bt.printf("%s", kom1); |
leonardene | 9:4b38e90b8f10 | 94 | y = z; |
leonardene | 5:7209741a08bc | 95 | } else { |
leonardene | 9:4b38e90b8f10 | 96 | z = y; |
leonardene | 5:7209741a08bc | 97 | pc.printf("\nMeassurement (%d):\t%.2fcm\n",n, y); |
leonardene | 5:7209741a08bc | 98 | bt.printf("\nMeassurement (%d):\t%.2fcm\n",n, y); |
leonardene | 4:690abfca1f4d | 99 | } |
leonardene | 5:7209741a08bc | 100 | for(int x = 1; x <= 10; x++) { |
leonardene | 9:4b38e90b8f10 | 101 | (x*35 < y) ? bar[x] = 0 : bar[x] = 1; |
leonardene | 5:7209741a08bc | 102 | } |
leonardene | 4:690abfca1f4d | 103 | n++; |
leonardene | 4:690abfca1f4d | 104 | wait_ms (50); |
leonardene | 9:4b38e90b8f10 | 105 | } else if (a != 'n') { |
leonardene | 9:4b38e90b8f10 | 106 | char kom3 [] = {"\nInvalid input\n"}; |
leonardene | 9:4b38e90b8f10 | 107 | pc.printf("%s", kom3); |
leonardene | 9:4b38e90b8f10 | 108 | bt.printf("%s", kom3); |
leonardene | 6:5fee13e621e0 | 109 | } |
leonardene | 1:f3c6c8f735a5 | 110 | } |
leonardene | 4:690abfca1f4d | 111 | for(int x = 1; x <= 10; x++) |
leonardene | 4:690abfca1f4d | 112 | bar[x] = 0; |
leonardene | 1:f3c6c8f735a5 | 113 | } |
leonardene | 1:f3c6c8f735a5 | 114 | |
leonardene | 5:7209741a08bc | 115 | /* Funksjonen Void detektor() er en funksjon som detekterer bevegelse. LED blinker ved deteksjon. */ |
leonardene | 5:7209741a08bc | 116 | |
leonardene | 1:f3c6c8f735a5 | 117 | void detektor(float y) |
leonardene | 1:f3c6c8f735a5 | 118 | { |
leonardene | 4:690abfca1f4d | 119 | int b = 0; |
leonardene | 9:4b38e90b8f10 | 120 | while (input() != 'n') { |
leonardene | 7:112b745b2b9e | 121 | if (y+2 < srf05.read() || y-2 > srf05.read()) { |
leonardene | 4:690abfca1f4d | 122 | led = !led; |
leonardene | 4:690abfca1f4d | 123 | if (b == 0) { |
leonardene | 9:4b38e90b8f10 | 124 | char kom1 [] = {"\nMotion Detected\n"}; |
leonardene | 9:4b38e90b8f10 | 125 | pc.printf("%s", kom1); |
leonardene | 9:4b38e90b8f10 | 126 | bt.printf("%s", kom1); |
leonardene | 9:4b38e90b8f10 | 127 | |
leonardene | 4:690abfca1f4d | 128 | b = 1; |
leonardene | 4:690abfca1f4d | 129 | } |
leonardene | 17:5d08825028b0 | 130 | } else |
leonardene | 15:324186d4ea61 | 131 | b = led = 0; |
leonardene | 2:d8543bd496a0 | 132 | wait_ms (50); |
leonardene | 2:d8543bd496a0 | 133 | } |
leonardene | 1:f3c6c8f735a5 | 134 | } |
leonardene | 1:f3c6c8f735a5 | 135 | |
leonardene | 5:7209741a08bc | 136 | /* Funksjonen Void Hastighet() måler hastigheten til et objekt foran ultralyd sensoren. Hastigheten skrives til pc & bluetooth i km/t. */ |
leonardene | 5:7209741a08bc | 137 | |
leonardene | 12:4cefe940afbc | 138 | void hastighet () |
leonardene | 1:f3c6c8f735a5 | 139 | { |
leonardene | 3:1eb0ad70f562 | 140 | Timer t; |
leonardene | 11:cc2d82380198 | 141 | float start, stopp, fart; |
leonardene | 9:4b38e90b8f10 | 142 | while(input() != 'n') { |
leonardene | 4:690abfca1f4d | 143 | t.start(); |
leonardene | 7:112b745b2b9e | 144 | start = srf05.read(); |
leonardene | 11:cc2d82380198 | 145 | while (t.read() < 0.25 && srf05.read() > 10) {} |
leonardene | 4:690abfca1f4d | 146 | t.stop(); |
leonardene | 7:112b745b2b9e | 147 | stopp = srf05.read(); |
leonardene | 11:cc2d82380198 | 148 | fart = (start - stopp)*(3.6/(t.read()*100)); |
leonardene | 11:cc2d82380198 | 149 | if (stopp < 10) { |
leonardene | 11:cc2d82380198 | 150 | char kom1 [] = {"\nERROR: Closer than 10cm from the Ultra Sonic Ranger."}; |
leonardene | 11:cc2d82380198 | 151 | pc.printf("%s", kom1); |
leonardene | 11:cc2d82380198 | 152 | bt.printf("%s", kom1); |
leonardene | 14:215006ed25d8 | 153 | wait_ms (250); |
leonardene | 11:cc2d82380198 | 154 | } |
leonardene | 11:cc2d82380198 | 155 | if (0.5 < fart && start < 350 && stopp < 350) { |
leonardene | 11:cc2d82380198 | 156 | pc.printf("\nVelocity is = %0.2f km/t", fart); |
leonardene | 11:cc2d82380198 | 157 | bt.printf("\nVelocity is = %0.2f km/t", fart); |
leonardene | 6:5fee13e621e0 | 158 | } |
leonardene | 4:690abfca1f4d | 159 | t.reset(); |
leonardene | 1:f3c6c8f735a5 | 160 | } |
leonardene | 7:112b745b2b9e | 161 | } |
leonardene | 7:112b745b2b9e | 162 | |
leonardene | 9:4b38e90b8f10 | 163 | /*Funksjonen plasseringiplan måler avstanden til et objekt i målesonen, fra to forskjellige vinkler. Avstanden til objektet plasseres i et 2D-plan. Størrelsen på planet er 100cm langs x akse og 50cm langs y akse.*/ |
leonardene | 20:c334179dfbf8 | 164 | |
leonardene | 17:5d08825028b0 | 165 | void triangulering () |
leonardene | 22:0606a03a5b6a | 166 | { |
leonardene | 17:5d08825028b0 | 167 | double vinkel_b, vinkel_a, vinkel_x, vinkel_y, y_akse, x_akse, hyp, PI; |
leonardene | 24:06d3a7b85602 | 168 | int a, b, c, n, m; |
leonardene | 17:5d08825028b0 | 169 | float d; |
leonardene | 17:5d08825028b0 | 170 | PI = acos(-1.0); |
leonardene | 24:06d3a7b85602 | 171 | hyp = 305.0; |
leonardene | 24:06d3a7b85602 | 172 | char h; |
leonardene | 24:06d3a7b85602 | 173 | while (h != 'n') { |
leonardene | 24:06d3a7b85602 | 174 | while (pc.readable() == 0 && bt.readable() == 0) {} |
leonardene | 24:06d3a7b85602 | 175 | h = input(); |
leonardene | 24:06d3a7b85602 | 176 | if (h == 'y') { |
leonardene | 24:06d3a7b85602 | 177 | char _2DplanX[12] = {'|','O','O','O','O','O','O','O','O','O','O','|'}; |
leonardene | 24:06d3a7b85602 | 178 | char _2Dplan[12] = {'|','O','O','O','O','O','O','O','O','O','O','|'}; |
leonardene | 24:06d3a7b85602 | 179 | d = 0.0; |
leonardene | 24:06d3a7b85602 | 180 | b = n = m = 0; |
leonardene | 24:06d3a7b85602 | 181 | while (d < 180.0 || d > 280.0 && n < 10) { |
leonardene | 24:06d3a7b85602 | 182 | d = srf05.read(); |
leonardene | 24:06d3a7b85602 | 183 | n++; |
leonardene | 24:06d3a7b85602 | 184 | wait_ms (100); |
leonardene | 24:06d3a7b85602 | 185 | } |
leonardene | 24:06d3a7b85602 | 186 | while (b < 150 || b > 250 && m < 10) { |
leonardene | 24:06d3a7b85602 | 187 | b = srf02.getDistanceCm(); |
leonardene | 24:06d3a7b85602 | 188 | wait_ms (250); |
leonardene | 24:06d3a7b85602 | 189 | m++; |
leonardene | 24:06d3a7b85602 | 190 | } |
leonardene | 24:06d3a7b85602 | 191 | if (n == 10 || m == 10) { |
leonardene | 24:06d3a7b85602 | 192 | char kom1 []= {"\nERROR: No object detected within the bouandaries\n"}; |
leonardene | 24:06d3a7b85602 | 193 | pc.printf("%s", kom1); |
leonardene | 24:06d3a7b85602 | 194 | bt.printf("%s", kom1); |
leonardene | 24:06d3a7b85602 | 195 | } else { |
leonardene | 24:06d3a7b85602 | 196 | vinkel_b = acos((hyp*hyp+d*d-b*b)/(2.0*hyp*d)); |
leonardene | 24:06d3a7b85602 | 197 | vinkel_a = acos((hyp*hyp+b*b-d*d)/(2.0*hyp*b)); |
leonardene | 24:06d3a7b85602 | 198 | vinkel_y = PI-0.8076167287-vinkel_b; |
leonardene | 24:06d3a7b85602 | 199 | if( vinkel_y > PI/2.0) |
leonardene | 24:06d3a7b85602 | 200 | vinkel_y = 0.8076167287+vinkel_b; |
leonardene | 24:06d3a7b85602 | 201 | y_akse = sin(vinkel_y)*d; |
leonardene | 24:06d3a7b85602 | 202 | vinkel_x = PI-0.7631795981-vinkel_a; |
leonardene | 24:06d3a7b85602 | 203 | if( vinkel_x > PI/2.0) |
leonardene | 24:06d3a7b85602 | 204 | vinkel_x = 0.7631795981+vinkel_a; |
leonardene | 24:06d3a7b85602 | 205 | x_akse = sin(vinkel_x)*b; |
leonardene | 24:06d3a7b85602 | 206 | for (c = 1; c < 11; c++) { |
leonardene | 24:06d3a7b85602 | 207 | if (((c-1)*10+150) <= x_akse && x_akse < (c*10+150)) |
leonardene | 24:06d3a7b85602 | 208 | _2DplanX[c] = 'X'; |
leonardene | 24:06d3a7b85602 | 209 | else |
leonardene | 24:06d3a7b85602 | 210 | _2DplanX[c] = 'O'; |
leonardene | 22:0606a03a5b6a | 211 | } |
leonardene | 24:06d3a7b85602 | 212 | for (c = 280; c >= 190; c=c-10) { |
leonardene | 24:06d3a7b85602 | 213 | if (c-10 <= y_akse && y_akse < c) { |
leonardene | 24:06d3a7b85602 | 214 | for(a = 0; a < 12; a++) { |
leonardene | 24:06d3a7b85602 | 215 | pc.printf("%c",_2DplanX[a]); |
leonardene | 24:06d3a7b85602 | 216 | bt.printf("%c",_2DplanX[a]); |
leonardene | 24:06d3a7b85602 | 217 | } |
leonardene | 24:06d3a7b85602 | 218 | } else { |
leonardene | 24:06d3a7b85602 | 219 | for(a = 0; a < 12; a++) { |
leonardene | 24:06d3a7b85602 | 220 | pc.printf("%c",_2Dplan[a]); |
leonardene | 24:06d3a7b85602 | 221 | bt.printf("%c",_2Dplan[a]); |
leonardene | 24:06d3a7b85602 | 222 | } |
leonardene | 24:06d3a7b85602 | 223 | } |
leonardene | 24:06d3a7b85602 | 224 | char kom2 []= {"\n"}; |
leonardene | 24:06d3a7b85602 | 225 | pc.printf("%s", kom2); |
leonardene | 24:06d3a7b85602 | 226 | bt.printf("%s", kom2); |
leonardene | 22:0606a03a5b6a | 227 | } |
leonardene | 22:0606a03a5b6a | 228 | } |
leonardene | 24:06d3a7b85602 | 229 | } else if (h != 'n') { |
leonardene | 24:06d3a7b85602 | 230 | char kom3 [] = {"\nInvalid input\n"}; |
leonardene | 24:06d3a7b85602 | 231 | pc.printf("%s", kom3); |
leonardene | 24:06d3a7b85602 | 232 | bt.printf("%s", kom3); |
leonardene | 22:0606a03a5b6a | 233 | } |
leonardene | 17:5d08825028b0 | 234 | } |
leonardene | 20:c334179dfbf8 | 235 | } |