Giuseppe Falagario
/
Amaldi_RobotFinale_Rev4
Robot Amaldi Rev 4.0 Messaggio don't touch me
RobotFinale3-3.cpp
- Committer:
- pinofal
- Date:
- 2019-03-05
- Revision:
- 16:17df60afe302
- Parent:
- RobotFinale3-2.cpp@ 15:ef808556423b
File content as of revision 16:17df60afe302:
//++++++++++++++++++++ ATTENZIONE ++++++++++++++++++++++++++++++++++++++++++++++++ // rivedere tutte le giunzioni. su alcune ci sono fili di rame che fuoriescono. // rivedere i cavi nei connettori. In alcuni sono moto tesi // inserire una ventola per raffreddare transistor e resistenza //++++++++++++++++++++ ATTENZIONE ++++++++++++++++++++++++++++++++++++++++++++++++ // mbed specific header files. #include "mbed.h" // include suono del motore #include "SampledSoundGurgle.h" // rumore del motore da fermo durante gli spsotamenti #include "SampledSoundWelcomeDizione.h" // messaggio di benvenuto #include "SampledSoundFarewellDizione.h" // messaggio di Arrivederci #include "SampledSoundMotosega.h" // rumore durante lo spostamento con Cesoia //#include "SampledSoundMotosega.h" //#include "SampledSoundTrattore.h" // TimeOut in [microsec] per verificare la presenza del sensore prossimità. Se il sensore non è presente il timer supera TIMEOUTPROXSENSOR #define TIMEOUTPROXSENSOR 1000 //tempo in [microsec] // numero di campioni che compongono un periodo della sinusoide in Output sull'ADC #define CLACSONSAMPLENUM 45 // consigliabile avere multipli di 45 // numero di campioni acquisiti su cui effettuare la media di luminosità #define NUMLIGHTSAMPLE 100 // Parametri di soglia per la luce. Accendi/spegni Luci se la luminosità scende/sale sotto/sopra SOGLIALUCIMAX e SOGLIALUCIMIN #define SOGLIALUCIMAX (1.85) #define SOGLIALUCIMIN (1.45) // parametri dell'onda coseno da generare #define PI (3.141592653589793238462) #define AMPLITUDE 32767 //(1.0) // x * 3.3V #define PHASE (PI/2) // 2*pi è un periodo #define OFFSET 32767 //(0x7FFF) // variabile che modula l'amplificazione dei segnali audio. 1= non cambia niente. 0=amplificazione 0; #define SOUNDGAIN (1.0) // ticker per la generazione dell'onda con DAC Ticker SampleOutTicker; // Timer per il calcolo dei tempi del sensore di prossimità Timer TimerProxSensor; // distanza in cm dell'ostacolo double fDistance; // tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni int nTimerStart, nTimerCurrent, nTimerStop, nTimerTillNow; // Buffer contenente la sinusoide da porre in output come Clacson. unsigned short usaClacson[CLACSONSAMPLENUM]; // prototipo di funzione che genera i campioni della sinusoide da utilizzare per la generazione tramite DAC void CalculateSinewave(void); // Periodo di generazione campioni in output DeltaT = T/NumSample double fDeltaTClacsonSound; double fDeltaTEngineSound; // amplificazione per i suoni da generare con l'ADC double fAmpEngineSound; // rumore di Engine double fAmpClacsonSound; // rumore di Clacson double fAmpShearSound; // rumore di Shear // frequenza segnale audio da generare per clacson e motore double fFreqClacsonSound; double fFreqEngineSound; // periodo della sinusoide audio da generare come suono del clacson double fPeriodClacsonSOund; // numero di campioni di clacson già inviati in output sul DAC int nClacsonSampleCount; // indice dell'array di generazione campioni clacson int nClacsonSampleIndex; // indice dell'Array di generazione suoni del motore volatile int nEngineSampleIndex; // Flag che decide se generare oppure no il suono del motore. '1'=non generare il suono del motore, '0'=genera il suono del motore int bEngineSoundStop; // valore medio della Luminosità su NUMACQUISIZIONI acquisizioni double fAvgLight; // valore numerico, di tensione e di luce letto dall'ADC volatile unsigned short usReadADC; volatile float fReadVoltage; // valore di luminosità letto dall'ADC volatile float fLight; // posizione del Cofano '0' = chiuso, '1'=aperto. Inizialmente DEVE essere chiuso (cioè '0') int nPosizioneCofano=0; // indice per il conteggio dei campioni di luce acquisiti dal fotoresistore int nLightSampleIndex; // timer per il calcolo della velocità Timer TimerHall; // variabile che conta il numero di fronti si salita del segnale encoder del motore di movimento robot volatile int nCountRiseEdge; // variabile che ricorda lo stato di StandBy: '0' = Operativo, '1'=StandBy int nStandBy; // variabile che permette di modificare il Gain di tutti i suoni float fSoundGain=SOUNDGAIN; // inizialmente fissato da un define // pin di pilotaggio Motore DC DigitalOut OutMotorA (PB_0); DigitalOut OutMotorB (PC_1); // Output Digitali usati per i LED DigitalOut LedWAD (PC_2); DigitalOut LedWAS (PC_3); DigitalOut LedWPD (PH_0); DigitalOut LedWPS (PA_0) ; DigitalOut LedYAD (PC_9); DigitalOut LedYAS (PC_8); DigitalOut LedRPD (PA_13); DigitalOut LedRPS (PA_14) ; DigitalOut LedYRAll (PC_7); // COn questo pin si pilotano contemporaneamente i Led: YLD1, YLD2, YLD3, YLD4, YLS1, YLS2, YLS3, YLS4, RPD1, RPS1 // Input/Output Digitali usati per interfaccia RPI DigitalIn InShearRPI (PB_11); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17 DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20 DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore. Collegato al Raspberry GPIO16 DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione. Collegato al Raspberry GPIO13 DigitalIn InFutureUse2RPI (PC_15); // usi futuri 1 di comunicazione. Collegato al Raspberry GPIO25 //DigitalIn InFutureUse1PI (PC_15); // usi futuri 2 di comunicazione. Collegato al Raspberry GPIO12 DigitalIn InStandByRPI (PB_2,PullDown); // StandBy ON/OFF. '1' = robot in StandBy; '0' = robot operativo. Collegato al Raspberry GPIO12 // Input e Output per i sensori e attuatori AnalogOut OutWave(PA_4); // pin A2 di output per la forma d'onda analogica dedicata al suono AnalogIn InWaveLight(PA_1); // pin A1 di input per la forma d'onda analogica dedicata alla luminosità DigitalInOut InOutProxSensor (PC_0, PIN_OUTPUT, PullDown, 0); // Pin di tipo In-Out per la gestione del segnale Sig del Sensore di prossimità a ultrasuoni InterruptIn InEncoderA(PA_9); // Primo Pin di input dall'encoder ottico collegato al motore per misurare lo spostamento //InterruptIn InEncoderB(PC_7); // Secondo Pin di input dall'encoder ottico collegato al motore. predisposizione per usi futuri // Input/Output utilizzati da funzioni default su scheda NUCLEO DigitalOut led2(LED2);// LED verde sulla scheda. Associato a PA_5 Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13 // input di diagnostica DigitalIn InDiag1(PA_15,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN7 pin17-pin19 //DigitalIn InDiag2(PB_11,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20 //**************************** // Create the sinewave buffer //**************************** void CalculateSinewave(int nOffset, int nAmplitude, double fPhase) { // variabile contenente l'angolo in radianti double fRads; // indici per i cicli int nIndex; // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE double fDeltaF; // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF double fAngle; fDeltaF = 360.0/CLACSONSAMPLENUM; for (nIndex = 0; nIndex < CLACSONSAMPLENUM; nIndex++) { fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide fRads = (PI * fAngle)/180.0; // Convert degree in radian //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET; usaClacson[nIndex] = fSoundGain * nAmplitude * cos(fRads + fPhase) + nOffset; } } /********************************************************/ /* Funzione avviata all'inizio come saluto e Benvenuto */ /********************************************************/ void WelcomeMessage() { // indice per i cicli interni alla funzione int nIndex; // indice per l'array di welcome message int nWelcomeMsgIndex; // parametri per generare il messaggio di welcome double fAmpWelcomeSound; double fFreqWelcomeSound; double fDeltaTWelcomeSound; //++++++++++++ INIZIO Accendi le Luci in sequenza +++++++++++++++++ // accendi tutte le luci LedWAD = 1; wait_ms(100); LedWAS = 1; wait_ms(100); LedWPD = 1; wait_ms(100); LedWPS = 1; wait_ms(100); LedYAD = 1; wait_ms(100); LedYAS = 1; wait_ms(100); LedRPD = 1; wait_ms(100); LedRPS = 1; //++++++++++++ FINE Accendi le Luci in sequenza +++++++++++++++++ //++++++++++++ INIZIO generazione messaggio di benvenuto +++++++++++++++++ fAmpWelcomeSound = 1.0; // fissa l'amplificazione per il messaggio di welcome. Valori da 0[min] a 1[max] fFreqWelcomeSound=nSamplePerSecWelcome/nUnderSampleFactorWelcome;// campioni per secondo del welcome message da generare = nSamplePerSec/nUnderSampleFactor fDeltaTWelcomeSound = (1.0/fFreqWelcomeSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento for(nWelcomeMsgIndex=0; nWelcomeMsgIndex < nSampleNumWelcome; nWelcomeMsgIndex++) { // mette in output un campione della forma d'onda del welcome message moltiplicato per l'amplificazione fAmp OutWave.write_u16(naInputSoundWaveWelcome[nWelcomeMsgIndex]*fAmpWelcomeSound*fSoundGain); // tra un campione e l'altro attendi un periodo pari al periodo di campionamento //wait(fDeltaTWelcomeSound); wait_us(37); } //++++++++++++ FINE generazione messaggio di benvenuto +++++++++++++++++ //++++++++++++ INIZIO Spegni le Luci in sequenza +++++++++++++++++ // spegni le Luci in sequenza for(nIndex=0; nIndex<3; nIndex++) { wait_ms(50); LedWAD = 1; wait_ms(50); LedWAD = 0; } for(nIndex=0; nIndex<3; nIndex++) { wait_ms(50); LedWAS = 1; wait_ms(50); LedWAS = 0; } for(nIndex=0; nIndex<3; nIndex++) { wait_ms(50); LedWPD = 1; wait_ms(50); LedWPD = 0; } for(nIndex=0; nIndex<3; nIndex++) { wait_ms(50); LedWPS = 1; wait_ms(50); LedWPS = 0; } for(nIndex=0; nIndex<3; nIndex++) { wait_ms(50); LedYAD = 1; wait_ms(50); LedYAD =0; } for(nIndex=0; nIndex<3; nIndex++) { wait_ms(50); LedYAS = 1; wait_ms(50); LedYAS = 0; } for(nIndex=0; nIndex<3; nIndex++) { wait_ms(50); LedRPD = 1; wait_ms(50); LedRPD = 0; } for(nIndex=0; nIndex<3; nIndex++) { wait_ms(50); LedRPS = 1; wait_ms(50); LedRPS = 0; } for(nIndex=0; nIndex<3; nIndex++) { wait_ms(50); LedYRAll = 1; wait_ms(50); LedYRAll = 0; } //++++++++++++ FINE Spegni le Luci in sequenza +++++++++++++++++ } /***************************************************************************/ /* Genera Messaggio di Arrivederci e spegni i LED quando passa in SyandBy */ /***************************************************************************/ void FarewellMessage() { // indice per l'array di Farewell message int nFarewellMsgIndex; // parametri per generare il messaggio di Farewell double fAmpFarewellSound; double fFreqFarewellSound; double fDeltaTFarewellSound; //++++++++++++ INIZIO generazione messaggio di Arrivederci +++++++++++++++++ fAmpFarewellSound = 1.0; // fissa l'amplificazione per il messaggio di Farewell. Valori da 0[min] a 1[max] fFreqFarewellSound=nSamplePerSecFarewell/nUnderSampleFactorFarewell;// campioni per secondo del Farewell message da generare = nSamplePerSec/nUnderSampleFactor fDeltaTFarewellSound = (1.0/fFreqFarewellSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento for(nFarewellMsgIndex=0; nFarewellMsgIndex < nSampleNumFarewell; nFarewellMsgIndex++) { // mette in output un campione della forma d'onda del Farewell message moltiplicato per l'amplificazione fAmp OutWave.write_u16(naInputSoundWaveFarewell[nFarewellMsgIndex]*fAmpFarewellSound*fSoundGain); // tra un campione e l'altro attendi un periodo pari al periodo di campionamento //wait(fDeltaTFarewellSound); wait_us(57); } //++++++++++++ FINE generazione messaggio di Arrivederci +++++++++++++++++ //++++++++++++ INIZIO Spegni tutti i LED in sequenza +++++++++++++++++ // spegni tutti i LED LedWAD = 0; wait_ms(100); LedWAS = 0; wait_ms(100); LedWPD = 0; wait_ms(100); LedWPS = 0; wait_ms(100); LedYAD = 0; wait_ms(100); LedYAS = 0; wait_ms(100); LedRPD = 0; wait_ms(100); LedRPS = 0; wait_ms(100); LedYRAll = 0; //++++++++++++ FINE Spegni tutti i LED in sequenza +++++++++++++++++ } /***********************************************************************/ /* Genera il suono di una motosega. */ /* Attivo quando arriva il comando di spostamento Cesoie da Raspberry */ /***********************************************************************/ void ShearSoundGeneration() { // indice per l'array di suono Shear int nShearSoundIndex; // parametri per generare il messaggio di shear double fAmpShearSound; double fFreqShearSound; double fDeltaTShearSound; //++++++++++++ INIZIO generazione suono di motosega +++++++++++++++++ fAmpShearSound = 1.0; // fissa l'amplificazione per il suono di Shear. Valori da 0[min] a 1[max] fFreqShearSound=nSamplePerSecShear/nUnderSampleFactorShear;// campioni per secondo del Shear da generare = nSamplePerSec/nUnderSampleFactor fDeltaTShearSound = (1.0/fFreqShearSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento for(nShearSoundIndex=0; nShearSoundIndex < nSampleNumShear; nShearSoundIndex++) { // mette in output un campione della forma d'onda del suono di Shear, moltiplicato per l'amplificazione fAmp OutWave.write_u16(naInputSoundWaveShear[nShearSoundIndex]*fAmpShearSound*fSoundGain); // tra un campione e l'altro attendi un periodo pari al periodo di campionamento wait(fDeltaTShearSound); } //++++++++++++ FINE generazione suono di motosega +++++++++++++++++ } /***********************************************************************/ /* generazione suoni con i sample da file di campioni in SoundSample.h */ /***********************************************************************/ void SampleOut() { // interrompi il suono del motore per generare altri suoni. '1' = interrompi i suoni if(bEngineSoundStop == 0) { // mette in output un campione della forma d'onda del rumore motore moltiplicato per l'amplificazione fAmp OutWave.write_u16(naInputSoundWave[nEngineSampleIndex]*fAmpEngineSound*fSoundGain); // incrementa l'indice del campione in output, nSampleNum è il numero dei campioni nle file Sound.h nEngineSampleIndex++; if(nEngineSampleIndex >= nSampleNum) nEngineSampleIndex=0; } } /**************************************************************************************/ /* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */ /**************************************************************************************/ void riseEncoderIRQ() { nCountRiseEdge++; } /********/ /* Main */ /********/ int main() { // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto pc.baud(921600); //921600 bps // definisci il mode del segnale digitale di EncoderA InEncoderA.mode(PullUp); // Associa routine di Interrup all'evento fronte di salita del segnale di encoder InEncoderA.rise(&riseEncoderIRQ); // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove //InEncoderA.enable_irq(); // definisci il mode del segnale di InStandBy da RPI ('0' = operativo; '1' = StandBy) InStandByRPI.mode(PullDown); //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO TEST ++++++++++++++++++++++++++++++++++ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ /* while(true) { if(myButton == 0) { led2=1; WelcomeMessage(); FarewellMessage(); } else { } } */ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+++++++++++++++++++++++++++++++++++++ FINE CICLO TEST ++++++++++++++++++++++++++++++++++ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO OPERATIVO ++++++++++++++++++++++++++++++++++ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+++++++++++ inizializza Gain dei suoni +++++++++++++ fSoundGain = SOUNDGAIN; // inizialmente fissato a SOUNDGAIN che può essere fissato a 0 per modalità di debug //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++ fFreqClacsonSound = 440.0; // frequenza in Hz del tono del Clacson da generare fAmpClacsonSound = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare fDeltaTClacsonSound = 1.0/(fFreqClacsonSound*CLACSONSAMPLENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmpClacsonSound*fSoundGain), (PI/2.0)); // generazione della sinusoide con valori nominali //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++ // avvia routine di saluto di benvenuto bEngineSoundStop = 1; // per generare il messaggio di benvenuto il suono del motore è spento WelcomeMessage(); bEngineSoundStop = 0; // riattiva il suono del motore //+++++++ INIZIO avvio rumore del motore a frequenza da fermo +++++++++ fAmpEngineSound = 1.0; // fissa l'amplificazione per il rumore motore. Valori da 0[min] a 1[max] fFreqEngineSound=nSamplePerSec/nUnderSampleFactor;// campioni per secondo del rumore motore da generare = nSamplePerSec/nUnderSampleFactor fDeltaTEngineSound = (1.0/fFreqEngineSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento nEngineSampleIndex =0; // Avvia indice di generazione suono motore SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione //+++++++ FINE avvio rumore del motore a frequenza da fermo +++++++++ //inizializza variabili nEngineSampleIndex =0; // avvia l'indice di generazione suoni nCountRiseEdge=0; // azzera il contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA bEngineSoundStop =0; // inizialmente il suono del motore è generato nPosizioneCofano=0; // inizializza la posizione del cofano chiuso nStandBy=0; // iniazializza la modalità StandBy/Operation del robot. nStandBy=0 : modalità Operation while(true) { if(InStandByRPI == 0) { // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove InEncoderA.enable_irq(); // se appena uscito dalla modalità di StandBy, è ancora nStandBy = 1, emetti messaggio di benvenuto if(nStandBy == 1) { // blocca il suono del motore per emettere messaggio di benvenuto bEngineSoundStop=1; // se modalità StandBy = OFF, riattiva audio; fSoundGain = SOUNDGAIN; //Genera messaggio di benvenuto WelcomeMessage(); // rispristina il suono del motore bEngineSoundStop=0; } // imposta lo stato di StandBy OFF nStandBy = 0; //++++++++++ INIZIO calcola spostamento con encoder sul motore +++++++++++++++++ // abilita l'interrupt su fronte di salita del segnale di encoder nCountRiseEdge=0; InEncoderA.enable_irq(); // conta il numero di impulsi del segnale di encoder che si verificano in un timer pari a 500ms TimerHall.start(); nTimerStart=TimerHall.read_ms(); // per 200ms verifica se ci sono impulsi sull'encoder while( (nTimerCurrent-nTimerStart) < 200) // attende il passare di 200ms { nTimerCurrent=TimerHall.read_ms(); // pc.printf("CounterTimer= %d\r\n", (nTimerCurrent-nTimerStart)); } TimerHall.stop(); InEncoderA.disable_irq(); //++++++++++ FINE calcola spostamento con encoder sul motore +++++++++++++++++ //++++++++++ INIZIO genera diverso suono con motore fermo e in movimento +++++++++++++++++ // se nella IRQ sono stati contati fronti di salita del dell'encoder, il robot si sta muovendo if(nCountRiseEdge != 0) //if(InDiag1==1) { // sono stati contati impulsi di encoder, il robot si sta muovendo fDeltaTEngineSound = (0.5/fFreqEngineSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione } else { // se ci sono stati impulsi di encoder, il robot è fermo, genera rumore del motore fermo fDeltaTEngineSound = (1.0/fFreqEngineSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento SampleOutTicker.attach(&SampleOut,fDeltaTEngineSound); // avvia generazione } // riazzera il contatore di impulsi di encoder. Questo contatore viene incrementato nella rouine di interrupt nCountRiseEdge=0; // disabilita interrupt sul segnale di encoder. In questo modo non occupiamo inutilmente la CPU InEncoderA.disable_irq(); // L'interrupt sarà di nuovo abilitato quando si ricomincia il while (true) //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++ //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++ // inizializza il valore medio della Luminosità fAvgLight=0.0; for(nLightSampleIndex=0; nLightSampleIndex < NUMLIGHTSAMPLE; nLightSampleIndex++) { // acquisisce dato da ADC usReadADC = InWaveLight.read_u16(); fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC fAvgLight+=fLight; } // calcola valore medio su NUMSAMPLE acquisizioni fAvgLight/= NUMLIGHTSAMPLE; // Accendi/Spegni i LED Bianchi se il valore medio della luminosità è sotto/sopra soglia if(fAvgLight < SOGLIALUCIMIN) { // Accendi LED Bianchi //led2 = 1; LedWAD = 1; LedWAS = 1; LedWPD = 1; LedWPS = 1; } else { if(fAvgLight > SOGLIALUCIMAX) { // Spegni LED Bianchi //led2 = 0; LedWAD = 0; LedWAS = 0; LedWPD = 0; LedWPS = 0; } } // invia il dato al PC //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight); //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++ //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++ //inizializza misura di distanza fDistance=0.0; // Fissa come Output il pin InOutProxSensor InOutProxSensor.output(); // Poni 'L' sul Pin e mantienilo per qualche microsecondo InOutProxSensor.write(0); wait_us(5); // Poni 'H' sul Pin e mantienilo per qualche microsecondo InOutProxSensor.write(1); wait_us(10); // Poni 'L' sul Pin e mantienilo per qualche microsecondo InOutProxSensor.write(0); // Attendi assestamento e Fissa come Input il pin InOutProxSensor wait_us(5); InOutProxSensor.input(); InOutProxSensor.mode(PullDown); // se non è presente il sensore, il pin rimane a '0' // attende la risposta del sensore di prossimità per un tempo fissato da TIMEOUTPROXSENSOR. Dopo tale tempo dichiara inesistente il sensore TimerProxSensor.start(); nTimerStart = TimerProxSensor.read_us(); nTimerTillNow=(TimerProxSensor.read_us()-nTimerStart); while((InOutProxSensor ==0) && (nTimerTillNow< TIMEOUTPROXSENSOR)) { nTimerCurrent = TimerProxSensor.read_us(); nTimerTillNow=nTimerCurrent-nTimerStart; led2=1; // se rimane nel while il LED rimane acceso pc.printf("sono qui 2 \r\n"); } TimerProxSensor.stop(); // spegne il timer che serve per misurare il timeout quando assente il sensore di prossimità pc.printf("\r\nUscita dal while, nTimerTillNow = %d\r\n", nTimerTillNow); // se nTimerTillNow è inferiore al TIMEOUT, il sensore è presente e quindi misura la distanza dell'ostacolo if(nTimerTillNow < TIMEOUTPROXSENSOR) { // riattiva il timer per misurare la distanza dell'ostacolo TimerProxSensor.start(); nTimerStart = TimerProxSensor.read_us(); while(InOutProxSensor == 1) { led2=1; // se rimane nel while il LED rimane acceso } TimerProxSensor.stop(); nTimerStop = TimerProxSensor.read_us(); pc.printf("\r\nSensore Presente, nTimerTillNow = %d\r\n", nTimerTillNow); // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us] // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us] fDistance = (nTimerStop-nTimerStart)/58.2; // invia il dato al PC pc.printf("distanza dell'ostacolo = %f0.2\r\n", fDistance); } else { // quando esce dai while bloccanti, il LED si spegne led2=0; pc.printf("\r\nTimeOut\r\n"); } //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++ //++++++++++++++ INIZIO Suona Clacson +++++++++ //escludi le misure oltre il max if((fDistance <= 50.0) && (fDistance >= 3)) //if(InDiag1 == 1) { // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima if(fDistance < 22) { // blocca altri suoni quando genera suono del clacson bEngineSoundStop=1; // INIZIO generazione tono nClacsonSampleIndex=0; // Genera il suono del clacson for(nClacsonSampleCount=0; nClacsonSampleCount<7000; nClacsonSampleCount++) { OutWave.write_u16(usaClacson[nClacsonSampleIndex]); //max 32767 //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici wait(fDeltaTClacsonSound); // genera ciclicamente nClacsonSampleIndex++; if(nClacsonSampleIndex >= CLACSONSAMPLENUM) { nClacsonSampleIndex=0; } // a metà genera un wait per doppio clacson if(nClacsonSampleCount == 2000) { wait_ms(100); } } //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente OutWave.write_u16(0); // sblocca altri suoni dopo aver generato suono del clacson bEngineSoundStop=0; } // if(fDistance < soglia) suona clacson } // if( (fDistance < Max) && (fDistance > Min)) //++++++++++++++ FINE Suona Clacson +++++++++ //++++++++++++++ INIZIO pilotaggio motore cofano +++++++++++++++++++ if((InMotorSwitchRPI==1) && (nPosizioneCofano ==0)) //if((myButton==1) && (nPosizioneCofano ==0)) { //Ferma motore OutMotorA=0; OutMotorB=0; //pc.printf("Stop motore; OutA OutB = 00\r\n"); wait_ms(10); //Ferma motore OutMotorA=0; OutMotorB=1; //pc.printf("Stop motore; OutA OutB = 01\r\n"); wait_ms(10); // Ruota Right OutMotorA=1; OutMotorB=1; //pc.printf("Ruota Right; OutA OutB = 11\r\n"); wait_ms(710); // Ferma Motore OutMotorA=0; OutMotorB=1; //pc.printf("Stop Motore; OutA OutB = 01\r\n"); wait_ms(10); //Ferma motore OutMotorA=0; OutMotorB=0; //pc.printf("Stop motore; OutA OutB = 00\r\n"); wait_ms(10); // cambia posizione del cofano. E' Stato Aperto nPosizioneCofano = 1; } // se arriva comando di chiusura cofano & il cofano è aperto, muovi motore //if((myButton==0) && (nPosizioneCofano == 1)) if((InMotorSwitchRPI==0) && (nPosizioneCofano ==1)) { //pc.printf("\r\nCofano aperto & comando di chiusura\r\n"); //Ferma motore OutMotorA=0; OutMotorB=0; //pc.printf("Stop motore; OutA OutB = 00\r\n"); wait_ms(10); // Ruota Left OutMotorA=1; OutMotorB=0; //pc.printf("Ruota Left; OutA OutB = 10\r\n"); wait_ms(730); //Ferma motore OutMotorA=0; OutMotorB=0; //pc.printf("Stop motore; OutA OutB = 00\r\n"); wait_ms(10); // cambia posizione del cofano. E' Stato Chiuso nPosizioneCofano = 0; } //++++++++++++++ FINE Pilotaggio Motore +++++++++++++ //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++ if(InLightSwitchRPI ==1) { // accendi i LED di abbellimento //led2=1; LedYAD = 1; LedYAS = 1; LedRPD = 1; LedRPS = 1; LedYRAll = 1; } else { // spegni i LED di abbellimento //led2=0; LedYAD = 0; LedYAS = 0; LedRPD = 0; LedRPS = 0; LedYRAll = 0; } //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++ //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++ if(InShearRPI == 1) { // funzione di generazione suono motosega bEngineSoundStop=1; // disattiva suono del motore ShearSoundGeneration(); bEngineSoundStop=0; // riattiva suono del motore } //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++ }// if(InStandByRPI == 0) else { // ricevuto da RPI, il comando di StandBy = ON // ricevuto il comando di StandBy (InStandBy == 1) // la prima volta che entra in questo else, la variabile di stato nStandBy è '0'. Solo la prima volta Genera il messaggio di arrivederci if(nStandBy == 0) { // blocca il suono del motore per emettere messaggio di arrivederci bEngineSoundStop=1; //Genera messaggio di arrivederci FarewellMessage(); // rispristina il suono del motore bEngineSoundStop=0; // cambia lo stato dello StandBy nStandBy = 1; } // se modalità StandBy = ON, disattiva audio; fSoundGain = 0.0; } } //while(true) //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+++++++++++++++++++++++++++++++ FINE CICLO OPERATIVO +++++++++++++++++++++++++++++++++++++++++++++ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ }