RobotFinale3-2.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 // pin di pilotaggio Motore DC
00122 DigitalOut OutMotorA (PB_0);
00123 DigitalOut OutMotorB (PC_1);
00124 
00125 // Output Digitali usati per i LED
00126 DigitalOut LedWAD (PC_2);
00127 DigitalOut LedWAS (PC_3);
00128 DigitalOut LedWPD (PH_0);
00129 DigitalOut LedWPS (PA_0) ;
00130 DigitalOut LedYAD (PC_9); 
00131 DigitalOut LedYAS (PC_8);
00132 DigitalOut LedRPD (PA_13);
00133 DigitalOut LedRPS (PA_14) ; 
00134 DigitalOut LedYRAll (PC_7); // COn questo pin si pilotano contemporaneamente i Led: YLD1, YLD2, YLD3, YLD4, YLS1, YLS2, YLS3, YLS4, RPD1, RPS1
00135 
00136 
00137 // Input/Output Digitali usati per interfaccia RPI
00138 DigitalIn InShearRPI (PB_11); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17
00139 DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20
00140 DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore. Collegato al Raspberry GPIO16
00141 DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione. Collegato al Raspberry GPIO13
00142 DigitalIn InFutureUse2RPI (PC_15); // usi futuri 1 di comunicazione. Collegato al Raspberry GPIO25
00143 //DigitalIn InFutureUse1PI (PC_15); // usi futuri 2 di comunicazione. Collegato al Raspberry GPIO12
00144 DigitalIn InStandByRPI (PB_2,PullDown); // StandBy ON/OFF. '1' = robot in StandBy; '0' = robot operativo. Collegato al Raspberry GPIO12
00145 
00146 // Input e Output per i sensori e attuatori
00147 AnalogOut OutWave(PA_4); // pin A2 di output per la forma d'onda analogica dedicata al suono
00148 AnalogIn InWaveLight(PA_1); // pin A1 di input per la forma d'onda analogica dedicata alla luminosità
00149 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
00150 InterruptIn InEncoderA(PA_9); // Primo Pin di input dall'encoder ottico collegato al motore per misurare lo spostamento
00151 //InterruptIn InEncoderB(PC_7); // Secondo Pin di input dall'encoder ottico collegato al motore. predisposizione per usi futuri
00152 
00153 // Input/Output utilizzati da funzioni default su scheda NUCLEO
00154 DigitalOut led2(LED2);// LED verde sulla scheda. Associato a PA_5
00155 Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12
00156 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13
00157 
00158 // input di diagnostica
00159 DigitalIn InDiag1(PA_15,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN7 pin17-pin19 
00160 //DigitalIn InDiag2(PB_11,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20 
00161 
00162 
00163 //****************************
00164 // Create the sinewave buffer
00165 //****************************
00166 void CalculateSinewave(int nOffset, int nAmplitude, double fPhase)
00167 {
00168     // variabile contenente l'angolo in radianti
00169     double fRads;
00170     // indici per i cicli
00171     int nIndex;
00172     // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE
00173     double fDeltaF;
00174     // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF
00175     double fAngle;
00176     
00177     fDeltaF = 360.0/CLACSONSAMPLENUM;
00178     for (nIndex = 0; nIndex < CLACSONSAMPLENUM; nIndex++) 
00179     {
00180         fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide
00181         fRads = (PI * fAngle)/180.0; // Convert degree in radian
00182         //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET;
00183         usaClacson[nIndex] = fSoundGain * nAmplitude * cos(fRads + fPhase) + nOffset;
00184     }
00185 }
00186 
00187 /********************************************************/
00188 /* Funzione avviata all'inizio come saluto e Benvenuto  */
00189 /********************************************************/
00190 void WelcomeMessage()
00191 {
00192     // indice per i cicli interni alla funzione
00193     int nIndex;
00194     
00195     // indice per l'array di welcome message
00196     int nWelcomeMsgIndex;
00197     // parametri per generare il messaggio di welcome
00198     double fAmpWelcomeSound;
00199     double fFreqWelcomeSound;
00200     double fDeltaTWelcomeSound;
00201     
00202     //++++++++++++ INIZIO Accendi le Luci in sequenza +++++++++++++++++
00203     // accendi tutte le luci
00204     LedWAD = 1;
00205     wait_ms(100);
00206     LedWAS = 1;
00207     wait_ms(100);
00208     LedWPD = 1;
00209     wait_ms(100);
00210     LedWPS = 1;
00211     wait_ms(100);
00212     LedYAD = 1; 
00213     wait_ms(100);
00214     LedYAS = 1;
00215     wait_ms(100);
00216     LedRPD = 1;
00217     wait_ms(100);
00218     LedRPS = 1;
00219     //++++++++++++ FINE Accendi le Luci in sequenza +++++++++++++++++
00220      
00221     //++++++++++++ INIZIO generazione messaggio di benvenuto +++++++++++++++++ 
00222     fAmpWelcomeSound = 1.0;  // fissa l'amplificazione per il messaggio di welcome. Valori da 0[min] a 1[max] 
00223     fFreqWelcomeSound=nSamplePerSecWelcome/nUnderSampleFactorWelcome;// campioni per secondo del welcome message da generare = nSamplePerSec/nUnderSampleFactor
00224     fDeltaTWelcomeSound = (1.0/fFreqWelcomeSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00225    
00226     
00227     for(nWelcomeMsgIndex=0; nWelcomeMsgIndex < nSampleNumWelcome; nWelcomeMsgIndex++)
00228     {
00229         // mette in output un campione della forma d'onda del welcome message  moltiplicato per l'amplificazione fAmp
00230         OutWave.write_u16(naInputSoundWaveWelcome[nWelcomeMsgIndex]*fAmpWelcomeSound*fSoundGain);
00231         
00232         // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
00233         //wait(fDeltaTWelcomeSound);
00234         wait_us(55);
00235     }
00236     //++++++++++++ FINE generazione messaggio di benvenuto +++++++++++++++++
00237     
00238     //++++++++++++ INIZIO Spegni le Luci in sequenza +++++++++++++++++
00239     // spegni le Luci in sequenza
00240     for(nIndex=0; nIndex<3; nIndex++)
00241     {
00242         wait_ms(50); 
00243         LedWAD = 1;
00244         wait_ms(50); 
00245         LedWAD = 0;
00246     }
00247     for(nIndex=0; nIndex<3; nIndex++)
00248     {
00249         wait_ms(50); 
00250         LedWAS = 1;
00251         wait_ms(50); 
00252         LedWAS = 0;
00253     }
00254     for(nIndex=0; nIndex<3; nIndex++)
00255     {
00256         wait_ms(50); 
00257         LedWPD = 1;
00258         wait_ms(50); 
00259         LedWPD = 0;
00260     }
00261     for(nIndex=0; nIndex<3; nIndex++)
00262     {
00263         wait_ms(50); 
00264         LedWPS = 1;
00265         wait_ms(50); 
00266         LedWPS = 0;
00267     }
00268     for(nIndex=0; nIndex<3; nIndex++)
00269     {
00270         wait_ms(50); 
00271         LedYAD = 1;
00272         wait_ms(50); 
00273         LedYAD =0;
00274     }
00275     for(nIndex=0; nIndex<3; nIndex++)
00276     {
00277         wait_ms(50); 
00278         LedYAS = 1;
00279         wait_ms(50); 
00280         LedYAS = 0;
00281     }
00282     for(nIndex=0; nIndex<3; nIndex++)
00283     {
00284         wait_ms(50); 
00285         LedRPD = 1;
00286         wait_ms(50); 
00287         LedRPD = 0;
00288     }
00289     for(nIndex=0; nIndex<3; nIndex++)
00290     {
00291         wait_ms(50); 
00292         LedRPS = 1;
00293         wait_ms(50); 
00294         LedRPS = 0;
00295     }
00296     for(nIndex=0; nIndex<3; nIndex++)
00297     {
00298         wait_ms(50); 
00299         LedYRAll = 1;
00300         wait_ms(50); 
00301         LedYRAll = 0;
00302     }
00303     //++++++++++++ FINE Spegni le Luci in sequenza +++++++++++++++++
00304             
00305 }
00306 
00307 /***************************************************************************/
00308 /* Genera Messaggio di Arrivederci e spegni i LED quando passa in SyandBy */
00309 /***************************************************************************/
00310 void FarewellMessage()
00311 {
00312     // indice per l'array di Farewell message
00313     int nFarewellMsgIndex;
00314     // parametri per generare il messaggio di Farewell
00315     double fAmpFarewellSound;
00316     double fFreqFarewellSound;
00317     double fDeltaTFarewellSound;
00318     
00319     
00320      
00321     //++++++++++++ INIZIO generazione messaggio di Arrivederci +++++++++++++++++ 
00322     fAmpFarewellSound = 1.0;  // fissa l'amplificazione per il messaggio di Farewell. Valori da 0[min] a 1[max] 
00323     fFreqFarewellSound=nSamplePerSecFarewell/nUnderSampleFactorFarewell;// campioni per secondo del Farewell message da generare = nSamplePerSec/nUnderSampleFactor
00324     fDeltaTFarewellSound = (1.0/fFreqFarewellSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00325    
00326     
00327     for(nFarewellMsgIndex=0; nFarewellMsgIndex < nSampleNumFarewell; nFarewellMsgIndex++)
00328     {
00329         // mette in output un campione della forma d'onda del Farewell message  moltiplicato per l'amplificazione fAmp
00330         OutWave.write_u16(naInputSoundWaveFarewell[nFarewellMsgIndex]*fAmpFarewellSound*fSoundGain);
00331         
00332         // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
00333         //wait(fDeltaTFarewellSound);
00334         wait_us(55);
00335     }
00336     //++++++++++++ FINE generazione messaggio di Arrivederci +++++++++++++++++
00337     
00338     //++++++++++++ INIZIO Spegni tutti i LED in sequenza +++++++++++++++++
00339     // spegni tutti i LED
00340     LedWAD = 0;
00341     wait_ms(100);
00342     LedWAS = 0;
00343     wait_ms(100);
00344     LedWPD = 0;
00345     wait_ms(100);
00346     LedWPS = 0;
00347     wait_ms(100);
00348     LedYAD = 0; 
00349     wait_ms(100);
00350     LedYAS = 0;
00351     wait_ms(100);
00352     LedRPD = 0;
00353     wait_ms(100);
00354     LedRPS = 0;
00355     wait_ms(100);
00356     LedYRAll = 0;
00357     //++++++++++++ FINE Spegni tutti i LED in sequenza +++++++++++++++++
00358             
00359 }
00360 /***********************************************************************/
00361 /* Genera il suono di una motosega.                                    */ 
00362 /* Attivo quando arriva il comando di spostamento Cesoie da Raspberry  */
00363 /***********************************************************************/
00364 void ShearSoundGeneration()
00365 {
00366     // indice per l'array di suono Shear  
00367     int nShearSoundIndex;
00368     // parametri per generare il messaggio di shear
00369     double fAmpShearSound;
00370     double fFreqShearSound;
00371     double fDeltaTShearSound;
00372     
00373     //++++++++++++ INIZIO generazione suono di motosega +++++++++++++++++ 
00374     fAmpShearSound = 1.0;  // fissa l'amplificazione per il suono di Shear. Valori da 0[min] a 1[max] 
00375     fFreqShearSound=nSamplePerSecShear/nUnderSampleFactorShear;// campioni per secondo del Shear da generare = nSamplePerSec/nUnderSampleFactor
00376     fDeltaTShearSound = (1.0/fFreqShearSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00377    
00378     
00379     for(nShearSoundIndex=0; nShearSoundIndex < nSampleNumShear; nShearSoundIndex++)
00380     {
00381         // mette in output un campione della forma d'onda del suono di Shear,  moltiplicato per l'amplificazione fAmp
00382         OutWave.write_u16(naInputSoundWaveShear[nShearSoundIndex]*fAmpShearSound*fSoundGain);
00383         
00384         // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
00385         wait(fDeltaTShearSound);
00386     }
00387     //++++++++++++ FINE generazione suono di motosega +++++++++++++++++
00388     
00389     
00390     
00391 }
00392 /***********************************************************************/
00393 /* generazione suoni con i sample da file di campioni in SoundSample.h */
00394 /***********************************************************************/
00395 void SampleOut() 
00396 {
00397     // interrompi il suono del motore per generare altri suoni. '1' = interrompi i suoni
00398     if(bEngineSoundStop == 0)
00399     {
00400         // mette in output un campione della forma d'onda del rumore motore moltiplicato per l'amplificazione fAmp
00401         OutWave.write_u16(naInputSoundWave[nEngineSampleIndex]*fAmpEngineSound*fSoundGain);
00402         // incrementa l'indice del campione in output, nSampleNum è il numero dei campioni nle file Sound.h
00403         nEngineSampleIndex++;
00404         if(nEngineSampleIndex >= nSampleNum)
00405             nEngineSampleIndex=0;
00406     }        
00407 }
00408 
00409 
00410  /**************************************************************************************/
00411 /* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
00412 /**************************************************************************************/
00413 void riseEncoderIRQ()
00414 {
00415     nCountRiseEdge++;
00416 }       
00417      
00418 /********/
00419 /* Main */
00420 /********/
00421 int main()
00422 {
00423     // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
00424     pc.baud(921600); //921600 bps
00425     
00426     // definisci il mode del segnale digitale di EncoderA
00427     InEncoderA.mode(PullUp);
00428     
00429     // Associa routine di Interrup all'evento fronte di salita del segnale di encoder
00430     InEncoderA.rise(&riseEncoderIRQ);
00431 
00432     // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove     
00433     //InEncoderA.enable_irq();
00434     
00435     // definisci il mode del segnale di InStandBy da RPI ('0' = operativo; '1' = StandBy)
00436     InStandByRPI.mode(PullDown);
00437     
00438     // avvia routine di saluto di benvenuto
00439     WelcomeMessage();    
00440        
00441     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00442     //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO TEST ++++++++++++++++++++++++++++++++++
00443     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00444     /*
00445     while(true)
00446     {
00447         if(InStandByRPI == 0)
00448         {
00449             led2=0;
00450             LedYRAll=0;
00451         }
00452         else
00453         {
00454             led2=1;
00455             LedYRAll =1;
00456             FarewellMessage();
00457         }
00458        
00459     }
00460     */
00461     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00462     //+++++++++++++++++++++++++++++++++++++ FINE CICLO TEST ++++++++++++++++++++++++++++++++++
00463     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00464     
00465     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00466     //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO OPERATIVO ++++++++++++++++++++++++++++++++++
00467     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00468     
00469     //+++++++++++ inizializza Gain dei suoni +++++++++++++
00470     fSoundGain = SOUNDGAIN; // inizialmente fissato a SOUNDGAIN che può essere fissato a 0 per modalità di debug
00471     
00472     //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
00473     fFreqClacsonSound = 440.0; // frequenza in Hz del tono del Clacson da generare
00474     fAmpClacsonSound = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
00475     fDeltaTClacsonSound = 1.0/(fFreqClacsonSound*CLACSONSAMPLENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
00476     CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmpClacsonSound*fSoundGain), (PI/2.0)); // generazione della sinusoide con valori nominali
00477     //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
00478           
00479     //+++++++ INIZIO avvio rumore del motore a frequenza da fermo +++++++++
00480     fAmpEngineSound = 1.0;  // fissa l'amplificazione per il rumore motore. Valori da 0[min] a 1[max] 
00481     fFreqEngineSound=nSamplePerSec/nUnderSampleFactor;// campioni per secondo del rumore motore da generare = nSamplePerSec/nUnderSampleFactor
00482     fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00483     nEngineSampleIndex =0; // Avvia indice di generazione suono motore
00484     SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00485     //+++++++ FINE avvio ruomre del motore a frequenza da fermo +++++++++
00486    
00487     //inizializza variabili
00488     nEngineSampleIndex =0; // avvia l'indice di generazione suoni
00489     nCountRiseEdge=0; // azzera il  contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
00490     bEngineSoundStop =0; // inizialmente il suono del motore è generato
00491     nPosizioneCofano=0; // inizializza la posizione del cofano chiuso
00492     nStandBy=0; // iniazializza la modalità StandBy/Operation del robot. nStandBy=0 : modalità Operation
00493     
00494     while(true)
00495     {
00496         if(InStandByRPI == 0)
00497         {
00498             // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove     
00499             InEncoderA.enable_irq();
00500             
00501             // se appena uscito dalla modalità di StandBy, è ancora nStandBy = 1, emetti messaggio di benvenuto
00502             if(nStandBy == 1)
00503             {
00504                 
00505                 // blocca il suono del motore per emettere messaggio di benvenuto
00506                 bEngineSoundStop=1;
00507                 
00508                 // se modalità StandBy = OFF, riattiva audio;
00509                 fSoundGain = SOUNDGAIN;
00510                 
00511                 
00512                 //Genera messaggio di benvenuto
00513                 WelcomeMessage();
00514                                
00515                 // rispristina il suono del motore
00516                 bEngineSoundStop=0;
00517             }
00518             
00519             // imposta  lo stato di StandBy OFF
00520             nStandBy = 0;
00521                         
00522             //++++++++++ INIZIO genera diverso suono con motore fermo e in movimento +++++++++++++++++
00523             // se nella IRQ sono stati contati fronti di salita del dell'encoder, il robot si sta muovendo         
00524             if(nCountRiseEdge != 0)
00525             //if(InDiag1==1)
00526             {
00527                // sono stati contati impulsi di encoder, il robot si sta muovendo            
00528                fDeltaTEngineSound = (0.5/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00529                SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00530             }
00531             else
00532             {
00533                 // se ci sono stati impulsi di encoder, il robot è fermo, genera rumore del motore fermo
00534                fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00535                SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00536              
00537             }   
00538             // riazzera il contatore di impulsi di encoder. Questo contatore viene incrementato nella rouine di interrupt
00539             nCountRiseEdge=0;
00540             // disabilita interrupt sul segnale di encoder. In questo modo non occupiamo inutilmente la CPU
00541             InEncoderA.disable_irq(); // L'interrupt sarà di nuovo abilitato quando si ricomincia il while (true)
00542             
00543             //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++
00544             
00545             //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++
00546             // inizializza il valore medio della Luminosità 
00547             fAvgLight=0.0;
00548             for(nLightSampleIndex=0; nLightSampleIndex < NUMLIGHTSAMPLE; nLightSampleIndex++)
00549             {
00550                // acquisisce dato da ADC
00551                usReadADC = InWaveLight.read_u16();
00552                fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
00553                //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
00554                fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
00555                fAvgLight+=fLight;
00556             }   
00557             // calcola valore medio su NUMSAMPLE acquisizioni 
00558             fAvgLight/= NUMLIGHTSAMPLE;
00559             
00560             // Accendi/Spegni i LED Bianchi se il valore medio della luminosità è sotto/sopra soglia
00561             if(fAvgLight < SOGLIALUCIMIN)
00562             {
00563                // Accendi LED Bianchi
00564                //led2 = 1;
00565                LedWAD = 1;
00566                LedWAS = 1;
00567                LedWPD = 1;
00568                LedWPS = 1;
00569             }
00570             else
00571             {
00572                 if(fAvgLight > SOGLIALUCIMAX)
00573                 {
00574                    // Spegni LED Bianchi
00575                    //led2 = 0;
00576                    LedWAD = 0;
00577                    LedWAS = 0;
00578                    LedWPD = 0;
00579                    LedWPS = 0;
00580                 }
00581             }
00582             
00583             // invia il dato al PC
00584             //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
00585             //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
00586            
00587            //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
00588             //inizializza misura di distanza
00589             fDistance=0.0;
00590             // Fissa come Output il pin InOutProxSensor
00591             InOutProxSensor.output();
00592             // Poni 'L' sul Pin e mantienilo per qualche microsecondo
00593             InOutProxSensor.write(0);
00594             wait_us(5);
00595             // Poni 'H' sul Pin e mantienilo per qualche microsecondo
00596             InOutProxSensor.write(1);
00597             wait_us(10);
00598             // Poni 'L' sul Pin e mantienilo per qualche microsecondo
00599             InOutProxSensor.write(0);
00600             // Attendi assestamento e Fissa come Input il pin InOutProxSensor
00601             wait_us(5);
00602             InOutProxSensor.input();
00603             InOutProxSensor.mode(PullDown); // se non è presente il sensore, il pin rimane a '0'
00604             
00605             // attende la risposta del sensore di prossimità per un tempo fissato da TIMEOUTPROXSENSOR. Dopo tale tempo dichiara inesistente il sensore
00606             TimerProxSensor.start();
00607             nTimerStart = TimerProxSensor.read_us();
00608             nTimerTillNow=(TimerProxSensor.read_us()-nTimerStart);
00609             while((InOutProxSensor ==0) && (nTimerTillNow< TIMEOUTPROXSENSOR))
00610             {
00611                 nTimerCurrent = TimerProxSensor.read_us();
00612                 nTimerTillNow=nTimerCurrent-nTimerStart;
00613                 led2=1; // se rimane nel while il LED rimane acceso
00614                 pc.printf("sono qui 2 \r\n");
00615             }
00616             TimerProxSensor.stop(); // spegne il timer che serve per misurare il timeout quando assente il sensore di prossimità
00617             pc.printf("\r\nUscita dal while, nTimerTillNow = %d\r\n", nTimerTillNow);
00618             // se nTimerTillNow è inferiore al TIMEOUT, il sensore è presente e quindi misura la distanza dell'ostacolo
00619             if(nTimerTillNow < TIMEOUTPROXSENSOR)
00620             {
00621                 // riattiva il timer per misurare la distanza dell'ostacolo
00622                 TimerProxSensor.start();
00623                 nTimerStart = TimerProxSensor.read_us();
00624                 while(InOutProxSensor == 1)
00625                 {
00626                     led2=1; // se rimane nel while il LED rimane acceso
00627                 }
00628                 TimerProxSensor.stop();
00629                 nTimerStop = TimerProxSensor.read_us();
00630                
00631                 pc.printf("\r\nSensore Presente, nTimerTillNow = %d\r\n", nTimerTillNow);
00632             
00633                 // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
00634                 // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
00635                 // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
00636                 fDistance = (nTimerStop-nTimerStart)/58.2;
00637                 // invia il dato al PC
00638                 pc.printf("distanza dell'ostacolo = %f0.2\r\n", fDistance);
00639             }    
00640             else
00641             {
00642                // quando esce dai while bloccanti, il LED si spegne
00643                led2=0;
00644                pc.printf("\r\nTimeOut\r\n");
00645             }
00646             //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++ 
00647            //++++++++++++++ INIZIO Suona Clacson +++++++++
00648             //escludi le misure oltre il max
00649             if((fDistance <= 50.0) && (fDistance >= 3)) 
00650             //if(InDiag1 == 1)
00651             {
00652               // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
00653               if(fDistance < 22)
00654               {
00655                     // blocca altri suoni quando genera suono del clacson
00656                     bEngineSoundStop=1;
00657                     // INIZIO generazione tono  
00658                     nClacsonSampleIndex=0;
00659                     // Genera il suono del clacson
00660                     for(nClacsonSampleCount=0; nClacsonSampleCount<7000; nClacsonSampleCount++)
00661                     {
00662                        OutWave.write_u16(usaClacson[nClacsonSampleIndex]); //max 32767
00663                        //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
00664                        wait(fDeltaTClacsonSound);
00665                        // genera ciclicamente
00666                        nClacsonSampleIndex++;
00667                        if(nClacsonSampleIndex >= CLACSONSAMPLENUM)
00668                        {
00669                            nClacsonSampleIndex=0;
00670                        }   
00671                        // a metà genera un wait per doppio clacson
00672                        if(nClacsonSampleCount == 2000)
00673                        {
00674                            wait_ms(100);
00675                        }
00676                     
00677                     } 
00678                     //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
00679                     OutWave.write_u16(0);
00680                     
00681                     // sblocca altri suoni dopo aver generato suono del clacson
00682                     bEngineSoundStop=0;
00683                     
00684                 } // if(fDistance < soglia) suona clacson
00685                 
00686             } // if( (fDistance < Max) && (fDistance > Min)) 
00687             //++++++++++++++ FINE Suona Clacson +++++++++ 
00688             
00689             
00690             
00691             //++++++++++++++  INIZIO pilotaggio motore cofano +++++++++++++++++++
00692             if((InMotorSwitchRPI==1) && (nPosizioneCofano ==0))    
00693             //if((myButton==1) && (nPosizioneCofano ==0))    
00694             {    
00695                //Ferma motore
00696                 OutMotorA=0;
00697                 OutMotorB=0;
00698                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
00699                 wait_ms(10);
00700                 
00701                 //Ferma motore
00702                 OutMotorA=0;
00703                 OutMotorB=1;
00704                 //pc.printf("Stop motore; OutA OutB = 01\r\n");
00705                 wait_ms(10);
00706                 
00707                 // Ruota Right
00708                 OutMotorA=1;
00709                 OutMotorB=1;
00710                 //pc.printf("Ruota Right; OutA OutB = 11\r\n");
00711                 wait_ms(710);
00712                 
00713                 // Ferma Motore
00714                 OutMotorA=0;
00715                 OutMotorB=1;
00716                 //pc.printf("Stop Motore; OutA OutB = 01\r\n");
00717                 wait_ms(10);
00718                 
00719                 //Ferma motore
00720                 OutMotorA=0;
00721                 OutMotorB=0;
00722                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
00723                 wait_ms(10);
00724                 // cambia posizione del cofano. E' Stato Aperto
00725                 nPosizioneCofano = 1;
00726             }       
00727             // se arriva comando di chiusura cofano & il cofano è aperto, muovi motore
00728             //if((myButton==0) && (nPosizioneCofano == 1))
00729             if((InMotorSwitchRPI==0) && (nPosizioneCofano ==1))   
00730             {
00731                 //pc.printf("\r\nCofano aperto & comando di chiusura\r\n");
00732                                      
00733                 //Ferma motore
00734                 OutMotorA=0;
00735                 OutMotorB=0;
00736                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
00737                 wait_ms(10);
00738                             
00739                 // Ruota Left
00740                 OutMotorA=1;
00741                 OutMotorB=0;
00742                 //pc.printf("Ruota Left; OutA OutB = 10\r\n");
00743                 wait_ms(730);
00744                 
00745                 //Ferma motore
00746                 OutMotorA=0;
00747                 OutMotorB=0;
00748                 //pc.printf("Stop motore; OutA OutB = 00\r\n");
00749                 wait_ms(10);
00750                 
00751                 // cambia posizione del cofano. E' Stato Chiuso
00752                 nPosizioneCofano = 0;
00753             }   
00754             //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
00755             
00756             
00757             
00758             //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
00759             if(InLightSwitchRPI ==1)
00760             {
00761                 // accendi i LED di abbellimento
00762                 //led2=1;
00763                 LedYAD = 1; 
00764                 LedYAS = 1;
00765                 LedRPD = 1;
00766                 LedRPS = 1;
00767                 LedYRAll = 1;
00768             } 
00769             else
00770             {
00771                 
00772                 // spegni i LED di abbellimento
00773                 //led2=0;
00774                 LedYAD = 0; 
00775                 LedYAS = 0;
00776                 LedRPD = 0;
00777                 LedRPS = 0;
00778                 LedYRAll = 0;
00779                  
00780             }
00781             //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++
00782             
00783             //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
00784             if(InShearRPI == 1)
00785             {
00786                 // funzione di generazione suono motosega
00787                 bEngineSoundStop=1; // disattiva suono del motore
00788                 ShearSoundGeneration();
00789                 bEngineSoundStop=0; // riattiva suono del motore
00790             }
00791             //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
00792         }// if(InStandByRPI == 0)
00793         else
00794         {
00795             
00796             // ricevuto da RPI, il comando di StandBy = ON
00797             // ricevuto il comando di StandBy (InStandBy == 1)
00798             
00799             // la prima volta che entra in questo else, la variabile di stato nStandBy è '0'. Solo la prima volta Genera il messaggio di arrivederci
00800             if(nStandBy == 0)
00801             {
00802                 // blocca il suono del motore per emettere messaggio di arrivederci
00803                 bEngineSoundStop=1;
00804                 
00805                 //Genera messaggio di arrivederci
00806                 FarewellMessage();
00807                                
00808                 // rispristina il suono del motore
00809                 bEngineSoundStop=0;
00810                 
00811                 // cambia lo stato dello StandBy
00812                 nStandBy = 1;
00813             }
00814             
00815             // se modalità StandBy = ON, disattiva audio;
00816             fSoundGain = 0.0;
00817             
00818             
00819            
00820         }
00821     } //while(true)
00822     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00823     //+++++++++++++++++++++++++++++++  FINE CICLO OPERATIVO +++++++++++++++++++++++++++++++++++++++++++++
00824     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00825         
00826     
00827 }
00828