Revision 12:00ce5d30b82c, committed 2018-11-22

Comitter:

pinofal

Date:

Thu Nov 22 00:42:32 2018 +0000

Parent:

11:a7e099ee909e

Commit message:

Amaldi Robot Ver 1.0

Changed in this revision

diff -r a7e099ee909e -r 00ce5d30b82c Exercise_IHM05A1.cpp
--- a/Exercise_IHM05A1.cpp	Wed Nov 21 15:52:33 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,222 +0,0 @@
-/**
- ******************************************************************************
- * @file    main.cpp
- * @author  IPC Rennes
- * @version V1.0.0
- * @date    April 13th, 2016
- * @brief   mbed simple application for the STMicroelectronics X-NUCLEO-IHM05A1
- *          Motor Control Expansion Board: control of 1 motor.
- ******************************************************************************
- * @attention
- *
- * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
- *
- * Redistribution and use in source and binary forms, with or without modification,
- * are permitted provided that the following conditions are met:
- *   1. Redistributions of source code must retain the above copyright notice,
- *      this list of conditions and the following disclaimer.
- *   2. Redistributions in binary form must reproduce the above copyright notice,
- *      this list of conditions and the following disclaimer in the documentation
- *      and/or other materials provided with the distribution.
- *   3. Neither the name of STMicroelectronics nor the names of its contributors
- *      may be used to endorse or promote products derived from this software
- *      without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- ******************************************************************************
- */
-
-/* Includes ------------------------------------------------------------------*/
-
-/* mbed specific header files. */
-#include "mbed.h"
-
-/* Component specific header files. */
-#include "L6208.h"
-
- DigitalIn myButton(USER_BUTTON);
- DigitalIn Input (PC_0);
-
-/* Definitions ---------------------------------------------------------------*/
-#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
-
-Serial pc(SERIAL_TX, SERIAL_RX);
-
-/* Variables -----------------------------------------------------------------*/
-
-/* 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)
-};
-
-/* Motor Control Component. */
-L6208 *motor;
-
-/* Functions -----------------------------------------------------------------*/
-
-/**
- * @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)
-{
-  pc.printf("    WARNING: \"FLAG\" interrupt triggered:\r\n");
-  motor->disable();
-  pc.printf("    Motor disabled.\r\n\n");
-}
-
-/**
- * @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)
-{
-  /* Printing to the console. */
-  pc.printf("Error %d detected\r\n\n", error);
-  
-  /* Infinite loop */
-  while (true) {
-  }    
-}
-
-/* Main ----------------------------------------------------------------------*/
-
-int main()
-{
-  // 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");
-  
-  /* Printing to the console. */
-  pc.printf("STARTING MAIN PROGRAM\r\n");
-  pc.printf("    Reminder:\r\n");
-  pc.printf("    The position unit is in agreement to the step mode.\r\n");
-  pc.printf("    The speed, acceleration or deceleration unit depend on the step mode:\r\n");
-  pc.printf("    - For normal mode and half step mode, the unit is steps/s or /s^2.\r\n");
-  pc.printf("    - For microstep modes, the unit is (1/16)steps/s or /s^2.\r\n");
-    
-//----- Initialization 
-  /* 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);
-  
- 
-
-  
-//----- 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());
-  
-  /* 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);
-int i,a=0;
-  
-  while (true) {
-  
- /* if ( Input == 0 ) 
-  {
-     a=a+1;
-   }
-   if(a>1)
-   {
-    a=0;
-     
-   }*/
-         
-     if(myButton==0)
-     {
-        /* Request device to go position -3200 */
-        motor->go_to(150);
-        /* Waiting while the motor is active. */
-       motor->wait_while_active();
-    }
-    else 
-    {
-        /* Request device to go position -3200 */
-     motor->go_to(-150);
-     /* Waiting while the motor is active. */
-     motor->wait_while_active();
-  
-    }
-    
-
-}
-}
-/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

diff -r a7e099ee909e -r 00ce5d30b82c RobotFinale.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RobotFinale.cpp	Thu Nov 22 00:42:32 2018 +0000
@@ -0,0 +1,426 @@
+/* mbed specific header files. */
+#include "mbed.h"
+
+/* Component specific header files. */
+#include "L6208.h"
+
+// numero di campioni che compongono un periodo della sinusoide in Output sull'ADC
+#define SAMPLESINENUM   45 // consigliabile avere  multipli di 45
+
+// numero di campioni acquisiti su cui effettuare la media di luminosità
+#define NUMLIGHTSAMPLE 100
+ 
+// 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
+
+// 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;
+
+// distanza in cm dell'ostacolo
+double fDistance;
+
+// Buffer contenente la sinusoide da porre in output.
+unsigned short usaSine[SAMPLESINENUM];
+
+// 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;
+
+// frequenza segnale audio da generare
+double fFreq;
+
+// periodo della sinusoide audio da generare
+double fPeriod;
+
+// indice per i cicli
+int nIndex;
+
+// numero di campioni di onda sonora già generati 
+int nSampleCount;
+
+
+// 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;
+
+// Output Digitali usati per i LED
+DigitalOut LedWAD (PC_2);
+DigitalOut LedWAS (PC_3);
+DigitalOut LedWPD (PC_10);
+DigitalOut LedWPS (PA_0) ;
+/*XXXXXXXX   ATTENZIONE NON UTILIZZABILE COME LED PERCHE' UTILIZZATO DA IHM05A1 XXXXXXX
+DigitalOut LedYAD (PC_13); 
+*/
+DigitalOut LedYAS (PC_14);
+DigitalOut LedRPD (PC_12);
+/*XXXXXXXX   ATTENZIONE NON UTILIZZABILE COME LED PERCHE' UTILIZZATO DA SENSORE DI LUMINOSITA' XXXXXXX
+DigitalOut LedRPS (PA_1) ; 
+*/
+// 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 1 di comunicazione = RPI 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)
+};
+
+/* Motor Control Component. */
+L6208 *motor;
+
+
+//****************************
+// 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/SAMPLESINENUM;
+    for (nIndex = 0; nIndex < SAMPLESINENUM; 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;
+    }
+}
+
+/********************************************************/
+/* Funzione avviata all'inizio come saluto e Benvenuto  */
+/********************************************************/
+void WelcomeMessage()
+{
+}
+
+/********************************************************************/
+/* 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)
+{
+  pc.printf("    WARNING: \"FLAG\" interrupt triggered:\r\n");
+  motor->disable();
+  pc.printf("    Motor disabled.\r\n\n");
+}
+
+/***************************************************************
+ * 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)
+{
+  /* Printing to the console. */
+  pc.printf("Error %d detected\r\n\n", error);
+  
+  /* Infinite loop */
+  while (true) 
+  {}    
+}
+
+/********/
+/* Main */
+/********/
+int main()
+{
+    // 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);
+       
+    
+    //----- 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());
+    
+    /* 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);
+    
+    //+++++++++++++ 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
+    //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
+    
+    /*
+    while(true)
+    {
+       
+    }
+    */
+    
+    //++++ INIZIO Ciclo Principale ++++
+    while (true) 
+    {
+        //++++++++++++++ INIZIO Pilotaggio Motore su comando da Raspberry+++++++++++++
+        if(InMotorSwitchRPI==1)
+        {
+          // Request device to go position -3200 
+          motor->go_to(150);
+          // Waiting while the motor is active. 
+          motor->wait_while_active();
+        }
+        else 
+        {
+           // Request device to go position -3200 
+           motor->go_to(-150);
+           // Waiting while the motor is active. 
+           motor->wait_while_active();
+        }
+        //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
+        //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
+        if(InLightSwitchRPI ==1)
+        {
+            // accendi i LED di abbellimento
+            led2=1;
+        } 
+        else
+        {
+            // spegni i LED di abbellimento
+            led2=0;
+        }
+       //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
+       //++++++++++++ INIZIO Misura della Luminosità e accensione LED ++++++++++++++
+       // inizializza il valore medio della Luminosità 
+       fAvgLight=0.0;
+       for(nIndex=0; nIndex < NUMLIGHTSAMPLE; nIndex++)
+       {
+           // 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)
+       {
+           led2 = 1;
+       }
+       else
+       {
+           led2 = 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 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)) 
+       {
+          // 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)
+          {
+                // INIZIO generazione tono  
+                nIndex=0;
+                //Genera il suono del clacson
+                for(nSampleCount=0; nSampleCount<7000; nSampleCount++)
+                {
+                   OutWave.write_u16(usaSine[nIndex]); //max 32767
+                   //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
+                   wait(fDeltaT);
+                   // genera ciclicamente
+                   nIndex++;
+                   if(nIndex >= SAMPLESINENUM)
+                   {
+                       nIndex=0;
+                   }   
+                   // a metà genera un wait per doppio clacson
+                   if(nSampleCount == 2000)
+                   {
+                       wait_ms(100);
+                   }
+                    
+                } 
+                //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
+                OutWave.write_u16(0);
+              
+            } // if(fDistance < soglia) suona clacson
+            
+       } // if( (fDistance < Max) && (fDistance > Min)) 
+        //++++++++++++++ FINE Suona Clacson +++++++++
+       wait_ms(100); // se effettuata la misura dai tempo prima di misurare nuovamente    
+         
+    }
+    //++++ FINE Ciclo Principale ++++
+}
+