RobotFinale4.cpp

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