Brazo Robot de 6 ejes con PIC y SD21

Consiste en el control de un brazo de 6 ejes con un PIC. En este caso se va a utilizar, junto con el PIC, una controladora de servos SD21. Por supuesto se podría prescindir de ella y controlar los servos directamente con el PIC tal como se mostró en el caso de un brazo con 4 ejes simplemente añadiendo otras dos señales moduladas para el control de los dos servos adicionales. Sin embargo, el uso de esta controladora aporta ciertas ventajas, como el poder separar la alimentación de los servos (de 6V a 7,5V) de la alimentación del circuito lógico (5V) con una única fuente de alimentación y aportar mayor fuerza a los servos al proporcionarles una tensión superior a los 5V.

Como muestra didáctica, el ejemplo realizará un ciclo repetitivo del brazo robot. Este ciclo consistirá en recoger una pieza de una posición inicial y depositarla en una segunda ubicación. Vuelve a recoger la pieza y la retorna a  la posición inicial. Por supuesto todo de una forma autónoma. El ciclo se repetirá indefinidamente.

Lo primero será difinir, en la cabecera del programa, los valores de los registros del SD21 con su correspondiente eje o servo:

#define  servo1  0 

#define  Servo2  3 

#define  Servo3  6          

#define  Servo4  9                                  

#define  Servo5  12 

#define  Servo6  15

 
Para cada uno de los ejes, deberemos enviar la información correspondiente para la nueva posición desde el PIC al módulo SD21. Esto lo haremos mediante en bus I2C. La subrutina de comunicación será la siguiente:

void sd21_servo(byte SD21_ID,int num_servo,int velocidad,int16 posicion){
   i2c_start();            //Comienzo de la comunicación I2C ...

   i2c_write(SD21_ID);     //...con la dirección del SD21...

   i2c_write(num_servo);   //...apuntando al registro del servo num_servo

   i2c_write(velocidad);   //velocidad

   i2c_write(posicion);    //Parte baja de Posicion    

   i2c_write(posicion>>8); //Parte alta de posicion

   i2c_stop ();            //Finalización de la transmisión
}

Cuando queremos establecer una posición del brazo, indicamos las posiciones de cada uno de los ejes o servos además de la velocidad con la que queremos que se produzca el movimiento, y hacemos una llamada a la subrutina anterior:

velocidad = 10;

num_servo = servo1;

posicion = 1500;                

sd21_servo (SD21_ID, num_servo, velocidad, posicion);

Esto para cada uno de los servos. Para cada nueva posición del brazo  completo enviaremos la información de todos los ejes. Cuando alguna posición  no varíe o la velocidad sea la misma no será necesario enviar esa información concreta, ya residente en el SD21:

//Movimiento nueva posición
velocidad = 10;
num_servo = servo2;
posicion = 2000;                 
sd21_servo (SD21_ID, num_servo, velocidad, posicion);
num_servo = servo3;
posicion = 1200;                
sd21_servo (SD21_ID, num_servo, velocidad, posicion);
velocidad = 15;
num_servo = servo4;
posicion = 1850;                
sd21_servo (SD21_ID, num_servo, velocidad, posicion);
num_servo = servo5;
posicion = 1500;                 
sd21_servo (SD21_ID, num_servo, velocidad, posicion);

Previamente habremos configurado las variables empleadas en el control:

int velocidad;          //Valor de velocidad de servo

int16 posicion;         //Valor de la posición del servo

int num_servo;          //Identificación del servo a controlar

[+/-] Ver / Ocultar programa completo en C

//////////////////////////////////////////////////////////////////////////////// 

//                                                                            // 

//                    Brazo Robot 6 ejes con PIC y SD21                       //

//                                                                            //

//                      http://robotypic.blogspot.com/                        //

//                          (c) RobotyPic  2013                               //

//                                                                            // 

//////////////////////////////////////////////////////////////////////////////// 

#include <16F876A.h>                      //PIC a utilizar 

#fuses XT,NOWDT,NOPROTECT                 

#use delay(clock=4000000)                 //Frecuencia cristal de cuarzo

#use i2c(master, SCL=PIN_C3, SDA=PIN_C4)  //Configuración comunicación I2C

#use standard_io(C)  

#use standard_io(B)

#use standard_io(A)

#define  servo1  0 

#define  Servo2  3 

#define  Servo3  6          

#define  Servo4  9                                  

#define  Servo5  12 

#define  Servo6  15          

#define  Servo7  18 

#define  Servo8  21 

#define  Servo9  24 

#define  Servo10 27 

#define  Servo11 30 

#define  Servo12 33 

#define  Servo13 36 

#define  Servo14 39 

#define  Servo15 42 

#define  Servo16 45 

#define  Servo17 48 

#define  Servo18 51 

#define  servo19 54 

#define  Servo20 57 

#define  Servo21 60          

int SD21_ID   =  0xC2;     //Dirección I2C de acceso al SD21

/*********************** Variables globales ***********************************/

int velocidad;             //Valor de velocidad de servo

int16 posicion;            //Valor de la posición del servo

int num_servo;             //Identificación del servo a controlar

/******************************************************************************/

/*********************** FUNCIÓN CONTROL DE SERVOS  ***************************/

void sd21_servo (byte SD21_ID, int num_servo, int velocidad, int16 posicion){

   i2c_start();            //Comienzo de la comunicación I2C ...

   i2c_write(SD21_ID);     //...con la dirección del SD21...

   i2c_write(num_servo);   //...apuntando al registro del servo num_servo

   i2c_write(velocidad);   //velocidad

   i2c_write(posicion);    //Parte baja de Posicion    

   i2c_write(posicion>>8); //Parte alta de posicion

   i2c_stop ();            //Finalización de la transmisión

   delay_ms(60);

}

/******************************************************************************/

/************************ FUNCIÓN PRINCIPAL ***********************************/

void main() { 

   //Posición reposo

   velocidad = 10;

   num_servo = servo1;

   posicion = 1500;                

   sd21_servo (SD21_ID, num_servo, velocidad, posicion);

   num_servo = servo2;

   posicion = 1050;                 

   sd21_servo (SD21_ID, num_servo, velocidad, posicion);

   num_servo = servo3;

   posicion = 1500;                 

   sd21_servo (SD21_ID, num_servo, velocidad, posicion);

   num_servo = servo4;

   posicion = 1500;                

   sd21_servo (SD21_ID, num_servo, velocidad, posicion);

   num_servo = servo5;

   posicion = 1500;                

   sd21_servo (SD21_ID, num_servo, velocidad, posicion);

   num_servo = servo6;

   posicion = 2100;                 

   sd21_servo (SD21_ID, num_servo, velocidad, posicion);

   

   delay_ms(5000);   //Tiempo de espera hasta inicio de ciclo

   

   while(true){      //Ciclo repetitivo

   

      //Posición inicial

      velocidad = 10;

      num_servo = servo1;

      posicion = 1500;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo2;

      posicion = 1050;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1500;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo4;

      posicion = 1500;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo5;

      posicion = 1500;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo6;

      posicion = 2100;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento  

      delay_ms(3000);

      

      //1º Movimiento - Semilevantado pinza abierta

      velocidad = 10;

      num_servo = servo2;

      posicion = 1500;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1050;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      velocidad = 15;

      num_servo = servo5;

      posicion = 2400;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo6;

      posicion = 1000;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento              

      delay_ms(1000);

      

      //2º Movimiento - Levantado posición pieza 1

      velocidad = 10;

      num_servo = servo1;

      posicion = 1050;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 600;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo4;

      posicion = 950;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo5;

      posicion = 1500;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento 

      delay_ms(1000);   

     

      //3º Movimiento - Bajo posición pieza 1

      velocidad = 10;

      num_servo = servo2;

      posicion = 2000;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1200;               

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      velocidad = 15;

      num_servo = servo4;

      posicion = 1850;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo5;

      posicion = 1500;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

       

      //Retardo hasta siguiente movimiento 

      delay_ms(2000);

      

      //4º Movimiento - Coger pieza posición 1

      velocidad = 10;

      num_servo = servo6;

      posicion = 1800;   

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento 

      delay_ms(2000);

     

      //5º Movimiento - Semilevantado posición 1

      velocidad = 10;

      num_servo = servo2;

      posicion = 1500;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1050;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      velocidad = 15;

      num_servo = servo4;

      posicion = 950;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento 

      delay_ms(1000);

          

      //6º Movimiento - Semilevantado posición 2

      velocidad = 15;

      num_servo = servo1;

      posicion = 1950;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento 

      delay_ms(1000);

      

      //7º Movimiento - Posición baja posición 2

      velocidad = 10;

      num_servo = servo2;

      posicion = 2000;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1200;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      velocidad = 20;

      num_servo = servo4;

      posicion = 1850;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

         

      //Retardo hasta siguiente movimiento 

      delay_ms(2000);

      

      //8º Movimiento - Dejar pieza posición 2

      velocidad = 10;

      num_servo = servo6;

      posicion = 1000;   

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento 

      delay_ms(2000);

         

      //9º Movimiento - Semilevantado posición 2

      velocidad = 10;

      num_servo = servo2;

      posicion = 1500;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1050;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      velocidad = 20;

      num_servo = servo4;

      posicion = 950;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento 

      delay_ms(1000);

      

      //10º Movimiento - Posición inicial reposo

      velocidad = 10;

      num_servo = servo1;

      posicion = 1500;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo2;

      posicion = 1050;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1500;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo4;

      posicion = 1500;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo5;

      posicion = 1500;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo6;

      posicion = 2100;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento 

      delay_ms(3000);

        

      //11º Movimiento - Semilevantado posición 2

      velocidad = 10;

      num_servo = servo1;

      posicion = 1950;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo2;

      posicion = 1500;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1050;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      velocidad = 20;

      num_servo = servo4;

      posicion = 950;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo6;

      posicion = 1000;   

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento

      delay_ms(1000);

        

      //12º Movimiento - Posición baja posición 2

      velocidad = 10;

      num_servo = servo2;

      posicion = 2000;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1200;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      velocidad = 20;

      num_servo = servo4;

      posicion = 1850;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

         

      //Retardo hasta siguiente movimiento

      delay_ms(2000);

      

      //13º Movimiento - Coger pieza posición 2

      velocidad = 10;

      num_servo = servo6;

      posicion = 1800;   

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento

      delay_ms(2000);

      

      //14º Movimiento - Semilevantado posición 2

      velocidad = 10;

      num_servo = servo1;

      posicion = 1950;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo2;

      posicion = 1500;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1050;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      velocidad = 20;

      num_servo = servo4;

      posicion = 950;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

       

      //Retardo hasta siguiente movimiento

      delay_ms(1000);

      

      //15º Movimiento - Semilevantado posición 1

      velocidad = 15;

      num_servo = servo1;

      posicion = 1050;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

            

      //Retardo hasta siguiente movimiento

      delay_ms(1000);

      

      //16º Movimiento - Posición baja posición 1

      velocidad = 10;

      num_servo = servo2;

      posicion = 2000;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1200;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      velocidad = 20;

      num_servo = servo4;

      posicion = 1850;                

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo5;

      posicion = 1500;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

       

      //Retardo hasta siguiente movimiento

      delay_ms(2000);

      

      //17º Movimiento - Dejar pieza posición 1

      velocidad = 10;

      num_servo = servo6;

      posicion = 1000;   

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento

      delay_ms(2000);

      

      //18º Movimiento - Semilevantado posición 1

      velocidad = 10;

      num_servo = servo2;

      posicion = 1500;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      num_servo = servo3;

      posicion = 1050;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      velocidad = 20;

      num_servo = servo4;

      posicion = 950;                 

      sd21_servo (SD21_ID, num_servo, velocidad, posicion);

      

      //Retardo hasta siguiente movimiento

      delay_ms(1000);

    

   }

}

Los archivos se pueden descargar en este enlace:

Descargar brazo robot 6 ejes

El proyecto contenido en el artículo ha sido diseñado con la versión v4.084 de CCS PIC C COMPILER

El servo mostrado a continuación es el modelo que se ha empleado en el diseño. Dentro de su gama es de los más económicos con respecto a sus prestaciones. No es necesario emplear el mismo servo pero se añade como orientación a la hora de elegir el modelo a emplear en este tipo de sistemas.