File content as of revision 3:2816fec3acee:

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

/*---COSTANTI---*/
    #define MAX 999999 //valore intero massimo utilizzabile
    
    #define DISTANCE_CTRL 20 /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore
                               per capire quando la macchina si deve fermare*/
                               
    #define NUM_CICLIC_CTRL 200 /*costante indicante il numero di volte che acquisisco un valore dall'ultrasuoni prima di dicidere se sia affidabile*/
     
    #define MIN_RANGE_ULTRASOUND 5 /*indica, in centimetri, il range minimo dell'ultrasuono*/
    
    #define MAX_RANGE_ULTRASOUND 400 /*indica, in centimetri, il range massimo dell'ultrasuono*/ 
                               
    #define NOT_CONTROL_TIME 300 /*tempo in cui il robot esegue movimenti senza controlli da parte dei sensori*/

    #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

    /*---COSTANTI PER GESTIONE SIRENA---*/
    #define MIN_CHECK 50
    #define PIN_SIRENA D11  //D11 - pin aggancio sensore sonoro
    /*----------------------------------*/

    /*---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 )*/
    #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); //oggetti controllo motori
    DigitalOut motorBdirection(DIRB);
    
    PwmOut motorApower(PWMA);
    DigitalOut motorAdirection(DIRA);
    /*---------------------*/
    
    /*---CONTROLLO ULTRASUONI---*/
    HCSR04 ultrasuoniAvanti(AVANTI_TRIGGER, AVANTI_ECHO); //istanzio oggetti per utilizzo ultrasuoni
    HCSR04 ultrasuoniSinistra(SINISTRA_TRIGGER, SINISTRA_ECHO); 
    HCSR04 ultrasuoniDestra(DESTRA_TRIGGER, DESTRA_ECHO);
    /*--------------------------*/
    
    DigitalOut ledAutomatic(LED_AUTOMATIC); //pin in rifermineto al led controllo mod automatica
    
    DigitalIn sirena(PIN_SIRENA); //pin in riferimento all'aggancio del sensore sonoro
    
    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;
/*----------------*/

/*---PROTOTIPI---*/
    void avanti();
    void indietro();
    void destra();
    void sinistra();
    void fermo();
    bool checkFermo();
    void automaticProgram();
    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;
    
    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 )
    
    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 < NUM_CICLIC_CTRL && ( distanceTemp < MIN_RANGE_ULTRASOUND || distanceTemp > MAX_RANGE_ULTRASOUND && 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 automaticProgram()
{
    int avantiDis, distance; //la variabile direction contiene 0 se il robot deve andare a destra, 1 se deve andare a sinistra
    
    int distanceDx, distanceSx;
    
    fermo();
    
    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
            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 ){    
                /*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*/    
                distanceDx = MAX;
                distanceSx = MAX;
                for( int i = 0; i < NUM_CICLIC_CTRL && ( distanceDx < MIN_RANGE_ULTRASOUND || distanceDx > MAX_RANGE_ULTRASOUND ); 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 < NUM_CICLIC_CTRL && ( distanceSx < MIN_RANGE_ULTRASOUND || distanceSx > MAX_RANGE_ULTRASOUND ); 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 < MIN_RANGE_ULTRASOUND || distanceSx > MAX_RANGE_ULTRASOUND ){
                    distanceSx = MAX;
                    distanceDx = MAX;
                    continue;
                }
                    
                if( distanceDx < MIN_RANGE_ULTRASOUND || distanceDx > MAX_RANGE_ULTRASOUND ){
                    distanceDx = MAX;
                    distanceSx = MAX;
                    continue;
                }
                
                /*if( distanceDx == MAX && distanceSx == MAX ) //mod veloce
                    continue;*/
                
                //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 && distanceSx != MAX){
                    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 lo avverte di aver trovato una superficie piana
                        ultrasuoniDestra.start() ;
                        distance = ultrasuoniDestra.get_dist_cm();
                        //pc.printf("distance: %d \r\n",distance);
                    }
                }
                else if( distanceDx != MAX ){
                    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 destra lo avverte di aver trovato una superficie piana
                        ultrasuoniSinistra.start();
                        distance = ultrasuoniSinistra.get_dist_cm();
                        //pc.printf("distance: %d \r\n",distance);
                    }
                }
                
                wait_ms(NOT_CONTROL_TIME);
                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*/
    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 proamma 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) 
{
    /*la funzione controlla il pin dedicato al sensore sonoro per un lasso di tempo fisso.
    Se in questo lasso di tempo riceve abbastanza valori che li indicano una sirena allora esce direttamente
    dalla struttura di controllo e blocca il robot, se in questo lasso invece dopo un po non riceve alcun valore
    attendibile allora esce della struttura di controllo prima del previsto e ricomincia i suo controlli da capo.*/
    int val, contaHigh = 0, contaLow = 0;
    int aumentoLow = 100; //rispetto al controllo dello stato HIGH, lo stato LOW e' controllato per piu' volte
    while(true) {
        for(int i = 0; i < 500 && ( contaHigh < MIN_CHECK ) && ( contaLow < MIN_CHECK+aumentoLow ); i++){
            val = sirena.read();
            //pc.printf("recived: %d \r\n",conta); //stampa a video
            if( val == 1 )
                contaHigh++;
            else
                if( val == 0 )
                    contaLow++;
            wait_ms(5);
        }
        if( contaLow >= MIN_CHECK+aumentoLow )
            flagSirena = true;
        else
            if( contaHigh >= MIN_CHECK )
                flagSirena = true;  //mettere FALSE dopo istallazione sensore sonoro
        contaHigh = 0;
        contaLow = 0;
        /*val = sirena.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); // 700%
    motorApower.pulsewidth(0.01); // 100%
}

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