En esta ocasión presento un programa para controlar un brazo robótico virtual realizado en MATLAB mediante el uso de un microcontrolador AVR. La función del microcontrolador es la de controlar las articulaciones del robot mediante potenciometros los cuales mediante un tratamiento por el código en MATLAB representaran los ángulos en que se podrá desplazar el robot.
DESARROLLO:
La practica en esta ocasión consta de dos programas, uno realizado en un microcontrolador AVR utilizando el compilador GCC y el otro es un programa realizado en MATLAB para la visualización del movimiento del robot virtual. Comenzaremos explicando el funcionamiento del programa en el microcontrolador.
MICROCONTROLADOR:
ISR(USART_RX_vect):
En esta sección del programa se realiza la recepción de datos por parte del microcontrolador, cada vez que un dato es enviado a este, surge una interrupción y a continuación el programa lee el dato recibido, una vez realizado esto, identifica el canal para leer dependiendo del dato recibido; es decir si recibe el carácter '0' mediante la condición switch() selecciona el canal 0 del modulo ADC para leer el valor de voltaje del potenciometro conectado a el, esto lo realiza con los caracteres del '0' al '5'.
switch(recibido)
Enseguida lee los el canal seleccionado del ADC y separando unidades, decenas y centenas mediante la función desconcatenar(), asigna los caracteres correspondientes para enviarlos por el USART hacia MATLAB y manipularlos para funcionar con el software.
n=convADC();
valor[1]=(n-100*valor[0])/10;
valor[2]=n-100*valor[0]-10*valor[1];
{
envio[i]=desconcatenar(valor[i]);
}
Para finalizar envía cada uno de los datos incluyendo un cuarto dato que es que indica el final de la cadena 'LF' con el cual MATLAB sabrá que ha terminado el envió.
for (i=0;i<4;i++)
{
TransmiteUART0(envio[i]);
}
Para hacer todo esto el programa debe enviar desde MATLAB dos datos esto es para evitar leer datos erróneos por parte del ADC.
MAIN:
En esta parte del programa se inicializan el USART, ADC y e interrupciones.
DESCONCATENAR(unsigned char dato):
En esta sección se asigna el carácter correspondiente en ASCII al numero recibido, para enviar por el buffer del USART.
unsigned char desconcatenar(unsigned char dato)
MATLAB:
El programa en MATLAB únicamente se explicara el envió y recepción de datos y el tratamiento de estos para ser utilizados la sección siguiente incluyen conocimientos sobre teoría de robótica mas en especifico sobre parámetros de Denavit-Hartenberg.
INICIALIZACIÓN:
Primeramente se cierra las ventanas que podrían estar abiertas y se limpia el workspace, se declaran variables simbólicas esto es para otra funciones posteriores a modificar, esto no será tratado en esta entrada.
clear all
close all
syms alpha gamma phi dx dy dz
CONFIGURACIÓN DEL PUERTO SERIAL:
A continuación configuramos nuestro puerto serial bajo los siguientes parámetros:
- Indicamos en que puerto se ha conectado nuestro convertidor USB-RS232.
- Seleccionamos el baudrate que programamos en el microcontrolador.
- Los bits de datos a enviar
- Bits de stop de la trama.
- Indicamos la paridad, en nuestro caso no tenemos paridad.
- El tamaño del buffer de entrada.
- Indicamos el timeout.
- Configuramos cual sera el caracter que indique el final de la cadena.
- abrimos el puerto.
%configuracion de puerto serial
s2 = serial('COM4','BaudRate',19200,'DataBits',8,'StopBits',2);
set(s2,'Parity','none'); % se configura sin paridad
set(s2,'InputBufferSize',4); % ”n” es el número de bytes a recibir
set(s2,'Timeout',1); % 1 segundos de tiempo de espera para finalizar
set(s2,'FlowControl','none');
set(s2,'Terminator','LF');
%abrir puerto
fopen(s2)
WHILE(true):
Comenzamos inicializando las variables que guardaran los datos del buffer de entrada para el control del robot.
A2=[];
A3=[];
A4=[];
A5=[];
A6=[];
RECEPCIÓN DE DATOS:
Primeramente enviamos el carácter '0' y lo volvemos a enviar con 3 milisegundos de retraso para que el microcontrolador envié los datos correctamente. En seguida dentro de un bucle for leemos los datos enviados por el microcontrolador, y los convertimos en tipo char.
fprintf(s2,'%c','0')
pause(0.003)
fprintf(s2,'%c','0')
for j=0:3
A1=[A1 fread(s2,1,'uchar')];
end
B1=char(A1);
Convertimos mediante la función str2double el dato convertido a char y modificamos los para metros en los que el robot podrá mover sus articulaciones, para el primer parámetro el movimiento se encuentra desde -160 a 160 grados. Para terminar se convierten los grados a radianes con la función degtorad.
T1=str2double(B1);
P1=320*T1/255-160;
t1=degtorad(P1);
Se realizara lo mismo para la recepción de los datos adquiridos por los seis potenciometros que controlaran todas y cada una de las articulaciones y finalizando con ayuda de la programación de los parámetros de Denavit - Hartenberg.
NOTA:
Alguna duda o sugerencia puede indicarla en los comentarios para poder mejorar la presentación de los programas e incrementar la variedad de de temas a tratar también si desean que se trate la teoría sobre la programación de microcontroladores se podrá indicar los aspectos importantes para su comprensión, dejo al final el link de los programas para que puedan descargarlos.
PROGRAMAS:
ATMEL STUDIO:
/*
* COM_MATLAB_ROBOT.c
*
* Created: 15/03/2014 14:41:07
* Author: TERRAFORMARS
*/
#include <avr/io.h>
#ifndef F_CPU
#define F_CPU 8000000UL // XTAL de 8 MHz
#endif
#include "InitADC.h"
#include "UARTAiNit.h"
#include <util/delay.h>
#include <avr/interrupt.h>
#define setbit(sfr,bit) (_SFR_BYTE(sfr)|=(_BV(bit)))
#define clearbit(sfr,bit) (_SFR_BYTE(sfr)&=~(_BV(bit)))
#define bittoggle(sfr,bit)(_SFR_BYTE(sfr)^=_BV(bit))
unsigned char i,n,valor[3]={0},envio[4]={0},recibido,unidad,doble=0;
unsigned char desconcatenar(unsigned char dato);
ISR (USART_RX_vect){
recibido=ReceiveUART0();
switch(recibido){
case '0':
canalADC(0);
break;
case '1':
canalADC(1);
break;
case '2':
canalADC(2);
break;
case '3':
canalADC(3);
break;
case '4':
canalADC(4);
break;
case '5':
canalADC(5);
break;
}
n=convADC();
if (doble==1)
{
valor[0]=n/100;
valor[1]=(n-100*valor[0])/10;
valor[2]=n-100*valor[0]-10*valor[1];
for (i=0;i<3;i++)
{
envio[i]=desconcatenar(valor[i]);
}
envio[3]=10;
for (i=0;i<4;i++)
{
TransmiteUART0(envio[i]);
}
doble=0;
}
else
{
doble=1;
}
}
int main(void)
{
canalADC(0);
initADC(2,1,5);
initializeUART0(19200,0,8,2,2);
setbit(UCSR0B,RXCIE0);
sei();
while(1)
{
//TODO:: Please write your application code
}
}
unsigned char desconcatenar(unsigned char dato){
switch(dato){
case 0:
unidad='0';
break;
case 1:
unidad='1';
break;
case 2:
unidad='2';
break;
case 3:
unidad='3';
break;
case 4:
unidad='4';
break;
case 5:
unidad='5';
break;
case 6:
unidad='6';
break;
case 7:
unidad='7';
break;
case 8:
unidad='8';
break;
case 9:
unidad='9';
break;
}
return unidad;
}
MATLAB:
clc
clear all
close all
syms alpha gamma phi dx dy dz
%configuracion de puerto serial
s2 = serial('COM4','BaudRate',19200,'DataBits',8,'StopBits',2);
set(s2,'Parity','none'); % se configura sin paridad
set(s2,'InputBufferSize',4); % ”n” es el número de bytes a recibir
set(s2,'Timeout',1); % 1 segundos de tiempo de espera para finalizar
set(s2,'FlowControl','none');
set(s2,'Terminator','LF');
%abrir puerto
fopen(s2)
while (true)
%Trz=[cos(alpha),-sin(alpha),0,0;sin(alpha),cos(alpha),0,0;0,0,1,0;0,0,0,1]
%Trx=[1,0,0,0;0,cos(gamma),-sin(gamma),0;0,sin(gamma),cos(gamma),0;0,0,0,1]
%Ttz=[1,0,0,0;0,1,0,0;0,0,1,dz;0,0,0,1]
%Ttx=[1,0,0,dx;0,1,0,0;0,0,1,0;0,0,0,1]
%T=Trz*Ttz*Ttx*Trx%matriz de multiplicacion de rotaciones
A1=[];
A2=[];
A3=[];
A4=[];
A5=[];
A6=[];
fprintf(s2,'%c','0')
pause(0.003)
fprintf(s2,'%c','0')
for j=0:3
A1=[A1 fread(s2,1,'uchar')];
end
B1=char(A1);
T1=str2double(B1);
P1=320*T1/255-160;
t1=degtorad(P1);
fprintf(s2,'%c','1')
pause(0.003)
fprintf(s2,'%c','1')
for j=0:3
A2=[A2 fread(s2,1,'uchar')];
end
B2=char(A2);
T2=str2double(B2);
P2=270*T2/255-225;
t2=degtorad(P2);
fprintf(s2,'%c','2')
pause(0.003)
fprintf(s2,'%c','2')
for j=0:3
A3=[A3 fread(s2,1,'uchar')];
end
B3=char(A3);
T3=str2double(B3);
P3=270*T3/255-45;
t3=degtorad(P3);
fprintf(s2,'%c','3')
pause(0.003)
fprintf(s2,'%c','3')
for j=0:3
A4=[A4 fread(s2,1,'uchar')];
end
B4=char(A4);
T4=str2double(B4);
P4=280*T4/255-110;
t4=degtorad(P4);
fprintf(s2,'%c','4')
pause(0.003)
fprintf(s2,'%c','4')
for j=0:3
A5=[A5 fread(s2,1,'uchar')];
end
B5=char(A5);
T5=str2double(B5);
P5=200*T5/255-100;
t5=degtorad(P5);
fprintf(s2,'%c','5')
pause(0.003)
fprintf(s2,'%c','5')
for j=0:3
A6=[A6 fread(s2,1,'uchar')];
end
B6=char(A6);
T6=str2double(B6);
P6=532*T6/255-266;
t6=degtorad(P6);
%Matriz A01
alpha1=degtorad(-90);
d1=660.4;
a1=0;
Trz=[cos(t1),-sin(t1),0,0;sin(t1),cos(t1),0,0;0,0,1,0;0,0,0,1];
Trx=[1,0,0,0;0,cos(alpha1),-sin(alpha1),0;0,sin(alpha1),cos(alpha1),0;0,0,0,1];
Ttz=[1,0,0,0;0,1,0,0;0,0,1,d1;0,0,0,1];
Ttx=[1,0,0,a1;0,1,0,0;0,0,1,0;0,0,0,1];
A01=Trz*Ttz*Ttx*Trx;
%Matriz A12
alpha2=0;
d2=149.09;
a2=431.8;
Trz=[cos(t2),-sin(t2),0,0;sin(t2),cos(t2),0,0;0,0,1,0;0,0,0,1];
Trx=[1,0,0,0;0,cos(alpha2),-sin(alpha2),0;0,sin(alpha2),cos(alpha2),0;0,0,0,1];
Ttz=[1,0,0,0;0,1,0,0;0,0,1,d2;0,0,0,1];
Ttx=[1,0,0,a2;0,1,0,0;0,0,1,0;0,0,0,1];
A12=Trz*Ttz*Ttx*Trx;
%Matriz A23
alpha3=degtorad(90);
d3=0;
a3=-20.32;
Trz=[cos(t3),-sin(t3),0,0;sin(t3),cos(t3),0,0;0,0,1,0;0,0,0,1];
Trx=[1,0,0,0;0,cos(alpha3),-sin(alpha3),0;0,sin(alpha3),cos(alpha3),0;0,0,0,1];
Ttz=[1,0,0,0;0,1,0,0;0,0,1,d3;0,0,0,1];
Ttx=[1,0,0,a3;0,1,0,0;0,0,1,0;0,0,0,1];
A23=Trz*Ttz*Ttx*Trx
%Matriz A34
alpha4=degtorad(90);
d4=433.07;
a4=0;
Trz=[cos(t4),-sin(t4),0,0;sin(t4),cos(t4),0,0;0,0,1,0;0,0,0,1];
Trx=[1,0,0,0;0,cos(alpha4),-sin(alpha4),0;0,sin(alpha4),cos(alpha4),0;0,0,0,1];
Ttz=[1,0,0,0;0,1,0,0;0,0,1,d4;0,0,0,1];
Ttx=[1,0,0,a4;0,1,0,0;0,0,1,0;0,0,0,1];
A34=Trz*Ttz*Ttx*Trx;
%Matriz A45
alpha5=degtorad(90);
d5=0;
a5=0;
Trz=[cos(t5),-sin(t5),0,0;sin(t5),cos(t5),0,0;0,0,1,0;0,0,0,1];
Trx=[1,0,0,0;0,cos(alpha5),-sin(alpha5),0;0,sin(alpha5),cos(alpha5),0;0,0,0,1];
Ttz=[1,0,0,0;0,1,0,0;0,0,1,d5;0,0,0,1];
Ttx=[1,0,0,a5;0,1,0,0;0,0,1,0;0,0,0,1];
A45=Trz*Ttz*Ttx*Trx;
%Matriz A56
alpha6=0;
d6=56.25;
a6=0;
Trz=[cos(t6),-sin(t6),0,0;sin(t6),cos(t6),0,0;0,0,1,0;0,0,0,1];
Trx=[1,0,0,0;0,cos(alpha6),-sin(alpha6),0;0,sin(alpha6),cos(alpha6),0;0,0,0,1];
Ttz=[1,0,0,0;0,1,0,0;0,0,1,d6;0,0,0,1];
Ttx=[1,0,0,a6;0,1,0,0;0,0,1,0;0,0,0,1];
A56=Trz*Ttz*Ttx*Trx;
A02=A01*A12;
A03=A02*A23;
A04=A03*A34;
A05=A04*A45;
A06=A01*A12*A23*A34*A45*A56;
clf
view (135,22)
axis([-800 800 -800 800 0 800])
line([0 100],[0 0],[0 0],'color','r')
line([0 0],[0 100],[0 0],'color','g')
line([0 0],[0 0],[0 100],'color','b')
line([A01(1,4) A01(1,4)+A01(1,1)],[A01(2,4) A01(1,4)+A01(1,1)],[0 0],'color','r')
line([0 0],[0 100],[0 0],'color','g')
line([0 0],[0 0],[0 100],'color','b')
%trazamos el primer eslabon del puma
line([0,A01(1,4)],[0,A01(2,4)],[0,A01(3,4)],'color','k','linewidth',3)
%trazamos de A01 a A12
line([A01(1,4),A02(1,4)],[A01(2,4),A02(2,4)],[A01(3,4),A02(3,4)],'color','c','linewidth',3)
line([A02(1,4),A03(1,4)],[A02(2,4),A03(2,4)],[A02(3,4),A03(3,4)],'color','k','linewidth',3)
line([A03(1,4),A04(1,4)],[A03(2,4),A04(2,4)],[A03(3,4),A04(3,4)],'color','k','linewidth',3)
line([A04(1,4),A05(1,4)],[A04(2,4),A05(2,4)],[A04(3,4),A05(3,4)],'color','k','linewidth',3)
line([A05(1,4),A06(1,4)],[A05(2,4),A06(2,4)],[A05(3,4),A06(3,4)],'color','k','linewidth',3)
pause(0.1)
end
%cerrar puerto
fclose(s2)
delete(s2)
clear s2
instrfind
VÍDEO DE FUNCIONAMIENTO:
IMÁGENES:
SIMULACIÓN DE BRAZO ROBÓTICO:
PROGRAMA EN ATMEL STUDIO:
SIMULACIÓN EN ISIS PROTEUS:
EJECUCIÓN EN SIMULACIÓN:
LINKS:
MATLAB:
https://mega.co.nz/#!toUVATCZ!GYwNB1TUsc4ATQ1_0bLKZHr3nyu_1xnLL85-5eP2fo8
MICROCONTROLADOR:
https://mega.co.nz/#!QtN32YrI!MS1XSrPPgF-YapKILeC9S-Qy6FBmd_1rNpdqmBNkI28
No hay comentarios:
Publicar un comentario