Problema con Arduino y Servos: Se detiene el codigo.

Hola comunidad, buen dia, soy nuevo en este foro.
Bueno vine en busca de una posible solucion a mi problema:

Me encuentro desde hace unos meses desarrollando un hexapodo con el poco conocimiento de arduino que tengo, consta de un Arduino AT Mega 2560, 5 servos MG995, un modulo bluetooth HC-05, un sensor ultrasonico y todo alimentado con una bateria recargable NP7-6 de 6V 7.0Ah.
El hexapodo lo controlo mediante un android mandandole caracteres que lee el modulo y ejecuta una parte del codigo; el problema viene cuando despues de unos 2 a 5 minutos de mandarle ordenes todo el hexapodo se detiene y ya no quiere recibir ninguna orden, entonces me veo obligado a resetear el arduino y reconectar modulo.

La tierra de los servos y el arduino estan juntas, sus señales estan en pines PWM, no veo fallas de voltaje. Los demas componentes estan bien conectados.

Ya investigue, hice pruebas con la bateria, alimentacion USB, cambiando partes del codigo y nada. Espero me puedan ayudar, gracias de antemano.

El codigo con la parte de los servos (exclui la parte del sensor ultrasonico y una matriz de led´s):

#include <Servo.h>
#define Pecho 7
#define Ptrig 8
long duracion, distancia; 
Servo s1;
Servo s2;
Servo s3;
Servo s4;
Servo s5;
int estado ;  // inicia detenido
int x;
int pos = x;


void setup() {
  Serial.begin(9600); // inicia el puerto serial para comunicacion con el Bluetooth
  s1.attach(2);
  s2.attach(3);
  s3.attach(4);
  s4.attach(5);
  s5.attach(6);
   pinMode(Pecho, INPUT);     // define el pin 7 como entrada (echo)
  pinMode(Ptrig, OUTPUT);    // define el pin 8 como salida  (triger)

  pinMode(13, OUTPUT);
}

void loop() {

  if (Serial.available() > 0) { // lee el bluetooth y almacena en estado
    estado = Serial.read();
  }

  if (estado == 'a') { // Camina hacia adelante
    do
    {
      s3.write(160);
      delay(200);
    } while (digitalRead(4) == HIGH);
    delay(100);
    s1.write(30);
    s2.write(30);
    s4.write(30);
    s5.write(30);
    delay(300);
    {
      do
      {
        s3.write(10);
        delay(200);
      } while (digitalRead(4) == HIGH);
      delay(100);
      s1.write(110);
      s2.write(110);
      s4.write(110);
      s5.write(110);
      delay(300);
    }
  }

  if (estado == 'b') { // Camina hacia atras
    do
    {
      s3.write(10);
      delay(200);
    } while (digitalRead(4) == HIGH);
    delay(100);
    s1.write(30);
    s2.write(30);
    s4.write(30);
    s5.write(30);
    delay(300);
    {
      do
      {
        s3.write(160);
        delay(200);
      } while (digitalRead(4) == HIGH);
      delay(100);
      s1.write(110);
      s2.write(110);
      s4.write(110);
      s5.write(110);
      delay(300);
    }
  }

  if (estado == 'c') { // Camina hacia la derecha
    do
    {
      s3.write(160);
      delay(200);
    } while (digitalRead(4) == HIGH);
    delay(100);
    s1.write(30);
    s2.write(30);
    s4.write(70);
    s5.write(70);
    delay(300);
    {
      do
      {
        s3.write(10);
        delay(200);
      } while (digitalRead(4) == HIGH);
      delay(100);
      s1.write(110);
      s2.write(110);
      s4.write(70);
      s5.write(70);
      delay(300);
    }
  }
  if (estado == 'd') { // Camina hacia la izquierda
    do
    {
      s3.write(160);
      delay(200);
    } while (digitalRead(4) == HIGH);
    delay(100);
    s1.write(70);
    s2.write(70);
    s4.write(30);
    s5.write(30);
    delay(300);
    {
      do
      {
        s3.write(10);
        delay(200);
      } while (digitalRead(4) == HIGH);
      delay(100);
      s1.write(70);
      s2.write(70);
      s4.write(110);
      s5.write(110);
      delay(300);
    }
  }

  if (estado == 'e') { // Enciende con un caminar lento
    do
    {
      s3.write(160);
      delay(300);
    } while (digitalRead(4) == HIGH);
    delay(200);
    s1.write(30);
    s2.write(30);
    s4.write(30);
    s5.write(30);
    delay(600);
    {
      do
      {
        s3.write(10);
        delay(300);
      } while (digitalRead(4) == HIGH);
      delay(200);
      s1.write(110);
      s2.write(110);
      s4.write(110);
      s5.write(110);
      delay(600);
    }
  }

  if (estado == 'f') { // Enciende con un caminar rapido
    do
    {
      s3.write(160);
      delay(200);
    } while (digitalRead(4) == HIGH);
    delay(100);
    s1.write(30);
    s2.write(30);
    s4.write(30);
    s5.write(30);
    delay(200);
    {
      do
      {
        s3.write(10);
        delay(200);
      } while (digitalRead(4) == HIGH);
      delay(100);
      s1.write(110);
      s2.write(110);
      s4.write(110);
      s5.write(110);
      delay(200);
    }
  }

  if (estado == 'g') { // Detiene todos los servos
    s1.write(70);
    s2.write(70);
    s3.write(70);
    s4.write(70);
    s5.write(70);
    delay(500);
  }
}

Hola Ryan Marquez A, bienvenido al foro. Gracias por leer las normas y postear debidamente tu problema.

Un comentario. Pin 4 esta claro que es un sensor pero no dices en el código que es porque no esta comentado. Tienes varios loops do while que dependen de que se ponga en HIGH para salir luego de un comando enviado desde tu móvil. Sería posible que el cable este suelto y que haga un falso contacto o que el sensor falle? Revisa por ahi.

Otra cosa pero no puedes advertirlo salvo con algún led es que alertes cuando el sensor se active o este en LOW.

Veamos si esta sugerencia conduce a algo.

Algo que no me gusta de tu código es que usas muchos delays y eso demora la respuesta de tus comandos pero son solo cientos de milisegundos (400 sumados) no es tan grave. Para la versión 2.0 considera crear una máquina de estados para que la respuesta del hexapodo será mas instantánea, eso si vale la pena claro!!

Hola surbyte, gracias por responder.

Bueno en el pin 4 tengo conectado un servomotor, en mi modelo de hexapodo está en la parte de abajo centrado, lo que hace es levantar el hexapodo hacia un lado para después dos patas (servos) de un lado arrastren hacia atrás y al mismo tiempo las otras dos se muevan hacia enfrente sin tocar el piso.

Como no tengo mucha experiencia en lenguaje de programación lo único que logré para mi objetivo fue eso, y funciona pero como observaste no de manera óptima. Habría alguna mejor manera de hacerlo sin tantos loops y delays?

En todo caso el LOW o HIGH vienen del servo y creo que esos pulsos me puede estar fallando también por qué he notado que le cuesta a la batería suministrar energía a todos los servos después de un tiempo, entonces estaba buscando como conectarle unos condensadores para solucionarlo aunque no se mucho del tema.

Según yo los delays son para el tiempo que quiero que tarden los servos en llegar a esa posición.

Sigo en busca de optimizar y voy a tomar tus sugerencias a ver qué resulta de momento aunque creo que falta algo más.

De antemano gracias.

Ya hice la prueba definitiva con el voltaje, no hay ningún problema con demandas altas, el voltaje es constante y varía entre 6.0v a 5.7v entre cada movimiento.

Por lo tanto lo más lógico sería que es problema del código. Cuando le digo que camine y no le mando ninguna otra indicación lo hace sin problemas hasta que después de unos minutos para completamente y no hace nada más ni recibe nada más. Si le mando indicaciones una tras otra las realiza hasta que después ya no hace nada.

No sé si en el código al final de cada loop se necesite alguna declaración que diga que mientras no reciba ningún carácter continue (aunque así es pero se para detiene como dije) y que cuando llegue un carácter que vuelva a posición 0 (todos los servomotores neutros) de inmediato y luego realice el bloque pedido.

No entiendo esto

TIenes un servo conectado a pin4

s3.attach(4);

y luego en tu código esperas digitalRead(4)

do
    {
      s3.write(160);
      delay(200);
    } while (digitalRead(4) == HIGH);

Eso es una incoherencia!!

Tiene sentido que se cuelgue!!

No recuerdo en base a qué hice ese programa, tal vez copié ese bloque, lo cambiaré entonces.

Te queda claro porque lo digo? A ver. tienes un servo conectado al pin 4 es asi? Y ademas tienes un sensor conectado al pin 4? Tu debes responderlo