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)