![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Projecte final de Sistemes Encastats
Dependencies: Hc05 RawSerialPc mbed-rtos mbed
main.cpp
- Committer:
- jcabello7
- Date:
- 2016-01-19
- Revision:
- 9:75a136453e21
- Parent:
- 8:019cd6583613
File content as of revision 9:75a136453e21:
#include "mbed.h" #include "rtos.h" #include "Hc05.h" #include "RawSerialPc.h" //JOSEP RawSerialPc pc(PC_10, PC_11); //Port serie TX->PC_10 RX->PC_11 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) { //Intenta llegir un string, i si el llegeix, el tracta if(bt.llegirString()) bt.tractaString(); //Comprova la connexió (mira si ha rebut alguna cosa) bt.comprovaConnexio(); //Temps del Thread que està esperant Thread::wait(100); } } //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 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 bt.calculaMotors(S1, S2, S3); //Calcula com s'han de moure els motors //snprintf(m, 128, "Mode: %d | timer: %dms", bt.getMode(), bt.getTimer()); //Per visualitzar el mode i el timer, no es necessari //pc.enviaString(m); //Per visualitzar el mode i el timer, no es necessari //pc.enviaString(bt.strPos); //Per visualitzar la posicio on ha d'anar el robot, no es necessari if((bt.getMode() == 0) || (bt.getMode() == 1) || (bt.getMode() == 2)) { pc.enviaString(bt.strM1); //String motor1 que s'envia a l'arduino d'en Marcel pc.enviaString(bt.strM2); //String motor2 que s'envia a l'arduino d'en Marcel pc.enviaString(bt.strM3); //String motor3 que s'envia a l'arduino d'en Marcel } else if (bt.getMode() == 3) { ModeAutomatic(); } else if (bt.getMode() == 4) { ;//Mode mando (programa Marc) } else { pc.enviaString("V000FM1"); //String motor1 que s'envia a l'arduino d'en Marcel pc.enviaString("V000FM2"); //String motor2 que s'envia a l'arduino d'en Marcel pc.enviaString("V000FM3"); //String motor3 que s'envia a l'arduino d'en Marcel } Thread::wait(500); //Temps del Thread que està esperant } }