////////////////////////////////////////////////////////////////////////////////
// //
// 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);
}
}
No hay comentarios:
Publicar un comentario