function varargout = CinematicaInversa_Prueba(varargin)

% CINEMATICAINVERSA_PRUEBA MATLAB code for

CinematicaInversa_Prueba.fig
% CINEMATICAINVERSA_PRUEBA, by itself, creates a
new CINEMATICAINVERSA_PRUEBA or raises the existing
% singleton*.
%
% H = CINEMATICAINVERSA_PRUEBA returns the handle
to a new CINEMATICAINVERSA_PRUEBA or the handle to
% the existing singleton*.
%
%
CINEMATICAINVERSA_PRUEBA('CALLBACK',hObject,eventData,ha
ndles,...) calls the local
% function named CALLBACK in
CINEMATICAINVERSA_PRUEBA.M with the given input
arguments.
%
% CINEMATICAINVERSA_PRUEBA('Property','Value',...)
creates a new CINEMATICAINVERSA_PRUEBA or raises the
% existing singleton*. Starting from the left,
property value pairs are
% applied to the GUI before
CinematicaInversa_Prueba_OpeningFcn gets called. An
% unrecognized property name or invalid value makes
property application
% stop. All inputs are passed to
CinematicaInversa_Prueba_OpeningFcn via varargin.
%
% *See GUI Options on GUIDE's Tools menu. Choose
"GUI allows only one
% instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help

CinematicaInversa_Prueba

% Last Modified by GUIDE v2.5 28-Mar-2023 10:01:46

% Begin initialization code - DO NOT EDIT

gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
 'gui_OpeningFcn',
@CinematicaInversa_Prueba_OpeningFcn, ...
'gui_OutputFcn',
@CinematicaInversa_Prueba_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 CinematicaInversa_Prueba is

made visible.
function CinematicaInversa_Prueba_OpeningFcn(hObject,
eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future
version of MATLAB
% handles structure with handles and user data (see
GUIDATA)
% varargin command line arguments to
CinematicaInversa_Prueba (see VARARGIN)

% Choose default command line output for

CinematicaInversa_Prueba
handles.output = hObject;

% Update handles structure

guidata(hObject, handles);

% UIWAIT makes CinematicaInversa_Prueba wait for user

response (see UIRESUME)
% uiwait(handles.figure1);
% --- Outputs from this function are returned to the
command line.
function varargout =
CinematicaInversa_Prueba_OutputFcn(hObject, eventdata,
handles)
% varargout cell array for returning output args (see
VARARGOUT);
% hObject handle to figure
% eventdata reserved - to be defined in a future
version of MATLAB
% handles structure with handles and user data (see
GUIDATA)

% Get default command line output from handles structure

varargout{1} = handles.output;

function Px_Callback(hObject, eventdata, handles)

% hObject handle to Px (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 Px as

text
% str2double(get(hObject,'String')) returns
contents of Px as a double

% --- Executes during object creation, after setting all

properties.
function Px_CreateFcn(hObject, eventdata, handles)
% hObject handle to Px (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 && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

% --- Executes on button press in Mover.

function Mover_Callback(hObject, eventdata, handles)
% hObject handle to Mover (see GCBO)
% eventdata reserved - to be defined in a future
version of MATLAB
% handles structure with handles and user data (see
GUIDATA)
%% Se obtiene la posicion x,y,z deseada por el usuario
ValorX=get(handles.Px,'String');
ValorX=str2num(ValorX);
ValorY=get(handles.Py,'String');
ValorY=str2num(ValorY);
ValorZ=get(handles.Pz,'String');
ValorZ=str2num(ValorZ);

%% Declaracion de variables globales

global Qinic;
global posXaux, global posYaux, global posZaux,global
vant;
%% envio de la posicion para el caculo de la posicion de
articulaciones
Qinic=RobotAnimacion(Qinic,ValorX,ValorY,ValorZ);
%% mostrar en pantalla la solucion de cinematica inversa

if (vant==0)
set(handles.Px,'String',posXaux)
set(handles.Py,'String',posYaux)
set(handles.Pz,'String',posZaux)

end
if (vant==1)
posXaux=ValorX;
posYaux=ValorY;
posZaux=ValorZ;
end

%% funcion de animacion del robot

function Pq123=RobotAnimacion(qi,posX,posY,posZ)
velocidad=0.05;
global vant;
qf=[0 0 0 ];%%qf
%% Parametros DH del robot RRR
Q1=Revolute('d',6,'a',0,'alpha',-pi/2);
Q2=Revolute('d',0,'a',15,'alpha',0);
Q3=Revolute('d',0,'a',10,'alpha',0);
RobotBrazo=SerialLink([Q1 Q2 Q3],'name','ROBOT RRR');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
pause(1);
%%%%%%%%%%%%
destino=transl(posX,posY,posZ);
qf=RobotBrazo.ikine(destino,'q0',qi,'mask',[1 1 1 0 0
0 ]);
Existe=size(qf);
if (Existe(1)==0 && Existe(2)==0)
msgbox('Coordenada fuera de rango','ERROR');
vant=0;
Pq123=qi;
return
end
Aq=qf-qi;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (Aq(1)>0)
while Aq(1)>0
qi(1)=qi(1)+velocidad;
Aq(1)=qf(1)-qi(1);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
else
while Aq(1)<0
qi(1)=qi(1)-velocidad;
Aq(1)=qf(1)-qi(1);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (Aq(2)>0)
while Aq(2)>0
qi(2)=qi(2)+velocidad;
Aq(2)=qf(2)-qi(2);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
else
while Aq(2)<0
qi(2)=qi(2)-velocidad;
Aq(2)=qf(2)-qi(2);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (Aq(3)>0)
while Aq(3)>0
qi(3)=qi(3)+velocidad;
Aq(3)=qf(3)-qi(3);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
else
while Aq(3)<0
qi(3)=qi(3)-velocidad;
Aq(3)=qf(3)-qi(3);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
qi=qf;
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
Pq123=qi;
vant=1;
%%

function Py_Callback(hObject, eventdata, handles)

% hObject handle to Py (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 Py as

text
% str2double(get(hObject,'String')) returns
contents of Py as a double
% --- Executes during object creation, after setting all
properties.
function Py_CreateFcn(hObject, eventdata, handles)
% hObject handle to Py (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 && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function Pz_Callback(hObject, eventdata, handles)

% hObject handle to Pz (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 Pz as

text
% str2double(get(hObject,'String')) returns
contents of Pz as a double

% --- Executes during object creation, after setting all

properties.
function Pz_CreateFcn(hObject, eventdata, handles)
% hObject handle to Pz (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 && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function Q1_Callback(hObject, eventdata, handles)

% hObject handle to Q1 (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 Q1 as

text
% str2double(get(hObject,'String')) returns
contents of Q1 as a double

% --- Executes during object creation, after setting all

properties.
function Q1_CreateFcn(hObject, eventdata, handles)
% hObject handle to Q1 (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 && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function Q2_Callback(hObject, eventdata, handles)

% hObject handle to Q2 (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 Q2 as

text
% str2double(get(hObject,'String')) returns
contents of Q2 as a double

% --- Executes during object creation, after setting all

properties.
function Q2_CreateFcn(hObject, eventdata, handles)
% hObject handle to Q2 (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 && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function Q3_Callback(hObject, eventdata, handles)

% hObject handle to Q3 (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 Q3 as

text
% str2double(get(hObject,'String')) returns
contents of Q3 as a double
% --- Executes during object creation, after setting all
properties.
function Q3_CreateFcn(hObject, eventdata, handles)
% hObject handle to Q3 (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 && isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

% --- Executes during object creation, after setting all

properties.
function figure1_CreateFcn(hObject, eventdata, handles)
% hObject handle to figure1 (see GCBO)
% eventdata reserved - to be defined in a future
version of MATLAB
% handles empty - handles not created until after all
CreateFcns called
global Qinic, global posXaux, global posYaux, global
posZaux;

global Qinic1, global angXaux,global angYaux,global

angZaux,global vant;
Qinic=[0 -pi/2 0];
posXaux=0;
posYaux=0;
posZaux=31;

Qinic1=[0 -pi/2 0];

angXaux=0;
angYaux=0;
angZaux=-45;

Qinic=RobotAnimacion(Qinic,posXaux,posYaux,posZaux);
Qinic1=RobotAnimacion(Qinic1,angXaux,angYaux,angZaux);
% set(handles.Px,'String',num2str(posXaux))
% set(handles.Py,'String',num2str(posYaux))
% set(handles.Pz,'String',num2str(posZaux))
% set(handles.Q1,'String',round(Qinic(1)*180/pi,2))
% set(handles.Q2,'String',round(Qinic(2)*180/pi,2))
% set(handles.Q3,'String',round(Qinic(3)*180/pi,2))

% --- Executes on button press in pushbutton4.

function pushbutton4_Callback(hObject, eventdata,
handles)
% hObject handle to pushbutton4 (see GCBO)
% eventdata reserved - to be defined in a future
version of MATLAB
% handles structure with handles and user data (see
GUIDATA)
angX=get(handles.Q1,'String');
angX=str2num(angX);
angX=angX*pi/180;

angY=get(handles.Q2,'String');
angY=str2num(angY);
angY=angY*pi/180;

angZ=get(handles.Q3,'String');
angZ=str2num(angZ);
angZ=angZ*pi/180;

global Qinic1;
global angXaux,global angYaux,global angZaux,global
vant;

Qinic1=RobotAnimacion(Qinic1,angX,angY,angZ);
%% mostrar en pantalla la solucion de cinematica inversa

if (vant==0)
set(handles.Q1,'String',angXaux)
set(handles.Q2,'String',angYaux)
set(handles.Q3,'String',angZaux)
end
if (vant==1)

angXaux=angX;
angYaux=angY;
 angZaux=angZ;
end

%% funcion de animacion del robot

function Pq123=RobotAnimacion(qi,angx,angy,angz)
velocidad=0.05;
global vant;
qf=[0 0 0 ];%%qf
%% Parametros DH del robot RRR
Q1=Revolute('d',6,'a',0,'alpha',-pi/2);
Q2=Revolute('d',0,'a',15,'alpha',0);
Q3=Revolute('d',0,'a',10,'alpha',0);
RobotBrazo=SerialLink([Q1 Q2 Q3],'name','ROBOT RRR');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
pause(1);
%%%%%%%%%%%%
destino=transl(angx,angy,angz);
qf=RobotBrazo.ikine(destino,'q0',qi,'mask',[1 1 1 1 0 0
0 0]);
Existe=size(qf);
if (Existe(1)==0 && Existe(2)==0)
msgbox('Coordenada fuera de rango','ERROR');
vant=0;
Pq123=qi;
return
end
Aq=qf-qi;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (Aq(1)>0)
while Aq(1)>0
qi(1)=qi(1)+velocidad;
Aq(1)=qf(1)-qi(1);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
else
while Aq(1)<0
qi(1)=qi(1)-velocidad;
Aq(1)=qf(1)-qi(1);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (Aq(2)>0)
while Aq(2)>0
qi(2)=qi(2)+velocidad;
Aq(2)=qf(2)-qi(2);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
else
while Aq(2)<0
qi(2)=qi(2)-velocidad;
Aq(2)=qf(2)-qi(2);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (Aq(3)>0)
while Aq(3)>0
qi(3)=qi(3)+velocidad;
Aq(3)=qf(3)-qi(3);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
else
while Aq(3)<0
qi(3)=qi(3)-velocidad;
Aq(3)=qf(3)-qi(3);
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

qi=qf;
RobotBrazo.plot(qi,'workspace',[-40 40 -40 40 -10 45] );
Pq123=qi;
vant=1;