Giuseppe Falagario
/
Amaldi_RobotFinale_Rev4
Robot Amaldi Rev 4.0 Messaggio don't touch me
Diff: RobotFinale3-3.cpp
- Revision:
- 16:17df60afe302
- Parent:
- 15:ef808556423b
diff -r ef808556423b -r 17df60afe302 RobotFinale3-3.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RobotFinale3-3.cpp Tue Mar 05 17:44:30 2019 +0000 @@ -0,0 +1,846 @@ +//++++++++++++++++++++ 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 +++++++++++++++++++++++++++++++++++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + + +} +