Comunicación entre dos arduinos con BT HC-05 y HC-06

Buenas, estoy intentando comunicar dos Arduinos para que hagan lo siguiente:

Arduino Emisor: Tiene 5 pulsadores, 4 de los cuales actúan como interruptores, es decir, al pulsar la primera vez se activa y al volver a pulsar se desactiva. Y uno como pulsador puro. Conectado con un BT HC05

Arduino Receptor: Tiene 5 relés conectados y se activan y desactiva dependiendo de lo que le llegue por el serial del BT. Conectado con un BT HC06

El módulo receptor funciona perfectamente, ya que le envío desde un terminal Bluetooth desde un teléfono los caracteres y activa y desactiva los relés correctamente.

Ahora bien, el módulo emisor pienso yo que es el que está dando problemas y considero que puede ser por software. Ya que las primeras veces lo hace bien pero luego tarda más en enviar el caracter por serial. Y creo que se debe a que está continuamente mandando el estado de los 5 pulsadores y no consigo localizar el error.

Código del Emisor

#include <SoftwareSerial.h>

SoftwareSerial BT(10, 11); // RX, TX

boolean EstadoPul1 = 0;
boolean EstadoPul2= 0;
boolean EstadoPul3 = 0;
boolean EstadoPul4 = 0;
boolean EstadoPul5 = 0;

boolean EstadoPul1Anterior = 0;
boolean EstadoPul2Anterior = 0;
boolean EstadoPul3Anterior = 0;
boolean EstadoPul4Anterior = 0;

boolean SetPul1 = 0;
boolean SetPul2= 0;
boolean SetPul3 = 0;
boolean SetPul4 = 0;
boolean SetPul5 = 0;

int Pul1 = 2;
int Pul2 = 3;
int Pul3 = 4;
int Pul4 = 5;
int Pul5 = 6;

void setup()
{  
  pinMode(Pul1,INPUT);
  pinMode(Pul2,INPUT);
  pinMode(Pul3,INPUT);
  pinMode(Pul4,INPUT);
  pinMode(Pul5,INPUT);
 
  BT.begin(9600);
//  Serial.begin(115200);
  

}
void loop(){

EstadoPul1 = digitalRead (Pul1);
EstadoPul2 = digitalRead (Pul2);
EstadoPul3= digitalRead (Pul3);
EstadoPul4 = digitalRead (Pul4);
EstadoPul5 = digitalRead (Pul5);


  
if (EstadoPul1==HIGH && EstadoPul1Anterior ==0){
    SetPul1 = 1 - SetPul1; 
//    delay(10);
   
}
  EstadoPul1Anterior = EstadoPul1;
  
  if (SetPul1 == 1){
    BT.write('1');
//    Serial.println("Rele 1 activado");
  }

  else{
     BT.write('a');
//     Serial.println("Rele 1 desactivado");
    }



if (EstadoPul2==HIGH && EstadoPul2Anterior ==0){
    SetPul2 = 1 - SetPul2; 
//    delay(10);
   
}
  EstadoPul2Anterior = EstadoPul2;
  
  if (SetPul2 == 1){
    BT.write('2');
//    Serial.println("Rele 2 activado");
  }

  else{
     BT.write('b');
//     Serial.println("Rele 2 desactivado");
    }


if (EstadoPul3==HIGH && EstadoPul3Anterior ==0){
    SetPul3 = 1 - SetPul3; 
//    delay(10);
   
}
  EstadoPul3Anterior = EstadoPul3;
  
  if (SetPul3 == 1){
    BT.write('3');
  }

  else{
     BT.write('c');
    }


if (EstadoPul4==HIGH && EstadoPul4Anterior ==0){
    SetPul4 = 1 - SetPul4; 
//    delay(10);
   
}
  EstadoPul4Anterior = EstadoPul4;
  
  if (SetPul4 == 1){
    BT.write('4');
  }

  else{
     BT.write('d');
    }




  if (EstadoPul5==HIGH){
      BT.write('5');
    }
  if (EstadoPul5==LOW){ 
      BT.write('0');

    }
  
 }

Código del Receptor

#include <SoftwareSerial.h>

SoftwareSerial BT(11, 10); // RX, TX
char c;
void setup() {
  // put your setup code here, to run once:
   BT.begin(9600);
//   Serial.begin(9600);
   
//se configura los pines de entrada y salida   
   pinMode(2,OUTPUT);
   pinMode(3,OUTPUT);
   pinMode(4,OUTPUT);
   pinMode(5,OUTPUT);
   pinMode(6,OUTPUT);
   
// inicializa el estdo de los RELÉS  
   digitalWrite(2,HIGH);
   digitalWrite(3,HIGH);
   digitalWrite(4,HIGH); // LÓGICA INVERTIDA
   digitalWrite(5,HIGH);
   digitalWrite(6,HIGH);    
   
}

void loop() 
{
  if(BT.available())
  {
    c=BT.read();
//    c = Serial.read();
//    Serial.println(c);

    if(c=='a'){ 
      digitalWrite(2,HIGH);
    }
    if(c=='1'){ 
      digitalWrite(2,LOW);
    }
    if(c=='b'){ 
      digitalWrite(3,HIGH);
    }
    if(c=='2'){
      digitalWrite(3,LOW);
    }
    if(c=='c'){ 
      digitalWrite(4,HIGH);
    }
    if(c=='3'){
      digitalWrite(4,LOW);   
    }
    if(c=='d') {
      digitalWrite(5,HIGH);
    }
    if(c=='4'){
      digitalWrite(5,LOW);
    }

   if(c=='5') {
    digitalWrite(6,LOW);
   }
   if(c=='0'){
    digitalWrite(6,HIGH);     
   }
  }
}

Un saludo y gracias

A nivel de emisor yo cambiaría las cosas de lo que tienes a esto

#include <SoftwareSerial.h>

SoftwareSerial BT(10, 11); // RX, TX

boolean EstadoPul1 = 0;
boolean EstadoPul2= 0;
boolean EstadoPul3 = 0;
boolean EstadoPul4 = 0;
boolean EstadoPul5 = 0;

boolean EstadoPul1Anterior = 0;
boolean EstadoPul2Anterior = 0;
boolean EstadoPul3Anterior = 0;
boolean EstadoPul4Anterior = 0;
boolean EstadoPul5Anterior = 0;

boolean SetPul1 = 0;
boolean SetPul2 = 0;
boolean SetPul3 = 0;
boolean SetPul4 = 0;
boolean SetPul5 = 0;

int Pul1 = 2;
int Pul2 = 3;
int Pul3 = 4;
int Pul4 = 5;
int Pul5 = 6;

void setup()
{ 
  pinMode(Pul1,INPUT);
  pinMode(Pul2,INPUT);
  pinMode(Pul3,INPUT);
  pinMode(Pul4,INPUT);
  pinMode(Pul5,INPUT);
 
  BT.begin(9600);
//  Serial.begin(115200);
 

}
void loop(){

  EstadoPul1 = digitalRead (Pul1);
  EstadoPul2 = digitalRead (Pul2);
  EstadoPul3 = digitalRead (Pul3);
  EstadoPul4 = digitalRead (Pul4);
  EstadoPul5 = digitalRead (Pul5);

  if (EstadoPul1 && EstadoPul1Anterior){
      SetPul1 = 1 - SetPul1;
      //    delay(10);
      if (SetPul1 == 1){
          BT.write('1');
          //    Serial.println("Rele 1 activado");
      }
      else{
          BT.write('a');
          // Serial.println("Rele 1 desactivado");
      }
  }
  EstadoPul1Anterior = EstadoPul1;

  if (EstadoPul2 && EstadoPul2Anterior){
      SetPul2 = 1 - SetPul2;
      //    delay(10);
      if (SetPul2 == 1){
          BT.write('2');
          // Serial.println("Rele 2 activado");
      }
      else{
          BT.write('b');
          // Serial.println("Rele 2 desactivado");
      }
  }
  EstadoPul2Anterior = EstadoPul2;

  if (EstadoPul3 && EstadoPul3Anterior){
      SetPul3 = 1 - SetPul3;
      //    delay(10);
      if (SetPul3 == 1){
          BT.write('3');
      }
      else{
          BT.write('c');
      }
  }
  EstadoPul3Anterior = EstadoPul3;

  if (EstadoPul4 && EstadoPul4Anterior){
      SetPul4 = 1 - SetPul4;
      //    delay(10);
      if (SetPul4 == 1){
          BT.write('4');
      }
      else{
          BT.write('d');
      }
  }
  EstadoPul4Anterior = EstadoPul4;

  if (EstadoPul5 != EstadoPul5Anterior){
      if (EstadoPul5) {
          BT.write('5');
      }
      else {
          BT.write('0');
      }
  }
  EstadoPul5Anterior = EstadoPul5;
}

Todavía haría otro cambio pero… es algo tan sutil que no veras modificación de perfomance.
Para mi enviar 1 msg para cada cosa es como demasiado
Yo enviare solo 1 msg con el status de todo, asi que por ejemplo

algo de este modo

10001 diria que el Pul5 Puls4 Puls3 Puls2 Puls1 son 5 bits
1 para pulsador en ON y 0 para pulsador en OFF
Del otro lado el receptor solo recibe datos cuando hay un cambio

presionas el pulsador 3 entonces 10001 cambiaria a 10101
Solo envio eso, y tienes el status gral en el receptor.

y cuando pulses de nuevo 10101 cambia a 10001 de nuevo.
Muy facil

Como dije es elegante y no te cambia mucho. Pero solo consideralo.