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

% Last Modified by GUIDE v2.5 12-Mar-2008 14:40:03

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

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

% Update handles structure
guidata(hObject, handles);

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


% --- Outputs from this function are returned to the command line.
function varargout = BallControl_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 during object creation, after setting all properties.
function editangle1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editangle1 (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 editvelocity1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editvelocity1 (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 editaxis1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editaxis1 (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 editscript_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editscript (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 editport_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editport (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 dispaxis_CreateFcn(hObject, eventdata, handles)
% hObject    handle to dispaxis (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called


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


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


%----------------------------------------------------------------
% --- Executes on button press in connectbutton.
function connectbutton_Callback(hObject, eventdata, handles)
% hObject    handle to connectbutton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global s connected cAxis invertAxis t;
delete(instrfind)
s = serial(get(handles.editport,'String'),'BAUD',19200);           % Create serial object (PORT Dependent)
fopen(s)                                    % Open the serial port for r/w
cAxis = 0;
invertAxis = 0;
%enable auto/manual radios
set([handles.disconnectbutton, handles.radiomanual,...
     handles.radioautomatic],'Enable','on');
%enable commands for auto/manual
set(handles.connectbutton,'Enable','off');
if (get(handles.radiomanual, 'Value') == 1) %manual mode
    set([handles.homebutton, handles.repeatbutton,...
         handles.rotatebutton1, handles.rotatebutton2,...
         handles.axisbutton1, handles.axisbutton2,...
         handles.editvelocity1, handles.editvelocity2,...
         handles.editangle1, handles.editangle2,...
         handles.editaxis1, handles.editaxis2,...
         handles.editrepose],'Enable','on');
else %automatic mode
    set([handles.editscript, handles.loadbutton,...
         handles.runbutton],'Enable','on');
end
%should we start reading data from pic here? some sort of time-delayed
%t = timer('ExecutionMode','fixedRate','TimerFcn','fprintf(s, ''%s'', ''Z''); fprintf(fscanf(s))', 'Period', 1.0);
%disp('timer succesfully set up');
%start(t);
%fprintf(fscanf(s))                      % Read Data back from PIC


% --- Executes on button press in disconnectbutton.
function disconnectbutton_Callback(hObject, eventdata, handles)
% disconnects serial connection and greys out commands
global s t;
fclose(s);                                  % Close the serial port
delete(s);                                  % Delete the serial object
%stop(t);
%delete(instrfind)                          % if doesn't delete properly?
set([handles.disconnectbutton, handles.radiomanual,...
     handles.radioautomatic, handles.homebutton,...
     handles.rotatebutton1, handles.rotatebutton2,...
     handles.axisbutton1, handles.axisbutton2,...
     handles.editvelocity1, handles.editvelocity2,...
     handles.editangle1, handles.editangle2,...
     handles.editaxis1, handles.editaxis2,...
     handles.editscript, handles.editrepose,...
     handles.loadbutton, handles.runbutton,...
     handles.stopbutton, handles.repeatbutton],'Enable','off');
set(handles.connectbutton,'Enable','on');


% --- Executes on button press in radiomanual.
function radiomanual_Callback(hObject, eventdata, handles)
% un-greys appropriate commands for manual control
set([handles.homebutton, handles.repeatbutton,...
     handles.rotatebutton1, handles.rotatebutton2,...
     handles.axisbutton1, handles.axisbutton2,...
     handles.editvelocity1, handles.editvelocity2,...
     handles.editangle1, handles.editangle2,...
     handles.editaxis1, handles.editaxis2,...
     handles.editrepose],'Enable','on');
set([handles.editscript, handles.loadbutton,...
     handles.runbutton, handles.stopbutton],'Enable','off');
%should also cancel any actions in the automatic queue.


% --- Executes on button press in radioautomatic.
function radioautomatic_Callback(hObject, eventdata, handles)
% un-greys appropriate commands for automatic control
set([handles.editscript, handles.loadbutton,...
     handles.runbutton],'Enable','on');
set([handles.homebutton, handles.repeatbutton,...
     handles.rotatebutton1, handles.rotatebutton2,...
     handles.axisbutton1, handles.axisbutton2,...
     handles.editvelocity1, handles.editvelocity2,...
     handles.editangle1, handles.editangle2,...
     handles.editaxis1, handles.editaxis2,...
     handles.editrepose],'Enable','off');

 
% --- Executes on button press in homebutton.
function homebutton_Callback(hObject, eventdata, handles)
% hObject    handle to homebutton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global s;
sendData(s, 0, 0, 0, 0); %will send 'A000000000' to PIC

% --- Executes on button press in rotatebutton1.
function rotatebutton1_Callback(hObject, eventdata, handles)
% hObject    handle to rotatebutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global s rollDir turndirection;

xDes = round(str2num(get(handles.editangle1, 'String')));
vDes = round(str2num(get(handles.editvelocity1, 'String')));
repose = round(str2num(get(handles.editrepose, 'String')));

[xDes,rollDir] = rolldirection(xDes);
sendData(s, rollDir, xDes, repose, vDes); %will send 'ADXXXRRVVV' to PIC

% --- Executes on button press in axisbutton1.
function axisbutton1_Callback(hObject, eventdata, handles)
% hObject    handle to axisbutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global s cAxis turndirection invertAxis;

aDes = round(str2num(get(handles.editaxis1, 'String')));
if (aDes < 0)
        turndirection = 4;
        aDes = abs(aDes);
else turndirection = 3;
end
if((cAxis + aDes> 90) || (cAxis - aDes < -90))
    aDes = abs(aDes - 180) %please don't put in angles >180 (why would you?)
    turndirection = 7 - turndirection;
    invertAxis = 1-invertAxis;
end
sendData(s, turndirection, aDes, 0, 0); %will send 'ADXXX00000' to PIC
if (turndirection == 3)
    cAxis = cAxis + aDes;
else cAxis = cAxis - aDes;
end


% --- Executes on button press in rotatebutton2.
function rotatebutton2_Callback(hObject, eventdata, handles)
% hObject    handle to rotatebutton2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global s;
global rollDir;
xDes = round(str2num(get(handles.editangle2, 'String')));
vDes = round(str2num(get(handles.editvelocity2, 'String')));
repose = round(str2num(get(handles.editrepose, 'String')));

[xDes,rollDir] = rolldirection(xDes);
sendData(s, rollDir, xDes, repose, vDes); %will send 'ADXXXRRVVV' to PIC

% --- Executes on button press in axisbutton2.
function axisbutton2_Callback(hObject, eventdata, handles)
% hObject    handle to axisbutton2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
global s cAxis turndirection invertAxis;

aDes = round(str2num(get(handles.editaxis2, 'String')));
if (aDes < 0)
        turndirection = 4;
        aDes = abs(aDes);
else turndirection = 3;
end
if((cAxis + aDes> 90) || (cAxis - aDes < -90))
    aDes = abs(aDes - 180) %please don't put in angles >180 (why would you?)
    turndirection = 7 - turndirection;
    invertAxis = 1-invertAxis;
end
sendData(s, turndirection, aDes, 0, 0); %will send 'ADXXX00000' to PIC
if (turndirection == 3)
    cAxis = cAxis + aDes;
else cAxis = cAxis - aDes;
end

% --- Executes on button press in loadbutton.
function loadbutton_Callback(hObject, eventdata, handles)
% hObject    handle to loadbutton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


% --- Executes on button press in runbutton.
function runbutton_Callback(hObject, eventdata, handles)
% hObject    handle to runbutton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.stopbutton,'Enable','on')
set([handles.runbutton, handles.loadbutton,...
     handles.editscript],'Enable','off')

 
% --- Executes on button press in stopbutton.
function stopbutton_Callback(hObject, eventdata, handles)
% hObject    handle to stopbutton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.stopbutton,'Enable','off')
set([handles.runbutton, handles.loadbutton,...
     handles.editscript],'Enable','on')

% --- Executes on button press in repeatbutton.
function repeatbutton_Callback(hObject, eventdata, handles)
% hObject    handle to repeatbutton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

%should run axis1, rotate1, axis2, rotate2 in order.
%for teh demo today, this will hopefully get data back from pic
global s;
fprintf(s, '%s', 'Z');
fprintf(fscanf(s))


% --- function to send data via serial
function sendData(s, d1, d2, d3, d4)
data1 = num2str(d1);
data2 = num2str(d2);
data3 = num2str(d3);
data4 = num2str(d4);
%pads data2,data3,data4 with zeros until they're 2-3 characters long
while (size(data2,2) < 3)
    data2 = ['0' data2];
end
while (size(data3,2) < 2)
    data3 = ['0' data3];
end
while (size(data4,2) < 3)
    data4 = ['0' data4];
end
data = [data1 data2 data3 data4];
fprintf(s, '%s', 'A');
pause(0.05);
for (i = 1:1:(size(data,2)))
    fprintf(s, '%s', data(i));
    pause(0.05); %pauses for 0.01s before sending next byte
end
disp('Done sending data')
fprintf(fscanf(s))                      % Read Data back from PIC


% --- function to set roll directions
function [x,dir] = rolldirection(x)
global cAxis invertAxis;

if (invertAxis == 1 && x > 0)
    x = x;
    dir = 2;
elseif (invertAxis == 1 && x < 0)
    x = -x;
    dir = 1;
elseif (invertAxis == 0 && x > 0)
    x = x;
    dir = 1;
elseif (invertAxis == 0 && x < 0)
    x = -x;
    dir = 2;
end
     
function editport_Callback(hObject, eventdata, handles)
% hObject    handle to editport (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 editport as text
%        str2double(get(hObject,'String')) returns contents of editport as a double

function editangle1_Callback(hObject, eventdata, handles)
% hObject    handle to editangle1 (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 editangle1 as text
%        str2double(get(hObject,'String')) returns contents of editangle1 as a double


function editvelocity1_Callback(hObject, eventdata, handles)
% hObject    handle to editvelocity1 (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 editvelocity1 as text
%        str2double(get(hObject,'String')) returns contents of editvelocity1 as a double




function editaxis1_Callback(hObject, eventdata, handles)
% hObject    handle to editaxis1 (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 editaxis1 as text
%        str2double(get(hObject,'String')) returns contents of editaxis1 as a double

function editscript_Callback(hObject, eventdata, handles)
% hObject    handle to editscript (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 editscript as text
%        str2double(get(hObject,'String')) returns contents of editscript as a double

function editrepose_Callback(hObject, eventdata, handles)
% hObject    handle to editrepose (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 editrepose as text
%        str2double(get(hObject,'String')) returns contents of editrepose as a double

% --- Executes during object creation, after setting all properties.
function editrepose_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editrepose (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 editangle2_Callback(hObject, eventdata, handles)
% hObject    handle to editangle2 (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 editangle2 as text
%        str2double(get(hObject,'String')) returns contents of editangle2 as a double

% --- Executes during object creation, after setting all properties.
function editangle2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editangle2 (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 editvelocity2_Callback(hObject, eventdata, handles)
% hObject    handle to editvelocity2 (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 editvelocity2 as text
%        str2double(get(hObject,'String')) returns contents of editvelocity2 as a double

% --- Executes during object creation, after setting all properties.
function editvelocity2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editvelocity2 (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 editaxis2_Callback(hObject, eventdata, handles)
% hObject    handle to editaxis2 (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 editaxis2 as text
%        str2double(get(hObject,'String')) returns contents of editaxis2 as a double

% --- Executes during object creation, after setting all properties.
function editaxis2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to editaxis2 (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

