Buenas a todos,
Tengo construido un pequeño robot de dos ruedas, con un detector de proximidad por ultrasonidos y unos encoders ( dfrobot SEN0038) en cada motor, todo funciona bien pero los encoders no cuentan bien los giros de los motores cuenta las primeras vueltas pero luego se para, después de darle mil vueltas aún no he encontrado que problema puede tener. Estan con attachInterrupt.
El funcionamiento es basico, se mueve hacia delante hasta que encuentra un obstaculo y mira a ambos lados para ver por donde puede ir.
Os dejo el código pero creo que no tiene problema.
//Librerias
#include <Servo.h>
//Declaración E/S
int EM1 = 0; //Encoder motor 1 (Izquierda) (Entrada 2)
int EM2 = 1; //Encoder motor 2 (Derecha) (Entrada 3)
int M2D = 4; //Motor 2 control de dirección (Derecha)
int M1P = 6; //Motor 1 control PWM
int M2P = 5; //Motor 2 control PWM
int M1D = 7; //Motor 1 control de dirección (Izquierda)
int SU = 8; //Sensor de distancia por ultrasonidos
Servo servo1; //Servomotor
int BI = 10; //Boton de inicio
int BP = 11; //Boton de paro
//Declaración variables
volatile long CRI = 0; //Contador rueda izquierda
volatile long CRD = 0; //Contador rueda derecha
int EBI = 0; //Estado boton inicio
int velocidad = 200; //Velocidad de los motores
long dist = 0; //Distancia del robot (cm)
long duracion = 0; //Duración del recorrido del ping (microsegundos)
int VEA = 0; //variable encendido/apagado
int EBP = 0; //Estado boton de paro
long distizq = 0; // Distancia del robot (cm) a su izquierda
long distder = 0; // Distancia del robot (cm) a su derecha
byte giro = 0; //Variable para saber por donde girar
byte caso = 0; //Para seleccionar el caso de giro
byte redireccionado= 0;
void setup() {
pinMode(M1D, OUTPUT);
pinMode(M2D, OUTPUT);
pinMode(2, INPUT);
digitalWrite(2, HIGH);
pinMode(3, INPUT);
digitalWrite(3, HIGH);
servo1.attach(9);
servo1.write(100);
pinMode(BI, INPUT);
pinMode(BP, INPUT);
attachInterrupt(EM1, ContadorRuedaIzq, CHANGE); //Cada vez que cambie el valor ira a la función
attachInterrupt(EM2, ContadorRuedaDer, CHANGE); //Cada vez que cambie el valor ira a la función
}
void loop() {
EBP=digitalRead(BP);
EBI=digitalRead(BI);
if (EBI == HIGH){
VEA = 1;
}
else if(EBP == HIGH) {
VEA = 0;
}
dist = sensorultrasonidos(); //Llama a la función para saber la distancia
if (VEA == 1 && dist > 15){ //Si la variable encendido/apagado tiene valor high y hay distancia suficiente
avanzar(); //Ir a la función avanzar
}
if (VEA == 1 && dist < 15){ //Si la variable encendido/apagado tiene valor high y no hay distancia suficiente
if (redireccionado == 0){
caso = comprobarbandas();
}
switch(caso) {
case 1:
derecha();
break;
case 2:
izquierda();
break;
case 3:
giro180();
break;
}
}
}
void avanzar () {
redireccionado= 0;
while(dist >15){
dist = sensorultrasonidos(); //Llama a la función para saber la distancia
digitalWrite(M1D,LOW);
digitalWrite(M2D, LOW);
analogWrite(M1P, velocidad);
analogWrite(M2P, velocidad);
}
frenar();
}
void ContadorRuedaIzq() {
CRI++; //Suma uno al contador de rueda izquierda
}
void ContadorRuedaDer() {
CRD++; //Suma uno al contador de rueda izquierda
}
int sensorultrasonidos() { //Función para medir la distancia con el sensor de ultrasonidos (cm)
pinMode(SU, OUTPUT); //Configuramos el sensor de ultrasonidos como salida
digitalWrite(SU, LOW); //Hacemos ping LOW-HIGH-LOW
delayMicroseconds(2);
digitalWrite(SU, HIGH);
delayMicroseconds(15);
digitalWrite(SU, LOW);
delayMicroseconds(20);
pinMode(SU, INPUT); //Configuramos el sensor de ultrasonidos como entrada
duracion = pulseIn(SU, HIGH); //Leemos la duración del pulso
delay(100);
return duracion / 29 / 2; // Conversión de microsegundos a la distancia cm (velocidad del sonido 340m/s o 29 microsegundos por centimetro y son ida y vuelta /2)
}
void frenar (){
digitalWrite(M1D,HIGH);
digitalWrite(M2D, HIGH);
analogWrite(M1P, 0);
analogWrite(M2P, 0);
}
void derecha() {
while (CRI <= 15 && CRD <=15 ){
digitalWrite(M1D,LOW);
digitalWrite(M2D, HIGH);
analogWrite(M1P, velocidad);
analogWrite(M2P, velocidad);
}
frenar();
}
void izquierda() {
while (CRI <= 15 && CRD <=15 ){
digitalWrite(M1D,HIGH);
digitalWrite(M2D, LOW);
analogWrite(M1P, velocidad);
analogWrite(M2P, velocidad);
}
frenar();
}
void giro180() {
while (CRI <= 30 && CRD <=30 ){
digitalWrite(M1D,LOW);
digitalWrite(M2D, HIGH);
analogWrite(M1P, velocidad);
analogWrite(M2P, velocidad);
}
frenar();
}
int comprobarbandas() {
CRI = 0;
CRD = 0;
servo1.write(10);
delay(500);
distder = sensorultrasonidos();
servo1.write(170);
delay(500);
distizq = sensorultrasonidos();
servo1.write(100);
delay(500);
redireccionado = 1;
if (distder > distizq) {
giro = 1;
}
else if(distizq > distder) {
giro = 2;
}
else if(distizq = distder) {
giro = 3;
}
return giro;
}
Gracias de antemano