function varargout = Brazo_UNI(varargin)

% BRAZO_UNI MATLAB code for Brazo_UNI.fig

% BRAZO_UNI, by itself, creates a new BRAZO_UNI or raises the existing
% singleton*.
%
% H = BRAZO_UNI returns the handle to a new BRAZO_UNI or the handle to
% the existing singleton*.
%
% BRAZO_UNI('CALLBACK',hObject,eventData,handles,...) calls the local
% function named CALLBACK in BRAZO_UNI.M with the given input arguments.
%
% BRAZO_UNI('Property','Value',...) creates a new BRAZO_UNI or raises the
% existing singleton*. Starting from the left, property value pairs are
% unrecognized property name or invalid value makes property application
% stop. All inputs are pa% applied to the GUI before Brazo_UNI_OpeningFcn gets
called. An
%ssed to Brazo_UNI_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 Brazo_UNI

% Last Modified by GUIDE v2.5 12-Nov-2016 10:08:07

% Begin initialization code - DO NOT EDIT

gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @Brazo_UNI_OpeningFcn, ...
'gui_OutputFcn', @Brazo_UNI_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 Brazo_UNI is made visible.

function Brazo_UNI_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 Brazo_UNI (see VARARGIN)

% Choose default command line output for Brazo_UNI

handles.output = hObject;

% Update handles structure

guidata(hObject, handles);

% UIWAIT makes Brazo_UNI wait for user response (see UIRESUME)

% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = Brazo_UNI_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;
%%
%----------------------------%
% 1.CONFIGURACION INICIAL
%---------------------------%
%1.1 Posicion deseada
xc=3;
yc=0;
zc=2;
%1.2 Ploteo del entorno
T0=eye(4);
plot_frame(T0,'color','k','view','auto');
axis([-2 4 -2 4 0 3])
hold on
grid on
xlabel('x(m)')
ylabel('y(m)')
plot3(xc,yc,zc,'ro','MarkerFaceColor','k','MarkerSize',10)
%---------------------------------
%2.Parametros Mecanicos Del Robot
%--------------------------------
L1=0.2;
L2=1.4;
L3=1.6;
L4=0.627;
global my_robot
my_robot=robot_RRRR;
my_robot.set_dimensions(L1,L2,L3,L4)
%--------------------%
%3.CINEMATICA INVERSA
%--------------------%
%3.1 UISAMOS EL METODO "cinemtica_inversa"
% q=my_robot.cinematica_inversa(xc,yc,zc);
% q1d=q(1);
% q2d=q(2);
% q3d=q(3);
% q4d=q(4);
% %3.2Salidas
% disp('Angulos')
% disp(q)
% disp(q*180/pi)
%----------------%
%4.1 Eslabon 1
%->Hallamos A1
global theta1 theta2 theta3 theta4
global h1 h2 h3 h4
theta1=0;
A1=my_robot.compute_matrix_A1(theta1);
%Ploteamos
T01=A1;
h1=plot_frame(T01,'frame','1','color','r');

%4.2 Eslabon2
%->Hallamos A2
theta2=0;
A2=my_robot.compute_matrix_A2(theta2);
%Ploteamos
T02=T01*A2;
h2=plot_frame(T02,'frame','2','color','m');

%4.2 Eslabon3
%->Hallamos A3
theta3=0;
A3=my_robot.compute_matrix_A3(theta3);
%Ploteamos
T03=T02*A3;
h3=plot_frame(T03,'frame','3','color','b');

%4.2 Eslabon4
%->Hallamos A4
theta4=0;
A4=my_robot.compute_matrix_A4(theta4);
%Ploteamos
T04=T03*A4;
h4=plot_frame(T04,'frame','4','color','b');

global conectado
conectado = 0;

% --- Executes on button press in pushbutton1.

function pushbutton1_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
xc = str2num(get(handles.textbox1,'String'));
yc = str2num(get(handles.textbox2,'String'));
zc = str2num(get(handles.textbox3,'String'));
plot3(xc, yc, zc, 'ro', 'MarkerFaceColor', 'k', 'MarkerSize',3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%
global my_robot

%-------------------------------------------------------------------------%
% 3. Cinematica Inversa
%-------------------------------------------------------------------------%
% 3.1. Usamos el Metodo " Cinematica_ Ivarsa"
q = my_robot.cinematica_inversa(xc, yc, zc);
q1d = q(1);
q2d = q(2);
q3d = q(3);
q4d = q(4);
% 3.2. Salidas
% disp('angulos')
% disp(q)
% disp(q*180/pi)
% qc = q*180/pi;
%
% set(handles.th1,'String',qc(1))
% set(handles.th2,'String',qc(2))
% set(handles.th3,'String',qc(3))
% set(handles.th3,'String',qc(4))

global theta1 theta2 theta3 theta4

global h1 h2 h3 h4
global conectado
global s1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%
disp(' Robt del tipo RR de 3gdl')

disp('ROBOT DEL TIPO RRR DE 2GDL')

disp('PRESIONE UNA TECLA PARA EMPEZAR LA SIMULACION')

%5.2Definimos los angulos

STEPS=20;
THETA1=linspace(theta1,q1d,STEPS);
THETA2=linspace(theta2,q2d,STEPS);
THETA3=linspace(theta3,q3d,STEPS);
THETA4=linspace(theta4,q4d,STEPS);
%5. Lazo Principal
for i=1:STEPS
%A.Obtenemos Los angulos
theta1=THETA1(i);
theta2=THETA2(i);
theta3=THETA3(i);
theta4=THETA4(i);
 set(handles.th1,'String',theta1*180/pi())
set(handles.th2,'String',theta2*180/pi())
set(handles.th3,'String',theta3*180/pi())
set(handles.th4,'String',theta4*180/pi())
m=[theta1 theta2 theta3 theta4]*180/pi();
[m1,m2,m3,m4]=thetas(m(1),m(2),m(3),m(4));
if conectado==1
fwrite(s1,[m1,m2,m3,m4],'uchar');
end

m=[theta1 theta2 theta3 theta4];

%B.Obtenemos Los angulos
A1=my_robot.compute_matrix_A1(theta1);
A2=my_robot.compute_matrix_A2(theta2);
A3=my_robot.compute_matrix_A3(theta3);
A4=my_robot.compute_matrix_A4(theta4);

T01=A1;
T02=A1*A2;
T03=T02*A3;
T04=T03*A4;
set(handles.xc,'String',T04(1,4))
set(handles.yc,'String',T04(2,4))
set(handles.zc,'String',T04(3,4))
%C.PLOTEAMOS LOS SITEMAS COORDENADOS
plot_frame(h1,T01);
plot_frame(h2,T02);
plot_frame(h3,T03);
plot_frame(h4,T04);

%DELTA DE TIEMPO
pause(0.3)
end

theta1 = q1d;
theta2 = q2d;
theta3 = q3d;
theta4 = q4d;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%

function textbox1_Callback(hObject, eventdata, handles)

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

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

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

function textbox1_CreateFcn(hObject, eventdata, handles)
% hObject handle to textbox1 (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 textbox2_Callback(hObject, eventdata, handles)

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

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

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

function textbox2_CreateFcn(hObject, eventdata, handles)
% hObject handle to textbox2 (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 textbox3_Callback(hObject, eventdata, handles)

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

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

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

function textbox3_CreateFcn(hObject, eventdata, handles)
% hObject handle to textbox3 (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 textbox6_Callback(hObject, eventdata, handles)

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

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

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

function textbox6_CreateFcn(hObject, eventdata, handles)
% hObject handle to textbox6 (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 textbox5_Callback(hObject, eventdata, handles)

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

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

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

function textbox5_CreateFcn(hObject, eventdata, handles)
% hObject handle to textbox5 (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 textbox4_Callback(hObject, eventdata, handles)

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

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

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

function textbox4_CreateFcn(hObject, eventdata, handles)
% hObject handle to textbox4 (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 pushbutton2.

function pushbutton2_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

global theta1 theta2 theta3 theta4

global h1 h2 h3 h4
global my_robot
global conectado s1

q1d = str2num(get(handles.textbox4,'String'))*pi()/180;
q2d = str2num(get(handles.textbox5,'String'))*pi()/180;
q3d = str2num(get(handles.textbox6,'String'))*pi()/180;
q4d = str2num(get(handles.textbox7,'String'))*pi()/180;

STEPS=20;
THETA1=linspace(theta1,q1d,STEPS);
THETA2=linspace(theta2,q2d,STEPS);
THETA3=linspace(theta3,q3d,STEPS);
THETA4=linspace(theta4,q4d,STEPS);
%5. Lazo Principal
for i=1:STEPS
%A.Obtenemos Los angulos
theta1=THETA1(i);
theta2=THETA2(i);
theta3=THETA3(i);
theta4=THETA4(i);
%%
m=[theta1 theta2 theta3 theta4]*180/pi();
[m1,m2,m3,m4]=thetas(m(1),m(2),m(3),m(4));
if conectado==1
fwrite(s1,[m1,m2,m3,m4],'uchar');
 end
%%
set(handles.th1,'String',theta1*180/pi())
set(handles.th2,'String',theta2*180/pi())
set(handles.th3,'String',theta3*180/pi())
set(handles.th4,'String',theta4*180/pi())
m=[theta1 theta2 theta3 theta4];
disp('m:')
disp(m*180/pi)
%B.Obtenemos Los angulos
A1=my_robot.compute_matrix_A1(theta1);
A2=my_robot.compute_matrix_A2(theta2);
A3=my_robot.compute_matrix_A3(theta3);
A4=my_robot.compute_matrix_A4(theta4);

T01=A1;
T02=A1*A2;
T03=T02*A3;
T04=T03*A4;
set(handles.xc,'String',T04(1,4))
set(handles.yc,'String',T04(2,4))
set(handles.zc,'String',T04(3,4))
%C.PLOTEAMOS LOS SITEMAS COORDENADOS
plot_frame(h1,T01);
plot_frame(h2,T02);
plot_frame(h3,T03);
plot_frame(h4,T04);

%DELTA DE TIEMPO
pause(0.3)
end

theta1 = q1d;
theta2 = q2d;
theta3 = q3d;
theta4 = q4d;

% --- Executes on button press in conectar.

function conectar_Callback(hObject, eventdata, handles)
% hObject handle to conectar (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

% Hint: get(hObject,'Value') returns toggle state of conectar

clear puerto
global conectado
global puerto
global s1
conectado = 1-conectado;
puerto = get(handles.puerto,'String');
puerto = strcat('com',puerto);
if conectado == 1
fclose('all');
delete(instrfind)
s1=serial(puerto);
fopen(s1);
disp(puerto)
old=[0,0,0,0];
fwrite(s1,old,'uchar');
end

function puerto_Callback(hObject, eventdata, handles)

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

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

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

function puerto_CreateFcn(hObject, eventdata, handles)
% hObject handle to puerto (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 th4_CreateFcn(hObject, eventdata, handles)
% hObject handle to th4 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called

function textbox7_Callback(hObject, eventdata, handles)

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

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

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

function textbox7_CreateFcn(hObject, eventdata, handles)
% hObject handle to textbox7 (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