RB_interface(varargin)

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

% Last Modified by GUIDE v2.5 10-Sep-2007 15:50:00
%
% Copyright 2007, The MathWorks, Inc.
%

% This test because the interface does not work together with certain models
if strcmp(get_param([bdroot,'/Plant'],'blockchoice'),'Identified Model')
    disp('Trajectory interface does not work with ''Identified Model'' option')
    set_param(bdroot,'simulationcommand','stop')
    return
elseif strcmp(get_param([bdroot,'/Plant'],'blockchoice'),'Model')
    choice=get_param([bdroot,'/Plant/Model/Motion System'],'blockchoice');
    if strcmp(choice,'LTI Model') || ...
       strcmp(choice,'Simscape Model') || ... 
       strcmp(choice,'SimMechanics Model')
        disp(sprintf('Trajectory interface does not work with ''%s'' option',choice));
        disp('Try Rigid Body P2P without GUI');
        set_param(bdroot,'simulationcommand','stop')
        return
    end
end

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @RB_interface_OpeningFcn, ...
                   'gui_OutputFcn',  @RB_interface_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{:});
    clc % added to remove incorrect error warning text (geck: 392795)
end
% End initialization code - DO NOT EDIT


% --- Executes just before RB_interface is made visible.
function RB_interface_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 RB_interface (see VARARGIN)
initialize_gui(gcbf, handles, true);

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

% Update handles structure
guidata(hObject, handles);

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


% --- Outputs from this function are returned to the command line.
function varargout = RB_interface_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 button press in enable.
function enable_Callback(hObject, eventdata, handles)
% hObject    handle to enable (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
enable=get(hObject,'Value');
assignin('base','enable',enable);
set_param(bdroot,'simulationcommand','update')
if enable==0
    set(handles.status,'String','Status: Trajectory planner disabled')
else
    set(handles.status,'String','Status: Trajectory planner enabled, press ''Single Step''')
end

function stepsize_Callback(hObject, eventdata, handles)
% hObject    handle to stepsize (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 stepsize as text
%        str2double(get(hObject,'String')) returns contents of stepsize as a double
stepsize=str2num(get(hObject,'string'));
if stepsize>1000
    set(handles.status,'String','Status: Step size set to maximum value')
    stepsize=1000;
elseif stepsize<1
    set(handles.status,'String','Status: Step size set to minimum value')
    stepsize=1;
else
    set(handles.status,'String','Status: Step size adjusted')
end
assignin('base','stepsize',stepsize);
set_param(bdroot,'simulationcommand','update')
set(handles.stepsize,'String',num2str(stepsize,'%.4g'))

% --- Executes during object creation, after setting all properties.
function stepsize_CreateFcn(hObject, eventdata, handles)
% hObject    handle to stepsize (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 velocity_Callback(hObject, eventdata, handles)
% hObject    handle to velocity (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 velocity as text
%        str2double(get(hObject,'String')) returns contents of velocity as a double
vmax=str2num(get(hObject,'string'));
if vmax>250
    set(handles.status,'String','Status: Max Velocity set to maximum value')
    vmax=250;
elseif vmax<1
    set(handles.status,'String','Status: Max Velocity set to minimum value')
    vmax=1;
else
    set(handles.status,'String','Status: Max Velocity adjusted')
end    
assignin('base','vmax',vmax);
set_param(bdroot,'simulationcommand','update')
set(handles.velocity,'String',num2str(vmax,'%.4g'))

% --- Executes during object creation, after setting all properties.
function velocity_CreateFcn(hObject, eventdata, handles)
% hObject    handle to velocity (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 acceleration_Callback(hObject, eventdata, handles)
% hObject    handle to acceleration (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 acceleration as text
%        str2double(get(hObject,'String')) returns contents of acceleration as a double
amax=str2num(get(hObject,'string'));
if amax>5000
    set(handles.status,'String','Status: Max Acceleration set to maximum value')
    amax=5000;
elseif amax<1
    set(handles.status,'String','Status: Max Acceleration set to minimum value')
    amax=1;
else
    set(handles.status,'String','Status: Max Acceleration adjusted')
end
assignin('base','amax',amax);
set_param(bdroot,'simulationcommand','update')
set(handles.acceleration,'String',num2str(amax,'%.4g'))

% --- Executes during object creation, after setting all properties.
function acceleration_CreateFcn(hObject, eventdata, handles)
% hObject    handle to acceleration (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 reset.
function reset_Callback(hObject, eventdata, handles)
% hObject    handle to reset (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
%
reset_gui(gcbf, handles)


% --- Executes on button press in oldspec.
function oldspec_Callback(hObject, eventdata, handles)
% hObject    handle to oldspec (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.velocity,     'String', '150');
assignin('base','vmax',              150);
set(handles.acceleration, 'String', '2000');
assignin('base','amax',              2000);
set(handles.settlingtime, 'String', '2');
assignin('base','Tsettle',           2);
% Update parameters
set_param(bdroot,'simulationcommand','update')
set(handles.status,'String','Status: Parameters set to old specifications')
% Update handles structure
guidata(handles.figure1, handles);


function settlingtime_Callback(hObject, eventdata, handles)
% hObject    handle to settlingtime (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 settlingtime as text
%        str2double(get(hObject,'String')) returns contents of settlingtime as a double
set(handles.status,'String','Status: Settling time adjusted')
assignin('base','Tsettle',str2num(get(hObject,'string')));
set_param(bdroot,'simulationcommand','update')

% --- Executes during object creation, after setting all properties.
function settlingtime_CreateFcn(hObject, eventdata, handles)
% hObject    handle to settlingtime (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 totalmove.
function totalmove_Callback(hObject, eventdata, handles)
% hObject    handle to totalmove (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 totalmove
set(handles.status,'String','Status: ''Settling time'' determines total move time')
assignin('base','settlingchoice',2);
set(handles.totalmove,    'Value' , 1);
set(handles.settling,     'Value' , 0);
set_param(bdroot,'simulationcommand','update')

% --- Executes on button press in settling.
function settling_Callback(hObject, eventdata, handles)
% hObject    handle to settling (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 settling
set(handles.status,'String','Status: ''Settling time'' determines waiting time')
assignin('base','settlingchoice',1);
set(handles.totalmove,    'Value' , 0);
set(handles.settling,     'Value' , 1);
set_param(bdroot,'simulationcommand','update')

% --- Executes on button press in singlestep.
function singlestep_Callback(hObject, eventdata, handles)
% hObject    handle to singlestep (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
reqmove=evalin('base','reqmove');
enable=evalin('base','enable');
if enable==0
    set(handles.status,'String','Warning: ''Single Step'' triggered, but trajectory planner disabled')
else
    set(handles.status,'String','Status: ''Single Step'' triggered')
    if reqmove==0
        assignin('base','reqmove',1);
    else
        assignin('base','reqmove',0);
    end    
    set_param(bdroot,'simulationcommand','update')
end

% --- Executes on button press in continuous.
function continuous_Callback(hObject, eventdata, handles)
% hObject    handle to continuous (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 continuous
continuous=get(hObject,'Value');
if continuous==0
    set(handles.status,'String','Status: Single step mode, press ''Single Step'' to step')
else
    set(handles.status,'String','Status: Continuous mode, press ''Single Step'' to start')
end    
assignin('base','continuous',continuous);
set_param(bdroot,'simulationcommand','update')

% --- Executes on button press in totalmove.
function RB_ff_Callback(hObject, eventdata, handles)
% hObject    handle to totalmove (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 totalmove
set(handles.status,'String','Status: Rigid-Body feedforward activated')
assignin('base','FFactive',1);
assignin('base','orderchoice',0);
set(handles.RB_ff,    'Value' , 0);
set(handles.RB_ff,    'Value' , 1);
set(handles.ffinactive,  'Value' , 0);
set_param(bdroot,'simulationcommand','update')

% --- Executes on button press in totalmove.
function ffinactive_Callback(hObject, eventdata, handles)
% hObject    handle to totalmove (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 totalmove
set(handles.status,'String','Status: Feedforward de-activated')
assignin('base','FFactive',0);

set(handles.RB_ff,    'Value' , 0);

set(handles.ffinactive,  'Value' , 1);
set_param(bdroot,'simulationcommand','update')


% --------------------------------------------------------------------
function initialize_gui(fig_handle, handles, isreset)
% If the stepsize field is present and the reset flag is false, it means
% we are we are just re-initializing a GUI by calling it from the cmd line
% while it is up. So, bail out as we dont want to reset the data.
if isfield(handles, 'stepsize') && ~isreset
    return;
end

reset_gui(fig_handle, handles)
assignin('base','reqmove',1);

if strcmp(get_param(bdroot, 'SimulationMode'),'normal')
    set(handles.startsim,   'Value'  , 1);
    set(handles.startsim,   'String' , 'Stop');
    set(handles.status,'String','Status: Simulating')
else
    set(handles.startsim,   'Value'  , 0);
    set(handles.startsim,   'String' , 'Start');
    set(handles.status,'String','Status: Ready, press start button')
end

% Update parameters
set_param(bdroot,'simulationcommand','update')
% Update handles structure
guidata(handles.figure1, handles);

function reset_gui(fig_handle, handles)
% reset the GUI to the default values.
set(handles.stepsize,     'String', '100');
assignin('base','stepsize',          100);
set(handles.velocity,     'String', '250');
assignin('base','vmax',              250);
set(handles.acceleration, 'String', '5000');
assignin('base','amax',              5000);

set(handles.settlingtime, 'String', '1');
assignin('base','Tsettle',           1);

set(handles.totalmove,    'Value' , 1);
set(handles.settling,     'Value' , 0);
assignin('base','settlingchoice',2);

set(handles.RB_ff,    'Value' , 1);

set(handles.ffinactive,  'Value' , 0);
assignin('base','FFactive',1);
assignin('base','orderchoice',0);

set(handles.enable,       'Value' , 1);
assignin('base','enable',1);
set(handles.continuous,   'Value' , 1);
% set(handles.continuous,   'String' , 'Single');
assignin('base','continuous',1);
% assignin('base','reqmove',1);

% Update parameters
set_param(bdroot,'simulationcommand','update')
set(handles.status,'String','Status: Parameters set to default values')
% Update handles structure
guidata(handles.figure1, handles);

% --- Executes on button press in startsim.
function startsim_Callback(hObject, eventdata, handles)
% hObject    handle to startsim (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 startsim
startsim=get(hObject,'String');
if strcmp(startsim,'Start')
    if strcmp(get_param(bdroot, 'SimulationMode'),'external')
        set(handles.status,'String','Status: Connecting...')
        set_param(bdroot,'simulationcommand','connect')
        set(handles.status,'String','Status: Running in real-time')
    else
        set(handles.status,'String','Status: Simulating')
    end
    set_param(bdroot,'simulationcommand','start')
    set(handles.startsim,   'String' , 'Stop');
else
    if strcmp(get_param(bdroot, 'SimulationMode'),'external') & get(handles.enable,'Value')==1
        set(handles.status,'String','Warning: First disable trajectory planner')
    else
        pause(1);
        reset_gui(gcbf, handles);
        set_param(bdroot,'simulationcommand','stop')
        set(handles.startsim,   'String' , 'Start');
        set(handles.status,'String','Status: Stopped')
    end
end