Codigo matlab Robot Scara con servomotores

% Begin initialization code - DO NOT EDIT

gui_Singleton = 1;

gui_State = struct('gui_Name', mfilename, ...

'gui_Singleton', gui_Singleton, ...

'gui_OpeningFcn', @Interfaz_Robot_Scara_OpeningFcn, ...

'gui_OutputFcn', @Interfaz_Robot_Scara_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

% End initialization code - DO NOT EDIT

% --- Executes just before Interfaz_Robot_Scara is made visible.

function Interfaz_Robot_Scara_OpeningFcn(hObject, eventdata, handles, varargin)

handles.output = hObject;

guidata(hObject, handles);

%Declaramos las variables globales que son las constantes de los brazos, los servos1,2 y

%de arduino para todo el programa

global ar;

ar = arduino('COM3', 'Uno', 'Libraries', 'Servo')

global s1;

s1=servo(ar,'D2', 'MinPulseDuration', 500*10^-6, 'MaxPulseDuration', 2500*10^-6);

writePosition(s1,0);

global s2;

s2=servo(ar,'D3');

writePosition(s2,0);

global s3;

global s4;

s4=servo(ar,'D5')

writePosition(s4,0);

global l1;

global l2;

global l3;

l1=30;

l2=17;

l3=17;

function varargout = Interfaz_Robot_Scara_OutputFcn(hObject, eventdata, handles)

varargout{1} = handles.output;

%Cuadro de Texto de q1

function q1_Callback(hObject, eventdata, handles)

function q1_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

set(hObject,'BackgroundColor','white');

end

%Cuadro de Texto de q2

function q2_Callback(hObject, eventdata, handles)

function q2_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

set(hObject,'BackgroundColor','white');

end

%Cuadro de Texto de q3

function q3_Callback(hObject, eventdata, handles)

function q3_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

set(hObject,'BackgroundColor','white');

end

%Cuadro de Texto de Px

function Px_Callback(hObject, eventdata, handles)

function Px_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

set(hObject,'BackgroundColor','white');

end

%Cuadro de Texto de Py

function Py_Callback(hObject, eventdata, handles)

function Py_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

set(hObject,'BackgroundColor','white');

end

%Cuadro de Texto de Pz

function Pz_Callback(hObject, eventdata, handles)

function Pz_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

set(hObject,'BackgroundColor','white');

end

%Boton de Cinematica Inversa

function pushbutton1_Callback(hObject, eventdata, handles)

% Llamamos a las medidas de los eslabones

global l1;

global l2;

global l3;

global a;

%guardamos los valores de Px Py y Pz introducidos

Px = str2double(get(handles.Px, 'String'));

Py = str2double(get(handles.Py, 'String'));

Pz = str2double(get(handles.Pz, 'String'));

%Resovemos las ecuaciones de Cinematica inversa para encontrar los valores de las q

R=sqrt(Px^2+Py^2);

beta=acosd(((l2^2)+(l3^2)-(R^2))/(2*l2*l3))

alfa=acosd((R^2+l2^2-l3^2)/(2*R*l2))

atang=atand(Py/Px)

q2=180-beta

q1=180+(atang-alfa)

q1=abs(q1)

q3=l1-Pz

%Pasamos los Grados a radianes

Q1= q1*(pi/180)

Q2= q2*(pi/180)

%Resolvemos en modo de cinematica directa con los valores de las q obtenids

%para graficar la posicion final del brazo

teta = [Q1 Q2 0]

d = [l1 0 q3] %Falta saber la distancia d(3) entre articulacion 2 y la pinza

alfa = [l2 l3 0]

a = [0 pi/2 0]

%Matrices de transformación homogénea

A01 = [cos(teta(1)), -sin(teta(1)), 0, alfa(1)*cos(teta(1)); sin(teta(1)), cos(teta(1)), 0, alfa(1)*sin(teta(1)); 0, 0 , 1, d(1); 0, 0, 0, 1]

A12 = [cos(teta(2)), sin(teta(2)),

...