RobotFinale.cpp

00001 /*modifca pin led gialli
00002  modifica soglia luminsità*/
00003 
00004 
00005 /* mbed specific header files. */
00006 #include "mbed.h"
00007 
00008 /* Component specific header files. */
00009 #include "L6208.h"
00010 
00011 // numero di campioni che compongono un periodo della sinusoide in Output sull'ADC
00012 #define SAMPLESINENUM   45 // consigliabile avere  multipli di 45
00013 
00014 // numero di campioni acquisiti su cui effettuare la media di luminosità
00015 #define NUMLIGHTSAMPLE 100
00016  
00017 // parametri dell'onda coseno da generare
00018 #define PI        (3.141592653589793238462)
00019 #define AMPLITUDE 32767 //(1.0)    // x * 3.3V
00020 #define PHASE     (PI/2) // 2*pi è un periodo
00021 #define OFFSET    32767 //(0x7FFF)
00022 
00023 // Parametro di soglia per la luce. Accendi Luci se la luminosità scende sotto SOGLIALUCI
00024 #define SOGLIALUCI (1.85)
00025 
00026 // Definizione Parametri per il motore
00027 #ifdef TARGET_NUCLEO_F334R8
00028 #define VREFA_PWM_PIN D11
00029 #define VREFB_PWM_PIN D9
00030 #elif TARGET_NUCLEO_F302R8
00031 #define VREFA_PWM_PIN D11
00032 #define VREFB_PWM_PIN D15 // HW mandatory patch: bridge manually D9 with D15
00033 #else
00034 #define VREFA_PWM_PIN D3
00035 #define VREFB_PWM_PIN D9
00036 #endif
00037 
00038 // definizioni funzioni di default su scheda
00039 Serial pc(SERIAL_TX, SERIAL_RX); // seriale di comunicazione con il PC
00040 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda
00041 DigitalOut led2(LED2);// LED verde sulla scheda
00042 
00043 // pin A2 di output per la forma d'onda analogica dedicata al suono
00044 AnalogOut OutWave(PA_4); 
00045 
00046 // pin A1 di input per la forma d'onda analogica dedicata alla luminosità
00047 AnalogIn InWaveLight(PA_1);
00048 
00049 // Pin di tipo In-Out per la gestione del segnale Sig del Sensore di prossimità a ultrasuoni
00050 DigitalInOut myProx (PC_0, PIN_OUTPUT, PullNone, 0); 
00051 // Timer per il calcolo dei tempi del sensore di prossimità
00052 Timer myTimer;
00053 
00054 // tempo inizio intermedio e fine del timer che misura la distanza con il sensore ultrasuoni
00055 int nTimerStart, nTimer, nTimerStop;
00056 
00057 // distanza in cm dell'ostacolo
00058 double fDistance;
00059 
00060 // Buffer contenente la sinusoide da porre in output.
00061 unsigned short usaSine[SAMPLESINENUM];
00062 
00063 // prototipo di funzione che genera i campioni della sinusoide da utilizzare per la generazione tramite DAC
00064 void CalculateSinewave(void);
00065  
00066 
00067 // Periodo di generazione campioni in output DeltaT = T/NumSample
00068 double fDeltaT;
00069 // amplificazione per il dato da spedire sull'ADC
00070 double fAmp;
00071 
00072 // frequenza segnale audio da generare
00073 double fFreq;
00074 
00075 // periodo della sinusoide audio da generare
00076 double fPeriod;
00077 
00078 // indice per i cicli
00079 int nIndex;
00080 
00081 // numero di campioni di onda sonora già generati 
00082 int nSampleCount;
00083 
00084 
00085 // valore medio della Luminosità su NUMACQUISIZIONI acquisizioni
00086 double fAvgLight;
00087 
00088 // valore numerico, di tensione e di luce letto dall'ADC
00089 volatile unsigned short usReadADC;
00090 volatile float fReadVoltage;
00091 
00092 // valore di luminosità letto dall'ADC
00093 volatile float fLight;
00094 
00095 // Output Digitali usati per i LED
00096 DigitalOut LedWAD (PC_2);
00097 DigitalOut LedWAS (PC_3);
00098 DigitalOut LedWPD (PH_0);
00099 DigitalOut LedWPS (PA_0) ;
00100 DigitalOut LedYAD (PC_9); 
00101 DigitalOut LedYAS (PC_8);
00102 DigitalOut LedRPD (PA_13);
00103 DigitalOut LedRPS (PA_14) ; 
00104 DigitalOut OutA (PB_0);
00105 DigitalOut OutB (PC_1);
00106 
00107 // Input/Output Digitali usati per interfaccia RPI
00108 DigitalIn InLightSwitchRPI (PB_9); // accende e spegne le Luci rosse e gialle = GPIO20
00109 DigitalIn InMotorSwitchRPI (PB_8); // accende e spegne il motore = RPI GPIO16
00110 DigitalIn InFutureUse0RPI (PB_7); // usi futuri 0 di comunicazione = RPI GPIO13
00111 DigitalIn InFutureUse1RPI (PB_2); // usi futuri 1 di comunicazione = RPI GPIO25
00112 DigitalIn InFutureUse2RPI (PC_15); // usi futuri 2 di comunicazione = RPI GPIO12
00113 
00114 /****************************************************************************/
00115 /* Initialization parameters of the motor connected to the expansion board. */
00116 /****************************************************************************/
00117 l6208_init_t init =
00118 {
00119  
00120   1500,            //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
00121   20,              //Acceleration current torque in % (from 0 to 100)
00122   1500,            //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
00123   30,              //Deceleration current torque in % (from 0 to 100)
00124   1500,            //Running speed in step/s or (1/16)th step/s for microstep modes
00125   50,              //Running current torque in % (from 0 to 100)
00126   20,              //Holding current torque in % (from 0 to 100)
00127   STEP_MODE_1_16,  //Step mode via enum motorStepMode_t
00128   FAST_DECAY,      //Decay mode via enum motorDecayMode_t
00129   0,               //Dwelling time in ms
00130   FALSE,           //Automatic HIZ STOP
00131   100000           //VREFA and VREFB PWM frequency (Hz)
00132 };
00133 
00134 /* Motor Control Component. */
00135 L6208 *motor;
00136 
00137 
00138 //****************************
00139 // Create the sinewave buffer
00140 //****************************
00141 void CalculateSinewave(int nOffset, int nAmplitude, double fPhase)
00142 {
00143     // variabile contenente l'angolo in radianti
00144     double fRads;
00145     // indici per i cicli
00146     int nIndex;
00147     // passo in frequenza fissato dal numero di campioni in cui voglio dividere un periodo di sinusoide: DeltaF = 360°/NUMSAMPLE
00148     double fDeltaF;
00149     // angolo per il quale bisogna calcolare il valore di sinusoide: fAngle = nIndex*DeltaF
00150     double fAngle;
00151     
00152     fDeltaF = 360.0/SAMPLESINENUM;
00153     for (nIndex = 0; nIndex < SAMPLESINENUM; nIndex++) 
00154     {
00155         fAngle = nIndex*fDeltaF; // angolo per il quale bisogna calcolare il campione di sinusoide
00156         fRads = (PI * fAngle)/180.0; // Convert degree in radian
00157         //usaSine[nIndex] = AMPLITUDE * cos(fRads + PHASE) + OFFSET;
00158         usaSine[nIndex] = nAmplitude * cos(fRads + fPhase) + nOffset;
00159     }
00160 }
00161 
00162 /********************************************************/
00163 /* Funzione avviata all'inizio come saluto e Benvenuto  */
00164 /********************************************************/
00165 void WelcomeMessage()
00166 {
00167 }
00168 
00169 /********************************************************************/
00170 /* brief  This is an example of user handler for the flag interrupt.*/
00171 /* param  None                                                      */
00172 /* retval None                                                      */
00173 /* note   If needed, implement it, and then attach and enable it:   */
00174 /*            + motor->attach_flag_irq(&my_flag_irq_handler);       */
00175 /*            + motor->enable_flag_irq();                           */
00176 /*          To disable it:                                          */
00177 /*            + motor->DisbleFlagIRQ();                             */
00178 /*********************************************************************/
00179 void my_flag_irq_handler(void)
00180 {
00181   pc.printf("    WARNING: \"FLAG\" interrupt triggered:\r\n");
00182   motor->disable();
00183   pc.printf("    Motor disabled.\r\n\n");
00184 }
00185 
00186 /***************************************************************
00187  * brief  This is an example of error handler.
00188  * param[in] error Number of the error
00189  * retval None
00190  * note   If needed, implement it, and then attach it:
00191  *           + motor->attach_error_handler(&my_error_handler);
00192  **************************************************************/
00193 void my_error_handler(uint16_t error)
00194 {
00195   /* Printing to the console. */
00196   pc.printf("Error %d detected\r\n\n", error);
00197   
00198   /* Infinite loop */
00199   while (true) 
00200   {}    
00201 }
00202 
00203 /********/
00204 /* Main */
00205 /********/
00206 int main()
00207 {
00208     // configura velocità della comunicazione seriale su USB-VirtualCom e invia messaggio di benvenuto
00209     pc.baud(921600); //921600 bps
00210     //pc.baud(9600); //256000 bps
00211     pc.printf("*** Test Motor ***\n\r");
00212     
00213     /* Initializing Motor Control Component. */
00214     motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
00215     if (motor->init(&init) != COMPONENT_OK) 
00216     {
00217         exit(EXIT_FAILURE);
00218     }
00219     
00220     /* Attaching and enabling an interrupt handler. */
00221     motor->attach_flag_irq(&my_flag_irq_handler);
00222     motor->enable_flag_irq();
00223     
00224     /* Attaching an error handler */
00225     motor->attach_error_handler(&my_error_handler);
00226     
00227     /* Printing to the console. */
00228     pc.printf("Motor Control Application Example for 1 Motor\r\n");
00229     
00230     //----- run the motor BACKWARD
00231     pc.printf("--> Running the motor backward.\r\n");
00232     motor->run(StepperMotor::BWD);
00233        
00234     
00235     //----- Soft stop required while running
00236     pc.printf("--> Soft stop requested.\r\n");
00237     motor->soft_stop(); 
00238     
00239     
00240     //----- Change step mode to full step mode
00241     motor->set_step_mode(StepperMotor::STEP_MODE_FULL);
00242     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());
00243     
00244     /* Get current position of device and print to the console */
00245     pc.printf("    Position: %d.\r\n", motor->get_position());
00246     
00247     /* Set speed, acceleration and deceleration to scale with normal mode */
00248     motor->set_max_speed(init.maxSpeedSps>>4);
00249     motor->set_acceleration(motor->get_acceleration()>>4);
00250     motor->set_deceleration(motor->get_deceleration()>>4);
00251     /* Print parameters to the console */  
00252     pc.printf("    Motor Max Speed: %d step/s.\r\n", motor->get_max_speed());
00253     pc.printf("    Motor Min Speed: %d step/s.\r\n", motor->get_min_speed());
00254     pc.printf("    Motor Acceleration: %d step/s.\r\n", motor->get_acceleration());
00255     pc.printf("    Motor Deceleration: %d step/s.\r\n", motor->get_deceleration());
00256     
00257     //----- move of 200 steps in the FW direction
00258     pc.printf("--> Moving forward 200 steps.\r\n");
00259     motor->move(StepperMotor::FWD, 200);
00260     
00261     //+++++++++++++ INIZIO Genera Sinusoide ++++++++++++++++++
00262     fFreq = 440.0; // frequenza in Hz del tono da generare
00263     fAmp = 1.0; // coefficiente per il quale viene moltiplicato l'ampiezza massima del tono da generare
00264     fDeltaT = 1.0/(fFreq*SAMPLESINENUM); // intervallo di tempo tra un campione e l'altro, per generare la frequenza desiderata
00265     CalculateSinewave(AMPLITUDE, (AMPLITUDE*fAmp), (PI/2.0)); // generazione della sinusoide con valori nominali
00266     //+++++++++++++ FINE Genera Sinusoide +++++++++++++++++++++
00267     
00268     
00269     while(true)
00270     {
00271      if(myButton==0)
00272         {
00273          //Ferma motore
00274         OutA=0;
00275         OutB=0;
00276         pc.printf("OutA OutB = 00\r\n");
00277         wait_ms(2000);
00278         
00279         
00280         // Ruota Left
00281         OutA=0;
00282         OutB=1;
00283         pc.printf("OutA OutB = 01\r\n");
00284         wait_ms(2000);
00285          //Ferma motore
00286         OutA=0;
00287         OutB=0;
00288         pc.printf("OutA OutB = 00\r\n");
00289         wait_ms(2000);
00290         }
00291         else 
00292         {
00293            //Ferma motore
00294         OutA=1;
00295         OutB=0;
00296         pc.printf("OutA OutB = 10\r\n");
00297         wait_ms(2000);
00298         
00299         // Ruota Right
00300         OutA=1;
00301         OutB=1;
00302         pc.printf("OutA OutB = 11\r\n");
00303         wait_ms(2000);
00304         
00305         OutA=1;
00306         OutB=0;
00307         pc.printf("OutA OutB = 10\r\n");
00308         wait_ms(2000);   
00309     }
00310     
00311     
00312     //++++ INIZIO Ciclo Principale ++++
00313     while (true) 
00314     {
00315         //++++++++++++++ INIZIO Pilotaggio Motore su comando da Raspberry+++++++++++++
00316         if(InMotorSwitchRPI==1)
00317         {
00318          //Ferma motore
00319         OutA=0;
00320         OutB=0;
00321         pc.printf("OutA OutB = 00\r\n");
00322         wait_ms(2000);
00323         
00324         
00325         // Ruota Left
00326         OutA=0;
00327         OutB=1;
00328         pc.printf("OutA OutB = 01\r\n");
00329         wait_ms(2000);
00330          //Ferma motore
00331         OutA=0;
00332         OutB=0;
00333         pc.printf("OutA OutB = 00\r\n");
00334         wait_ms(2000);
00335         }
00336         else 
00337         {
00338            //Ferma motore
00339         OutA=1;
00340         OutB=0;
00341         pc.printf("OutA OutB = 10\r\n");
00342         wait_ms(2000);
00343         
00344         // Ruota Right
00345         OutA=1;
00346         OutB=1;
00347         pc.printf("OutA OutB = 11\r\n");
00348         wait_ms(2000);
00349         
00350         OutA=1;
00351         OutB=0;
00352         pc.printf("OutA OutB = 10\r\n");
00353         wait_ms(2000);
00354         }
00355         //++++++++++++++ FINE Pilotaggio Motore +++++++++++++
00356         //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
00357         if(InLightSwitchRPI ==1)
00358         {
00359             
00360             // accendi i LED di abbellimento
00361             //led2=1;
00362             LedYAD = 1; 
00363             LedYAS = 1;
00364             LedRPD = 1;
00365             LedRPS = 1;
00366             
00367             
00368         } 
00369         else
00370         {
00371             
00372             // spegni i LED di abbellimento
00373             //led2=0;
00374             LedYAD = 0; 
00375             LedYAS = 0;
00376             LedRPD = 0;
00377             LedRPS = 0;
00378             
00379              
00380         }
00381        //++++++++++++++ INIZIO Accensione LED da comando Raspberry +++++++
00382        
00383        //++++++++++++ INIZIO Misura della Luminosità e accensione LED Bianchi ++++++++++++++
00384        // inizializza il valore medio della Luminosità 
00385        fAvgLight=0.0;
00386        for(nIndex=0; nIndex < NUMLIGHTSAMPLE; nIndex++)
00387        {
00388            // acquisisce dato da ADC
00389            usReadADC = InWaveLight.read_u16();
00390            fReadVoltage=(usReadADC*3.3)/65535.0; // converte in Volt il valore numerico letto dall'ADC
00391            //fReadVoltage=InWave.read(); // acquisisce il valore dall'ADC come valore di tensione in volt
00392            fLight= fReadVoltage; //ATTENZIONE Visualizza il valore grezzo letto dall'ADC
00393            fAvgLight+=fLight;
00394        }   
00395        // calcola valore medio su NUMSAMPLE acquisizioni 
00396        fAvgLight/= NUMLIGHTSAMPLE;
00397        // Accendi LED Bianchi se illuminazione è sottosoglia
00398        if(fAvgLight < SOGLIALUCI)
00399        {
00400            
00401            // Accendi LED Bianchi
00402            //led2 = 1;
00403            LedWAD = 1;
00404            LedWAS = 1;
00405            LedWPD = 1;
00406            LedWPS = 1;
00407            
00408        }
00409        else
00410        {
00411            
00412            // Spegni LED Bianchi
00413            //led2 = 0;
00414            LedWAD = 0;
00415            LedWAS = 0;
00416            LedWPD = 0;
00417            LedWPS = 0;
00418            
00419        }
00420        
00421        // invia il dato al PC
00422        //pc.printf("\n\r--- Digital= %d [Volt]; Brightness= %.2f ---\n\r", usReadADC, fAvgLight);
00423        //++++++++++++ FINE Misura della Luminosità e accensione LED ++++++++++++++
00424         
00425         
00426        //++++++++++++++ INIZIO Acquisisci distanza ostacoli +++++++++
00427        //inizializza misura di distanza
00428        fDistance=0.0;
00429        // Fissa come Output il pin myProx
00430        myProx.output();
00431        // Poni 'L' sul Pin e mantienilo per qualche microsecondo
00432        myProx.write(0);
00433        wait_us(5);
00434        // Poni 'H' sul Pin e mantienilo per qualche microsecondo
00435        myProx.write(1);
00436        wait_us(10);
00437        // Poni 'L' sul Pin e mantienilo per qualche microsecondo
00438        myProx.write(0);
00439        // Attendi assestamento e Fissa come Input il pin myProx
00440        wait_us(5);
00441        myProx.input();
00442        
00443        // misura il tempo per cui il pin rimane alto. E' il tempo necessario al suono per raggiungere l'ostacolo e ritornare sul sensore
00444        nTimer =0;
00445        /*
00446        myTimer.start(); // avvia il timer per verificare la presenza del sensore
00447        while((myProx ==0) && (nTimer <=50000)) // esci se il senore risponde oppure se passano oltre 10ms
00448        {
00449            // misura il tempo passato
00450            nTimer=myTimer.read_us();
00451        }
00452        */
00453        while(myProx ==0)
00454        {}
00455        myTimer.stop(); // in ogni caso ferma il timer
00456        // vai avanti solo se il sensore ha risposto
00457        //if(nTimer <= 50000) // se è passato più tempo il sensore non è presente
00458        {
00459            myTimer.start();
00460            nTimerStart = myTimer.read_us();
00461            while(myProx == 1)
00462            {}
00463            myTimer.stop();
00464            nTimerStop = myTimer.read_us();
00465            
00466           
00467            // velocità del suono = 343 [m/s] = 0.0343 [cm/us] = 1/29.1 [cm/us]
00468            // tempo di andata e ritorno del segnale [us] = (TimerStop-TimerStart)[us]; per misurare la distanza bisogna dividere per due questo valore
00469            // distanza dell'ostacolo [cm] = (TimerStop-TimerStart)/2 [us] * 1/29.1[cm/us]
00470            fDistance = (nTimerStop-nTimerStart)/58.2;
00471            // invia il dato al PC
00472            //pc.printf("\n\r The Distance was = %.2f [cm]\n\r", fDistance);
00473             
00474        }
00475        //++++++++++++++ FINE Acquisisci distanza ostacoli +++++++++
00476         
00477        //++++++++++++++ INIZIO Suona Clacson +++++++++
00478        //escludi le misure oltre il max
00479        if((fDistance <= 50.0) && (fDistance >= 3)) 
00480        {
00481           // visualizza il valore misurato
00482           printf("The Distance was %f [cm]\n\r", fDistance);
00483            
00484           // SUONA IL CLACSON se l'ostacolo si trova ad una distanza inferiore ad una soglia minima
00485           if(fDistance < 22)
00486           {
00487                 // INIZIO generazione tono  
00488                 nIndex=0;
00489                 //Genera il suono del clacson
00490                 for(nSampleCount=0; nSampleCount<7000; nSampleCount++)
00491                 {
00492                    OutWave.write_u16(usaSine[nIndex]); //max 32767
00493                    //OutWave.write_u16(32767); //uscita analogica per scopi diagnostici
00494                    wait(fDeltaT);
00495                    // genera ciclicamente
00496                    nIndex++;
00497                    if(nIndex >= SAMPLESINENUM)
00498                    {
00499                        nIndex=0;
00500                    }   
00501                    // a metà genera un wait per doppio clacson
00502                    if(nSampleCount == 2000)
00503                    {
00504                        wait_ms(100);
00505                    }
00506                     
00507                 } 
00508                 //assicurati di inviare 0 come ultimo campione per spegnere l'amplificatore e non dissipare inutilmente corrente
00509                 OutWave.write_u16(0);
00510               
00511             } // if(fDistance < soglia) suona clacson
00512             
00513        } // if( (fDistance < Max) && (fDistance > Min)) 
00514         //++++++++++++++ FINE Suona Clacson +++++++++
00515        wait_ms(100); // se effettuata la misura dai tempo prima di misurare nuovamente    
00516          
00517     }
00518     //++++ FINE Ciclo Principale ++++
00519 }
00520 }