File content as of revision 8:019cd6583613:

#include "mbed.h"
#include "rtos.h"
#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) {
        //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(); //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
    }
}