Diff: RobotFinale3.cpp

Revision:

0:74808e8c8d35

diff -r 000000000000 -r 74808e8c8d35 RobotFinale3.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotFinale3.cpp	Thu Feb 14 09:31:28 2019 +0000
@@ -0,0 +1,684 @@
+// mbed specific header files.
+#include "mbed.h"
+
+// include suono del motore
+#include "SampledSoundGurgle.h" // rumore del motore da fermo durante gli spsotamenti
+#include "SampledSoundFarewellDizione.h" // messaggio di arrivederci
+#include "SampledSoundWelcomeDizione.h" // messaggio di benvenuto
+#include "SampledSoundMotosega.h" // rumore durante lo spostamento con Cesoia
+//#include "SampledSoundFarewell.h" // messaggio di arrivederci
+//#include "SampledSoundWelcome.h" // messaggio di benvenuto
+
+//#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)
+
+// Timer per il calcolo dei tempi del sensore di prossimità
+Timer TimerProxSensor;
+
+// ticker per la generazione dell'onda con DAC
+Ticker SampleOutTicker;            
+
+// distanza in cm dell'ostacolo
+double fDistance;
+
+// 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;
+    
+// tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni
+int nTimerStart, nTimerCurrent, nTimerStop, nTimerTillNow;
+
+
+// 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); 
+DigitalOut LedYRAll (PC_7); // COn questo pin si pilotano contemporaneamente i Led: YLD1, YLD2, YLD3, YLD4, YLS1, YLS2, YLS3, YLS4, RPD1, RPS1
+DigitalOut LedYRAllGND (PB_6); // GND di Led. Sempre a '0' per permettere d LedYRAll di accendere e spegnere i LED
+
+
+// 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 InStandByRPI (PC_15); // StandBy ON/OFF. '1' = robot in StandBy; '0' = robot operativo. 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
+
+
+// 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); 
+
+//****************************
+// 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;
+    }
+    for(nIndex=0; nIndex<3; nIndex++)
+    {
+        wait_ms(100); 
+        LedYRAll = 1;
+        wait_ms(100); 
+        LedYRAll = 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()
+{
+    // ogni volta che riceve un fornte di salita sul segnale di encoder del motore, entra in questa routine e incrementa il contatore
+    nCountRiseEdge++;
+}       
+     
+/********/
+/* Main */
+/********/
+int main()
+{
+    // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
+    pc.baud(921600); //921600 bps
+    
+    // inizializza variabili
+    LedYRAllGND =0; // assegna lo '0' al segnale LedYRAllGND = 0, Così sarà il riferimento negativo per LedYRAll
+        
+    // 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 ++++++++++++++++++++++++++++++++++++++++++++
+    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    /*
+    while(true)
+    {
+    }
+    */
+    //+++++++++++++++++++++++++++++++ FINE CICLO TEST +++++++++++++++++++++++++++++++++++++++++++++++
+    
+    
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO PRINCIPALE ++++++++++++++++++++++++++++++++++
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    
+    //+++++++++++++ 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 e meno del min
+        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)) 
+        wait_ms(100); // dai tempo prima di ripetere nuovamente la misuradella distanza    
+        //++++++++++++++ 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(355);
+            
+            // 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(365);
+            
+            //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;
+            LedYRAll = 1;
+        } 
+        else
+        {
+            
+            // spegni i LED di abbellimento
+            //led2=0;
+            LedYAD = 0; 
+            LedYAS = 0;
+            LedRPD = 0;
+            LedRPS = 0;
+            LedYRAll = 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 PRINCIPALE ++++++++++++++++++++++++++++++++++++++++++++
+    
+    
+    
+        
+}
+
+