![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Projecte final de Sistemes Encastats
Dependencies: Hc05 RawSerialPc mbed-rtos mbed
Diff: main.cpp
- Revision:
- 8:019cd6583613
- Parent:
- 7:723c1e609ac8
- Child:
- 9:75a136453e21
--- a/main.cpp Wed Dec 23 15:06:25 2015 +0000 +++ b/main.cpp Wed Dec 23 18:25:42 2015 +0000 @@ -3,10 +3,170 @@ #include "Hc05.h" #include "RawSerialPc.h" - +//JOSEP RawSerialPc pc(USBTX, USBRX); //Per provar si funciona, no cal al programa Hc05 bt(D8, D2); //Crea l'objecte bt, connectat als pins D8 (TX) i D2 (RX) (és la connexió sèrie BT) +//EVA + int maximumRange = 4000; // Rang màxim + int minimumRange = 30; // Rang mínim + + float duration; //Variable per calcular l'amplada en us + float distance; //Variable per calcular la distància en mm després de la conversió + + float S1,S2,S3; + + char F[100]; //Taula per poder passar els enters a strings + +float SensorUltrasons(int n){ + + if (n==1){ + DigitalOut trigPin1(D3); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant) + Timer timer1; // Creem un comptador per saber el temps que té de resposta + + trigPin1 = 0; + wait_ms(2); + + trigPin1 = 1; //Envia un pols de 10ms pel pin “Trigger” + wait_ms(10); + trigPin1 = 0; + + DigitalIn echoPin1(D3); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida) + + while (!echoPin1); //Espera a la pujada + timer1.start(); + + while (echoPin1); //Espera a la baixada + duration = timer1.read_us(); + } + + if (n==2){ + DigitalOut trigPin2(D5); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant) + Timer timer2; // Creem un comptador per saber el temps que té de resposta + + trigPin2 = 0; + wait_ms(2); + + trigPin2 = 1; //Envia un pols de 10ms pel pin “Trigger” + wait_ms(10); + trigPin2 = 0; + + DigitalIn echoPin2(D5); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida) + + while (!echoPin2); //Espera a la pujada + timer2.start(); + + while (echoPin2); //Espera a la baixada + duration = timer2.read_us(); + + } + + if (n==3){ + DigitalOut trigPin3(D6); // Trigger Pin (Utilitzarem el mateix per l'entrada més endavant) + Timer timer3; // Creem un comptador per saber el temps que té de resposta + + trigPin3 = 0; + wait_ms(2); + + trigPin3 = 1; //Envia un pols de 10ms pel pin “Trigger” + wait_ms(10); + trigPin3 = 0; + + DigitalIn echoPin3(D6); // Echo Pin (Utilitzem el mateix port D6 que haviem fet servir per la sortida) + + while (!echoPin3); //Espera a la pujada + timer3.start(); + + while (echoPin3); //Espera a la baixada + duration = timer3.read_us(); + } + + // Calcular la distancia (en mm) + float x = (duration/5.6); + return x; + } + + +void ModeAutomatic(){ + + float a=3999; + float b=3999; + float c=3999; + + pc.enviaString("V050FM2"); + pc.enviaString("V050FM3"); + //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 0º + while ((a>100)&&(bt.getMode()==3)){ + + a=SensorUltrasons(1); //Crida la funció amb valor n=1, per tant sensor 1 + + //pc.enviaString(p); + + if (a >= maximumRange || a <= minimumRange){ + // 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. + pc.enviaString("Sensor 1 fora de Rang \n"); + } + else { + // Enviar la distancia si està dins el rang de 0 a 4m + snprintf(F, 100, "Sensor 1: Distancia: %f mm\n",a); + pc.enviaString(F); + } + } + + pc.enviaString("V000FM2"); + pc.enviaString("V000FM3"); + + pc.enviaString("V050FM1"); + pc.enviaString("V050FM3"); + //MARCEL: PARA MOTORS + //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 120º + + while ((b>100)&&(bt.getMode()==3)){ + b=SensorUltrasons(2); //Crida la funció amb valor n=2, per tant sensor 2 + + if (b >= maximumRange || b <= minimumRange){ + // 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. + pc.enviaString("Sensor 2 fora de Rang \n"); + } + else { + // Enviar la distancia si està dins el rang de 0 a 4m + snprintf(F, 100, "Sensor 2: Distancia: %f mm\n",b); + pc.enviaString(F); + } + } + + pc.enviaString("V000FM1"); + pc.enviaString("V000FM3"); + + pc.enviaString("V050FM1"); + pc.enviaString("V050FM2"); + //MARCEL: PARA MOTORS + //MARCEL: ENGEGA MOTORS EN DIRECCIÓ 240º + + while ((c>100)&&(bt.getMode()==3)){ + c=SensorUltrasons(3); //Crida la funció amb valor n=3, per tant sensor 3 + + if (c >= maximumRange || c <= minimumRange){ + // 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. + pc.enviaString("Sensor 3 fora de Rang \n"); + } + else { + // Enviar la distancia si està dins el rang de 0 a 4m + snprintf(F, 100, "Sensor 3: Distancia: %f mm\n",c); + pc.enviaString(F); + } + } + + pc.enviaString("V000FM1"); + pc.enviaString("V000FM2"); + //MARCEL: PARA MOTORS + +} + + + + + //Aquest serà el meu fil, que va llegint cada 300ms del HC-05 void llegir_thread(void const *args) { while (true) { @@ -21,10 +181,27 @@ } } + +//Aquest serà el fil de l'Eva, que va llegint els sensors d'ultrasons +void llegir_ultrathread(void const *args) { + while (true) { + S1=SensorUltrasons(1); + S2=SensorUltrasons(2); + S3=SensorUltrasons(3); + + //Temps del Thread que està esperant + Thread::wait(100); + + } +} + //Aquest serà el programa principal int main() { + //osThreadSetPriority(osThreadGetId(),osPriorityRealtime); + Thread thread(llegir_thread); //Inicia el fil llegir_thread + thread.set_priority(osPriorityLow); - Thread thread(llegir_thread); //Inicia el fil llegir_thread + //Thread thread2(llegir_ultrathread); //Inicia el fil dels ultrasons //char m[128]; //Per visualitzar el mode i el timer, no es necessari while (true) { //pc.enviaString(bt.strLlegit); //Per veure l'ultim string llegit, no es necessari @@ -38,7 +215,7 @@ pc.enviaString(bt.strM3); //String motor3 que s'envia a l'arduino d'en Marcel } else if (bt.getMode() == 3) { - ;//Mode automàtic (programa Eva) + ModeAutomatic(); } else if (bt.getMode() == 4) { ;//Mode mando (programa Marc)