Potrebbe funzionare con la nuova APP

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
pinofal
Date:
Wed Jul 08 19:55:47 2020 +0000
Parent:
17:e74856e7a232
Commit message:
Potrebbe funzionare con la nuova APP

Changed in this revision

MicRobot-Rev084.cpp Show diff for this revision Revisions of this file
MicRobot-Rev085.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r e74856e7a232 -r 4b1d35d5f4fd MicRobot-Rev084.cpp
--- a/MicRobot-Rev084.cpp	Wed Jul 08 10:14:02 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,657 +0,0 @@
-// pilotaggio carrello tramite BLE.
-// testato su L476RG e F401RE
-
-#include "mbed.h"
-#include<stdlib.h>
-
-// attivare questa #define quando si vuole simulare l'arrivo di un segnale di encoder dai motori in movimento
-//#define ENCODERSIMULATE 
-
-// pi greco
-#define PI           3.14159265358979323846
-
-// dimensione massima del pacchetto ricevuto su seriale
-#define PACKETDIM   8
-
-// 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 7
-
-// 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 7
-
-// intervallo di tempo in [sec], in cui vengono contati gli impulsi di encoder per il calcolo della velocità
-#define DELTAT (0.5)
-
-
-// Parametri moltiplicativi. Queste operazioni vengono fatte una sola volta, evitando di farle ad ogni ciclo
-#define fDistanzaPerStep (PI*DIAMETRORUOTA/IMPULSIPERGIRO)
-
-
-// Ogni Ticker viene calcolata la velocità. Se il ticker viene richiamato ogni DELTAT sec, la velocità potrà essere calcolata come v = spazio/DELTAT
-Ticker SpeedCalculateTicker;
-
-//!!!!!!!!!!!!!!!!!!! INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!
-#ifdef ENCODERSIMULATE
-Ticker EncoderSimulateTicker; // Ticker per simulare un segnale proveniente da encoder sul motore
-#endif
-//!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!    
-
-// 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 // F401
-//Serial myBLE(PG_7, PG_8, 9600); //Tx, Rx, bps // L496
-
-// 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.
-
-// 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_3);  // 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_4);  // 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
-
-PwmOut MotoreCoda (PB_8);  // Output movimento coda
-
-//carattere di comando ricevuto dal BLE e relativo parametro
-volatile char cCommandBLE; // cambia nella routine di interrupt
-volatile char cParamBLE; // cambia nella routine di interrupt
-volatile int nParamBLE; // corrispondente valore numerico di cParamBLE
-
-// memorizza l'ultimo comando ricevuto e relativo parametro. Ci saranno delle azioni solo se il comando ricevuto o il parametro è cambiato rispetto al precedente
-char cOldCommandBLE;
-int nOldParamBLE; 
-
-// coordinate polari del joystick sulla APP, fornite dalla routine di interrupt
-volatile double fTeta;
-volatile double fRo;
-volatile int nRo;
-volatile int nTeta;
-
-// coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt
-volatile double fX, fY;
-// memorizza ultimi valori delle coordinate del Joystick
-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
-volatile double fDistanzaPercorsa; // calcolata nel main, utilizzata nelle IRQ
-
-// velocità calcolata gli impulsi contati in un intervallo DELTAT msec
-volatile double fSpeed; // calcolata nel main, utilizzata nelle IRQ
-
-// Scopi diagnostici:  Ogni fDeltaTick viene simulata la generazione di un impulso di encoder.
-// velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
-double fDeltaTick;
-
- // indice per i cicli
-int nIndex;
-
-// esponente della base 10, per cui bisognerà moltiplicare i caratteri per trasformarli in numeri
-double fEsponente;
-
-// array per la ricezione dei messaggi da BLE
-volatile char caRxPacket[PACKETDIM]; 
-// contatore di caratteri ricevuti daBLE
-volatile int nCharCount; 
-
-// flag che indica se il sw è in Reset
-volatile bool bReset;
-
-// flag che indica se la coda è in movimento/ferma true/false
-volatile bool bCodaInMovimento;
-
-/**************************************************************************************/
-/* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
-/**************************************************************************************/
-void riseEncoderIRQ()
-{
-    // incrementa il contatore di impulsi contati, se il sw non è resettato, cioè se bReset = false
-    //if(!bReset) 
-        nCountRiseEdge++;
-        
-     //pc.printf("Sono qui 0 \n\r"); // diagnostica
-}       
-
-
-/****************************************************************************************/
-/* 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 EncoderSimulate()
-{
-    // ad ogni tick viene simulata la ricezione di un impulso da encoder.
-    // Esempio:
-    //  fDeltaTick = 0.05 sec
-    //  diametro ruota, DIAMETRORUOTA = 0.1 metri
-    //  circonferenza ruota = 0.1*3.14= 0.314 metri
-    //  impulsi per giro dall'encoder, IMPULSIPERGIRO = 4
-    //  un tick simula l'arrivo di un impulso da encoder e quindi simula la percorrenza di 1/4 di circonferenza
-    //  ogni volta che arriva un tick simulato da encoder, si presume di aver percorso circonferenza/4 = 0.314/4 = 0.0785 metri
-    //  il tick arriva ogni fDeltaTick secondi e a ogni tick percorro 0.0785 metri ->  velocità = 0.0785/0.05 = 1.57 [m/s]
-    //  spostamento = (Spazio per ogni tick)/(tempo per ogni tick)
-    //  velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
-    
-    // simula impulso inviato dall'encoder se il sw non è resettato, cioè se bReset = false
-    //if(!bReset)
-        nCountRiseEdge++;
-} 
-
-
-/**********************************************/
-//          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:
-            // Invia Stringa di comando al Robot
-            myBLE.printf("\r\n> Prova di Trasmissione \r\n");
-         }
-    }
-}
-
-//**********************************************/
-//          IRQ associata a Rx da BLE 
-//**********************************************/
-void BLERxInterrupt(void)
-{
-    
-    // carattere ricevuto da BLE
-    char cReadChar; 
-    
-    // indice per l'array di caratteri ricevuti
-    int nCharIndex;
-    
-    // variabile ausiliaria
-    int nAux;
-    
-    // flag che diventa true quando viene rilevata una incongruenza dei valori di Ro e Teta, presumibilmente dovuta a errori di comunicazione
-    bool bIncongruenza;
-
-    while((myBLE.readable()))
-    {
-        // acquisice stringa in input e memorizza in array
-        cReadChar = myBLE.getc(); // Read character
-        caRxPacket[nCharCount]=cReadChar;
-        nCharCount++;
-        pc.printf("%c", cReadChar); // diagnostica
-        
-        // inizializza flag di incongruenza
-        bIncongruenza = false;
-        if(caRxPacket[0]=='(')
-        {
-            cReadChar = myBLE.getc(); // Read character
-            caRxPacket[nCharCount]=cReadChar;
-            nCharCount++;
-            pc.printf("%c", cReadChar); // diagnostica
-            
-            // +++++++++++++++++ INIZIO gestione Comando da Button +++++++++++++++++
-            // Ho ricevuto il comando da un Button se il carattere numero 1, è una lettera maiuscola
-            if((caRxPacket[1] > 0x40) && (caRxPacket[1] < 0x4E)) // caratteri alfabetici
-            {
-                cCommandBLE = caRxPacket[1]; // legge e memorizza il primo carattere
-                nParamBLE = caRxPacket[2]-0x30;
-                // visualizza comando e parametro inviato da BLE
-                pc.printf("> %c%d \r\n\r",cCommandBLE, nParamBLE); // diagnostica
-            }
-            // +++++++++++++++++ FINE gestione Comando da Button +++++++++++++++++
-            
-            if(cReadChar=='X')
-            {
-                nIndex = 0;
-                fX=0;
-                do
-                {
-                    cReadChar = myBLE.getc(); // Read character
-                    caRxPacket[nCharCount]=cReadChar;
-                    nCharCount++;
-                    pc.printf("%c", cReadChar); // diagnostica
-                   
-                    fX = fX+ double(cReadChar - 0x30), nIndex;
-                    nIndex++;
-                }
-                while((cReadChar >= 0x30) && (cReadChar <= 0x39));
-                
-                
-                if(cReadChar=='Y')
-                {
-                    nIndex = 0;
-                    fX=0;
-                    do
-                    {
-                        cReadChar = myBLE.getc(); // Read character
-                        caRxPacket[nCharCount]=cReadChar;
-                        nCharCount++;
-                        pc.printf("%c", cReadChar); // diagnostica
-                        fY = fY+ double(cReadChar - 0x30), nIndex;
-                        nIndex++;
-                    } 
-                    while((cReadChar >= 0x30) && (cReadChar <= 0x39));  
-                }
-                else
-                {
-                    pc.printf("%c", cReadChar); // diagnostica
-                }    
-            }
-        }
-        else
-        {
-            pc.printf("%c", cReadChar); // diagnostica
-        }
-        if(cReadChar==')')
-        {
-            pc.printf("tutto bene. ricevuto > %c%d \r\n\r"); // diagnostica
-        }
-            
-            
-                      
-            
-            
-        //+++++++++++++++++++ INIZIO  converte le coordinate polari del joystick in coordinate cartesiane ++++++++++++++++
-        // se il sw è resettato, restituisci sempre posizioni fX=0, fY=0.
-        if(!bReset)
-        {
-            // Solo in caso di congruenza cambia i valori di fX e fY. In caso di incongruenza, mantieni i valori precedenti
-            if(!bIncongruenza)
-            {
-                //fX = double(nRo)*cos((double)nTeta*((double)PI/180.0));
-                fX = fX-100;
-                //fY = double(nRo)*sin((double)nTeta*((double)PI/180.0));
-                fY = -fY;
-                fY = fY+100;
-            }
-            //pc.printf("> (fX,fY) = (%.2f,%.2f) \n\r\n\r",fX, fY); // diagnostica    
-        }
-        else
-        {
-            // in caso di reset mantieni fermi i motori anche se sulla APP il joystick si sta muovendo
-            fX = 0.0;
-            fY = 0.0;
-        }
-        //+++++++++++++++++++ FINE   converte le coordinate polari del joystick in coordinate cartesiane ++++++++++++++++
-        
-        // reinizializza contatore di caratteri ricevuti 
-        nCharCount = 0;
-            
-        // comunica al cellulare vleocità e spostamento mentre si sta spostando
-        //myBLE.printf(" Speed= %d [m/s]; Trip= d [m]\n\r",nRo, nTeta ); 
-        
-    }
-}
-
-/*********************************************************************************************************************************************/
-/* ogni DELTAT secondi scatta questo ticker.                                                                                                 */
-/* Tra due Tick viene contato il numero di mpulsi impulsi di encoder ricevuti con degli interrupt e contentuo nella variabile nCountRiseEdge */
-/*********************************************************************************************************************************************/
-void SpeedCalculate()
-{
-       
-    // se bReset = true non fare nessun calcolo della velocità e spostamento e azzera velocità e spostamento 
-    if(!bReset)
-    {
-        //pc.printf("Sono qui 1 \n\r"); // diagnostica 
-        //+++++++++++++++++++++++++ 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
-        {
-            //pc.printf("nCountRiseEdge= %d ; nOldCountRiseEdge= %d \n\r", nCountRiseEdge, nOldCountRiseEdge); // diagnostica 
-            
-            // Distanza Persorsa[metri] = ( (circonferenza ruota)/(numero impulsi per giro) ) * (Numero di Impulsi contati)
-            fDistanzaPercorsa = fDistanzaPerStep*nCountRiseEdge; 
-            
-            // calcola la velocità in [m/sec]. DELTAT è in [sec] lo spostamento è in [m]
-            //fSpeed = float((PI*DIAMETRORUOTA/IMPULSIPERGIRO)*(nCountRiseEdge-nOldCountRiseEdge))/DELTAT); 
-            fSpeed = (fDistanzaPerStep*(nCountRiseEdge-nOldCountRiseEdge))/DELTAT; 
-            
-            // ricorda lo spostamento
-            nOldCountRiseEdge = nCountRiseEdge;
-           
-            // comunica al cellulare vleocità e spostamento mentre si sta spostando
-            //PRIMA ERA QUI ma si bloccava myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
-        }
-        else
-        {
-            // se non ci sono variazioni di impulsi, il robot è fermo, la velocità è 0.0
-            fSpeed= 0.0;
-        }
-        
-        //myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); // diagnostica
-    }
-    else
-    {
-        // bReset = true
-        // comunica al cellulare vleocità e spostamento nulli
-        nOldCountRiseEdge=0; // non ci sono variazioni di numero di impulsi
-        nCountRiseEdge=0; // non ci sono variazioni di numero di impulsi
-        fSpeed =0.0;
-        fDistanzaPercorsa = 0.0;
-        myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
-    }    
-    //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
-}
-
-
-
-/**********/
-/*  MAIN  */
-/**********/
-int main()
-{  
-    // inizializza PWM del motore coda
-    MotoreCoda.period_ms(50);  // periodo PWM
-    bCodaInMovimento = false;
-  
-    // 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
-    cParamBLE = 0;  // inizialmente nessun parametro da BLE
-    nParamBLE=0;    // inizialmente nessun parametro da BLE
-    nOldParamBLE=0; // inizialmente nessun parametro da BLE
-    fX = 0;     // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
-    fOldX = 0;  // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
-    fY = 0;     // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
-    fOldY = 0;  // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
-    bReset = false; //bReset = true/false quando riceve un comando (R1)/(R0) dalla APP
-          
-    // inizializza variabili
-    fDistanzaPercorsa = 0.0;       
-    fSpeed = 0.0;
-    
-    // inizializza array di caratteri ricevuti
-    for(nIndex=0; nIndex < PACKETDIM; nIndex++)
-     {caRxPacket[nIndex]=0;}
-    nCharCount=0;
-        
-    
-    // inizializza i valori di modulo e fase ricevuti dal joystick
-    nRo = 0;
-    nTeta = 0;
-       
-    //+++++++++++++++++ INIZIO Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    // conta il numero di impulsi del segnale di encoder che si verificano in DELTAT millisecondi
-    // gli impulsi di encoder vengono contati da una IRQ collegata all'input da encoder
-    // ogni DELTAT secondi scatta un ticker che calcola la velocità
-    
-    // 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;               
-    
-    InEncoderA.enable_irq();
-    SpeedCalculateTicker.attach(&SpeedCalculate,DELTAT);
-    //+++++++++++++++++ FINE Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    
-    // Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
-    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. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!
-    #ifdef ENCODERSIMULATE
-    // attiva il Ticker per simulare il calcolo della velocità. Ogni fDeltaTick viene simulato l'arrivo di un impulso dall'encoder del motore
-    fDeltaTick = 0.05; // velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
-    EncoderSimulateTicker.attach(&EncoderSimulate,fDeltaTick); // Diagnostica
-    #endif
-    //!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!!!!!
-    
-    
-    //++++++++++++++++++++++++++++++ INIZIO Test motore Coda +++++++++++++++++++++++++++++++++++++++++
-    /*
-    //pc.printf("Coda in movimento \n\r"); //diagnostica
-    while(true)
-    {
-        
-        if ( ( fX != 0 ) || (fY != 0))
-        {
-            if(!bCodaInMovimento) // attiva il PWM solo se la coda è ferma
-            {
-                MotoreCoda.write (0.6);
-                bCodaInMovimento = true;
-                pc.printf("Coda in movimento \n\r"); 
-            }
-        }
-        else
-        {
-            // il joystick è in posizione (0,0), ferma la coda e comunica una sola volta che la velocità è 0
-            if(bCodaInMovimento) // spegne il PWM solo se la coda è in movimento
-            {
-                pc.printf("Coda ferma \n\r"); 
-                MotoreCoda.write (0.0);
-                bCodaInMovimento = false;
-                // comunica al cellulare vleocità nulla
-                // Disattiva/Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
-                //NVIC_DisableIRQ(USART1_IRQn);
-                myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r", 100.0, 1000 ); 
-                //NVIC_EnableIRQ(USART1_IRQn);
-            
-            }
-        }   
-        
-    }
-    */
-    //++++++++++++++++++++++++++++++ FINE Test Motore Coda +++++++++++++++++++++++++++++++++++++++++++
-    
-     
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++    
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-        
-    while(true)
-    {
-    
-        //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++
-        
-        if ((fX!=0) || (fY!=0))     //la coda non si muove se il Joystick è nella posizione (0,0) 
-        {
-            // il joystick è in posizione diversa da (0,0), fai muovere la coda
-            if(!bCodaInMovimento) // attiva il PWM solo se la coda è ferma
-            {
-                //pc.printf("Coda in movimento \n\r"); //diagnostica
-                MotoreCoda.write (0.4);
-                bCodaInMovimento = true;
-                
-            }
-        }
-        else
-        {
-            // il joystick è in posizione (0,0), ferma la coda e comunica una sola volta che la velocità è 0
-            if(bCodaInMovimento) // spegne il PWM solo se la coda è in movimento
-            {
-                //pc.printf("Coda ferma \n\r"); //diagnostica
-                MotoreCoda.write (0.0);
-                bCodaInMovimento = false;
-                // comunica al cellulare vleocità nulla
-                // Disattiva/Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
-                //NVIC_DisableIRQ(USART1_IRQn);
-                //myBLE.printf("Speed= 0.0 [m/s]; Trip= %.2f [m]\n\r", fDistanzaPercorsa ); 
-                //NVIC_EnableIRQ(USART1_IRQn);
-            
-            }
-        }   
-        
-        if((cCommandBLE != cOldCommandBLE) || (nParamBLE != nOldParamBLE))
-        {
-            switch (cCommandBLE)
-            { 
-                case 'T': // accendi/spegni LED su scheda
-                {
-                    myLed = nParamBLE;
-                } break;
-                case 'L': // Accendi/spegni illuminazione a LED
-                {   
-                    Light = nParamBLE;
-                } break;
-                case 'R': // Reset odometria e illuminazione
-                {    
-                    if(nParamBLE==1)
-                    {
-                        bReset = true;
-                        nCountRiseEdge = 0;
-                        nOldCountRiseEdge = 0;
-                        Light = 0;
-                        fDistanzaPercorsa = 0.0;
-                        fSpeed = 0.0;
-                    }
-                    else
-                    {
-                        // se nParamBLE = 0, e comunque diverso da 1, bReset=false -> ricomincia a funzionare normalmente
-                        bReset = false;
-                    }  
-                } break;
-                
-                default: break;
-            }
-            //pc.printf("Comando = %c, Parametro = %d \r\n", cCommandBLE, nParamBLE); // diagnostica
-            cOldCommandBLE = cCommandBLE;
-            nOldParamBLE = nParamBLE;
-        }
-        
-        //++++++++++++++++++++++++++++++++++++++++++++ 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 Joystick (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); // diagnostica   
-            //pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); // diagnostica   
-            //pc.printf("> Velocita' Right R = %.2f\r\n", fR); // diagnostica   
-            //pc.printf("> Velocita' Left  L = %.2f\r\n\r\n", fL); // diagnostica   
-            
-            // comunica al cellulare vleocità e spostamento mentre si sta spostando
-            // Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
-            NVIC_DisableIRQ(USART1_IRQn);
-            myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
-            NVIC_EnableIRQ(USART1_IRQn);
-            
-            // 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)
-        } //if( (fX != fOldX) || (fY != fOldY))
-       
-        //++++++++++++++++++++++ FINE Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++++
-    } //while (true) Ciclo principale
-    
-    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    //++++++++++++++ FINE Ciclo Principale  +++++++++++++++++++    
-    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-    
-} // main()
diff -r e74856e7a232 -r 4b1d35d5f4fd MicRobot-Rev085.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MicRobot-Rev085.cpp	Wed Jul 08 19:55:47 2020 +0000
@@ -0,0 +1,648 @@
+// pilotaggio carrello tramite BLE.
+// testato su L476RG e F401RE
+
+#include "mbed.h"
+#include<stdlib.h>
+
+// attivare questa #define quando si vuole simulare l'arrivo di un segnale di encoder dai motori in movimento
+//#define ENCODERSIMULATE 
+
+// pi greco
+#define PI           3.14159265358979323846
+
+// dimensione massima del pacchetto ricevuto su seriale
+#define PACKETDIM   8
+
+// 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 7
+
+// 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 7
+
+// intervallo di tempo in [sec], in cui vengono contati gli impulsi di encoder per il calcolo della velocità
+#define DELTAT (0.5)
+
+
+// Parametri moltiplicativi. Queste operazioni vengono fatte una sola volta, evitando di farle ad ogni ciclo
+#define fDistanzaPerStep (PI*DIAMETRORUOTA/IMPULSIPERGIRO)
+
+
+// Ogni Ticker viene calcolata la velocità. Se il ticker viene richiamato ogni DELTAT sec, la velocità potrà essere calcolata come v = spazio/DELTAT
+Ticker SpeedCalculateTicker;
+
+//!!!!!!!!!!!!!!!!!!! INIZIO COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!
+#ifdef ENCODERSIMULATE
+Ticker EncoderSimulateTicker; // Ticker per simulare un segnale proveniente da encoder sul motore
+#endif
+//!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!    
+
+// 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 // F401
+//Serial myBLE(PG_7, PG_8, 9600); //Tx, Rx, bps // L496
+
+// 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.
+
+// 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_3);  // 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_4);  // 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
+
+PwmOut MotoreCoda (PB_8);  // Output movimento coda
+
+//carattere di comando ricevuto dal BLE e relativo parametro
+volatile char cCommandBLE; // cambia nella routine di interrupt
+volatile char cParamBLE; // cambia nella routine di interrupt
+volatile int nParamBLE; // corrispondente valore numerico di cParamBLE
+
+// memorizza l'ultimo comando ricevuto e relativo parametro. Ci saranno delle azioni solo se il comando ricevuto o il parametro è cambiato rispetto al precedente
+char cOldCommandBLE;
+int nOldParamBLE; 
+
+// coordinate polari del joystick sulla APP, fornite dalla routine di interrupt
+volatile double fTeta;
+volatile double fRo;
+volatile int nRo;
+volatile int nTeta;
+
+// coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt
+volatile double fX, fY;
+// memorizza ultimi valori delle coordinate del Joystick
+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
+volatile double fDistanzaPercorsa; // calcolata nel main, utilizzata nelle IRQ
+
+// velocità calcolata gli impulsi contati in un intervallo DELTAT msec
+volatile double fSpeed; // calcolata nel main, utilizzata nelle IRQ
+
+// Scopi diagnostici:  Ogni fDeltaTick viene simulata la generazione di un impulso di encoder.
+// velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
+double fDeltaTick;
+
+ // indice per i cicli
+int nIndex;
+
+// esponente della base 10, per cui bisognerà moltiplicare i caratteri per trasformarli in numeri
+double fEsponente;
+
+// array per la ricezione dei messaggi da BLE
+volatile char caRxPacket[PACKETDIM]; 
+// contatore di caratteri ricevuti daBLE
+volatile int nCharCount; 
+
+// flag che indica se il sw è in Reset
+volatile bool bReset;
+
+// flag che indica se la coda è in movimento/ferma true/false
+volatile bool bCodaInMovimento;
+
+/**************************************************************************************/
+/* Routine di gestione Interrupt associata al fronte di salita del segnale di encoder */
+/**************************************************************************************/
+void riseEncoderIRQ()
+{
+    // incrementa il contatore di impulsi contati, se il sw non è resettato, cioè se bReset = false
+    //if(!bReset) 
+        nCountRiseEdge++;
+        
+     //pc.printf("Sono qui 0 \n\r"); // diagnostica
+}       
+
+
+/****************************************************************************************/
+/* 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 EncoderSimulate()
+{
+    // ad ogni tick viene simulata la ricezione di un impulso da encoder.
+    // Esempio:
+    //  fDeltaTick = 0.05 sec
+    //  diametro ruota, DIAMETRORUOTA = 0.1 metri
+    //  circonferenza ruota = 0.1*3.14= 0.314 metri
+    //  impulsi per giro dall'encoder, IMPULSIPERGIRO = 4
+    //  un tick simula l'arrivo di un impulso da encoder e quindi simula la percorrenza di 1/4 di circonferenza
+    //  ogni volta che arriva un tick simulato da encoder, si presume di aver percorso circonferenza/4 = 0.314/4 = 0.0785 metri
+    //  il tick arriva ogni fDeltaTick secondi e a ogni tick percorro 0.0785 metri ->  velocità = 0.0785/0.05 = 1.57 [m/s]
+    //  spostamento = (Spazio per ogni tick)/(tempo per ogni tick)
+    //  velocità = ( (DIAMTERO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
+    
+    // simula impulso inviato dall'encoder se il sw non è resettato, cioè se bReset = false
+    //if(!bReset)
+        nCountRiseEdge++;
+} 
+
+
+/**********************************************/
+//          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:
+            // Invia Stringa di comando al Robot
+            myBLE.printf("\r\n> Prova di Trasmissione \r\n");
+         }
+    }
+}
+
+//**********************************************/
+//          IRQ associata a Rx da BLE 
+//**********************************************/
+void BLERxInterrupt(void)
+{
+    
+    // carattere ricevuto da BLE
+    char cReadChar; 
+    
+    while((myBLE.readable()))
+    {
+        // acquisice stringa in input e memorizza in array
+        cReadChar = myBLE.getc(); // Read character
+        //caRxPacket[nCharCount]=cReadChar;
+        //nCharCount++;
+        pc.printf("%c", cReadChar); // diagnostica
+        
+        // acquisisce il carattere di start comando o coordinate da APP
+        if(cReadChar=='(')
+        {
+            // acquisisce il primo carattere di comando o di coordinate
+            cReadChar = myBLE.getc(); // Read character       
+            pc.printf("%c", cReadChar); // diagnostica 
+            
+            // +++++++++++++++++ INIZIO gestione Comando da Button +++++++++++++++++
+            // Ho ricevuto il comando da un Button se il primo carattere è una lettera maiuscola tra A e T
+            if((cReadChar > 0x40) && (cReadChar < 0x55)) // caratteri alfabetici da 'A' a 'T'
+            {
+                // memorizza come comando il carattere appena letto
+                cCommandBLE = cReadChar; 
+                // legge e memorizza come paramentro il successivo carattere
+                cReadChar = myBLE.getc(); // legge parametro
+                pc.printf("%c", cReadChar); // diagnostica 
+                nParamBLE = cReadChar-0x30;
+                cReadChar = myBLE.getc(); // legge la ')' di chiusura comando       
+                pc.printf("%c", cReadChar); // diagnostica 
+
+                // visualizza comando e parametro inviato da BLE
+                pc.printf("> %c%d \r\n\r",cCommandBLE, nParamBLE); // diagnostica
+            }
+            // +++++++++++++++++ FINE gestione Comando da Button +++++++++++++++++
+            else
+            {
+                if(cReadChar=='X') // è stato acquisisto carattere X di inizio valori numeri dell'ascissa?
+                {
+                    //+++++++++ INIZIO acquisisce il valore dell'ascissa Xnnn +++++++++++++++
+                    nCharCount = 0;
+                    do
+                    {
+                        cReadChar = myBLE.getc(); // Read character
+                        caRxPacket[nCharCount]=cReadChar;
+                        nCharCount++;
+                        pc.printf("%c", cReadChar); // diagnostica
+                    }
+                    while((cReadChar >= 0x30) && (cReadChar <= 0x39));
+                    //+++++++++ FINE acquisisce il valore dell'ascissa Xnnn +++++++++++++++
+                    
+                    //+++++++++ INIZIO converte i caratteri in valore numerico dell'ascissa fX +++++++++++++++
+                    nCharCount -= 2; // ultimo carattere dopo i numeri, mi aspetto sia 'Y'
+                    fX=0;
+                    for(nIndex =0; nIndex <= nCharCount; nIndex++)
+                    {
+                        fX = fX + (caRxPacket[nIndex]-0x30)*pow(10.0, (nCharCount - nIndex));
+                    }
+                    //+++++++++ FINE converte i caratteri in valore numerico dell'ascissa fX +++++++++++++++
+                    
+                    // verifica se l'ultimo carattere ricevuto dopo Xnnn è stato Y
+                    if(cReadChar=='Y')
+                    {
+                        //+++++++++ INIZIO acquisisce il valore dell'ordinata Ynnn +++++++++++++++
+                        nCharCount = 0;
+                        do
+                        {
+                            cReadChar = myBLE.getc(); // Read character
+                            caRxPacket[nCharCount]=cReadChar;
+                            nCharCount++;
+                            pc.printf("%c", cReadChar); // diagnostica
+                        } 
+                        while((cReadChar >= 0x30) && (cReadChar <= 0x39));  
+                        //+++++++++ FINE acquisisce il valore dell'ordinata Ynnn +++++++++++++++
+    
+                        //+++++++++ INIZIO converte i caratteri in valore numerico dell'ordinata fY +++++++++++++++
+                        nCharCount -= 2; // ultimo carattere dopo i numeri, mi aspetto sia 'Y'
+                        fY=0;
+                        for(nIndex =0; nIndex <= nCharCount; nIndex++)
+                        {
+                            fY = fY + (caRxPacket[nIndex]-0x30)*pow(10.0, (nCharCount - nIndex));
+                        }
+                        //+++++++++ FINE converte i caratteri in valore numerico dell'ordinata fY +++++++++++++++
+                        
+                        // se riceve la coda del comando ), stampa le coordinate ricevute
+                        if(cReadChar==')') 
+                        {
+                            pc.printf("(fX , fY) = (%.1f , %.1f) \r\n", fX, fY); // diagnostica
+                            
+                            // trasporta x e y nei range desiderati
+                            fY= 100 - fY;
+                            fX = fX - 100;
+                            
+                            pc.printf("\n\r(fX , fY) traslate = (%.1f , %.1f) \r\n\n", fX, fY); // diagnostica
+                        }
+                        else // dopo la Y e i numeri, mi attendo parentesi chiusa )
+                        {
+                            pc.printf("> Errore = %c invece di ) \r\n\n", cReadChar); // diagnostica
+                        }
+                    }
+                    else // dopo la X e i numeri mi aspetto Y
+                    {
+                        pc.printf("> Errore = %c invece di Y \r\n\n", cReadChar); // diagnostica
+                    }
+                }
+                else // dopo la ( mi aspetto Y
+                {
+                    pc.printf("> Errore = %c invece di X \r\n\n", cReadChar); // diagnostica
+                }
+            } // if(comandi alfanumerici) 
+        } // if(cReadChar == '(')
+    }
+}
+
+/*********************************************************************************************************************************************/
+/* ogni DELTAT secondi scatta questo ticker.                                                                                                 */
+/* Tra due Tick viene contato il numero di mpulsi impulsi di encoder ricevuti con degli interrupt e contentuo nella variabile nCountRiseEdge */
+/*********************************************************************************************************************************************/
+void SpeedCalculate()
+{
+       
+    // se bReset = true non fare nessun calcolo della velocità e spostamento e azzera velocità e spostamento 
+    if(!bReset)
+    {
+        //pc.printf("Sono qui 1 \n\r"); // diagnostica 
+        //+++++++++++++++++++++++++ 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
+        {
+            //pc.printf("nCountRiseEdge= %d ; nOldCountRiseEdge= %d \n\r", nCountRiseEdge, nOldCountRiseEdge); // diagnostica 
+            
+            // Distanza Persorsa[metri] = ( (circonferenza ruota)/(numero impulsi per giro) ) * (Numero di Impulsi contati)
+            fDistanzaPercorsa = fDistanzaPerStep*nCountRiseEdge; 
+            
+            // calcola la velocità in [m/sec]. DELTAT è in [sec] lo spostamento è in [m]
+            //fSpeed = float((PI*DIAMETRORUOTA/IMPULSIPERGIRO)*(nCountRiseEdge-nOldCountRiseEdge))/DELTAT); 
+            fSpeed = (fDistanzaPerStep*(nCountRiseEdge-nOldCountRiseEdge))/DELTAT; 
+            
+            // ricorda lo spostamento
+            nOldCountRiseEdge = nCountRiseEdge;
+           
+            // comunica al cellulare vleocità e spostamento mentre si sta spostando
+            //PRIMA ERA QUI ma si bloccava myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
+        }
+        else
+        {
+            // se non ci sono variazioni di impulsi, il robot è fermo, la velocità è 0.0
+            fSpeed= 0.0;
+        }
+        
+        //myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); // diagnostica
+    }
+    else
+    {
+        // bReset = true
+        // comunica al cellulare vleocità e spostamento nulli
+        nOldCountRiseEdge=0; // non ci sono variazioni di numero di impulsi
+        nCountRiseEdge=0; // non ci sono variazioni di numero di impulsi
+        fSpeed =0.0;
+        fDistanzaPercorsa = 0.0;
+        myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
+    }    
+    //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
+}
+
+
+
+/**********/
+/*  MAIN  */
+/**********/
+int main()
+{  
+    // inizializza PWM del motore coda
+    MotoreCoda.period_ms(50);  // periodo PWM
+    bCodaInMovimento = false;
+  
+    // 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
+    cParamBLE = 0;  // inizialmente nessun parametro da BLE
+    nParamBLE=0;    // inizialmente nessun parametro da BLE
+    nOldParamBLE=0; // inizialmente nessun parametro da BLE
+    fX = 0;     // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
+    fOldX = 0;  // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
+    fY = 0;     // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
+    fOldY = 0;  // Joystick inizialmente nell'origine (X , Y) = (0 , 0)
+    bReset = false; //bReset = true/false quando riceve un comando (R1)/(R0) dalla APP
+          
+    // inizializza variabili
+    fDistanzaPercorsa = 0.0;       
+    fSpeed = 0.0;
+    
+    // inizializza array di caratteri ricevuti
+    for(nIndex=0; nIndex < PACKETDIM; nIndex++)
+     {caRxPacket[nIndex]=0;}
+    nCharCount=0;
+        
+    
+    // inizializza i valori di modulo e fase ricevuti dal joystick
+    nRo = 0;
+    nTeta = 0;
+       
+    //+++++++++++++++++ INIZIO Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    // conta il numero di impulsi del segnale di encoder che si verificano in DELTAT millisecondi
+    // gli impulsi di encoder vengono contati da una IRQ collegata all'input da encoder
+    // ogni DELTAT secondi scatta un ticker che calcola la velocità
+    
+    // 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;               
+    
+    InEncoderA.enable_irq();
+    SpeedCalculateTicker.attach(&SpeedCalculate,DELTAT);
+    //+++++++++++++++++ FINE Attivazione Interrupt per segnale di Encoder +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    
+    // Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
+    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. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!
+    #ifdef ENCODERSIMULATE
+    // attiva il Ticker per simulare il calcolo della velocità. Ogni fDeltaTick viene simulato l'arrivo di un impulso dall'encoder del motore
+    fDeltaTick = 0.05; // velocità = ( (DIAMETRO*PI) / IMPULSIPERGIRO )/ fDeltaTick [m/s]
+    EncoderSimulateTicker.attach(&EncoderSimulate,fDeltaTick); // Diagnostica
+    #endif
+    //!!!!!!!!!!!!!!!!!!! FINE COMMENTARE QUESTA FUNZIONE DURANTE IL NORMALE FUNZIONAMENTO CON ROBOT IN MOVIMENTO. UTILIZZO PER DIAGNOSTICA !!!!!!!!!!!!!!!!!!!!
+    
+    
+    //++++++++++++++++++++++++++++++ INIZIO Test motore Coda +++++++++++++++++++++++++++++++++++++++++
+    /*
+    //pc.printf("Coda in movimento \n\r"); //diagnostica
+    while(true)
+    {
+        
+        if ( ( fX != 0 ) || (fY != 0))
+        {
+            if(!bCodaInMovimento) // attiva il PWM solo se la coda è ferma
+            {
+                MotoreCoda.write (0.6);
+                bCodaInMovimento = true;
+                pc.printf("Coda in movimento \n\r"); 
+            }
+        }
+        else
+        {
+            // il joystick è in posizione (0,0), ferma la coda e comunica una sola volta che la velocità è 0
+            if(bCodaInMovimento) // spegne il PWM solo se la coda è in movimento
+            {
+                pc.printf("Coda ferma \n\r"); 
+                MotoreCoda.write (0.0);
+                bCodaInMovimento = false;
+                // comunica al cellulare vleocità nulla
+                // Disattiva/Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
+                //NVIC_DisableIRQ(USART1_IRQn);
+                myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r", 100.0, 1000 ); 
+                //NVIC_EnableIRQ(USART1_IRQn);
+            
+            }
+        }   
+        
+    }
+    */
+    //++++++++++++++++++++++++++++++ FINE Test Motore Coda +++++++++++++++++++++++++++++++++++++++++++
+    
+     
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++    
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+        
+    while(true)
+    {
+    
+        //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++
+        
+        if ((fX!=0) || (fY!=0))     //la coda non si muove se il Joystick è nella posizione (0,0) 
+        {
+            // il joystick è in posizione diversa da (0,0), fai muovere la coda
+            if(!bCodaInMovimento) // attiva il PWM solo se la coda è ferma
+            {
+                //pc.printf("Coda in movimento \n\r"); //diagnostica
+                MotoreCoda.write (0.4);
+                bCodaInMovimento = true;
+                
+            }
+        }
+        else
+        {
+            // il joystick è in posizione (0,0), ferma la coda e comunica una sola volta che la velocità è 0
+            if(bCodaInMovimento) // spegne il PWM solo se la coda è in movimento
+            {
+                //pc.printf("Coda ferma \n\r"); //diagnostica
+                MotoreCoda.write (0.0);
+                bCodaInMovimento = false;
+                // comunica al cellulare vleocità nulla
+                // Disattiva/Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
+                //NVIC_DisableIRQ(USART1_IRQn);
+                //myBLE.printf("Speed= 0.0 [m/s]; Trip= %.2f [m]\n\r", fDistanzaPercorsa ); 
+                //NVIC_EnableIRQ(USART1_IRQn);
+            
+            }
+        }   
+        
+        if((cCommandBLE != cOldCommandBLE) || (nParamBLE != nOldParamBLE))
+        {
+            switch (cCommandBLE)
+            { 
+                case 'T': // accendi/spegni LED su scheda
+                {
+                    myLed = nParamBLE;
+                } break;
+                case 'L': // Accendi/spegni illuminazione a LED
+                {   
+                    Light = nParamBLE;
+                } break;
+                case 'R': // Reset odometria e illuminazione
+                {    
+                    if(nParamBLE==1)
+                    {
+                        bReset = true;
+                        nCountRiseEdge = 0;
+                        nOldCountRiseEdge = 0;
+                        Light = 0;
+                        fDistanzaPercorsa = 0.0;
+                        fSpeed = 0.0;
+                    }
+                    else
+                    {
+                        // se nParamBLE = 0, e comunque diverso da 1, bReset=false -> ricomincia a funzionare normalmente
+                        bReset = false;
+                    }  
+                } break;
+                
+                default: break;
+            }
+            //pc.printf("Comando = %c, Parametro = %d \r\n", cCommandBLE, nParamBLE); // diagnostica
+            cOldCommandBLE = cCommandBLE;
+            nOldParamBLE = nParamBLE;
+        }
+        
+        //++++++++++++++++++++++++++++++++++++++++++++ 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 Joystick (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); // diagnostica   
+            //pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW); // diagnostica   
+            //pc.printf("> Velocita' Right R = %.2f\r\n", fR); // diagnostica   
+            //pc.printf("> Velocita' Left  L = %.2f\r\n\r\n", fL); // diagnostica   
+            
+            // comunica al cellulare vleocità e spostamento mentre si sta spostando
+            // Attiva la IRQ per la RX su seriale e sulla Rx della BLE  
+            NVIC_DisableIRQ(USART1_IRQn);
+            myBLE.printf("Speed= %.2f [m/s]; Trip= %.2f [m]\n\r",fSpeed, fDistanzaPercorsa ); 
+            NVIC_EnableIRQ(USART1_IRQn);
+            
+            // 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)
+        } //if( (fX != fOldX) || (fY != fOldY))
+       
+        //++++++++++++++++++++++ FINE Ottieni X e Y dal Joystick e trasformali in comandi per il motore Right e Left +++++++++++++++++++++++++++++
+    } //while (true) Ciclo principale
+    
+    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    //++++++++++++++ FINE Ciclo Principale  +++++++++++++++++++    
+    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+    
+} // main()
diff -r e74856e7a232 -r 4b1d35d5f4fd mbed.bld
--- a/mbed.bld	Wed Jul 08 10:14:02 2020 +0000
+++ b/mbed.bld	Wed Jul 08 19:55:47 2020 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file