MicRobot-Rev083.cpp

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