Diff: Mic-Rev03.cpp

Revision:

7:7043da244e4b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mic-Rev03.cpp	Thu Aug 08 09:04:07 2019 +0000
@@ -0,0 +1,839 @@
+// pilotaggio carrello tramite BLE.
+// testato su L476RG e F401RE
+
+#include "mbed.h"
+#include<stdlib.h>
+
+
+// pi greco
+#define PI           3.14159265358979323846
+
+// diametro della ruota in [metri]
+#define DIAMETRORUOTA (0.1)
+
+// numero di impulsi per giro generati dall'encoder
+#define IMPULSIPERGIRO 4
+
+// numero di cifre con cui si vuole rappresentare la distanza percorsa in [m]. NUMCIFREDISTANZAPERCORSA = 5, significa che la distanza è rappresentata come xxx.xx [m]
+#define NUMCIFREDISTANZAPERCORSA 5
+
+// 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]
+#define NUMCIFRESPEED 5
+
+// intervallo di tempo in [msec], cui vengono contati gli impulsi di encoder per il calcolo della velocità
+#define DELTAT 200
+
+// ticker per il calcolo della velocità
+//Ticker SpeedTicker;
+
+// Timer per il calcolo della velocità. Nel periodo di tempo del timer, conta gli impulsi ricevuti come interrupt dall'encoder
+Timer TimerSpeed;
+
+// tempo inizio intermedio e fine del timer che misura il tempo per il calcolo della velocità
+int nTimerStart, nTimerCurrent, nTimerStop, nTimerTillNow;    
+
+// Ticker per la simulazione di segnale proveniente da encoder sul motore
+Ticker SpeedTicker;
+    
+// Definizione periferica USB seriale del PC
+Serial pc(USBTX, USBRX, 921600); // seriale di comunicazione con il PC. Associati a PA_11 e PA_12
+
+// Definizione periferica seriale del Modulo BLE ELETT114A
+Serial myBLE(PA_9, PA_10, 9600); //Tx, Rx, bps
+
+// Input di Reset per il Modulo BLE HC-05
+DigitalOut BleRst(PA_8);
+
+// User Button, LED  
+DigitalIn myButton(USER_BUTTON); // pulsante Blu sulla scheda. Associato a PC_13
+DigitalOut myLed(LED2); // LED verde sulla scheda. Associato a PA_5
+
+// output digitale per pilotaggio illuminazione a LED
+DigitalOut Light(PA_0);
+//DigitalIn InDiag(PC_0,PullUp); // Di Default è a Vcc. Può essere collegato a GND con un ponticello su CN10 pin18-pin20 
+InterruptIn InEncoderA(PC_0); // segnale di encoder di un motore.
+
+// carattere letto dalla seriale del PC
+char cReadChar;
+
+// DutyCycle del segnale PWM
+float fDutyCycle;
+
+// variabile che conta il numero di fronti si salita del segnale encoder di uno dei motori del robot
+volatile int nCountRiseEdge; 
+volatile int nOldCountRiseEdge;
+
+// Input/Output
+DigitalOut PostOutBI1 (PA_6);  // Output 1 per pilotaggio input BI1 del Motore B Posteriore
+PwmOut PostOutPWB (PB_6);  // Output per pilotaggio input PWM del motore B Posteriore
+//DigitalOut PostOutPWB (PA_7);  // Scopi Diagnostici: Output Digitale per pilotaggio PWM del motore B Posteriore
+DigitalOut PostOutBI2 (PA_7);  // Output 2 per pilotaggio input BI2 del Motore B Posteriore
+DigitalIn PostInNE1 (PC_7); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore
+
+DigitalOut AntOutBI1 (PB_4);  // Output 1 per pilotaggio input BI1 del Motore B Anteriore
+PwmOut AntOutPWB (PB_5);  // Output per pilotaggio input PWM del motore B Anteriore
+//DigitalOut AntOutPWB (PB_5);  // Scopi diagnostici: Output Digitalte per pilotaggio PWM del motore B Anteriore
+DigitalOut AntOutBI2 (PB_3);  // Output 2 per pilotaggio input BI2 del Motore B Posteriore
+DigitalIn AntInNE1 (PB_10); // Input per acquisire i segnali NET1 in output dall'encoder Anteriore
+
+
+//carattere di comando ricevuto dal BLE
+volatile char cCommandBLE; // cambia nella routine di interrupt
+// memorizza l'ultimo comando ricevuto. Ci saranno delle azioni solo se il comando ricevuto è cambiato rispetto al precedente
+char cOldCommandBLE;
+
+// coordinate polari del joistick sulla APP, fornite dalla routine di interrupt
+volatile double fTeta;
+volatile double fRo;
+
+// coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt
+volatile double fX, fY;
+// memorizza ultimi valori delle coordinate del joistick
+double fOldX, fOldY;
+
+// variabili ausiliarie per l'algoritmo di posizionamento
+double fV, fW;
+
+// velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore
+double fR, fL;
+
+// distanza percorsa in [m], calcolata utilizzando gli impulsi dell'encoder sul motore
+double fDistanzaPercorsa;
+
+// velocità calcolata gli impulsi contati in un intervallo DELTAT msec
+double fSpeed;
+
+// Scopi diagnostici:  Ogni fDeltaTick viene simulata la generazione di un impulso di encoder.
+// velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
+double fDeltaTick;
+
+// distanza percorsa e relativo indice, calcolata in [m] e trasformata in caratteri   | centinaia di [m] | decine di [m] |  [m] | decimi di [m] | centesimi di [m]
+//char caDistanzaPercorsa[NUMCIFREDISTANZAPERCORSA];
+//int nIndexDistanzaPercorsa;
+
+//**********************************************/
+//          IRQ associata a Rx da BLE 
+//**********************************************/
+void BLERxInterrupt(void)
+{
+    // array per la ricezione dei messaggi da seriale
+    char cReadChar;
+    
+    //indice per i cicli
+    int nIndex;
+    
+    // array per la ricezione dei messaggi da seriale
+    char caRxPacket[8];
+    //int nRxPacketSize;
+           
+    // variabile per estrarre le cifre della distanza percorsa. La distanza Percorsa viene calcolata nel MAIN con la varibile fDistanzaPercorsa
+    int nDistanzaPercorsa;
+    // distanza percorsa in [m], divisa in caratteri in caratteri e relativo indice   | centinaia di [m] | decine di [m] |  [m] | decimi di [m] | centesimi di [m]
+    char caDistanzaPercorsa[NUMCIFREDISTANZAPERCORSA];
+    int nIndexDistanzaPercorsa;
+
+    // variabile per estrarre le cifre della velocità di percorrenza. La velocità viene calcolata nel MAIN con la varibile fSpeed
+    int nSpeed;
+    // distanza percorsa in [m], divisa in caratteri in caratteri e relativo indice   | centinaia di [m] | decine di [m] |  [m] | decimi di [m] | centesimi di [m]
+    char caSpeed[NUMCIFRESPEED];
+    int nIndexSpeed;
+    
+   
+    //pc.printf("BLE RxInterrupt: \n\r");
+    
+    // ricevi caratteri su seriale, se disponibili   
+    while((myBLE.readable()))
+    {
+        // acquisice stringa in input e relativa dimensione
+        cReadChar = myBLE.getc(); // Read character
+        if(cReadChar == 0x02)
+        {
+            
+            //++++++++++++++++++++++++++++++++++++++++++++ INIZIO ricevi messaggio Button da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++    
+            //pc.printf(">Ricevuto\r\n "); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
+                 
+            //-- command will be 8 bytes for joystick values
+            //-- command will be 3 bytes for button change event
+            //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03)
+
+
+            caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX
+            cReadChar = myBLE.getc(); // legge il secondo carattere
+            if(cReadChar > 0x40)
+            {
+                // Button:
+                //-- Button events send a single character in a 3-byte packet
+                //-- B1 uses "A" for changed to on, "B" for changed to off -- command packet when B1 is click on is <STX> <"A"> <ETX>
+                //-- B2 uses "C" for changed to on, "D" for changed to off
+                //-- B3..B6 follow in order; valid button even characters are "A".."L"
+                caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP
+                caRxPacket[2] = myBLE.getc();  // legge e memorizza il terzo carattere ETX
+                // passa il comando ricevuto nella variabile globale
+                cCommandBLE = caRxPacket[1];
+                
+                // Diagnostica
+                /*
+                pc.printf(">: 0x%02x, 0x%02x, 0x%02x\n\r\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2]); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
+                */
+                //++++++++++++++++++++++++++++++++++++++++++++ FINE ricevi messaggio Button da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++    
+            }   
+            else
+            {
+                //++++++++++++++++++++++++++++++++++++++++++++ INIZIO ricevi messaggi Joystick da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+                // Joystick:
+                //-- The JBTC joystick values range from -100 to 100; these are values are transmitted in ASCII format after an offset of 200 is added to each axis
+                //-- offset is added so that values can be sent as three ASCII chars: hundreds digit, tens digit, ones digit without sign indicator
+                //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX>
+                caRxPacket[1] = cReadChar; // memorizza il secondo carattere
+                for(nIndex=2; nIndex<8; nIndex++)
+                {
+                    caRxPacket[nIndex] = myBLE.getc();
+                }
+                // dal messaggio estrae e visualizza le coordinate cartesiane
+                fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200);
+                fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200);
+                // converte la posizione del joistick in coordinate polari
+                fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo
+                if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360°
+                fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100
+            
+                // diagnostica
+                /*
+                pc.printf(">: 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2], caRxPacket[3], caRxPacket[4], caRxPacket[5], caRxPacket[6], caRxPacket[7]); // visualizza comando da BLE
+                pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane
+                pc.printf(">: fTeta = %.2f;  fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari
+                */
+            }
+            //++++++++++++++++++++++++++++++++++++++++++++ FINE ricevi messaggio Joystick da BLE e estrae dati +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+           
+            //++++++++++++++++++++++++++++++++++++++++++++ INIZIO estrae le digits della distanza percorsa +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+            //fDistanzaPercorsa = 980.20; // diagnostica
+            nDistanzaPercorsa = fDistanzaPercorsa*100; // moltiplica per 100 per considerare fino ai centesimi della distanza percorsa
+            nSpeed = fSpeed*100; // moltiplica per 100 per considerare fino ai centesimi della velocità
+            
+            // inizializza caDistanzaPercorsa[]
+            for(nIndexDistanzaPercorsa = 0; nIndexDistanzaPercorsa < NUMCIFREDISTANZAPERCORSA; nIndexDistanzaPercorsa++)
+            {
+                caDistanzaPercorsa[nIndexDistanzaPercorsa] = '0';
+            }
+            
+            // estrae le singole cifre di fDistanza, fino alla seconda cifra decimale
+            nIndexDistanzaPercorsa = 0;
+            do
+            {   
+                // genera digits della distanza percorsa
+                caDistanzaPercorsa[nIndexDistanzaPercorsa] =  nDistanzaPercorsa % 10 + '0';
+                
+                //pc.printf("caDistanzaPercorsa[%d]: %c \n\r", nIndexDistanzaPercorsa, caDistanzaPercorsa[nIndexDistanzaPercorsa]);    // diagnostica
+                nIndexDistanzaPercorsa++;
+            } 
+            while ((nDistanzaPercorsa /= 10) > 0) ;  
+            // Diagnostica: visualizza la distanza percorsa sul PC
+            //pc.printf("DistanzaPercorsa: %c,%c,%c,%c,%c \n\r", caDistanzaPercorsa[4], caDistanzaPercorsa[3], caDistanzaPercorsa[2], caDistanzaPercorsa[1], caDistanzaPercorsa[0]); // diagnostica
+            //++++++++++++++++++++++++++++++++++++++++++++ FINE estrae le digits della distanza percorsa +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+            
+            //++++++++++++++++++++++++++++++++++++++++++++ INIZIO estrae le digits della velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+            // inizializza caSpeed[]
+            for(nIndexSpeed = 0; nIndexSpeed < NUMCIFREDISTANZAPERCORSA; nIndexSpeed++)
+            {
+                caSpeed[nIndexSpeed] = '0';
+            }
+            
+            // estrae le singole cifre di fSpeed, fino alla seconda cifra decimale
+            nIndexSpeed = 0;
+            do
+            {   
+                // genera digits della velocità
+                caSpeed[nIndexSpeed] =  nSpeed % 10 + '0';     
+                
+                //pc.printf("caSpeed[%d]: %c \n\r", nIndexSpeed, caSpeed[nIndexSpeed]);    // diagnostica
+                nIndexSpeed++;
+            } 
+            while ((nSpeed /= 10) > 0) ;  
+            //++++++++++++++++++++++++++++++++++++++++++++ FINE estrae le digits della velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+            
+            //++++++++++++++++++++++++++++++++++++++++++++ INIZIO Invia messaggio con spostamento e velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+          
+            // Diagnostica: visualizza la velocità sul PC 
+            //pc.printf("Speed: %c,%c,%c,%c,%c \n\r", caSpeed[4], caSpeed[3], caSpeed[2], caSpeed[1], caSpeed[0]); // diagnostica
+            
+            // Struttura Nominale del comando da inviare al robot                   STX ,B6, B5, B4,  B3  B2 B1  0x01,Data1,...........................................................................................................,0x04,Data2,.....................................................,0x05,Data3,...................,ETX;
+            myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,' ',' ',' ',' ',' ',' ',0x01,caDistanzaPercorsa[4],caDistanzaPercorsa[3],caDistanzaPercorsa[2],',',caDistanzaPercorsa[1],caDistanzaPercorsa[0],0x04,caSpeed[4],caSpeed[3],caSpeed[2], ',',caSpeed[1],caSpeed[0],0x05,0x30,0x30,0x30,0x30,0x30, 0x03);
+            
+            //++++++++++++++++++++++++++++++++++++++++++++ FINE Invia messaggio con spostamento e velocità +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+         }
+    }
+}
+
+/**********************************************/
+//          IRQ associata a Rx da PC 
+//**********************************************/
+void pcRxInterrupt(void)
+{
+    // array per la ricezione dei messaggi da seriale
+    char cReadChar; 
+    
+    // ricevi caratteri su seriale, se disponibili   
+    while((pc.readable()))
+    {
+        // acquisice stringa in input e relativa dimensione
+        cReadChar = pc.getc(); // read character from PC
+        //myBLE.putc(cReadChar); // Diagnostica: write char to BLE
+        //pc.putc(cReadChar); // Diagnostica: write char to PC
+        
+        //pc.printf("W>: 0x%02x\n\r",cReadChar); // diagnostica
+        if(cReadChar == '0') // se scrivo '0', invia questa stringa
+        {
+            // DIAGNOSTICA:
+            // pc.printf("W>: Inviato comando a BLE\n\r"); // diagnostica
+            //If sending a response, the packet will contain four strings with additional separators: <STX> <buttons> <$01> <Data1> <$04> <Data2> <$05> <Data3> <ETX>
+            //-- button status is binary formatted string (no indicator)
+            //-- data fields sent as strings
+            //-- send empty string to unused field (not sure if short response packet is allowed without additional testing)
+            // Struttura Nominale del comando da inviare al robot                   STX , B6, B5, B4, B3  B2  B1 0x01, Data1,....................,0x04,Data2,..............,0x05,Data3,...................,ETX;
+            myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','1','1','1','0',0x01,'9','8','7','6',',','0','2',0x04,'-','5','4', ',','9',0x05,0x35,0x34,0x33,0x32,0x31, 0x03);
+         }
+    }
+}
+
+
+/**************************************************************************************/
+/* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
+/**************************************************************************************/
+void riseEncoderIRQ()
+{
+    nCountRiseEdge++;
+}       
+
+/****************************************************************************************/
+/* Diagnostica:                                                                         */
+/* COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO   */
+/* Routine di gestione del ticker per simulare encoder                                  */  
+/* Simula il segnale di encoder ricevuto con un determinato DELTAT                      */
+/* A robot fermo, il segnale di encoder non genera interrupt.                           */
+/* Questo Ticker simula l'arrivo del segnale da encoder                                 */
+/****************************************************************************************/
+void SpeedCalculate()
+{
+    // ad ogni tick viene ricevuto un impulso da encoder.
+    // velocità = (Spazio per ogni tick)/(tempo per ogni tick)
+    // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
+    
+    // simula impulso inviato dall'encoder 
+    nCountRiseEdge++;
+} 
+
+
+/**********/
+/*  MAIN  */
+/**********/
+int main()
+{
+   
+    // messaggio di benvenuto
+    pc.printf("\r\n************  Hallo ****************** \r\n");
+    pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
+    
+    // inizializza variabili da BLE
+    cCommandBLE = 0; // inizialmente nessun comando da BLE
+    cOldCommandBLE = 0; // inizialmente nessun comando da BLE
+    fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
+    fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
+    fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
+    fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
+            
+    // inizializza il BLE
+    BleRst = 0;
+    wait_ms(100);
+    BleRst = 1;
+    cCommandBLE = '0';
+    cOldCommandBLE = '0';
+           
+    // inizializza i PWM di pilotaggio dei  motori Posteriore e Anteriore
+    PostOutPWB.period_us(10); // periodo del PWM Posteriore
+    PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore)
+    AntOutPWB.period_us(10); // periodo del PWM Anteriore
+    AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore)
+           
+    // inizializza variabili
+    fDistanzaPercorsa = 0.0;       
+    fSpeed = 0.0;
+           
+    // Attiva la IRQ per la RX su seriale   
+    myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del BLE
+    pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC
+    
+    // attiva un ticker per simulare robot in movimento. 
+    //+++++++++ INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO +++++++++++++
+    /*
+    fDeltaTick = 0.05; // velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
+    SpeedTicker.attach(&SpeedCalculate,fDeltaTick); // Diagnostica
+    */
+    //+++++++++ FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO +++++++++++++
+        
+    //+++++++++++++++++ INIZIO Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    // definisci il mode del segnale digitale di EncoderA
+    InEncoderA.mode(PullUp);
+    
+    // Associa routine di Interrup all'evento fronte di salita del segnale di encoder
+    InEncoderA.rise(&riseEncoderIRQ);
+    
+    // azzera il  contatore dei fronti di salita del segnale di encoder. Saranno contati nella IRQ legata a InEncoderA
+    nCountRiseEdge=0;
+    nOldCountRiseEdge=0;           
+    //+++++++++++++++++ FINE Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    
+    
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++
+    //++++++++++++++ INIZIO Ciclo Principale +++++++++++++    
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++
+    
+    while(true)
+    {
+        //+++++++++++++++++++++++++++ INIZIO calcola spostamento con encoder sul motore +++++++++++++++++++++++++++++++++++++
+        
+        // abilita interrupt sul segnale di encoder per contare il numero di impulsi e quindi la velocità di spostamento del robot     
+        InEncoderA.enable_irq();
+        
+        // conta il numero di impulsi del segnale di encoder che si verificano in DELTAT millisecondi
+        TimerSpeed.start();
+        nTimerStart=TimerSpeed.read_ms();
+              
+        // per 100ms conta gliimpulsi sull'encoder 
+        while( (nTimerCurrent-nTimerStart) < DELTAT) // attende il passare di DELTAT millisec
+        {
+            nTimerCurrent=TimerSpeed.read_ms();
+            // pc.printf("CounterTimer= %d\r\n", (nTimerCurrent-nTimerStart));   
+        }
+        TimerSpeed.stop(); // ferma il timer
+        
+        // disabilita interrupt sul segnale di encoder per contare il numero di impulsi. Disattivandolo, si evita di continuare a mandare in interrupt il processore
+        InEncoderA.enable_irq();
+        //+++++++++++++++++++++++++++++ FINE calcola spostamento con encoder sul motore ++++++++++++++++++++++++++++++++++++++++      
+    
+        //+++++++++++++++++++++++++ INIZIO Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++
+        //nCountRiseEdge++; //----diagnostica
+        // se nella IRQ, durante il periodo di calcolo della velocità,  sono stati contati fronti di salita dell'encoder, il robot si sta muovendo         
+        if(nCountRiseEdge != nOldCountRiseEdge) // se c'è stata una variazione di conteggio impulsi, il robot si sta muovendo
+        {
+            // Distanza Persorsa[metri] = ( (circonferenza ruota)/(numero impulsi per giro) ) * (Numero di Impulsi contati)
+            fDistanzaPercorsa = (PI*DIAMETRORUOTA/IMPULSIPERGIRO)*nCountRiseEdge; 
+            
+            // calcola la velocità in [m/sec]. DELTAT è in [msec] lo spostamento è in [m]
+            fSpeed = float((PI*DIAMETRORUOTA/IMPULSIPERGIRO)*(nCountRiseEdge-nOldCountRiseEdge))/float(DELTAT/1000.0); 
+            
+            // ricorda lo spostamento
+            nOldCountRiseEdge = nCountRiseEdge;
+        }
+        //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
+        
+        //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++
+        if(cCommandBLE != cOldCommandBLE)
+        {
+            switch (cCommandBLE)
+            { 
+                case 'A': // accendi LED su scheda
+                {
+                    myLed = 1;
+                }; break;
+                case 'B': // spegni LED su scheda
+                {    
+                    myLed = 0;
+                }; break;
+                case 'C': // accendi illuminazione a LED
+                {
+                    Light = 1;
+                }; break;
+                case 'D': // spegni illuminazione a LED
+                {    
+                    Light = 0;
+                }; break;
+                case 'E': // Reset odometria e illuminazione
+                {    
+                    nCountRiseEdge = 0;
+                    nOldCountRiseEdge = 0;
+                    Light = 0;
+                    fDistanzaPercorsa = 0.0;
+                    fSpeed = 0.0;
+                    myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','0','0','0','0',0x01,'0','0','0',',','0','0',0x04,'0','0','0', ',','0','0',0x05,0x30,0x30,0x30,0x30,0x30, 0x03);
+                }; break;
+                case 'F': // Toglie Reset da odometria e illuminazione
+                {    
+                }; break;
+                
+                default: break;
+            }
+            pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica
+            cOldCommandBLE = cCommandBLE;
+        }
+        //++++++++++++++++++++++++++++++++++++++++++++ FINE Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        
+        //+++++++++++++++++++++++++++++++ INIZIO Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++
+        //Invert X
+        //Calcola R+L (Call it V): V =(100-ABS(X)) * (Y/100) + Y
+        //Calcola R-L (Call it W): W= (100-ABS(Y)) * (X/100) + X
+        //Calcola R: R = (V+W) /2
+        //Calcola L: L= (V-W)/2
+        //Scala i valori di L e R in base all'hardware.
+        //invia i valori al robot.
+        // se ci sono stati cambiamenti nella posizione del joystick, cambia i comandi di velocità delle ruote
+        if( (fX != fOldX) || (fY != fOldY))
+        {
+            fOldX = fX;
+            fOldY = fY;
+            // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL)
+            fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio
+            fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio
+            fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
+            fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100)
+         
+            // diagnostica   
+            pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY);
+            pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW);
+            pc.printf("> Velocita' Right R = %.2f\r\n", fR);
+            pc.printf("> Velocita' Left  L = %.2f\r\n\r\n", fL);
+            
+            // algoritmo di movimentazione delle ruote.
+            if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
+            {
+                fR =-fR;
+                // Vai indietro
+                PostOutBI1 = 1;
+                PostOutBI2 = 0; 
+            }
+            else
+            {
+                if(fR >0)
+                {
+                    // Vai avanti
+                    PostOutBI1 = 0;
+                    PostOutBI2 = 1; 
+                }
+                else
+                {
+                    // spegni 
+                    PostOutBI1 = 0;
+                    PostOutBI2 = 0;
+                }
+            }
+            PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore)
+            if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore
+            {
+                fL =-fL;
+                // Vai indietro
+                AntOutBI1 = 1;
+                AntOutBI2 = 0; 
+            }
+            else
+            {
+                if(fL >0)
+                {
+                    // Vai avanti
+                    AntOutBI1 = 0;
+                    AntOutBI2 = 1; 
+                    
+                }
+                else
+                {
+                    // spegni 
+                    AntOutBI1 = 0;
+                    AntOutBI2 = 0;
+                }
+            }
+            AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
+        }
+        //++++++++++++++++++++++ FINE Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++++
+    } //while (true) Ciclo principale
+   
+    //++++++++++++++++++++++++++++++++++++++++++++++++++
+    //++++++++++++++ FINE Ciclo Principale +++++++++++++    
+    //++++++++++++++++++++++++++++++++++++++++++++++++++
+    
+    
+    
+    
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++
+    //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++    
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++
+    
+    //++++++++++++++ INIZIO Test di calcolo e trasmissione distanza percorsa +++++++++++++++++++++++++++++++
+    /*
+    pc.printf("myButton = 1; iniziale\r\n");
+    while(myButton == 1);
+    pc.printf("myButton = 0; \r\n");
+    while(myButton == 0);
+    pc.printf("myButton = 1; finale\r\n\r\n");
+    fDistanzaPercorsa = 980.20;
+    nDistanzaPercorsa = fDistanzaPercorsa*100; // considera fino ai centesimi della distanza percorsa
+    
+    // inizializza caDistanzaPercorsa[]
+    for(nIndexDistanzaPercorsa = 0; nIndexDistanzaPercorsa < NUMCIFREDISTANZAPERCORSA; nIndexDistanzaPercorsa++)
+    {
+        caDistanzaPercorsa[nIndexDistanzaPercorsa] = '0';
+    }
+    
+    // estrae le singole cifre di fDistanza, fino alla seconda cifra decimale
+    nIndexDistanzaPercorsa = 0;
+    do
+    {   
+        // generate digits in reverse order 
+        caDistanzaPercorsa[nIndexDistanzaPercorsa] =  nDistanzaPercorsa % 10 + '0';    // get next digit 
+        
+        //pc.printf("caDistanzaPercorsa[%d]: %c \n\r", nIndexDistanzaPercorsa, caDistanzaPercorsa[nIndexDistanzaPercorsa]);    // diagnostica
+        nIndexDistanzaPercorsa++;
+    } 
+    while ((nDistanzaPercorsa /= 10) > 0) ;  
+    
+    // invia la distana percorsa al PC
+    pc.printf("DistanzaPercorsa: %c,%c,%c,%c,%c, \n\r", caDistanzaPercorsa[4], caDistanzaPercorsa[3], caDistanzaPercorsa[2], caDistanzaPercorsa[1], caDistanzaPercorsa[0]); // diagnostica
+    
+    // Struttura Nominale del comando da inviare al robot                 STX , B6, B5, B4, B3  B2  B1 0x01, Data1,..........................................................................................................,0x04,Data2,..............,0x05,Data3,...................,ETX;
+    //myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','0','0','0','0',0x01,caDistanzaPercorsa[4],caDistanzaPercorsa[3],caDistanzaPercorsa[2],',',caDistanzaPercorsa[1],caDistanzaPercorsa[0],0x04,'-','0','0', ',','0',0x00,0x00,0x00,0x00,0x00,0x00, 0x03);
+    myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','0','0','0','0',0x01,caDistanzaPercorsa[4],caDistanzaPercorsa[3],caDistanzaPercorsa[2],',',caDistanzaPercorsa[1],caDistanzaPercorsa[0],0x04,'-','5','4', ',','9',0x05,0x35,0x34,0x33,0x32,0x31, 0x03);
+    while(true);   
+    */                       
+    //+++++++++++++++ Fine Test di calcolo e trasmissione distanza percorsa ++++++++++++++++++++++++
+    
+   
+   
+   
+    //++++++++++++ INIZIO Test Ricezione Comandi da BLE ++++++++++++++++++++++++++++
+    /*
+    while(true)
+    {
+        if(cCommandBLE != cOldCommandBLE)
+        {
+            
+            switch (cCommandBLE)
+            { 
+                case 'A':
+                {
+                    myLed = 1;
+                }; break;
+                case 'B':
+                {    
+                    myLed = 0;
+                }; break;
+                case 'C':
+                {
+                    Light = 1;
+                }; break;
+                case 'D':
+                {    
+                    Light = 0;
+                }; break;
+                default: break;
+            }
+            // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica
+            cOldCommandBLE = cCommandBLE;
+        }
+        
+    } // while(true) Test comandi da BLE  
+    */
+    //++++++++++++ FINE ricezione comandi BLE ++++++++++++
+    
+    //++++++++++++ INIZIO Test modalità di funzionamento Motori con PWM ++++++++++++
+    /*
+    PostOutBI1 = 0;
+    PostOutBI2 = 0;    
+    AntOutBI1 = 0;
+    AntOutBI2 = 0;    
+    
+    fDutyCycle = 0.0;
+    
+    // inizializza il pin PWM
+    //+++PostOutPWB.period_us(100); // periodo del PWM Posteriore
+    //+++PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
+    //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore
+    //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
+    while(true)
+    {
+        // Vai avanti Anteriore
+        PostOutBI1 = 0;
+        PostOutBI2 = 0;  
+        AntOutBI1 = 1;
+        AntOutBI2 = 0;    
+        PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
+        AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+        pc.printf("Avanti Anteriore\r\n");
+        wait (2);
+        
+        // spegni 
+        PostOutBI1 = 0;
+        PostOutBI2 = 0; 
+        AntOutBI1 = 0;
+        AntOutBI2 = 0; 
+        pc.printf("Fermo\r\n\r\n");
+        wait (3);
+        
+        // Vai Indietro Anteriore
+        PostOutBI1 = 0;
+        PostOutBI2 = 0;
+        AntOutBI1 = 0;
+        AntOutBI2 = 1;
+        PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
+        AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+        pc.printf("Indietro Anteriore \r\n");
+        wait (2);
+        
+        // spegni 
+        PostOutBI1 = 0;
+        PostOutBI2 = 0;
+        AntOutBI1 = 0;
+        AntOutBI2 = 0;
+        pc.printf("Fermo\r\n\r\n");
+        wait (3);
+        
+        // Vai avanti Posteriore
+        PostOutBI1 = 1;
+        PostOutBI2 = 0;  
+        AntOutBI1 = 0;
+        AntOutBI2 = 0;    
+        PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+        AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
+        pc.printf("Avanti Posteriore\r\n");
+        wait (2);
+        
+        // spegni 
+        PostOutBI1 = 0;
+        PostOutBI2 = 0; 
+        AntOutBI1 = 0;
+        AntOutBI2 = 0; 
+        pc.printf("Fermo\r\n\r\n");
+        wait (3);
+        
+        // Vai Indietro Posteriore
+        PostOutBI1 = 0;
+        PostOutBI2 = 1;
+        AntOutBI1 = 0;
+        AntOutBI2 = 0;
+        PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+        AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
+        pc.printf("Indietro Posteriore \r\n");
+        wait (2);
+        
+        // spegni 
+        PostOutBI1 = 0;
+        PostOutBI2 = 0;
+        AntOutBI1 = 0;
+        AntOutBI2 = 0;
+        pc.printf("Fermo\r\n\r\n");
+        wait (3);
+            
+        // Vai avanti Anteriore + Posteriore
+        PostOutBI1 = 1;
+        PostOutBI2 = 0;  
+        AntOutBI1 = 1;
+        AntOutBI2 = 0;    
+        PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+        AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+        pc.printf("Avanti Anteriore + Posteriore\r\n");
+        wait (2);
+        
+        // spegni 
+        PostOutBI1 = 0;
+        PostOutBI2 = 0; 
+        AntOutBI1 = 0;
+        AntOutBI2 = 0; 
+        pc.printf("Fermo\r\n\r\n");
+        wait (3);
+        
+        // Vai Indietro Anteriore + Posteriore
+        PostOutBI1 = 0;
+        PostOutBI2 = 1;
+        AntOutBI1 = 0;
+        AntOutBI2 = 1;
+        PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+        AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+        pc.printf("Indietro Anteriore + Posteriore \r\n");
+        wait (2);
+        
+        // spegni 
+        PostOutBI1 = 0;
+        PostOutBI2 = 0;
+        AntOutBI1 = 0;
+        AntOutBI2 = 0;
+        pc.printf("Fermo\r\n\r\n");
+        wait (3);
+        
+        // Vai avanti Anteriore + Posteriore velocità ridotta
+        PostOutBI1 = 1;
+        PostOutBI2 = 0;  
+        AntOutBI1 = 1;
+        AntOutBI2 = 0;    
+        PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
+        AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
+        pc.printf("Avanti Anteriore + Posteriore velocita' ridotta\r\n");
+        wait (2);
+        
+        // spegni 
+        PostOutBI1 = 0;
+        PostOutBI2 = 0; 
+        AntOutBI1 = 0;
+        AntOutBI2 = 0; 
+        pc.printf("Fermo\r\n\r\n");
+        wait (3);
+        
+        // Vai Indietro Anteriore + Posteriore velocità ridotta
+        PostOutBI1 = 0;
+        PostOutBI2 = 1;
+        AntOutBI1 = 0;
+        AntOutBI2 = 1;
+        PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
+        AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
+        pc.printf("Indietro Anteriore + Posteriore velocita' ridotta\r\n");
+        wait (2);
+        
+        // spegni 
+        PostOutBI1 = 0;
+        PostOutBI2 = 0;
+        AntOutBI1 = 0;
+        AntOutBI2 = 0;
+        pc.printf("Fermo\r\n\r\n");
+        wait (3);
+        
+        // Ruota a destra
+        PostOutBI1 = 0;
+        PostOutBI2 = 1;
+        AntOutBI1 = 1;
+        AntOutBI2 = 0;
+        PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+        AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+        pc.printf("Ruota a Destra\r\n");
+        wait (2);
+        
+        // spegni 
+        PostOutBI1 = 0;
+        PostOutBI2 = 0;
+        AntOutBI1 = 0;
+        AntOutBI2 = 0; 
+        pc.printf("Fermo\r\n\r\n");
+        wait (3);
+        
+        // Ruota a sinistra
+        PostOutBI1 = 1;
+        PostOutBI2 = 0;
+        AntOutBI1 = 0;
+        AntOutBI2 = 1;
+        PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
+        AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
+        pc.printf("Ruota a Sinistra\r\n");
+        wait (2);
+        
+        // spegni 
+        PostOutBI1 = 0;
+        PostOutBI2 = 0;
+        AntOutBI1 = 0;
+        AntOutBI2 = 0; 
+        pc.printf("Fermo\r\n\r\n");
+        wait (3);
+        
+    } // while(true) Test motore con PWM    
+    */
+    //++++++++++++ FINE Test Motore con PWM ++++++++++++
+     
+    //++++++++++++++++++++++++++++++++++++++++++++++++++
+    //++++++++++++++ FINE Ciclo test +++++++++++++++++++    
+    //++++++++++++++++++++++++++++++++++++++++++++++++++
+
+} // main()