File content as of revision 23:ef6be2f2fa42:

// 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.05)

// numero di impulsi per giro generati dall'encoder
#define IMPULSIPERGIRO 3

// 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_7);  // Output 1 per pilotaggio input BI1 del Motore B Posteriore
PwmOut PostOutPWB (PB_5);  // 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_8);  // 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_6);  // Output per pilotaggio input PWM del motore B Anteriore
//DigitalOut AntOutPWB (PB_3);  // 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
DigitalOut WatchDog (PB_0); // output WatchDog che deve oscillare con un periodo t < 1.5 s
//Yes+++PwmOut MotoreCoda (PA_1);  // Output movimento coda
PwmOut MotoreCoda (PA_1);  // Output movimento coda

DigitalInOut SwitchRouter (PB_9, PIN_OUTPUT, OpenDrain, 0); //Uscita opendrain No Pull


//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("*W\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 1 invece di ) \r\n\n"); // diagnostica
                        }
                    }
                    else // dopo la X e i numeri mi aspetto Y
                    {
                        pc.printf("> Errore 2 invece di Y \r\n\n"); // diagnostica
                    }
                }
                else // dopo la ( mi aspetto Y
                {
                    pc.printf("> Errore 3 invece di X \r\n\n"); // 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/100; 
            
            // 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)/100; 
            
            // 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("*WSpeed= %.2f [m/s]; Trip= %.2f [m]\n\r*",fSpeed, fDistanzaPercorsa ); //*W=carattere di riconoscimento per comunicazione con APP (inviare messaggio)
    }   
    // stimola il watchdog esterno facendo oscillare la linea WatchDog
    if(WatchDog == 0)
    {
        WatchDog = 1;
       // pc.printf("WatchDog = 1\n\r"); 
    }
    else
    {
        WatchDog = 0;
    //    pc.printf("WatchDog = 0\n\r");
    }
    //++++++++++++++++++++++++++ FINE Calcola spostamento odometrico e velocità +++++++++++++++++++++++++++++++++++++++++++++
}



/**********/
/*  MAIN  */
/**********/
int main()
{  
  
    // messaggio di benvenuto
    pc.printf("\r\n************  Hallo ****************** \r\n");
    pc.printf("*** Modulo di Ispezione Condutture ***\r\n");
   // myBLE.printf("*W \r\nHallo\r\n*");
    //myBLE.printf("*W Modulo di Ispezione Condutture \r\n*");
    
    // inizializza PWM del motore coda
    MotoreCoda.period_ms(50);  // periodo PWM
    bCodaInMovimento = false;
    MotoreCoda.write (0.0);
    // inizializza Opendrain
   
    SwitchRouter.mode (PullNone);
     
    // 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
    WatchDog = 0; // inizialmente watchdog = 0; oscillerà per non ricevere reset esterno
          
    // 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 Raw Test Motore ++++++++++++
  /*
    while(1)
    {
        //if(myButton == 0)
        {
            //Yes+++MotoreCoda.write (1);
           // pc.printf("*** Run TAIL ***\r\n");
            //wait (1);
            //Yes+++MotoreCoda.write (0.0);  
            //pc.printf("*** Stop RAIL ***\r\n");  
            //wait (1); 
            pc.printf("*** Run TAIL ***\r\n");
           // MotoreCoda.write (1.0);
            
            // CW
            PostOutPWB = 1;
            PostOutBI1 = 1;
            PostOutBI2 = 0;    
            AntOutPWB = 1;
            AntOutBI1 = 0;
            AntOutBI2 = 1;    
            pc.printf("Run CW\r\n\r\n");
            //Yes+++MotoreCoda.write (1);
            
            wait (3);
            
            
            // spegni 
            PostOutPWB = 1;
            PostOutBI1 = 0;
            PostOutBI2 = 0; 
            AntOutPWB = 1;
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
            pc.printf("Stop CW\r\n\r\n");
            wait (3);
            
    
            // CCW
            PostOutPWB = 1;
            PostOutBI1 = 0;
            PostOutBI2 = 1;
            AntOutPWB = 1;
            AntOutBI1 = 1;
            AntOutBI2 = 0;
            pc.printf("Run CCW\r\n\r\n");
            wait (3);
            
            // spegni 
            PostOutPWB = 1;
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            AntOutPWB = 1;
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
            //Yes+++MotoreCoda.write (0);
            //pc.printf("*** Stop TAIL ***\r\n");
            pc.printf("Stop CCW\r\n\r\n");
            wait (3);
            
            
        }
    } // while(true)  Raw test motore
   */
    //++++++++++++++++++++++++++++++ FINE Test Motore Raw ++++++++++++++++++++++++++++++++++++++++
    //++++++++++++++++++++++++++++++ INIZIO Test WatchDog +++++++++++++++++++++++++++++++++++++++++
    /* while(true)
    {
        if(WatchDog == 0)
        {
            WatchDog = 1;
            pc.printf("WatchDog = 1\n\r"); 
        }
        else
        {
            WatchDog = 0;
            pc.printf("WatchDog = 0\n\r");
        }
               
        wait_ms(100);
        
    }
    */
    //++++++++++++++++++++++++++++++ FINE Test WatchDog +++++++++++++++++++++++++++++++++++++++++
    
    //++++++++++++++++++++++++++++++ INIZIO Test motore Coda +++++++++++++++++++++++++++++++++++++++++
    //pc.printf("Coda in movimento \n\r"); //diagnostica
    /*
    while(1)
    {      
        MotoreCoda.write (1.0);
        pc.printf("*** Run TAIL ***\r\n");
        wait (2);
        MotoreCoda.write (0.0);  
        pc.printf("*** Stop RAIL ***\r\n");  
        wait (2); 
    }
    */
    //++++++++++++++++++++++++++++++ FINE Test Motore Coda +++++++++++++++++++++++++++++++++++++++++++
    //++++++++++++ INIZIO Test Segnali ++++++++++++
    /*
    while(1)
    {
        //if(myButton == 0)
        {
            // CW
            //PostOutPWB = 1;
            PostOutBI1 = 1;
            PostOutBI2 = 0;    
            pc.printf("Run CW\r\n\r\n");
            wait (3);
            
            
            // spegni 
            //PostOutPWB = 1;
            PostOutBI1 = 0;
            PostOutBI2 = 0; 
            pc.printf("Stop CW\r\n\r\n");
            wait (3);
            

            // CCW
            //PostOutPWB = 1;
            PostOutBI1 = 0;
            PostOutBI2 = 1;
            pc.printf("Run CCW\r\n\r\n");
            wait (3);
            
            // spegni 
            PostOutPWB = 1;
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            pc.printf("Run CCW\r\n\r\n");
            wait (1);
        }
    } // while(true)  Raw test motore
    */
    //++++++++++++++++ FINE TEST Segnali +++++++++++++++++++++++++++++++
    
    
    
     
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++    
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
       
    while(true)
    {
        //+++++++++++++++++++++++++++++++++ INIZIO muovi coda ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
        
         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);
                
                }
            }  
        
        //+++++++++++++++++++++++++++++++++++++++ FINE muovi coda +++++++++++++++++++++++++++++++++++++++++
        //++++++++++++++++++++++++++ INIZIO Interpreta Comandi da Pulsanti della APP ++++++++++++++++++++++++++++++++++++++++++++        
        
        if((cCommandBLE != cOldCommandBLE) || (nParamBLE != nOldParamBLE))
        {
            switch (cCommandBLE)
            { 
                case 'T': // accendi/spegni LED su scheda
                {
                    myLed = nParamBLE;
                } break;
                 case 'C': // accendi/spegni LED su scheda
                {
                     if (nParamBLE == 1)
                    {
                      SwitchRouter = 1;
                     // pc.printf("acceso\n\r");
                    }
                    else
                    {
                      SwitchRouter = 0;
                     // pc.printf("spento\n\r");
                    }
                } 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)
            fL = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
            fR = (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   
            
            //+++++ INIZIO comunica al cellulare velocità 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("*WSpeed= %.2f [m/s]; Trip= %.2f [m]\n\r*",fSpeed, fDistanzaPercorsa ); 
            NVIC_EnableIRQ(USART1_IRQn);
            //+++++ FINE comunica al cellulare velocità e spostamento mentre si sta spostando ++++++
            
            //++++++++++++++++++ INIZIO algoritmo di movimentazione delle ruote ++++++++++++++++
            if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
            {
                fR =-fR;
                // Vai indietro
                PostOutBI1 = 1;
                PostOutBI2 = 0; 
                pc.printf("Right Backward. fR= %.2f\n\r", fR);  
            }
            else
            {
                if(fR >0)
                {
                    // Vai avanti
                    PostOutBI1 = 0;
                    PostOutBI2 = 1; 
                    pc.printf("Right Forward. fR= %.2f\n\r", fR);  
                }
                else
                {
                    // spegni 
                    PostOutBI1 = 0;
                    PostOutBI2 = 0;
                    pc.printf("Right Stopped. fR= %.2f\n\r", fR);  
                }
            }
            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; 
                pc.printf("Left Backward. fL= %.2f\n\r", fL);  
            }
            else
            {
                if(fL >0)
                {
                    // Vai avanti
                    AntOutBI1 = 0;
                    AntOutBI2 = 1; 
                    pc.printf("Left Forward. fL= %.2f\n\r", fL);  
                    
                }
                else
                {
                    // spegni 
                    AntOutBI1 = 0;
                    AntOutBI2 = 0;
                    pc.printf("Left Stopped. fL= %.2f\n\r", fL);  
                }
            }
            AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
            //++++++++++++++++++ FINE algoritmo di movimentazione delle ruote ++++++++++++++++
            
        } //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()