Problema con BLE scheda Genuino 101

Salve a tutti!
Ho un problema con il software del mio robot bilanciante.
Il progetto prevede che il robot sia in grado di tenersi in equilibrio e ricevere dei comandi via bluetooth.

Tuttavia ho un problema con l’uso del Bluetooth. Uso il servizio UART della ap nrf toolbox per comunicare. Il bluetooth per ora va a scrivere in base ai comandi su alcune variabili, ma non effettua nessuna vera modifica nel comportamento del robot. Tuttavia assisto ad un fenomeno particolare: Il robot si bilancia come atteso fino a che non connetto l’app del cellulare. Quando la connessione viene stabilita il robot non è piu in grado di tenersi in equilibrio. Basta interrompere la comunicazione per riportare il robot al normale funzionamento. Non ho idea del perché ciò accada. Premetto che sono nuovo del forum e che questo è il mio primo progetto con genuino 101.
Il codice è il seguente:

double Setpoint = 0;                                                        
double inputAngle = 0;                                                     
double outputPwm1;                                                        
int outMax = 255;                                                           
int outMin = -255;                                                          
float lastInput = 0;                                                            
double ITerm =0;                                                          

// Costanti del PID

double kp = 45;           // costante Proporzionale
double ki = 0;            // costante Integrale
double kd = 0;            // costante Derivativa



// definizione dei pin per il comando dei motori

int motorSspeed = 6;
int motorDspeed = 5;

int motorDA = 11;
int motorDB = 12;
int motorSA = 9;
int motorSB = 8;

// fine definizione dei pin motori


#include <CurieBle.h>                                                         // include la libreria per la comunicazione BLE

BLEPeripheral blePeripheral;                                                  // BLE Peripheral Device (the board you're programming)
BLEService UART("6E400001-B5A3-F393-E0A9-E50E24DCCA9E");                      // Imposta il codice identificativo del servizio BLE UART Service

// BLE UART Characteristic 
BLEUnsignedCharCharacteristic TX("6E400002-B5A3-F393-E0A9-E50E24DCCA9E", BLEWrite);
BLEUnsignedCharCharacteristic RX("6E400003-B5A3-F393-E0A9-E50E24DCCA9E", BLENotify | BLERead);

const int ledPin = 13;       // pin di comando del LED on-board di arduino

char sendingChar = 0x41;     // variabile per l'invio di caratteri tramite bluetooth 
char txChar = 0;


#include "CurieImu.h"        // include la libreria per l'utilizzo di accelerometro e giroscopio
#include <Math.h>            // include la libreria per l'utilizzo di funzioni matematiche

double angolo = 0;          // variabile per il salvataggio delle misure di angolo

int16_t ax, ay, az;         
int16_t gx, gy, gz;         

  int pwww;
  int minimo = 0;
  int velocita = 0;        
 




double  dt = 0;                 // differenziale di tempo per il calcolo di integrazione nel filtro complementare
unsigned long t = 0;
unsigned long prevT = 0;


// variabili per comandi movimento:

int muovi = 0;




//Subroutine che calcola il valore dell'uscita del PID
double compute(double input)
{

      double error = Setpoint - input;
      ITerm = ITerm + error * ki;                  // calcolo dell'integrale
      if (ITerm > outMax) ITerm = outMax;          // se il termine integrale è maggiore o minore 
      if (ITerm < outMin) ITerm = outMin;          // del limite massimo o minimo, viene riportato nel range
      
      double dInput = (input - lastInput);         
      double output = kp * error + ITerm + kd * dInput;   
      
      if(output > outMax) output = outMax;               
      else if(output < outMin) output = outMin;           
    
      // ricorda l'input precedente
      lastInput = input;
      
      return output;                                     
}


void setup() {

  
  Serial.begin(9600);
  


  //inizializzazione e calibrazione IMU
  //
  CurieImu.initialize();
  
  CurieImu.autoCalibrateGyroOffset();
  CurieImu.autoCalibrateXAccelOffset(0);
  CurieImu.autoCalibrateYAccelOffset(0);
  CurieImu.autoCalibrateZAccelOffset(1);
  CurieImu.setGyroOffsetEnabled(true);
  CurieImu.setAccelOffsetEnabled(true);
  //
  // fine calibrazione IMU

  // impostazione come pin di output dei pin di comando motori e LED
  pinMode(motorDspeed, OUTPUT);
  pinMode(motorSspeed, OUTPUT);
  pinMode(motorDA, OUTPUT);
  pinMode(motorDB, OUTPUT);
  pinMode(motorSA, OUTPUT);
  pinMode(motorSB, OUTPUT);
  pinMode(ledPin, OUTPUT);
  // fine impostazione OUTPUT




  // imposta nome Bluetooth della scheda and service UUID:
  blePeripheral.setLocalName("BB");
  blePeripheral.setAdvertisedServiceUuid(UART.uuid());
  // aggiunge i servizi e le caratteristiche:
  blePeripheral.addAttribute(UART);
  blePeripheral.addAttribute(TX);
  blePeripheral.addAttribute(RX);
  // imposta il valore iniziale delle caratteristiche:
  TX.setValue(0);
  RX.setValue(128);
  // avvia la comunizazione BLE:
  blePeripheral.begin();
  
  
  
}

void loop() {
  
   digitalWrite(ledPin, HIGH); 

  
  t = micros();             
  dt = t - prevT;
  dt = dt / 1000000;
  prevT = t;

  
  CurieImu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     

 
  angolo = atan( ay / sqrt(pow(ax, 2) + pow(az, 2)));      //calcolo dell'angolo
  angolo = (360 * angolo) / 6.28 ;
  

  inputAngle = angolo;
  


  
  if (inputAngle > 2.5 || inputAngle < -2.5 ) kp = 28;
  else kp = 48;
  outputPwm1 = compute(inputAngle);  
  


  sendingChar = angolo;
  RX.setValue(sendingChar + '0');
 

  
  BLECentral central = blePeripheral.central();          
  
  if (central) {                                    
   
    if (central.connected()) {
      
      
      if (TX.written()) 
      {                                      
        
        txChar = TX.value();
        
        switch (txChar)
        {
          case '0' :
            muovi = 0;
            break;
          
          case '1' :
          
             
             
              muovi = 0;
            break;

          
          
          case '2' :
             muovi = 1;
            break;
         
          
          case '3' :
             muovi = 0;
            break;
          
          
          case '4' :
             muovi = 2;
            break;
          
          
          case '5' :
             muovi = 0;
          
            break;  

          
          case '6' :
               muovi = 3;  
            break;
          
          
          case '7' :
            break;
          
          
          case '8' :
            break;
          
          
          case '9' :
                muovi = 0;
                 
          
            break;
        }
        
        
      }
    }
  }
  
  
  if (outputPwm1 < 0)                  
  {
    
    digitalWrite(motorSA, HIGH);      
    digitalWrite(motorSB, LOW);
  
    digitalWrite(motorDA, HIGH);
    digitalWrite(motorDB, LOW);
  
    velocita = -outputPwm1;                
    velocita = floor(velocita);
  
   pwww = velocita + minimo;

   if (pwww > 255) pwww=255;
   
   
    analogWrite(motorSspeed, pwww);
    analogWrite(motorDspeed, pwww);
   
    
  }
  else if (outputPwm1 > 0)              // se l'uscita del PID è positiva
  {
    
    digitalWrite(motorSA, LOW);
    digitalWrite(motorSB, HIGH);
  
    digitalWrite(motorDA, LOW);
    digitalWrite(motorDB, HIGH);
    
    velocita = outputPwm1;
    
    velocita = floor(velocita);
    
   if (avvio > 0)
   {
     velocita = velocita + 100;
     avvio--;
    
   }


 
   pwww = velocita + minimo;

   if (pwww > 255) pwww=255;
   
   
    analogWrite(motorSspeed, pwww);
    analogWrite(motorDspeed, pwww);
 
  }

}

Grazie mille in anticipo

Salve,
essendo il tuo primo post, ti chiederei cortesemente di presentarti QUI (spiegando bene quali conoscenze hai di elettronica e di programmazione … possibilmente evitando di scrivere solo una riga di saluto) e di leggere con attenzione il REGOLAMENTO … Grazie.

Guglielmo