Robot utilizando motores pap y TB6600

International Español Hardware
1

Hola a todos..

Estoy teniendo dificultades para hacer correctamente el codigo para mi proyecto. La idea es que cuando detecte un obstaculo a 20cm, gire 90 grados y reempreanda la marcha, pero no se por que motivo el codigo del sensor de distancia y el codigo del servomotor que va moviendo el sensor de lado a lado crea algun tipo de conflicto ya que empiezan a hacer ruido los driver de los motores, pero estos no se mueven.. Si dejo solo el codigo de los motores pap si que giran sin problemas.. aunque no he sabido hacer la parte de que cuando detecte un objeto, gire 90 grados...

Este es el codigo que estoy utilizando de momento:

#include <Servo.h>                    
  Servo servo1;   

   int  PUL = 6 ;  // definir pulso pin 
   int  DIR = 5 ;  // definir Direction pin 
   int  ENA = 4 ;  // define Enable Pin 

   int  PUL2 = 12 ;  // definir pulso pin 
   int  DIR2 = 11 ;  // definir Direction pin 
   int  ENA2 = 10 ;  // define Enable Pin 
   
  #include "Ultrasonic.h"
  int trigPin = 7;
  int echoPin = 8;
  Ultrasonic ultrasonido(trigPin,echoPin); 
  int dis = 10;
 
  int pinServo = 9;    // Pin digital con PWM para el Servo
  int pulsoMin = 650;  // Pulso en us para girar un ángulo de 0º
  int pulsoMax = 2550; // Pulso en us para girar un ángulo de 180º
  int angulo = 0; // Variable para guardar el ángulo del servo
 
  void setup()
  {
    servo1.attach(pinServo, pulsoMin, pulsoMax);

    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT); 

    pinMode (PUL, OUTPUT); 
    pinMode (DIR, OUTPUT); 
    pinMode (ENA, OUTPUT);
    
    pinMode (PUL2, OUTPUT); 
    pinMode (DIR2, OUTPUT); 
    pinMode (ENA2, OUTPUT);
  }
 
  void loop()
  {   
    
    // El Servo avanza de 0º a 120º
    for(angulo = 0; angulo <= 120; angulo++)
    {
      // Avanza 1 grado cada 10 ms
      servo1.write(angulo);
      delay(15);
    }
   
    // El Servo retrocede de 120º a 0º
    for(angulo = 120; angulo >= 0; angulo--)
    {
      // Retrocede 1 grado cada 10 ms
      servo1.write(angulo);
      delay(15);
    }

    int distancia;
    distancia = ultrasonido.Ranging(CM);

    if (distancia < dis){
    digitalWrite(DIR,HIGH);
    digitalWrite(ENA,HIGH);
    digitalWrite(PUL,HIGH);
    delayMicroseconds(50);
    digitalWrite(PUL,LOW);
    delayMicroseconds(50);

    digitalWrite(DIR2,LOW);
    digitalWrite(ENA2,HIGH);
    digitalWrite(PUL2,HIGH);
    delayMicroseconds(50);
    digitalWrite(PUL2,LOW);
    delayMicroseconds(50);  
  
  } else {  
    
    digitalWrite(DIR,LOW);
    digitalWrite(ENA,HIGH);
    digitalWrite(PUL,HIGH);
    delayMicroseconds(50);
    digitalWrite(PUL,LOW);
    delayMicroseconds(50);

    digitalWrite(DIR2,HIGH);
    digitalWrite(ENA2,HIGH);
    digitalWrite(PUL2,HIGH);
    delayMicroseconds(50);
    digitalWrite(PUL2,LOW);
    delayMicroseconds(50);  
  }        
  
  }

Este es el diagrama que estoy utilizando para los motores pap y los driver TB6600

800×556 122 KB

Y unas imagenes del cacharro:

1032×774 162 KB

1032×774 174 KB

1032×774 117 KB

Gracias por la ayuda..

2

Muy bien presentado tu problema. El único detalle para la proxima cuando postees usa AUTO FORMATO (CONTROL+T) para que el código luzca perfecto.

No se comprende la lógica de tu código.

Dice mas o menos esto:

mueve el servo de 0 a 120 en pasos de 1 grado cada 15 mseg. Para que?
muevete de 120 a 0 en pasos de 1 grado cada 15 mseg. Para que x 2?
O sea que mueves el servo a de 0 a 120 y de 120 a 0 y no se que hace... porque el sonar no hace nada.

si lo que quieres es saber donde hay un obstáculo es otro tema.
Luego los dos códigos para manejar los motores al menos ponle un comentario para que entendamos que hace en cada caso?

3

surbyte:
Muy bien presentado tu problema. El único detalle para la proxima cuando postees usa AUTO FORMATO (CONTROL+T) para que el código luzca perfecto.

Ok.

surbyte:
No se comprende la lógica de tu código.

Dice mas o menos esto:

mueve el servo de 0 a 120 en pasos de 1 grado cada 15 mseg. Para que?
muevete de 120 a 0 en pasos de 1 grado cada 15 mseg. Para que x 2?
O sea que mueves el servo a de 0 a 120 y de 120 a 0 y no se que hace... porque el sonar no hace nada. si lo que quieres es saber donde hay un obstáculo es otro tema.

Esa parte del codigo es para que el servo mueva el sensor como si fuese un radar, de esa forma cubre mas espacio a la hora de detectar algun obstaculo. No es que quiera saber donde esta el obstaculo, lo que quiero es que cuando detecte un obstaculo se pare el robot, gire 90 grados sobre si mismo y vuelva a seguir adelante:

surbyte:
Luego los dos códigos para manejar los motores al menos ponle un comentario para que entendamos que hace en cada caso?

El codigo de los motores lo copie de internet pero solo es de prueba.. lo unico que hace es girar los dos motores pap a cierta velocidad, pero como no he podido solucionar el tema que comento en el primer post, lo he dejado asi.. No se, es muy raro porque si descomento la parte del sensor o del servo, los motores pap ya no se mueven.. ese es el principal problema ahora mismo..

Gracias!

4

Esta parte es para que actue como radar pero no tomas ninguna lectura del ultrasónico entonces?

Intenta hacer comentarios en tu código para que nosotros podamos entender las partes que lo conforman.

En cada paso de los for deberían haber lecturas del ultrasónico.

Prueba con este código a ver que tal va.

5

Te marco en negrita la parte donde toma la lectura el sensor. Dentro del "if distancia" es donde deberia ir el codigo de los motores para que pare cuando detecte un obstaculo gire 90 grados y continue, pero no hagas caso de ese codigo, es solo un codigo de prueba que copie de aqui:

#include <Servo.h>                    
  Servo servo1;   

   int  PUL = 6 ;  // definir pulso pin 
   int  DIR = 5 ;  // definir Direction pin 
   int  ENA = 4 ;  // define Enable Pin 

   int  PUL2 = 12 ;  // definir pulso pin 
   int  DIR2 = 11 ;  // definir Direction pin 
   int  ENA2 = 10 ;  // define Enable Pin 
   
  #include "Ultrasonic.h"
  int trigPin = 7;
  int echoPin = 8;
  Ultrasonic ultrasonido(trigPin,echoPin); 
  int dis = 10;
 
  int pinServo = 9;    // Pin digital con PWM para el Servo
  int pulsoMin = 650;  // Pulso en us para girar un ángulo de 0º
  int pulsoMax = 2550; // Pulso en us para girar un ángulo de 180º
  int angulo = 0; // Variable para guardar el ángulo del servo
 
  void setup()
  {
    servo1.attach(pinServo, pulsoMin, pulsoMax);

    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT); 

    pinMode (PUL, OUTPUT); 
    pinMode (DIR, OUTPUT); 
    pinMode (ENA, OUTPUT);
    
    pinMode (PUL2, OUTPUT); 
    pinMode (DIR2, OUTPUT); 
    pinMode (ENA2, OUTPUT);
  }
 
  void loop()
  {   
    
    // El Servo avanza de 0º a 120º
    for(angulo = 0; angulo <= 120; angulo++)
    {
      // Avanza 1 grado cada 10 ms
      servo1.write(angulo);
      delay(15);
    }
   
    // El Servo retrocede de 120º a 0º
    for(angulo = 120; angulo >= 0; angulo--)
    {
      // Retrocede 1 grado cada 10 ms
      servo1.write(angulo);
      delay(15);
    }

    [b]int distancia;
    distancia = ultrasonido.Ranging(CM);

    if (distancia < dis){[/b]

    digitalWrite(DIR,HIGH);
    digitalWrite(ENA,HIGH);
    digitalWrite(PUL,HIGH);
    delayMicroseconds(50);
    digitalWrite(PUL,LOW);
    delayMicroseconds(50);

    digitalWrite(DIR2,LOW);
    digitalWrite(ENA2,HIGH);
    digitalWrite(PUL2,HIGH);
    delayMicroseconds(50);
    digitalWrite(PUL2,LOW);
    delayMicroseconds(50);  
  
  } else {  
    
    digitalWrite(DIR,LOW);
    digitalWrite(ENA,HIGH);
    digitalWrite(PUL,HIGH);
    delayMicroseconds(50);
    digitalWrite(PUL,LOW);
    delayMicroseconds(50);

    digitalWrite(DIR2,HIGH);
    digitalWrite(ENA2,HIGH);
    digitalWrite(PUL2,HIGH);
    delayMicroseconds(50);
    digitalWrite(PUL2,LOW);
    delayMicroseconds(50);  
  }        
  
  }
6

El codigo para los motores puedo hacerlo yo con ensayo error.. el principal problema ahora mismo es que hay algun conflicto que hace que no se muevan los motores sin descomento el codigo tanto del servo como el del sensor.. si dejo solo esto si se meven los motores:

digitalWrite(DIR,HIGH);
    digitalWrite(ENA,HIGH);
    digitalWrite(PUL,HIGH);
    delayMicroseconds(50);
    digitalWrite(PUL,LOW);
    delayMicroseconds(50);

    digitalWrite(DIR2,LOW);
    digitalWrite(ENA2,HIGH);
    digitalWrite(PUL2,HIGH);
    delayMicroseconds(50);
    digitalWrite(PUL2,LOW);
    delayMicroseconds(50);
7

No hace nada de lo que imaginas.
Esto si lo hace, aunque no se como se comportará con el motor.

#include <Servo.h>                    
Servo servo1;   

int  PUL = 6 ;  // definir pulso pin 
int  DIR = 5 ;  // definir Direction pin 
int  ENA = 4 ;  // define Enable Pin 

int  PUL2 = 12 ;  // definir pulso pin 
int  DIR2 = 11 ;  // definir Direction pin 
int  ENA2 = 10 ;  // define Enable Pin 

#include "Ultrasonic.h"
int trigPin = 7;
int echoPin = 8;
Ultrasonic ultrasonido(trigPin,echoPin); 
int dis = 10;

int pinServo = 9;    // Pin digital con PWM para el Servo
int pulsoMin = 650;  // Pulso en us para girar un ángulo de 0º
int pulsoMax = 2550; // Pulso en us para girar un ángulo de 180º

int distancia;
int angulo = 0; // Variable para guardar el ángulo del servo
int desde = 0;
int hasta = 120;
int paso = 1;
	
 
 void setup() {
    servo1.attach(pinServo, pulsoMin, pulsoMax);

    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT); 

    pinMode (PUL, OUTPUT); 
    pinMode (DIR, OUTPUT); 
    pinMode (ENA, OUTPUT);
    
    pinMode (PUL2, OUTPUT); 
    pinMode (DIR2, OUTPUT); 
    pinMode (ENA2, OUTPUT);
}
 
void loop() {   

	// El Servo avanza de 0º a 120º
	for (angulo = desde; angulo <= hasta; angulo += paso) {
		// Avanza 1 grado cada 10 ms
		servo1.write(angulo);
		delay(15);
		distancia = ultrasonido.Ranging(CM);
		if (distancia < dis){
			adelantar();

		} else {  
			detener();
		}
		if (angulo >= 120) {
		    paso = -1;
		    desde = 120;
		    hasta = 0;
		}
		if (angulo == 0) {
		    paso = 1;
		    desde = 0;
		    hasta = 120;
		}
	}  
}

void adelantar() {
	digitalWrite(DIR,HIGH);
	digitalWrite(ENA,HIGH);
	digitalWrite(PUL,HIGH);
	delayMicroseconds(50);
	digitalWrite(PUL,LOW);
	delayMicroseconds(50);

	digitalWrite(DIR2,LOW);
	digitalWrite(ENA2,HIGH);
	digitalWrite(PUL2,HIGH);
	delayMicroseconds(50);
	digitalWrite(PUL2,LOW);
	delayMicroseconds(50);  
}

void detener() {
	digitalWrite(DIR,LOW);
	digitalWrite(ENA,HIGH);
	digitalWrite(PUL,HIGH);
	delayMicroseconds(50);
	digitalWrite(PUL,LOW);
	delayMicroseconds(50);

	digitalWrite(DIR2,HIGH);
	digitalWrite(ENA2,HIGH);
	digitalWrite(PUL2,HIGH);
	delayMicroseconds(50);
	digitalWrite(PUL2,LOW);
	delayMicroseconds(50);
}
8

Muchas gracias por el codigo, pero hace lo mismo de antes, los motores no se mueven y ahora el servo tampoco se mueve..

9

Como sigo sin entender y sigo sin ver comentarios sobre el funcionamiento d elos movimientos corrijo el tema del radar.

#include <Servo.h>                    
Servo servo1;   

int  PUL = 6 ;  // definir pulso pin 
int  DIR = 5 ;  // definir Direction pin 
int  ENA = 4 ;  // define Enable Pin 

int  PUL2 = 12 ;  // definir pulso pin 
int  DIR2 = 11 ;  // definir Direction pin 
int  ENA2 = 10 ;  // define Enable Pin 

#include "Ultrasonic.h"
int trigPin = 7;
int echoPin = 8;
Ultrasonic ultrasonido(trigPin,echoPin); 
int dis = 10;

int pinServo = 9;    // Pin digital con PWM para el Servo
int pulsoMin = 650;  // Pulso en us para girar un ángulo de 0º
int pulsoMax = 2550; // Pulso en us para girar un ángulo de 180º

int distancia;
int angulo = 0; // Variable para guardar el ángulo del servo
int desde = 0;
int hasta = 120;
int paso = 1;
	
 
 void setup() {
    servo1.attach(pinServo, pulsoMin, pulsoMax);
    Serial.begin(9600);

    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT); 

    pinMode (PUL, OUTPUT); 
    pinMode (DIR, OUTPUT); 
    pinMode (ENA, OUTPUT);
    
    pinMode (PUL2, OUTPUT); 
    pinMode (DIR2, OUTPUT); 
    pinMode (ENA2, OUTPUT);
}
 
void loop() {   
	// El Servo avanza de 0º a 120º
	angulo += paso;
	// Avanza 1 grado cada 10 ms
	servo1.write(angulo);
	Serial.print("angulo = "); 	Serial.println(angulo);
	delay(15);
	distancia = ultrasonido.Ranging(CM);
	if (distancia < dis){
		adelantar();
	} else {  
		detener();
	}
	if (angulo >= 120) {
	    paso = -1;
	    desde = 120;
	    hasta = 0;
	}
	if (angulo == 0) {
	    paso = 1;
	    desde = 0;
	    hasta = 120;
	}  
}

void adelantar() {
	digitalWrite(DIR,HIGH);
	digitalWrite(ENA,HIGH);
	digitalWrite(PUL,HIGH);
	delayMicroseconds(50);
	digitalWrite(PUL,LOW);
	delayMicroseconds(50);

	digitalWrite(DIR2,LOW);
	digitalWrite(ENA2,HIGH);
	digitalWrite(PUL2,HIGH);
	delayMicroseconds(50);
	digitalWrite(PUL2,LOW);
	delayMicroseconds(50);  
}

void detener() {
	digitalWrite(DIR,LOW);
	digitalWrite(ENA,HIGH);
	digitalWrite(PUL,HIGH);
	delayMicroseconds(50);
	digitalWrite(PUL,LOW);
	delayMicroseconds(50);

	digitalWrite(DIR2,HIGH);
	digitalWrite(ENA2,HIGH);
	digitalWrite(PUL2,HIGH);
	delayMicroseconds(50);
	digitalWrite(PUL2,LOW);
	delayMicroseconds(50);
}

Esto ahora si mueve de 0 a 120 y de 120 a 0 y debería tomar lecturas del radar. Si no lo hace es porque tu sensor no funciona o de lo contrario algo no esta haciendo bien.

Debería a cada grado tomar una lectura del sensor y en funcion de esa lectura se muevo o gira.. no entiendo que debe hacer.

10

Agradecer a surbyte su inestimable ayuda. Al final lo he solucionado utilizando dos placas Arduino UNO ya que no he sido capaz de hacer funcionar el servo, el sensor de distancia y los motores pap en el mismo codigo.. pero bueno, ya esta solucionado..

Dejo los codigos por si a alguien le puede servir:

Codigo de los motores en Placa 1:

//Motor1
int  PUL = 4 ;  // definir pulso pin
int  DIR = 3 ;  // definir Direction pin
int  ENA = 2 ;  // define Enable Pin

//Motor2
int  PUL2 = 7 ;  // definir pulso pin
int  DIR2 = 6 ;  // definir Direction pin
int  ENA2 = 5 ;  // define Enable Pin

int ledrele = 11;

void setup() {

  pinMode (PUL, OUTPUT);
  pinMode (DIR, OUTPUT);
  pinMode (ENA, OUTPUT);

  pinMode (PUL2, OUTPUT);
  pinMode (DIR2, OUTPUT);
  pinMode (ENA2, OUTPUT);

  pinMode(ledrele, OUTPUT);
  digitalWrite(ledrele, LOW);

//  Serial.begin(9600);

}

void loop() {

  int sensorValue = analogRead(A0);
//  Serial.println(sensorValue);

  if (sensorValue < 10) {
    detener();
  } else {
   adelantar();
  }

}

void adelantar() {
  digitalWrite(DIR, HIGH);
  digitalWrite(ENA, HIGH);
  digitalWrite(PUL, HIGH);
  delayMicroseconds(50);
  digitalWrite(PUL, LOW);
  delayMicroseconds(50);

  digitalWrite(DIR2, HIGH);
  digitalWrite(ENA2, HIGH);
  digitalWrite(PUL2, HIGH);
  delayMicroseconds(50);
  digitalWrite(PUL2, LOW);
  delayMicroseconds(50);
}

void detener() {
  digitalWrite(DIR, LOW);
  digitalWrite(ENA, HIGH);
  digitalWrite(PUL, HIGH);
  delayMicroseconds(50);
  digitalWrite(PUL, LOW);
  delayMicroseconds(50);

  digitalWrite(DIR2, HIGH);
  digitalWrite(ENA2, HIGH);
  digitalWrite(PUL2, HIGH);
  delayMicroseconds(50);
  digitalWrite(PUL2, LOW);
  delayMicroseconds(50);
}

Codigo del servo y sensor de distancia en Placa 2:

#include <Servo.h>
Servo servo1;

#include "Ultrasonic.h"
int trigPin = 8;
int echoPin = 9;
Ultrasonic ultrasonido(trigPin, echoPin);
int dis = 20;
int distancia;

int pinServo = 10;    // Pin digital con PWM para el Servo
int pulsoMin = 650;  // Pulso en us para girar un ángulo de 0º
int pulsoMax = 2550; // Pulso en us para girar un ángulo de 180º

int angulo = 0; // Variable para guardar el ángulo del servo
int desde = 0;
int hasta = 120;
int paso = 1;

int PinArd2 = 3;

void setup() {

  servo1.attach(pinServo);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(PinArd2, OUTPUT);
  digitalWrite(PinArd2, HIGH);

}

void loop() {

  angulo += paso;
  servo1.write(angulo);
  delay(5);

  if (angulo >= 120) {
    paso = -1;
    desde = 120;
    hasta = 0;
  }
  if (angulo == 0) {
    paso = 1;
    desde = 0;
    hasta = 120;
  }

  distancia = ultrasonido.Ranging(CM);
  if (distancia < dis) {
    digitalWrite(PinArd2, HIGH);
  } else {
    digitalWrite(PinArd2, LOW);
  }

}

Lo que hace el codigo del servo y el sensor de distancia es poner en alto el pin 3 cuando detecta un obstaculo a 20 cm, luego el codigo de los motores recibe el valor en el pin A0 con analogRead y hace avanzar o girar el robot segun el estado.

Seguramente habra otra forma mejor de hacerlo, pero despues de algunas horas intentadolo, esto es lo mejor que he conseguido..

Aqui se puede ver el robot funcionando:

Video en Youtube

Saludos.

11

Siempre he querido hacer algo mas inteligente que ese radar que gira loco sin cesar cuando podria simplemente moverse de acuerdo a donde ha encontrado el obstáculo y luego tener una búsqueda mas inteligente, por supuesto que mirando adelante pero en menos grados, solo resguardando el ancho del robot.
En tu caso el radar se mueve demasiado a posiciones que el robot no llegaría jamás. De hecho muchos usan 3 sensores fijos.
Mi punto de vista es que obviamente debe resguardar la posicon hacia adelante o sea no encontrar obstaculo adelante. Dicha posicon es conocida. Luego barrer los grados que corresponden al ancho del robot y el resto descartarlo.
Tal vez 45 grados respecto de la transversal serían suficientes y eso restringe a la mitad el tiempo de rastreo.
Incluso si llevara un array con las distancias.... solo prestar atención a aque rango que se vuelve peligroso paso a paso. Lo que agrega mas cómputo pero se puede hacer con eficiencia.

Es una idea que podría funcionar.