Brazo robotico jugando trique

Publicado el: 21 de julio de 2019
Intermedio
Acerca de este proyecto
Brazo robótico que a través de arduino y visual basic juega contra el usuario el famoso juego de tres en línea o trique.
robot juego
Publicidad
DETALLES

Quiero compartir este proyecto realizado de un brazo robótico que juega el famoso juego de triqui como es llamado en Colombia, tatetí en Argentina tres en línea en España y juego del gato en otros países, contra un humano.

El brazo es controlado por un arduino uno el cual recibe comandos de un programa en visual basic a través del puerto serial.

LISTA DE MATERIALES


  • 1 Brazo robótico con pinza.
  • Camara web de buena resolución.
  • Arduino uno o equivalente.
  • Tablero de 3 x 3 cuadrados.
  • Piezas o fichas rojas y blancas.

FUNCIONAMIENTO

Las fichas de color rojo son para el jugador humano y las de color blanco son las fichas del computador, el jugador es quien tiene el primer turno.

La cámara web conectada al computador captura la imagen del tablero de 3×3 y esta imagen es procesada por el programa en visual basic net, que hace uso de la librería web cam, ( en el enlace se incluye la librería ). La imagen capturada se divide en nueve cuadros iguales de 60 pixeles y luego es recorrida por un bucle para determinar la cantidad de pixeles rojos que contiene cada cuadro. Si el pixel es de color rojo se marca con un punto del mismo color y la imagen es pasada a un control picture.

Luego en la imagen del control picture también se recorre cada cuadro para contar la cantidad de pixeles rojos, si la cantidad de pixeles rojos de cada cuadro es igual o superior al umbral establecido, se asume que en esa posición hay una ficha de color rojo que corresponde al movimiento del jugador humano.

Después que el jugador haya colocado su ficha, presiona un botón para indicarle al robot que es su turno de jugar. Este botón llama a un procedimiento que es el encargado de recorrer los pixeles mencionados en el párrafo anterior, y de llamar a una función encargada de decidir en que posición jugar el computador, gracias a un algoritmo usado en juegos de turnos de dos personas llamado minimax. Después de haber escogido en que posición jugar, se envía a través del puerto serial al Arduino un string con el número de la posición escogida por el algoritmo y el arduino mueve el brazo hacia dicha posición.

A continuación se muestra el video del proyecto en funcionamiento:


Podemos dividir el proyecto en cuatro partes:

  1. El brazo robótico.
  2. La captura de la imagen.
  3. La lógica del juego.
  4. Movimiento del brazo.

1 - EL BRAZO ROBÓTICO.


l brazo robótico utilizado en este proyecto fue comprado el kit para armar, pero si desea puedes armarte tu propio brazo. Está construido en acrílico de alta resistencia ,tiene 3 + 1 grados de libertad y contiene los siguientes servomotores:

  • Base (servomotor futaba S3003 con un rango de movimiento de 0° a 180°)
  • Hombro (servomotor futaba S3003 con un rango de movimiento de 0° a 180°)
  • Muñeca (servomotor futaba S3003 con un rango de movimiento de 0° a 180°)
  • Pinza (servomotor tower pro Sg90 con un rango de movimiento de 60° a 120°)

Brazo robotico

El servomotor de la base se conecta al pin número 5 del arduino.

El servomotor del hombro se conecta al pin número 6 del arduino.

El servomotor de la muñeca  se conecta al pin número 9 del arduino.

El servomotor de la pinza se conecta al pin número 10 del arduino.

Diagrama


2 - CAPTURA DE LA IMAGEN.


Para el proceso de captura de la imagen en el programa de visual basic, se usa la librería web cam; esta se encarga del manejo de la web cam conectada a nuestro computador. Al agregar la referencia a esta librería nos inserta un control en el formulario donde se mostrará la imagen que visualiza la cámara. Un control timer se encuentra monitoreando cada 50 ms el tablero cada 5 pixeles ( se hace un escaneo de cada 5 pixeles con el fin de reducir el tiempo de búsqueda ), se obtiene el color del pixel examinado y si corresponde a un color rojo, este píxel se marca con un punto del mismo color.

Camara web

Juego


Después que el jugador haya colocado su ficha de color rojo en la posición deseada, se presiona un botón para indicarle al robot que es su turno de jugar. En este proceso, se transfiere la imagen del control web Cam hacia un control picture, se divide la imagen en 9 cuadros y a través de un ciclo For se realiza la búsqueda de píxeles de color rojo, cada vez que se encuentre un pixel de color rojo se incrementa un contador que lleva la cuenta de la cantidad de pixeles rojos hallados en ese cuadro. Si la cantidad de pixeles rojos es igual o mayor al umbral establecido ( en este proyecto se compara con el número 75 ), entonces se asume que en ese cuadro se encuentra una ficha de color rojo, y se marca dicho cuadro con una “x” en la grilla de juego.

Cuando la grilla detecta que el jugador ha movido su ficha, se ejecuta una función que es la encargada de decidir en que posición debe jugar el robot. Esta lógica de juego será explicada en el próximo bloque.


3 - LA LÓGICA DEL JUEGO.


El programa utiliza un algoritmo llamado minimax, el cual es un algoritmo utilizado en lógica de juegos especialmente en aquellos de turnos de dos jugadores. De manera general, consiste en recorrer todo el árbol de soluciones del juego a partir de un estado dado, es decir, según las casillas que ya han sido rellenadas. Por tanto, minimax se ejecutará cada vez que le toque mover a la IA. Si desea aprender más sobre el algoritmo minimax recomienda ler la información albergada en wikipedia.

Como se mencionaba en el párrafo anterior, cuando la grilla detecta que se marcó con una “x” alguna posición, se activa la inteligencia artificial del juego, el algoritmo minimax selecciona el mejor movimiento para el robot, tratando en todo lo posible de evitar que el jugador humano sea el ganador. Al habr seleccionado su posición, marca el cuadro seleccionado en la grilla con una “o“, al tiempo que envía a través del puerto serial al arduino un String con la posición seleccionada por el algoritmo.


Logica del juego


Descargue el código fuente del programa en visual basic haciendo clic aca.

4- MOVIMIENTO DEL BRAZO.


Cuando el arduino recibe a través del puerto serial la posición escogida por el computador para jugar, dependiendo de la posición se ejecuta un procedimiento encargado de mover el servo hacia la posición especificada. Como son nueve posiciones, son nueve procedimientos, cada uno con valores de ángulos diferentes según la posición.

Puede descargar el código fuente del programa en arduino haciendo clic acá.


Si tienes dudas, sugerencias o inquietudes, dejalas en los comentarios.






CODIFICACIÓN
Codigo arduino-
Este es el código que se carga al arduino.
#include <Servo.h>
int i,j;

Servo base;
Servo hombro;
Servo codo;
Servo pinza;

int angulo=0;
char dato;

int anguloBase;
int anguloHombro;
int anguloCodo;
int anguloPinza;
int anguloAgarrePinza;


  void setup() 
  {
    Serial.begin(9600);
    
     
    base.attach(5);
    hombro.attach(6);
    codo.attach(9);
    pinza.attach(10);

    anguloBase=90;
    anguloHombro=90;
    anguloCodo=90;
    anguloPinza=70;
    anguloAgarrePinza=130;
    
    
    base.write(90);
    delay(300);
    hombro.write(90);
    delay(300);
    codo.write(90);
    delay(300);
    pinza.write(70);
    delay(300);
    
    
  }
void girarBaseDerecha()
{
  anguloBase=anguloBase+2;
  base.write(anguloBase);
}

void girarBaseIzquierda()
{
  anguloBase = anguloBase-2;
  base.write(anguloBase);
}

void subirHombro()
{
  anguloHombro=anguloHombro-2;
  hombro.write(anguloHombro);
  Serial.println(anguloHombro);  
}
void bajarHombro()
{
  anguloHombro=anguloHombro+2;
  hombro.write(anguloHombro);
  Serial.println(anguloHombro);  
}

void levantarCodo()
{
  anguloCodo=anguloCodo+2;
  codo.write(anguloCodo);
}

void bajarCodo()
{
  anguloCodo=anguloCodo-2;
  codo.write(anguloCodo);
}

void abrirPinza()
{
  anguloPinza=anguloPinza-5;
  pinza.write(anguloPinza);
}

void cerrarPinza()
{
  anguloPinza=anguloPinza+5;
  pinza.write(anguloPinza);
}

void posicionInicial()
{
  for(i=90;i<=180;i++)
  {
    base.write(i);
  }  
  delay(300);
  hombro.write(90);
  delay(300);
  codo.write(90);
  delay(300);
  pinza.write(70);
}

void recogerFicha()
{
  for(i=90;i<=174;i++)
  {
    hombro.write(i);
    delay(20);
  }  
  delay(200);
  
  for(i=90;i<=114;i++)
  {
    codo.write(i);
        delay(20);
  }  
  delay(200);
  
  for(i=70;i<=anguloAgarrePinza;i++)
  {
    pinza.write(i);
    delay(20);
  }
  delay(200);
  
  for(i=176;i>=140;i--)
  {
    hombro.write(i);
        delay(20);
  }    
}
//************************************************************************++
void primeraPosicion()
{
   for(i=180;i>=92;i--)
  {
    base.write(i);
    delay(20);
  }
  
  delay(200);
  
  for(i=140;i<=170;i++)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
  for(i=114;i<=124;i++)
  {
    codo.write(i);
    delay(20);
  }
  
  delay(200);
  
  for(i=170;i<=176;i++)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
  for(i=124;i<=130;i++)
  {
    codo.write(i);
    delay(20);
  }
  
  delay(200);
  
  for(i=105;i>=70;i--)
  {
    pinza.write(i);
    delay(20);
  }
  
  delay(200);
  
  for(i=176;i>=140;i--);
  {
    hombro.write(i);
    delay(20);
  }
  
  posicionInicial();
  
}


void segundaPosicion()
{
  //Muevo la base
  for(i=180;i>=88;i--)
  {
    base.write(i);
    delay(20);
  }
  
  delay(200);

  //Muevo el codo hasta 90
  for(i=114;i>=90;i--)
  {
    codo.write(i);
    delay(20);
  }
  
  delay(200);


  //Muevo el hombro  hasta 160
  for(i=140;i<=170;i++)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);

//Muevo el codo  
  for(i=90;i<=102;i++)
  {
    codo.write(i);
    delay(20);
  }  
  
  delay(200);
    
  //Abro la pinza
  for(i=110;i>=90;i--)
  {
    pinza.write(i);
    delay(20);
  }

  delay(200);

  //Subo el hombro  
  for(i=170;i>=140;i--)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
  posicionInicial();
  
}

void terceraPosicion()
{
  //Muevo la base hasta los 90 grados
  for(i=180;i>=90;i--)
  {
    base.write(i);
    delay(20);
  }
  
  delay(200);
  
   //Muevo el codo hasta los 70 grados
  for(i=114;i>=70;i--)
  {
    codo.write(i);
    delay(20);
  }
  
  //Muevo el hombro hasta 170 grados
  for(i=140;i<=170;i++)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
   //Muevo el codo hasta los 80 grados
  for(i=70;i<=80;i++)
  {
    codo.write(i);
    delay(20);
  }
   
  delay(200);
  
  //Abro la pinza  
  for(i=105;i>=70;i--)
  {
    pinza.write(i);
    delay(20);
  }
  
  delay(200);
    
  for(i=170;i>=140;i--);
  {
    hombro.write(i);
    delay(20);
  }
  
  posicionInicial();
  
}


void cuartaPosicion()
{
  //Muevo la base
  for(i=180;i>=104;i--)
  {
    base.write(i);
    delay(20);
  }
  
  delay(200);

  //Muevo el hombro  
  for(i=140;i<=172;i++)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Muevo el codo  
  for(i=114;i<=122;i++)
  {
    codo.write(i);
    delay(20);
  }
  
  delay(200);
    
 //Muevo el hombro  
  for(i=172;i<=176;i++)
  {
    hombro.write(i);
    delay(20);
  } 
  
  delay(200);

//Muevo el codo  
  for(i=122;i<=132;i++)
  {
    codo.write(i);
    delay(20);
  }  
  
  delay(200);
    
  //Abro la pinza
  for(i=110;i>=90;i--)
  {
    pinza.write(i);
    delay(20);
  }

  delay(200);

  //Subo el hombro  
  for(i=170;i>=140;i--)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
  posicionInicial();
  
}

void quintaPosicion()
{
  //Muevo la base 
  for(i=180;i>=106;i--)
  {
    base.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Muevo el codo
  for(i=114;i>=104;i--)
  {
    codo.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Muevo el hombro
  for(i=140;i<=170;i++)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
   
  for(i=105;i>=70;i--)
  {
    pinza.write(i);
    delay(20);
  }
  
  delay(200);
  
  for(i=170;i>=140;i--);
  {
    hombro.write(i);
    delay(20);
  }
  
  posicionInicial();
  
}

void sextaPosicion()
{
  //Muevo la base 
  for(i=180;i>=108;i--)
  {
    base.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Muevo el codo
  for(i=114;i>=74;i--)
  {
    codo.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Muevo el hombro
  for(i=140;i<=160;i++)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
   
  for(i=105;i>=70;i--)
  {
    pinza.write(i);
    delay(20);
  }
  
  delay(200);
  
  for(i=160;i>=140;i--);
  {
    hombro.write(i);
    delay(20);
  }
  
  posicionInicial();
  
}


void septimaPosicion()
{
  //Muevo la base
  for(i=180;i>=116;i--)
  {
    base.write(i);
    delay(20);
  }
  
  delay(200);

  //Muevo el hombro  
  for(i=140;i<=164;i++)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Muevo el codo  
  for(i=114;i<=120;i++)
  {
    codo.write(i);
    delay(20);
  }
  
  delay(200);
    
 //Muevo el hombro  
  for(i=164;i<=172;i++)
  {
    hombro.write(i);
    delay(20);
  } 
  
  delay(200);

//Muevo el codo  
  for(i=120;i<=138;i++)
  {
    codo.write(i);
    delay(20);
  }  
  
  delay(200);
    
  //Abro la pinza
  for(i=110;i>=90;i--)
  {
    pinza.write(i);
    delay(20);
  }

  delay(200);

  //Subo el hombro  
  for(i=172;i>=140;i--)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
  posicionInicial();
  
}

void octavaPosicion()
{
  //Muevo la base hasta los 120 grados
  for(i=180;i>=120;i--)
  {
    base.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Muevo el hombro hasta los 170 grados
  for(i=140;i<=170;i++)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Muevo el codo hasta los 94 grados
  for(i=114;i>=112;i--)
  {
    codo.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Abro la pinza  
  for(i=anguloAgarrePinza;i>=70;i--)
  {
    pinza.write(i);
    delay(20);
  }
  
  delay(200);
  
  for(i=170;i>=140;i--);
  {
    hombro.write(i);
    delay(20);
  }
  
  posicionInicial();
  
}

void novenaPosicion()
{
  //Muevo la base hasta 132 grados
  for(i=180;i>=132;i--)
  {
    base.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Muevo el codo hasta los 94 grados
  for(i=114;i>=94;i--)
  {
    codo.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Muevo el hombro hasta los 170 grados
  for(i=140;i<=170;i++)
  {
    hombro.write(i);
    delay(20);
  }
  
  delay(200);
  
  //Abro la pinza  
  for(i=anguloAgarrePinza;i>=70;i--)
  {
    pinza.write(i);
    delay(20);
  }
  
  delay(200);
  
  for(i=170;i>=140;i--);
  {
    hombro.write(i);
    delay(20);
  }
  
  posicionInicial();
  
}

void happy()
{
  //Muevo el codo hacia arriba
  for(i=90;i<=180;i++);
  {
    codo.write(i);
  }
  
  delay(200);
  
  for(j=0;j<=10;j++)
  {
    
        for(i=180;i>=120;i--);
        {
          base.write(i);
        }
        
        delay(100);
        
        for(i=120;i<=180;i++);
        {
          base.write(i);
        }
        
        delay(100);
        
        for(i=180;i>=120;i--);
        {
          base.write(i);
        }
        
        delay(100);
        
        for(i=120;i<=180;i++);
        {
          base.write(i);
        }
        
        delay(100);
  }
  
  //Muevo la pinza
  for(j=0;j<=10;j++)
  
  {
  
      for(i=70;i<=140;i++);
      {
        pinza.write(i);
      }
      
      delay(200);
      
      for(i=140;i<=120;i--);
      {
        pinza.write(i);
      }
      
      delay(200);
      
      for(i=100;i<=120;i++);
      {
        pinza.write(i);
      }
      
      delay(200);
      
      for(i=140;i<=100;i--);
      {
        pinza.write(i);
      }
      
  }
  
  
}

  void loop() 
  {
    
    
    if(Serial.available()>0)
      {
        char dato= Serial.read();
          
          if(dato=='B')
            {
              girarBaseDerecha();
            }
            
          if(dato=='b')
            {
              girarBaseIzquierda();
            }
          
          if(dato=='H')
            {
              subirHombro();
            }
            
           if(dato=='h')
            {
             bajarHombro(); 
            }
            
           if(dato=='C')
             {
               levantarCodo();
             }
            
           if(dato=='c')
             {
               bajarCodo();
             }
  
          if(dato=='P')
             {
               abrirPinza();
             }
            
           if(dato=='p')
             {
               cerrarPinza();
             }
             
           if(dato=='i')
             {
               posicionInicial();
             }
             
            if(dato=='r')
             {
               recogerFicha();
             }
             
            if(dato=='1')
             {
               primeraPosicion();
             }
             
              if(dato=='2')
             {
               segundaPosicion();
             }
             
              if(dato=='3')
             {
               terceraPosicion();
             }
                          
             if(dato=='4')
             {
               cuartaPosicion();
             }
             
              if(dato=='5')
             {
              quintaPosicion();
             }
             
              if(dato=='6')
             {
              sextaPosicion();
             }
             
             if(dato=='7')
             {
               septimaPosicion();
             }

             if(dato=='8')
             {
               octavaPosicion();
             }
             
              if(dato=='9')
             {
               novenaPosicion();
             }
             
             if(dato=='w')
             {
               happy();
             }
        
        
      }
  }

Autor: sistemasymicros
26 proyectos 6 seguidores 0 siguiendo

Ingeniero en electrónica. Maker, apasionado por la programación. Sigueme en sistemasymicros.

PROYECTOS RELACIONADOS
COMENTARIOS
Para publicar un comentario debes