UNIVERSIDAD MAYOR DE SAN SIMN FACULTAD DE CIENCIAS Y TECNOLOGA CARRERA DE INGENIERIA ELECTRNICA

DISEO E IMPLEMENTACION DE UN BRAZO ROBOT DE 3 GRADOS DE LIBERTAD

UNIV.: Paredes Milan Riber Rubin de Celiz Daniel FECHA: 20 de julio de 2011

COCHABAMBA BOLIVIA

1 INTRODUCCIN A continuacin se presentan algunos conceptos importantes para el control de robots. Coordenadas y matrices homogneas . La representacin mediante coordenadas homogneas de la localizacin de slidos en un espacio n-dimensional se realiza a travs de coordenadas de un espacio (n+1)-dimensional. Es decir un espacio n-dimensional se encuentra representado en coordenadas homogneas por (n+1) dimensiones. A partir de la definicin de las coordenadas homogneas surge inmediatamente el concepto matriz de transformacin homognea T a una matriz de dimensin 4 x 4 que representa la transformacin de un vector de coordenadas homognea de un sistema de coordenadas a otro. R T= 3x3 0 Cinemtica del Robot La cinemtica del robot estudia el movimiento del mismo con respecto a un sistema de referencia. As, la cinemtica se interesa por la descripcin analtica del movimiento espacial del robot como una funcin del tiempo, y en particular por las relaciones entre la posicin y la orientacin del extremo final del robot con los valores que toman sus coordenadas articulares. Existen dos problemas fundamentales a resolver en la cinemtica del robot; el primero de ellos se conoce como el problema cinemtico directo, y consiste en determinar cual es la posicin y la orientacin del extremo final del robot, con respecto a un sistema de coordenadas que se toma como referencia , conocidos los valores de las articulaciones y los parmetros geomtricos de los elementos del robot; el segundo, denominado problema cinemtico inverso, resuelve la configuracin que debe adoptar el robot para una posicin y orientacin del extremo conocidas. Denavit y Hartenberg propusieron un mtodo sistemtico para describir y representar la geometra espacial de los elementos de una cadena cinemtica, y en particular de un robot, con respecto a un sistema de referencia fijo. Este mtodo utiliza una matriz de transformacin homognea para describir la relacin espacial entre dos elementos rgidos adyacentes, reducindose el problema cinemtico directo a encontrar una matriz de transformacin homognea 4 x 4 que relacione la localizacin espacial del extremo del robot con respecto al sistema de coordenadas de su base. Por otra parte la cinemtica del robot trata tambin de encontrar las relaciones entre las velocidades del movimiento de las articulaciones y las del extremo. Esta relacin viene dada por el modelo diferencial expresado mediante la matriz Jacobiana. p3x3 Rotacion Traslacion = 1 Perspectiva Escalado

Diagrama de relacin entre cinemtica directa e inversa

3 OBJETIVOS 3.1 OBJETIVO GENERAL Disear un brazo robot de 3 grados de libertad

3.2 OBJETIVOS ESPECFICOS Haciendo uso de los conceptos aprendidos en la materia de Rotica aplicarlas en el brazo robot como la rotacin traslacin. El diseo tendr q comprender la cinemtica directa y la inversa. Controlar motores DC. Con la utilizacin de encoders pticos, y la inversin de giro. Realizar un sistemas de comunicacin va USB

4 DESCRIPCIN DEL PROYECTO Las cuatro articulaciones son rotacionales, la base que tiene su eje Z apuntando hacia arriba, la segunda articulacin tiene su eje Z perpendicular al anterior, el tercer grado de libertad es similar al segundo grado de libertad, y por ultimo el cuarto grado de libertad es la orientacin en la mueca. Foto del robot ya terminado.

y z x

RoboQ

Robot visto del plano XZ (Lado)

y z RoboQ

800

-600

-400

-200

CINEMATICA DEL ROBOT Parmetros

L{1}=link([pi/2 L{2}=link([0 L{3}=link([0
-400 -200

0 X

Robot visto del plano XY (Arriba)

200 400

600

800

-800

-600

0 18 16

0 X

0 0 0

200

10.5 0 6

400

0],'standard'); 0],'standard'); 0],'standard');

600

800

500

y x z

0 RoboQ -500

500 0 0 -500 Y -500 X 500

DH CONTROL El control del robot se lo realiza mediante una interfaz realizada en MATLAB 7.0, en el cual se ha utilizado el Robotics Toolbox de Corke Peter para realizar una simulacin de los movimientos que realizara el robot.

Botn SIMULAR: Si los valores introducidos no exceden los lmites fsicos, la interfaz permite simular para observar los movimientos que realizara y la configuracin que tomara una vez finalizada las rdenes. Botn EJECUTAR: Inicia la comunicacin y luego enva la informacin del motor correspondiente como ser el sentido de giro y el nmero de pulsos del encoger de dicho motor. Botn DIRECTA: Los datos introducidos en las tres cajas de texto, son los correspondientes ngulos de cada articulacin, si un ngulo excede un limite fsico este se pinta de rojo y el botn EJECUTAR se inhabilita. HARDWARE

SOFTWARE
Microcontrolador #include <18F2550.h> #fuses HSPLL,NOWDT,NOPROTECT,NOLVP,NODEBUG,USBDIV,PLL1,CPUDIV1,VREGEN,MCLR,NOPBADEN #use delay(clock=48000000) #use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7) #define USB_HID_DEVICE FALSE #define USB_EP1_TX_ENABLE USB_ENABLE_BULK #define USB_EP1_RX_ENABLE USB_ENABLE_BULK #define USB_EP1_TX_SIZE 64 #define USB_EP1_RX_SIZE 64 #include <pic18_usb.h> #include <PicUSB.h> #include <usb.c> #define #define #define #define LEDV PIN_C1 LEDR PIN_C0 LED_ON output_high LED_OFF output_low

#BYTE TRISA = 0x0F92 #BYTE TRISB = 0x0F93 #BYTE TRISC = 0x0F94 #BYTE PORTA = 0x0F80 #BYTE PORTB = 0x0F81 #BYTE PORTC = 0x0F82 #BYTE ADCON1 = 0x0FC1 #BYTE CMCON = 0x0FB4

#bit enc1 = 0x0F80.0 #bit enc2 = 0x0F80.1 #bit enc3 = 0x0F80.2 int8 dato[USB_EP1_TX_SIZE]; int8 m1,m2,m3 int16 c1,c2,c3 void main(void) { PORTC = 0x00; LED_ON(LEDV); LED_OFF(LEDR); // Se declara el puerto B como salidas //encendemos led en RB6 para indicar presencia de energia

usb_init(); // inicializamos el USB usb_task(); // habilita periferico usb e interrupciones usb_wait_for_enumeration(); // esperamos hasta que el PicUSB sea configurado por el host LED_OFF(LEDV); LED_ON(LEDR); TRISA = 0xFF; TRISB = 0X00; TRISC = 0X04; PORTB = 0x00; ADCON1 = 0x0F; // encendemos led en RB7 al establecer contacto con la PC // Se declara el puerto A como entradas // Se declara el puerto B como salidas // Se configura al ADC para entradas digitales

c1=0; c2=0; c3=0; m1=0; m2=0; m3=0; while (TRUE) { if(usb_enumerated()) // si el Pic est configurado via USB { if (usb_kbhit(1)) // si el endpoint de salida contiene datos del host { usb_get_packet(1, dato, USB_EP1_TX_SIZE); // cojemos el paquete de tamao 8bytes del EP1 y almacenamos en dato m1=dato[0]; m2=dato[2]; m3=dato[4]; c1 = m1; c2 = m2; c3 = m3; while(c1 > 0) { PORTB=dato[1]; while(enc1); c1=c1-1; } PORTB = 0x00; while(c2 > 0) { PORTB=dato[3]; while(enc2); c2=c2-1; } PORTB = 0x00; while(c3 > 0) { PORTB=dato[5]; while(enc3); c3=c3-1; } PORTB = 0x00; dato[63] = 0xff; usb_put_packet(1, dato, USB_EP1_TX_SIZE, USB_DTS_TOGGLE); //y enviamos el mismo paquete de tamao 64bytes del EP1 al PC RESET_CPU(); } } }

}La funcin del microcontrolador es proporcionar una la interfaz entre el robot y la computadora. Para un mejor control. Computadora La fraccin de cdigo que se presenta es la parte mas importante de la interfaz.
function varargout = RoboQ(varargin) gui_Singleton = 1;

gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @RoboQ_OpeningFcn, ... 'gui_OutputFcn', @RoboQ_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end function RoboQ_OpeningFcn(hObject, eventdata, handles, varargin) L{1}=link([pi/2 L{2}=link([0 L{3}=link([0 0 18 16 0 0 0 10.5 0 6 0],'standard'); 0],'standard'); 0],'standard');

handles.rQ = robot(L,'RoboQ','umss'); load('lastRQ.mat','q0'); handles.q0 = q0; handles.qf = q0; handles.sec = []; handles.dir = []; handles.giro = []; q0_tmp = handles.q0; for j = 1:3 if q0_tmp(j)*180/pi < 1 & q0_tmp(j)*180/pi > -1 q0_tmp(j) = round(q0_tmp(j)*180/pi*1e4)/1e4*pi/180; end end set(handles.text_q1,'String',num2str(q0_tmp(1)*180/pi)) set(handles.text_q2,'String',num2str(q0_tmp(2)*180/pi)) set(handles.text_q3,'String',num2str(q0_tmp(3)*180/pi)) T = fkine(handles.rQ,handles.q0); for j = 1:3 if T(j,4) < 1 & T(j,4) > -1 T(j,4) = round(T(j,4)*1e4)/1e4; end end set(handles.text_x,'String',num2str(T(1,4))) set(handles.text_y,'String',num2str(T(2,4))) set(handles.text_z,'String',num2str(T(3,4))) handles.kine = 'Directa'; handles.lims_ang = [180 90 0]*pi/180; handles.limi_ang = [-180 0 -150]*pi/180; handles.lims_pto_base = [0 0 12]; handles.limi_pto_base = [50 50 0]; handles.lims_pto_cuerpo = [15 15 inf]; handles.limi_pto_cuerpo = [-15 -15 63]; handles.coef_red = [61 11 9.5]/pi; % Choose default command line output for RoboQ handles.output = hObject; handles.rgb = []; handles.noback = []; % Update handles structure guidata(hObject, handles); if strcmp(get(hObject,'Visible'),'off') try handles.vidobj = videoinput('winvideo',2); % Update handles structure start(handles.vidobj); guidata(hObject, handles); vidRes = get(handles.vidobj, 'VideoResolution'); nBands = get(handles.vidobj, 'NumberOfBands'); hImage = image(zeros(vidRes(2), vidRes(1), nBands), 'Parent',... handles.video); preview(handles.vidobj,hImage); end end % UIWAIT makes RoboQ wait for user response (see UIRESUME) % uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line. function varargout = RoboQ_OutputFcn(hObject, eventdata, handles) % Get default command line output from handles structure varargout{1} = handles.output; function edit1_Callback(hObject, eventdata, handles) % hObject handle to edit1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of edit1 as text % str2double(get(hObject,'String')) returns contents of edit1 as a double set(handles.pushbutton_sim,'Enable','off') set(handles.pushbutton_exe,'Enable','off') val1 = str2double(get(handles.edit1,'String')); val2 = str2double(get(handles.edit2,'String')); val3 = str2double(get(handles.edit3,'String')); if isnan(val1) set(hObject,'String','') set(hObject,'ForegroundColor','black') else if handles.kine == 'Directa' val1 = val1*pi/180; val2 = val2*pi/180; val3 = val3*pi/180; [vrfc,long] = verif_ang(val1,val2,val3,handles); elseif handles.kine == help'Inversa' [vrfc,long] = verif_pto(val1,val2,val3,handles); end if ~vrfc(1) set(hObject,'ForegroundColor','red') end if sum(vrfc&long) == 3 set(handles.edit1,'Enable','off') set(handles.edit2,'Enable','off') set(handles.edit3,'Enable','off') if handles.kine == 'Directa' q = [val1 val2 val3]; T = fkine(handles.rQ,q); vrfc = verif_pto(T(1,4),T(2,4),T(3,4),handles); if sum(vrfc) == 3 handles.qf = q; else warndlg('Punto limitado','!! Warning !!') end elseif handles.kine == 'Inversa' T = transl(val1,val2,val3); try q = ikineRQ(T); vrfc = verif_ang(q(1,1),q(1,2),q(1,3),handles); if sum(vrfc) == 3 handles.qf = q(1,:); else vrfc = verif_ang(q(2,1),q(2,2),q(2,3),handles); if sum(vrfc) == 3 handles.qf = q(2,:); else warndlg('Angulo limitado','!! Warning !!') vrfc = logical(zeros(1,3)); end end catch errordlg('Punto inalcanzable','!! Error !!') vrfc = logical(zeros(1,3)); end end set(handles.edit1,'Enable','on') set(handles.edit2,'Enable','on') set(handles.edit3,'Enable','on') if sum(vrfc) == 3 set(handles.pushbutton_sim,'Enable','on') set(handles.pushbutton_exe,'Enable','on') else set(hObject,'ForegroundColor','red') end end rest = vrfc&long; if rest(1) set(handles.edit1,'ForegroundColor','black') end if rest(2) set(handles.edit2,'ForegroundColor','black') end if rest(3) set(handles.edit3,'ForegroundColor','black') end end guidata(hObject,handles)

% --- Executes during object creation, after setting all properties. function edit1_CreateFcn(hObject, eventdata, handles) % hObject handle to edit1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end

function edit2_Callback(hObject, eventdata, handles) % hObject handle to edit2 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of edit2 as text % str2double(get(hObject,'String')) returns contents of edit2 as a double set(handles.pushbutton_sim,'Enable','off') set(handles.pushbutton_exe,'Enable','off') val1 = str2double(get(handles.edit1,'String')); val2 = str2double(get(handles.edit2,'String')); val3 = str2double(get(handles.edit3,'String')); if isnan(val2) set(hObject,'String','') set(hObject,'ForegroundColor','black') else if handles.kine == 'Directa' val1 = val1*pi/180; val2 = val2*pi/180; val3 = val3*pi/180; [vrfc,long] = verif_ang(val1,val2,val3,handles); elseif handles.kine == 'Inversa' [vrfc,long] = verif_pto(val1,val2,val3,handles); end if ~vrfc(2) set(hObject,'ForegroundColor','red') end if sum(vrfc&long) == 3 set(handles.edit1,'Enable','off') set(handles.edit2,'Enable','off') set(handles.edit3,'Enable','off') if handles.kine == 'Directa' q = [val1 val2 val3]; T = fkine(handles.rQ,q); vrfc = verif_pto(T(1,4),T(2,4),T(3,4),handles); if sum(vrfc) == 3 handles.qf = q; else warndlg('Punto limitado','!! Warning !!') end elseif handles.kine == 'Inversa' T = transl(val1,val2,val3); try q = ikineRQ(T); vrfc = verif_ang(q(1,1),q(1,2),q(1,3),handles); if sum(vrfc) == 3 handles.qf = q(1,:); else vrfc = verif_ang(q(2,1),q(2,2),q(2,3),handles); if sum(vrfc) == 3 handles.qf = q(2,:); else warndlg('Angulo limitado','!! Warning !!') vrfc = logical(zeros(1,3)); end end catch errordlg('Punto inalcanzable','!! Error !!') vrfc = logical(zeros(1,3)); end end set(handles.edit1,'Enable','on') set(handles.edit2,'Enable','on') set(handles.edit3,'Enable','on') if sum(vrfc) == 3 set(handles.pushbutton_sim,'Enable','on') set(handles.pushbutton_exe,'Enable','on') else set(hObject,'ForegroundColor','red') end end

rest = vrfc&long; if rest(1) set(handles.edit1,'ForegroundColor','black') end if rest(2) set(handles.edit2,'ForegroundColor','black') end if rest(3) set(handles.edit3,'ForegroundColor','black') end end guidata(hObject,handles) % --- Executes during object creation, after setting all properties. function edit2_CreateFcn(hObject, eventdata, handles) % hObject handle to edit2 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end

function edit3_Callback(hObject, eventdata, handles) % hObject handle to edit3 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of edit3 as text % str2double(get(hObject,'String')) returns contents of edit3 as a double set(handles.pushbutton_sim,'Enable','off') set(handles.pushbutton_exe,'Enable','off') val1 = str2double(get(handles.edit1,'String')); val2 = str2double(get(handles.edit2,'String')); val3 = str2double(get(handles.edit3,'String')); if isnan(val3) set(hObject,'String','') set(hObject,'ForegroundColor','black') else if handles.kine == 'Directa' val1 = val1*pi/180; val2 = val2*pi/180; val3 = val3*pi/180; [vrfc,long] = verif_ang(val1,val2,val3,handles); elseif handles.kine == 'Inversa' [vrfc,long] = verif_pto(val1,val2,val3,handles); end if ~vrfc(3) set(hObject,'ForegroundColor','red') end if sum(vrfc&long) == 3 set(handles.edit1,'Enable','off') set(handles.edit2,'Enable','off') set(handles.edit3,'Enable','off') if handles.kine == 'Directa' q = [val1 val2 val3]; T = fkine(handles.rQ,q); vrfc = verif_pto(T(1,4),T(2,4),T(3,4),handles); if sum(vrfc) == 3 handles.qf = q; else warndlg('Punto limitado','!! Warning !!') end elseif handles.kine == 'Inversa' T = transl(val1,val2,val3); try q = ikineRQ(T); vrfc = verif_ang(q(1,1),q(1,2),q(1,3),handles); if sum(vrfc) == 3 handles.qf = q(1,:); else vrfc = verif_ang(q(2,1),q(2,2),q(2,3),handles); if sum(vrfc) == 3 handles.qf = q(2,:); else warndlg('Angulo limitado','!! Warning !!') vrfc = logical(zeros(1,3)); end end catch errordlg('Punto inalcanzable','!! Error !!') vrfc = logical(zeros(1,3)); end

end set(handles.edit1,'Enable','on') set(handles.edit2,'Enable','on') set(handles.edit3,'Enable','on') if sum(vrfc) == 3 set(handles.pushbutton_sim,'Enable','on') set(handles.pushbutton_exe,'Enable','on') else set(hObject,'ForegroundColor','red') end end rest = vrfc&long; if rest(1) set(handles.edit1,'ForegroundColor','black') end if rest(2) set(handles.edit2,'ForegroundColor','black') end if rest(3) set(handles.edit3,'ForegroundColor','black') end

end guidata(hObject,handles)

% --- Executes during object creation, after setting all properties. function edit3_CreateFcn(hObject, eventdata, handles) % hObject handle to edit3 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end % --- Executes on button press in pushbutton_sim. function pushbutton_sim_Callback(hObject, eventdata, handles) % hObject handle to pushbutton_sim (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) set(handles.pushbutton_sim,'Enable','off') [sec,dir,giro] = select_tray(handles); qf = handles.q0; qf0 = qf; for i = 1:3 mot = sec(i); qf(mot) = handles.qf(mot); if i == 1 q1 = linspace(qf0(1),qf(1),giro(mot)); q2 = linspace(qf0(2),qf(2),giro(mot)); q3 = linspace(qf0(3),qf(3),giro(mot)); elseif i == 2 q4 = linspace(qf0(1),qf(1),giro(mot)); q5 = linspace(qf0(2),qf(2),giro(mot)); q6 = linspace(qf0(3),qf(3),giro(mot)); else q7 = linspace(qf0(1),qf(1),giro(mot)); q8 = linspace(qf0(2),qf(2),giro(mot)); q9 = linspace(qf0(3),qf(3),giro(mot)); end qf0 = qf; end q = [q1 q4 q7;q2 q5 q8;q3 q6 q9]'; figure plot(handles.rQ,q) set(handles.pushbutton_sim,'Enable','on') % --- Executes on button press in pushbutton_exe. function pushbutton_exe_Callback(hObject, eventdata, handles) % hObject handle to pushbutton_exe (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA)

set(handles.pushbutton_sim,'Enable','off') set(handles.pushbutton_exe,'Enable','off') [sec,dir,giro,qf] = select_tray(handles); for i = 1:3

giro_tmp(i) = ((-1)^dir(i))*giro(i); end [handles.qf*180/pi;qf*180/pi;giro_tmp] qf0 = handles.q0; for i = 1:3 motor = sec(i); qf0(motor) = qf(motor); qf0_tmp = qf0; for j = 1:3 if qf0_tmp(j)*180/pi < 1 & qf0_tmp(j)*180/pi > -1 qf0_tmp(j) = round(qf0_tmp(j)*180/pi*1e4)/1e4*pi/180; end end set(handles.text_q1,'String',num2str(qf0_tmp(1)*180/pi)) set(handles.text_q2,'String',num2str(qf0_tmp(2)*180/pi)) set(handles.text_q3,'String',num2str(qf0_tmp(3)*180/pi)) T = fkine(handles.rQ,qf0); for j = 1:3 if T(j,4) < 1 & T(j,4) > -1 T(j,4) = round(T(j,4)*1e4)/1e4; end end set(handles.text_x,'String',num2str(T(1,4))) set(handles.text_y,'String',num2str(T(2,4))) set(handles.text_z,'String',num2str(T(3,4))) end if giro_tmp(1) <= 0 d1 = 2; else d1 = 1; end if giro_tmp(2) <= 0 d2 = 4; else d2 = 8; end if giro_tmp(3) <= 0 d3 = 32; else d3 = 16; end loadlibrary mpusbapi _mpusbapi.h alias libreria data_in = eye(1,64,'uint8'); data_out = eye(1,64,'uint8'); vid_pid_norm = libpointer('int8Ptr',[uint8('vid_04d8&pid_0011') 0]); out_pipe = libpointer('int8Ptr',[uint8('\MCHP_EP1') 0]); in_pipe = libpointer('int8Ptr',[uint8('\MCHP_EP1') 0]); [conectado] = calllib('libreria','MPUSBGetDeviceCount',vid_pid_norm) if conectado == 1 [my_out_pipe] = calllib('libreria', 'MPUSBOpen',uint8 (0), vid_pid_norm, out_pipe, uint8(0), uint8 (0)); % Se abre el tunel de envio [my_in_pipe] = calllib('libreria', 'MPUSBOpen',uint8 (0), vid_pid_norm, in_pipe, uint8 (1), uint8 (0)); % Se abre el tunel de recepci data_out (1)= abs(giro_tmp(1)); data_out (2)= d1; data_out (3)= abs(giro_tmp(2)); data_out (4)= d2; data_out (5)= abs(giro_tmp(3)); data_out (6)= d3; calllib('libreria', 'MPUSBWrite',my_out_pipe, data_out, uint8(64), uint8(64), uint8(10)); [aa,bb,data_in,dd] = calllib('libreria', 'MPUSBRead',my_in_pipe, data_in, uint8(64), uint8(64), uint8(10)); data_in(64) calllib('libreria', 'MPUSBClose', my_in_pipe); calllib('libreria', 'MPUSBClose', my_out_pipe); end unloadlibrary libreria % Importante descargar la librera de memoria, de lo contrario genera errores handles.q0 = qf0; guidata(hObject,handles) q0 = handles.q0; save('lastRQ.mat','q0') % --- Executes on selection change in popupmenu_time. function popupmenu_time_Callback(hObject, eventdata, handles)

% --- Executes during object creation, after setting all properties. function popupmenu_time_CreateFcn(hObject, eventdata, handles) % Hint: popupmenu controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white');

else end

set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));

% --- Executes on button press in radiobutton_directa. function radiobutton_directa_Callback(hObject, eventdata, handles) % Hint: get(hObject,'Value') returns toggle state of radiobutton_directa set(hObject,'Value',1) set(handles.text1,'String','q1') set(handles.text2,'String','q2') set(handles.text3,'String','q3') if handles.kine == 'Inversa' set(handles.pushbutton_sim,'Enable','off') set(handles.pushbutton_exe,'Enable','off') set(handles.edit1,'String','') set(handles.edit2,'String','') set(handles.edit3,'String','') handles.kine = 'Directa'; end guidata(hObject,handles) % --- Executes on button press in radiobutton_inversa. function radiobutton_inversa_Callback(hObject, eventdata, handles) % Hint: get(hObject,'Value') returns toggle state of radiobutton_inversa set(hObject,'Value',1) set(handles.text1,'String','x') set(handles.text2,'String','y') set(handles.text3,'String','z') if handles.kine == 'Directa' set(handles.pushbutton_sim,'Enable','off') set(handles.pushbutton_exe,'Enable','off') set(handles.edit1,'String','') set(handles.edit2,'String','') set(handles.edit3,'String','') handles.kine = 'Inversa'; end guidata(hObject,handles) function [ang,long] = verif_ang(q1,q2,q3,handles) if nargin == 4 long = logical([~isnan(q1) ~isnan(q2) ~isnan(q3)]); ang = logical(ones(1,3)); if long(1) if q1 > handles.lims_ang(1) | q1 < handles.limi_ang(1) ang(1) = false; end end if long(2) if q2 > handles.lims_ang(2) | q2 < handles.limi_ang(2) ang(2) = false; end end if long(3) if q3 > handles.lims_ang(3) | q3 < handles.limi_ang(3) ang(3) = false; end end error('Falta parametros')

else end

function [pto,long] = verif_pto(x,y,z,handles) if nargin == 4 long = logical([~isnan(x) ~isnan(y) ~isnan(z)]); pto = logical(ones(1,3)); if sum(long) == 1 if long(3) if z < handles.limi_pto_base(3) pto = ~long; end end elseif sum(long) == 2 if long(1) & long(2) if (x < handles.lims_pto_cuerpo(1) & x > handles.limi_pto_cuerpo(1)) & ... (y < handles.lims_pto_cuerpo(2) & y > handles.limi_pto_cuerpo(2)) pto = ~long; end else

end else

if z < handles.limi_pto_base(3) pto = ~long; end & & & & x y ... x > handles.limi_pto_cuerpo(1)) & ... y > handles.limi_pto_cuerpo(2))) | ... ... > handles.limi_pto_base(1)) & ... > handles.limi_pto_base(2))) | ...

if ((z > handles.lims_pto_base(3)) (x < handles.lims_pto_cuerpo(1) (y < handles.lims_pto_cuerpo(2) ((z < handles.lims_pto_base(3)) (x < handles.lims_pto_base(1) & (y < handles.lims_pto_base(2) & (z < handles.limi_pto_base(3)) pto = ~long; end

else end

end error('Falta parametros')

function [sec,dir,giro,qf] = select_tray(handles) if nargin == 1 sec = []; q = handles.q0; q(1) = handles.qf(1); T = fkine(handles.rQ,q); vrfc = verif_pto(T(1,4),T(2,4),T(3,4),handles); if sum(vrfc) == 3 sec(1) = 1; q(2) = handles.qf(2); T = fkine(handles.rQ,q); vrfc = verif_pto(T(1,4),T(2,4),T(3,4),handles); if sum(vrfc) == 3 sec(2:3) = [2 3]; else sec(2:3) = [3 2]; end else q(1) = handles.q0(1); q(2) = handles.qf(2); T = fkine(handles.rQ,q); vrfc = verif_pto(T(1,4),T(2,4),T(3,4),handles); if sum(vrfc) == 3 sec(1:3) = [2 3 1]; else sec(1:3) = [3 2 1]; end end for i = 1:3 mot = sec(i); if (handles.qf(mot)-handles.q0(mot)) >= 0 dir(mot) = 0; else dir(mot) = 1; end giro_tmp = abs((handles.qf(mot)-handles.q0(mot))*handles.coef_red(mot)); giro(mot) = round(giro_tmp); qf(mot) = ((-1)^dir(mot))*giro(mot)/handles.coef_red(mot) + ... handles.q0(mot);

end else end

error('Falta parametros')