File content as of revision 2:b5a8e835fb14:

#include "mbed.h"
#include "Motor.h"

#define N true
#define B false
#define NUMSENS 4
#define SPEED 0.5

//----------------------------------------------------------------------------//
//                      Inizializzazione dei Pin                              //
//----------------------------------------------------------------------------//

Motor MotS(D9, D10, D12);   //pwm (E1), fwd (1A), rev (1B)   motore sinistro
Motor MotD(D6, D7, D8);     //pwm (E2), fwd (2A), rev (2B)   motore destro
DigitalIn start(PC_13);
DigitalIn sensd[] = {D2, D3, D4, D5};        //array dei sensori digitali, da sinistra a destra
AnalogIn sensa[] = {A0, A1, A2, A3};         //array dei sensori analogici, da sinistra a destra
PwmOut led[] = {D11, D14, D15};        //array dei sensori digitali, da sinistra a destra

int LetturaSensore(void);
void Avanti(void);
void Destra(int curv);
void Sinistra(int curv);
void Inversione(void);
void ledrgb(int st);

Serial pc(USBTX, USBRX);

//----------------------------------------------------------------------------//

int stato = 0;

int main()
{   
    // Inizializzazione PWM
    for(int i=0; i<=2; i++)
        led[i].period(0.01);
    
    wait(1);
    
    // Inizializzazione macchinina
    for(int i=0; i<3; i++){
        for(int j=0; j<3; j++){
            led[0] = 0;
            led[1] = 0;
            led[2] = 0;

            led[j] = 1;
            wait_ms(150);
        }
    }
    
    // Pulsante di Start
    while(start==1){
        ledrgb(stato);    
    }
    
    // Ciclo principale
    while(1) {      
        stato = LetturaSensore();
        //pc.printf("stato %i\r\n", stato);

        // andare avanti
        if(stato==6) {
            Avanti();
        }
        // Girare a sinistra
        else if(stato==12||stato==14) {
            Sinistra(0);
        }
        // Girare a destra
        else if(stato==3||stato==7) {
            Destra(0);
        }
        // Girare stretta a sinistra
        else if(stato==8) {
            Sinistra(1);
        }
        // Girare stretta a destra
        else if(stato==1) {
            Destra(1);
        }
        // Inversione
        else if(stato==15) {
            wait_ms(100);
            if(stato==15){
                Inversione();
            }
        }
        ledrgb(stato);
    }
}

//----------------------------------------------------------------------------//
//                              Funzioni                                      //
//----------------------------------------------------------------------------//
void Avanti(){
    MotS.speed(SPEED);
    MotD.speed(SPEED);
}
void Destra(int curv){
    if(curv == 0){
        MotS.speed(SPEED);
        MotD.speed(SPEED-0.1);
    }else if(curv == 1){
        MotS.speed(SPEED+0.1);
        MotD.speed(SPEED-0.3);   
    }
}
void Sinistra(int curv){
    if(curv == 0){
        MotS.speed(SPEED-0.1);
        MotD.speed(SPEED);
    }else if(curv == 1){
        MotS.speed(SPEED-0.3);
        MotD.speed(SPEED+0.1);   
    }
}
void Inversione(){
    MotS.speed(-0.35);
    MotD.speed(0.35);
}

//Funzione potenza utilizzata per la conversione da binario a decimale
int pow(int a, int ex){
    if(ex==0)
        return 1;
    if(ex==1)
        return a;

    int c=a;
    for(; ex!=1; ex=ex-1){
        a=a*c;
    }
    return a;
}

//Lettura sensore e conversione dello stato da binario->decimale
int LetturaSensore(){
    int data = 0;
    for(int i=NUMSENS-1; i>=0; i=i-1)
        data = data + sensd[i]*pow(2,NUMSENS-1-i);
    return data;
}

void ledrgb(int st){
        if (st == 0){
            led[0] = 1;   
            led[1] = 0;    
            led[2] = 1;
        }else{
            led[0] = sensa[0];   
            led[1] = (sensa[1]+sensa[2])/2;    
            led[2] = sensa[3];
        }
}