Leo Bach / Mbed 2 deprecated Prosjekt_2

Test

Dependencies:   SRF02 SRF05 mbed

Fork of Prosjekt_2 by HIOF Programmering

main.cpp@25:a35c991995ce, 2016-04-30 (annotated)

Committer:
leonardene
Date:
Sat Apr 30 11:00:06 2016 +0000
Revision:
25:a35c991995ce
Parent:
24:06d3a7b85602 
Flytta array _2d;

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 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 25:a35c991995ce 167 char _2DplanX[12] = {'|','O','O','O','O','O','O','O','O','O','O','|'};
leonardene 25:a35c991995ce 168 char _2Dplan[12] = {'|','O','O','O','O','O','O','O','O','O','O','|'};
leonardene 17:5d08825028b0 169 double vinkel_b, vinkel_a, vinkel_x, vinkel_y, y_akse, x_akse, hyp, PI;
leonardene 24:06d3a7b85602 170 int a, b, c, n, m;
leonardene 17:5d08825028b0 171 float d;
leonardene 17:5d08825028b0 172 PI = acos(-1.0);
leonardene 24:06d3a7b85602 173 hyp = 305.0;
leonardene 24:06d3a7b85602 174 char h;
leonardene 24:06d3a7b85602 175 while (h != 'n') {
leonardene 24:06d3a7b85602 176 while (pc.readable() == 0 && bt.readable() == 0) {}
leonardene 24:06d3a7b85602 177 h = input();
leonardene 24:06d3a7b85602 178 if (h == 'y') {
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 }