File content as of revision 14:d58d474fae35:

// mbed specific header files.
#include "mbed.h"

// include suono del motore
#include "SampledSoundGurgle.h" // rumore del motore da fermo durante gli spsotamenti
//#include "Gurgle.h"
#include "SampledSoundWelcome.h" // messaggio di benvenuto
#include "SampledSoundMotosega.h" // rumore durante lo spostamento con Cesoia

//#include "SampledSoundMotosega.h"
//#include "SampledSoundTrattore.h"
 

// TimeOut in [microsec] per verificare la presenza del sensore prossimità. Se il sensore non è presente il timer supera TIMEOUTPROXSENSOR
#define TIMEOUTPROXSENSOR 1000 //tempo in [microsec]

// numero di campioni che compongono un periodo della sinusoide in Output sull'ADC
#define CLACSONSAMPLENUM   45 // consigliabile avere  multipli di 45

// numero di campioni acquisiti su cui effettuare la media di luminosità
#define NUMLIGHTSAMPLE 100

// Parametri di soglia per la luce. Accendi/spegni Luci se la luminosità scende/sale sotto/sopra  SOGLIALUCIMAX e SOGLIALUCIMIN
#define SOGLIALUCIMAX (1.85)
#define SOGLIALUCIMIN (1.45)

// parametri dell'onda coseno da generare
#define PI        (3.141592653589793238462)
#define AMPLITUDE 32767 //(1.0)    // x * 3.3V
#define PHASE     (PI/2) // 2*pi è un periodo
#define OFFSET    32767 //(0x7FFF)


// ticker per la generazione dell'onda con DAC
Ticker SampleOutTicker;            


// Timer per il calcolo dei tempi del sensore di prossimità
Timer TimerProxSensor;

// distanza in cm dell'ostacolo
double fDistance;


// tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni
int nTimerStart, nTimerCurrent, nTimerStop, nTimerTillNow;

// Buffer contenente la sinusoide da porre in output come Clacson.
unsigned short usaClacson[CLACSONSAMPLENUM];

// prototipo di funzione che genera i campioni della sinusoide da utilizzare per la generazione tramite DAC
void CalculateSinewave(void);
 

// Periodo di generazione campioni in output DeltaT = T/NumSample
double fDeltaTClacsonSound;
double fDeltaTEngineSound;

// amplificazione per i suoni da generare con l'ADC
double fAmpEngineSound; // rumore di Engine 
double fAmpClacsonSound; // rumore di Clacson
double fAmpShearSound; // rumore di Shear

// frequenza segnale audio da generare per clacson e motore
double fFreqClacsonSound;
double fFreqEngineSound;

// periodo della sinusoide audio da generare come suono del clacson
double fPeriodClacsonSOund;

// numero di campioni di clacson già inviati in output sul DAC
int nClacsonSampleCount;
// indice dell'array di generazione campioni clacson 
int nClacsonSampleIndex;

// indice dell'Array di generazione suoni del motore
volatile int nEngineSampleIndex;

// Flag che decide se generare oppure no il suono del motore. '1'=non generare il suono del motore, '0'=genera il suono del motore
int bEngineSoundStop;



// valore medio della Luminosità su NUMACQUISIZIONI acquisizioni
double fAvgLight;

// valore numerico, di tensione e di luce letto dall'ADC
volatile unsigned short usReadADC;
volatile float fReadVoltage;

// valore di luminosità letto dall'ADC
volatile float fLight;

// posizione del Cofano '0' = chiuso, '1'=aperto. Inizialmente DEVE essere chiuso (cioè '0')
int nPosizioneCofano=0;


// indice per il conteggio dei campioni di luce acquisiti dal fotoresistore
int nLightSampleIndex;
   
// timer per il calcolo della velocità
Timer TimerHall;
    


// variabile che conta il numero di fronti si salita del segnale encoder
volatile int nCountRiseEdge; 
    
// pin di pilotaggio Motore DC
DigitalOut OutMotorA (PB_0);
DigitalOut OutMotorB (PC_1);

// Output Digitali usati per i LED
DigitalOut LedWAD (PC_2);
DigitalOut LedWAS (PC_3);
DigitalOut LedWPD (PH_0);
DigitalOut LedWPS (PA_0) ;
DigitalOut LedYAD (PC_9); 
DigitalOut LedYAS (PC_8);
DigitalOut LedRPD (PA_13);
DigitalOut LedRPS (PA_14) ; 


// Input/Output Digitali usati per interfaccia RPI
DigitalIn InShearRPI (PB_11); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17
DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20
DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore. Collegato al Raspberry GPIO16
DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione. Collegato al Raspberry GPIO13
DigitalIn InFutureUse1RPI (PB_2); // usi futuri 1 di comunicazione. Collegato al Raspberry GPIO25
DigitalIn InFutureUse2RPI (PC_15); // usi futuri 2 di comunicazione. Collegato al Raspberry GPIO12

// Input e Output per i sensori e attuatori
AnalogOut OutWave(PA_4); // pin A2 di output per la forma d'onda analogica dedicata al suono
AnalogIn InWaveLight(PA_1); // pin A1 di input per la forma d'onda analogica dedicata alla luminosità
DigitalInOut InOutProxSensor (PC_0, PIN_OUTPUT, PullDown, 0); // Pin di tipo In-Out per la gestione del segnale Sig del Sensore di prossimità a ultrasuoni
InterruptIn InEncoderA(PA_9); // Primo Pin di input dall'encoder ottico collegato al motore per misurare lo spostamento
InterruptIn InEncoderB(PC_7); // Secondo Pin di input dall'encoder ottico collegato al motore. predisposizione per usi futuri

// Input/Output utilizzati da funzioni default su scheda NUCLEO
DigitalOut led2(LED2);// LED verde sulla scheda. Associato a PA_5
Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12
DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13

// input di diagnostica
DigitalIn InDiag1(PA_15,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN7 pin17-pin19 
//DigitalIn InDiag2(PB_11,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20 


//****************************
// Create the sinewave buffer
//****************************
void CalculateSinewave(int nOffset, int nAmplitude, double fPhase)
{
    // variabile contenente l'angolo in radianti
    double fRads;
    // indici per i cicli
    int nIndex;
    // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE
    double fDeltaF;
    // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF
    double fAngle;
    
    fDeltaF = 360.0/CLACSONSAMPLENUM;
    for (nIndex = 0; nIndex < CLACSONSAMPLENUM; nIndex++) 
    {
        fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide
        fRads = (PI * fAngle)/180.0; // Convert degree in radian
        //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET;
        usaClacson[nIndex] = nAmplitude * cos(fRads + fPhase) + nOffset;
    }
}

/********************************************************/
/* Funzione avviata all'inizio come saluto e Benvenuto  */
/********************************************************/
void WelcomeMessage()
{
    // indice per i cicli interni alla funzione
    int nIndex;
    
    // indice per l'array di welcome message
    int nWelcomeMsgIndex;
    // parametri per generare il messaggio di welcome
    double fAmpWelcomeSound;
    double fFreqWelcomeSound;
    double fDeltaTWelcomeSound;
    
    //++++++++++++ INIZIO Accendi le Luci in sequenza +++++++++++++++++
    // accendi tutte le luci
    LedWAD = 1;
    wait_ms(300);
    LedWAS = 1;
    wait_ms(300);
    LedWPD = 1;
    wait_ms(300);
    LedWPS = 1;
    wait_ms(300);
    LedYAD = 1; 
    wait_ms(300);
    LedYAS = 1;
    wait_ms(300);
    LedRPD = 1;
    wait_ms(300);
    LedRPS = 1;
    //++++++++++++ FINE Accendi le Luci in sequenza +++++++++++++++++
     
    //++++++++++++ INIZIO generazione messaggio di benvenuto +++++++++++++++++ 
    fAmpWelcomeSound = 1.0;  // fissa l'amplificazione per il messaggio di welcome. Valori da 0[min] a 1[max] 
    fFreqWelcomeSound=nSamplePerSecWelcome/nUnderSampleFactorWelcome;// campioni per secondo del welcome message da generare = nSamplePerSec/nUnderSampleFactor
    fDeltaTWelcomeSound = (1.0/fFreqWelcomeSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
   
    
    for(nWelcomeMsgIndex=0; nWelcomeMsgIndex < nSampleNumWelcome; nWelcomeMsgIndex++)
    {
        // mette in output un campione della forma d'onda del welcome message  moltiplicato per l'amplificazione fAmp
        OutWave.write_u16(naInputSoundWaveWelcome[nWelcomeMsgIndex]*fAmpWelcomeSound);
        
        // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
        //wait(fDeltaTWelcomeSound);
        wait_us(50);
    }
    //++++++++++++ FINE generazione messaggio di benvenuto +++++++++++++++++
    
    //++++++++++++ INIZIO Spegni le Luci in sequenza +++++++++++++++++
    // spegni le Luci in sequenza
    for(nIndex=0; nIndex<3; nIndex++)
    {
        wait_ms(100); 
        LedWAD = 1;
        wait_ms(100); 
        LedWAD = 0;
    }
    for(nIndex=0; nIndex<3; nIndex++)
    {
        wait_ms(100); 
        LedWAS = 1;
        wait_ms(100); 
        LedWAS = 0;
    }
    for(nIndex=0; nIndex<3; nIndex++)
    {
        wait_ms(100); 
        LedWPD = 1;
        wait_ms(100); 
        LedWPD = 0;
    }
    for(nIndex=0; nIndex<3; nIndex++)
    {
        wait_ms(100); 
        LedWPS = 1;
        wait_ms(100); 
        LedWPS = 0;
    }
    for(nIndex=0; nIndex<3; nIndex++)
    {
        wait_ms(100); 
        LedYAD = 1;
        wait_ms(100); 
        LedYAD =0;
    }
    for(nIndex=0; nIndex<3; nIndex++)
    {
        wait_ms(100); 
        LedYAS = 1;
        wait_ms(100); 
        LedYAS = 0;
    }
    for(nIndex=0; nIndex<3; nIndex++)
    {
        wait_ms(100); 
        LedRPD = 1;
        wait_ms(100); 
        LedRPD = 0;
    }
    for(nIndex=0; nIndex<3; nIndex++)
    {
        wait_ms(100); 
        LedRPS = 1;
        wait_ms(100); 
        LedRPS = 0;
    }
    //++++++++++++ FINE Spegni le Luci in sequenza +++++++++++++++++
            
}


/***********************************************************************/
/* Genera il suono di una motosega.                                    */ 
/* Attivo quando arriva il comando di spostamento Cesoie da Raspberry  */
/***********************************************************************/
void ShearSoundGeneration()
{
    // indice per l'array di suono Shear  
    int nShearSoundIndex;
    // parametri per generare il messaggio di shear
    double fAmpShearSound;
    double fFreqShearSound;
    double fDeltaTShearSound;
    
    //++++++++++++ INIZIO generazione suono di motosega +++++++++++++++++ 
    fAmpShearSound = 1.0;  // fissa l'amplificazione per il suono di Shear. Valori da 0[min] a 1[max] 
    fFreqShearSound=nSamplePerSecShear/nUnderSampleFactorShear;// campioni per secondo del Shear da generare = nSamplePerSec/nUnderSampleFactor
    fDeltaTShearSound = (1.0/fFreqShearSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
   
    
    for(nShearSoundIndex=0; nShearSoundIndex < nSampleNumShear; nShearSoundIndex++)
    {
        // mette in output un campione della forma d'onda del suono di Shear,  moltiplicato per l'amplificazione fAmp
        OutWave.write_u16(naInputSoundWaveShear[nShearSoundIndex]*fAmpShearSound);
        
        // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
        wait(fDeltaTShearSound);
    }
    //++++++++++++ FINE generazione suono di motosega +++++++++++++++++
    
    
    
}
/***********************************************************************/
/* generazione suoni con i sample da file di campioni in SoundSample.h */
/***********************************************************************/
void SampleOut() 
{
    // interrompi il suono del motore per generare altri suoni. '1' = interrompi i suoni
    if(bEngineSoundStop == 0)
    {
        // mette in output un campione della forma d'onda del rumore motore moltiplicato per l'amplificazione fAmp
        OutWave.write_u16(naInputSoundWave[nEngineSampleIndex]*fAmpEngineSound);
        // incrementa l'indice del campione in output, nSampleNum è il numero dei campioni nle file Sound.h
        nEngineSampleIndex++;
        if(nEngineSampleIndex >= nSampleNum)
            nEngineSampleIndex=0;
    }        
}


 /**************************************************************************************/
/* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
/**************************************************************************************/
void riseEncoderIRQ()
{
    nCountRiseEdge++;
}       
     
/********/
/* Main */
/********/
int main()
{
    // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
    pc.baud(921600); //921600 bps
    
    // definisci il mode del segnale digitale di EncoderA
    InEncoderA.mode(PullUp);
    
    // Associa routine di Interrup all'evento fronte di salita del segnale di encoder
    InEncoderA.rise(&riseEncoderIRQ);

    // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove     
    InEncoderA.enable_irq();
    
    // avvia routine di saluto di benvenuto
    WelcomeMessage();    
       
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO TEST ++++++++++++++++++++++++++++++++++
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    
    //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
    fFreqClacsonSound = 440.0; // frequenza in Hz del tono del Clacson da generare
    fAmpClacsonSound = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
    fDeltaTClacsonSound = 1.0/(fFreqClacsonSound*CLACSONSAMPLENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
    CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmpClacsonSound), (PI/2.0)); // generazione della sinusoide con valori nominali
    //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
          
    //+++++++ INIZIO avvio rumore del motore a frequenza da fermo +++++++++
    fAmpEngineSound = 1.0;  // fissa l'amplificazione per il rumore motore. Valori da 0[min] a 1[max] 
    fFreqEngineSound=nSamplePerSec/nUnderSampleFactor;// campioni per secondo del rumore motore da generare = nSamplePerSec/nUnderSampleFactor
    fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
    nEngineSampleIndex =0; // Avvia indice di generazione suono motore
    SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
    //+++++++ FINE avvio ruomre del motore a frequenza da fermo +++++++++
   
    //inizializza variabili
    nEngineSampleIndex =0; // avvia l'indice di generazione suoni
    nCountRiseEdge=0; // azzera il  contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
    bEngineSoundStop =0; // inizialmente il suono del motore è generato
    nPosizioneCofano=0; // inizializza la posizione del cofano chiuso
    while(true)
    {
       //++++++++++ INIZIO genera diverso suono con motore fermo e in movimento +++++++++++++++++
       // se nella IRQ sono stati contati fronti di salita del dell'encoder, il robot si sta muovendo         
       if(nCountRiseEdge != 0)
       //if(InDiag1==1)
       {
           // sono stati contati impulsi di encoder, il robot si sta muovendo            
           fDeltaTEngineSound = (0.5/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
           SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
       }
       else
       {
            // se ci sono stati impulsi di encoder, il robot è fermo, genera rumore del motore fermo
           fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
           SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
         
       }   
       nCountRiseEdge=0;
       //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++
       
       //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++
        // inizializza il valore medio della Luminosità 
        fAvgLight=0.0;
        for(nLightSampleIndex=0; nLightSampleIndex < NUMLIGHTSAMPLE; nLightSampleIndex++)
        {
           // acquisisce dato da ADC
           usReadADC = InWaveLight.read_u16();
           fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
           //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
           fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
           fAvgLight+=fLight;
        }   
        // calcola valore medio su NUMSAMPLE acquisizioni 
        fAvgLight/= NUMLIGHTSAMPLE;
        
        // Accendi/Spegni i LED Bianchi se il valore medio della luminosità è sotto/sopra soglia
        if(fAvgLight < SOGLIALUCIMIN)
        {
           // Accendi LED Bianchi
           //led2 = 1;
           LedWAD = 1;
           LedWAS = 1;
           LedWPD = 1;
           LedWPS = 1;
        }
        else
        {
            if(fAvgLight > SOGLIALUCIMAX)
            {
               // Spegni LED Bianchi
               //led2 = 0;
               LedWAD = 0;
               LedWAS = 0;
               LedWPD = 0;
               LedWPS = 0;
            }
        }
        
        // invia il dato al PC
        //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
        //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
       //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
        //inizializza misura di distanza
        fDistance=0.0;
        // Fissa come Output il pin InOutProxSensor
        InOutProxSensor.output();
        // Poni 'L' sul Pin e mantienilo per qualche microsecondo
        InOutProxSensor.write(0);
        wait_us(5);
        // Poni 'H' sul Pin e mantienilo per qualche microsecondo
        InOutProxSensor.write(1);
        wait_us(10);
        // Poni 'L' sul Pin e mantienilo per qualche microsecondo
        InOutProxSensor.write(0);
        // Attendi assestamento e Fissa come Input il pin InOutProxSensor
        wait_us(5);
        InOutProxSensor.input();
        InOutProxSensor.mode(PullDown); // se non è presente il sensore, il pin rimane a '0'
        
        // attende la risposta del sensore di prossimità per un tempo fissato da TIMEOUTPROXSENSOR. Dopo tale tempo dichiara inesistente il sensore
        TimerProxSensor.start();
        nTimerStart = TimerProxSensor.read_us();
        nTimerTillNow=(TimerProxSensor.read_us()-nTimerStart);
        while((InOutProxSensor ==0) && (nTimerTillNow< TIMEOUTPROXSENSOR))
        {
            nTimerCurrent = TimerProxSensor.read_us();
            nTimerTillNow=nTimerCurrent-nTimerStart;
            led2=1; // se rimane nel while il LED rimane acceso
            pc.printf("sono qui 2 \r\n");
        }
        TimerProxSensor.stop(); // spegne il timer che serve per misurare il timeout quando assente il sensore di prossimità
        pc.printf("\r\nUscita dal while, nTimerTillNow = %d\r\n", nTimerTillNow);
        // se nTimerTillNow è inferiore al TIMEOUT, il sensore è presente e quindi misura la distanza dell'ostacolo
        if(nTimerTillNow < TIMEOUTPROXSENSOR)
        {
            // riattiva il timer per misurare la distanza dell'ostacolo
            TimerProxSensor.start();
            nTimerStart = TimerProxSensor.read_us();
            while(InOutProxSensor == 1)
            {
                led2=1; // se rimane nel while il LED rimane acceso
            }
            TimerProxSensor.stop();
            nTimerStop = TimerProxSensor.read_us();
           
            pc.printf("\r\nSensore Presente, nTimerTillNow = %d\r\n", nTimerTillNow);
        
            // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
            // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
            // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
            fDistance = (nTimerStop-nTimerStart)/58.2;
            // invia il dato al PC
            pc.printf("distanza dell'ostacolo = %f0.2\r\n", fDistance);
        }    
        else
        {
           // quando esce dai while bloccanti, il LED si spegne
           led2=0;
           pc.printf("\r\nTimeOut\r\n");
        }
        //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++ 
       //++++++++++++++ INIZIO Suona Clacson +++++++++
        //escludi le misure oltre il max
        if((fDistance <= 50.0) && (fDistance >= 3)) 
        //if(InDiag1 == 1)
        {
          // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
          if(fDistance < 22)
          {
                // blocca altri suoni quando genera suono del clacson
                bEngineSoundStop=1;
                // INIZIO generazione tono  
                nClacsonSampleIndex=0;
                // Genera il suono del clacson
                for(nClacsonSampleCount=0; nClacsonSampleCount<7000; nClacsonSampleCount++)
                {
                   OutWave.write_u16(usaClacson[nClacsonSampleIndex]); //max 32767
                   //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
                   wait(fDeltaTClacsonSound);
                   // genera ciclicamente
                   nClacsonSampleIndex++;
                   if(nClacsonSampleIndex >= CLACSONSAMPLENUM)
                   {
                       nClacsonSampleIndex=0;
                   }   
                   // a metà genera un wait per doppio clacson
                   if(nClacsonSampleCount == 2000)
                   {
                       wait_ms(100);
                   }
                
                } 
                //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
                OutWave.write_u16(0);
                
                // sblocca altri suoni dopo aver generato suono del clacson
                bEngineSoundStop=0;
                
            } // if(fDistance < soglia) suona clacson
            
        } // if( (fDistance < Max) && (fDistance > Min)) 
        //++++++++++++++ FINE Suona Clacson +++++++++ 
        
        
        
        //++++++++++++++  INIZIO pilotaggio motore cofano +++++++++++++++++++
        if((InMotorSwitchRPI==1) && (nPosizioneCofano ==0))    
        //if((myButton==1) && (nPosizioneCofano ==0))    
        {    
           //Ferma motore
            OutMotorA=0;
            OutMotorB=0;
            //pc.printf("Stop motore; OutA OutB = 00\r\n");
            wait_ms(10);
            
            //Ferma motore
            OutMotorA=0;
            OutMotorB=1;
            //pc.printf("Stop motore; OutA OutB = 01\r\n");
            wait_ms(10);
            
            // Ruota Right
            OutMotorA=1;
            OutMotorB=1;
            //pc.printf("Ruota Right; OutA OutB = 11\r\n");
            wait_ms(710);
            
            // Ferma Motore
            OutMotorA=0;
            OutMotorB=1;
            //pc.printf("Stop Motore; OutA OutB = 01\r\n");
            wait_ms(10);
            
            //Ferma motore
            OutMotorA=0;
            OutMotorB=0;
            //pc.printf("Stop motore; OutA OutB = 00\r\n");
            wait_ms(10);
            // cambia posizione del cofano. E' Stato Aperto
            nPosizioneCofano = 1;
        }       
        // se arriva comando di chiusura cofano & il cofano è aperto, muovi motore
        //if((myButton==0) && (nPosizioneCofano == 1))
        if((InMotorSwitchRPI==0) && (nPosizioneCofano ==1))   
        {
            //pc.printf("\r\nCofano aperto & comando di chiusura\r\n");
                                 
            //Ferma motore
            OutMotorA=0;
            OutMotorB=0;
            //pc.printf("Stop motore; OutA OutB = 00\r\n");
            wait_ms(10);
                        
            // Ruota Left
            OutMotorA=1;
            OutMotorB=0;
            //pc.printf("Ruota Left; OutA OutB = 10\r\n");
            wait_ms(730);
            
            //Ferma motore
            OutMotorA=0;
            OutMotorB=0;
            //pc.printf("Stop motore; OutA OutB = 00\r\n");
            wait_ms(10);
            
            // cambia posizione del cofano. E' Stato Chiuso
            nPosizioneCofano = 0;
        }   
        //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
        
        
        
        //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
        if(InLightSwitchRPI ==1)
        {
            // accendi i LED di abbellimento
            //led2=1;
            LedYAD = 1; 
            LedYAS = 1;
            LedRPD = 1;
            LedRPS = 1;
        } 
        else
        {
            
            // spegni i LED di abbellimento
            //led2=0;
            LedYAD = 0; 
            LedYAS = 0;
            LedRPD = 0;
            LedRPS = 0;
             
        }
        //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++
        
        //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
        if(InShearRPI == 1)
        {
            // funzione di generazione suono motosega
            bEngineSoundStop=1; // disattiva suono del motore
            ShearSoundGeneration();
            bEngineSoundStop=0; // riattiva suono del motore
        }
        //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
    } 
    //+++++++++++++++++++++++++++++++  FINE CICLO TEST ++++++++++++++++++++++++++++++++++++++++++++
    
    
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //+++++++++++++++++++++++++++++++  INIZIO CICLO OPERATIVO ++++++++++++++++++++++++++++++++++++++++++++
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
    fFreqClacsonSound = 440.0; // frequenza in Hz del tono del Clacson da generare
    fAmpClacsonSound = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
    fDeltaTClacsonSound = 1.0/(fFreqClacsonSound*CLACSONSAMPLENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
    CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmpClacsonSound), (PI/2.0)); // generazione della sinusoide con valori nominali
    //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
       
       
    //+++++++ INIZIO avvio ruomre del motore a frequenza da fermo +++++++++
    fAmpEngineSound = 1.0;  // fissa l'amplificazione per il rumore motore. Valori da 0[min] a 1[max] 
    fFreqEngineSound=nSamplePerSec/nUnderSampleFactor;// campioni per secondo del rumore motore da generare = nSamplePerSec/nUnderSampleFactor
    fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
    nEngineSampleIndex =0; // Avvia indice di generazione suono motore
    SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
    //+++++++ FINE avvio ruomre del motore a frequenza da fermo +++++++++
   
    //inizializza variabili
    nEngineSampleIndex =0; // avvia l'indice di generazione suoni
    nCountRiseEdge=0; // azzera il  contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
    bEngineSoundStop =0; // inizialmente il suono del motore è generato
   
    //++++ INIZIO Ciclo Principale ++++
    while (true) 
    {
        //++++++++++ INIZIO calcola spostamento con encoder sul motore +++++++++++++++++
        // abilita l'interrupt su fronte di salita del segnale di encoder
        nCountRiseEdge=0;
        InEncoderA.enable_irq();
        
        // conta il numero di impulsi del segnale di encoder che si verificano in un timer pari a 500ms
        TimerHall.start();
        nTimerStart=TimerHall.read_ms();
              
        // per 500ms verifica se ci sono impulsi sull'encoder     
        while( (nTimerCurrent-nTimerStart) < 300) // attende il passare di 300ms
        {
            nTimerCurrent=TimerHall.read_ms();
            // pc.printf("CounterTimer= %d\r\n", (nTimerCurrent-nTimerStart));   
        }
        TimerHall.stop();
        InEncoderA.disable_irq();
        //++++++++++ FINE calcola spostamento con encoder sul motore +++++++++++++++++
        
        //++++++++++ INIZIO genera diverso suono con motore fermo e in movimento +++++++++++++++++
        // se nella IRQ sono stati contati fronti di salita del dell'encoder, il robot si sta muovendo         
        if(nCountRiseEdge != 0)
        //if(InDiag1==1)
        {
            // sono stati contati impulsi di encoder, il robot si sta muovendo            
            fDeltaTEngineSound = (0.5/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
            SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
        }
        else
        {
             // se ci sono stati impulsi di encoder, il robot è fermo, genera rumore del motore fermo
            fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
            SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
        }   
        //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++
        
        //++++++++++++++ INIZIO Pilotaggio Motore su comando da Raspberry+++++++++++++
        // se arriva comando di apertura & il cofano è chiuso, muovi motore
        if((InMotorSwitchRPI==1) && (nPosizioneCofano ==0))    
        {    
            //pc.printf("\r\ncofano chiuso & comando di apertura\r\n");
        
            //Ferma motore
            OutMotorA=0;
            OutMotorB=0;
            //pc.printf("Stop motore; OutA OutB = 00\r\n");
            wait_ms(10);
            
            //Ferma motore
            OutMotorA=0;
            OutMotorB=1;
            //pc.printf("Stop motore; OutA OutB = 01\r\n");
            wait_ms(10);
            
            // Ruota Right
            OutMotorA=1;
            OutMotorB=1;
            //pc.printf("Ruota Right; OutA OutB = 11\r\n");
            wait_ms(710);
            
            // Ferma Motore
            OutMotorA=0;
            OutMotorB=1;
            //pc.printf("Stop Motore; OutA OutB = 01\r\n");
            wait_ms(10);
            
            //Ferma motore
            OutMotorA=0;
            OutMotorB=0;
            //pc.printf("Stop motore; OutA OutB = 00\r\n");
            wait_ms(10);
            // cambia posizione del cofano. E' Stato Aperto
            nPosizioneCofano = 1;
        }       
        // se arriva comando di chiusura cofano & il cofano è aperto, muovi motore
        if((myButton==0) && (nPosizioneCofano == 1))
        {
             //pc.printf("\r\nCofano aperto & comando di chiusura\r\n");
                       
            //Ferma motore
            OutMotorA=0;
            OutMotorB=0;
            //pc.printf("Stop motore; OutA OutB = 00\r\n");
            wait_ms(10);
                        
            // Ruota Left
            OutMotorA=1;
            OutMotorB=0;
            //pc.printf("Ruota Left; OutA OutB = 10\r\n");
            wait_ms(730);
            
            //Ferma motore
            OutMotorA=0;
            OutMotorB=0;
            //pc.printf("Stop motore; OutA OutB = 00\r\n");
            wait_ms(10);
            
            // cambia posizione del cofano. E' Stato Chiuso
            nPosizioneCofano = 0;
        }   
        //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
        //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
        if(InLightSwitchRPI ==1)
        {
            // accendi i LED di abbellimento
            //led2=1;
            LedYAD = 1; 
            LedYAS = 1;
            LedRPD = 1;
            LedRPS = 1;
        } 
        else
        {
            
            // spegni i LED di abbellimento
            //led2=0;
            LedYAD = 0; 
            LedYAS = 0;
            LedRPD = 0;
            LedRPS = 0;
             
        }
        //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++
        
        //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
        if(InShearRPI == 1)
        {
            // funzione di generazione suono motosega
            bEngineSoundStop=1; // disattiva suono del motore
            ShearSoundGeneration();
            bEngineSoundStop=0; // riattiva suono del motore
        }
        //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
        
        
        //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++
        // inizializza il valore medio della Luminosità 
        fAvgLight=0.0;
        for(nLightSampleIndex=0; nLightSampleIndex < NUMLIGHTSAMPLE; nLightSampleIndex++)
        {
           // acquisisce dato da ADC
           usReadADC = InWaveLight.read_u16();
           fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
           //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
           fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
           fAvgLight+=fLight;
        }   
        // calcola valore medio su NUMSAMPLE acquisizioni 
        fAvgLight/= NUMLIGHTSAMPLE;
        
        // Accendi/Spegni i LED Bianchi se il valore medio della luminosità è sotto/sopra soglia
        if(fAvgLight > SOGLIALUCIMAX)
        {
           // Accendi LED Bianchi
           //led2 = 1;
           LedWAD = 1;
           LedWAS = 1;
           LedWPD = 1;
           LedWPS = 1;
           
        }
        else
        {
            if(fAvgLight < SOGLIALUCIMIN)
            {
               
               // Spegni LED Bianchi
               //led2 = 0;
               LedWAD = 0;
               LedWAS = 0;
               LedWPD = 0;
               LedWPS = 0;
               
            }
        }    
        
        // invia il dato al PC
        //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
        //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
        
        //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
        //inizializza misura di distanza
        fDistance=0.0;
        // Fissa come Output il pin InOutProxSensor
        InOutProxSensor.output();
        // Poni 'L' sul Pin e mantienilo per qualche microsecondo
        InOutProxSensor.write(0);
        wait_us(5);
        // Poni 'H' sul Pin e mantienilo per qualche microsecondo
        InOutProxSensor.write(1);
        wait_us(10);
        // Poni 'L' sul Pin e mantienilo per qualche microsecondo
        InOutProxSensor.write(0);
        // Attendi assestamento e Fissa come Input il pin InOutProxSensor
        wait_us(5);
        InOutProxSensor.input();
        InOutProxSensor.mode(PullDown); // se non è presente il sensore, il pin rimane a '0'
        
        // attende la risposta del sensore di prossimità per un tempo fissato da TIMEOUTPROXSENSOR. Dopo tale tempo dichiara inesistente il sensore
        TimerProxSensor.start();
        nTimerStart = TimerProxSensor.read_us();
        nTimerTillNow=(TimerProxSensor.read_us()-nTimerStart);
        while((InOutProxSensor ==0) && (nTimerTillNow< TIMEOUTPROXSENSOR))
        {
            nTimerCurrent = TimerProxSensor.read_us();
            nTimerTillNow=nTimerCurrent-nTimerStart;
            led2=1; // se rimane nel while il LED rimane acceso
            pc.printf("sono qui 2 \r\n");
        }
        TimerProxSensor.stop(); // spegne il timer che serve per misurare il timeout quando assente il sensore di prossimità
        pc.printf("\r\nUscita dal while, nTimerTillNow = %d\r\n", nTimerTillNow);
        // se nTimerTillNow è inferiore al TIMEOUT, il sensore è presente e quindi misura la distanza dell'ostacolo
        if(nTimerTillNow < TIMEOUTPROXSENSOR)
        {
            // riattiva il timer per misurare la distanza dell'ostacolo
            TimerProxSensor.start();
            nTimerStart = TimerProxSensor.read_us();
            while(InOutProxSensor == 1)
            {
                led2=1; // se rimane nel while il LED rimane acceso
            }
            TimerProxSensor.stop();
            nTimerStop = TimerProxSensor.read_us();
           
            pc.printf("\r\nSensore Presente, nTimerTillNow = %d\r\n", nTimerTillNow);
        
            // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
            // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
            // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
            fDistance = (nTimerStop-nTimerStart)/58.2;
            // invia il dato al PC
            pc.printf("distanza dell'ostacolo = %f0.2\r\n", fDistance);
        }    
        else
        {
           // quando esce dai while bloccanti, il LED si spegne
           led2=0;
           pc.printf("\r\nTimeOut\r\n");
        }
        //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++ 
        
        
        //++++++++++++++ INIZIO Suona Clacson +++++++++
        //escludi le misure oltre il max
        if((fDistance <= 50.0) && (fDistance >= 3)) 
        {
          // visualizza il valore misurato
          printf("The Distance was %f [cm]\n\r", fDistance);
           
          // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
          if(fDistance < 22)
          {
                // blocca altri suoni quando genera suono del clacson
                bEngineSoundStop=1;
                // INIZIO generazione tono  
                nClacsonSampleIndex=0;
                // Genera il suono del clacson
                for(nClacsonSampleCount=0; nClacsonSampleCount<7000; nClacsonSampleCount++)
                {
                   OutWave.write_u16(usaClacson[nClacsonSampleIndex]); //max 32767
                   //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
                   wait(fDeltaTClacsonSound);
                   // genera ciclicamente
                   nClacsonSampleIndex++;
                   if(nClacsonSampleIndex >= CLACSONSAMPLENUM)
                   {
                       nClacsonSampleIndex=0;
                   }   
                   // a metà genera un wait per doppio clacson
                   if(nClacsonSampleCount == 2000)
                   {
                       wait_ms(100);
                   }
                
                } 
                //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
                OutWave.write_u16(0);
                
                // sblocca altri suoni dopo aver generato suono del clacson
                bEngineSoundStop=0;
                
            } // if(fDistance < soglia) suona clacson
            
        } // if( (fDistance < Max) && (fDistance > Min)) 
        //++++++++++++++ FINE Suona Clacson +++++++++
        wait_ms(100); // se effettuata la misura dai tempo prima di misurare nuovamente    
         
    }
    //++++ FINE Ciclo Principale ++++
}