Amaldi
/
Amaldi_RobotFinale_Rev2-4_FUNZIONA
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: RobotFinale.cpp
- Revision:
- 14:d58d474fae35
- Parent:
- 13:459e008582b3
--- a/RobotFinale.cpp Sat Nov 24 13:30:24 2018 +0000
+++ b/RobotFinale.cpp Thu Feb 14 16:32:40 2019 +0000
@@ -1,81 +1,84 @@
-/* mbed specific header files. */
+// mbed specific header files.
#include "mbed.h"
-/* Component specific header files. */
-#include "L6208.h"
+// include suono del motore
+#include "SampledSoundGurgle.h" // rumore del motore da fermo durante gli spsotamenti
+//#include "Gurgle.h"
+#include "SampledSoundWelcome.h" // messaggio di benvenuto
+#include "SampledSoundMotosega.h" // rumore durante lo spostamento con Cesoia
+
+//#include "SampledSoundMotosega.h"
+//#include "SampledSoundTrattore.h"
+
+
+// TimeOut in [microsec] per verificare la presenza del sensore prossimità. Se il sensore non è presente il timer supera TIMEOUTPROXSENSOR
+#define TIMEOUTPROXSENSOR 1000 //tempo in [microsec]
// numero di campioni che compongono un periodo della sinusoide in Output sull'ADC
-#define SAMPLESINENUM 45 // consigliabile avere multipli di 45
+#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)
-// Parametro di soglia per la luce. Accendi Luci se la luminosità scende sotto SOGLIALUCI
-#define SOGLIALUCI (1.5)
-// Definizione Parametri per il motore
-#ifdef TARGET_NUCLEO_F334R8
-#define VREFA_PWM_PIN D11
-#define VREFB_PWM_PIN D9
-#elif TARGET_NUCLEO_F302R8
-#define VREFA_PWM_PIN D11
-#define VREFB_PWM_PIN D15 // HW mandatory patch: bridge manually D9 with D15
-#else
-#define VREFA_PWM_PIN D3
-#define VREFB_PWM_PIN D9
-#endif
+// ticker per la generazione dell'onda con DAC
+Ticker SampleOutTicker;
-// definizioni funzioni di default su scheda
-Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC
-DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda
-DigitalOut led2(LED2);// LED verde sulla scheda
-// pin A2 di output per la forma d'onda analogica dedicata al suono
-AnalogOut OutWave(PA_4);
-
-// pin A1 di input per la forma d'onda analogica dedicata alla luminosità
-AnalogIn InWaveLight(PA_1);
-
-// Pin di tipo In-Out per la gestione del segnale Sig del Sensore di prossimità a ultrasuoni
-DigitalInOut myProx (PC_0, PIN_OUTPUT, PullNone, 0);
// Timer per il calcolo dei tempi del sensore di prossimità
-Timer myTimer;
-
-// tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni
-int nTimerStart, nTimer, nTimerStop;
+Timer TimerProxSensor;
// distanza in cm dell'ostacolo
double fDistance;
-// Buffer contenente la sinusoide da porre in output.
-unsigned short usaSine[SAMPLESINENUM];
+
+// 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 fDeltaT;
-// amplificazione per il dato da spedire sull'ADC
-double fAmp;
+double fDeltaTClacsonSound;
+double fDeltaTEngineSound;
-// frequenza segnale audio da generare
-double fFreq;
+// 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
-double fPeriod;
+// periodo della sinusoide audio da generare come suono del clacson
+double fPeriodClacsonSOund;
-// indice per i cicli
-int nIndex;
+// numero di campioni di clacson già inviati in output sul DAC
+int nClacsonSampleCount;
+// indice dell'array di generazione campioni clacson
+int nClacsonSampleIndex;
-// numero di campioni di onda sonora già generati
-int nSampleCount;
+// 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
@@ -88,46 +91,59 @@
// 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
+volatile int nCountRiseEdge;
+
+// 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_6);
-DigitalOut LedYAS (PC_9);
+DigitalOut LedYAD (PC_9);
+DigitalOut LedYAS (PC_8);
DigitalOut LedRPD (PA_13);
DigitalOut LedRPS (PA_14) ;
// Input/Output Digitali usati per interfaccia RPI
-DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle = GPIO20
-DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore = RPI GPIO16
-DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione = RPI GPIO13
-DigitalIn InFutureUse1RPI (PB_2); // usi futuri 1 di comunicazione = RPI GPIO25
-DigitalIn InFutureUse2RPI (PC_15); // usi futuri 2 di comunicazione = RPI GPIO12
+DigitalIn InShearRPI (PB_11); // arriva un segnale alto su questo input quando Raspberry Invia un comando di apertura/chiusura cesoie. Collegato a Raspberry GPIO17
+DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle. Collegato al Raspberry GPIO20
+DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore. Collegato al Raspberry GPIO16
+DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione. Collegato al Raspberry GPIO13
+DigitalIn InFutureUse1RPI (PB_2); // usi futuri 1 di comunicazione. Collegato al Raspberry GPIO25
+DigitalIn InFutureUse2RPI (PC_15); // usi futuri 2 di comunicazione. Collegato al Raspberry GPIO12
-/****************************************************************************/
-/* Initialization parameters of the motor connected to the expansion board. */
-/****************************************************************************/
-l6208_init_t init =
-{
-
- 1500, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
- 20, //Acceleration current torque in % (from 0 to 100)
- 1500, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
- 30, //Deceleration current torque in % (from 0 to 100)
- 1500, //Running speed in step/s or (1/16)th step/s for microstep modes
- 50, //Running current torque in % (from 0 to 100)
- 20, //Holding current torque in % (from 0 to 100)
- STEP_MODE_1_16, //Step mode via enum motorStepMode_t
- FAST_DECAY, //Decay mode via enum motorDecayMode_t
- 0, //Dwelling time in ms
- FALSE, //Automatic HIZ STOP
- 100000 //VREFA and VREFB PWM frequency (Hz)
-};
+// 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
-/* Motor Control Component. */
-L6208 *motor;
+// 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
//****************************
@@ -144,13 +160,13 @@
// angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF
double fAngle;
- fDeltaF = 360.0/SAMPLESINENUM;
- for (nIndex = 0; nIndex < SAMPLESINENUM; nIndex++)
+ 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;
- usaSine[nIndex] = nAmplitude * cos(fRads + fPhase) + nOffset;
+ usaClacson[nIndex] = nAmplitude * cos(fRads + fPhase) + nOffset;
}
}
@@ -159,42 +175,173 @@
/********************************************************/
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(300);
+ LedWAS = 1;
+ wait_ms(300);
+ LedWPD = 1;
+ wait_ms(300);
+ LedWPS = 1;
+ wait_ms(300);
+ LedYAD = 1;
+ wait_ms(300);
+ LedYAS = 1;
+ wait_ms(300);
+ LedRPD = 1;
+ wait_ms(300);
+ 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);
+
+ // tra un campione e l'altro attendi un periodo pari al periodo di campionamento
+ //wait(fDeltaTWelcomeSound);
+ wait_us(50);
+ }
+ //++++++++++++ FINE generazione messaggio di benvenuto +++++++++++++++++
+
+ //++++++++++++ INIZIO Spegni le Luci in sequenza +++++++++++++++++
+ // spegni le Luci in sequenza
+ for(nIndex=0; nIndex<3; nIndex++)
+ {
+ wait_ms(100);
+ LedWAD = 1;
+ wait_ms(100);
+ LedWAD = 0;
+ }
+ for(nIndex=0; nIndex<3; nIndex++)
+ {
+ wait_ms(100);
+ LedWAS = 1;
+ wait_ms(100);
+ LedWAS = 0;
+ }
+ for(nIndex=0; nIndex<3; nIndex++)
+ {
+ wait_ms(100);
+ LedWPD = 1;
+ wait_ms(100);
+ LedWPD = 0;
+ }
+ for(nIndex=0; nIndex<3; nIndex++)
+ {
+ wait_ms(100);
+ LedWPS = 1;
+ wait_ms(100);
+ LedWPS = 0;
+ }
+ for(nIndex=0; nIndex<3; nIndex++)
+ {
+ wait_ms(100);
+ LedYAD = 1;
+ wait_ms(100);
+ LedYAD =0;
+ }
+ for(nIndex=0; nIndex<3; nIndex++)
+ {
+ wait_ms(100);
+ LedYAS = 1;
+ wait_ms(100);
+ LedYAS = 0;
+ }
+ for(nIndex=0; nIndex<3; nIndex++)
+ {
+ wait_ms(100);
+ LedRPD = 1;
+ wait_ms(100);
+ LedRPD = 0;
+ }
+ for(nIndex=0; nIndex<3; nIndex++)
+ {
+ wait_ms(100);
+ LedRPS = 1;
+ wait_ms(100);
+ LedRPS = 0;
+ }
+ //++++++++++++ FINE Spegni le Luci in sequenza +++++++++++++++++
+
}
-/********************************************************************/
-/* brief This is an example of user handler for the flag interrupt.*/
-/* param None */
-/* retval None */
-/* note If needed, implement it, and then attach and enable it: */
-/* + motor->attach_flag_irq(&my_flag_irq_handler); */
-/* + motor->enable_flag_irq(); */
-/* To disable it: */
-/* + motor->DisbleFlagIRQ(); */
-/*********************************************************************/
-void my_flag_irq_handler(void)
+
+/***********************************************************************/
+/* Genera il suono di una motosega. */
+/* Attivo quando arriva il comando di spostamento Cesoie da Raspberry */
+/***********************************************************************/
+void ShearSoundGeneration()
{
- pc.printf(" WARNING: \"FLAG\" interrupt triggered:\r\n");
- motor->disable();
- pc.printf(" Motor disabled.\r\n\n");
+ // 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);
+
+ // 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);
+ // incrementa l'indice del campione in output, nSampleNum è il numero dei campioni nle file Sound.h
+ nEngineSampleIndex++;
+ if(nEngineSampleIndex >= nSampleNum)
+ nEngineSampleIndex=0;
+ }
}
-/***************************************************************
- * brief This is an example of error handler.
- * param[in] error Number of the error
- * retval None
- * note If needed, implement it, and then attach it:
- * + motor->attach_error_handler(&my_error_handler);
- **************************************************************/
-void my_error_handler(uint16_t error)
+
+ /**************************************************************************************/
+/* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
+/**************************************************************************************/
+void riseEncoderIRQ()
{
- /* Printing to the console. */
- pc.printf("Error %d detected\r\n\n", error);
-
- /* Infinite loop */
- while (true)
- {}
-}
-
+ nCountRiseEdge++;
+}
+
/********/
/* Main */
/********/
@@ -202,101 +349,285 @@
{
// configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
pc.baud(921600); //921600 bps
- //pc.baud(9600); //256000 bps
- pc.printf("*** Test Motor ***\n\r");
- /* Initializing Motor Control Component. */
- motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
- if (motor->init(&init) != COMPONENT_OK)
- {
- exit(EXIT_FAILURE);
- }
-
- /* Attaching and enabling an interrupt handler. */
- motor->attach_flag_irq(&my_flag_irq_handler);
- motor->enable_flag_irq();
-
- /* Attaching an error handler */
- motor->attach_error_handler(&my_error_handler);
-
- /* Printing to the console. */
- pc.printf("Motor Control Application Example for 1 Motor\r\n");
-
- //----- run the motor BACKWARD
- pc.printf("--> Running the motor backward.\r\n");
- motor->run(StepperMotor::BWD);
-
+ // definisci il mode del segnale digitale di EncoderA
+ InEncoderA.mode(PullUp);
- //----- Soft stop required while running
- pc.printf("--> Soft stop requested.\r\n");
- motor->soft_stop();
-
-
- //----- Change step mode to full step mode
- motor->set_step_mode(StepperMotor::STEP_MODE_FULL);
- pc.printf(" Motor step mode: %d (0:FS, 1:1/2, 2:1/4, 3:1/8, 4:1/16).\r\n", motor->get_step_mode());
-
- /* Get current position of device and print to the console */
- pc.printf(" Position: %d.\r\n", motor->get_position());
+ // 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();
- /* Set speed, acceleration and deceleration to scale with normal mode */
- motor->set_max_speed(init.maxSpeedSps>>4);
- motor->set_acceleration(motor->get_acceleration()>>4);
- motor->set_deceleration(motor->get_deceleration()>>4);
- /* Print parameters to the console */
- pc.printf(" Motor Max Speed: %d step/s.\r\n", motor->get_max_speed());
- pc.printf(" Motor Min Speed: %d step/s.\r\n", motor->get_min_speed());
- pc.printf(" Motor Acceleration: %d step/s.\r\n", motor->get_acceleration());
- pc.printf(" Motor Deceleration: %d step/s.\r\n", motor->get_deceleration());
-
- //----- move of 200 steps in the FW direction
- pc.printf("--> Moving forward 200 steps.\r\n");
- motor->move(StepperMotor::FWD, 200);
+ // avvia routine di saluto di benvenuto
+ WelcomeMessage();
+
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ //+++++++++++++++++++++++++++++++++++++ INIZIO CICLO TEST ++++++++++++++++++++++++++++++++++
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
- fFreq = 440.0; // frequenza in Hz del tono da generare
- fAmp = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
- fDeltaT = 1.0/(fFreq*SAMPLESINENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
- CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmp), (PI/2.0)); // generazione della sinusoide con valori nominali
+ fFreqClacsonSound = 440.0; // frequenza in Hz del tono del Clacson da generare
+ fAmpClacsonSound = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
+ fDeltaTClacsonSound = 1.0/(fFreqClacsonSound*CLACSONSAMPLENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
+ CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmpClacsonSound), (PI/2.0)); // generazione della sinusoide con valori nominali
//+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
-
- /*
+
+ //+++++++ 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 ruomre 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
while(true)
{
+ //++++++++++ 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
+
+ }
+ nCountRiseEdge=0;
+ //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++
- }
- */
-
- //++++ INIZIO Ciclo Principale ++++
- while (true)
- {
- //++++++++++++++ INIZIO Pilotaggio Motore su comando da Raspberry+++++++++++++
- if(InMotorSwitchRPI==1)
+ //++++++++++++ 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
{
- // Request device to go position -3200
- motor->go_to(150);
- // Waiting while the motor is active.
- motor->wait_while_active();
+ // quando esce dai while bloccanti, il LED si spegne
+ led2=0;
+ pc.printf("\r\nTimeOut\r\n");
}
- else
+ //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++
+ //++++++++++++++ INIZIO Suona Clacson +++++++++
+ //escludi le misure oltre il max
+ if((fDistance <= 50.0) && (fDistance >= 3))
+ //if(InDiag1 == 1)
{
- // Request device to go position -3200
- motor->go_to(-150);
- // Waiting while the motor is active.
- motor->wait_while_active();
- }
+ // 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;
-
}
else
{
@@ -309,26 +640,201 @@
LedRPS = 0;
}
- //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
+ //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++
+
+ //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
+ if(InShearRPI == 1)
+ {
+ // funzione di generazione suono motosega
+ bEngineSoundStop=1; // disattiva suono del motore
+ ShearSoundGeneration();
+ bEngineSoundStop=0; // riattiva suono del motore
+ }
+ //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
+ }
+ //+++++++++++++++++++++++++++++++ FINE CICLO TEST ++++++++++++++++++++++++++++++++++++++++++++
+
+
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ //+++++++++++++++++++++++++++++++ INIZIO CICLO OPERATIVO ++++++++++++++++++++++++++++++++++++++++++++
+ //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
+ fFreqClacsonSound = 440.0; // frequenza in Hz del tono del Clacson da generare
+ fAmpClacsonSound = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
+ fDeltaTClacsonSound = 1.0/(fFreqClacsonSound*CLACSONSAMPLENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
+ CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmpClacsonSound), (PI/2.0)); // generazione della sinusoide con valori nominali
+ //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
+
- //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++
- // inizializza il valore medio della Luminosità
- fAvgLight=0.0;
- for(nIndex=0; nIndex < NUMLIGHTSAMPLE; nIndex++)
- {
+ //+++++++ INIZIO avvio ruomre 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 ruomre 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
+
+ //++++ INIZIO Ciclo Principale ++++
+ while (true)
+ {
+ //++++++++++ 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 500ms verifica se ci sono impulsi sull'encoder
+ while( (nTimerCurrent-nTimerStart) < 300) // attende il passare di 300ms
+ {
+ 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
+ }
+ //++++++++++ FINE genera diverso suono con motore fermo e in movimento +++++++++++++++++
+
+ //++++++++++++++ INIZIO Pilotaggio Motore su comando da Raspberry+++++++++++++
+ // se arriva comando di apertura & il cofano è chiuso, muovi motore
+ if((InMotorSwitchRPI==1) && (nPosizioneCofano ==0))
+ {
+ //pc.printf("\r\ncofano chiuso & comando di apertura\r\n");
+
+ //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))
+ {
+ //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;
+ }
+ else
+ {
+
+ // spegni i LED di abbellimento
+ //led2=0;
+ LedYAD = 0;
+ LedYAS = 0;
+ LedRPD = 0;
+ LedRPS = 0;
+
+ }
+ //++++++++++++++ FINE Accensione LED da comando Raspberry +++++++
+
+ //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
+ if(InShearRPI == 1)
+ {
+ // funzione di generazione suono motosega
+ bEngineSoundStop=1; // disattiva suono del motore
+ ShearSoundGeneration();
+ bEngineSoundStop=0; // riattiva suono del motore
+ }
+ //++++++++++++++ INIZIO Genera Suono MOTOSEGA quando arriva comando di movimento Cesoie da Raspberry +++++++++
+
+
+ //++++++++++++ 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 LED Bianchi se illuminazione è sottosoglia
- if(fAvgLight < SOGLIALUCI)
- {
-
+ }
+ // calcola valore medio su NUMSAMPLE acquisizioni
+ fAvgLight/= NUMLIGHTSAMPLE;
+
+ // Accendi/Spegni i LED Bianchi se il valore medio della luminosità è sotto/sopra soglia
+ if(fAvgLight > SOGLIALUCIMAX)
+ {
// Accendi LED Bianchi
//led2 = 1;
LedWAD = 1;
@@ -336,114 +842,132 @@
LedWPD = 1;
LedWPS = 1;
- }
- else
- {
+ }
+ else
+ {
+ if(fAvgLight < SOGLIALUCIMIN)
+ {
+
+ // 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();
- // 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 ++++++++++++++
+ 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 Acquisisci distanza ostacoli +++++++++
- //inizializza misura di distanza
- fDistance=0.0;
- // Fissa come Output il pin myProx
- myProx.output();
- // Poni 'L' sul Pin e mantienilo per qualche microsecondo
- myProx.write(0);
- wait_us(5);
- // Poni 'H' sul Pin e mantienilo per qualche microsecondo
- myProx.write(1);
- wait_us(10);
- // Poni 'L' sul Pin e mantienilo per qualche microsecondo
- myProx.write(0);
- // Attendi assestamento e Fissa come Input il pin myProx
- wait_us(5);
- myProx.input();
-
- // misura il tempo per cui il pin rimane alto. E' il tempo necessario al suono per raggiungere l'ostacolo e ritornare sul sensore
- nTimer =0;
- /*
- myTimer.start(); // avvia il timer per verificare la presenza del sensore
- while((myProx ==0) && (nTimer <=50000)) // esci se il senore risponde oppure se passano oltre 10ms
- {
- // misura il tempo passato
- nTimer=myTimer.read_us();
- }
- */
- while(myProx ==0)
- {}
- myTimer.stop(); // in ogni caso ferma il timer
- // vai avanti solo se il sensore ha risposto
- //if(nTimer <= 50000) // se è passato più tempo il sensore non è presente
- {
- myTimer.start();
- nTimerStart = myTimer.read_us();
- while(myProx == 1)
- {}
- myTimer.stop();
- nTimerStop = myTimer.read_us();
-
-
- // 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("\n\r The Distance was = %.2f [cm]\n\r", fDistance);
-
- }
- //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++
-
- //++++++++++++++ INIZIO Suona Clacson +++++++++
- //escludi le misure oltre il max
- if((fDistance <= 50.0) && (fDistance >= 3))
- {
+ //++++++++++++++ INIZIO Suona Clacson +++++++++
+ //escludi le misure oltre il max
+ if((fDistance <= 50.0) && (fDistance >= 3))
+ {
// visualizza il valore misurato
printf("The Distance was %f [cm]\n\r", fDistance);
// 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
- nIndex=0;
- //Genera il suono del clacson
- for(nSampleCount=0; nSampleCount<7000; nSampleCount++)
+ nClacsonSampleIndex=0;
+ // Genera il suono del clacson
+ for(nClacsonSampleCount=0; nClacsonSampleCount<7000; nClacsonSampleCount++)
{
- OutWave.write_u16(usaSine[nIndex]); //max 32767
+ OutWave.write_u16(usaClacson[nClacsonSampleIndex]); //max 32767
//OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
- wait(fDeltaT);
+ wait(fDeltaTClacsonSound);
// genera ciclicamente
- nIndex++;
- if(nIndex >= SAMPLESINENUM)
+ nClacsonSampleIndex++;
+ if(nClacsonSampleIndex >= CLACSONSAMPLENUM)
{
- nIndex=0;
+ nClacsonSampleIndex=0;
}
// a metà genera un wait per doppio clacson
- if(nSampleCount == 2000)
+ 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))
+ } // if( (fDistance < Max) && (fDistance > Min))
//++++++++++++++ FINE Suona Clacson +++++++++
- wait_ms(100); // se effettuata la misura dai tempo prima di misurare nuovamente
+ wait_ms(100); // se effettuata la misura dai tempo prima di misurare nuovamente
}
//++++ FINE Ciclo Principale ++++