Accende/spegne LED se in movimento/fermo Emette Messaggio "don't touch me" anche se ostacolo a distanza < 10cm
Dependencies: mbed
Revision 19:eb21b7baa88b, committed 2019-03-20
- Comitter:
- pinofal
- Date:
- Wed Mar 20 12:46:02 2019 +0000
- Parent:
- 18:f1faab783b47
- Commit message:
- Accende/spegne LED se in movimento/fermo ; Emette Messaggio "don't touch me" anche se ostacolo a distanza < 10cm
Changed in this revision
RobotFinale4.cpp | Show diff for this revision Revisions of this file |
RobotFinale5.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/RobotFinale4.cpp Mon Mar 11 21:42:01 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1267 +0,0 @@ -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ -// Revisione del 16/03/2019 -//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - -// 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 "SampledSoundDontTouch.h" // Messaggio di Don't Touch Me - -//#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 - -// sensore di prossimità. '1' = Sensore Presente, '0' = Sesnore Assente -int nProximitySensorPresent; - -// 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, PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17 -DigitalIn InLightSwitchRPI (PB_9, PullDown); // accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20 -DigitalIn InMotorSwitchRPI (PB_8, PullDown); // accende e spegne il motore del Cofano. Collegato al Raspberry GPIO16 -//DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione. Collegato al Raspberry GPIO13 -DigitalIn InDontTouchRPI (PB_7, PullDown); // 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 Messaggio di Don't Touch Me */ -/**************************************/ -void DontTouchMessage() -{ - // indice per l'array di DontTouch message - int nDontTouchMsgIndex; - // parametri per generare il messaggio di DontTouch - double fAmpDontTouchSound; - double fFreqDontTouchSound; - double fDeltaTDontTouchSound; - - - - //++++++++++++ INIZIO generazione messaggio di Don't Touch +++++++++++++++++ - fAmpDontTouchSound = 1.0; // fissa l'amplificazione per il messaggio di DontTouch. Valori da 0[min] a 1[max] - fFreqDontTouchSound=nSamplePerSecDontTouch/nUnderSampleFactorDontTouch;// campioni per secondo del DontTouch message da generare = nSamplePerSec/nUnderSampleFactor - fDeltaTDontTouchSound = (1.0/fFreqDontTouchSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento - - - for(nDontTouchMsgIndex=0; nDontTouchMsgIndex < nSampleNumDontTouch; nDontTouchMsgIndex++) - { - // mette in output un campione della forma d'onda del DontTouch message moltiplicato per l'amplificazione fAmp - OutWave.write_u16(naInputSoundWaveDontTouch[nDontTouchMsgIndex]*fAmpDontTouchSound*fSoundGain); - - // tra un campione e l'altro attendi un periodo pari al periodo di campionamento - //wait(fDeltaTDontTouchSound); - wait_us(57); - } - //++++++++++++ FINE generazione messaggio di Don't Touch +++++++++++++++++ - - //++++++++++++ INIZIO ACCENDI tutti i LED in sequenza e spegnili subito dopo +++++++++++++++++ - // spegni tutti i LED - LedWAD = 1; - wait_ms(50); - LedWAS = 1; - wait_ms(50); - LedWPD = 1; - wait_ms(50); - LedWPS = 1; - wait_ms(50); - LedYAD = 1; - wait_ms(50); - LedYAS = 1; - wait_ms(50); - LedRPD = 1; - wait_ms(50); - LedRPS = 1; - wait_ms(50); - LedYRAll = 1; - wait(1); - LedWAD = 0; - LedWAS = 0; - LedWPD = 0; - LedWPS = 0; - LedYAD = 0; - LedYAS = 0; - LedRPD = 0; - LedRPS = 0; - LedYRAll = 0; - //++++++++++++ FINE ACCENDI tutti i LED in sequenza e spegnili subito dopo +++++++++++++++++ - -} - - - -/***********************************************************************/ -/* 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); - InShearRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17 - InLightSwitchRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando che accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20 - InMotorSwitchRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando che accende e spegne il motore del Cofano. Collegato al Raspberry GPIO16 - InDontTouchRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando per generare messaggio Audio "don't Touch me" GPIO13 - - - - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - //+++++++++++++++++++++++++++++++++++++ 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 = 550.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 - - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO OPERATIVO +++++++++++++++++++++++++++++ - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - // Fissa come Output il pin InOutProxSensor - 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 interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove - //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 100ms verifica se ci sono impulsi sull'encoder. Gli impulsi vengono contati lungo tutto il ciclo ma se il ciclo è attraversato troppo rapidamente rischio di non contaniente e aggiungo questo ritarda. - while( (nTimerCurrent-nTimerStart) < 50) // attende il passare di 100ms - { - 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 - //pc.printf("\r\nIn Movimento \r\n"); //Diagnostica - } - else - { - // se non 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 - //pc.printf("\r\nFermo \r\n"); //Diagnostica - } - // riazzera il contatore di impulsi di encoder. Questo contatore viene incrementato nella rouine di interrupt - nCountRiseEdge=0; - //++++++++++ 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"); // Diagnotica - } - 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); // Diagnostica - // 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); // Diagnostica - - // 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); // Diagnostica - } - - //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++ - //++++++++++++++ INIZIO Suona Clacson +++++++++ - //escludi le misure oltre il max - //if(myButton == 0) fDistance = 20; //Diagnostica - 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 Cofano +++++++++++++ - - //++++++++++++++ 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 - } - //++++++++++++++ FINE Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++ - //++++++++++++++ INIZIO Genera Messaggio Audio "Don't Touch Me" quando arriva il comando da Raspberry +++++++++ - if ((InDontTouchRPI == 1) || (myButton == 0)) // myButton premuto per scopi Diagnostici - { - // funzione di generazione suono motosega - bEngineSoundStop=1; // disattiva suono del motore - DontTouchMessage(); - bEngineSoundStop=0; // riattiva suono del motore - } - //++++++++++++++ Fine Genera Messaggio Audio "Don't Touch Me" quando arriva il comando 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; - } // fine if(nStandByRPI == 1) - - } // fine ciclo while(true) - - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - //+++++++++++++++++++++++++++++++++++++ FINE CICLO OPERATIVO++++++++++++++++++++++++++++++++ - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - - - - - - - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO TEST ++++++++++++++++++++++++++++++++ - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - /* - 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 - } - //++++++++++++++ FINE 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 TEST ++++++++++++++++++++++++++++++++ - //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ - - -} -
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RobotFinale5.cpp Wed Mar 20 12:46:02 2019 +0000 @@ -0,0 +1,938 @@ +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// Revisione del 21/03/2019 +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + +// 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 "SampledSoundDontTouch.h" // Messaggio di Don't Touch Me + +//#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 + +// sensore di prossimità. '1' = Sensore Presente, '0' = Sesnore Assente +int nProximitySensorPresent; + +// 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, PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17 +DigitalIn InLightSwitchRPI (PB_9, PullDown); // accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20 +DigitalIn InMotorSwitchRPI (PB_8, PullDown); // accende e spegne il motore del Cofano. Collegato al Raspberry GPIO16 +//DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione. Collegato al Raspberry GPIO13 +DigitalIn InDontTouchRPI (PB_7, PullDown); // 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 Messaggio di Don't Touch Me */ +/**************************************/ +void DontTouchMessage() +{ + // indice per l'array di DontTouch message + int nDontTouchMsgIndex; + // parametri per generare il messaggio di DontTouch + double fAmpDontTouchSound; + //double fFreqDontTouchSound; + //double fDeltaTDontTouchSound; + + + + //++++++++++++ INIZIO generazione messaggio di Don't Touch +++++++++++++++++ + fAmpDontTouchSound = 1.0; // fissa l'amplificazione per il messaggio di DontTouch. Valori da 0[min] a 1[max] + //fFreqDontTouchSound=nSamplePerSecDontTouch/nUnderSampleFactorDontTouch;// campioni per secondo del DontTouch message da generare = nSamplePerSec/nUnderSampleFactor + //fDeltaTDontTouchSound = (1.0/fFreqDontTouchSound); // fFreq dipende dal periodo di campionamento e dal fattore di sottocampionamento + + + for(nDontTouchMsgIndex=0; nDontTouchMsgIndex < nSampleNumDontTouch; nDontTouchMsgIndex++) + { + // mette in output un campione della forma d'onda del DontTouch message moltiplicato per l'amplificazione fAmp + OutWave.write_u16(naInputSoundWaveDontTouch[nDontTouchMsgIndex]*fAmpDontTouchSound*fSoundGain); + + // tra un campione e l'altro attendi un periodo pari al periodo di campionamento + //wait(fDeltaTDontTouchSound); + wait_us(57); + } + //++++++++++++ FINE generazione messaggio di Don't Touch +++++++++++++++++ + + //++++++++++++ INIZIO ACCENDI tutti i LED in sequenza e spegnili subito dopo +++++++++++++++++ + // spegni tutti i LED + LedWAD = 1; + wait_ms(50); + LedWAS = 1; + wait_ms(50); + LedWPD = 1; + wait_ms(50); + LedWPS = 1; + wait_ms(50); + LedYAD = 1; + wait_ms(50); + LedYAS = 1; + wait_ms(50); + LedRPD = 1; + wait_ms(50); + LedRPS = 1; + wait_ms(50); + LedYRAll = 1; + wait(1); + LedWAD = 0; + LedWAS = 0; + LedWPD = 0; + LedWPS = 0; + LedYAD = 0; + LedYAS = 0; + LedRPD = 0; + LedRPS = 0; + LedYRAll = 0; + //++++++++++++ FINE ACCENDI tutti i LED in sequenza e spegnili subito dopo +++++++++++++++++ + +} + + + +/***********************************************************************/ +/* 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); + InShearRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17 + InLightSwitchRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando che accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20 + InMotorSwitchRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando che accende e spegne il motore del Cofano. Collegato al Raspberry GPIO16 + InDontTouchRPI.mode(PullDown); // arriva un segnale alto su questo input quando Raspberry Invia un comando per generare messaggio Audio "don't Touch me" GPIO13 + + + + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + //+++++++++++++++++++++++++++++++++++++ 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 = 550.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 ++++++++++++++++++++++++++++++++++++++++++++++++++ + + //++++++++ INIZIO 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 + //++++++++ FINE inizializza variabili +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + + + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO TEST ++++++++++++++++++++++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + /* + while(true) + { + } //while(true) + */ + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + //+++++++++++++++++++++++++++++++++++++ FINE CICLO TEST ++++++++++++++++++++++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + + + + + + + + + // Fissa come Output il pin InOutProxSensor + 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 interrupt sul segnale di encoder per contare il numero di impulsi e quindi verificare se il robot si muove + //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 100ms verifica se ci sono impulsi sull'encoder. Gli impulsi vengono contati lungo tutto il ciclo ma se il ciclo è attraversato troppo rapidamente rischio di non contaniente e aggiungo questo ritarda. + while( (nTimerCurrent-nTimerStart) < 50) // attende il passare di 100ms + { + 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 + //pc.printf("\r\nIn Movimento \r\n"); //Diagnostica + + // se il robot si muove, accendi LED indicatori di movimento + LedYAD = 1; + LedYAS = 1; + LedRPD = 1; + LedRPS = 1; + } + else + { + // se non 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 + //pc.printf("\r\nFermo \r\n"); //Diagnostica + + // se il robot è fermo, spegni LED indicatori di movimento + LedYAD = 0; + LedYAS = 0; + LedRPD = 0; + LedRPS = 0; + } + // riazzera il contatore di impulsi di encoder. Questo contatore viene incrementato nella rouine di interrupt + nCountRiseEdge=0; + //++++++++++ 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"); // Diagnotica + } + 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); // Diagnostica + // 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); // Diagnostica + + // 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); // Diagnostica + } + + //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++ + //++++++++++++++ INIZIO Suona Clacson +++++++++ + //escludi le misure oltre il max + //if(myButton == 0) fDistance = 20; //Diagnostica + if((fDistance <= 50.0) && (fDistance >= 3)) + //if(InDiag1 == 1) + { + // SUONA IL CLACSON se l'ostacolo si trova ad una distanza tra 10 e 22cm oppure emetti messaggio DON'T TOUCH ME se l'ostacolo è più vicino di 10cm + if(fDistance < 30) + { + // se la distanza a cui si trova l'ostacolo è inferiore a 10cm, genera messaggio "Don't touch me" + if(fDistance < 10) + { + // funzione di generazione messaggio DOn't Touch me + bEngineSoundStop=1; // disattiva suono del motore + DontTouchMessage(); // genera messaggio DON'T TOUCH ME + bEngineSoundStop=0; // riattiva suono del motore + } + else + { + // L'ostacolo si trova tra 10cm e 22cm, GENERA SUONO DEL CLACSON + // 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 < 10) + + } // if(fDistance < 22)) + + } // if( (fDistance < 50) && (fDistance > 3)) + //++++++++++++++ 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 Cofano +++++++++++++ + + //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++ + if(InLightSwitchRPI ==1) + { + // accendi i LED di abbellimento + LedYRAll = 1; + } + else + { + // spegni i LED di abbellimento + 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 + } + //++++++++++++++ FINE Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++ + + //++++++++++++++ INIZIO Genera Messaggio Audio "Don't Touch Me" quando arriva il comando da Raspberry +++++++++ + if (InDontTouchRPI == 1) + { + // funzione di generazione messaggio DOn't Touch me + bEngineSoundStop=1; // disattiva suono del motore + DontTouchMessage(); + bEngineSoundStop=0; // riattiva suono del motore + } + //++++++++++++++ Fine Genera Messaggio Audio "Don't Touch Me" quando arriva il comando 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; + } // fine if(nStandByRPI == 1) + + } // fine ciclo while(true) + + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + //+++++++++++++++++++++++++++++++++++++ FINE CICLO OPERATIVO++++++++++++++++++++++++++++++++ + //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ + +} +