Projecte final de Sistemes Encastats
Dependencies: Hc05 RawSerialPc mbed-rtos mbed
main.cpp@9:75a136453e21, 2016-01-19 (annotated)
- Committer:
- jcabello7
- Date:
- Tue Jan 19 14:47:31 2016 +0000
- Revision:
- 9:75a136453e21
- Parent:
- 8:019cd6583613
- Canvi del port s?rie per un que funciona; - Afegides funcions ultrasons (Eva); - Afegit un fil que comprova continuament la dist?ncia dels objectes; - El robot no pot avan?ar si hi ha un obstacle en el seu cam?; - Incorporat el mode autom?tic de l'Eva
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jcabello7 | 0:be638f6ee353 | 1 | #include "mbed.h" |
jcabello7 | 0:be638f6ee353 | 2 | #include "rtos.h" |
jcabello7 | 4:0a37a963b724 | 3 | #include "Hc05.h" |
jcabello7 | 4:0a37a963b724 | 4 | #include "RawSerialPc.h" |
jcabello7 | 0:be638f6ee353 | 5 | |
EvaSP | 8:019cd6583613 | 6 | //JOSEP |
jcabello7 | 9:75a136453e21 | 7 | RawSerialPc pc(PC_10, PC_11); //Port serie TX->PC_10 RX->PC_11 |
jcabello7 | 2:1ac2d1debc92 | 8 | Hc05 bt(D8, D2); //Crea l'objecte bt, connectat als pins D8 (TX) i D2 (RX) (és la connexió sèrie BT) |
jcabello7 | 1:c603de57c8b6 | 9 | |
EvaSP | 8:019cd6583613 | 10 | //EVA |
EvaSP | 8:019cd6583613 | 11 | int maximumRange = 4000; // Rang màxim |
EvaSP | 8:019cd6583613 | 12 | int minimumRange = 30; // Rang mínim |
EvaSP | 8:019cd6583613 | 13 | |
EvaSP | 8:019cd6583613 | 14 | float duration; //Variable per calcular l'amplada en us |
EvaSP | 8:019cd6583613 | 15 | float distance; //Variable per calcular la distància en mm després de la conversió |
EvaSP | 8:019cd6583613 | 16 | |
EvaSP | 8:019cd6583613 | 17 | float S1,S2,S3; |
EvaSP | 8:019cd6583613 | 18 | |
EvaSP | 8:019cd6583613 | 19 | char F[100]; //Taula per poder passar els enters a strings |
EvaSP | 8:019cd6583613 | 20 | |
EvaSP | 8:019cd6583613 | 21 | float SensorUltrasons(int n){ |
EvaSP | 8:019cd6583613 | 22 | |
EvaSP | 8:019cd6583613 | 23 | if (n==1){ |
EvaSP | 8:019cd6583613 | 24 | DigitalOut trigPin1(D3); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant) |
EvaSP | 8:019cd6583613 | 25 | Timer timer1; // Creem un comptador per saber el temps que té de resposta |
EvaSP | 8:019cd6583613 | 26 | |
EvaSP | 8:019cd6583613 | 27 | trigPin1 = 0; |
EvaSP | 8:019cd6583613 | 28 | wait_ms(2); |
EvaSP | 8:019cd6583613 | 29 | |
EvaSP | 8:019cd6583613 | 30 | trigPin1 = 1; //Envia un pols de 10ms pel pin “Trigger” |
EvaSP | 8:019cd6583613 | 31 | wait_ms(10); |
EvaSP | 8:019cd6583613 | 32 | trigPin1 = 0; |
EvaSP | 8:019cd6583613 | 33 | |
EvaSP | 8:019cd6583613 | 34 | DigitalIn echoPin1(D3); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida) |
EvaSP | 8:019cd6583613 | 35 | |
EvaSP | 8:019cd6583613 | 36 | while (!echoPin1); //Espera a la pujada |
EvaSP | 8:019cd6583613 | 37 | timer1.start(); |
EvaSP | 8:019cd6583613 | 38 | |
EvaSP | 8:019cd6583613 | 39 | while (echoPin1); //Espera a la baixada |
EvaSP | 8:019cd6583613 | 40 | duration = timer1.read_us(); |
EvaSP | 8:019cd6583613 | 41 | } |
EvaSP | 8:019cd6583613 | 42 | |
EvaSP | 8:019cd6583613 | 43 | if (n==2){ |
EvaSP | 8:019cd6583613 | 44 | DigitalOut trigPin2(D5); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant) |
EvaSP | 8:019cd6583613 | 45 | Timer timer2; // Creem un comptador per saber el temps que té de resposta |
EvaSP | 8:019cd6583613 | 46 | |
EvaSP | 8:019cd6583613 | 47 | trigPin2 = 0; |
EvaSP | 8:019cd6583613 | 48 | wait_ms(2); |
EvaSP | 8:019cd6583613 | 49 | |
EvaSP | 8:019cd6583613 | 50 | trigPin2 = 1; //Envia un pols de 10ms pel pin “Trigger” |
EvaSP | 8:019cd6583613 | 51 | wait_ms(10); |
EvaSP | 8:019cd6583613 | 52 | trigPin2 = 0; |
EvaSP | 8:019cd6583613 | 53 | |
EvaSP | 8:019cd6583613 | 54 | DigitalIn echoPin2(D5); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida) |
EvaSP | 8:019cd6583613 | 55 | |
EvaSP | 8:019cd6583613 | 56 | while (!echoPin2); //Espera a la pujada |
EvaSP | 8:019cd6583613 | 57 | timer2.start(); |
EvaSP | 8:019cd6583613 | 58 | |
EvaSP | 8:019cd6583613 | 59 | while (echoPin2); //Espera a la baixada |
EvaSP | 8:019cd6583613 | 60 | duration = timer2.read_us(); |
EvaSP | 8:019cd6583613 | 61 | |
EvaSP | 8:019cd6583613 | 62 | } |
EvaSP | 8:019cd6583613 | 63 | |
EvaSP | 8:019cd6583613 | 64 | if (n==3){ |
EvaSP | 8:019cd6583613 | 65 | DigitalOut trigPin3(D6); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant) |
EvaSP | 8:019cd6583613 | 66 | Timer timer3; // Creem un comptador per saber el temps que té de resposta |
EvaSP | 8:019cd6583613 | 67 | |
EvaSP | 8:019cd6583613 | 68 | trigPin3 = 0; |
EvaSP | 8:019cd6583613 | 69 | wait_ms(2); |
EvaSP | 8:019cd6583613 | 70 | |
EvaSP | 8:019cd6583613 | 71 | trigPin3 = 1; //Envia un pols de 10ms pel pin “Trigger” |
EvaSP | 8:019cd6583613 | 72 | wait_ms(10); |
EvaSP | 8:019cd6583613 | 73 | trigPin3 = 0; |
EvaSP | 8:019cd6583613 | 74 | |
EvaSP | 8:019cd6583613 | 75 | DigitalIn echoPin3(D6); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida) |
EvaSP | 8:019cd6583613 | 76 | |
EvaSP | 8:019cd6583613 | 77 | while (!echoPin3); //Espera a la pujada |
EvaSP | 8:019cd6583613 | 78 | timer3.start(); |
EvaSP | 8:019cd6583613 | 79 | |
EvaSP | 8:019cd6583613 | 80 | while (echoPin3); //Espera a la baixada |
EvaSP | 8:019cd6583613 | 81 | duration = timer3.read_us(); |
EvaSP | 8:019cd6583613 | 82 | } |
EvaSP | 8:019cd6583613 | 83 | |
EvaSP | 8:019cd6583613 | 84 | // Calcular la distancia (en mm) |
EvaSP | 8:019cd6583613 | 85 | float x = (duration/5.6); |
EvaSP | 8:019cd6583613 | 86 | return x; |
EvaSP | 8:019cd6583613 | 87 | } |
EvaSP | 8:019cd6583613 | 88 | |
EvaSP | 8:019cd6583613 | 89 | |
EvaSP | 8:019cd6583613 | 90 | void ModeAutomatic(){ |
EvaSP | 8:019cd6583613 | 91 | |
EvaSP | 8:019cd6583613 | 92 | float a=3999; |
EvaSP | 8:019cd6583613 | 93 | float b=3999; |
EvaSP | 8:019cd6583613 | 94 | float c=3999; |
EvaSP | 8:019cd6583613 | 95 | |
EvaSP | 8:019cd6583613 | 96 | pc.enviaString("V050FM2"); |
EvaSP | 8:019cd6583613 | 97 | pc.enviaString("V050FM3"); |
EvaSP | 8:019cd6583613 | 98 | //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 0º |
EvaSP | 8:019cd6583613 | 99 | while ((a>100)&&(bt.getMode()==3)){ |
EvaSP | 8:019cd6583613 | 100 | |
EvaSP | 8:019cd6583613 | 101 | a=SensorUltrasons(1); //Crida la funció amb valor n=1, per tant sensor 1 |
EvaSP | 8:019cd6583613 | 102 | |
EvaSP | 8:019cd6583613 | 103 | //pc.enviaString(p); |
EvaSP | 8:019cd6583613 | 104 | |
EvaSP | 8:019cd6583613 | 105 | if (a >= maximumRange || a <= minimumRange){ |
EvaSP | 8:019cd6583613 | 106 | // Enviar "Fora de Rang" per indicar-ho si escau a menys de 0m o més de 4m ja que segur que és un error. |
EvaSP | 8:019cd6583613 | 107 | pc.enviaString("Sensor 1 fora de Rang \n"); |
EvaSP | 8:019cd6583613 | 108 | } |
EvaSP | 8:019cd6583613 | 109 | else { |
EvaSP | 8:019cd6583613 | 110 | // Enviar la distancia si està dins el rang de 0 a 4m |
EvaSP | 8:019cd6583613 | 111 | snprintf(F, 100, "Sensor 1: Distancia: %f mm\n",a); |
EvaSP | 8:019cd6583613 | 112 | pc.enviaString(F); |
EvaSP | 8:019cd6583613 | 113 | } |
EvaSP | 8:019cd6583613 | 114 | } |
EvaSP | 8:019cd6583613 | 115 | |
EvaSP | 8:019cd6583613 | 116 | pc.enviaString("V000FM2"); |
EvaSP | 8:019cd6583613 | 117 | pc.enviaString("V000FM3"); |
EvaSP | 8:019cd6583613 | 118 | |
EvaSP | 8:019cd6583613 | 119 | pc.enviaString("V050FM1"); |
EvaSP | 8:019cd6583613 | 120 | pc.enviaString("V050FM3"); |
EvaSP | 8:019cd6583613 | 121 | //MARCEL: PARA MOTORS |
EvaSP | 8:019cd6583613 | 122 | //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 120º |
EvaSP | 8:019cd6583613 | 123 | |
EvaSP | 8:019cd6583613 | 124 | while ((b>100)&&(bt.getMode()==3)){ |
EvaSP | 8:019cd6583613 | 125 | b=SensorUltrasons(2); //Crida la funció amb valor n=2, per tant sensor 2 |
EvaSP | 8:019cd6583613 | 126 | |
EvaSP | 8:019cd6583613 | 127 | if (b >= maximumRange || b <= minimumRange){ |
EvaSP | 8:019cd6583613 | 128 | // Enviar "Fora de Rang" per indicar-ho si escau a menys de 0m o més de 4m ja que segur que és un error. |
EvaSP | 8:019cd6583613 | 129 | pc.enviaString("Sensor 2 fora de Rang \n"); |
EvaSP | 8:019cd6583613 | 130 | } |
EvaSP | 8:019cd6583613 | 131 | else { |
EvaSP | 8:019cd6583613 | 132 | // Enviar la distancia si està dins el rang de 0 a 4m |
EvaSP | 8:019cd6583613 | 133 | snprintf(F, 100, "Sensor 2: Distancia: %f mm\n",b); |
EvaSP | 8:019cd6583613 | 134 | pc.enviaString(F); |
EvaSP | 8:019cd6583613 | 135 | } |
EvaSP | 8:019cd6583613 | 136 | } |
EvaSP | 8:019cd6583613 | 137 | |
EvaSP | 8:019cd6583613 | 138 | pc.enviaString("V000FM1"); |
EvaSP | 8:019cd6583613 | 139 | pc.enviaString("V000FM3"); |
EvaSP | 8:019cd6583613 | 140 | |
EvaSP | 8:019cd6583613 | 141 | pc.enviaString("V050FM1"); |
EvaSP | 8:019cd6583613 | 142 | pc.enviaString("V050FM2"); |
EvaSP | 8:019cd6583613 | 143 | //MARCEL: PARA MOTORS |
EvaSP | 8:019cd6583613 | 144 | //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 240º |
EvaSP | 8:019cd6583613 | 145 | |
EvaSP | 8:019cd6583613 | 146 | while ((c>100)&&(bt.getMode()==3)){ |
EvaSP | 8:019cd6583613 | 147 | c=SensorUltrasons(3); //Crida la funció amb valor n=3, per tant sensor 3 |
EvaSP | 8:019cd6583613 | 148 | |
EvaSP | 8:019cd6583613 | 149 | if (c >= maximumRange || c <= minimumRange){ |
EvaSP | 8:019cd6583613 | 150 | // Enviar "Fora de Rang" per indicar-ho si escau a menys de 0m o més de 4m ja que segur que és un error. |
EvaSP | 8:019cd6583613 | 151 | pc.enviaString("Sensor 3 fora de Rang \n"); |
EvaSP | 8:019cd6583613 | 152 | } |
EvaSP | 8:019cd6583613 | 153 | else { |
EvaSP | 8:019cd6583613 | 154 | // Enviar la distancia si està dins el rang de 0 a 4m |
EvaSP | 8:019cd6583613 | 155 | snprintf(F, 100, "Sensor 3: Distancia: %f mm\n",c); |
EvaSP | 8:019cd6583613 | 156 | pc.enviaString(F); |
EvaSP | 8:019cd6583613 | 157 | } |
EvaSP | 8:019cd6583613 | 158 | } |
EvaSP | 8:019cd6583613 | 159 | |
EvaSP | 8:019cd6583613 | 160 | pc.enviaString("V000FM1"); |
EvaSP | 8:019cd6583613 | 161 | pc.enviaString("V000FM2"); |
EvaSP | 8:019cd6583613 | 162 | //MARCEL: PARA MOTORS |
EvaSP | 8:019cd6583613 | 163 | |
EvaSP | 8:019cd6583613 | 164 | } |
EvaSP | 8:019cd6583613 | 165 | |
EvaSP | 8:019cd6583613 | 166 | |
EvaSP | 8:019cd6583613 | 167 | |
EvaSP | 8:019cd6583613 | 168 | |
EvaSP | 8:019cd6583613 | 169 | |
jcabello7 | 1:c603de57c8b6 | 170 | //Aquest serà el meu fil, que va llegint cada 300ms del HC-05 |
jcabello7 | 0:be638f6ee353 | 171 | void llegir_thread(void const *args) { |
jcabello7 | 0:be638f6ee353 | 172 | while (true) { |
jcabello7 | 6:c4af95f6e155 | 173 | //Intenta llegir un string, i si el llegeix, el tracta |
jcabello7 | 3:4a598ab10e87 | 174 | if(bt.llegirString()) |
jcabello7 | 7:723c1e609ac8 | 175 | bt.tractaString(); |
jcabello7 | 6:c4af95f6e155 | 176 | //Comprova la connexió (mira si ha rebut alguna cosa) |
jcabello7 | 3:4a598ab10e87 | 177 | bt.comprovaConnexio(); |
jcabello7 | 6:c4af95f6e155 | 178 | //Temps del Thread que està esperant |
jcabello7 | 3:4a598ab10e87 | 179 | Thread::wait(100); |
jcabello7 | 3:4a598ab10e87 | 180 | |
jcabello7 | 0:be638f6ee353 | 181 | } |
jcabello7 | 0:be638f6ee353 | 182 | } |
jcabello7 | 1:c603de57c8b6 | 183 | |
EvaSP | 8:019cd6583613 | 184 | |
EvaSP | 8:019cd6583613 | 185 | //Aquest serà el fil de l'Eva, que va llegint els sensors d'ultrasons |
EvaSP | 8:019cd6583613 | 186 | void llegir_ultrathread(void const *args) { |
EvaSP | 8:019cd6583613 | 187 | while (true) { |
EvaSP | 8:019cd6583613 | 188 | S1=SensorUltrasons(1); |
EvaSP | 8:019cd6583613 | 189 | S2=SensorUltrasons(2); |
EvaSP | 8:019cd6583613 | 190 | S3=SensorUltrasons(3); |
EvaSP | 8:019cd6583613 | 191 | |
EvaSP | 8:019cd6583613 | 192 | //Temps del Thread que està esperant |
EvaSP | 8:019cd6583613 | 193 | Thread::wait(100); |
EvaSP | 8:019cd6583613 | 194 | |
EvaSP | 8:019cd6583613 | 195 | } |
EvaSP | 8:019cd6583613 | 196 | } |
EvaSP | 8:019cd6583613 | 197 | |
jcabello7 | 1:c603de57c8b6 | 198 | //Aquest serà el programa principal |
jcabello7 | 0:be638f6ee353 | 199 | int main() { |
EvaSP | 8:019cd6583613 | 200 | //osThreadSetPriority(osThreadGetId(),osPriorityRealtime); |
EvaSP | 8:019cd6583613 | 201 | Thread thread(llegir_thread); //Inicia el fil llegir_thread |
EvaSP | 8:019cd6583613 | 202 | thread.set_priority(osPriorityLow); |
jcabello7 | 1:c603de57c8b6 | 203 | |
EvaSP | 8:019cd6583613 | 204 | //Thread thread2(llegir_ultrathread); //Inicia el fil dels ultrasons |
jcabello7 | 7:723c1e609ac8 | 205 | //char m[128]; //Per visualitzar el mode i el timer, no es necessari |
jcabello7 | 0:be638f6ee353 | 206 | while (true) { |
jcabello7 | 7:723c1e609ac8 | 207 | //pc.enviaString(bt.strLlegit); //Per veure l'ultim string llegit, no es necessari |
jcabello7 | 9:75a136453e21 | 208 | bt.calculaMotors(S1, S2, S3); //Calcula com s'han de moure els motors |
jcabello7 | 7:723c1e609ac8 | 209 | //snprintf(m, 128, "Mode: %d | timer: %dms", bt.getMode(), bt.getTimer()); //Per visualitzar el mode i el timer, no es necessari |
jcabello7 | 7:723c1e609ac8 | 210 | //pc.enviaString(m); //Per visualitzar el mode i el timer, no es necessari |
jcabello7 | 7:723c1e609ac8 | 211 | //pc.enviaString(bt.strPos); //Per visualitzar la posicio on ha d'anar el robot, no es necessari |
jcabello7 | 7:723c1e609ac8 | 212 | if((bt.getMode() == 0) || (bt.getMode() == 1) || (bt.getMode() == 2)) { |
jcabello7 | 7:723c1e609ac8 | 213 | pc.enviaString(bt.strM1); //String motor1 que s'envia a l'arduino d'en Marcel |
jcabello7 | 7:723c1e609ac8 | 214 | pc.enviaString(bt.strM2); //String motor2 que s'envia a l'arduino d'en Marcel |
jcabello7 | 7:723c1e609ac8 | 215 | pc.enviaString(bt.strM3); //String motor3 que s'envia a l'arduino d'en Marcel |
jcabello7 | 7:723c1e609ac8 | 216 | } |
jcabello7 | 7:723c1e609ac8 | 217 | else if (bt.getMode() == 3) { |
EvaSP | 8:019cd6583613 | 218 | ModeAutomatic(); |
jcabello7 | 7:723c1e609ac8 | 219 | } |
jcabello7 | 7:723c1e609ac8 | 220 | else if (bt.getMode() == 4) { |
jcabello7 | 7:723c1e609ac8 | 221 | ;//Mode mando (programa Marc) |
jcabello7 | 7:723c1e609ac8 | 222 | } |
jcabello7 | 7:723c1e609ac8 | 223 | else { |
jcabello7 | 7:723c1e609ac8 | 224 | pc.enviaString("V000FM1"); //String motor1 que s'envia a l'arduino d'en Marcel |
jcabello7 | 7:723c1e609ac8 | 225 | pc.enviaString("V000FM2"); //String motor2 que s'envia a l'arduino d'en Marcel |
jcabello7 | 7:723c1e609ac8 | 226 | pc.enviaString("V000FM3"); //String motor3 que s'envia a l'arduino d'en Marcel |
jcabello7 | 7:723c1e609ac8 | 227 | } |
jcabello7 | 6:c4af95f6e155 | 228 | Thread::wait(500); //Temps del Thread que està esperant |
jcabello7 | 0:be638f6ee353 | 229 | } |
jcabello7 | 0:be638f6ee353 | 230 | } |