Leo Bach / Mbed 2 deprecated Prosjekt_2

Test

Dependencies:   SRF02 SRF05 mbed

Fork of Prosjekt_2 by HIOF Programmering

main.cpp@19:f57c238efcd8, 2016-04-29 (annotated)

Committer:
leonardene
Date:
Fri Apr 29 17:40:27 2016 +0000
Revision:
19:f57c238efcd8
Parent:
18:4f19ae59bf07
Child:
20:c334179dfbf8
en

Who changed what in which revision?

UserRevisionLine numberNew 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 0:bf27900bdc22 18
leonardene 18:4f19ae59bf07 19
leonardene 1:f3c6c8f735a5 20 int main()
leonardene 1:f3c6c8f735a5 21 {
leonardene 1:f3c6c8f735a5 22 float y;
leonardene 0:bf27900bdc22 23 while(1) {
leonardene 11:cc2d82380198 24 char kom1 [] = {"\n\nMenu\n1)\tMeassure Distance\n2)\tMotion Detection\n3)\tMeassure Velocity\n4)\tPlacement\n"};
leonardene 9:4b38e90b8f10 25 pc.printf("%s", kom1);
leonardene 9:4b38e90b8f10 26 bt.printf("%s", kom1);
leonardene 4:690abfca1f4d 27 while (pc.readable() == 0 && bt.readable() == 0) {}
leonardene 4:690abfca1f4d 28 switch (input()) {
leonardene 1:f3c6c8f735a5 29 case('1'): {
leonardene 10:e83df36b417a 30 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 40cm\n"};
leonardene 9:4b38e90b8f10 31 pc.printf("%s", kom2);
leonardene 9:4b38e90b8f10 32 bt.printf("%s", kom2);
leonardene 4:690abfca1f4d 33 avstand();
leonardene 1:f3c6c8f735a5 34 break;
leonardene 1:f3c6c8f735a5 35 }
leonardene 1:f3c6c8f735a5 36 case('2'): {
leonardene 9:4b38e90b8f10 37 char kom3 [] = {"\nLights will flash when a motion is detected\nPress 'n' to return to the menu\n"};
leonardene 9:4b38e90b8f10 38 pc.printf("%s", kom3);
leonardene 9:4b38e90b8f10 39 bt.printf("%s", kom3);
leonardene 7:112b745b2b9e 40 y = srf05.read();
leonardene 2:d8543bd496a0 41 detektor(y);
leonardene 1:f3c6c8f735a5 42 break;
leonardene 1:f3c6c8f735a5 43 }
leonardene 1:f3c6c8f735a5 44 case('3'): {
leonardene 11:cc2d82380198 45 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 46 pc.printf("%s", kom4);
leonardene 9:4b38e90b8f10 47 bt.printf("%s", kom4);
leonardene 12:4cefe940afbc 48 hastighet();
leonardene 1:f3c6c8f735a5 49 break;
leonardene 2:d8543bd496a0 50 }
leonardene 7:112b745b2b9e 51 case('4'): {
leonardene 17:5d08825028b0 52 char kom5 [] = {"\nThis feature returns the positon of an object in a 2D-table\nEach 'O' represents 10square centimeters.\nPlace componentes according to the datasheet\n"};
leonardene 9:4b38e90b8f10 53 pc.printf("%s", kom5);
leonardene 9:4b38e90b8f10 54 bt.printf("%s", kom5);
leonardene 14:215006ed25d8 55 triangulering();
leonardene 14:215006ed25d8 56 break;
leonardene 17:5d08825028b0 57 }
leonardene 11:cc2d82380198 58 default:
leonardene 11:cc2d82380198 59 char kom6 [] = {"\nERROR: Invalid value\n"};
leonardene 11:cc2d82380198 60 pc.printf("%s", kom6);
leonardene 11:cc2d82380198 61 bt.printf("%s", kom6);
leonardene 11:cc2d82380198 62 }
leonardene 11:cc2d82380198 63 }
leonardene 9:4b38e90b8f10 64 }
leonardene 5:7209741a08bc 65
leonardene 5:7209741a08bc 66 /* Char Input() er en funksjon som avgjør om input kommer fra bluetooth eller pc. Deretter returnerer den input verdien. */
leonardene 5:7209741a08bc 67
leonardene 4:690abfca1f4d 68 char input()
leonardene 4:690abfca1f4d 69 {
leonardene 4:690abfca1f4d 70 if(pc.readable())
leonardene 4:690abfca1f4d 71 return getchar();
leonardene 4:690abfca1f4d 72 else if (bt.readable())
leonardene 4:690abfca1f4d 73 return bt.getc();
leonardene 4:690abfca1f4d 74 else
leonardene 4:690abfca1f4d 75 return 1;
leonardene 0:bf27900bdc22 76 }
leonardene 1:f3c6c8f735a5 77
leonardene 5:7209741a08bc 78 /* 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 79
leonardene 4:690abfca1f4d 80 void avstand()
leonardene 4:690abfca1f4d 81 {
leonardene 9:4b38e90b8f10 82 DigitalOut bar[] = {NC, p21, p22, p23, p24, p25, p26, p27, p28, p29, p30};
leonardene 4:690abfca1f4d 83 char a;
leonardene 14:215006ed25d8 84 float z, y;
leonardene 4:690abfca1f4d 85 int n = 0;
leonardene 1:f3c6c8f735a5 86
leonardene 9:4b38e90b8f10 87 while (a != 'n') {
leonardene 4:690abfca1f4d 88 while (pc.readable() == 0 && bt.readable() == 0) {}
leonardene 4:690abfca1f4d 89 a = input();
leonardene 4:690abfca1f4d 90 if (a == 'y') {
leonardene 7:112b745b2b9e 91 y = srf05.read();
leonardene 5:7209741a08bc 92 if (y > 350) {
leonardene 9:4b38e90b8f10 93 char kom1 [] = {"\nDetection area is exceeded (350cm)\n"};
leonardene 9:4b38e90b8f10 94 pc.printf("%s", kom1);
leonardene 9:4b38e90b8f10 95 bt.printf("%s", kom1);
leonardene 9:4b38e90b8f10 96 y = z;
leonardene 5:7209741a08bc 97 } else {
leonardene 9:4b38e90b8f10 98 z = y;
leonardene 5:7209741a08bc 99 pc.printf("\nMeassurement (%d):\t%.2fcm\n",n, y);
leonardene 5:7209741a08bc 100 bt.printf("\nMeassurement (%d):\t%.2fcm\n",n, y);
leonardene 4:690abfca1f4d 101 }
leonardene 5:7209741a08bc 102 for(int x = 1; x <= 10; x++) {
leonardene 9:4b38e90b8f10 103 (x*35 < y) ? bar[x] = 0 : bar[x] = 1;
leonardene 5:7209741a08bc 104 }
leonardene 4:690abfca1f4d 105 n++;
leonardene 4:690abfca1f4d 106 wait_ms (50);
leonardene 9:4b38e90b8f10 107 } else if (a != 'n') {
leonardene 9:4b38e90b8f10 108 char kom3 [] = {"\nInvalid input\n"};
leonardene 9:4b38e90b8f10 109 pc.printf("%s", kom3);
leonardene 9:4b38e90b8f10 110 bt.printf("%s", kom3);
leonardene 6:5fee13e621e0 111 }
leonardene 1:f3c6c8f735a5 112 }
leonardene 4:690abfca1f4d 113 for(int x = 1; x <= 10; x++)
leonardene 4:690abfca1f4d 114 bar[x] = 0;
leonardene 1:f3c6c8f735a5 115 }
leonardene 1:f3c6c8f735a5 116
leonardene 5:7209741a08bc 117 /* Funksjonen Void detektor() er en funksjon som detekterer bevegelse. LED blinker ved deteksjon. */
leonardene 5:7209741a08bc 118
leonardene 1:f3c6c8f735a5 119 void detektor(float y)
leonardene 1:f3c6c8f735a5 120 {
leonardene 4:690abfca1f4d 121 int b = 0;
leonardene 9:4b38e90b8f10 122 while (input() != 'n') {
leonardene 7:112b745b2b9e 123 if (y+2 < srf05.read() || y-2 > srf05.read()) {
leonardene 4:690abfca1f4d 124 led = !led;
leonardene 4:690abfca1f4d 125 if (b == 0) {
leonardene 9:4b38e90b8f10 126 char kom1 [] = {"\nMotion Detected\n"};
leonardene 9:4b38e90b8f10 127 pc.printf("%s", kom1);
leonardene 9:4b38e90b8f10 128 bt.printf("%s", kom1);
leonardene 9:4b38e90b8f10 129
leonardene 4:690abfca1f4d 130 b = 1;
leonardene 4:690abfca1f4d 131 }
leonardene 17:5d08825028b0 132 } else
leonardene 15:324186d4ea61 133 b = led = 0;
leonardene 2:d8543bd496a0 134 wait_ms (50);
leonardene 2:d8543bd496a0 135 }
leonardene 1:f3c6c8f735a5 136 }
leonardene 1:f3c6c8f735a5 137
leonardene 5:7209741a08bc 138 /* Funksjonen Void Hastighet() måler hastigheten til et objekt foran ultralyd sensoren. Hastigheten skrives til pc & bluetooth i km/t. */
leonardene 5:7209741a08bc 139
leonardene 12:4cefe940afbc 140 void hastighet ()
leonardene 1:f3c6c8f735a5 141 {
leonardene 3:1eb0ad70f562 142 Timer t;
leonardene 11:cc2d82380198 143 float start, stopp, fart;
leonardene 9:4b38e90b8f10 144 while(input() != 'n') {
leonardene 4:690abfca1f4d 145 t.start();
leonardene 7:112b745b2b9e 146 start = srf05.read();
leonardene 11:cc2d82380198 147 while (t.read() < 0.25 && srf05.read() > 10) {}
leonardene 4:690abfca1f4d 148 t.stop();
leonardene 7:112b745b2b9e 149 stopp = srf05.read();
leonardene 11:cc2d82380198 150 fart = (start - stopp)*(3.6/(t.read()*100));
leonardene 11:cc2d82380198 151 if (stopp < 10) {
leonardene 11:cc2d82380198 152 char kom1 [] = {"\nERROR: Closer than 10cm from the Ultra Sonic Ranger."};
leonardene 11:cc2d82380198 153 pc.printf("%s", kom1);
leonardene 11:cc2d82380198 154 bt.printf("%s", kom1);
leonardene 14:215006ed25d8 155 wait_ms (250);
leonardene 11:cc2d82380198 156 }
leonardene 11:cc2d82380198 157 if (0.5 < fart && start < 350 && stopp < 350) {
leonardene 11:cc2d82380198 158 pc.printf("\nVelocity is = %0.2f km/t", fart);
leonardene 11:cc2d82380198 159 bt.printf("\nVelocity is = %0.2f km/t", fart);
leonardene 6:5fee13e621e0 160 }
leonardene 4:690abfca1f4d 161 t.reset();
leonardene 1:f3c6c8f735a5 162 }
leonardene 7:112b745b2b9e 163 }
leonardene 7:112b745b2b9e 164
leonardene 9:4b38e90b8f10 165 /*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 17:5d08825028b0 166 void triangulering ()
leonardene 19:f57c238efcd8 167 {
leonardene 19:f57c238efcd8 168 /*
leonardene 17:5d08825028b0 169 int b;
leonardene 17:5d08825028b0 170 b = srf02.getDistanceCm();
leonardene 17:5d08825028b0 171 pc.printf("\nx(srf02) = %d", b);
leonardene 19:f57c238efcd8 172 */
leonardene 19:f57c238efcd8 173
leonardene 8:63f1033aaea2 174 char _2DplanX[12] = {'|','O','O','O','O','O','O','O','O','O','O','|'};
leonardene 8:63f1033aaea2 175 char _2Dplan[12] = {'|','O','O','O','O','O','O','O','O','O','O','|'};
leonardene 17:5d08825028b0 176 double vinkel_b, vinkel_a, vinkel_x, vinkel_y, y_akse, x_akse, hyp, PI;
leonardene 17:5d08825028b0 177 int a, b, c, n;
leonardene 17:5d08825028b0 178 float d;
leonardene 17:5d08825028b0 179 PI = acos(-1.0);
leonardene 19:f57c238efcd8 180 while (d < 180.0 || d > 280.0 && n < 10) {
leonardene 17:5d08825028b0 181 d = srf05.read();
leonardene 17:5d08825028b0 182 n++;
leonardene 17:5d08825028b0 183 wait_ms (100);
leonardene 17:5d08825028b0 184 }
leonardene 17:5d08825028b0 185 n = 0;
leonardene 19:f57c238efcd8 186 while (b < 150 || b > 250 && n < 10) {
leonardene 17:5d08825028b0 187 b = srf02.getDistanceCm();
leonardene 17:5d08825028b0 188 wait_ms (250);
leonardene 17:5d08825028b0 189 n++;
leonardene 17:5d08825028b0 190 }
leonardene 15:324186d4ea61 191 pc.printf("\nx(srf02) = %d\ny(srf05) = %f\n", b, d);
leonardene 8:63f1033aaea2 192 bt.printf("\nx = %d\ny = %f\n", b, d);
leonardene 19:f57c238efcd8 193 if (n > 10) {
leonardene 17:5d08825028b0 194 char kom1 []= {"\nERROR: No object detected within the bouandaries\n"};
leonardene 17:5d08825028b0 195 pc.printf("%s", kom1);
leonardene 17:5d08825028b0 196 bt.printf("%s", kom1);
leonardene 17:5d08825028b0 197 }
leonardene 17:5d08825028b0 198 hyp = 305.0;
leonardene 17:5d08825028b0 199 vinkel_b = acos((hyp*hyp+d*d-b*b)/(2.0*hyp*d));
leonardene 17:5d08825028b0 200 vinkel_a = acos((hyp*hyp+b*b-d*d)/(2.0*hyp*b));
leonardene 19:f57c238efcd8 201 vinkel_y = PI-0.8076167287-vinkel_b;
leonardene 17:5d08825028b0 202 if( vinkel_y > PI/2.0)
leonardene 19:f57c238efcd8 203 vinkel_y = 0.8076167287+vinkel_b;
leonardene 17:5d08825028b0 204 y_akse = sin(vinkel_y)*d;
leonardene 19:f57c238efcd8 205 vinkel_x = PI-0.7631795981-vinkel_a;
leonardene 17:5d08825028b0 206 if( vinkel_x > PI/2.0)
leonardene 19:f57c238efcd8 207 vinkel_x = 0.7631795981+vinkel_a;
leonardene 17:5d08825028b0 208 x_akse = sin(vinkel_x)*b;
leonardene 17:5d08825028b0 209
leonardene 17:5d08825028b0 210 pc.printf("\nx = %lf\ny = %lf\n", x_akse, y_akse);
leonardene 17:5d08825028b0 211
leonardene 17:5d08825028b0 212
leonardene 17:5d08825028b0 213 for (c = 1; c < 11; c++) {
leonardene 17:5d08825028b0 214 if (((c-1)*10+150) <= x_akse && x_akse < (c*10+150))
leonardene 17:5d08825028b0 215 _2DplanX[c] = 'X';
leonardene 8:63f1033aaea2 216 else
leonardene 8:63f1033aaea2 217 _2DplanX[c] = 'O';
leonardene 17:5d08825028b0 218 }
leonardene 17:5d08825028b0 219 for (c = 280; c >= 190; c=c-10) {
leonardene 17:5d08825028b0 220 if (c-10 <= y_akse && y_akse < c) {
leonardene 17:5d08825028b0 221 for(a = 0; a < 12; a++) {
leonardene 17:5d08825028b0 222 pc.printf("%c",_2DplanX[a]);
leonardene 17:5d08825028b0 223 bt.printf("%c",_2DplanX[a]);
leonardene 17:5d08825028b0 224 }
leonardene 17:5d08825028b0 225 } else {
leonardene 17:5d08825028b0 226 for(a = 0; a < 12; a++) {
leonardene 17:5d08825028b0 227 pc.printf("%c",_2Dplan[a]);
leonardene 17:5d08825028b0 228 bt.printf("%c",_2Dplan[a]);
leonardene 17:5d08825028b0 229 }
leonardene 17:5d08825028b0 230 }
leonardene 17:5d08825028b0 231 printf("\n");
leonardene 17:5d08825028b0 232 }
leonardene 19:f57c238efcd8 233
leonardene 17:5d08825028b0 234 }
leonardene 17:5d08825028b0 235 /*
leonardene 17:5d08825028b0 236 void triangulering ()
leonardene 17:5d08825028b0 237 {
leonardene 17:5d08825028b0 238 char _2DplanX[12] = {'|','O','O','O','O','O','O','O','O','O','O','|'};
leonardene 17:5d08825028b0 239 char _2Dplan[12] = {'|','O','O','O','O','O','O','O','O','O','O','|'};
leonardene 17:5d08825028b0 240 double vinkel_b, vinkel_a, vinkel_x, vinkel_y, y_akse, x_akse, hyp, PI;
leonardene 17:5d08825028b0 241 int a, b, c, n;
leonardene 17:5d08825028b0 242 float d;
leonardene 17:5d08825028b0 243 PI = acos(-1.0);
leonardene 17:5d08825028b0 244 while (d < 140.0 || d > 190.0 && n < 10) {
leonardene 17:5d08825028b0 245 d = srf05.read();
leonardene 17:5d08825028b0 246 n++;
leonardene 17:5d08825028b0 247 wait_ms (100);
leonardene 17:5d08825028b0 248 }
leonardene 17:5d08825028b0 249 n = 0;
leonardene 17:5d08825028b0 250 while (b < 60 || b > 160 && n < 10) {
leonardene 17:5d08825028b0 251 b = srf02.getDistanceCm();
leonardene 17:5d08825028b0 252 wait_ms (250);
leonardene 17:5d08825028b0 253 n++;
leonardene 17:5d08825028b0 254 }
leonardene 17:5d08825028b0 255 pc.printf("\nx(srf02) = %d\ny(srf05) = %f\n", b, d);
leonardene 17:5d08825028b0 256 bt.printf("\nx = %d\ny = %f\n", b, d);
leonardene 17:5d08825028b0 257 if (n == 10) {
leonardene 17:5d08825028b0 258 char kom1 []= {"\nERROR: No object detected within the bouandaries\n"};
leonardene 17:5d08825028b0 259 pc.printf("%s", kom1);
leonardene 17:5d08825028b0 260 bt.printf("%s", kom1);
leonardene 17:5d08825028b0 261 }
leonardene 17:5d08825028b0 262 hyp = 198.0284071;
leonardene 17:5d08825028b0 263 vinkel_b = acos((hyp*hyp+d*d-b*b)/(2.0*hyp*d));
leonardene 17:5d08825028b0 264 vinkel_a = acos((hyp*hyp+b*b-d*d)/(2.0*hyp*b));
leonardene 17:5d08825028b0 265 vinkel_y = PI-0.9848945605-vinkel_b;
leonardene 17:5d08825028b0 266 if( vinkel_y > PI/2.0)
leonardene 17:5d08825028b0 267 vinkel_y = 0.9848945605+vinkel_b;
leonardene 17:5d08825028b0 268 y_akse = sin(vinkel_y)*d;
leonardene 17:5d08825028b0 269 vinkel_x = PI-0.5859017663-vinkel_a;
leonardene 17:5d08825028b0 270 if( vinkel_x > PI/2.0)
leonardene 17:5d08825028b0 271 vinkel_x = 0.5859017663+vinkel_a;
leonardene 17:5d08825028b0 272 x_akse = sin(vinkel_x)*b;
leonardene 17:5d08825028b0 273 for (c = 1; c < 11; c++) {
leonardene 17:5d08825028b0 274 if (((c-1)*10+60) <= x_akse && x_akse < (c*10+60))
leonardene 17:5d08825028b0 275 _2DplanX[c] = 'X';
leonardene 17:5d08825028b0 276 else
leonardene 17:5d08825028b0 277 _2DplanX[c] = 'O';
leonardene 17:5d08825028b0 278 }
leonardene 17:5d08825028b0 279 for (c = 190; c >= 150; c=c-10) {
leonardene 17:5d08825028b0 280 if (c-10 <= y_akse && y_akse < c) {
leonardene 9:4b38e90b8f10 281 for(a = 0; a < 12; a++) {
leonardene 8:63f1033aaea2 282 pc.printf("%c",_2DplanX[a]);
leonardene 8:63f1033aaea2 283 bt.printf("%c",_2DplanX[a]);
leonardene 9:4b38e90b8f10 284 }
leonardene 8:63f1033aaea2 285 } else {
leonardene 9:4b38e90b8f10 286 for(a = 0; a < 12; a++) {
leonardene 8:63f1033aaea2 287 pc.printf("%c",_2Dplan[a]);
leonardene 8:63f1033aaea2 288 bt.printf("%c",_2Dplan[a]);
leonardene 9:4b38e90b8f10 289 }
leonardene 8:63f1033aaea2 290 }
leonardene 8:63f1033aaea2 291 printf("\n");
leonardene 8:63f1033aaea2 292 }
leonardene 9:4b38e90b8f10 293 }
leonardene 14:215006ed25d8 294 */