Reporte de robot RP

2.- Diseños electrónicos.

ITA16RP_N001

ITA16RP_N002

Conector para motor de eje Prismático

Conector para motor de eje Rotativo

Terminales para conectar 5,5,24 Volts

Salida a24Volts para bobina

3.- Principio de operación.

Este dispositivo consta de dos etapas principales, elprograma del PIC18F250 y una interfaz de control desdeLabVIEW. El programa de LabVIEW es el que manada todos lospuntos haciendo cálculos utilizando formulas de la cinemáticainversa para el robot RP.

En el programa del PIC18F2550 se tiene solo el control de laposición de los motores, el PIC recibe una cantidad de pasosque es la que determina hacia donde se deben de mover ycuantos pasos.

Para comprender mejor la forma en que trabaja el PIC18F2550véase el siguiente diagrama.

INICIO

Dar valores a los pasos de los motores. Estos dependen de la posición de home.

Leer constantemente el puerto serial

Los datos recibidos representan el número de pasos para los dos motores además de un número más que representa la ejecución del efector finalAl terminar de posicionar los dos motores el PIC manda una señal para indicar que ya se han

terminado de ejecutar los pasos

Figura 3.1 Diagrama para explicar la logica del programa del PIC18F2550.

Para realizar el codigo para el PIC18F2550 se utilizo elcompilador de mikroelectronica “microC PRO”, y el codigograbado en el PIC fue el siguente:

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

// Declaracion de variables.

unsigned short int sec1[4]={16,32,64,128}, sec2[4]={128,64,32,16}; // Variablespara secuencia de motor del eje prismatico

unsigned short int cont=0, i,x,y, uni, dec, cen, mil, mil2; // Variablesauxiliares

unsigned short int uni1, dec1, cen1, suj, suja=0; // Variablesauxiliares

char uart_rd; // Variable para guardar datode UART1

unsigned int pmra=576, pmr=0, pmpa=9600, pmp=0, duty=65036; // Variables paraguardar

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

// Declaracion de funciones

void rotativo(void);

void prismatico(void);

void conversion(void);

void sujecion(void);

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

// Funcion principal.

void main() {

// Configuracion de PIC18F2550

OSCCON = 0x6E; // Oscilador interno a 4MHZ.

ADCON1 = 0x0F; // Todas las entradas y salidas como digitales.

CMCON = 0x07; // Deshabilitar comparadores

TRISA = 0x0F; // Byte bajo de puerto A como entrada, y byte alto comosalida.

PORTA = 0x00; // Iniciar puerto a en ceros

TRISB = 0x00; // PTB como salida.

PORTB = 0x00; // Iniciar Puerto B en ceros

TRISC = 0x00; // PTC como salida.

PORTC = 0x00; // Iniciar Puerto C en ceros

INTCON = 0xE0; // Habilitar interrupciones por TMR0

T0CON = 0x80; // Encender TMR0 con prescales de 2:1 y a 16 bits.

TMR0H = 0xFD; // Iniciar registros de TMR0...

TMR0H = 0x12; // ...para obtener PWM para servo de efector final

PWM1_Init(800); // Iniciar PWM a 800Hz.

PWM1_Set_Duty(128); // Duty_Cycle en 50%

UART1_Init(9600); // Inicia Modulo USART a 9600 bps.

Delay_ms(100); // Retardo para estabilizar Modulo USART.

INTCON.B2=0;

while (1) { // Bucle infinito

ini: //leer datos enviados por UART1...

if (UART1_Data_Ready()) {

uart_rd = UART1_Read(); // Si el dato leido es la A...

if(uart_rd==65){ // entonces saltar al sigueinte IF

inicio:

if (UART1_Data_Ready()){

uart_rd = UART1_Read(); // Guardar el 1er byte leido del UART

uni1=uart_rd;

inicio2:


uart_rd = UART1_Read();

dec1=uart_rd; // Guardar el 2do byte leido del UART

inicio3:



cen1=uart_rd; // Guardar el 3er byte leido del UART

inicio4:



uni=uart_rd; // Guardar el 4to byte leido del UART

inicio5:


uart_rd = UART1_Read(); // Guardar el 5to byte leido del UART

dec=uart_rd;

inicio6:



cen=uart_rd; // Guardar el 6to byte leido del UART

inicio7:



mil=uart_rd; // Guardar byte 7 leido del UART

inicio8:



mil2=uart_rd; // Guardar el byte 8 leido del UART

inicio9:



suj=uart_rd; // Guardar el byte 9 leido del UART

rotativo(); // Ejecutar funcion para Moto.R

prismatico(); // Ejecutar funcion para Moto.P

sujecion(); // Ejecutar funcion estado de elemento de sujecion.

UART1_Write_Text("A"); // Mandar señal para avisar q ya ejecuto lospasos

goto ini; // Regresar a bucle infinito y esperar otro dato delUART1.

}

else {goto inicio9;}

}


}


}


}


}


}


}


}

else {goto inicio;}

}

else{goto ini;}

}

}

}

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

// Funcion para ejecutar pasos de Motor.R

void rotativo(){

i=uni1; conversion(); pmr=(i*100);//*******************************************************//

i=dec1; conversion(); pmr=pmr+(i*10); //**Convertir datos de UART a num depasos para Motor.R**//

i=cen1; conversion(); pmr=pmr+i;//*******************************************************//

if(pmra>pmr){PORTC.B0=1;} // Girar a la derecha

if(pmra<pmr){PORTC.B0=0;} // Girar a la izquierda

while(pmra!=pmr){ // bucle mientas Pasos actuales != a pasos leidos

PWM1_Start(); // Start PWM1.

Delay_us(1250); // Tiempo para dar un paso

PWM1_Stop(); // Deshabilitar Driver

Delay_ms(5); // Delay para disminuir la frecuencia de PWM1

if(pmra>pmr){pmra--;} // Disminuir en uno variable de pasos actuales sies mayor a los pasos leidos

else{pmra++;} // En caso contrario aumentar esta variable

}

}

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

// Funcion para ejecutar pasos del motor prismatico

void prismatico(){

i=uni; conversion(); pmp=(i*10000);

i=dec; conversion(); pmp=pmp+(i*1000);

i=cen; conversion(); pmp=pmp+(i*100);

i=mil; conversion(); pmp=pmp+(i*10);

i=mil2; conversion(); pmp=pmp+1;

while(pmpa!=pmp){

if(pmpa>pmp){pmpa--; PORTB=sec2[x];}

else{pmpa++; PORTB=sec1[x];}

Delay_ms(3);

x++;

if(x>3){x=0;}

}

PORTB=0;

}

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

void conversion(){ // Funcion para Localizar #'s enviado por USART.

switch(i){

case 48: i=0; break;










}

}

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

//Funcion de sujecion

void sujecion(){

i=suj; conversion(); suj=i; // Convertir Byte 9 recibido de uart a valor INT

if(suja!=suj){ // Comparar variables suja vs suj

if(suj==1){ // si suj es uno entonces efector baja

duty=64585; // Cambiar duty cicle de PWM generado por TMR0

Delay_ms(1000); // Pausa de 1s

PORTA.B5=1; // Activar salida de 24Volts para Bobina



}

if(suj==0){ // si suj es UNO entonces efector sube



PORTA.B5=0; // Desactivar salida de 24Volts para Bobina



}

suja=suj; // igualar variables

}

}

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

void interrupt(){

cont++; // Incrementar cont

if(cont==1){ // Si cont es UNO

TMR0H=duty >> 8;

TMR0L=duty;

PORTA.B4=1; //Entonces encender PB4 el tiempo asignado a DUTY

}

if(cont==2){ // Si cont es DOS

TMR0H=0XDC;

TMR0L=0XD8;

PORTA.B4=0; //Entonces apagar PB4 por 18ms

cont=0; //Reiniciar variable

}

INTCON.B2=0; //Volver activar interrupciones de TMR0

}

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

En el programa de Labwie se aplican las formulas de lacinematica inversa para crear varios puntos a lo largo de unapendiente generada desde los puntos ingresados por el usuario.

Una vez que el programa calcula el primer punto conviente estascoordenadas en pasos de acuerdo a la resolucion que tenemos ennuestros motores.

Para el primer motor, que es el rotativo, se tiene unaresolucion por paso de 0.156125°, de tal manera que el anguloque se calcula en LabVIEW se divide entre este numero y asi seobtiene el numero de pasos que debera dar el motor rotativo.

Para el caso del motor del eje prismatico, se tiene unaresolucion de 0.00625mm por paso, de tal manera que el valor delradio obtenido se divide entre este valor y asi se obtiene elnumero de pasos que debe de dar el motor del eje prismatico.

4.- Manual de operación.

La función de este robot es transportar piezas a un cierto puntoen un área de trabajo de 20x20cm. La ruta para transportar laspiezas debe de ser en línea recta.

El robot RP opera con un panel frontal creado en LabVIEW, esteprograma manda los puntos que debe crear el robot por medio delpuerto serial. Si se desea utilizar una laptop sin puerto serialse necesitara un cable USB a serial.

Para operar este robot se deben de seguir los siguientes pasos.

1. Verificar que el robot RP está correctamente conectado, consus dos fuentes de alimentación necesarias conectadas en loslugares correctos. verificar que los motores estén conectadosa la tarjeta electrónica, además de la bobina del efectorfinal.

En las siguientes figuras se muestras las conexiones necesarias:

Figura 4.1 Fuente de alimentacion de 5volts (cable rojo), el negro es negativo.

Figura 4.2 Fuente de alimentacion de 24volts para bobina de efector final

Figura 4.3 Terminales de alimentacion al circuito del robot RP.

La primera terminal de 5volts es para alimentación delPIC18F2550 y del EASY DRIVER que controla al motor rotativo. Lasegunda terminal es para la etapa de potencia del motor del ejeprismático, se dejó independiente por si se desea alimentar elmotor a mayor tensión. La terminal de 24 volts es para la bobinadel efector final.

2. Colocar el robot en la posición inicial (home). Dichaposición será colocando el eslabón principal al centro del

5V 5V tensio

24V

GND

Ang= 90°

6.7cm

área de trabajo. Como se muestra en la figura.

Figura 4.4 Pocision inicial del robot RP.

3. Ya que la tarjeta este energizada, abrir el panel de controlpara el robot RP de LabVIEW. En este panel se pueden colocarcinco puntos a donde el robot tendrá que transportarse pararecoger las piezas y solo un punto a donde las transportara.Antes de iniciar el programa, se debe verificar si los puntosdonde se localizan las piezas son los correctos o los que sedesean.

Botón deinicioBotón de

Stop

Controlesparamodificarlos puntosdonde se

Coordenadasde punto adonde setrasladarán

4. Verificar que el cable de comunicación serial sea reconocidopor la PC o laptop, según sea el caso.

5. Identificar el puerto al que se conectó la tarjeta del robot.6. Una vez que el robot energizado debe de estar como el de la

figura 4.6.

Figura 4.6 Robot RP energizado.

Figura 4.5 Software de control de robot RP.

Datos enviadosal PIC

Puerto decomunicación

7. Antes de iniciar el programa el programa verifica que elrobot tenga una hoja donde dibujar y verificar si el plumóndel robot esta óptimas condiciones.

8. Iniciar el programa de LabVIEW, el robot deberá comenzar amoverse hacia el primer punto.

9. Esperar a que el robot termine de transportar todas laspiezas.

10. Al terminar de ejecutar todos los puntos el programa deLabVIEW se apagara, si se desea repetir la secuencia solobasta con dar inicio al programa nuevamente. Si se desea sepueden cambiar los puntos a los que se moverá el robot.

11. Al terminar de usar el robot solo desconecte el robotprocurando dejar el robot en la posición de home.

Nota* El robot solo puede transportar piezas metálicas ya que utiliza una bobina quefunciona como electroimán.

5.- Software

Para este robot hay dos programas, el programa que se grabó almicrocontrolador PIC18f2550 y la interfaz de control creada enLabVIEW.

Comenzaremos por explicar el programa del PIC18F2550. La funciónde este programa es leer la cantidad de pasos que deberán moversecada motor tomando como referencia la posición de home del robot,la cual se vio en el capítulo anterior.

El programa del PÍC18F2550 lee estos pasos por el puerto serial,después de codificar estos datos el PIC ejecuta los pasos delmotor rotativo y después los pasos para el motor del ejeprismático. Después lee la posición del efector final, si estesufrió un cambio entonces ejecuta una función donde recoge o dejarla pieza según sea el caso.

Los datos que se envían al pic son los siguientes:

El software de control de LabVIEW funciona de la siguiente manera.

1. En el panel frontal se tienen los controles de 5 puntos dondese localizan las piezas que se deben mover. Además se tieneotro control para el punto a donde se deberán mover todasestas piezas. También se tiene un control del puerto decomunicación, el cual será serial.

Letra enviadaal PIC paraindicar elinicio de los

Indica lacantidad depasos para elmotor rotativo.

Indica lacantidad depasos para elmotor

Indica si elrobot deberecoger unapieza o soltar

Figura 5.1 Datos que se envial al PIC18F2550.

Figura 5.2 Panel frontal del software de control para robot RP.

2. La función principal de este programa es mandar muchos puntosal PIC para generar rectas. En la primera parte del programase calcula la pendiente de primera recta que se forma desdela posición de HOME hasta el punto 1. Dentro de un case selee las veces que se ha ejecutado el WHILE principal para asídeterminar qué puntos se tomaran para generar la pendiente.

Figura 5.3 Calculo de la pendiente de la primer recta que debera generar elrobot.

3. Después de que se calcula la primer pendiente, el programaentra a otro WHILE donde genera varios puntos con la formulasde la cinemática inversa de un robot RP.

Figura 5.4 Formulas de la cinematica inversa del Robot RP.

4. Despues de hacer los calculos del primer punto, el programaenvia los datos obtenidos al PIC para que este los ejecute.Despues de enviarlos espera a que el PIC le mande un dato.Este dato indica que ya termino de ejecutar los pasos. De talmanera que el programa ya puede volver hacer los calculos delsiguiente punto y volver a enviarlos al PIC. Este ciclo serepite hasta que se terminan de calcular todos los puntos dela recta que se formo con los dos puntos.

Figura 5.5 Envio de datos al PIC18F2550.

5. Despues de terminar de ejecutar todos los puntos para laprimera recta, se vuelve a calcular una nueva pendiente deotra recta, ahora tomado el punto donde termino el robot y elpunto a donde tendra que que trasportar la pieza para generarlos puntos.

6. El programa tiene dos maneras de parar. Una es dando STOP ocuando termine de ejecutar todos los puntos de todas lasrectas.

Reporte de robot RP

Documents

Transcript of Reporte de robot RP