File content as of revision 0:cd38bc5c5a72:

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

/*---COSTANTI---*/
#define PWMB PC_7  //D9
#define DIRB D8    //D8

#define PWMA PB_4  //D5
#define DIRA D4    //D4

#define avantiTrigger PA_5  //D13
#define avantiEcho PA_6     //D12
#define sinistraTrigger PA_8  //D7
#define sinistraEcho PB_10    //D6
/*--------------*/

/*---VARIABILI GLOBALI---*/
PwmOut motorApower(PWMA); //istanzio oggetto per controllo motore A ( vedi scheda per capire quale sia )
DigitalOut motorAdirection(DIRA);

PwmOut motorBpower(PWMB); //istanzio oggetto per controllo motore B ( vedi scheda per capire quale sia )
DigitalOut motorBdirection(DIRB);

HCSR04 ultrasuoniAvanti(avantiTrigger, avantiEcho); //istanzio oggetti per controllo ultrasuoni
HCSR04 ultrasuoniSinistra(sinistraTrigger, sinistraEcho); 

Serial pc(USBTX, USBRX); //istanzio oggetto per utilizzo monitor seriale

int distanceCtrl = 10; /*decido una distanza, in cm, per i vari controlli dell' ultrasuono, esempio utilizzo come valore
                         per capire quando la macchina si deve fermare*/
/*-----------------------*/


/*---PROTOTIPI DELLE FUNZIONI---*/
void avanti();
void indietro();
void destra();
void sinistra();
void fermo();
bool checkFermo();
/*-------------------------------*/

int main() {
    
    int avantiDis, sinistraDis;
    
    motorBpower.period_ms(10); //setto periodo per impulsi da inviare ai motori ( consiglio di non variare )
    motorApower.period_ms(10);
    
    fermo();
    
    while(true){
        ultrasuoniAvanti.start();
        avantiDis = ultrasuoniAvanti.get_dist_cm();
        pc.printf("avanti: %d \r\n",avantiDis);
        while( avantiDis >= distanceCtrl ){ //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() ){
            destra();
            wait_ms(200);
            
            ultrasuoniSinistra.start();
            sinistraDis = ultrasuoniSinistra.get_dist_cm();
            pc.printf("sinistra: %d \r\n",sinistraDis);
            while(sinistraDis >= distanceCtrl){ //il robot continua la rotazione per evitare l' ostacolo finche l' ultrasuono di destra lo avverte di aver trovato una superficie piana
                ultrasuoniSinistra.start();
                sinistraDis = ultrasuoniSinistra.get_dist_cm();
                pc.printf("sinistra: %d \r\n",sinistraDis);
            }
            wait_ms(200);
            
            fermo();
        }
    }
    
}

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 >= distanceCtrl ){
            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
}