Manipulator_GUI(varargin)

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

% Copyright 2002-2003 The MathWorks, Inc.

% Edit the above text to modify the response to help Manipulator_GUI

% Last Modified by GUIDE v2.5 09-Oct-2009 02:33:24

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @Manipulator_GUI_OpeningFcn, ...
                   'gui_OutputFcn',  @Manipulator_GUI_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 Manipulator_GUI is made visible.
function Manipulator_GUI_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 Manipulator_GUI (see VARARGIN)

% Choose default command line output for Manipulator_GUI
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);
world = vrworld('Robotic_Arm.wrl');
open(world);
set(world, 'Description', 'Robotic Arm Kinematics');
view(world);
vrdrawnow;

%L1 Sliders
slider_step(1) = 0.05/(2*pi);
slider_step(2) = 1/(2*pi);
set(handles.L1Yaw,'sliderstep',slider_step,...
    'max',pi,'min',-pi,'Value',0)

set(handles.L1Pitch,'sliderstep',slider_step,...
    'max',pi,'min',-pi,'Value',0)

slider_step(1) = 0.5/(2*pi);
slider_step(2) = 1/(2*pi);
set(handles.L1Roll,'sliderstep',slider_step,...
    'max',pi,'min',-pi,'Value',0)

%L2 Sliders
slider_step(1) = 0.05/(2*pi);
slider_step(2) = 1/(2*pi);
set(handles.L2Pitch,'sliderstep',slider_step,...
    'max',pi,'min',-pi,'Value',0)

slider_step(1) = 0.5/(2*pi);
slider_step(2) = 1/(2*pi);
set(handles.L2Roll,'sliderstep',slider_step,...
    'max',pi,'min',-pi,'Value',0)

%L3 Sliders
slider_step(1) = 0.05/(2*pi);
slider_step(2) = 1/(2*pi);
set(handles.L3Yaw,'sliderstep',slider_step,...
    'max',pi,'min',-pi,'Value',0)

set(handles.L3Pitch,'sliderstep',slider_step,...
    'max',pi,'min',-pi,'Value',0)

%Gripper Slider
slider_step(1) = 1/(2*pi);
slider_step(2) = 1/(2*pi);
set(handles.Gonoff,'sliderstep',slider_step,...
    'max',100*pi/180,'min',50*pi/180,'Value',50*pi/180)





% UIWAIT makes Manipulator_GUI wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = Manipulator_GUI_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;


% --- Executes on slider movement.
function L1Yaw_Callback(hObject, eventdata, handles)
% hObject    handle to L1Yaw (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
% get(handles.L1Yaw,'Value')
world = vrworld('Robotic_Arm.wrl');
L1 = vrnode(world, 'Link1');
rot = makehgtform('xrotate',get(handles.L1Pitch,'Value'))*makehgtform('zrotate',get(handles.L1Yaw,'Value'))*makehgtform('yrotate',get(handles.L1Roll,'Value'));
L1.rotation = vrrotmat2vec(rot(1:3,1:3));
set(handles.AY,'String',num2str(get(handles.L1Yaw,'Value')*180/pi,'%+7.2f'));
vrdrawnow;

% --- Executes during object creation, after setting all properties.
function L1Yaw_CreateFcn(hObject, eventdata, handles)
% hObject    handle to L1Yaw (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end




% --- Executes on slider movement.
function L1Pitch_Callback(hObject, eventdata, handles)
% hObject    handle to L1Pitch (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
world = vrworld('Robotic_Arm.wrl');
L1 = vrnode(world, 'Link1');
rot = makehgtform('xrotate',get(handles.L1Pitch,'Value'))*makehgtform('zrotate',get(handles.L1Yaw,'Value'))*makehgtform('yrotate',get(handles.L1Roll,'Value'));
L1.rotation = vrrotmat2vec(rot(1:3,1:3));
set(handles.AP,'String',num2str(get(handles.L1Pitch,'Value')*180/pi,'%+7.2f'));
vrdrawnow;

% --- Executes during object creation, after setting all properties.
function L1Pitch_CreateFcn(hObject, eventdata, handles)
% hObject    handle to L1Pitch (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on slider movement.
function L1Roll_Callback(hObject, eventdata, handles)
% hObject    handle to L1Roll (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
world = vrworld('Robotic_Arm.wrl');
L1 = vrnode(world, 'Link1');
rot = makehgtform('xrotate',get(handles.L1Pitch,'Value'))*makehgtform('zrotate',get(handles.L1Yaw,'Value'))*makehgtform('yrotate',get(handles.L1Roll,'Value'));
L1.rotation = vrrotmat2vec(rot(1:3,1:3));
set(handles.AR,'String',num2str(get(handles.L1Roll,'Value')*180/pi,'%+7.2f'));
vrdrawnow;


% --- Executes during object creation, after setting all properties.
function L1Roll_CreateFcn(hObject, eventdata, handles)
% hObject    handle to L1Roll (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end




% --- Executes on slider movement.
function L2Pitch_Callback(hObject, eventdata, handles)
% hObject    handle to L2Pitch (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
world = vrworld('Robotic_Arm.wrl');
L2 = vrnode(world, 'Link2');
rot = makehgtform('xrotate',get(handles.L2Pitch,'Value'))*makehgtform('yrotate',get(handles.L2Roll,'Value'));
L2.rotation = vrrotmat2vec(rot(1:3,1:3));
set(handles.FP,'String',num2str(get(handles.L2Pitch,'Value')*180/pi,'%+7.2f'));
vrdrawnow;



% --- Executes during object creation, after setting all properties.
function L2Pitch_CreateFcn(hObject, eventdata, handles)
% hObject    handle to L2Pitch (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on slider movement.
function L2Roll_Callback(hObject, eventdata, handles)
% hObject    handle to L2Roll (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
world = vrworld('Robotic_Arm.wrl');
L2 = vrnode(world, 'Link2');
rot = makehgtform('xrotate',get(handles.L2Pitch,'Value'))*makehgtform('yrotate',get(handles.L2Roll,'Value'));
L2.rotation = vrrotmat2vec(rot(1:3,1:3));
set(handles.FR,'String',num2str(get(handles.L2Roll,'Value')*180/pi,'%+7.2f'));
vrdrawnow;


% --- Executes during object creation, after setting all properties.
function L2Roll_CreateFcn(hObject, eventdata, handles)
% hObject    handle to L2Roll (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end




% --- Executes on slider movement.
function L3Pitch_Callback(hObject, eventdata, handles)
% hObject    handle to L3Pitch (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
world = vrworld('Robotic_Arm.wrl');
L3 = vrnode(world, 'Link3');
rot = makehgtform('xrotate',get(handles.L3Pitch,'Value'))*makehgtform('zrotate',get(handles.L3Yaw,'Value'));
L3.rotation = vrrotmat2vec(rot(1:3,1:3));
set(handles.HP,'String',num2str(get(handles.L3Pitch,'Value')*180/pi,'%+7.2f'));
vrdrawnow;


% --- Executes during object creation, after setting all properties.
function L3Pitch_CreateFcn(hObject, eventdata, handles)
% hObject    handle to L3Pitch (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on slider movement.
function L3Yaw_Callback(hObject, eventdata, handles)
% hObject    handle to L3Yaw (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
world = vrworld('Robotic_Arm.wrl');
L3 = vrnode(world, 'Link3');
rot = makehgtform('xrotate',get(handles.L3Pitch,'Value'))*makehgtform('zrotate',get(handles.L3Yaw,'Value'));
L3.rotation = vrrotmat2vec(rot(1:3,1:3));
set(handles.HY,'String',num2str(get(handles.L3Yaw,'Value')*180/pi,'%+7.2f'));
vrdrawnow;


% --- Executes during object creation, after setting all properties.
function L3Yaw_CreateFcn(hObject, eventdata, handles)
% hObject    handle to L3Yaw (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end




% --- Executes on slider movement.
function Gonoff_Callback(hObject, eventdata, handles)
% hObject    handle to Gonoff (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
world = vrworld('Robotic_Arm.wrl');
GLL1 = vrnode(world, 'GripperLL1');
GRL1 = vrnode(world, 'GripperRL1');
rot = makehgtform('zrotate',-get(handles.Gonoff,'Value'));
GLL1.rotation = vrrotmat2vec(rot(1:3,1:3));
rot = makehgtform('zrotate',get(handles.Gonoff,'Value'));
GRL1.rotation = vrrotmat2vec(rot(1:3,1:3));
vrdrawnow;

% --- Executes during object creation, after setting all properties.
function Gonoff_CreateFcn(hObject, eventdata, handles)
% hObject    handle to Gonoff (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end




% --- Executes on button press in home.
function home_Callback(hObject, eventdata, handles)
% hObject    handle to home (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
world = vrworld('Robotic_Arm.wrl');
L1 = vrnode(world, 'Link1');
rot = makehgtform('xrotate',0)*makehgtform('zrotate',0)*makehgtform('yrotate',0);
L1.rotation = vrrotmat2vec(rot(1:3,1:3));
L2 = vrnode(world, 'Link2');
rot = makehgtform('xrotate',0)*makehgtform('yrotate',0);
L2.rotation = vrrotmat2vec(rot(1:3,1:3));
L3 = vrnode(world, 'Link3');
rot = makehgtform('xrotate',0)*makehgtform('zrotate',0);
L3.rotation = vrrotmat2vec(rot(1:3,1:3));
GLL1 = vrnode(world, 'GripperLL1');
GRL1 = vrnode(world, 'GripperRL1');
rot = makehgtform('zrotate',-50*pi/180);
GLL1.rotation = vrrotmat2vec(rot(1:3,1:3));
rot = makehgtform('zrotate',50*pi/180);
GRL1.rotation = vrrotmat2vec(rot(1:3,1:3));
set(handles.L1Pitch,'Value',0);
set(handles.L1Yaw,'Value',0);
set(handles.L1Roll,'Value',0);
set(handles.L2Pitch,'Value',0);
set(handles.L2Roll,'Value',0);
set(handles.L3Pitch,'Value',0);
set(handles.L3Yaw,'Value',0);
set(handles.Gonoff,'Value',50*pi/180);
set(handles.AY,'String',num2str(get(handles.L1Yaw,'Value')*180/pi,'%+7.2f'));
set(handles.AP,'String',num2str(get(handles.L1Pitch,'Value')*180/pi,'%+7.2f'));
set(handles.AR,'String',num2str(get(handles.L1Roll,'Value')*180/pi,'%+7.2f'));
set(handles.FP,'String',num2str(get(handles.L2Pitch,'Value')*180/pi,'%+7.2f'));
set(handles.FR,'String',num2str(get(handles.L2Roll,'Value')*180/pi,'%+7.2f'));
set(handles.HP,'String',num2str(get(handles.L3Pitch,'Value')*180/pi,'%+7.2f'));
set(handles.HY,'String',num2str(get(handles.L3Yaw,'Value')*180/pi,'%+7.2f'));
vrdrawnow;