Amaldi
/
Amaldi_RobotFinale_Rev5_2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Diff: RobotFinale5.cpp
- Revision:
- 21:b08f43a73ecd
- Parent:
- 20:dd351590581d
diff -r dd351590581d -r b08f43a73ecd RobotFinale5.cpp
--- a/RobotFinale5.cpp Fri Mar 22 11:57:01 2019 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,1054 +0,0 @@
-//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Revisione del 21/03/2019
-//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
-// mbed specific header files.
-#include "mbed.h"
-
-// numero di acqusizioni su cui effettuare la media della temperatura
-#define NUMSAMPLE 300
-
-// 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;
-
-
-//***************************
-// Acquisizione da ADC
-//***************************
-
- // ticker per l'acquisizione dell'onda con ADC
-Ticker SamplingTicker;
-
-// carattere in arrivo dal PC ed equivalente numerico
-volatile char cReadChar;
-volatile int nReadChar;
-
-// flag che diventa true quando si vuole fermare l'acquisizione
-volatile bool bStop;
-
-// valore letto dall'ADC e corrispondente in tensione
-//volatile unsigned short usReadADC;
-//volatile float fReadVoltage;
-
-// valore di temperatura letto dall'ADC
-volatile float fTemp;
-
-/*********************************************/
-/*********************************************/
-
-
-// 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
-AnalogIn InWave(PC_0);
-
-// 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 temperatura*/
-/****************************************/
-
-void Sampling()
-{
- // 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
-
- // indice per i cicli
- int nIndex;
- // valore medio della temperatura su NUMACQUISISIONI acquisizioni
- float fAvgTemp;
-
- // se è stato inviato il comando Stop, non fare niente fino a nuovo comando
- if(bStop)
- {
- }
- else // se non è stato inviato il comando di bStop continua
- {
- // inizializza il valore medio della temperatura
- fAvgTemp=0.0;
- for(nIndex=0; nIndex < NUMSAMPLE; nIndex++)
- {
- // acquisisce dato da ADC
- usReadADC = InWave.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
- fTemp= ((fReadVoltage-0.25)*100.0)/(3.05-0.25); //applica la formula della retta tra i valori minimo e massimo del sensore
- fAvgTemp+=fTemp;
- }
- // calcola valore medio su NUMSAMPLE acquisizioni
- fAvgTemp/= NUMSAMPLE;
-
- // accendi LED in base a superamento soglie
- if (fAvgTemp <= 30.0)
- {
- // 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);
- }
- else
- {
- }
-
-
-
-
- // invia il dato al PC
- //pc.printf("\n\r--- Voltage= %.1f [Volt]; Temperature= %.1f [Celsius] ---\n\r", fReadVoltage, fTemp);
- pc.printf("\n\r--- Digital= %d [Volt]; Temperature= %.2f [Celsius] ---\n\r", usReadADC, fTemp);
-
-
-
- /*
- // prepara il pacchetto di dati acquisiti da restituire al PC
- caTxPacket[nSampleInCount]= (char)(usReadADC&0xFF);
- //+++caTxPacket[nSampleInCount]= 'a';
- nSampleInCount++;
- caTxPacket[nSampleInCount] = (char)((usReadADC>>8)&0xFF);
- //++++caTxPacket[nSampleInCount]= 'b';
- */
-
- }
-
-
-
-/**************************************/
-/* 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++++++++++++++++++++++++++++++++
- //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
-
-
-
-
-
-