RobotFinale3.cpp

00001 // mbed specific header files.
00002 #include "mbed.h"
00003 
00004 // include suono del motore
00005 #include "SampledSoundGurgle.h" // rumore del motore da fermo durante gli spsotamenti
00006 #include "SampledSoundFarewellDizione.h" // messaggio di arrivederci
00007 #include "SampledSoundWelcomeDizione.h" // messaggio di benvenuto
00008 #include "SampledSoundMotosega.h" // rumore durante lo spostamento con Cesoia
00009 //#include "SampledSoundFarewell.h" // messaggio di arrivederci
00010 //#include "SampledSoundWelcome.h" // messaggio di benvenuto
00011 
00012 //#include "SampledSoundMotosega.h"
00013 //#include "SampledSoundTrattore.h"
00014  
00015 
00016 // TimeOut in [microsec] per verificare la presenza del sensore prossimità. Se il sensore non è presente il timer supera TIMEOUTPROXSENSOR
00017 #define TIMEOUTPROXSENSOR 1000 //tempo in [microsec]
00018 
00019 // numero di campioni che compongono un periodo della sinusoide in Output sull'ADC
00020 #define CLACSONSAMPLENUM   45 // consigliabile avere  multipli di 45
00021 
00022 // numero di campioni acquisiti su cui effettuare la media di luminosità
00023 #define NUMLIGHTSAMPLE 100
00024 
00025 // Parametri di soglia per la luce. Accendi/spegni Luci se la luminosità scende/sale sotto/sopra  SOGLIALUCIMAX e SOGLIALUCIMIN
00026 #define SOGLIALUCIMAX (1.85)
00027 #define SOGLIALUCIMIN (1.45)
00028 
00029 // parametri dell'onda coseno da generare
00030 #define PI        (3.141592653589793238462)
00031 #define AMPLITUDE 32767 //(1.0)    // x * 3.3V
00032 #define PHASE     (PI/2) // 2*pi è un periodo
00033 #define OFFSET    32767 //(0x7FFF)
00034 
00035 // Timer per il calcolo dei tempi del sensore di prossimità
00036 Timer TimerProxSensor;
00037 
00038 // ticker per la generazione dell'onda con DAC
00039 Ticker SampleOutTicker;            
00040 
00041 // distanza in cm dell'ostacolo
00042 double fDistance;
00043 
00044 // Buffer contenente la sinusoide da porre in output come Clacson.
00045 unsigned short usaClacson[CLACSONSAMPLENUM];
00046 
00047 // prototipo di funzione che genera i campioni della sinusoide da utilizzare per la generazione tramite DAC
00048 void CalculateSinewave(void);
00049  
00050 
00051 // Periodo di generazione campioni in output DeltaT = T/NumSample
00052 double fDeltaTClacsonSound;
00053 double fDeltaTEngineSound;
00054 
00055 // amplificazione per i suoni da generare con l'ADC
00056 double fAmpEngineSound; // rumore di Engine 
00057 double fAmpClacsonSound; // rumore di Clacson
00058 double fAmpShearSound; // rumore di Shear
00059 
00060 // frequenza segnale audio da generare per clacson e motore
00061 double fFreqClacsonSound;
00062 double fFreqEngineSound;
00063 
00064 // periodo della sinusoide audio da generare come suono del clacson
00065 double fPeriodClacsonSOund;
00066 
00067 // numero di campioni di clacson già inviati in output sul DAC
00068 int nClacsonSampleCount;
00069 // indice dell'array di generazione campioni clacson 
00070 int nClacsonSampleIndex;
00071 
00072 // indice dell'Array di generazione suoni del motore
00073 volatile int nEngineSampleIndex;
00074 
00075 // Flag che decide se generare oppure no il suono del motore. '1'=non generare il suono del motore, '0'=genera il suono del motore
00076 int bEngineSoundStop;
00077 
00078 
00079 
00080 // valore medio della Luminosità su NUMACQUISIZIONI acquisizioni
00081 double fAvgLight;
00082 
00083 // valore numerico, di tensione e di luce letto dall'ADC
00084 volatile unsigned short usReadADC;
00085 volatile float fReadVoltage;
00086 
00087 // valore di luminosità letto dall'ADC
00088 volatile float fLight;
00089 
00090 // posizione del Cofano '0' = chiuso, '1'=aperto. Inizialmente DEVE essere chiuso (cioè '0')
00091 int nPosizioneCofano=0;
00092 
00093 
00094 // indice per il conteggio dei campioni di luce acquisiti dal fotoresistore
00095 int nLightSampleIndex;
00096    
00097 // timer per il calcolo della velocità
00098 Timer TimerHall;
00099     
00100 // tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni
00101 int nTimerStart, nTimerCurrent, nTimerStop, nTimerTillNow;
00102 
00103 
00104 // variabile che conta il numero di fronti si salita del segnale encoder
00105 volatile int nCountRiseEdge; 
00106     
00107 // pin di pilotaggio Motore DC
00108 DigitalOut OutMotorA (PB_0);
00109 DigitalOut OutMotorB (PC_1);
00110 
00111 // Output Digitali usati per i LED
00112 DigitalOut LedWAD (PC_2);
00113 DigitalOut LedWAS (PC_3);
00114 DigitalOut LedWPD (PH_0);
00115 DigitalOut LedWPS (PA_0) ;
00116 DigitalOut LedYAD (PC_9); 
00117 DigitalOut LedYAS (PC_8);
00118 DigitalOut LedRPD (PA_13);
00119 DigitalOut LedRPS (PA_14); 
00120 DigitalOut LedYRAll (PC_7); // COn questo pin si pilotano contemporaneamente i Led: YLD1, YLD2, YLD3, YLD4, YLS1, YLS2, YLS3, YLS4, RPD1, RPS1
00121 DigitalOut LedYRAllGND (PB_6); // GND di Led. Sempre a '0' per permettere d LedYRAll di accendere e spegnere i LED
00122 
00123 
00124 // Input/Output Digitali usati per interfaccia RPI
00125 DigitalIn InShearRPI (PB_11); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17
00126 DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20
00127 DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore. Collegato al Raspberry GPIO16
00128 DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione. Collegato al Raspberry GPIO13
00129 DigitalIn InFutureUse1RPI (PB_2); // usi futuri 1 di comunicazione. Collegato al Raspberry GPIO25
00130 DigitalIn InStandByRPI (PC_15); // StandBy ON/OFF. '1' = robot in StandBy; '0' = robot operativo. Collegato al Raspberry GPIO12
00131 
00132 // Input e Output per i sensori e attuatori
00133 AnalogOut OutWave(PA_4); // pin A2 di output per la forma d'onda analogica dedicata al suono
00134 AnalogIn InWaveLight(PA_1); // pin A1 di input per la forma d'onda analogica dedicata alla luminosità
00135 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
00136 InterruptIn InEncoderA(PA_9); // Primo Pin di input dall'encoder ottico collegato al motore per misurare lo spostamento
00137 
00138 
00139 // Input/Output utilizzati da funzioni default su scheda NUCLEO
00140 DigitalOut led2(LED2);// LED verde sulla scheda. Associato a PA_5
00141 Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12
00142 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13
00143 
00144 // input di diagnostica
00145 DigitalIn InDiag1(PA_15,PullUp); 
00146 
00147 //****************************
00148 // Create the sinewave buffer
00149 //****************************
00150 void CalculateSinewave(int nOffset, int nAmplitude, double fPhase)
00151 {
00152     // variabile contenente l'angolo in radianti
00153     double fRads;
00154     // indici per i cicli
00155     int nIndex;
00156     // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE
00157     double fDeltaF;
00158     // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF
00159     double fAngle;
00160     
00161     fDeltaF = 360.0/CLACSONSAMPLENUM;
00162     for (nIndex = 0; nIndex < CLACSONSAMPLENUM; nIndex++) 
00163     {
00164         fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide
00165         fRads = (PI * fAngle)/180.0; // Convert degree in radian
00166         //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET;
00167         usaClacson[nIndex] = nAmplitude * cos(fRads + fPhase) + nOffset;
00168     }
00169 }
00170 
00171 /********************************************************/
00172 /* Funzione avviata all'inizio come saluto e Benvenuto  */
00173 /********************************************************/
00174 void WelcomeMessage()
00175 {
00176     // indice per i cicli interni alla funzione
00177     int nIndex;
00178     
00179     // indice per l'array di welcome message
00180     int nWelcomeMsgIndex;
00181     // parametri per generare il messaggio di welcome
00182     double fAmpWelcomeSound;
00183     double fFreqWelcomeSound;
00184     double fDeltaTWelcomeSound;
00185     
00186     //++++++++++++ INIZIO Accendi le Luci in sequenza +++++++++++++++++
00187     // accendi tutte le luci
00188     LedWAD = 1;
00189     wait_ms(300);
00190     LedWAS = 1;
00191     wait_ms(300);
00192     LedWPD = 1;
00193     wait_ms(300);
00194     LedWPS = 1;
00195     wait_ms(300);
00196     LedYAD = 1; 
00197     wait_ms(300);
00198     LedYAS = 1;
00199     wait_ms(300);
00200     LedRPD = 1;
00201     wait_ms(300);
00202     LedRPS = 1;
00203     //++++++++++++ FINE Accendi le Luci in sequenza +++++++++++++++++
00204      
00205     //++++++++++++ INIZIO generazione messaggio di benvenuto +++++++++++++++++ 
00206     fAmpWelcomeSound = 1.0;  // fissa l'amplificazione per il messaggio di welcome. Valori da 0[min] a 1[max] 
00207     fFreqWelcomeSound=nSamplePerSecWelcome/nUnderSampleFactorWelcome;// campioni per secondo del welcome message da generare = nSamplePerSec/nUnderSampleFactor
00208     fDeltaTWelcomeSound = (1.0/fFreqWelcomeSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00209    
00210     
00211     for(nWelcomeMsgIndex=0; nWelcomeMsgIndex < nSampleNumWelcome; nWelcomeMsgIndex++)
00212     {
00213         // mette in output un campione della forma d'onda del welcome message  moltiplicato per l'amplificazione fAmp
00214         OutWave.write_u16(naInputSoundWaveWelcome[nWelcomeMsgIndex]*fAmpWelcomeSound);
00215         
00216         // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
00217         //wait(fDeltaTWelcomeSound);
00218         wait_us(50);
00219     }
00220     //++++++++++++ FINE generazione messaggio di benvenuto +++++++++++++++++
00221     
00222     //++++++++++++ INIZIO Spegni le Luci in sequenza +++++++++++++++++
00223     // spegni le Luci in sequenza
00224     for(nIndex=0; nIndex<3; nIndex++)
00225     {
00226         wait_ms(100); 
00227         LedWAD = 1;
00228         wait_ms(100); 
00229         LedWAD = 0;
00230     }
00231     for(nIndex=0; nIndex<3; nIndex++)
00232     {
00233         wait_ms(100); 
00234         LedWAS = 1;
00235         wait_ms(100); 
00236         LedWAS = 0;
00237     }
00238     for(nIndex=0; nIndex<3; nIndex++)
00239     {
00240         wait_ms(100); 
00241         LedWPD = 1;
00242         wait_ms(100); 
00243         LedWPD = 0;
00244     }
00245     for(nIndex=0; nIndex<3; nIndex++)
00246     {
00247         wait_ms(100); 
00248         LedWPS = 1;
00249         wait_ms(100); 
00250         LedWPS = 0;
00251     }
00252     for(nIndex=0; nIndex<3; nIndex++)
00253     {
00254         wait_ms(100); 
00255         LedYAD = 1;
00256         wait_ms(100); 
00257         LedYAD =0;
00258     }
00259     for(nIndex=0; nIndex<3; nIndex++)
00260     {
00261         wait_ms(100); 
00262         LedYAS = 1;
00263         wait_ms(100); 
00264         LedYAS = 0;
00265     }
00266     for(nIndex=0; nIndex<3; nIndex++)
00267     {
00268         wait_ms(100); 
00269         LedRPD = 1;
00270         wait_ms(100); 
00271         LedRPD = 0;
00272     }
00273     for(nIndex=0; nIndex<3; nIndex++)
00274     {
00275         wait_ms(100); 
00276         LedRPS = 1;
00277         wait_ms(100); 
00278         LedRPS = 0;
00279     }
00280     for(nIndex=0; nIndex<3; nIndex++)
00281     {
00282         wait_ms(100); 
00283         LedYRAll = 1;
00284         wait_ms(100); 
00285         LedYRAll = 0;
00286     }
00287     //++++++++++++ FINE Spegni le Luci in sequenza +++++++++++++++++
00288             
00289 }
00290 
00291 
00292 /***********************************************************************/
00293 /* Genera il suono di una motosega.                                    */ 
00294 /* Attivo quando arriva il comando di spostamento Cesoie da Raspberry  */
00295 /***********************************************************************/
00296 void ShearSoundGeneration()
00297 {
00298     // indice per l'array di suono Shear  
00299     int nShearSoundIndex;
00300     // parametri per generare il messaggio di shear
00301     double fAmpShearSound;
00302     double fFreqShearSound;
00303     double fDeltaTShearSound;
00304     
00305     //++++++++++++ INIZIO generazione suono di motosega +++++++++++++++++ 
00306     fAmpShearSound = 1.0;  // fissa l'amplificazione per il suono di Shear. Valori da 0[min] a 1[max] 
00307     fFreqShearSound=nSamplePerSecShear/nUnderSampleFactorShear;// campioni per secondo del Shear da generare = nSamplePerSec/nUnderSampleFactor
00308     fDeltaTShearSound = (1.0/fFreqShearSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00309    
00310     
00311     for(nShearSoundIndex=0; nShearSoundIndex < nSampleNumShear; nShearSoundIndex++)
00312     {
00313         // mette in output un campione della forma d'onda del suono di Shear,  moltiplicato per l'amplificazione fAmp
00314         OutWave.write_u16(naInputSoundWaveShear[nShearSoundIndex]*fAmpShearSound);
00315         
00316         // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
00317         wait(fDeltaTShearSound);
00318     }
00319     //++++++++++++ FINE generazione suono di motosega +++++++++++++++++
00320        
00321 }
00322 /***********************************************************************/
00323 /* generazione suoni con i sample da file di campioni in SoundSample.h */
00324 /***********************************************************************/
00325 void SampleOut() 
00326 {
00327     // interrompi il suono del motore per generare altri suoni. '1' = interrompi i suoni
00328     if(bEngineSoundStop == 0)
00329     {
00330         // mette in output un campione della forma d'onda del rumore motore moltiplicato per l'amplificazione fAmp
00331         OutWave.write_u16(naInputSoundWave[nEngineSampleIndex]*fAmpEngineSound);
00332         // incrementa l'indice del campione in output, nSampleNum è il numero dei campioni nle file Sound.h
00333         nEngineSampleIndex++;
00334         if(nEngineSampleIndex >= nSampleNum)
00335             nEngineSampleIndex=0;
00336     }        
00337 }
00338 
00339 
00340  /**************************************************************************************/
00341 /* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
00342 /**************************************************************************************/
00343 void riseEncoderIRQ()
00344 {
00345     // ogni volta che riceve un fornte di salita sul segnale di encoder del motore, entra in questa routine e incrementa il contatore
00346     nCountRiseEdge++;
00347 }       
00348      
00349 /********/
00350 /* Main */
00351 /********/
00352 int main()
00353 {
00354     // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
00355     pc.baud(921600); //921600 bps
00356     
00357     // inizializza variabili
00358     LedYRAllGND =0; // assegna lo '0' al segnale LedYRAllGND = 0, Così sarà il riferimento negativo per LedYRAll
00359         
00360     // definisci il mode del segnale digitale di EncoderA
00361     InEncoderA.mode(PullUp);
00362     
00363     // Associa routine di Interrup all'evento fronte di salita del segnale di encoder
00364     InEncoderA.rise(&riseEncoderIRQ);
00365 
00366     // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove     
00367     InEncoderA.enable_irq();
00368     
00369     // avvia routine di saluto di benvenuto
00370     WelcomeMessage();    
00371     
00372     //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00373     //+++++++++++++++++++++++++++++++  INIZIO CICLO TEST ++++++++++++++++++++++++++++++++++++++++++++
00374     //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00375     /*
00376     while(true)
00377     {
00378     }
00379     */
00380     //+++++++++++++++++++++++++++++++ FINE CICLO TEST +++++++++++++++++++++++++++++++++++++++++++++++
00381     
00382     
00383     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00384     //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO PRINCIPALE ++++++++++++++++++++++++++++++++++
00385     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00386     
00387     //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
00388     fFreqClacsonSound = 440.0; // frequenza in Hz del tono del Clacson da generare
00389     fAmpClacsonSound = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
00390     fDeltaTClacsonSound = 1.0/(fFreqClacsonSound*CLACSONSAMPLENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
00391     CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmpClacsonSound), (PI/2.0)); // generazione della sinusoide con valori nominali
00392     //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
00393           
00394     //+++++++ INIZIO avvio rumore del motore a frequenza da fermo +++++++++
00395     fAmpEngineSound = 1.0;  // fissa l'amplificazione per il rumore motore. Valori da 0[min] a 1[max] 
00396     fFreqEngineSound=nSamplePerSec/nUnderSampleFactor;// campioni per secondo del rumore motore da generare = nSamplePerSec/nUnderSampleFactor
00397     fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00398     nEngineSampleIndex =0; // Avvia indice di generazione suono motore
00399     SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00400     //+++++++ FINE avvio ruomre del motore a frequenza da fermo +++++++++
00401    
00402     //inizializza variabili
00403     nEngineSampleIndex =0; // avvia l'indice di generazione suoni
00404     nCountRiseEdge=0; // azzera il  contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
00405     bEngineSoundStop =0; // inizialmente il suono del motore è generato
00406     nPosizioneCofano=0; // inizializza la posizione del cofano chiuso
00407     while(true)
00408     {
00409        //++++++++++ INIZIO genera diverso suono con motore fermo e in movimento +++++++++++++++++
00410        // se nella IRQ sono stati contati fronti di salita del dell'encoder, il robot si sta muovendo         
00411        if(nCountRiseEdge != 0)
00412        //if(InDiag1==1)
00413        {
00414            // sono stati contati impulsi di encoder, il robot si sta muovendo            
00415            fDeltaTEngineSound = (0.5/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00416            SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00417        }
00418        else
00419        {
00420             // se ci sono stati impulsi di encoder, il robot è fermo, genera rumore del motore fermo
00421            fDeltaTEngineSound = (1.0/fFreqEngineSound);  // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento
00422            SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione
00423          
00424        }   
00425        nCountRiseEdge=0;
00426        //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++
00427        
00428        //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++
00429         // inizializza il valore medio della Luminosità 
00430         fAvgLight=0.0;
00431         for(nLightSampleIndex=0; nLightSampleIndex < NUMLIGHTSAMPLE; nLightSampleIndex++)
00432         {
00433            // acquisisce dato da ADC
00434            usReadADC = InWaveLight.read_u16();
00435            fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
00436            //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
00437            fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
00438            fAvgLight+=fLight;
00439         }   
00440         // calcola valore medio su NUMSAMPLE acquisizioni 
00441         fAvgLight/= NUMLIGHTSAMPLE;
00442         
00443         // Accendi/Spegni i LED Bianchi se il valore medio della luminosità è sotto/sopra soglia
00444         if(fAvgLight < SOGLIALUCIMIN)
00445         {
00446            // Accendi LED Bianchi
00447            //led2 = 1;
00448            LedWAD = 1;
00449            LedWAS = 1;
00450            LedWPD = 1;
00451            LedWPS = 1;
00452         }
00453         else
00454         {
00455             if(fAvgLight > SOGLIALUCIMAX)
00456             {
00457                // Spegni LED Bianchi
00458                //led2 = 0;
00459                LedWAD = 0;
00460                LedWAS = 0;
00461                LedWPD = 0;
00462                LedWPS = 0;
00463             }
00464         }
00465         // invia il dato al PC
00466         //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
00467         //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
00468         
00469         
00470        //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
00471         //inizializza misura di distanza
00472         fDistance=0.0;
00473         // Fissa come Output il pin InOutProxSensor
00474         InOutProxSensor.output();
00475         // Poni 'L' sul Pin e mantienilo per qualche microsecondo
00476         InOutProxSensor.write(0);
00477         wait_us(5);
00478         // Poni 'H' sul Pin e mantienilo per qualche microsecondo
00479         InOutProxSensor.write(1);
00480         wait_us(10);
00481         // Poni 'L' sul Pin e mantienilo per qualche microsecondo
00482         InOutProxSensor.write(0);
00483         // Attendi assestamento e Fissa come Input il pin InOutProxSensor
00484         wait_us(5);
00485         InOutProxSensor.input();
00486         InOutProxSensor.mode(PullDown); // se non è presente il sensore, il pin rimane a '0'
00487         
00488         // attende la risposta del sensore di prossimità per un tempo fissato da TIMEOUTPROXSENSOR. Dopo tale tempo dichiara inesistente il sensore
00489         TimerProxSensor.start();
00490         nTimerStart = TimerProxSensor.read_us();
00491         nTimerTillNow=(TimerProxSensor.read_us()-nTimerStart);
00492         while((InOutProxSensor ==0) && (nTimerTillNow< TIMEOUTPROXSENSOR))
00493         {
00494             nTimerCurrent = TimerProxSensor.read_us();
00495             nTimerTillNow=nTimerCurrent-nTimerStart;
00496             led2=1; // se rimane nel while il LED rimane acceso
00497             pc.printf("sono qui 2 \r\n");
00498         }
00499         TimerProxSensor.stop(); // spegne il timer che serve per misurare il timeout quando assente il sensore di prossimità
00500         pc.printf("\r\nUscita dal while, nTimerTillNow = %d\r\n", nTimerTillNow);
00501         // se nTimerTillNow è inferiore al TIMEOUT, il sensore è presente e quindi misura la distanza dell'ostacolo
00502         if(nTimerTillNow < TIMEOUTPROXSENSOR)
00503         {
00504             // riattiva il timer per misurare la distanza dell'ostacolo
00505             TimerProxSensor.start();
00506             nTimerStart = TimerProxSensor.read_us();
00507             while(InOutProxSensor == 1)
00508             {
00509                 led2=1; // se rimane nel while il LED rimane acceso
00510             }
00511             TimerProxSensor.stop();
00512             nTimerStop = TimerProxSensor.read_us();
00513            
00514             pc.printf("\r\nSensore Presente, nTimerTillNow = %d\r\n", nTimerTillNow);
00515         
00516             // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
00517             // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
00518             // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
00519             fDistance = (nTimerStop-nTimerStart)/58.2;
00520             // invia il dato al PC
00521             pc.printf("distanza dell'ostacolo = %f0.2\r\n", fDistance);
00522         }    
00523         else
00524         {
00525            // quando esce dai while bloccanti, il LED si spegne
00526            led2=0;
00527            pc.printf("\r\nTimeOut\r\n");
00528         }
00529         //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++ 
00530         
00531         //++++++++++++++ INIZIO Suona Clacson +++++++++
00532         //escludi le misure oltre il max e meno del min
00533         if((fDistance <= 50.0) && (fDistance >= 3)) 
00534         //if(InDiag1 == 1)
00535         {
00536           // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
00537           if(fDistance < 22)
00538           {
00539                 // blocca altri suoni quando genera suono del clacson
00540                 bEngineSoundStop=1;
00541                 // INIZIO generazione tono  
00542                 nClacsonSampleIndex=0;
00543                 // Genera il suono del clacson
00544                 for(nClacsonSampleCount=0; nClacsonSampleCount<7000; nClacsonSampleCount++)
00545                 {
00546                    OutWave.write_u16(usaClacson[nClacsonSampleIndex]); //max 32767
00547                    //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
00548                    wait(fDeltaTClacsonSound);
00549                    // genera ciclicamente
00550                    nClacsonSampleIndex++;
00551                    if(nClacsonSampleIndex >= CLACSONSAMPLENUM)
00552                    {
00553                        nClacsonSampleIndex=0;
00554                    }   
00555                    // a metà genera un wait per doppio clacson
00556                    if(nClacsonSampleCount == 2000)
00557                    {
00558                        wait_ms(100);
00559                    }
00560                 } 
00561                 //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
00562                 OutWave.write_u16(0);
00563                 
00564                 // sblocca altri suoni dopo aver generato suono del clacson
00565                 bEngineSoundStop=0;
00566                 
00567             } // if(fDistance < soglia) suona clacson
00568             
00569         } // if( (fDistance < Max) && (fDistance > Min)) 
00570         wait_ms(100); // dai tempo prima di ripetere nuovamente la misuradella distanza    
00571         //++++++++++++++ FINE Suona Clacson +++++++++ 
00572         
00573         
00574         
00575         //++++++++++++++  INIZIO pilotaggio motore cofano +++++++++++++++++++
00576         if((InMotorSwitchRPI==1) && (nPosizioneCofano ==0))    
00577         //if((myButton==1) && (nPosizioneCofano ==0))    
00578         {    
00579            //Ferma motore
00580             OutMotorA=0;
00581             OutMotorB=0;
00582             //pc.printf("Stop motore; OutA OutB = 00\r\n");
00583             wait_ms(10);
00584             
00585             //Ferma motore
00586             OutMotorA=0;
00587             OutMotorB=1;
00588             //pc.printf("Stop motore; OutA OutB = 01\r\n");
00589             wait_ms(10);
00590             
00591             // Ruota Right
00592             OutMotorA=1;
00593             OutMotorB=1;
00594             //pc.printf("Ruota Right; OutA OutB = 11\r\n");
00595             wait_ms(355);
00596             
00597             // Ferma Motore
00598             OutMotorA=0;
00599             OutMotorB=1;
00600             //pc.printf("Stop Motore; OutA OutB = 01\r\n");
00601             wait_ms(10);
00602             
00603             //Ferma motore
00604             OutMotorA=0;
00605             OutMotorB=0;
00606             //pc.printf("Stop motore; OutA OutB = 00\r\n");
00607             wait_ms(10);
00608             // cambia posizione del cofano. E' Stato Aperto
00609             nPosizioneCofano = 1;
00610         }       
00611         // se arriva comando di chiusura cofano & il cofano è aperto, muovi motore
00612         //if((myButton==0) && (nPosizioneCofano == 1))
00613         if((InMotorSwitchRPI==0) && (nPosizioneCofano ==1))   
00614         {
00615             //pc.printf("\r\nCofano aperto & comando di chiusura\r\n");
00616                                  
00617             //Ferma motore
00618             OutMotorA=0;
00619             OutMotorB=0;
00620             //pc.printf("Stop motore; OutA OutB = 00\r\n");
00621             wait_ms(10);
00622                         
00623             // Ruota Left
00624             OutMotorA=1;
00625             OutMotorB=0;
00626             //pc.printf("Ruota Left; OutA OutB = 10\r\n");
00627             wait_ms(365);
00628             
00629             //Ferma motore
00630             OutMotorA=0;
00631             OutMotorB=0;
00632             //pc.printf("Stop motore; OutA OutB = 00\r\n");
00633             wait_ms(10);
00634             
00635             // cambia posizione del cofano. E' Stato Chiuso
00636             nPosizioneCofano = 0;
00637         }   
00638         //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
00639         
00640         
00641         
00642         //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
00643         if(InLightSwitchRPI ==1)
00644         {
00645             // accendi i LED di abbellimento
00646             //led2=1;
00647             LedYAD = 1; 
00648             LedYAS = 1;
00649             LedRPD = 1;
00650             LedRPS = 1;
00651             LedYRAll = 1;
00652         } 
00653         else
00654         {
00655             
00656             // spegni i LED di abbellimento
00657             //led2=0;
00658             LedYAD = 0; 
00659             LedYAS = 0;
00660             LedRPD = 0;
00661             LedRPS = 0;
00662             LedYRAll = 0;
00663              
00664         }
00665         //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++
00666         
00667         //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
00668         if(InShearRPI == 1)
00669         {
00670             // funzione di generazione suono motosega
00671             bEngineSoundStop=1; // disattiva suono del motore
00672             ShearSoundGeneration();
00673             bEngineSoundStop=0; // riattiva suono del motore
00674         }
00675         //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
00676     } 
00677     //+++++++++++++++++++++++++++++++  FINE CICLO PRINCIPALE ++++++++++++++++++++++++++++++++++++++++++++
00678     
00679     
00680     
00681         
00682 }
00683 
00684