Problemi

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
francesco01
Date:
Thu Jul 23 10:19:54 2020 +0000
Parent:
18:4b1d35d5f4fd
Commit message:
Problemi

Changed in this revision

MicRobot-Rev085.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 4b1d35d5f4fd -r 733cea5788a4 MicRobot-Rev085.cpp
--- a/MicRobot-Rev085.cpp	Wed Jul 08 19:55:47 2020 +0000
+++ b/MicRobot-Rev085.cpp	Thu Jul 23 10:19:54 2020 +0000
@@ -4,43 +4,9 @@
 #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
@@ -56,29 +22,6 @@
 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
@@ -89,33 +32,6 @@
 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;
 
@@ -130,49 +46,9 @@
 // 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 
@@ -191,12 +67,12 @@
         //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
+       /* 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");
-         }
+         }*/
     }
 }
 
@@ -215,14 +91,14 @@
         cReadChar = myBLE.getc(); // Read character
         //caRxPacket[nCharCount]=cReadChar;
         //nCharCount++;
-        pc.printf("%c", cReadChar); // diagnostica
+       // 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 
+           // 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
@@ -232,160 +108,44 @@
                 cCommandBLE = cReadChar; 
                 // legge e memorizza come paramentro il successivo carattere
                 cReadChar = myBLE.getc(); // legge parametro
-                pc.printf("%c", cReadChar); // diagnostica 
+              //  pc.printf("%c", cReadChar); // diagnostica 
                 nParamBLE = cReadChar-0x30;
                 cReadChar = myBLE.getc(); // legge la ')' di chiusura comando       
-                pc.printf("%c", cReadChar); // diagnostica 
+                //pc.printf("%c", cReadChar); // diagnostica 
 
                 // visualizza comando e parametro inviato da BLE
-                pc.printf("> %c%d \r\n\r",cCommandBLE, nParamBLE); // diagnostica
+               // pc.printf("> %c%d \r\n\r",cCommandBLE, nParamBLE); // diagnostica
+                if(cReadChar==')') 
+                    {
+                        //pc.printf("(fX , fY) = (%.1f , %.1f) \r\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
+                    }
             }
             // +++++++++++++++++ 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 == '(')
-    }
+    }// chiude il while
 }
 
+
+
+
 /*********************************************************************************************************************************************/
 /* 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");
+    pc.printf("\r\n************  CLM ****************** \r\n");
+    pc.printf("*** Campanella Amaldi ***\r\n");
+    myBLE.printf("***W Campanella Amaldi ***\r\n");
     
     // inizializza variabili da BLE
     cCommandBLE = 0; // inizialmente nessun comando da BLE
@@ -393,94 +153,12 @@
     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 +++++++++++++++++++++++++++++++++++++++++++
-    
+    nCharCount=0;                   
      
     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
     //++++++++++++++ INIZIO Ciclo Principale +++++++++++++++++++    
@@ -490,62 +168,39 @@
     {
     
         //++++++++++++++++++++++++++ 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;
+                case 'A': // entrata
+                { 
+                    if(nParamBLE==1)
+                    {
+                        myBLE.printf("Entrate\n\r"); 
+                    }
+                    else
+                    {
+                    }  
                 } break;
-                case 'L': // Accendi/spegni illuminazione a LED
+                case 'B': // Cambio
                 {   
-                    Light = nParamBLE;
+                    if(nParamBLE==1)
+                    {
+                        pc.printf("Cambio\n\r"); 
+                    }
+                    else
+                    {
+                    }  
                 } break;
-                case 'R': // Reset odometria e illuminazione
+                case 'C': // Uscita
                 {    
                     if(nParamBLE==1)
                     {
-                        bReset = true;
-                        nCountRiseEdge = 0;
-                        nOldCountRiseEdge = 0;
-                        Light = 0;
-                        fDistanzaPercorsa = 0.0;
-                        fSpeed = 0.0;
+                        pc.printf("Uscite\n\r"); 
                     }
                     else
                     {
-                        // se nParamBLE = 0, e comunque diverso da 1, bReset=false -> ricomincia a funzionare normalmente
-                        bReset = false;
                     }  
                 } break;
                 
@@ -557,88 +212,6 @@
         }
         
         //++++++++++++++++++++++++++++++++++++++++++++ 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
     
     //++++++++++++++++++++++++++++++++++++++++++++++++++++++++