File content as of revision 0:9cabfa16c820:

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

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

/*----------------------------------------------------------------------------//
Con velocità base di 0.3

//----------------------------------------------------------------------------*/



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

Motor MotS(D9, D10, D11);   //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


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

Serial pc(USBTX, USBRX);

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

int stato = 0;

int main()
{

    while(start==1) {}

    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);
        }
        else if(stato==15) {
            wait_ms(200);
            if(stato==15){
                Inversione();
            }
        }
        wait_ms(150);
    }
}

//----------------------------------------------------------------------------//
//                              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.2);   
    }
}
void Sinistra(int curv){
    if(curv == 0){
        MotS.speed(SPEED-0.1);
        MotD.speed(SPEED);
    }else if(curv == 1){
        MotS.speed(SPEED-0.2);
        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;
}