Módulo SR04 deja de funcionar al ejecutar método librería Stepper [SOLUCIONADO]

Hola,

Necesito ayuda para identificar el siguiente problema, cuando ejecuto la lectura de distancia del módulo SR04 funciona correctamente hasta que ejecuto la librería Stepper para mover el motor.

Contexto: Coche cuatro ruedas con módulo bluetooth

  • Placa Mega2560 R3
  • 2 Módulos L298N para 4 motores
  • Módulo Bluetooth HC06
  • Módulo ultrasónico SR04
  • Stepper motor 4 hilos
  • Alimentación:
    Pila 9V para 2 L298N y 4 motores
    Resto componentes USB o Batería DC 5V 1000mA

Adjunto parte del código, excluyendo partes que no están relacionadas.

Tengo una función Case que uso para enviar instrucciones al módulo BT.
Si envío la orden para ejecutar Lectura_HCSR40() funciona perfectamente, devolviendo la medición todas las veces.
En cuanto envío la orden para ejecutar ComprobarDistancias (), la lectura del SR04 funciona perfectamente
la primera vez, devuelve todas las lecturas. A partir de aquí, el módulo SR04 no vuelve a realizar lecturas individuales
de una sola ejecución. Sin embargo, si ejecutas de nuevo la función ComprobarDistancias(), la primera lectura devuelve 0
pero el resto si devuelve valores de medición. Al terminar la función, la lectura del SR04 que hay al comienzo
del Loop siempre devuelve 0.

He leído que había algún problema con la librería Stepper y recomiendan usar Accelstepper,
no sé si este problema se resuelve haciendo el cambio de librería.

Gracias de antemano, un saludo.

#include "SR04.h"
#include <Stepper.h>
#include <string.h>

//SR04.h
#define TRIG_PIN 23
#define ECHO_PIN 6
SR04 sr04 = SR04(ECHO_PIN, TRIG_PIN);
long vA;
long vB;
long vC;
long aLecturaCM[5] = {0, 0, 0, 0, 0}; // En parada, contiene las lecturas del Sensor Ultrasónico
int aGiro[5] = {0, 256, 512, -256, -512}; // Indica el angulo de giro
int iMax[2] = {0, 0}; // En parada, contiene el angulo leído con mayor distancia

//Stepper.h
const int stepsPerRevolution = 256;  // change this to fit the number of steps per revolution
const int rolePerMinute = 100;         // Adjustable range of 28BYJ-48 stepper is 0~17 rpm
Stepper myStepper(stepsPerRevolution, 26, 12, 7, 13);

//string.h
char sCadena;

char vDir; // Guarda la orden de dirección
char vAntDir;

char DatosBT;
int pinVCC = 22;
int ledpin = 24;

//Motor 1 Traseo Izquierdo
int PinMotor1A = 2;
int PinMotor1B = 3;

//Motor 2 Trasero Derecho
int PinMotor2A = 4;
int PinMotor2B = 5;

//Motor 3 Delantero Izquierdo
int PinMotor3A = 8;
int PinMotor3B = 9;

//Motor 4 Delantero Derecho
int PinMotor4A = 10;
int PinMotor4B = 11;

void setup() {

  Serial.begin(9600);
  Serial.println("ejecutando setup");
  delay(200);

  pinMode(18, INPUT);
  pinMode(19, INPUT);

  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  digitalWrite(TRIG_PIN, LOW);
  digitalWrite(ECHO_PIN, LOW);

  Serial1.begin(9600);
  pinMode(ledpin, OUTPUT);
  pinMode(pinVCC, OUTPUT);
  digitalWrite(pinVCC, HIGH);

  vDir = 0;
  vAntDir = 0;

  myStepper.setSpeed(rolePerMinute);
}

void loop() {

  vA = 0;
  vA = Lectura_HCSR40();

  if (((vDir == '1') || (vDir == '2') || (vDir == '3')) && (vA <= 30)) {
    digitalWrite(ledpin, LOW);
    Serial.println("Parar");
    PararMotores();
    ComprobarDistancias();
    CambiarRuta();
    vDir = 0;
  }

  if (Serial1.available()) {

    DatosBT = Serial1.read();
    Serial.println(DatosBT);
    vDir = DatosBT;

    switch (vDir)
    {
      case '1':
        digitalWrite(ledpin, HIGH);
        Serial.println("Avanzar izquierda");
        AvanzarIzquierda();
        break;
      case '2':
        digitalWrite(ledpin, HIGH);
        Serial.println("Avanzar");
        AvanzarMotores();
        break;
      case '3':
        digitalWrite(ledpin, HIGH);
        Serial.println("Avanzar derecha");
        AvanzarDerecha();
        break;
      case '4':
        digitalWrite(ledpin, HIGH);
        if (vAntDir == '2') {
          Serial.println("Girar izquierda");
          GirarIzquierda();
        }
        if (vAntDir == '8') {
          Serial.println("Atras izquierda");
          GirarAtrasIzquierda();
        }
        break;
      case '5':
        digitalWrite(ledpin, LOW);
        Serial.println("Parar");
        PararMotores();
        break;
      case '6':
        digitalWrite(ledpin, HIGH);
        if (vAntDir == '2') {
          Serial.println("Girar derecha");
          GirarDerecha();
        }
        if (vAntDir == '8') {
          Serial.println("Atras derecha");
          GirarAtrasDerecha();
        }
        break;
      case '7':
        digitalWrite(ledpin, HIGH);
        Serial.println("Retroceder Izquierda");
        RetrocederIzquierda();
        break;
      case '8':
        digitalWrite(ledpin, HIGH);
        Serial.println("Retroceder");
        RetrocederMotores();
        break;
      case '9':
        digitalWrite(ledpin, HIGH);
        Serial.println("Retroceder Derecha");
        RetrocederDerecha();
        break;
      case 'B':
        Serial.println("Probar lectura");
        //ComprobarDistancias();
        vB = Lectura_HCSR40();
        Serial.println(vB);
        break;
      case 'A':
        Serial.println("counterclockwise");
        myStepper.step(stepsPerRevolution);
        delay(50);
        break;
      case 'C':
        Serial.println("counterclockwise");
        myStepper.step(-stepsPerRevolution);
        delay(50);
        break;
      default:
        break;
    }
    vAntDir = vDir;
  }
  delay(100);

}

long Lectura_HCSR40() {

  long distancia = 0;
  long duracion = 0;

  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  duracion = pulseIn(ECHO_PIN, HIGH);

  /* Calculo de la distancia efectiva */
  distancia = (duracion * 100) / 5882;

  delay(25);
  return distancia;
}


void ComprobarDistancias () {

  aLecturaCM[0] = Lectura_HCSR40();
  Serial.println("Lectura posicion 0");
  Serial.println(aLecturaCM[0]);

  myStepper.step(stepsPerRevolution);
  delayMicroseconds(2);
  aLecturaCM[1] = Lectura_HCSR40();
  Serial.println("Lectura posicion 1");
  Serial.println(aLecturaCM[1]);

  myStepper.step(stepsPerRevolution);
  delayMicroseconds(2);
  aLecturaCM[2] = Lectura_HCSR40();
  Serial.println("Lectura posicion 2");
  Serial.println(aLecturaCM[2]);

  myStepper.step(-768);
  delayMicroseconds(2);
  aLecturaCM[3] = Lectura_HCSR40();
  Serial.println("Lectura posicion 3");
  Serial.println(aLecturaCM[3]);

  myStepper.step(-stepsPerRevolution);
  delayMicroseconds(2);
  aLecturaCM[4] = Lectura_HCSR40();
  Serial.println("Lectura posicion 4");
  Serial.println(aLecturaCM[4]);

  myStepper.step(512);
  Serial.println("Posicion inicial...");

  // Comprobar valores leidos
  int i = 0;
  iMax[0] = 0; iMax[1] = 0;
  for (i = 0; i < 5; i = i + 1) {
    Serial.println(aLecturaCM[i]);
    if (aLecturaCM[i] > iMax[1]) {
      iMax[0] = i;
      iMax[1] = aLecturaCM[i];
    }
  }
  Serial.println("despues de comprobar los valores");
  Serial.println(iMax[0]);
  Serial.println(iMax[1]);
}

void CambiarRuta () {

  RetrocederMotores();
  delay(2000);
  PararMotores();

  if ((iMax[0] == 1) || (iMax[0] == 2)) { //Girar a la izquierda
    Serial.println("Voy a girar a la izquierda");
    GirarIzquierda();
    delay(800);
    PararMotores();
  }

  if ((iMax[0] == 3) || (iMax[0] == 4)) { //Girar a la derecha
    Serial.println("Voy a girar a la derecha");
    GirarDerecha();
    delay(800);
    PararMotores();
     }
}

void AvanzarMotores () {
  Motor1(1);
  Motor2(1);
  Motor3(1);
  Motor4(1);
}

void AvanzarIzquierda () {
 }

void AvanzarDerecha () {
  }

void RetrocederMotores () {
 }

void RetrocederIzquierda () {
 }

void RetrocederDerecha () {
  }

void PararMotores () {
  Motor1(3);
  Motor2(3);
  Motor3(3);
  Motor4(3);
}

void GirarIzquierda () {}

void GirarDerecha () {}


void GirarAtrasIzquierda () {}

void GirarAtrasDerecha () {}

void Motor1(int Sentido) {

  switch (Sentido) {
    case 1:
      //Avanzar
      digitalWrite (PinMotor1A, HIGH);
      digitalWrite (PinMotor1B, LOW);
      break;
    case 2:
      //Retroceder
      digitalWrite (PinMotor1A, LOW);
      digitalWrite (PinMotor1B, HIGH);
      break;
    case 3:
      //Parar
      digitalWrite (PinMotor1A, LOW);
      digitalWrite (PinMotor1B, LOW);
      break;
    default:
      break;
  }
}


void Motor2(int Sentido) {}

void Motor3(int Sentido) {}

void Motor4(int Sentido) {}

La rutina PararMotores() no esta.
No entiendo porque pusiste tu código separado en 3 o 4 partes? Solo se hace mas difícil y lleva tiempo integrarlo.

No se que es una función Case. No existe tal cosa pero como te entiendo quisiste decir una instrucción switch case que curiosamente no esta.

Modificado post e incorporado de nuevo el código. He quitado líneas de funciones porque superaba los 9000 caracteres. He dejado las declaraciones para poder compilar el código.

Correcto, con Case me refería a la instrucción Switch.

He continuado haciendo pruebas y ahora funciona más estable pero hay ocasiones que deja de funcionar la lectura. En estos casos, al ordenar avanzar (case 1, 2 o 3), el valor de distancia (vA) es siempre cero y ejecuta este bloque de código cuando no debería.

De los posibles errores (hardware, eléctrico o programación), creo que el problema debe ser de programación aunque me tiene confundido que ahora funcione mejor cuando antes dejaba de funcionar al mover el motor stepper.

La única diferencia significativa ha sido que he compilado y cargado el código desde otro ordenador y la última versión de arduino 1.8.12. No creo que sea relevante pero lo apunto como cambio.

Tu Problema son los delay(), tu funciones que involucran el stepper, están basadas en ellos. Es imposible que leas el sensor de distancia, mientras esta ejecutando alguna función de movimiento. Te aconsejo que leas los tutoriales que hay en la sección de documentación sobre milis().

He leído el uso de millis() pero no veo donde debería usarlo en el código para resolver el problema.

He creado otra función “ComprobarDistancias2” para no usar los delayMicroseconds(2) después de cada llamada a MyStepper.Step.

Las dos funciones ComprobarDistancia y ComprobarDistancia2 son ejecutadas al enviar el valor ‘B’ al módulo BT

He lanzado varias series de lecturas, cinco lecturas por cada serie y obtengo mejores lecturas con la función inicial ComprobarDistancia que con la nueva sin los delay. Sin poner los delay, la lectura del SR04 es peor. En ninguno de los casos es completa pero es mejor con la versión de código anterior.

Lecturas ComprobarDistancia2 (5 lecturas por ejecución, 5 ejecuciones)

106 0 0 0 0
114 0 0 0 0
29 28 47 0 28
63 69 69 50 69
0 89 0 0 85

Lecturas ComprobarDistancia

113 0 0 0 0
0 69 31 35 27
71 60 34 69 96
88 56 78 56 77
0 105 124 118 119

Si podéis precisar el tipo de cambio que debo realizar, sería de agradecer.

   case 'B':
        Serial.println("Probar lectura");
        ComprobarDistancias2();
 //ComprobarDistancias();
        break;


long Lectura_HCSR40() {

  long distancia = 0;
  long duracion = 0;

  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  duracion = pulseIn(ECHO_PIN, HIGH);

  /* Calculo de la distancia efectiva */
  distancia = (duracion * 100) / 5882;

  //delay(25);
  return distancia;
}

void ComprobarDistancias2 () {

  int i = 0;
  for (i = 0; i < 5; i = i + 1) {
     aLecturaCM[i] = Lectura_HCSR40();
     myStepper.step(aGiro[i]);
     delayMicroseconds(2);
     //delay(25);
  }    
  myStepper.step(512);
  
  // Comprobar valores leidos
  i = 0;
  iMax[0] = 0; iMax[1] = 0;
  Serial.println("lecturas realizadas");
  for (i = 0; i < 5; i = i + 1) {
    Serial.println(aLecturaCM[i]);
    if (aLecturaCM[i] > iMax[1]) {
      iMax[0] = i;
      iMax[1] = aLecturaCM[i];
    }
  }
  Serial.println("mayor distancia leída");
  Serial.println(iMax[0]);
  Serial.println(iMax[1]);
}


void ComprobarDistancias () {

  aLecturaCM[0] = Lectura_HCSR40();
  Serial.println("Lectura posicion 0");
  Serial.println(aLecturaCM[0]);

  myStepper.step(stepsPerRevolution);
  delayMicroseconds(2);
  aLecturaCM[1] = Lectura_HCSR40();
  Serial.println("Lectura posicion 1");
  Serial.println(aLecturaCM[1]);

  myStepper.step(stepsPerRevolution);
  delayMicroseconds(2);
  aLecturaCM[2] = Lectura_HCSR40();
  Serial.println("Lectura posicion 2");
  Serial.println(aLecturaCM[2]);

  myStepper.step(-768);
  delayMicroseconds(2);
  aLecturaCM[3] = Lectura_HCSR40();
  Serial.println("Lectura posicion 3");
  Serial.println(aLecturaCM[3]);

  myStepper.step(-stepsPerRevolution);
  delayMicroseconds(2);
  aLecturaCM[4] = Lectura_HCSR40();
  Serial.println("Lectura posicion 4");
  Serial.println(aLecturaCM[4]);

  myStepper.step(512);
  Serial.println("Posicion inicial...");

  // Comprobar valores leidos
  int i = 0;
  iMax[0] = 0; iMax[1] = 0;
  for (i = 0; i < 5; i = i + 1) {
    Serial.println(aLecturaCM[i]);
    if (aLecturaCM[i] > iMax[1]) {
      iMax[0] = i;
      iMax[1] = aLecturaCM[i];
    }
  }
  Serial.println("despues de comprobar los valores");
  Serial.println(iMax[0]);
  Serial.println(iMax[1]);
}

Problema identificado y resuelto.

El módulo SR04 ha comenzado a devolver el 100% de lecturas cuando he bajado el valor de las revoluciones del Stepper. Lo tenía en 100 y lo he bajado a 50, momento en el que he comenzado a obtener todas las lecturas. Lo he subido hasta 80 y funciona correctamente (const int rolePerMinute = 80;)

Otros temas que he aplicado que he visto por el foro por si os ayuda a alguno:

  • Añadir un bucle de lecturas por cada ejecución del módulo SR04. Podéis obtener una media de las lecturas por cada ejecución.

  • Los valores 0 de lecturas se producen porque el valor devuelvo por pulseIn(ECHO_PIN, HIGH) es muy bajo, valores por debajo de 10. Aplicando la fórmula de cálculo de distancia, esto se traduce en valores por debajo de 1 (0.05, …). Hay que tener en cuenta el tipo de variable en las operaciones ariméticas, usad float en lugar de long.

Dejo la función lectura del módulo SR04 por si alguno está interesado. Es igual que otras que hay circulando por la red.

long Lectura_HCSR40() {

  float distancia = 0;
  float duracion = 0;
  long lecturas = 0; 
  float alecturas[5] = {0, 0, 0, 0, 0};
  
  int i = 0, L = 0;
  for (i = 0; i < 5; i += 1) {
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    alecturas[i] = pulseIn(ECHO_PIN, HIGH);
    Serial.println(float(alecturas[i]));
    if (alecturas[i] > 0) L += 1; 
    lecturas += alecturas[i];
  }  
  //Serial.println(L);
  duracion = lecturas / L;
  distancia = (duracion * 100) / 5882;
  delay(25);
  return distancia;
}

Por si alguno revisa este post.

La solución anterior era incompleta. Las acciones realizadas ayudaron a mejorar la función de lectura del módulo SR04 pero el verdadero problema esta con la librería Stepper.

Cuando ejecutas el método Step de la librería para posicionar el motor donde quieres, de los 4 pines, 2 de ellos se quedaban en HIGH. Después de muchas pruebas, poniendo los 4 pines a LOW antes de ejecutar la lectura del sensor SR04, me ha funcionado correctamente. Incluso he subido las rpm 100 y también funciona bien.