RobotFinale.cpp

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