File content as of revision 5:f4923705f199:

/*---LIBRERIE---*/
    #include "mbed.h"
    #include "rtos.h"
    #include "hcsr04.h"
/*--------------*/

/*---COSTANTI---*/
    #define MAX 999999 //valore intero massimo utilizzabile

    #define CTRL_SIRENA D11  //D11 - pin aggancio sensore sonoro
    
    #define DISTANCE_CTRL 15 /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore
                               per capire quando la macchina si deve fermare*/

    #define NUMBER_CRC 9 //numero utilizzato per controlli nell avanzamento automatico (vedi nella funzione che lo utilizza per spiegazione approfondita)

    #define LED_AUTOMATIC PB_2  //attacco led controllo modalita automatica

    /*---PIN BLUETHOOT---*/
    /*I pin utilizzati per TX e RX sono pin morfo, a livello hardware collegare il pin TX dell HC05 al PA_12 e quello RX a PA_11 (a livello harware vanno invertiti rispetto a ciò che è scritto qui nella dichiarazione)*/
    #define TX PA_11
    #define RX PA_12
    /*-------------------*/
    
    /*---PIN CONTROLLO MOTORI---*/
    #define PWMB PC_7  //D9
    #define DIRB D8    //D8
    
    #define PWMA PB_4  //D5
    #define DIRA D4    //D4
    /*--------------------------*/
    
    /*---PIN ULTRASUONI---*/
    #define AVANTI_TRIGGER PA_5  //D13
    #define AVANTI_ECHO PA_6     //D12
    #define SINISTRA_TRIGGER PA_8  //D7
    #define SINISTRA_ECHO PB_10    //D6
    #define DESTRA_TRIGGER PA_10  //D2
    #define DESTRA_ECHO PB_3 //D3
    /*--------------------*/
/*--------------*/

/*---VARIABILI---*/
    /*---CONTROLLO MOTORI---*/
    PwmOut motorBpower(PWMB);
    DigitalOut motorBdirection(DIRB);
    
    PwmOut motorApower(PWMA);
    DigitalOut motorAdirection(DIRA);
    /*---------------------*/
    
    /*---CONTROLLO ULTRASUONI---*/
    HCSR04 ultrasuoniAvanti(AVANTI_TRIGGER, AVANTI_ECHO);
    HCSR04 ultrasuoniSinistra(SINISTRA_TRIGGER, SINISTRA_ECHO); 
    HCSR04 ultrasuoniDestra(DESTRA_TRIGGER, DESTRA_ECHO);
    /*--------------------------*/
    
    DigitalOut ledAutomatic(LED_AUTOMATIC); //pin in rifermineto al led controllo modalità automatica
    
    Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale
    
    Serial device(TX, RX); //oggetto per gestione seriale bluethoot
                             
    bool flagSirena = true; //utilizzato per bloccare il robot nel caso senta una sirena
    
    bool flagAutomatic; //utilizzo per sapere se utilizzare modalita automatica o meno
    
    bool flagStopAvanti; //utilizzo per fermare il robot se riceve da seriale un segnale di stop
/*----------------*/

/*---PROTOTIPI---*/
    void avanti();
    void indietro();
    void destra();
    void sinistra();
    void fermo();
    bool checkFermo();
    void automaticProgram();
    void setup();
    void check_sirena_thread(void const *args);
    void check_stop_thread(void const *args);
    void check_stop_avanti(void const *args); 
/*---------------*/

int main() {
    
    unsigned char rx;
    int distanceTemp;
    
    setup(); //settaggio iniziale del robot
    
    Thread sirenaThread(check_sirena_thread); // start Thread controllo sirena
    Thread ctrlStopThread(check_stop_thread); // start Thread controllo fine processo automatico
    Thread ctrlStopAvanti(check_stop_avanti); // start Thread controllo fine processo avanzamento
    
    while(true){
        if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
            rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
            //pc.printf("recived: %c \r\n",rx); //stampa a video
            switch(rx){ //in base alla lettera che riceve sa cosa deve fare
                case 'a':
                    flagStopAvanti = true; /*il thread check_stop_avanti viene avviato appena tale flag diventa true*/
                    /*a causa di problemi riscontrati con l'utilizzo dell'ultrasuono si e' deciso di ripere l'operazione di acquisizione
                      dati dall'ultrasuono per piu' volte (tante quante NUMBER_CRC) in modo da essere sicuri del valore ottenuto*/
                    for(int j = 0; j < NUMBER_CRC && flagStopAvanti; j++){
                        distanceTemp = MAX;
                        for( int i = 0; i < 100 && ( distanceTemp < 2 || distanceTemp > 400 && flagStopAvanti ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
                            ultrasuoniAvanti.start();
                            distanceTemp = ultrasuoniAvanti.get_dist_cm();
                            //wait_ms(10);
                        }
                        while( distanceTemp > 8 && flagStopAvanti ){
                            avanti();
                            ultrasuoniAvanti.start();
                            distanceTemp = ultrasuoniAvanti.get_dist_cm();
                        }
                        fermo();
                    }
                    break;
                case 'b':
                    indietro();
                    break;
                case 'c':
                    destra();
                    break;
                case 'd':
                    sinistra();
                    break;
                case 'e':
                    fermo();
                    break;
                case 'f':
                    flagAutomatic = true;
                    ledAutomatic = 1; //accensione led mod automatica
                    automaticProgram(); //funzione pilota automatico
                    ledAutomatic = 0; //spegnimento led
                    break;
            }
        }
    }
    
}

void setup()
{
    motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
    motorApower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
    fermo(); 
}

void automaticProgram()
{
    int avantiDis, distance;
    
    int distanceDx, distanceSx;
    
    fermo();
    
    /*tutte le strutture di controllo contengono i controllo dei FLAG sirena e automatic in modo che in qualunque momento uno
    dei due cambi il robot lo capisca subito*/
    
    while(flagAutomatic){ //flag modificabile dal thread che aspetta il segnale di stop
        while(flagSirena && flagAutomatic){ //il robot si puo' muovere liberamente finche non sente il suono della sirena tipo ambulanza oppure non riceve il segnale di stop dall'utente
            ultrasuoniAvanti.start();
            avantiDis = ultrasuoniAvanti.get_dist_cm();
            //pc.printf("avanti: %d \r\n",avantiDis);
            while( avantiDis >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //finche il valore letto dall' ultrasuono e' maggiore della distanza di urto la macchina puo' procedere
                avanti();
                ultrasuoniAvanti.start();
                avantiDis = ultrasuoniAvanti.get_dist_cm();
                //pc.printf("avanti: %d \r\n",avantiDis);
                wait_ms(10);
            }
            
            fermo();
            
            if( checkFermo() && flagSirena && flagAutomatic ){       
                distanceDx = MAX;
                distanceSx = MAX;
                 /*funzione utilizzata per leggere un valore attendibile ( nel range dell ultrasuono ). se dopo 100 tentativi
                  il valore non e' ancora attendibile allora esco dal controllo in modo da non rimanere bloccato*/ 
                for( int i = 0; i < 100 && ( distanceDx < 2 || distanceDx > 400 ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
                    ultrasuoniDestra.start();
                    distanceDx = ultrasuoniDestra.get_dist_cm();
                    //wait_ms(10);
                }
                
                for( int i = 0; i < 100 && ( distanceSx < 2 || distanceSx > 400 ); i++ ){ //questo perche il range dell'ultrasuono usato va da 2 cm a 4 m ( 400 cm )
                    ultrasuoniSinistra.start();
                    distanceSx = ultrasuoniSinistra.get_dist_cm();
                    //wait_ms(10);
                }
                
                /*controllo se il valori valori di uscita dalle iterazioni sono attendibili ( nel range )
                  se non soddisfano i criteri allora li setto al valore MAX in modo che i seguenti controlli non li prendano
                  in considerazione*/
                if( distanceSx < 2 || distanceSx > 400 )
                    distanceSx = MAX;
                    
                if( distanceDx < 2 || distanceDx > 400 )
                    distanceDx = MAX;
                
                //pc.printf("distanceSx: %d  distanceDx: %d\r\n",distanceSx, distanceDx);
                
                //in base ai valori letti capisco da che lato e' piu vicino la superficie piana e in base a cio sono da che parte girare
                if( distanceSx > distanceDx ){
                    sinistra();
                    wait_ms(1000);
                    
                    ultrasuoniDestra.start();
                    distance = ultrasuoniDestra.get_dist_cm();
                    //pc.printf("sinistra: %d \r\n",sinistraDis);
                    while( distance >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //il robot continua la rotazione per evitare l' ostacolo finche l' ultrasuono di destra avverte di aver trovato una superficie piana
                        ultrasuoniDestra.start();
                        distance = ultrasuoniDestra.get_dist_cm();
                        //pc.printf("distance: %d \r\n",distance);
                    }
                }
                else{
                    destra();
                    wait_ms(1000);
                    
                    ultrasuoniSinistra.start();
                    distance = ultrasuoniSinistra.get_dist_cm();
                    //pc.printf("sinistra: %d \r\n",sinistraDis);
                    while( distance >= DISTANCE_CTRL && flagSirena && flagAutomatic ){ //il robot continua la rotazione per evitare l' ostacolo finche l' ultrasuono di sinistra avverte di aver trovato una superficie piana
                        ultrasuoniSinistra.start();
                        distance = ultrasuoniSinistra.get_dist_cm();
                        //pc.printf("distance: %d \r\n",distance);
                    }
                }
                
                wait_ms(150);
                fermo();
            }
        }
    }
}

void check_stop_avanti(void const *args) 
{ 
    /*questo thread ha il compito di attendere che l utente richieda lo stop del mezzo, quando cio avviene il thread setta il flag a 
    false e interrompe sia se stesso che la funzione avanti (questo thread lavora solo sulla funzione avanti)*/
    unsigned char rx;
    while(true) 
        while( flagStopAvanti )
            if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
                rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
                if( rx == 'e' )
                    flagStopAvanti = false;
            }
}

void check_stop_thread(void const *args) 
{ 
    /*thread che controlla se l'utente via dispositivi ha chiesto lo stop del programma automatico*/
    unsigned char rx;
    while(true) 
        while( flagAutomatic )
            if( device.readable() ) { //funzione simile alla Serial.available() di arduino, solo se il monitor e` attivo permette di utilizzarlo
                rx = device.getc(); //funzione simile alla Serial.read() di arduino, legge il carattere dal seriale del bluethoot
                if( rx == 'g' )
                    flagAutomatic = false;
            }
}

void check_sirena_thread(void const *args) 
{
    /*thread che controlla sempre il pin dedicato alla sirena, se il pin diventa alto comunica il cambiamento 
    con un flag al main che si adatta alla situazione*/
    DigitalIn in(CTRL_SIRENA);
    while(true){
        int val = in.read();
        if( val == 1 )
            flagSirena = false;
        else
            flagSirena = true;
    } 
}

bool checkFermo()
{
    /*funzione utilizzata per evitare che disturbi dell ultrasuono vengano presi in considerazione come valori utili
    La funzione controlla per un TOT volte se il valore e' attendibile e in caso positivo ritorna TRUE*/
    bool ret = true;
    int avantiDis, ctrl = 50;
    for(int i = 0; i < ctrl; i++){
        ultrasuoniAvanti.start();
        avantiDis = ultrasuoniAvanti.get_dist_cm();
        if( avantiDis >= DISTANCE_CTRL ){
            ret = false;
            break;
        }
        wait_ms(10);
    }
    return ret;
}

void avanti()
{
    motorBdirection = 1; 
    motorAdirection = 0;
    motorBpower.pulsewidth(0.01); // 100%
    motorApower.pulsewidth(0.01); // 100%
}

void indietro()
{
    motorBdirection = 0; 
    motorAdirection = 1;
    motorBpower.pulsewidth(0.01); // 100%
    motorApower.pulsewidth(0.01); // 100%
}

void destra()
{
    motorBdirection = 0; 
    motorAdirection = 0;
    motorBpower.pulsewidth(0.01); // 100%
    motorApower.pulsewidth(0.007); // 70%
}

void sinistra()
{
    motorBdirection = 1; 
    motorAdirection = 1;
    motorBpower.pulsewidth(0.007); // 100%
    motorApower.pulsewidth(0.01); // 100%
}

void fermo()
{
    motorBpower.pulsewidth(0); // fermo
    motorApower.pulsewidth(0); // fermo
}