MicRobot-Rev085.cpp

00001 // pilotaggio carrello tramite BLE.
00002 // testato su L476RG e F401RE
00003 
00004 #include "mbed.h"
00005 #include<stdlib.h>
00006 
00007 // attivare questa #define quando si vuole simulare l'arrivo di un segnale di encoder dai motori in movimento
00008 //#define ENCODERSIMULATE 
00009 
00010 // pi greco
00011 #define PI           3.14159265358979323846
00012 
00013 // dimensione massima del pacchetto ricevuto su seriale
00014 #define PACKETDIM   8
00015 
00016 // diametro della ruota in [metri]
00017 #define DIAMETRORUOTA (0.1)
00018 
00019 // numero di impulsi per giro generati dall'encoder
00020 #define IMPULSIPERGIRO 4
00021 
00022 // numero di cifre con cui si vuole rappresentare la distanza percorsa in [m]. NUMCIFREDISTANZAPERCORSA = 5, significa che la distanza è rappresentata come xxx.xx [m]
00023 #define NUMCIFREDISTANZAPERCORSA 7
00024 
00025 // numero di cifre con cui si vuole rappresentare la velocità in [m/s]. NUMCIFRESPEED = 5, significa che la velocità è rappresentata come xxx.xx [m/s]
00026 #define NUMCIFRESPEED 7
00027 
00028 // intervallo di tempo in [sec], in cui vengono contati gli impulsi di encoder per il calcolo della velocità
00029 #define DELTAT (0.5)
00030 
00031 
00032 // Parametri moltiplicativi. Queste operazioni vengono fatte una sola volta, evitando di farle ad ogni ciclo
00033 #define fDistanzaPerStep (PI*DIAMETRORUOTA/IMPULSIPERGIRO)
00034 
00035 
00036 // Ogni Ticker viene calcolata la velocità. Se il ticker viene richiamato ogni DELTAT sec, la velocità potrà essere calcolata come v = spazio/DELTAT
00037 Ticker SpeedCalculateTicker;
00038 
00039 //!!!!!!!!!!!!!!!!!!! INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!
00040 #ifdef ENCODERSIMULATE
00041 Ticker EncoderSimulateTicker; // Ticker per simulare un segnale proveniente da encoder sul motore
00042 #endif
00043 //!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!    
00044 
00045 // Definizione periferica USB seriale del PC
00046 Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12
00047 
00048 // Definizione periferica seriale del Modulo BLE ELETT114A
00049 Serial myBLE(PA_9, PA_10, 9600); //Tx, Rx, bps // F401
00050 //Serial myBLE(PG_7, PG_8, 9600); //Tx, Rx, bps // L496
00051 
00052 // Input di Reset per il Modulo BLE HC-05
00053 DigitalOut BleRst(PA_8);
00054 
00055 // User Button, LED  
00056 DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13
00057 DigitalOut myLed(LED2); // LED verde sulla scheda. Associato a PA_5
00058 
00059 // output digitale per pilotaggio illuminazione a LED
00060 DigitalOut Light(PA_0);
00061 //DigitalIn InDiag(PC_0,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20 
00062 InterruptIn InEncoderA(PC_0); // segnale di encoder di un motore.
00063 
00064 // variabile che conta il numero di fronti si salita del segnale encoder di uno dei motori del robot
00065 volatile int nCountRiseEdge; 
00066 volatile int nOldCountRiseEdge;
00067 
00068 // Input/Output
00069 DigitalOut PostOutBI1 (PA_6);  // Output 1 per pilotaggio input BI1 del Motore B Posteriore
00070 PwmOut PostOutPWB (PB_6);  // Output per pilotaggio input PWM del motore B Posteriore
00071 //DigitalOut PostOutPWB (PA_7);  // Scopi Diagnostici: Output Digitale per pilotaggio PWM del motore B Posteriore
00072 DigitalOut PostOutBI2 (PA_7);  // Output 2 per pilotaggio input BI2 del Motore B Posteriore
00073 DigitalIn PostInNE1 (PC_7); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore
00074 
00075 DigitalOut AntOutBI1 (PB_3);  // Output 1 per pilotaggio input BI1 del Motore B Anteriore
00076 PwmOut AntOutPWB (PB_5);  // Output per pilotaggio input PWM del motore B Anteriore
00077 //DigitalOut AntOutPWB (PB_5);  // Scopi diagnostici: Output Digitalte per pilotaggio PWM del motore B Anteriore
00078 DigitalOut AntOutBI2 (PB_4);  // Output 2 per pilotaggio input BI2 del Motore B Posteriore
00079 DigitalIn AntInNE1 (PB_10); // Input per acquisire i segnali NET1 in output dall'encoder Anteriore
00080 
00081 PwmOut MotoreCoda (PB_8);  // Output movimento coda
00082 
00083 //carattere di comando ricevuto dal BLE e relativo parametro
00084 volatile char cCommandBLE; // cambia nella routine di interrupt
00085 volatile char cParamBLE; // cambia nella routine di interrupt
00086 volatile int nParamBLE; // corrispondente valore numerico di cParamBLE
00087 
00088 // memorizza l'ultimo comando ricevuto e relativo parametro. Ci saranno delle azioni solo se il comando ricevuto o il parametro è cambiato rispetto al precedente
00089 char cOldCommandBLE;
00090 int nOldParamBLE; 
00091 
00092 // coordinate polari del joystick sulla APP, fornite dalla routine di interrupt
00093 volatile double fTeta;
00094 volatile double fRo;
00095 volatile int nRo;
00096 volatile int nTeta;
00097 
00098 // coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt
00099 volatile double fX, fY;
00100 // memorizza ultimi valori delle coordinate del Joystick
00101 double fOldX, fOldY;
00102 
00103 // variabili ausiliarie per l'algoritmo di posizionamento
00104 double fV, fW;
00105 
00106 // velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore
00107 double fR, fL;
00108 
00109 // distanza percorsa in [m], calcolata utilizzando gli impulsi dell'encoder sul motore
00110 volatile double fDistanzaPercorsa; // calcolata nel main, utilizzata nelle IRQ
00111 
00112 // velocità calcolata gli impulsi contati in un intervallo DELTAT msec
00113 volatile double fSpeed; // calcolata nel main, utilizzata nelle IRQ
00114 
00115 // Scopi diagnostici:  Ogni fDeltaTick viene simulata la generazione di un impulso di encoder.
00116 // velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
00117 double fDeltaTick;
00118 
00119  // indice per i cicli
00120 int nIndex;
00121 
00122 // esponente della base 10, per cui bisognerà moltiplicare i caratteri per trasformarli in numeri
00123 double fEsponente;
00124 
00125 // array per la ricezione dei messaggi da BLE
00126 volatile char caRxPacket[PACKETDIM]; 
00127 // contatore di caratteri ricevuti daBLE
00128 volatile int nCharCount; 
00129 
00130 // flag che indica se il sw è in Reset
00131 volatile bool bReset;
00132 
00133 // flag che indica se la coda è in movimento/ferma true/false
00134 volatile bool bCodaInMovimento;
00135 
00136 /**************************************************************************************/
00137 /* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
00138 /**************************************************************************************/
00139 void riseEncoderIRQ()
00140 {
00141     // incrementa il contatore di impulsi contati, se il sw non è resettato, cioè se bReset = false
00142     //if(!bReset) 
00143         nCountRiseEdge++;
00144         
00145      //pc.printf("Sono qui 0 \n\r"); // diagnostica
00146 }       
00147 
00148 
00149 /****************************************************************************************/
00150 /* Diagnostica:                                                                         */
00151 /* COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO   */
00152 /* Routine di gestione del ticker per simulare encoder                                  */  
00153 /* Simula il segnale di encoder ricevuto con un determinato DELTAT                      */
00154 /* A robot fermo, il segnale di encoder non genera interrupt.                           */
00155 /* Questo Ticker simula l'arrivo del segnale da encoder                                 */
00156 /****************************************************************************************/
00157 void EncoderSimulate()
00158 {
00159     // ad ogni tick viene simulata la ricezione di un impulso da encoder.
00160     // Esempio:
00161     //  fDeltaTick = 0.05 sec
00162     //  diametro ruota, DIAMETRORUOTA = 0.1 metri
00163     //  circonferenza ruota = 0.1*3.14= 0.314 metri
00164     //  impulsi per giro dall'encoder, IMPULSIPERGIRO = 4
00165     //  un tick simula l'arrivo di un impulso da encoder e quindi simula la percorrenza di 1/4 di circonferenza
00166     //  ogni volta che arriva un tick simulato da encoder, si presume di aver percorso circonferenza/4 = 0.314/4 = 0.0785 metri
00167     //  il tick arriva ogni fDeltaTick secondi e a ogni tick percorro 0.0785 metri ->  velocità = 0.0785/0.05 = 1.57 [m/s]
00168     //  spostamento = (Spazio per ogni tick)/(tempo per ogni tick)
00169     //  velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
00170     
00171     // simula impulso inviato dall'encoder se il sw non è resettato, cioè se bReset = false
00172     //if(!bReset)
00173         nCountRiseEdge++;
00174 } 
00175 
00176 
00177 /**********************************************/
00178 //          IRQ associata a Rx da PC 
00179 //**********************************************/
00180 void pcRxInterrupt(void)
00181 {
00182     // array per la ricezione dei messaggi da seriale
00183     char cReadChar; 
00184     
00185     // ricevi caratteri su seriale, se disponibili   
00186     while((pc.readable()))
00187     {
00188         // acquisice stringa in input e relativa dimensione
00189         cReadChar = pc.getc(); // read character from PC
00190         //myBLE.putc(cReadChar); // Diagnostica: write char to BLE
00191         //pc.putc(cReadChar); // Diagnostica: write char to PC
00192         
00193         //pc.printf("W>: 0x%02x\n\r",cReadChar); // diagnostica
00194         if(cReadChar == '0') // se scrivo '0', invia questa stringa
00195         {
00196             // DIAGNOSTICA:
00197             // Invia Stringa di comando al Robot
00198             myBLE.printf("\r\n> Prova di Trasmissione \r\n");
00199          }
00200     }
00201 }
00202 
00203 //**********************************************/
00204 //          IRQ associata a Rx da BLE 
00205 //**********************************************/
00206 void BLERxInterrupt(void)
00207 {
00208     
00209     // carattere ricevuto da BLE
00210     char cReadChar; 
00211     
00212     while((myBLE.readable()))
00213     {
00214         // acquisice stringa in input e memorizza in array
00215         cReadChar = myBLE.getc(); // Read character
00216         //caRxPacket[nCharCount]=cReadChar;
00217         //nCharCount++;
00218         pc.printf("%c", cReadChar); // diagnostica
00219         
00220         // acquisisce il carattere di start comando o coordinate da APP
00221         if(cReadChar=='(')
00222         {
00223             // acquisisce il primo carattere di comando o di coordinate
00224             cReadChar = myBLE.getc(); // Read character       
00225             pc.printf("%c", cReadChar); // diagnostica 
00226             
00227             // +++++++++++++++++ INIZIO gestione Comando da Button +++++++++++++++++
00228             // Ho ricevuto il comando da un Button se il primo carattere è una lettera maiuscola tra A e T
00229             if((cReadChar > 0x40) && (cReadChar < 0x55)) // caratteri alfabetici da 'A' a 'T'
00230             {
00231                 // memorizza come comando il carattere appena letto
00232                 cCommandBLE = cReadChar; 
00233                 // legge e memorizza come paramentro il successivo carattere
00234                 cReadChar = myBLE.getc(); // legge parametro
00235                 pc.printf("%c", cReadChar); // diagnostica 
00236                 nParamBLE = cReadChar-0x30;
00237                 cReadChar = myBLE.getc(); // legge la ')' di chiusura comando       
00238                 pc.printf("%c", cReadChar); // diagnostica 
00239 
00240                 // visualizza comando e parametro inviato da BLE
00241                 pc.printf("> %c%d \r\n\r",cCommandBLE, nParamBLE); // diagnostica
00242             }
00243             // +++++++++++++++++ FINE gestione Comando da Button +++++++++++++++++
00244             else
00245             {
00246                 if(cReadChar=='X') // è stato acquisisto carattere X di inizio valori numeri dell'ascissa?
00247                 {
00248                     //+++++++++ INIZIO acquisisce il valore dell'ascissa Xnnn +++++++++++++++
00249                     nCharCount = 0;
00250                     do
00251                     {
00252                         cReadChar = myBLE.getc(); // Read character
00253                         caRxPacket[nCharCount]=cReadChar;
00254                         nCharCount++;
00255                         pc.printf("%c", cReadChar); // diagnostica
00256                     }
00257                     while((cReadChar >= 0x30) && (cReadChar <= 0x39));
00258                     //+++++++++ FINE acquisisce il valore dell'ascissa Xnnn +++++++++++++++
00259                     
00260                     //+++++++++ INIZIO converte i caratteri in valore numerico dell'ascissa fX +++++++++++++++
00261                     nCharCount -= 2; // ultimo carattere dopo i numeri, mi aspetto sia 'Y'
00262                     fX=0;
00263                     for(nIndex =0; nIndex <= nCharCount; nIndex++)
00264                     {
00265                         fX = fX + (caRxPacket[nIndex]-0x30)*pow(10.0, (nCharCount - nIndex));
00266                     }
00267                     //+++++++++ FINE converte i caratteri in valore numerico dell'ascissa fX +++++++++++++++
00268                     
00269                     // verifica se l'ultimo carattere ricevuto dopo Xnnn è stato Y
00270                     if(cReadChar=='Y')
00271                     {
00272                         //+++++++++ INIZIO acquisisce il valore dell'ordinata Ynnn +++++++++++++++
00273                         nCharCount = 0;
00274                         do
00275                         {
00276                             cReadChar = myBLE.getc(); // Read character
00277                             caRxPacket[nCharCount]=cReadChar;
00278                             nCharCount++;
00279                             pc.printf("%c", cReadChar); // diagnostica
00280                         } 
00281                         while((cReadChar >= 0x30) && (cReadChar <= 0x39));  
00282                         //+++++++++ FINE acquisisce il valore dell'ordinata Ynnn +++++++++++++++
00283     
00284                         //+++++++++ INIZIO converte i caratteri in valore numerico dell'ordinata fY +++++++++++++++
00285                         nCharCount -= 2; // ultimo carattere dopo i numeri, mi aspetto sia 'Y'
00286                         fY=0;
00287                         for(nIndex =0; nIndex <= nCharCount; nIndex++)
00288                         {
00289                             fY = fY + (caRxPacket[nIndex]-0x30)*pow(10.0, (nCharCount - nIndex));
00290                         }
00291                         //+++++++++ FINE converte i caratteri in valore numerico dell'ordinata fY +++++++++++++++
00292                         
00293                         // se riceve la coda del comando ), stampa le coordinate ricevute
00294                         if(cReadChar==')') 
00295                         {
00296                             pc.printf("(fX , fY) = (%.1f , %.1f) \r\n", fX, fY); // diagnostica
00297                             
00298                             // trasporta x e y nei range desiderati
00299                             fY= 100 - fY;
00300                             fX = fX - 100;
00301                             
00302                             pc.printf("\n\r(fX , fY) traslate = (%.1f , %.1f) \r\n\n", fX, fY); // diagnostica
00303                         }
00304                         else // dopo la Y e i numeri, mi attendo parentesi chiusa )
00305                         {
00306                             pc.printf("> Errore = %c invece di ) \r\n\n", cReadChar); // diagnostica
00307                         }
00308                     }
00309                     else // dopo la X e i numeri mi aspetto Y
00310                     {
00311                         pc.printf("> Errore = %c invece di Y \r\n\n", cReadChar); // diagnostica
00312                     }
00313                 }
00314                 else // dopo la ( mi aspetto Y
00315                 {
00316                     pc.printf("> Errore = %c invece di X \r\n\n", cReadChar); // diagnostica
00317                 }
00318             } // if(comandi alfanumerici) 
00319         } // if(cReadChar == '(')
00320     }
00321 }
00322 
00323 /*********************************************************************************************************************************************/
00324 /* ogni DELTAT secondi scatta questo ticker.                                                                                                 */
00325 /* Tra due Tick viene contato il numero di mpulsi impulsi di encoder ricevuti con degli interrupt e contentuo nella variabile nCountRiseEdge */
00326 /*********************************************************************************************************************************************/
00327 void SpeedCalculate()
00328 {
00329        
00330     // se bReset = true non fare nessun calcolo della velocità e spostamento e azzera velocità e spostamento 
00331     if(!bReset)
00332     {
00333         //pc.printf("Sono qui 1 \n\r"); // diagnostica 
00334         //+++++++++++++++++++++++++ INIZIO Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++
00335         //nCountRiseEdge++; //----diagnostica
00336         // se nella IRQ, durante il periodo di calcolo della velocità,  sono stati contati fronti di salita dell'encoder, il robot si sta muovendo         
00337         if(nCountRiseEdge != nOldCountRiseEdge) // se c'è stata una variazione di conteggio impulsi, il robot si sta muovendo
00338         {
00339             //pc.printf("nCountRiseEdge= %d ; nOldCountRiseEdge= %d \n\r", nCountRiseEdge, nOldCountRiseEdge); // diagnostica 
00340             
00341             // Distanza Persorsa[metri] = ( (circonferenza ruota)/(numero impulsi per giro) ) * (Numero di Impulsi contati)
00342             fDistanzaPercorsa = fDistanzaPerStep*nCountRiseEdge; 
00343             
00344             // calcola la velocità in [m/sec]. DELTAT è in [sec] lo spostamento è in [m]
00345             //fSpeed = float((PI*DIAMETRORUOTA/IMPULSIPERGIRO)*(nCountRiseEdge-nOldCountRiseEdge))/DELTAT); 
00346             fSpeed = (fDistanzaPerStep*(nCountRiseEdge-nOldCountRiseEdge))/DELTAT; 
00347             
00348             // ricorda lo spostamento
00349             nOldCountRiseEdge = nCountRiseEdge;
00350            
00351             // comunica al cellulare vleocità e spostamento mentre si sta spostando
00352             //PRIMA ERA QUI ma si bloccava myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
00353         }
00354         else
00355         {
00356             // se non ci sono variazioni di impulsi, il robot è fermo, la velocità è 0.0
00357             fSpeed= 0.0;
00358         }
00359         
00360         //myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); // diagnostica
00361     }
00362     else
00363     {
00364         // bReset = true
00365         // comunica al cellulare vleocità e spostamento nulli
00366         nOldCountRiseEdge=0; // non ci sono variazioni di numero di impulsi
00367         nCountRiseEdge=0; // non ci sono variazioni di numero di impulsi
00368         fSpeed =0.0;
00369         fDistanzaPercorsa = 0.0;
00370         myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
00371     }    
00372     //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
00373 }
00374 
00375 
00376 
00377 /**********/
00378 /*  MAIN  */
00379 /**********/
00380 int main()
00381 {  
00382     // inizializza PWM del motore coda
00383     MotoreCoda.period_ms(50);  // periodo PWM
00384     bCodaInMovimento = false;
00385   
00386     // messaggio di benvenuto
00387     pc.printf("\r\n************  Hallo ****************** \r\n");
00388     pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
00389     
00390     // inizializza variabili da BLE
00391     cCommandBLE = 0; // inizialmente nessun comando da BLE
00392     cOldCommandBLE = 0; // inizialmente nessun comando da BLE
00393     cParamBLE = 0;  // inizialmente nessun parametro da BLE
00394     nParamBLE=0;    // inizialmente nessun parametro da BLE
00395     nOldParamBLE=0; // inizialmente nessun parametro da BLE
00396     fX = 0;     // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
00397     fOldX = 0;  // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
00398     fY = 0;     // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
00399     fOldY = 0;  // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
00400     bReset = false; //bReset = true/false quando riceve un comando (R1)/(R0) dalla APP
00401           
00402     // inizializza variabili
00403     fDistanzaPercorsa = 0.0;       
00404     fSpeed = 0.0;
00405     
00406     // inizializza array di caratteri ricevuti
00407     for(nIndex=0; nIndex < PACKETDIM; nIndex++)
00408      {caRxPacket[nIndex]=0;}
00409     nCharCount=0;
00410         
00411     
00412     // inizializza i valori di modulo e fase ricevuti dal joystick
00413     nRo = 0;
00414     nTeta = 0;
00415        
00416     //+++++++++++++++++ INIZIO Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00417     // conta il numero di impulsi del segnale di encoder che si verificano in DELTAT millisecondi
00418     // gli impulsi di encoder vengono contati da una IRQ collegata all'input da encoder
00419     // ogni DELTAT secondi scatta un ticker che calcola la velocità
00420     
00421     // definisci il mode del segnale digitale di EncoderA
00422     InEncoderA.mode(PullUp);
00423     
00424     // Associa routine di Interrup all'evento fronte di salita del segnale di encoder
00425     InEncoderA.rise(&riseEncoderIRQ);
00426     // azzera il  contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
00427     nCountRiseEdge=0;
00428     nOldCountRiseEdge=0;               
00429     
00430     InEncoderA.enable_irq();
00431     SpeedCalculateTicker.attach(&SpeedCalculate,DELTAT);
00432     //+++++++++++++++++ FINE Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00433     
00434     // Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
00435     myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla seriale del BLE
00436     pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC
00437     
00438     // attiva un ticker per simulare robot in movimento. 
00439     //!!!!!!!!!!!!!!!!!!! INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!
00440     #ifdef ENCODERSIMULATE
00441     // attiva il Ticker per simulare il calcolo della velocità. Ogni fDeltaTick viene simulato l'arrivo di un impulso dall'encoder del motore
00442     fDeltaTick = 0.05; // velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
00443     EncoderSimulateTicker.attach(&EncoderSimulate,fDeltaTick); // Diagnostica
00444     #endif
00445     //!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!!!!!
00446     
00447     
00448     //++++++++++++++++++++++++++++++ INIZIO Test motore Coda +++++++++++++++++++++++++++++++++++++++++
00449     /*
00450     //pc.printf("Coda in movimento \n\r"); //diagnostica
00451     while(true)
00452     {
00453         
00454         if ( ( fX != 0 ) || (fY != 0))
00455         {
00456             if(!bCodaInMovimento) // attiva il PWM solo se la coda è ferma
00457             {
00458                 MotoreCoda.write (0.6);
00459                 bCodaInMovimento = true;
00460                 pc.printf("Coda in movimento \n\r"); 
00461             }
00462         }
00463         else
00464         {
00465             // il joystick è in posizione (0,0), ferma la coda e comunica una sola volta che la velocità è 0
00466             if(bCodaInMovimento) // spegne il PWM solo se la coda è in movimento
00467             {
00468                 pc.printf("Coda ferma \n\r"); 
00469                 MotoreCoda.write (0.0);
00470                 bCodaInMovimento = false;
00471                 // comunica al cellulare vleocità nulla
00472                 // Disattiva/Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
00473                 //NVIC_DisableIRQ(USART1_IRQn);
00474                 myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r", 100.0, 1000 ); 
00475                 //NVIC_EnableIRQ(USART1_IRQn);
00476             
00477             }
00478         }   
00479         
00480     }
00481     */
00482     //++++++++++++++++++++++++++++++ FINE Test Motore Coda +++++++++++++++++++++++++++++++++++++++++++
00483     
00484      
00485     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00486     //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++    
00487     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00488         
00489     while(true)
00490     {
00491     
00492         //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++
00493         
00494         if ((fX!=0) || (fY!=0))     //la coda non si muove se il Joystick è nella posizione (0,0) 
00495         {
00496             // il joystick è in posizione diversa da (0,0), fai muovere la coda
00497             if(!bCodaInMovimento) // attiva il PWM solo se la coda è ferma
00498             {
00499                 //pc.printf("Coda in movimento \n\r"); //diagnostica
00500                 MotoreCoda.write (0.4);
00501                 bCodaInMovimento = true;
00502                 
00503             }
00504         }
00505         else
00506         {
00507             // il joystick è in posizione (0,0), ferma la coda e comunica una sola volta che la velocità è 0
00508             if(bCodaInMovimento) // spegne il PWM solo se la coda è in movimento
00509             {
00510                 //pc.printf("Coda ferma \n\r"); //diagnostica
00511                 MotoreCoda.write (0.0);
00512                 bCodaInMovimento = false;
00513                 // comunica al cellulare vleocità nulla
00514                 // Disattiva/Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
00515                 //NVIC_DisableIRQ(USART1_IRQn);
00516                 //myBLE.printf("Speed= 0.0 [m/s]; Trip= %.2f [m]\n\r", fDistanzaPercorsa ); 
00517                 //NVIC_EnableIRQ(USART1_IRQn);
00518             
00519             }
00520         }   
00521         
00522         if((cCommandBLE != cOldCommandBLE) || (nParamBLE != nOldParamBLE))
00523         {
00524             switch (cCommandBLE)
00525             { 
00526                 case 'T': // accendi/spegni LED su scheda
00527                 {
00528                     myLed = nParamBLE;
00529                 } break;
00530                 case 'L': // Accendi/spegni illuminazione a LED
00531                 {   
00532                     Light = nParamBLE;
00533                 } break;
00534                 case 'R': // Reset odometria e illuminazione
00535                 {    
00536                     if(nParamBLE==1)
00537                     {
00538                         bReset = true;
00539                         nCountRiseEdge = 0;
00540                         nOldCountRiseEdge = 0;
00541                         Light = 0;
00542                         fDistanzaPercorsa = 0.0;
00543                         fSpeed = 0.0;
00544                     }
00545                     else
00546                     {
00547                         // se nParamBLE = 0, e comunque diverso da 1, bReset=false -> ricomincia a funzionare normalmente
00548                         bReset = false;
00549                     }  
00550                 } break;
00551                 
00552                 default: break;
00553             }
00554             //pc.printf("Comando = %c, Parametro = %d \r\n", cCommandBLE, nParamBLE); // diagnostica
00555             cOldCommandBLE = cCommandBLE;
00556             nOldParamBLE = nParamBLE;
00557         }
00558         
00559         //++++++++++++++++++++++++++++++++++++++++++++ FINE Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++++++++++++
00560         
00561         //+++++++++++++++++++++++++++++++ INIZIO Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++
00562         //Invert X
00563         //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y
00564         //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X
00565         //Calcola R: R = (V+W) /2
00566         //Calcola L: L= (V-W)/2
00567         //Scala i valori di L e R in base all'hardware.
00568         //invia i valori al robot.
00569         // se ci sono stati cambiamenti nella posizione del joystick, cambia i comandi di velocità delle ruote
00570         if( (fX != fOldX) || (fY != fOldY))
00571         {
00572             fOldX = fX;
00573             fOldY = fY;
00574             // algoritmo di conversione dalla posizione del Joystick (fX, fY) alla velocità delle ruote (fR, fL)
00575             fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio
00576             fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio
00577             fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
00578             fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100)
00579             // diagnostica   
00580             //pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY); // diagnostica   
00581             //pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); // diagnostica   
00582             //pc.printf("> Velocita' Right R = %.2f\r\n", fR); // diagnostica   
00583             //pc.printf("> Velocita' Left  L = %.2f\r\n\r\n", fL); // diagnostica   
00584             
00585             // comunica al cellulare vleocità e spostamento mentre si sta spostando
00586             // Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
00587             NVIC_DisableIRQ(USART1_IRQn);
00588             myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
00589             NVIC_EnableIRQ(USART1_IRQn);
00590             
00591             // algoritmo di movimentazione delle ruote.
00592             if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
00593             {
00594                 fR =-fR;
00595                 // Vai indietro
00596                 PostOutBI1 = 1;
00597                 PostOutBI2 = 0; 
00598             }
00599             else
00600             {
00601                 if(fR >0)
00602                 {
00603                     // Vai avanti
00604                     PostOutBI1 = 0;
00605                     PostOutBI2 = 1; 
00606                 }
00607                 else
00608                 {
00609                     // spegni 
00610                     PostOutBI1 = 0;
00611                     PostOutBI2 = 0;
00612                 }
00613             }
00614             PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore)
00615             if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore
00616             {
00617                 fL =-fL;
00618                 // Vai indietro
00619                 AntOutBI1 = 1;
00620                 AntOutBI2 = 0; 
00621             }
00622             else
00623             {
00624                 if(fL >0)
00625                 {
00626                     // Vai avanti
00627                     AntOutBI1 = 0;
00628                     AntOutBI2 = 1; 
00629                     
00630                 }
00631                 else
00632                 {
00633                     // spegni 
00634                     AntOutBI1 = 0;
00635                     AntOutBI2 = 0;
00636                 }
00637             }
00638             AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
00639         } //if( (fX != fOldX) || (fY != fOldY))
00640        
00641         //++++++++++++++++++++++ FINE Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++++
00642     } //while (true) Ciclo principale
00643     
00644     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00645     //++++++++++++++ FINE Ciclo Principale  +++++++++++++++++++    
00646     //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00647     
00648 } // main()