Servomotor desconecta el modulo de bluetooth

International Español Software
1

Hola buenas noches a tod@s,
Tengo el siguente codigo para manejar un cochecito rc reciclado, un Arduino UNO, un sensor Ultrasonido HC-SR04, un modulo de Bluetooth HC-06 conectado a una baquelita diseñada por mi, con un puente H realizado con TIPs 122/127, y un L293B, con el puente H manejo el motor de traccion de 19V 1.6A, con el L293B el motor de direccion 12V 0.7A.
Uso un servomotor para mover el sensor de ultrasonidos a derecha y a izquierda (para decidir si encuentra un obstaculo, si moverse a derecha o a izquierda)

#include <Servo.h>
#include <SoftwareSerial.h>

Servo ServoUS;
int posServoUS;

SoftwareSerial BT(0, 1); // RX | TX

long distF;
long distI;
long distD;
long tiempo;
int velT;
int velD;
int recibeBT;
boolean Estado;

unsigned long prevMillisRxBT = 0;
unsigned long prevMillisRxUS = 0;
const long rxBT = 500;
const long rxUS = 500;

int pinMT_A = 2; //Pin MOTOR DE TRACCION
int pinMT_B = 3; //Pin MOTOR DE TRACCION
int pinMD_A = 4; //Pin MOTOR DE DIRECCION
int pinMD_B = 5; //Pin MOTOR DE DIRECCION
int pinMD_C = 6; //Pin MOTOR AUX
int pinMD_D = 7; //Pin MOTOR AUX
int pintECH = 11; //Pin ECHO Ultrasonico
int pinTRG = 12; //Pin TRIGGER Ultrasonico

int pinRtnD = A0; //Pin Retorno Derecha
int pinRtnI = A1; //Pin Retorno Izquierda

int pwmMT_A; //Variable PWM MOTOR TRACCION
int pwmMT_B; //Variable PWM MOTOR TRACCION
int pwmMD_A; //Variable PWM MOTOR DIRECCION
int pwmMD_B; //Variable PWM MOTOR DIRECCION
int pwmMD_C; //Variable PWM MOTOR AUX
int pwmMD_D; //Variable PWM MOTOR AUX
int rtnD; //Variable RETORNO DERECHA
int rtnI; //Variable RETORNO IZQUIERDA

void setup()
{
  BT.begin(9600);
  Serial.begin(9600);
  ServoUS.attach(9);
  posServoUS = 90;
  pinMode(0, OUTPUT);
  pinMode(1, INPUT);
  pinMode(pinMT_A, OUTPUT);
  pinMode(pinMT_B, OUTPUT);
  pinMode(pinMD_A, OUTPUT);
  pinMode(pinMD_B, OUTPUT);
  pinMode(pinMD_C, OUTPUT);
  pinMode(pinMD_D, OUTPUT);
  pinMode(pinRtnD, INPUT);
  pinMode(pinRtnI, INPUT);
  pinMode(pintECH, INPUT);
  pinMode(pinTRG, OUTPUT);
}

void loop()
{ 
  if(Estado = HIGH && distF > 20)
  {
    Serial.print("Resultado: ");
    Serial.println(Estado);
    Avanzar();
  }
  else
  {
    Serial.print("Resultado: ");
    Serial.println(Estado);
    Detener();
    Explorar();
    if(distD < distI)
    {
      giroI();   
      Retroceder();
      delay(750);
      Detener();
    }
    else
    {
      giroD();   
      Retroceder();
      delay(750);
      Detener();
    }
  }
  
  unsigned long currentMillis = millis();
  
  if(currentMillis - prevMillisRxBT >= rxBT)
  {
    prevMillisRxBT = currentMillis;
    if (BT.available())
    {
      recibeBT = BT.read();
      switch (recibeBT)
      {
        case '0':
          Estado = LOW;
          Serial.print("Recibido: ");
          Serial.println(Estado);
        case '1':
          Estado = HIGH;
          Serial.print("Recibido: ");
          Serial.println(Estado);
        default:
          Serial.println("Sin datos recibidos");
      }
    }
    BT.flush();
  }
  
  if(currentMillis - prevMillisRxUS >= rxUS)
  {
    prevMillisRxUS = currentMillis;
    digitalWrite(pinTRG,LOW);
    delayMicroseconds(5);
    digitalWrite(pinTRG, HIGH);
    delayMicroseconds(10);
    tiempo = pulseIn(pintECH, HIGH);
    distF = int(0.017*tiempo);
    Serial.print("Distancia: ");
    Serial.println(distF);
  }
}

void Avanzar()
{
  pwmMT_A = map(velT, 0, 1023, 255, 0);
  pwmMT_B = map(velT, 0, 1023, 0, 255);
  analogWrite(pinMT_A,pwmMT_A);
  analogWrite(pinMT_B,pwmMT_B);
}

void Retroceder()
{
  pwmMT_A = map(velT, 0, 1023, 0, 255);
  pwmMT_B = map(velT, 0, 1023, 255, 0);
  analogWrite(pinMT_A,pwmMT_A);
  analogWrite(pinMT_B,pwmMT_B);
}

void giroD()
{  
  rtnD = analogRead(pinRtnD);
  Serial.print("Lectura A0: ");
  Serial.println(rtnD);
  if(rtnD < 380)
  {
    pwmMD_A = map(velD, 0, 1023, 255, 0);
    pwmMD_B = map(velD, 0, 1023, 0, 255);
    analogWrite(pinMD_A,pwmMD_A);
    analogWrite(pinMD_B,pwmMD_B);
  }
}

void giroI()
{
  rtnI = analogRead(pinRtnI);
  Serial.print("Lectura A1: ");
  Serial.println(rtnI);
  if(rtnI < 380)
  {
    pwmMD_A = map(velD, 0, 1023, 0, 255);
    pwmMD_B = map(velD, 0, 1023, 255, 0);
    analogWrite(pinMD_A,pwmMD_A);
    analogWrite(pinMD_B,pwmMD_B);
  }
}

void Detener()
{
  analogWrite(pinMT_A,LOW);
  analogWrite(pinMT_B,LOW);
  analogWrite(pinMD_A,LOW);
  analogWrite(pinMD_B,LOW);
}

void Explorar()
{
  posServoUS = 0;
  delay(250);
  digitalWrite(pinTRG,LOW);
  delayMicroseconds(5);
  digitalWrite(pinTRG, HIGH);
  delayMicroseconds(10);
  tiempo = pulseIn(pintECH, HIGH);
  distD = int(0.017*tiempo);
  Serial.print("Distancia a derecha: ");
  Serial.println(distD);
  posServoUS = 180;
  delay(250);
  digitalWrite(pinTRG,LOW);
  delayMicroseconds(5);
  digitalWrite(pinTRG, HIGH);
  delayMicroseconds(10);
  tiempo = pulseIn(pintECH, HIGH);
  distI = int(0.017*tiempo);
  Serial.print("Distancia a izquierda: ");
  Serial.println(distI);
  posServoUS = 90;
  delay(250);  
}

Mi problema está en que el servomotor se mueve aleatoriamente y el bluetooth se desconecta solo; he revisado las conexiones, las he fijado con silicona caliente (vamos que no se mueven) y sigue igual...
He revisado mil veces el código y no se si estoy usando bien millis() o es otro el error...
Tengo la sensacion que el servo interfiere en el bluetooth porque se desconecta cuando el servo comienza a moverse... o por lo menos esa es mi sensación.

Les agradezco si pueden revisar mi codigo, por si ven algun error o incongruencia.

Muchisimas gracias a tod@s

1 Like