Pages: [1]   Go Down
Author Topic: Problema con encoders [Solucionado]  (Read 1265 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 25
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
//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
« Last Edit: February 08, 2013, 09:39:06 am by Wk3 » Logged

Castellon, España
Offline Offline
Sr. Member
****
Karma: 5
Posts: 330
OR ganizando
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged


Offline Offline
Newbie
*
Karma: 0
Posts: 25
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 25
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Solucionado, muchas gracias josemanu
Logged

Castellon, España
Offline Offline
Sr. Member
****
Karma: 5
Posts: 330
OR ganizando
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

No hay de que, ha sido un placer ser de ayuda.
Logged


Offline Offline
Newbie
*
Karma: 0
Posts: 2
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Funciono?
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 25
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Pages: [1]   Go Up
Jump to: