Diff: RobotFinale.cpp

Revision:

13:24b2e46b0fcf

Parent:

12:00ce5d30b82c

diff -r 00ce5d30b82c -r 24b2e46b0fcf RobotFinale.cpp
--- a/RobotFinale.cpp	Thu Nov 22 00:42:32 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,426 +0,0 @@
-/* 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 ++++
-}
-