RobotFinale3-4.cpp

00001 //++++++++++++++++++++ ATTENZIONE ++++++++++++++++++++++++++++++++++++++++++++++++
00002 // rivedere tutte le giunzioni. su alcune ci sono fili di rame che fuoriescono.
00003 // rivedere i cavi nei connettori. In alcuni sono moto tesi
00004 // inserire una ventola per raffreddare transistor e resistenza
00005 //++++++++++++++++++++ ATTENZIONE ++++++++++++++++++++++++++++++++++++++++++++++++
00006 
00007 // mbed specific header files.
00008 #include "mbed.h"
00009 
00010 // include suono del motore
00011 #include "SampledSoundGurgle.h" // rumore del motore da fermo durante gli spsotamenti
00012 #include "SampledSoundWelcomeDizione.h" // messaggio di benvenuto
00013 #include "SampledSoundFarewellDizione.h" // messaggio di Arrivederci
00014 #include "SampledSoundMotosega.h" // rumore durante lo spostamento con Cesoia
00015 
00016 //#include "SampledSoundMotosega.h"
00017 //#include "SampledSoundTrattore.h"
00018  
00019 
00020 // TimeOut in [microsec] per verificare la presenza del sensore prossimità. Se il sensore non è presente il timer supera TIMEOUTPROXSENSOR
00021 #define TIMEOUTPROXSENSOR 1000 //tempo in [microsec]
00022 
00023 // numero di campioni che compongono un periodo della sinusoide in Output sull'ADC
00024 #define CLACSONSAMPLENUM   45 // consigliabile avere  multipli di 45
00025 
00026 // numero di campioni acquisiti su cui effettuare la media di luminosità
00027 #define NUMLIGHTSAMPLE 100
00028 
00029 // Parametri di soglia per la luce. Accendi/spegni Luci se la luminosità scende/sale sotto/sopra  SOGLIALUCIMAX e SOGLIALUCIMIN
00030 #define SOGLIALUCIMAX (1.85)
00031 #define SOGLIALUCIMIN (1.45)
00032 
00033 // parametri dell'onda coseno da generare
00034 #define PI        (3.141592653589793238462)
00035 #define AMPLITUDE 32767 //(1.0)    // x * 3.3V
00036 #define PHASE     (PI/2) // 2*pi è un periodo
00037 #define OFFSET    32767 //(0x7FFF)
00038 
00039 // variabile che modula l'amplificazione dei segnali audio. 1= non cambia niente. 0=amplificazione 0;
00040 #define SOUNDGAIN (1.0)
00041 
00042 // ticker per la generazione dell'onda con DAC
00043 Ticker SampleOutTicker;            
00044 
00045 
00046 // Timer per il calcolo dei tempi del sensore di prossimità
00047 Timer TimerProxSensor;
00048 
00049 // distanza in cm dell'ostacolo
00050 double fDistance;
00051 
00052 
00053 // tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni
00054 int nTimerStart, nTimerCurrent, nTimerStop, nTimerTillNow;
00055 
00056 // Buffer contenente la sinusoide da porre in output come Clacson.
00057 unsigned short usaClacson[CLACSONSAMPLENUM];
00058 
00059 // prototipo di funzione che genera i campioni della sinusoide da utilizzare per la generazione tramite DAC
00060 void CalculateSinewave(void);
00061  
00062 
00063 // Periodo di generazione campioni in output DeltaT = T/NumSample
00064 double fDeltaTClacsonSound;
00065 double fDeltaTEngineSound;
00066 
00067 // amplificazione per i suoni da generare con l'ADC
00068 double fAmpEngineSound; // rumore di Engine 
00069 double fAmpClacsonSound; // rumore di Clacson
00070 double fAmpShearSound; // rumore di Shear
00071 
00072 // frequenza segnale audio da generare per clacson e motore
00073 double fFreqClacsonSound;
00074 double fFreqEngineSound;
00075 
00076 // periodo della sinusoide audio da generare come suono del clacson
00077 double fPeriodClacsonSOund;
00078 
00079 // numero di campioni di clacson già inviati in output sul DAC
00080 int nClacsonSampleCount;
00081 // indice dell'array di generazione campioni clacson 
00082 int nClacsonSampleIndex;
00083 
00084 // indice dell'Array di generazione suoni del motore
00085 volatile int nEngineSampleIndex;
00086 
00087 // Flag che decide se generare oppure no il suono del motore. '1'=non generare il suono del motore, '0'=genera il suono del motore
00088 int bEngineSoundStop;
00089 
00090 
00091 
00092 // valore medio della Luminosità su NUMACQUISIZIONI acquisizioni
00093 double fAvgLight;
00094 
00095 // valore numerico, di tensione e di luce letto dall'ADC
00096 volatile unsigned short usReadADC;
00097 volatile float fReadVoltage;
00098 
00099 // valore di luminosità letto dall'ADC
00100 volatile float fLight;
00101 
00102 // posizione del Cofano '0' = chiuso, '1'=aperto. Inizialmente DEVE essere chiuso (cioè '0')
00103 int nPosizioneCofano=0;
00104 
00105 
00106 // indice per il conteggio dei campioni di luce acquisiti dal fotoresistore
00107 int nLightSampleIndex;
00108    
00109 // timer per il calcolo della velocità
00110 Timer TimerHall;
00111     
00112 // variabile che conta il numero di fronti si salita del segnale encoder del motore di movimento robot
00113 volatile int nCountRiseEdge; 
00114    
00115 // variabile che ricorda lo stato di StandBy: '0' = Operativo,  '1'=StandBy
00116 int nStandBy;
00117 
00118 // variabile che permette di modificare il Gain di tutti i suoni
00119 float fSoundGain=SOUNDGAIN; // inizialmente fissato da un define
00120  
00121 // sensore di prossimità. '1' = Sensore Presente, '0' = Sesnore Assente
00122 int nProximitySensorPresent; 
00123     
00124 // pin di pilotaggio Motore DC
00125 DigitalOut OutMotorA (PB_0);
00126 DigitalOut OutMotorB (PC_1);
00127 
00128 // Output Digitali usati per i LED
00129 DigitalOut LedWAD (PC_2);
00130 DigitalOut LedWAS (PC_3);
00131 DigitalOut LedWPD (PH_0);
00132 DigitalOut LedWPS (PA_0) ;
00133 DigitalOut LedYAD (PC_9); 
00134 DigitalOut LedYAS (PC_8);
00135 DigitalOut LedRPD (PA_13);
00136 DigitalOut LedRPS (PA_14) ; 
00137 DigitalOut LedYRAll (PC_7); // COn questo pin si pilotano contemporaneamente i Led: YLD1, YLD2, YLD3, YLD4, YLS1, YLS2, YLS3, YLS4, RPD1, RPS1
00138 
00139 
00140 // Input/Output Digitali usati per interfaccia RPI
00141 DigitalIn InShearRPI (PB_11, PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17
00142 DigitalIn InLightSwitchRPI (PB_9, PullDown); // accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20
00143 DigitalIn InMotorSwitchRPI (PB_8, PullDown); // accende e spegne il motore del Cofano. Collegato al Raspberry GPIO16
00144 DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione. Collegato al Raspberry GPIO13
00145 DigitalIn InFutureUse2RPI (PC_15); // usi futuri 1 di comunicazione. Collegato al Raspberry GPIO25
00146 //DigitalIn InFutureUse1PI (PC_15); // usi futuri 2 di comunicazione. Collegato al Raspberry GPIO12
00147 DigitalIn InStandByRPI (PB_2,PullDown); // StandBy ON/OFF. '1' = robot in StandBy; '0' = robot operativo. Collegato al Raspberry GPIO12
00148 
00149 // Input e Output per i sensori e attuatori
00150 AnalogOut OutWave(PA_4); // pin A2 di output per la forma d'onda analogica dedicata al suono
00151 AnalogIn InWaveLight(PA_1); // pin A1 di input per la forma d'onda analogica dedicata alla luminosità
00152 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
00153 
00154 InterruptIn InEncoderA(PA_9); // Primo Pin di input dall'encoder ottico collegato al motore per misurare lo spostamento
00155 //InterruptIn InEncoderB(PC_7); // Secondo Pin di input dall'encoder ottico collegato al motore. predisposizione per usi futuri
00156 
00157 // Input/Output utilizzati da funzioni default su scheda NUCLEO
00158 DigitalOut led2(LED2);// LED verde sulla scheda. Associato a PA_5
00159 Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12
00160 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13
00161 
00162 // input di diagnostica
00163 DigitalIn InDiag1(PA_15,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN7 pin17-pin19 
00164 //DigitalIn InDiag2(PB_11,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20 
00165 
00166 
00167 //****************************
00168 // Create the sinewave buffer
00169 //****************************
00170 void CalculateSinewave(int nOffset, int nAmplitude, double fPhase)
00171 {
00172     // variabile contenente l'angolo in radianti
00173     double fRads;
00174     // indici per i cicli
00175     int nIndex;
00176     // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE
00177     double fDeltaF;
00178     // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF
00179     double fAngle;
00180     
00181     fDeltaF = 360.0/CLACSONSAMPLENUM;
00182     for (nIndex = 0; nIndex < CLACSONSAMPLENUM; nIndex++) 
00183     {
00184         fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide
00185         fRads = (PI * fAngle)/180.0; // Convert degree in radian
00186         //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET;
00187         usaClacson[nIndex] = fSoundGain * nAmplitude * cos(fRads + fPhase) + nOffset;
00188     }
00189 }
00190 
00191 /********************************************************/
00192 /* Funzione avviata all'inizio come saluto e Benvenuto  */
00193 /********************************************************/
00194 void WelcomeMessage()
00195 {
00196     // indice per i cicli interni alla funzione
00197     int nIndex;
00198     
00199     // indice per l'array di welcome message
00200     int nWelcomeMsgIndex;
00201     // parametri per generare il messaggio di welcome
00202     double fAmpWelcomeSound;
00203     double fFreqWelcomeSound;
00204     double fDeltaTWelcomeSound;
00205     
00206     //++++++++++++ INIZIO Accendi le Luci in sequenza +++++++++++++++++
00207     // accendi tutte le luci
00208     LedWAD = 1;
00209     wait_ms(100);
00210     LedWAS = 1;
00211     wait_ms(100);
00212     LedWPD = 1;
00213     wait_ms(100);
00214     LedWPS = 1;
00215     wait_ms(100);
00216     LedYAD = 1; 
00217     wait_ms(100);
00218     LedYAS = 1;
00219     wait_ms(100);
00220     LedRPD = 1;
00221     wait_ms(100);
00222     LedRPS = 1;
00223     //++++++++++++ FINE Accendi le Luci in sequenza +++++++++++++++++
00224      
00225     //++++++++++++ INIZIO generazione messaggio di benvenuto +++++++++++++++++ 
00226     fAmpWelcomeSound = 1.0;  // fissa l'amplificazione per il messaggio di welcome. Valori da 0[min] a 1[max] 
00227     fFreqWelcomeSound=nSamplePerSecWelcome/nUnderSampleFactorWelcome;// campioni per secondo del welcome message da generare = nSamplePerSec/nUnderSampleFactor
00228     fDeltaTWelcomeSound = (1.0/fFreqWelcomeSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00229    
00230     
00231     for(nWelcomeMsgIndex=0; nWelcomeMsgIndex < nSampleNumWelcome; nWelcomeMsgIndex++)
00232     {
00233         // mette in output un campione della forma d'onda del welcome message  moltiplicato per l'amplificazione fAmp
00234         OutWave.write_u16(naInputSoundWaveWelcome[nWelcomeMsgIndex]*fAmpWelcomeSound*fSoundGain);
00235         
00236         // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
00237         //wait(fDeltaTWelcomeSound);
00238         wait_us(37);
00239     }
00240     //++++++++++++ FINE generazione messaggio di benvenuto +++++++++++++++++
00241     
00242     //++++++++++++ INIZIO Spegni le Luci in sequenza +++++++++++++++++
00243     // spegni le Luci in sequenza
00244     for(nIndex=0; nIndex<3; nIndex++)
00245     {
00246         wait_ms(50); 
00247         LedWAD = 1;
00248         wait_ms(50); 
00249         LedWAD = 0;
00250     }
00251     for(nIndex=0; nIndex<3; nIndex++)
00252     {
00253         wait_ms(50); 
00254         LedWAS = 1;
00255         wait_ms(50); 
00256         LedWAS = 0;
00257     }
00258     for(nIndex=0; nIndex<3; nIndex++)
00259     {
00260         wait_ms(50); 
00261         LedWPD = 1;
00262         wait_ms(50); 
00263         LedWPD = 0;
00264     }
00265     for(nIndex=0; nIndex<3; nIndex++)
00266     {
00267         wait_ms(50); 
00268         LedWPS = 1;
00269         wait_ms(50); 
00270         LedWPS = 0;
00271     }
00272     for(nIndex=0; nIndex<3; nIndex++)
00273     {
00274         wait_ms(50); 
00275         LedYAD = 1;
00276         wait_ms(50); 
00277         LedYAD =0;
00278     }
00279     for(nIndex=0; nIndex<3; nIndex++)
00280     {
00281         wait_ms(50); 
00282         LedYAS = 1;
00283         wait_ms(50); 
00284         LedYAS = 0;
00285     }
00286     for(nIndex=0; nIndex<3; nIndex++)
00287     {
00288         wait_ms(50); 
00289         LedRPD = 1;
00290         wait_ms(50); 
00291         LedRPD = 0;
00292     }
00293     for(nIndex=0; nIndex<3; nIndex++)
00294     {
00295         wait_ms(50); 
00296         LedRPS = 1;
00297         wait_ms(50); 
00298         LedRPS = 0;
00299     }
00300     for(nIndex=0; nIndex<3; nIndex++)
00301     {
00302         wait_ms(50); 
00303         LedYRAll = 1;
00304         wait_ms(50); 
00305         LedYRAll = 0;
00306     }
00307     //++++++++++++ FINE Spegni le Luci in sequenza +++++++++++++++++
00308             
00309 }
00310 
00311 /***************************************************************************/
00312 /* Genera Messaggio di Arrivederci e spegni i LED quando passa in SyandBy */
00313 /***************************************************************************/
00314 void FarewellMessage()
00315 {
00316     // indice per l'array di Farewell message
00317     int nFarewellMsgIndex;
00318     // parametri per generare il messaggio di Farewell
00319     double fAmpFarewellSound;
00320     double fFreqFarewellSound;
00321     double fDeltaTFarewellSound;
00322     
00323     
00324      
00325     //++++++++++++ INIZIO generazione messaggio di Arrivederci +++++++++++++++++ 
00326     fAmpFarewellSound = 1.0;  // fissa l'amplificazione per il messaggio di Farewell. Valori da 0[min] a 1[max] 
00327     fFreqFarewellSound=nSamplePerSecFarewell/nUnderSampleFactorFarewell;// campioni per secondo del Farewell message da generare = nSamplePerSec/nUnderSampleFactor
00328     fDeltaTFarewellSound = (1.0/fFreqFarewellSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00329    
00330     
00331     for(nFarewellMsgIndex=0; nFarewellMsgIndex < nSampleNumFarewell; nFarewellMsgIndex++)
00332     {
00333         // mette in output un campione della forma d'onda del Farewell message  moltiplicato per l'amplificazione fAmp
00334         OutWave.write_u16(naInputSoundWaveFarewell[nFarewellMsgIndex]*fAmpFarewellSound*fSoundGain);
00335         
00336         // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
00337         //wait(fDeltaTFarewellSound);
00338         wait_us(57);
00339     }
00340     //++++++++++++ FINE generazione messaggio di Arrivederci +++++++++++++++++
00341     
00342     //++++++++++++ INIZIO Spegni tutti i LED in sequenza +++++++++++++++++
00343     // spegni tutti i LED
00344     LedWAD = 0;
00345     wait_ms(100);
00346     LedWAS = 0;
00347     wait_ms(100);
00348     LedWPD = 0;
00349     wait_ms(100);
00350     LedWPS = 0;
00351     wait_ms(100);
00352     LedYAD = 0; 
00353     wait_ms(100);
00354     LedYAS = 0;
00355     wait_ms(100);
00356     LedRPD = 0;
00357     wait_ms(100);
00358     LedRPS = 0;
00359     wait_ms(100);
00360     LedYRAll = 0;
00361     //++++++++++++ FINE Spegni tutti i LED in sequenza +++++++++++++++++
00362             
00363 }
00364 /***********************************************************************/
00365 /* Genera il suono di una motosega.                                    */ 
00366 /* Attivo quando arriva il comando di spostamento Cesoie da Raspberry  */
00367 /***********************************************************************/
00368 void ShearSoundGeneration()
00369 {
00370     // indice per l'array di suono Shear  
00371     int nShearSoundIndex;
00372     // parametri per generare il messaggio di shear
00373     double fAmpShearSound;
00374     double fFreqShearSound;
00375     double fDeltaTShearSound;
00376     
00377     //++++++++++++ INIZIO generazione suono di motosega +++++++++++++++++ 
00378     fAmpShearSound = 1.0;  // fissa l'amplificazione per il suono di Shear. Valori da 0[min] a 1[max] 
00379     fFreqShearSound=nSamplePerSecShear/nUnderSampleFactorShear;// campioni per secondo del Shear da generare = nSamplePerSec/nUnderSampleFactor
00380     fDeltaTShearSound = (1.0/fFreqShearSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00381    
00382     
00383     for(nShearSoundIndex=0; nShearSoundIndex < nSampleNumShear; nShearSoundIndex++)
00384     {
00385         // mette in output un campione della forma d'onda del suono di Shear,  moltiplicato per l'amplificazione fAmp
00386         OutWave.write_u16(naInputSoundWaveShear[nShearSoundIndex]*fAmpShearSound*fSoundGain);
00387         
00388         // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
00389         wait(fDeltaTShearSound);
00390     }
00391     //++++++++++++ FINE generazione suono di motosega +++++++++++++++++
00392     
00393     
00394     
00395 }
00396 /***********************************************************************/
00397 /* generazione suoni con i sample da file di campioni in SoundSample.h */
00398 /***********************************************************************/
00399 void SampleOut() 
00400 {
00401     // interrompi il suono del motore per generare altri suoni. '1' = interrompi i suoni
00402     if(bEngineSoundStop == 0)
00403     {
00404         // mette in output un campione della forma d'onda del rumore motore moltiplicato per l'amplificazione fAmp
00405         OutWave.write_u16(naInputSoundWave[nEngineSampleIndex]*fAmpEngineSound*fSoundGain);
00406         // incrementa l'indice del campione in output, nSampleNum è il numero dei campioni nle file Sound.h
00407         nEngineSampleIndex++;
00408         if(nEngineSampleIndex >= nSampleNum)
00409             nEngineSampleIndex=0;
00410     }        
00411 }
00412 
00413 
00414  /**************************************************************************************/
00415 /* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
00416 /**************************************************************************************/
00417 void riseEncoderIRQ()
00418 {
00419     nCountRiseEdge++;
00420 }       
00421      
00422 /********/
00423 /* Main */
00424 /********/
00425 int main()
00426 {
00427     // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
00428     pc.baud(921600); //921600 bps
00429     
00430     // definisci il mode del segnale digitale di EncoderA
00431     InEncoderA.mode(PullUp);
00432     
00433     // Associa routine di Interrup all'evento fronte di salita del segnale di encoder
00434     InEncoderA.rise(&riseEncoderIRQ);
00435 
00436     // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove     
00437     InEncoderA.enable_irq();
00438     
00439     // definisci il mode del segnale di InStandBy da RPI ('0' = operativo; '1' = StandBy)
00440     InStandByRPI.mode(PullDown);
00441     InShearRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17
00442     InLightSwitchRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando che accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20
00443     InMotorSwitchRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando che accende e spegne il motore del Cofano. Collegato al Raspberry GPIO16
00444      
00445     
00446      
00447     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00448     //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO OPERATIVO ++++++++++++++++++++++++++++++++++
00449     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00450     
00451    
00452     //+++++++++++ inizializza Gain dei suoni +++++++++++++
00453     fSoundGain = SOUNDGAIN; // inizialmente fissato a SOUNDGAIN che può essere fissato a 0 per modalità di debug
00454     
00455     //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
00456     fFreqClacsonSound = 550.0; // frequenza in Hz del tono del Clacson da generare
00457     fAmpClacsonSound = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
00458     fDeltaTClacsonSound = 1.0/(fFreqClacsonSound*CLACSONSAMPLENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
00459     CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmpClacsonSound*fSoundGain), (PI/2.0)); // generazione della sinusoide con valori nominali
00460     //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
00461     
00462     // avvia routine di saluto di benvenuto
00463     bEngineSoundStop = 1; // per generare il messaggio di benvenuto il suono del motore è spento
00464     WelcomeMessage();    
00465     bEngineSoundStop = 0; // riattiva il suono del motore
00466           
00467     //+++++++ INIZIO avvio rumore del motore a frequenza da fermo +++++++++
00468     fAmpEngineSound = 1.0;  // fissa l'amplificazione per il rumore motore. Valori da 0[min] a 1[max] 
00469     fFreqEngineSound=nSamplePerSec/nUnderSampleFactor;// campioni per secondo del rumore motore da generare = nSamplePerSec/nUnderSampleFactor
00470     fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00471     nEngineSampleIndex =0; // Avvia indice di generazione suono motore
00472     SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00473     //+++++++ FINE avvio rumore del motore a frequenza da fermo +++++++++
00474    
00475     //inizializza variabili
00476     nEngineSampleIndex =0; // avvia l'indice di generazione suoni
00477     nCountRiseEdge=0; // azzera il  contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
00478     bEngineSoundStop =0; // inizialmente il suono del motore è generato
00479     nPosizioneCofano=0; // inizializza la posizione del cofano chiuso
00480     nStandBy=0; // iniazializza la modalità StandBy/Operation del robot. nStandBy=0 : modalità Operation
00481     
00482     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00483     //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO TEST ++++++++++++++++++++++++++++++++++
00484     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00485     // Fissa come Output il pin InOutProxSensor
00486     while(true)
00487     {
00488         if(InStandByRPI == 0)
00489         {
00490             // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove     
00491             InEncoderA.enable_irq();
00492             
00493             // se appena uscito dalla modalità di StandBy, è ancora nStandBy = 1, emetti messaggio di benvenuto
00494             if(nStandBy == 1)
00495             {
00496                 
00497                 // blocca il suono del motore per emettere messaggio di benvenuto
00498                 bEngineSoundStop=1;
00499                 
00500                 // se modalità StandBy = OFF, riattiva audio;
00501                 fSoundGain = SOUNDGAIN;
00502                 
00503                 
00504                 //Genera messaggio di benvenuto
00505                 WelcomeMessage();
00506                                
00507                 // rispristina il suono del motore
00508                 bEngineSoundStop=0;
00509             }
00510             
00511             // imposta  lo stato di StandBy OFF
00512             nStandBy = 0;
00513             //++++++++++ INIZIO calcola spostamento con encoder sul motore +++++++++++++++++
00514             // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove     
00515             //InEncoderA.enable_irq();
00516                 
00517             // conta il numero di impulsi del segnale di encoder che si verificano in un timer pari a 500ms
00518             TimerHall.start();
00519             nTimerStart=TimerHall.read_ms();
00520                   
00521             // per 100ms verifica se ci sono impulsi sull'encoder. Gli impulsi vengono contati lungo tutto il ciclo ma se il ciclo è attraversato troppo rapidamente rischio di non contaniente e aggiungo questo ritarda.     
00522             while( (nTimerCurrent-nTimerStart) < 50) // attende il passare di 100ms
00523             {
00524                 nTimerCurrent=TimerHall.read_ms();
00525                 // pc.printf("CounterTimer= %d\r\n", (nTimerCurrent-nTimerStart));   
00526             }
00527             TimerHall.stop();
00528             //InEncoderA.disable_irq();
00529             //++++++++++ FINE calcola spostamento con encoder sul motore +++++++++++++++++      
00530                   
00531             //++++++++++ INIZIO genera diverso suono con motore fermo e in movimento +++++++++++++++++
00532             // se nella IRQ sono stati contati fronti di salita del dell'encoder, il robot si sta muovendo         
00533             if(nCountRiseEdge != 0)
00534             //if(InDiag1==1)
00535             {
00536                 // sono stati contati impulsi di encoder, il robot si sta muovendo            
00537                 fDeltaTEngineSound = (0.5/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00538                 SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00539                 //pc.printf("\r\nIn Movimento \r\n"); //Diagnostica
00540             }
00541             else
00542             {
00543                 // se non ci sono stati impulsi di encoder, il robot è fermo, genera rumore del motore fermo
00544                 fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00545                 SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00546                 //pc.printf("\r\nFermo \r\n"); //Diagnostica
00547             }   
00548             // riazzera il contatore di impulsi di encoder. Questo contatore viene incrementato nella rouine di interrupt
00549             nCountRiseEdge=0;
00550             //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++
00551             
00552             //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++
00553             // inizializza il valore medio della Luminosità 
00554             fAvgLight=0.0;
00555             for(nLightSampleIndex=0; nLightSampleIndex < NUMLIGHTSAMPLE; nLightSampleIndex++)
00556             {
00557                // acquisisce dato da ADC
00558                usReadADC = InWaveLight.read_u16();
00559                fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
00560                //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
00561                fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
00562                fAvgLight+=fLight;
00563             }   
00564             // calcola valore medio su NUMSAMPLE acquisizioni 
00565             fAvgLight/= NUMLIGHTSAMPLE;
00566             
00567             // Accendi/Spegni i LED Bianchi se il valore medio della luminosità è sotto/sopra soglia
00568             if(fAvgLight < SOGLIALUCIMIN)
00569             {
00570                // Accendi LED Bianchi
00571                //led2 = 1;
00572                LedWAD = 1;
00573                LedWAS = 1;
00574                LedWPD = 1;
00575                LedWPS = 1;
00576             }
00577             else
00578             {
00579                 if(fAvgLight > SOGLIALUCIMAX)
00580                 {
00581                    // Spegni LED Bianchi
00582                    //led2 = 0;
00583                    LedWAD = 0;
00584                    LedWAS = 0;
00585                    LedWPD = 0;
00586                    LedWPS = 0;
00587                 }
00588             }
00589             
00590             // invia il dato al PC
00591             //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
00592             //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
00593             
00594             //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
00595             //inizializza misura di distanza
00596             fDistance=0.0;
00597             // Fissa come Output il pin InOutProxSensor
00598             InOutProxSensor.output();
00599             // Poni 'L' sul Pin e mantienilo per qualche microsecondo
00600             InOutProxSensor.write(0);
00601             wait_us(5);
00602             // Poni 'H' sul Pin e mantienilo per qualche microsecondo
00603             InOutProxSensor.write(1);
00604             wait_us(10);
00605             // Poni 'L' sul Pin e mantienilo per qualche microsecondo
00606             InOutProxSensor.write(0);
00607             // Attendi assestamento e Fissa come Input il pin InOutProxSensor
00608             wait_us(5);
00609             InOutProxSensor.input();
00610             InOutProxSensor.mode(PullDown); // se non è presente il sensore, il pin rimane a '0'
00611            
00612             // attende la risposta del sensore di prossimità per un tempo fissato da TIMEOUTPROXSENSOR. Dopo tale tempo dichiara inesistente il sensore
00613             TimerProxSensor.start();
00614             nTimerStart = TimerProxSensor.read_us();
00615             nTimerTillNow=(TimerProxSensor.read_us()-nTimerStart);
00616             while((InOutProxSensor ==0) && (nTimerTillNow< TIMEOUTPROXSENSOR))
00617             {
00618                 nTimerCurrent = TimerProxSensor.read_us();
00619                 nTimerTillNow=nTimerCurrent-nTimerStart;
00620                 led2=1; // se rimane nel while il LED rimane acceso
00621                 //pc.printf("sono qui 2 \r\n"); // Diagnotica
00622             }
00623             TimerProxSensor.stop(); // spegne il timer che serve per misurare il timeout quando assente il sensore di prossimità
00624             //pc.printf("\r\nUscita dal while, nTimerTillNow = %d\r\n", nTimerTillNow); // Diagnostica
00625             // se nTimerTillNow è inferiore al TIMEOUT, il sensore è presente e quindi misura la distanza dell'ostacolo
00626             if(nTimerTillNow < TIMEOUTPROXSENSOR)
00627             {
00628                 // riattiva il timer per misurare la distanza dell'ostacolo
00629                 TimerProxSensor.start();
00630                 nTimerStart = TimerProxSensor.read_us();
00631                 while(InOutProxSensor == 1)
00632                 {
00633                     led2=1; // se rimane nel while il LED rimane acceso
00634                 }
00635                 TimerProxSensor.stop();
00636                 nTimerStop = TimerProxSensor.read_us();
00637                
00638                 //pc.printf("\r\nSensore Presente, nTimerTillNow = %d\r\n", nTimerTillNow); // Diagnostica
00639             
00640                 // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
00641                 // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
00642                 // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
00643                 fDistance = (nTimerStop-nTimerStart)/58.2;
00644                 // invia il dato al PC
00645                 //pc.printf("distanza dell'ostacolo = %f0.2\r\n", fDistance); // Diagnostica
00646             }    
00647             
00648             //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++        
00649             //++++++++++++++ INIZIO Suona Clacson +++++++++
00650             //escludi le misure oltre il max
00651             if(myButton == 0) fDistance = 20; //RIPRISTINA TOGLIENDO QUESTA RIGA
00652             if((fDistance <= 50.0) && (fDistance >= 3)) 
00653             //if(InDiag1 == 1)
00654             {
00655               // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
00656               if(fDistance < 22)
00657               {
00658                     // blocca altri suoni quando genera suono del clacson
00659                     bEngineSoundStop=1;
00660                     // INIZIO generazione tono  
00661                     nClacsonSampleIndex=0;
00662                     // Genera il suono del clacson
00663                     for(nClacsonSampleCount=0; nClacsonSampleCount<7000; nClacsonSampleCount++)
00664                     {
00665                         OutWave.write_u16(usaClacson[nClacsonSampleIndex]); //max 32767
00666                         //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
00667                         
00668                         wait(fDeltaTClacsonSound);
00669                         
00670                         // genera ciclicamente
00671                         nClacsonSampleIndex++;
00672                         if(nClacsonSampleIndex >= CLACSONSAMPLENUM)
00673                         {
00674                            nClacsonSampleIndex=0;
00675                         }   
00676                         // a metà genera un wait per doppio clacson
00677                         if(nClacsonSampleCount == 2000)
00678                         {
00679                            wait_ms(100);
00680                         }
00681                     
00682                     } 
00683                     //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
00684                     OutWave.write_u16(0);
00685                     
00686                     // sblocca altri suoni dopo aver generato suono del clacson
00687                     bEngineSoundStop=0;
00688                     
00689                 } // if(fDistance < soglia) suona clacson
00690                 
00691             } // if( (fDistance < Max) && (fDistance > Min)) 
00692             //++++++++++++++ FINE Suona Clacson +++++++++ 
00693             //++++++++++++++  INIZIO pilotaggio motore cofano +++++++++++++++++++
00694             if((InMotorSwitchRPI==1) && (nPosizioneCofano ==0))    
00695             //if((myButton==1) && (nPosizioneCofano ==0))    
00696             {    
00697                //Ferma motore
00698                 OutMotorA=0;
00699                 OutMotorB=0;
00700                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
00701                 wait_ms(10);
00702                 
00703                 //Ferma motore
00704                 OutMotorA=0;
00705                 OutMotorB=1;
00706                 //pc.printf("Stop motore; OutA OutB = 01\r\n");
00707                 wait_ms(10);
00708                 
00709                 // Ruota Right
00710                 OutMotorA=1;
00711                 OutMotorB=1;
00712                 //pc.printf("Ruota Right; OutA OutB = 11\r\n");
00713                 wait_ms(710);
00714                 
00715                 // Ferma Motore
00716                 OutMotorA=0;
00717                 OutMotorB=1;
00718                 //pc.printf("Stop Motore; OutA OutB = 01\r\n");
00719                 wait_ms(10);
00720                 
00721                 //Ferma motore
00722                 OutMotorA=0;
00723                 OutMotorB=0;
00724                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
00725                 wait_ms(10);
00726                 // cambia posizione del cofano. E' Stato Aperto
00727                 nPosizioneCofano = 1;
00728             }       
00729             // se arriva comando di chiusura cofano & il cofano è aperto, muovi motore
00730             //if((myButton==0) && (nPosizioneCofano == 1))
00731             if((InMotorSwitchRPI==0) && (nPosizioneCofano ==1))   
00732             {
00733                 //pc.printf("\r\nCofano aperto & comando di chiusura\r\n");
00734                                      
00735                 //Ferma motore
00736                 OutMotorA=0;
00737                 OutMotorB=0;
00738                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
00739                 wait_ms(10);
00740                             
00741                 // Ruota Left
00742                 OutMotorA=1;
00743                 OutMotorB=0;
00744                 //pc.printf("Ruota Left; OutA OutB = 10\r\n");
00745                 wait_ms(730);
00746                 
00747                 //Ferma motore
00748                 OutMotorA=0;
00749                 OutMotorB=0;
00750                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
00751                 wait_ms(10);
00752                 
00753                 // cambia posizione del cofano. E' Stato Chiuso
00754                 nPosizioneCofano = 0;
00755             }   
00756             //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
00757             
00758             
00759             
00760             //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
00761             if(InLightSwitchRPI ==1)
00762             {
00763                 // accendi i LED di abbellimento
00764                 //led2=1;
00765                 LedYAD = 1; 
00766                 LedYAS = 1;
00767                 LedRPD = 1;
00768                 LedRPS = 1;
00769                 LedYRAll = 1;
00770             } 
00771             else
00772             {
00773                 
00774                 // spegni i LED di abbellimento
00775                 //led2=0;
00776                 LedYAD = 0; 
00777                 LedYAS = 0;
00778                 LedRPD = 0;
00779                 LedRPS = 0;
00780                 LedYRAll = 0;
00781                  
00782             }
00783             //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++
00784             
00785             //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
00786             if(InShearRPI == 1)
00787             {
00788                 // funzione di generazione suono motosega
00789                 bEngineSoundStop=1; // disattiva suono del motore
00790                 ShearSoundGeneration();
00791                 bEngineSoundStop=0; // riattiva suono del motore
00792             }
00793             //++++++++++++++ FINE Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
00794         }// if(InStandByRPI == 0)
00795         else
00796         {
00797             
00798             // ricevuto da RPI, il comando di StandBy = ON
00799             // ricevuto il comando di StandBy (InStandBy == 1)
00800             
00801             // la prima volta che entra in questo else, la variabile di stato nStandBy è '0'. Solo la prima volta Genera il messaggio di arrivederci
00802             if(nStandBy == 0)
00803             {
00804                 // blocca il suono del motore per emettere messaggio di arrivederci
00805                 bEngineSoundStop=1;
00806                 
00807                 //Genera messaggio di arrivederci
00808                 FarewellMessage();
00809                                
00810                 // rispristina il suono del motore
00811                 bEngineSoundStop=0;
00812                 
00813                 // cambia lo stato dello StandBy
00814                 nStandBy = 1;
00815             }
00816             
00817             // se modalità StandBy = ON, disattiva audio;
00818             fSoundGain = 0.0;
00819         } // fine if(nStandByRPI == 1)
00820         
00821     } // fine ciclo while(true)
00822     
00823     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00824     //+++++++++++++++++++++++++++++++++++++ FINE CICLO TEST ++++++++++++++++++++++++++++++++++
00825     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00826     
00827     
00828     
00829     
00830     
00831     
00832     
00833     while(true)
00834     {
00835         if(InStandByRPI == 0)
00836         {
00837             // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove     
00838             InEncoderA.enable_irq();
00839             
00840             // se appena uscito dalla modalità di StandBy, è ancora nStandBy = 1, emetti messaggio di benvenuto
00841             if(nStandBy == 1)
00842             {
00843                 
00844                 // blocca il suono del motore per emettere messaggio di benvenuto
00845                 bEngineSoundStop=1;
00846                 
00847                 // se modalità StandBy = OFF, riattiva audio;
00848                 fSoundGain = SOUNDGAIN;
00849                 
00850                 
00851                 //Genera messaggio di benvenuto
00852                 WelcomeMessage();
00853                                
00854                 // rispristina il suono del motore
00855                 bEngineSoundStop=0;
00856             }
00857             
00858             // imposta  lo stato di StandBy OFF
00859             nStandBy = 0;
00860             //++++++++++ INIZIO calcola spostamento con encoder sul motore +++++++++++++++++
00861             // abilita l'interrupt su fronte di salita del segnale di encoder
00862             nCountRiseEdge=0;
00863             InEncoderA.enable_irq();
00864             
00865             // conta il numero di impulsi del segnale di encoder che si verificano in un timer pari a 500ms
00866             TimerHall.start();
00867             nTimerStart=TimerHall.read_ms();
00868                   
00869             // per 200ms verifica se ci sono impulsi sull'encoder     
00870             while( (nTimerCurrent-nTimerStart) < 200) // attende il passare di 200ms
00871             {
00872                 nTimerCurrent=TimerHall.read_ms();
00873                 // pc.printf("CounterTimer= %d\r\n", (nTimerCurrent-nTimerStart));   
00874             }
00875             TimerHall.stop();
00876             InEncoderA.disable_irq();
00877             //++++++++++ FINE calcola spostamento con encoder sul motore +++++++++++++++++      
00878                   
00879             //++++++++++ INIZIO genera diverso suono con motore fermo e in movimento +++++++++++++++++
00880             // se nella IRQ sono stati contati fronti di salita del dell'encoder, il robot si sta muovendo         
00881             if(nCountRiseEdge != 0)
00882             //if(InDiag1==1)
00883             {
00884                // sono stati contati impulsi di encoder, il robot si sta muovendo            
00885                fDeltaTEngineSound = (0.5/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00886                SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00887             }
00888             else
00889             {
00890                 // se ci sono stati impulsi di encoder, il robot è fermo, genera rumore del motore fermo
00891                fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00892                SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00893              
00894             }   
00895             // riazzera il contatore di impulsi di encoder. Questo contatore viene incrementato nella rouine di interrupt
00896             nCountRiseEdge=0;
00897             // disabilita interrupt sul segnale di encoder. In questo modo non occupiamo inutilmente la CPU
00898             InEncoderA.disable_irq(); // L'interrupt sarà di nuovo abilitato quando si ricomincia il while (true)
00899             //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++
00900             
00901             //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++
00902             // inizializza il valore medio della Luminosità 
00903             fAvgLight=0.0;
00904             for(nLightSampleIndex=0; nLightSampleIndex < NUMLIGHTSAMPLE; nLightSampleIndex++)
00905             {
00906                // acquisisce dato da ADC
00907                usReadADC = InWaveLight.read_u16();
00908                fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
00909                //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
00910                fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
00911                fAvgLight+=fLight;
00912             }   
00913             // calcola valore medio su NUMSAMPLE acquisizioni 
00914             fAvgLight/= NUMLIGHTSAMPLE;
00915             
00916             // Accendi/Spegni i LED Bianchi se il valore medio della luminosità è sotto/sopra soglia
00917             if(fAvgLight < SOGLIALUCIMIN)
00918             {
00919                // Accendi LED Bianchi
00920                //led2 = 1;
00921                LedWAD = 1;
00922                LedWAS = 1;
00923                LedWPD = 1;
00924                LedWPS = 1;
00925             }
00926             else
00927             {
00928                 if(fAvgLight > SOGLIALUCIMAX)
00929                 {
00930                    // Spegni LED Bianchi
00931                    //led2 = 0;
00932                    LedWAD = 0;
00933                    LedWAS = 0;
00934                    LedWPD = 0;
00935                    LedWPS = 0;
00936                 }
00937             }
00938             
00939             // invia il dato al PC
00940             //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
00941             //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
00942            
00943             //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
00944             //inizializza misura di distanza
00945             fDistance=0.0;
00946             // Fissa come Output il pin InOutProxSensor
00947             InOutProxSensor.output();
00948             // Poni 'L' sul Pin e mantienilo per qualche microsecondo
00949             InOutProxSensor.write(0);
00950             wait_us(5);
00951             // Poni 'H' sul Pin e mantienilo per qualche microsecondo
00952             InOutProxSensor.write(1);
00953             wait_us(10);
00954             // Poni 'L' sul Pin e mantienilo per qualche microsecondo
00955             InOutProxSensor.write(0);
00956             // Attendi assestamento e Fissa come Input il pin InOutProxSensor
00957             wait_us(5);
00958             InOutProxSensor.input();
00959             InOutProxSensor.mode(PullDown); // se non è presente il sensore, il pin rimane a '0'
00960             
00961             // attende la risposta del sensore di prossimità per un tempo fissato da TIMEOUTPROXSENSOR. Dopo tale tempo dichiara inesistente il sensore
00962             TimerProxSensor.start();
00963             nTimerStart = TimerProxSensor.read_us();
00964             nTimerTillNow=(TimerProxSensor.read_us()-nTimerStart);
00965             while((InOutProxSensor ==0) && (nTimerTillNow< TIMEOUTPROXSENSOR))
00966             {
00967                 nTimerCurrent = TimerProxSensor.read_us();
00968                 nTimerTillNow=nTimerCurrent-nTimerStart;
00969                 led2=1; // se rimane nel while il LED rimane acceso
00970                 pc.printf("sono qui 2 \r\n");
00971             }
00972             TimerProxSensor.stop(); // spegne il timer che serve per misurare il timeout quando assente il sensore di prossimità
00973             pc.printf("\r\nUscita dal while, nTimerTillNow = %d\r\n", nTimerTillNow);
00974             // se nTimerTillNow è inferiore al TIMEOUT, il sensore è presente e quindi misura la distanza dell'ostacolo
00975             if(nTimerTillNow < TIMEOUTPROXSENSOR)
00976             {
00977                 // riattiva il timer per misurare la distanza dell'ostacolo
00978                 TimerProxSensor.start();
00979                 nTimerStart = TimerProxSensor.read_us();
00980                 while(InOutProxSensor == 1)
00981                 {
00982                     led2=1; // se rimane nel while il LED rimane acceso
00983                 }
00984                 TimerProxSensor.stop();
00985                 nTimerStop = TimerProxSensor.read_us();
00986                
00987                 pc.printf("\r\nSensore Presente, nTimerTillNow = %d\r\n", nTimerTillNow);
00988             
00989                 // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
00990                 // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
00991                 // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
00992                 fDistance = (nTimerStop-nTimerStart)/58.2;
00993                 // invia il dato al PC
00994                 pc.printf("distanza dell'ostacolo = %f0.2\r\n", fDistance);
00995             }    
00996             else
00997             {
00998                // quando esce dai while bloccanti, il LED si spegne
00999                led2=0;
01000                pc.printf("\r\nTimeOut\r\n");
01001             }
01002             //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++ 
01003            //++++++++++++++ INIZIO Suona Clacson +++++++++
01004             //escludi le misure oltre il max
01005             if((fDistance <= 50.0) && (fDistance >= 3)) 
01006             //if(InDiag1 == 1)
01007             {
01008               // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
01009               if(fDistance < 22)
01010               {
01011                     // blocca altri suoni quando genera suono del clacson
01012                     bEngineSoundStop=1;
01013                     // INIZIO generazione tono  
01014                     nClacsonSampleIndex=0;
01015                     // Genera il suono del clacson
01016                     for(nClacsonSampleCount=0; nClacsonSampleCount<7000; nClacsonSampleCount++)
01017                     {
01018                        OutWave.write_u16(usaClacson[nClacsonSampleIndex]); //max 32767
01019                        //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
01020                        wait(fDeltaTClacsonSound);
01021                        // genera ciclicamente
01022                        nClacsonSampleIndex++;
01023                        if(nClacsonSampleIndex >= CLACSONSAMPLENUM)
01024                        {
01025                            nClacsonSampleIndex=0;
01026                        }   
01027                        // a metà genera un wait per doppio clacson
01028                        if(nClacsonSampleCount == 2000)
01029                        {
01030                            wait_ms(100);
01031                        }
01032                     
01033                     } 
01034                     //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
01035                     OutWave.write_u16(0);
01036                     
01037                     // sblocca altri suoni dopo aver generato suono del clacson
01038                     bEngineSoundStop=0;
01039                     
01040                 } // if(fDistance < soglia) suona clacson
01041                 
01042             } // if( (fDistance < Max) && (fDistance > Min)) 
01043             //++++++++++++++ FINE Suona Clacson +++++++++ 
01044             
01045             
01046             
01047             //++++++++++++++  INIZIO pilotaggio motore cofano +++++++++++++++++++
01048             if((InMotorSwitchRPI==1) && (nPosizioneCofano ==0))    
01049             //if((myButton==1) && (nPosizioneCofano ==0))    
01050             {    
01051                //Ferma motore
01052                 OutMotorA=0;
01053                 OutMotorB=0;
01054                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
01055                 wait_ms(10);
01056                 
01057                 //Ferma motore
01058                 OutMotorA=0;
01059                 OutMotorB=1;
01060                 //pc.printf("Stop motore; OutA OutB = 01\r\n");
01061                 wait_ms(10);
01062                 
01063                 // Ruota Right
01064                 OutMotorA=1;
01065                 OutMotorB=1;
01066                 //pc.printf("Ruota Right; OutA OutB = 11\r\n");
01067                 wait_ms(710);
01068                 
01069                 // Ferma Motore
01070                 OutMotorA=0;
01071                 OutMotorB=1;
01072                 //pc.printf("Stop Motore; OutA OutB = 01\r\n");
01073                 wait_ms(10);
01074                 
01075                 //Ferma motore
01076                 OutMotorA=0;
01077                 OutMotorB=0;
01078                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
01079                 wait_ms(10);
01080                 // cambia posizione del cofano. E' Stato Aperto
01081                 nPosizioneCofano = 1;
01082             }       
01083             // se arriva comando di chiusura cofano & il cofano è aperto, muovi motore
01084             //if((myButton==0) && (nPosizioneCofano == 1))
01085             if((InMotorSwitchRPI==0) && (nPosizioneCofano ==1))   
01086             {
01087                 //pc.printf("\r\nCofano aperto & comando di chiusura\r\n");
01088                                      
01089                 //Ferma motore
01090                 OutMotorA=0;
01091                 OutMotorB=0;
01092                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
01093                 wait_ms(10);
01094                             
01095                 // Ruota Left
01096                 OutMotorA=1;
01097                 OutMotorB=0;
01098                 //pc.printf("Ruota Left; OutA OutB = 10\r\n");
01099                 wait_ms(730);
01100                 
01101                 //Ferma motore
01102                 OutMotorA=0;
01103                 OutMotorB=0;
01104                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
01105                 wait_ms(10);
01106                 
01107                 // cambia posizione del cofano. E' Stato Chiuso
01108                 nPosizioneCofano = 0;
01109             }   
01110             //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
01111             
01112             
01113             
01114             //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
01115             if(InLightSwitchRPI ==1)
01116             {
01117                 // accendi i LED di abbellimento
01118                 //led2=1;
01119                 LedYAD = 1; 
01120                 LedYAS = 1;
01121                 LedRPD = 1;
01122                 LedRPS = 1;
01123                 LedYRAll = 1;
01124             } 
01125             else
01126             {
01127                 
01128                 // spegni i LED di abbellimento
01129                 //led2=0;
01130                 LedYAD = 0; 
01131                 LedYAS = 0;
01132                 LedRPD = 0;
01133                 LedRPS = 0;
01134                 LedYRAll = 0;
01135                  
01136             }
01137             //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++
01138             
01139             //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
01140             if(InShearRPI == 1)
01141             {
01142                 // funzione di generazione suono motosega
01143                 bEngineSoundStop=1; // disattiva suono del motore
01144                 ShearSoundGeneration();
01145                 bEngineSoundStop=0; // riattiva suono del motore
01146             }
01147             //++++++++++++++ FINE Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
01148         }// if(InStandByRPI == 0)
01149         else
01150         {
01151             
01152             // ricevuto da RPI, il comando di StandBy = ON
01153             // ricevuto il comando di StandBy (InStandBy == 1)
01154             
01155             // la prima volta che entra in questo else, la variabile di stato nStandBy è '0'. Solo la prima volta Genera il messaggio di arrivederci
01156             if(nStandBy == 0)
01157             {
01158                 // blocca il suono del motore per emettere messaggio di arrivederci
01159                 bEngineSoundStop=1;
01160                 
01161                 //Genera messaggio di arrivederci
01162                 FarewellMessage();
01163                                
01164                 // rispristina il suono del motore
01165                 bEngineSoundStop=0;
01166                 
01167                 // cambia lo stato dello StandBy
01168                 nStandBy = 1;
01169             }
01170             
01171             // se modalità StandBy = ON, disattiva audio;
01172             fSoundGain = 0.0;
01173             
01174             
01175            
01176         }
01177     } //while(true)
01178     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
01179     //+++++++++++++++++++++++++++++++  FINE CICLO OPERATIVO +++++++++++++++++++++++++++++++++++++++++++++
01180     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
01181         
01182     
01183 }
01184