Go Down

Topic: Problema con encoders [Solucionado] (Read 2031 times) previous topic - next topic

Wk3

Feb 06, 2013, 07:54 pm Last Edit: Feb 08, 2013, 03:39 pm by Wk3 Reason: 1
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.

Code: [Select]

//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

josemanu

En teoría las interrupciones detienen el código que se esté utilizando para llamar a la función correspondiente. No se por que solo te funcionan durante un rato, pero es como matar moscas a cañonazos, al menos en mi opinión las interrupciones se deben dejar para casos extremos.

Te dejo un código que te puede ir bien de uno de mis tutoriales, detecta cambios de estado (por lo que te puede ir bien con los encoders) y lleva un contador dentro.

Espero te sea de utilidad.
http://www.ardumania.es/

Iniciación

Wk3

Hola josemanu,

Gracias por tu respuesta, el código de cambio de estado esta muy bien, probare los encoders sin interrupciones y probando tu código.

Wk3

Solucionado, muchas gracias josemanu

josemanu

No hay de que, ha sido un placer ser de ayuda.
http://www.ardumania.es/

Iniciación


Wk3

Si Abdielll, el problema era que el programa no funcionaba bien con los encoders en interrupción al ponerlos normal funcionan de maravilla

Go Up