Robot con arduino casero – Software

En esta segunda parte del artículo del robot casero nos centraremos en la parte de software y la demostración del funcionamiento del robot. (Ver primera parte Hardware)

Robot arduino

Para hacer un programa un poco largo  lo recomendable es plantear el algoritmo del funcionamiento, este nos ayudara mucho a la hora de programar. Un algoritmo es un conjunto de instrucciones que permite realizar una actividad.

Algoritmo:

Algoritmo robot

El programa se divide en el programa principal y luego varios subprogramas, en el principal es donde se encuentra los botones de inicio y paro y las llamas a los subprogramas de medición de distancia, avance y los diferentes giros dependiendo de las distancias laterales.

Programa:

//Librerias
#include <Servo.h>

//Declaración E/S
int EM1 = 2; //Encoder motor 1 (Izquierda)
int EM2 = 3; //Encoder motor 2 (Derecha)
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
int CRI = 0; //Contador rueda izquierda
int 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
int estadoAnterior = 0; //variables para cambio de estado del encoder izquierdo
int estadoActual;       //variables para cambio de estado del encoder izquierdo
int estadoAnterior1 = 0; //variables para cambio de estado del encoder derecho
int estadoActual1;      //variables para cambio de estado del encoder izquierdo

void setup() {
  pinMode(M1D, OUTPUT);   
  pinMode(M2D, OUTPUT);
  pinMode(EM1, INPUT);         //encoder 1 como entrada
  digitalWrite(EM1, HIGH);    //resistencia pull-up para encoder
  pinMode(EM2, INPUT);          //encoder 2 como entrada
  digitalWrite(EM2, HIGH);    //resistencia pull-up para encoder
  servo1.attach(9);  //inicializamos servo
  servo1.write(100);   //lo colocamos en una posición media
  pinMode(BI, INPUT);  //Boton de inicio como entrada
  pinMode(BP, INPUT);  //Boton de paro como entrada
}

void loop() {
  EBP=digitalRead(BP);    //lee los estados del boton
  EBI=digitalRead(BI);    //lee los estados del boton
  if (EBI == HIGH){        //si el estado del boton de inicio esta on
    VEA = 1;              //variable encendido/apagado = 1

  }
  else if(EBP == HIGH) {    //si esta el de paro on
    VEA = 0;               //variable encendido/apagado = 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
    caso = comprobarbandas();  //Comprobamos bandas y depende el caso ira a una función determinada
    switch(caso) {
      case 1:
      derecha();
      break;
      case 2:
      izquierda();
      break;
      case 3:
      giro180();
      break;
    }
  }
}
void avanzar () {  //Función de avanzar hasta que haya una distancia de 15cm con el objeto en frente
    digitalWrite(M1D,LOW); 
    digitalWrite(M2D, LOW);     
    analogWrite(M1P, velocidad); 
    analogWrite(M2P, velocidad);
  while(dist >15){
    dist = sensorultrasonidos(); //Llama a la función para saber la distancia
  }
  frenar();
}

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(50);
  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 (){    //funcion para frenar el robot
    digitalWrite(M1D,HIGH); 
    digitalWrite(M2D, HIGH);     
    analogWrite(M1P, 0); 
    analogWrite(M2P, 0);

void derecha() {    //función para girar a la derecha teniendo en cuenta la rotacion de los encoders
    digitalWrite(M1D,LOW); 
    digitalWrite(M2D, HIGH);     
    analogWrite(M1P, velocidad); 
    analogWrite(M2P, velocidad); 
  while (CRI <= 15 && CRD <=15 ){
  contador_izq();
  contador_der(); 
  }
  frenar();
}

void izquierda() {    //función para girar a la izquierda teniendo en cuenta la rotacion de los encoders
    digitalWrite(M1D,HIGH); 
    digitalWrite(M2D, LOW);   
    analogWrite(M1P, velocidad); 
    analogWrite(M2P, velocidad);   
  while (CRI <= 15 && CRD <=15 ){
  contador_izq();
  contador_der();
  }
  frenar();
}

void giro180() {    //función para girar 180º teniendo en cuenta la rotacion de los encoders
    digitalWrite(M1D,LOW); 
    digitalWrite(M2D, HIGH);     
    analogWrite(M1P, velocidad); 
    analogWrite(M2P, velocidad);
  while (CRI <= 30 && CRD <=30 ){
  contador_izq();
  contador_der();  
  }
  frenar();
}

void contador_izq(){    //Contaje de los estados del encoder izquierdo
estadoActual = digitalRead(EM1);
  if (estadoAnterior != estadoActual)  // ha habido un cambio de estado
  {
    CRI++;                          // cuenta los cambios de estado
    estadoAnterior = estadoActual;
  }
}

void contador_der(){    //Contaje de los estados del encoder derecho
estadoActual1 = digitalRead(EM1);
  if (estadoAnterior1 != estadoActual1)  // ha habido un cambio de estado
  {
    CRD++;                          // cuenta los cambios de estado
    estadoAnterior1 = estadoActual1;
  }
}

int comprobarbandas() {    //Función para comprovar bandas, mide la distancia de la izquierda y la derecha
    CRI = 0;
    CRD = 0;
    servo1.write(5);
    delay(500);
    distizq = sensorultrasonidos();
    servo1.write(175);
    delay(500);
    distder = sensorultrasonidos();
    servo1.write(100);
    delay(500);
    if (distder >= distizq && distder > 15) {    //si la distancia derecha es mayor o igual a la distancia izquierda, girara a la derecha
      giro = 1;
    }
    else if(distizq >= distder && distizq > 15) {  //si la distancia izquierda es mayor o igual a la distancia derecha, girara a la izquierda
      giro = 2;
    }
    else if(distizq < 15 && distder < 15) {
      giro = 3;
    }
    return giro;
    }

Demostración:

Primera parte Robot con arduino casero – hardware


Un comentario

  1.   MAURO PAGLIORITI dijo

    ME PODRIAS PASAR CONEXION DE SENSOR PROXIMIDAD ESTOY HACIENDO ALGO PARECIDO PERO CON MANDO POR WIFI CON DOS SERVOS DOS MOTORES DE RUEDAS PERO SE ME COMPLICABA PARA MANEJAR LA PROGRAMACION JUNTA SI PODES PASARME ESO T LO AGRADESCO SI NO MOLESTO IGUAL GRACIAS ME AYUDASTE MUCHO CON EL CODE ME AYUDASTES A TENER IDEA DE COMO JUNTAR TODO

Deja un comentario

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *