Diff: RobotFinale3-2.cpp

Revision:

16:17df60afe302

Parent:

15:ef808556423b

--- a/RobotFinale3-2.cpp	Wed Feb 20 21:01:29 2019 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,828 +0,0 @@
-//++++++++++++++++++++ ATTENZIONE ++++++++++++++++++++++++++++++++++++++++++++++++
-// rivedere tutte le giunzioni. su alcune ci sono fili di rame che fuoriescono.
-// rivedere i cavi nei connettori. In alcuni sono moto tesi
-// inserire una ventola per raffreddare transistor e resistenza
-//++++++++++++++++++++ ATTENZIONE ++++++++++++++++++++++++++++++++++++++++++++++++
-
-// mbed specific header files.
-#include "mbed.h"
-
-// include suono del motore
-#include "SampledSoundGurgle.h" // rumore del motore da fermo durante gli spsotamenti
-#include "SampledSoundWelcomeDizione.h" // messaggio di benvenuto
-#include "SampledSoundFarewellDizione.h" // messaggio di Arrivederci
-#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)
-
-// variabile che modula l'amplificazione dei segnali audio. 1= non cambia niente. 0=amplificazione 0;
-#define SOUNDGAIN (1.0)
-
-// 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 del motore di movimento robot
-volatile int nCountRiseEdge; 
-   
-// variabile che ricorda lo stato di StandBy: '0' = Operativo,  '1'=StandBy
-int nStandBy;
-
-// variabile che permette di modificare il Gain di tutti i suoni
-float fSoundGain=SOUNDGAIN; // inizialmente fissato da un define
-    
-// 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
-
-
-// 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 InFutureUse2RPI (PC_15); // usi futuri 1 di comunicazione. Collegato al Raspberry GPIO25
-//DigitalIn InFutureUse1PI (PC_15); // usi futuri 2 di comunicazione. Collegato al Raspberry GPIO12
-DigitalIn InStandByRPI (PB_2,PullDown); // 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
-//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] = fSoundGain * 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(100);
-    LedWAS = 1;
-    wait_ms(100);
-    LedWPD = 1;
-    wait_ms(100);
-    LedWPS = 1;
-    wait_ms(100);
-    LedYAD = 1; 
-    wait_ms(100);
-    LedYAS = 1;
-    wait_ms(100);
-    LedRPD = 1;
-    wait_ms(100);
-    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*fSoundGain);
-        
-        // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
-        //wait(fDeltaTWelcomeSound);
-        wait_us(55);
-    }
-    //++++++++++++ FINE generazione messaggio di benvenuto +++++++++++++++++
-    
-    //++++++++++++ INIZIO Spegni le Luci in sequenza +++++++++++++++++
-    // spegni le Luci in sequenza
-    for(nIndex=0; nIndex<3; nIndex++)
-    {
-        wait_ms(50); 
-        LedWAD = 1;
-        wait_ms(50); 
-        LedWAD = 0;
-    }
-    for(nIndex=0; nIndex<3; nIndex++)
-    {
-        wait_ms(50); 
-        LedWAS = 1;
-        wait_ms(50); 
-        LedWAS = 0;
-    }
-    for(nIndex=0; nIndex<3; nIndex++)
-    {
-        wait_ms(50); 
-        LedWPD = 1;
-        wait_ms(50); 
-        LedWPD = 0;
-    }
-    for(nIndex=0; nIndex<3; nIndex++)
-    {
-        wait_ms(50); 
-        LedWPS = 1;
-        wait_ms(50); 
-        LedWPS = 0;
-    }
-    for(nIndex=0; nIndex<3; nIndex++)
-    {
-        wait_ms(50); 
-        LedYAD = 1;
-        wait_ms(50); 
-        LedYAD =0;
-    }
-    for(nIndex=0; nIndex<3; nIndex++)
-    {
-        wait_ms(50); 
-        LedYAS = 1;
-        wait_ms(50); 
-        LedYAS = 0;
-    }
-    for(nIndex=0; nIndex<3; nIndex++)
-    {
-        wait_ms(50); 
-        LedRPD = 1;
-        wait_ms(50); 
-        LedRPD = 0;
-    }
-    for(nIndex=0; nIndex<3; nIndex++)
-    {
-        wait_ms(50); 
-        LedRPS = 1;
-        wait_ms(50); 
-        LedRPS = 0;
-    }
-    for(nIndex=0; nIndex<3; nIndex++)
-    {
-        wait_ms(50); 
-        LedYRAll = 1;
-        wait_ms(50); 
-        LedYRAll = 0;
-    }
-    //++++++++++++ FINE Spegni le Luci in sequenza +++++++++++++++++
-            
-}
-
-/***************************************************************************/
-/* Genera Messaggio di Arrivederci e spegni i LED quando passa in SyandBy */
-/***************************************************************************/
-void FarewellMessage()
-{
-    // indice per l'array di Farewell message
-    int nFarewellMsgIndex;
-    // parametri per generare il messaggio di Farewell
-    double fAmpFarewellSound;
-    double fFreqFarewellSound;
-    double fDeltaTFarewellSound;
-    
-    
-     
-    //++++++++++++ INIZIO generazione messaggio di Arrivederci +++++++++++++++++ 
-    fAmpFarewellSound = 1.0;  // fissa l'amplificazione per il messaggio di Farewell. Valori da 0[min] a 1[max] 
-    fFreqFarewellSound=nSamplePerSecFarewell/nUnderSampleFactorFarewell;// campioni per secondo del Farewell message da generare = nSamplePerSec/nUnderSampleFactor
-    fDeltaTFarewellSound = (1.0/fFreqFarewellSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
-   
-    
-    for(nFarewellMsgIndex=0; nFarewellMsgIndex < nSampleNumFarewell; nFarewellMsgIndex++)
-    {
-        // mette in output un campione della forma d'onda del Farewell message  moltiplicato per l'amplificazione fAmp
-        OutWave.write_u16(naInputSoundWaveFarewell[nFarewellMsgIndex]*fAmpFarewellSound*fSoundGain);
-        
-        // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
-        //wait(fDeltaTFarewellSound);
-        wait_us(55);
-    }
-    //++++++++++++ FINE generazione messaggio di Arrivederci +++++++++++++++++
-    
-    //++++++++++++ INIZIO Spegni tutti i LED in sequenza +++++++++++++++++
-    // spegni tutti i LED
-    LedWAD = 0;
-    wait_ms(100);
-    LedWAS = 0;
-    wait_ms(100);
-    LedWPD = 0;
-    wait_ms(100);
-    LedWPS = 0;
-    wait_ms(100);
-    LedYAD = 0; 
-    wait_ms(100);
-    LedYAS = 0;
-    wait_ms(100);
-    LedRPD = 0;
-    wait_ms(100);
-    LedRPS = 0;
-    wait_ms(100);
-    LedYRAll = 0;
-    //++++++++++++ FINE Spegni tutti i LED 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*fSoundGain);
-        
-        // 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*fSoundGain);
-        // 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();
-    
-    // definisci il mode del segnale di InStandBy da RPI ('0' = operativo; '1' = StandBy)
-    InStandByRPI.mode(PullDown);
-    
-    // avvia routine di saluto di benvenuto
-    WelcomeMessage();    
-       
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO TEST ++++++++++++++++++++++++++++++++++
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    /*
-    while(true)
-    {
-        if(InStandByRPI == 0)
-        {
-            led2=0;
-            LedYRAll=0;
-        }
-        else
-        {
-            led2=1;
-            LedYRAll =1;
-            FarewellMessage();
-        }
-       
-    }
-    */
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    //+++++++++++++++++++++++++++++++++++++ FINE CICLO TEST ++++++++++++++++++++++++++++++++++
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO OPERATIVO ++++++++++++++++++++++++++++++++++
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    
-    //+++++++++++ inizializza Gain dei suoni +++++++++++++
-    fSoundGain = SOUNDGAIN; // inizialmente fissato a SOUNDGAIN che può essere fissato a 0 per modalità di debug
-    
-    //+++++++++++++ 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*fSoundGain), (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
-    nStandBy=0; // iniazializza la modalità StandBy/Operation del robot. nStandBy=0 : modalità Operation
-    
-    while(true)
-    {
-        if(InStandByRPI == 0)
-        {
-            // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove     
-            InEncoderA.enable_irq();
-            
-            // se appena uscito dalla modalità di StandBy, è ancora nStandBy = 1, emetti messaggio di benvenuto
-            if(nStandBy == 1)
-            {
-                
-                // blocca il suono del motore per emettere messaggio di benvenuto
-                bEngineSoundStop=1;
-                
-                // se modalità StandBy = OFF, riattiva audio;
-                fSoundGain = SOUNDGAIN;
-                
-                
-                //Genera messaggio di benvenuto
-                WelcomeMessage();
-                               
-                // rispristina il suono del motore
-                bEngineSoundStop=0;
-            }
-            
-            // imposta  lo stato di StandBy OFF
-            nStandBy = 0;
-                        
-            //++++++++++ 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
-             
-            }   
-            // riazzera il contatore di impulsi di encoder. Questo contatore viene incrementato nella rouine di interrupt
-            nCountRiseEdge=0;
-            // disabilita interrupt sul segnale di encoder. In questo modo non occupiamo inutilmente la CPU
-            InEncoderA.disable_irq(); // L'interrupt sarà di nuovo abilitato quando si ricomincia il while (true)
-            
-            //++++++++++ 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;
-                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 +++++++++
-        }// if(InStandByRPI == 0)
-        else
-        {
-            
-            // ricevuto da RPI, il comando di StandBy = ON
-            // ricevuto il comando di StandBy (InStandBy == 1)
-            
-            // la prima volta che entra in questo else, la variabile di stato nStandBy è '0'. Solo la prima volta Genera il messaggio di arrivederci
-            if(nStandBy == 0)
-            {
-                // blocca il suono del motore per emettere messaggio di arrivederci
-                bEngineSoundStop=1;
-                
-                //Genera messaggio di arrivederci
-                FarewellMessage();
-                               
-                // rispristina il suono del motore
-                bEngineSoundStop=0;
-                
-                // cambia lo stato dello StandBy
-                nStandBy = 1;
-            }
-            
-            // se modalità StandBy = ON, disattiva audio;
-            fSoundGain = 0.0;
-            
-            
-           
-        }
-    } //while(true)
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    //+++++++++++++++++++++++++++++++  FINE CICLO OPERATIVO +++++++++++++++++++++++++++++++++++++++++++++
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        
-    
-}
-