File content as of revision 6:92b0e17593e0:

// bpilotaggio carrello tramite BLE.
// testato su L476RG

#include "mbed.h"
#include<stdlib.h>

// pi greco
#define PI           3.14159265358979323846

// 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, 115200); //Tx, Rx, bps

// Input di Reset per il Modulo BLE ELETT114A.
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

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 


// carattere letto dalla seriale del PC
char cReadChar;

// DutyCycle del segnale PWM
float fDutyCycle;


// 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

//DigitalInOut OutBlades (PB_9, PIN_OUTPUT, OpenDrain, 0); // Output per il pilotaggio del Relay di azionamento Lame Rotanti


DigitalOut AntOutBI1 (PB_4);  // Output 1 per pilotaggio input BI1 del Motore B Anteriore
PwmOut AntOutPWB (PB_5);  // Output per pilotaggio input PWM del motore B Anteriore
//DigitalOut AntOutPWB (PB_5);  // Scopi diagnostici: Output Digitalte per pilotaggio PWM del motore B Anteriore
DigitalOut AntOutBI2 (PB_3);  // Output 2 per pilotaggio input BI2 del Motore B Posteriore
DigitalIn AntInNE1 (PB_10); // Input per acquisire i segnali NET1 in output dall'encoder Anteriore



/*
//+++DigitalIn PostInNE1 (BHO); // Input per acquisire i segnali NET1 in output dall'encoder Posteriore
DigitalOut PostOutBI2 (PG_15);  // Output 2 per pilotaggio input BI2 del Motore B Posteriore
DigitalOut PostOutBI1 (PA_15);  // Output 1 per pilotaggio input BI1 del Motore B Posteriore
//PwmOut PostOutPWB (PH_13);  // Output per pilotaggio input PWM del motore B Posteriore
DigitalOut PostOutPWB (PH_13);  // per diagnostica del motore imponi PostOutPWB = 1 per muovere il motore, PostOutPWB = 0 per fermare il motore

DigitalOut AntOutBI2 (PB_5);  // Output 2 per pilotaggio input BI2 del Motore B Posteriore
DigitalOut AntOutBI1 (PA_5);  // Output 1 per pilotaggio input BI1 del Motore B Posteriore
//PwmOut AntOutPWB (PB_4);  // Output per pilotaggio input PWM del motore B Posteriore
DigitalOut AntOutPWB (PB_4);  // per diagnostica del motore imponi PostOutPWB = 1 per muovere il motore, PostOutPWB = 0 per fermare il motore
*/


//carattere di comando ricevuto dal BLE
volatile char cCommandBLE; // cambia nella routine di interrupt
// memorizza l'ultimo comando ricevuto. Ci saranno delle azioni solo se il comando ricevuto è cambiato rispetto al precedente
char cOldCommandBLE;

// coordinate polari del joistick sulla APP, fornite dalla routine di interrupt
volatile double fTeta;
volatile double fRo;

// coordinate cartesiane della posizione joystick sulla APP, fornite dalla routine di Interrupt
volatile double fX, fY;
// memorizza ultimi valori delle coordinate del joistick
double fOldX, fOldY;

// variabili ausiliarie per l'algoritmo di posizionamento
double fV, fW;

// velocità della ruota sinistra e della ruota destra. La Sinistra coincide con la ruota Anteriore, la destra con la Posteriore
double fR, fL;






//**********************************************/
//          IRQ associata a Rx da BLE 
//**********************************************/
void BLERxInterrupt(void)
{
    // array per la ricezione dei messaggi da seriale
    char cReadChar;
    
    //indice per i cicli
    int nIndex;
    
    // array per la ricezione dei messaggi da seriale
    char caRxPacket[8];
    //int nRxPacketSize;
    
    // coordinate cartesiane della posizione joystick
    //float fX, fY;
    // coordinate polari della posizione joistick
    //float fTeta;
    //float fRo;
    
    
   
    //pc.printf("BLE RxInterrupt: \n\r");
    
    // ricevi caratteri su seriale, se disponibili   
    while((myBLE.readable()))
    {
        // acquisice stringa in input e relativa dimensione
        cReadChar = myBLE.getc(); // Read character
        if(cReadChar == 0x02)
        {
            pc.printf(">Ricevuto\r\n "); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
                 
            //-- command will be 8 bytes for joystick values
            //-- command will be 3 bytes for button change event
            //-- all valid command packets begin with <STX> (0x02) and end with <ETX> (0x03)


            caRxPacket[0] = cReadChar; // legge e memorizza il primo carattere STX
            cReadChar = myBLE.getc(); // legge il secondo carattere
            if(cReadChar > 0x40)
            {
                // Button:
                //-- Button events send a single character in a 3-byte packet
                //-- B1 uses "A" for changed to on, "B" for changed to off -- command packet when B1 is click on is <STX> <"A"> <ETX>
                //-- B2 uses "C" for changed to on, "D" for changed to off
                //-- B3..B6 follow in order; valid button even characters are "A".."L"
                caRxPacket[1] = cReadChar; // memorizza il secondo carattere. Contiene il Comando dal Button della APP
                caRxPacket[2] = myBLE.getc();  // legge e memorizza il terzo carattere ETX
                // passa il comando ricevuto nella variabile globale
                cCommandBLE = caRxPacket[1];
                
                // Diagnostica
                /*
                pc.printf(">: 0x%02x, 0x%02x, 0x%02x\n\r\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2]); // visualizza comando inviato da BLE tramite pressione dei Button B1-B6
                */
               
            }    
            else
            {
                // Joystick:
                //-- The JBTC joystick values range from -100 to 100; these are values are transmitted in ASCII format after an offset of 200 is added to each axis
                //-- offset is added so that values can be sent as three ASCII chars: hundreds digit, tens digit, ones digit without sign indicator
                //-- With the joystick at 0, 0 the command packet is: <STX> <"2"> <"0"> <"0"> <"2"> <"0"> <"0"> <ETX>
                caRxPacket[1] = cReadChar; // memorizza il secondo carattere
                for(nIndex=2; nIndex<8; nIndex++)
                {
                    caRxPacket[nIndex] = myBLE.getc();
                }
                // dal messaggio estrae e visualizza le coordinate cartesiane
                fX = (((caRxPacket[1]-0x30)*100+(caRxPacket[2]-0x30)*10+(caRxPacket[3]-0x30))-200);
                fY = (((caRxPacket[4]-0x30)*100+(caRxPacket[5]-0x30)*10+(caRxPacket[6]-0x30))-200);
                // converte la posizione del joistick in coordinate polari
                fTeta=atan2(fY,fX)*(180.0/PI); // angolo in gradi nel terzo e quarto quadrante diventa negativo
                if(fTeta < 0) fTeta = fTeta+360.0; // angolo tra 0 e 360°
                fRo=sqrt(pow(fX,2)+pow(fY,2)); //*(10000.0/(sqrt(2)); // modulo del vettore polare. Valore Max =100
                
                // diagnostica
                /*
                pc.printf(">: 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x, 0x%02x\n\r",caRxPacket[0], caRxPacket[1], caRxPacket[2], caRxPacket[3], caRxPacket[4], caRxPacket[5], caRxPacket[6], caRxPacket[7]); // visualizza comando da BLE
                pc.printf(">: fX = %f; fY =%f\n\r", fX, fY); //// visualizza posizione joystick in coordinate cartesiane
                pc.printf(">: fTeta = %.2f;  fRo= %.2f\n\r\n\r", fTeta, fRo); // // visualizza posizione joystick in coordinate polari
                */
             }
        }
    }
    
}

/**********************************************/
//          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); // write char to BLE
        
        //pc.printf("W>: 0x%02x\n\r",cReadChar);
        if(cReadChar == '0') // se scrivo '0', invia questa stringa
        {
            //If sending a response, the packet will contain four strings with additional separators: <STX> <buttons> <$01> <Data1> <$04> <Data2> <$05> <Data3> <ETX>
            //-- button status is binary formatted string (no indicator)
            //-- data fields sent as strings
            //-- send empty string to unused field (not sure if short response packet is allowed without additional testing)
            // Struttura Nominale del comando da inviare al robot                   STX , B6, B5, B4, B3  B2  B1 0x01, Data1,....................,0x04,Data2,..............,0x05,Data3,...................,ETX;
            myBLE.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0x02,'0','0','1','1','1','0',0x01,'9','8','7','6',',','0','2',0x04,'-','5','4', ',','9',0x05,0x35,0x34,0x33,0x32,0x31, 0x03);
            // diagnostica
            /*
            pc.printf("W>: Inviato comando a BLE\n\r"); 
            */
        }
    }
}


/**********/
/*  MAIN  */
/**********/

int main()
{
   
    // messaggio di benvenuto
    pc.printf("\r\n*******  Hallo - Exercise ********** \r\n");
    pc.printf("\r\n*** L476 BLE MegaPi Motor Driver  ***\r\n");
    
    //Inizializza pin di output per Rotating Blades
  /*  
    OutBlades.mode(OpenDrain);     
    OutBlades.output();
    OutBlades.write(0);
    nMoveBladesCommand = 0; // il comando di movimento Blades inizialmente '0' = spente
    */
    // inizializza variabili
    cCommandBLE = 0; // inizialmente nessun comando da BLE
    cOldCommandBLE = 0; // inizialmente nessun comando da BLE
    fX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
    fOldX = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
    fY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
    fOldY = 0; // joistick inizialmente nell'origine (X , Y) = (0 , 0)
        
    // inizializza il BLE
    BleRst = 0;
    wait_ms(100);
    BleRst = 1;
    cCommandBLE = '0';
    cOldCommandBLE = '0';
      
    
    
    // inizializza i PWM di pilotaggio dei  motori Posteriore e Anteriore
    PostOutPWB.period_us(10); // periodo del PWM Posteriore
    PostOutPWB.write(0.0); // DutyCycle del PWM Destro (Posteriore)
    AntOutPWB.period_us(10); // periodo del PWM Anteriore
    AntOutPWB.write(0.0); // DutyCycle del PWM Sinistro (Anteriore)
           
          
    // Attiva la IRQ per la RX su seriale   
    myBLE.attach(&BLERxInterrupt,Serial::RxIrq); // // entra in questa routine quando riceve un carattere dalla seriale del BLE
    pc.attach(&pcRxInterrupt,Serial::RxIrq); // entra in questa routine quando riceve un carattere dalla USB del PC
    
    //++++++++++++++++++++++++++++++++++++++++++++++++++++
    //++++++++++++++ INIZIO Ciclo Principale +++++++++++++    
    //++++++++++++++++++++++++++++++++++++++++++++++++++++
   
    while(true)
    {
        
        if (InDiag == 1)
           myLed = 1 ;
        else
           myLed = 0 ;
          
        //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.
        if(cCommandBLE != cOldCommandBLE)
        {
            
            switch (cCommandBLE)
            { 
                case 'A':
                {
                    myLed = 1;
                }; break;
                case 'B':
                {    
                    myLed = 0;
                }; break;
                case 'C':
                {
                    Light = 1;
                }; break;
                case 'D':
                {    
                    Light = 0;
                }; break;
                default: break;
            }
            // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica
            cOldCommandBLE = cCommandBLE;
        }
        // se ci sono stati cambiamenti nella posizione del joistick, cambia i comandi di velocità delle ruote
        if( (fX != fOldX) || (fY != fOldY))
        {
            fOldX = fX;
            fOldY = fY;
            // algoritmo di conversione dalla posizione del joistick (fX, fY) alla velocità delle ruote (fR, fL)
            fV = (100.0 - fabs(fX)) * (fY/100.0) + fY; // calcolo intermedio
            fW = (100.0 - fabs(fY)) * (fX/100.0) + fX; // calcolo intermedio
            fR = (fV+fW)/2.0; // velocità della ruota destra (-100; +100)
            fL = (fV-fW)/2.0; // velocità della ruota sinistra (-100; +100)
         
            // diagnostica   
            pc.printf("\r\n> (X,Y) = (%.2f , %.2f) \r\n", fX,fY);
            pc.printf("> V , W = %.2f , %.2f\r\n", fV, fW);
            pc.printf("> Velocita' Right R = %.2f\r\n", fR);
            pc.printf("> Velocita' Left  L = %.2f\r\n\r\n", fL);
            
            // algoritmo di movimentazione delle ruote.
            if(fR < 0) //Ruota destra motorizzata coincide con quella posteriore
            {
                fR =-fR;
                // Vai indietro
                PostOutBI1 = 1;
                PostOutBI2 = 0; 
            }
            else
            {
             
                if(fR >0)
                {
                    // Vai avanti
                    PostOutBI1 = 0;
                    PostOutBI2 = 1; 
                }
                else
                {
                    // spegni 
                    PostOutBI1 = 0;
                    PostOutBI2 = 0;
                }
            }
            PostOutPWB.write(float(fR/100.0)); // DutyCycle del PWM Destro (Posteriore)
            if(fL < 0) //Ruota sinistra motorizzata coincide con quella Anteriore
            {
                fL =-fL;
                // Vai indietro
                AntOutBI1 = 1;
                AntOutBI2 = 0; 
            }
            else
            {
                if(fL >0)
                {
                    // Vai avanti
                    AntOutBI1 = 0;
                    AntOutBI2 = 1; 
                    
                }
                else
                {
                    // spegni 
                    AntOutBI1 = 0;
                    AntOutBI2 = 0;
                }
            }
            AntOutPWB.write(float(fL/100.0)); // DutyCycle del PWM Sinistro (Anteriore)
        }
    } //while (true) Ciclo principale
   
    //++++++++++++++++++++++++++++++++++++++++++++++++++
    //++++++++++++++ FINE Ciclo Principale +++++++++++++    
    //++++++++++++++++++++++++++++++++++++++++++++++++++
    
    
    
    
    //++++++++++++++++++++++++++++++++++++++++++++++++++++
    //++++++++++++++ INIZIO Ciclo test +++++++++++++++++++    
    //++++++++++++++++++++++++++++++++++++++++++++++++++++
 
    
    
   
    //++++++++++++ INIZIO Test Ricezione Comandi da BLE ++++++++++++++++++++++++++++
    while(true)
    {
        if(cCommandBLE != cOldCommandBLE)
        {
            
            switch (cCommandBLE)
            { 
                case 'A':
                {
                    myLed = 1;
                }; break;
                case 'B':
                {    
                    myLed = 0;
                }; break;
                case 'C':
                {
                    Light = 1;
                }; break;
                case 'D':
                {    
                    Light = 0;
                }; break;
                default: break;
            }
            // pc.printf("Comando = %c \r\n", cCommandBLE); // diagnostica
            cOldCommandBLE = cCommandBLE;
        }
        
    } // while(true) Test comandi da BLE  
    
    //++++++++++++ FINE ricezione comandi BLE ++++++++++++
    
    //++++++++++++ INIZIO Test modalità di funzionamento Motori con PWM ++++++++++++
    /*
    PostOutBI1 = 0;
    PostOutBI2 = 0;    
    AntOutBI1 = 0;
    AntOutBI2 = 0;    
    
    fDutyCycle = 0.0;
    
    // inizializza il pin PWM
    //+++PostOutPWB.period_us(100); // periodo del PWM Posteriore
    //+++PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
    //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore
    //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
    while(true)
    {
        if(cCommandBLE != cOldCommandBLE)
        {
            
            if(cCommandBLE == 'A')
            {
                myLed = 1;
                
                
            }
            
            else
            {    
                myLed = 0;
            }
            pc.printf("Comando = %c \r\n", cCommandBLE);
            cOldCommandBLE = cCommandBLE;
        }
        {
            // Vai avanti Anteriore
            PostOutBI1 = 0;
            PostOutBI2 = 0;  
            AntOutBI1 = 1;
            AntOutBI2 = 0;    
            PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
            AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
            pc.printf("Avanti Anteriore\r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0; 
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
            // Vai Indietro Anteriore
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            AntOutBI1 = 0;
            AntOutBI2 = 1;
            PostOutPWB.write(0.0); // duty cycle del PWM Posteriore
            AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
            pc.printf("Indietro Anteriore \r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            AntOutBI1 = 0;
            AntOutBI2 = 0;
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
            // Vai avanti Posteriore
            PostOutBI1 = 1;
            PostOutBI2 = 0;  
            AntOutBI1 = 0;
            AntOutBI2 = 0;    
            PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
            AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
            pc.printf("Avanti Posteriore\r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0; 
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
            // Vai Indietro Posteriore
            PostOutBI1 = 0;
            PostOutBI2 = 1;
            AntOutBI1 = 0;
            AntOutBI2 = 0;
            PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
            AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
            pc.printf("Indietro Posteriore \r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            AntOutBI1 = 0;
            AntOutBI2 = 0;
            pc.printf("Fermo\r\n\r\n");
            wait (3);
                
            // Vai avanti Anteriore + Posteriore
            PostOutBI1 = 1;
            PostOutBI2 = 0;  
            AntOutBI1 = 1;
            AntOutBI2 = 0;    
            PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
            AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
            pc.printf("Avanti Anteriore + Posteriore\r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0; 
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
            // Vai Indietro Anteriore + Posteriore
            PostOutBI1 = 0;
            PostOutBI2 = 1;
            AntOutBI1 = 0;
            AntOutBI2 = 1;
            PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
            AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
            pc.printf("Indietro Anteriore + Posteriore \r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            AntOutBI1 = 0;
            AntOutBI2 = 0;
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
            // Vai avanti Anteriore + Posteriore velocità ridotta
            PostOutBI1 = 1;
            PostOutBI2 = 0;  
            AntOutBI1 = 1;
            AntOutBI2 = 0;    
            PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
            AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
            pc.printf("Avanti Anteriore + Posteriore velocita' ridotta\r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0; 
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
            // Vai Indietro Anteriore + Posteriore velocità ridotta
            PostOutBI1 = 0;
            PostOutBI2 = 1;
            AntOutBI1 = 0;
            AntOutBI2 = 1;
            PostOutPWB.write(0.5); // duty cycle del PWM Posteriore
            AntOutPWB.write(0.5); // duty cycle del PWM Anteriore
            pc.printf("Indietro Anteriore + Posteriore velocita' ridotta\r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            AntOutBI1 = 0;
            AntOutBI2 = 0;
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
            
            
            // Ruota a destra
            PostOutBI1 = 0;
            PostOutBI2 = 1;
            AntOutBI1 = 1;
            AntOutBI2 = 0;
            PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
            AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
            pc.printf("Ruota a Destra\r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
            // Ruota a sinistra
            PostOutBI1 = 1;
            PostOutBI2 = 0;
            AntOutBI1 = 0;
            AntOutBI2 = 1;
            PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
            AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
            pc.printf("Ruota a Sinistra\r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
        }
    } // while(true) Test motore con PWM    
    */
    //++++++++++++ FINE Test Motore con PWM ++++++++++++
    
    //++++++++++++ INIZIO Test PWM tramite BLE ++++++++++++
    /*
    // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1'
    PostOutBI1 = 1;
    PostOutBI2 = 0;    
    AntOutBI1 = 1;
    AntOutBI2 = 0;    
    
    fDutyCycle = 0.0;
    
    // inizializza il pin PWM
    PostOutPWB.period_us(100); // periodo del PWM Posteriore
    PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
    AntOutPWB.period_us(100); // periodo del PWM Anteriore
    AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
    while(true)
    {
        //Parsing del comando ricevuto solo se cambia il comando.
        if(cCommandBLE != cOldCommandBLE)
        {
            // ricorda il vecchio comando
            cOldCommandBLE = cCommandBLE;
            //cambia velocità del PWM
            fDutyCycle = double(cOldCommandBLE - 0x41)/10.0; // converte il carattere numerico in numero '0' = 0x30 e lo porta in percentuale. p.e. '5' diventa 0,5
            PostOutPWB.write(fDutyCycle); // DutyCycle del PWM Posteriore
            AntOutPWB.write(fDutyCycle); // DutyCycle del PWM Anteriore
            pc.printf("Duty Cycle = %.2f\r\n\r\n", fDutyCycle);
            //PostOutPWB.pulsewidth_ms((cOldCommandBLE-0x41)*100); // Impulso ON del PWM
            //pc.printf("Pulse Width [ms]= %.2f\r\n", ((cOldCommandBLE-0x41)*100.0));
            
            //Se durante questo switch() riceve un nuovo comando da interrupt, il nuovo comando sarà considerato alla prossima iterazione
            switch (cOldCommandBLE) // usa cOldCommandBLE. 
             {
                case 'A':
                {
                    // se ricevi il comando 'A'    
                    // Vai avanti
                    PostOutBI1 = 1;
                    PostOutBI2 = 0;  
                    AntOutBI1 = 1;
                    AntOutBI2 = 0;    
                    PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
                    AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
                    pc.printf("Avanti\r\n\r\n");
                    wait (2);
                    
                    // spegni 
                    PostOutBI1 = 0;
                    PostOutBI2 = 0; 
                    AntOutBI1 = 0;
                    AntOutBI2 = 0; 
                    pc.printf("Fermo\r\n\r\n");
                    wait (1); 
                }break;
                case 'B':
                {
                    // se ricevi il comando 'B'    
                    // Vai Indietro
                    PostOutBI1 = 0;
                    PostOutBI2 = 1;
                    AntOutBI1 = 0;
                    AntOutBI2 = 1;
                    PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
                    AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
                    pc.printf("Indietro\r\n\r\n");
                    wait (2);
                    
                    // spegni 
                    PostOutBI1 = 0;
                    PostOutBI2 = 0;
                    AntOutBI1 = 0;
                    AntOutBI2 = 0;
                    pc.printf("Fermo\r\n\r\n");
                    wait (1);
                } break;
                
                case 'C':
                {
                    // se ricevi il comando 'C'   
                    // Ruota a destra
                    PostOutBI1 = 0;
                    PostOutBI2 = 1;
                    AntOutBI1 = 1;
                    AntOutBI2 = 0;
                    PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
                    AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
                    pc.printf("Destra\r\n\r\n");
                    wait (3.0);
                    
                    // spegni 
                    PostOutBI1 = 0;
                    PostOutBI2 = 0;
                    AntOutBI1 = 0;
                    AntOutBI2 = 0; 
                    pc.printf("Fermo\r\n\r\n");
                    wait (1); 
                } break;
                case 'D':
                {
                    // se ricevi il comando 'D' 
                    // Ruota a sinistra
                    PostOutBI1 = 1;
                    PostOutBI2 = 0;
                    AntOutBI1 = 0;
                    AntOutBI2 = 1;
                    PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
                    AntOutPWB.write(1.0); // duty cycle del PWM Anteriore
                    pc.printf("Sinistra\r\n\r\n");
                    wait (3.0);
                    
                    // spegni 
                    PostOutBI1 = 0;
                    PostOutBI2 = 0;
                    AntOutBI1 = 0;
                    AntOutBI2 = 0; 
                    pc.printf("Fermo\r\n\r\n");
                    wait (1);   
                } break;
                case 'L': // funziona solo se le ruote sono in movimento avendo ricevuto i comandi precedenti
                {
                    // se ricevi il comando 'L' Cambia direzione di movimento delle ruote
                    PostOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Posteriore impostato a 0
                    AntOutPWB.pulsewidth_ms(0); // Impulso ON del PWM Anteriore impostato a 0
            
                    // inverti direzione di movimento                    
                    PostOutBI1 = !PostOutBI1;
                    PostOutBI2 = !PostOutBI2;    
                    AntOutBI1 = !AntOutBI1;
                    AntOutBI2 = !AntOutBI2;    
                    // ricomincia a bassa velocità
                    PostOutPWB.write(0.3); // DutyCycle del PWM Posteriore
                    AntOutPWB.write(0.3); // DutyCycle del PWM Anteriore
                } break;
                default: 
                {
                    // se ricevi comandi diversi, spegni i motori
                    // spegni 
                    PostOutBI1 = 0;
                    PostOutBI2 = 0;
                    AntOutBI1 = 0;
                    AntOutBI2 = 0; 
                    pc.printf("Fermo\r\n\r\n");
                    wait (1);   
                } break;
            }
        }
    } // while(true) Test motore con PWM pilotato da BLE
    */
    //++++++++++++ FINE Test PWM tramite BLE ++++++++++++

   
    /*    
    //++++++++++++ INIZIO Test Motore Destra o Sinistra con PWM ++++++++++++
    PostOutBI1 = 1;
    PostOutBI2 = 0;    
    AntOutBI1 = 0;
    AntOutBI2 = 0;    
    
    fDutyCycle = 0.0;
    
    // inizializza il pin PWM
    PostOutPWB.period_us(100); // periodo del PWM Posteriore
    PostOutPWB.write(fDutyCycle); // duty cycle del PWM Posteriore
    //+++AntOutPWB.period_us(100); // periodo del PWM Anteriore
    //+++AntOutPWB.write(fDutyCycle); // duty cycle del PWM Anteriore
    while(true)
    {
        //+++if(cCommandBLE == 'A')
        {
            // Vai avanti
            PostOutBI1 = 1;
            PostOutBI2 = 0;  
            AntOutBI1 = 0;
            AntOutBI2 = 0;    
            PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
            AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
            pc.printf("Avanti\r\n\r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0; 
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
            // Vai Indietro
            PostOutBI1 = 0;
            PostOutBI2 = 1;
            AntOutBI1 = 0;
            AntOutBI2 = 0;
            PostOutPWB.write(1.0); // duty cycle del PWM Posteriore
            AntOutPWB.write(0.0); // duty cycle del PWM Anteriore
            pc.printf("Indietro\r\n\r\n");
            wait (2);
            
            // spegni 
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            AntOutBI1 = 0;
            AntOutBI2 = 0;
            pc.printf("Fermo\r\n\r\n");
            wait (3);
            
            
            
        }
    } // while(true) Test motore con PWM  
    */
    //++++++++++++ FINE Test Motore Destra o Sinistra con PWM ++++++++++++
        
        
        
    //++++++++++++ INIZIO Test Motore ++++++++++++
    /*
    while(1)
    {
        //if(myButton == 0)
        {
            // CW
            PostOutPWB = 1;
            PostOutBI1 = 1;
            PostOutBI2 = 0;    
            AntOutPWB = 1;
            AntOutBI1 = 0;
            AntOutBI2 = 1;    
            pc.printf("CW\r\n\r\n");
            wait (2);
            
            
            // spegni 
            PostOutPWB = 1;
            PostOutBI1 = 0;
            PostOutBI2 = 0; 
            AntOutPWB = 1;
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
            wait (1);
            

            // CCW
            PostOutPWB = 1;
            PostOutBI1 = 0;
            PostOutBI2 = 1;
            AntOutPWB = 1;
            AntOutBI1 = 1;
            AntOutBI2 = 0;
            wait (2);
            pc.printf("CCW\r\n\r\n");
            
            // spegni 
            PostOutPWB = 1;
            PostOutBI1 = 0;
            PostOutBI2 = 0;
            AntOutPWB = 1;
            AntOutBI1 = 0;
            AntOutBI2 = 0; 
             
            wait (1);
            
        }
    } // while(true)  test motore
    */
    //++++++++++++ FINE Test Motore ++++++++++++
          
       
    //++++++++++++ INIZIO Test PWM tramite seriale del PC ++++++++++++
    /*
    // inizializza segnali (BI1,BI2 = '1','0') per movimento CW. Per il movimento CCW (BI1,BI2 = '0','1'
    PostOutBI1 = 1;
    PostOutBI2 = 0;    
    fDutyCycle = 0.5;
    
    // inizializza il pin PWM
    PostOutPWB.period_ms(1000); // periodo del PWM
    PostOutPWB.write(fDutyCycle); // duty cycle del PWM
    while(true)
    {
        // verifica se è arrivato un carattere dalla seriale del PC
        if(pc.readable())
        {
            cReadChar = pc.getc(); // Read hyperterminal
            fDutyCycle = float(cReadChar - 0x30)/10.0; // converte il carattere numerico in numero '0' = 0x30 e lo porta in percentuale. p.e. '5' diventa 0,5
            pc.printf("Duty Cycle = %.2f\r\n", fDutyCycle);
            //PostOutPWB.period_ms(1000); // periodo del PWM
            //PostOutPWB.write(fDutyCycle); // DutyCycle del PWM
            
            // Inverti direzione del moto
            //PostOutBI1 != PostOutBI1;
            //PostOutBI2 != PostOutBI2;
            
            PostOutPWB.pulsewidth_ms((cReadChar-0x30)*100); // Impulso ON del PWM
            
        }
    } // while(true)  test PWM tramite seriale del PC
    */
    //++++++++++++ Fine Test PWM tramite seriale del PC ++++++++++++
    
    //++++++++++++++++++++++++++++++++++++++++++++++++++
    //++++++++++++++ FINE Ciclo test +++++++++++++++++++    
    //++++++++++++++++++++++++++++++++++++++++++++++++++

} // main()