FILDE CODE:

code matlab
function varargout = serial_GUI(varargin)
% Author: Roger Yeh
% Copyright 2010 MathWorks, Inc.
% Version: 1.0 | Date: 2010.01.13
% SERIAL_GUI M-file for serial_GUI.fig
% SERIAL_GUI, by itself, creates a new SERIAL_GUI or raises the existing
% singleton*.
% H = SERIAL_GUI returns the handle to a new SERIAL_GUI or the handle to
% the existing singleton*.
% SERIAL_GUI('CALLBACK',hObject,eventData,handles,...) calls the local
% function named CALLBACK in SERIAL_GUI.M with the given input arguments.
%
% SERIAL_GUI('Property','Value',...) creates a new SERIAL_GUI or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before serial_GUI_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property application
% stop. All inputs are passed to serial_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
% Edit the above text to modify the response to help serial_GUI
% Last Modified by GUIDE v2.5 05-Jun-2019 00:27:20
% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @serial_GUI_OpeningFcn, ...
'gui_OutputFcn', @serial_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 serial_GUI is made visible.
function serial_GUI_OpeningFcn(hObject, eventdata, handles, varargin)
serialPorts = instrhwinfo('serial');
nPorts = length(serialPorts.SerialPorts);
set(handles.portList, 'String', ...
[{'Select a port'} ; serialPorts.SerialPorts ]);
set(handles.portList, 'Value', 2);
set(handles.history_box, 'String', cell(1));
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
% UIWAIT makes serial_GUI wait for user response (see UIRESUME)
% uiwait(handles.figure1);
% --- Outputs from this function are returned to the command line.
function varargout = serial_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 selection change in portList.
function portList_Callback(hObject, eventdata, handles)
% hObject handle to portList (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: contents = cellstr(get(hObject,'String')) returns portList contents as cell array
% contents{get(hObject,'Value')} returns selected item from portList
% --- Executes during object creation, after setting all properties.
function portList_CreateFcn(hObject, eventdata, handles)
% hObject handle to portList (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: listbox 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 history_box_Callback(hObject, eventdata, handles)
% hObject handle to history_box (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 history_box as text
% str2double(get(hObject,'String')) returns contents of history_box as a double
% --- Executes during object creation, after setting all properties.
function history_box_CreateFcn(hObject, eventdata, handles)
% hObject handle to history_box (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 Tx_send_Callback(hObject, eventdata, handles)
fprintf(handles.serConn, 'a');
TxText = get(handles.Tx_send, 'String');
fprintf(handles.serConn, TxText);
currList = get(handles.history_box, 'String');
set(handles.history_box, 'String', ...
[currList ; ['Sent @ ' datestr(now) ': ' TxText] ]);
set(handles.history_box, 'Value', length(currList) + 1 );
set(hObject, 'String', '');
% --- Executes during object creation, after setting all properties.
function Tx_send_CreateFcn(hObject, eventdata, handles)
% hObject handle to Tx_send (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 rxButton.
function rxButton_Callback(hObject, eventdata, handles)
try
RxText = fscanf(handles.serConn);
currList = get(handles.history_box, 'String');
if length(RxText) < 1
RxText = 'Timeout @ ';
set(handles.history_box, 'String', ...
[currList ; [RxText datestr(now)] ]);
else
set(handles.history_box, 'String', ...
[currList ; ['Received @ ' datestr(now) ': ' RxText ] ]);
end
set(handles.history_box, 'Value', length(currList) + 1 );
catch e
disp(e)
end
function baudRateText_Callback(hObject, eventdata, handles)
% hObject handle to baudRateText (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 baudRateText as text
% str2double(get(hObject,'String')) returns contents of baudRateText as a double
% --- Executes during object creation, after setting all properties.
function baudRateText_CreateFcn(hObject, eventdata, handles)
% hObject handle to baudRateText (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 connectButton.
function connectButton_Callback(hObject, eventdata, handles)
if strcmp(get(hObject,'String'),'Connect') % currently disconnected
serPortn = get(handles.portList, 'Value');
if serPortn == 1
errordlg('Select valid COM port');
else
serList = get(handles.portList,'String');
serPort = serList{serPortn};
serConn = serial(serPort, 'TimeOut', 1, ...
'BaudRate', str2num(get(handles.baudRateText, 'String')));
try
fopen(serConn);
handles.serConn = serConn;
% enable Tx text field and Rx button
set(handles.Tx_send, 'Enable', 'On');
set(handles.Tx_send1, 'Enable', 'On');
set(handles.Tx_send2, 'Enable', 'On');
set(handles.rxButton, 'Enable', 'On');
set(handles.DHT, 'Enable', 'On');
set(handles.DHN, 'Enable', 'On');
set(handles.tetha1, 'Enable', 'On');
set(handles.tetha2, 'Enable', 'On');
set(handles.tetha3, 'Enable', 'On');
set(handles.editpx, 'Enable', 'On');
set(handles.editpy, 'Enable', 'On');
set(handles.editpz, 'Enable', 'On');
set(hObject, 'String','Disconnect')
catch e
errordlg(e.message);
end
end
else
set(handles.Tx_send, 'Enable', 'Off');
set(handles.Tx_send1, 'Enable', 'Off');
set(handles.Tx_send2, 'Enable', 'Off');
set(handles.rxButton, 'Enable', 'Off');
set(handles.DHT, 'Enable', 'Off');
set(handles.DHN, 'Enable', 'Off');
set(handles.tetha1, 'Enable', 'Off');
set(handles.tetha2, 'Enable', 'Off');
set(handles.tetha3, 'Enable', 'Off');
set(handles.editpx, 'Enable', 'Off');
set(handles.editpy, 'Enable', 'Off');
set(handles.editpz, 'Enable', 'Off');
set(hObject, 'String','Connect')
fclose(handles.serConn);
end
guidata(hObject, handles);
% --- Executes when user attempts to close figure1.
function figure1_CloseRequestFcn(hObject, eventdata, handles)
% hObject handle to figure1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
if isfield(handles, 'serConn')
fclose(handles.serConn);
end
% Hint: delete(hObject) closes the figure
delete(hObject);
% --- If Enable == 'on', executes on mouse press in 5 pixel border.
% --- Otherwise, executes on mouse press in 5 pixel border or over text2.
function text2_ButtonDownFcn(hObject, eventdata, handles)
% hObject handle to text2 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
function Tx_send1_Callback(hObject, eventdata, handles)
% hObject handle to Tx_send1 (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 Tx_send1 as text
% str2double(get(hObject,'String')) returns contents of Tx_send1 as a double
fprintf(handles.serConn, 'b');
TxText1 = get(handles.Tx_send1, 'String');
fprintf(handles.serConn, TxText1);
currList = get(handles.history_box, 'String');
set(handles.history_box, 'String', ...
[currList ; ['Sent @ ' datestr(now) ': ' TxText1] ]);
set(handles.history_box, 'Value', length(currList) + 1 );
set(hObject, 'String', '');
% --- Executes during object creation, after setting all properties.
function Tx_send1_CreateFcn(hObject, eventdata, handles)
% hObject handle to Tx_send1 (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 Tx_send2_Callback(hObject, eventdata, handles)
% hObject handle to Tx_send2 (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 Tx_send2 as text
% str2double(get(hObject,'String')) returns contents of Tx_send2 as a double
fprintf(handles.serConn, 'c');
TxText2 = get(handles.Tx_send2, 'String');
fprintf(handles.serConn, TxText2);
currList = get(handles.history_box, 'String');
set(handles.history_box, 'String', ...
[currList ; ['Sent @ ' datestr(now) ': ' TxText2] ]);
set(handles.history_box, 'Value', length(currList) + 1 );
set(hObject, 'String', '');
% --- Executes during object creation, after setting all properties.
function Tx_send2_CreateFcn(hObject, eventdata, handles)
% hObject handle to Tx_send2 (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 tetha1_Callback(hObject, eventdata, handles)
% hObject handle to tetha1 (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 tetha1 as text
% str2double(get(hObject,'String')) returns contents of tetha1 as a double
% --- Executes during object creation, after setting all properties.
function tetha1_CreateFcn(hObject, eventdata, handles)
% hObject handle to tetha1 (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 tetha2_Callback(hObject, eventdata, handles)
% hObject handle to tetha2 (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 tetha2 as text
% str2double(get(hObject,'String')) returns contents of tetha2 as a double
% --- Executes during object creation, after setting all properties.
function tetha2_CreateFcn(hObject, eventdata, handles)
% hObject handle to tetha2 (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 tetha3_Callback(hObject, eventdata, handles)
% hObject handle to tetha3 (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 tetha3 as text
% str2double(get(hObject,'String')) returns contents of tetha3 as a double
% --- Executes during object creation, after setting all properties.
function tetha3_CreateFcn(hObject, eventdata, handles)
% hObject handle to tetha3 (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 editpx_Callback(hObject, eventdata, handles)
% hObject handle to editpx (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 editpx as text
% str2double(get(hObject,'String')) returns contents of editpx as a double
% --- Executes during object creation, after setting all properties.
function editpx_CreateFcn(hObject, eventdata, handles)
% hObject handle to editpx (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 editpy_Callback(hObject, eventdata, handles)
% hObject handle to editpy (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 editpy as text
% str2double(get(hObject,'String')) returns contents of editpy as a double
% --- Executes during object creation, after setting all properties.
function editpy_CreateFcn(hObject, eventdata, handles)
% hObject handle to editpy (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 editpz_Callback(hObject, eventdata, handles)
% hObject handle to editpz (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 editpz as text
% str2double(get(hObject,'String')) returns contents of editpz as a double
% --- Executes during object creation, after setting all properties.
function editpz_CreateFcn(hObject, eventdata, handles)
% hObject handle to editpz (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 DHT.
function DHT_Callback(hObject, eventdata, handles)
% hObject handle to DHT (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
fprintf(handles.serConn, 'a');
d1 = get(handles.tetha1, 'String');
fprintf(handles.serConn, d1);
currList = get(handles.history_box, 'String');
set(handles.history_box, 'String', ...
[currList ; ['Sent @ ' datestr(now) ': ' d1] ]);
set(handles.history_box, 'Value', length(currList) + 1 );
set(hObject, 'String', '');
fprintf(handles.serConn, 'b');
tetha2 = get(handles.tetha2, 'String');
fprintf(handles.serConn, tetha2);
currList = get(handles.history_box, 'String');
set(handles.history_box, 'String', ...
[currList ; ['Sent @ ' datestr(now) ': ' tetha2] ]);
set(handles.history_box, 'Value', length(currList) + 1 );
set(hObject, 'String', '');
fprintf(handles.serConn, 'c');
tetha3 = get(handles.tetha3, 'String');
fprintf(handles.serConn, tetha3);
currList = get(handles.history_box, 'String');
set(handles.history_box, 'String', ...
[currList ; ['Sent @ ' datestr(now) ': ' tetha3] ]);
set(handles.history_box, 'Value', length(currList) + 1 );
set(hObject, 'String', '');
ModelName = 'robottest';
global var;
d1=str2num(d1);
tetha2=str2num(tetha2);
tetha3=str2num(tetha3);
set_param([ModelName '/Slider Gain2'],'Gain',num2str(d1))%set gia tri ra slider
set_param([ModelName '/Slider Gain'],'Gain',num2str(tetha2))
set_param([ModelName '/Slider Gain1'],'Gain',num2str(tetha3))
d=[1 0 0 0 ; 0 1 0 0 ; 0 0 1 d1; 0 0 0 1];
Te1=[cosd(tetha2) -sind(tetha2) 0 115*cosd(tetha2) ;sind(tetha2) cosd(tetha2) 0
115*sind(tetha2) ;0 0 1 0;0 0 0 1];
Te2=[cosd(tetha3) -sind(tetha3) 0 130*cosd(tetha3) ;sind(tetha3) cosd(tetha3) 0
130*sind(tetha3) ;0 0 1 0 ;0 0 0 1];
c=d*Te1*Te2;
px=c(1,4);
py=c(2,4);
pz=c(3,4);
set(handles.editpx,'string',num2str(px));
set(handles.editpy,'string',num2str(py));
set(handles.editpz,'string',num2str(pz));
% --- Executes on button press in DHN.
function DHN_Callback(hObject, eventdata, handles)
% hObject handle to DHN (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
ModelName = 'robottest';
global var;
px = get(handles.editpx, 'String');
px = str2num(px)
py=get(handles.editpy, 'string');
py = str2num(py)
pz=get(handles.editpz, 'string');
pz = str2num(pz)
d=pz;
l1=115;
l2=130;
c=sqrt((px)^2+(py)^2);
c3=(c^2-l1^2-l2^2)/(2*l1*l2);
s3=sqrt(1-(c3)^2);
tetha3=atan2d(s3,c3);
cb=(c^2+l1^2-l2^2)/(2*c*l1);
sb=sqrt(1-(cb)^2);
be=atan2d(sb,cb);
al=atan2d(py,px);
if (tetha3>0)
tetha2=al-be;
if (tetha3<0)
tetha2=al+be;
end
end
set(handles.tetha1,'string',num2str(d));
set(handles.tetha2,'string',num2str(tetha2));
set(handles.tetha3,'string',num2str(tetha3));
function edit12_Callback(hObject, eventdata, handles)
% hObject handle to edit12 (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 edit12 as text
% str2double(get(hObject,'String')) returns contents of edit12 as a double
% --- Executes during object creation, after setting all properties.
function edit12_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit12 (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 pushbutton5.
function pushbutton5_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton5 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
ModelName = 'robottest';
global var;
fprintf(handles.serConn, 'ff');
fprintf(handles.serConn, 'a');
d1='3';
fprintf(handles.serConn, d1);
pause(1);
fprintf(handles.serConn, 'b');
tetha2='45';
fprintf(handles.serConn, tetha2);
tetha2=str2num(tetha2);
set_param([ModelName '/Slider Gain'],'Gain',num2str(tetha2));
fprintf(handles.serConn, 'c');
tetha3='45';
fprintf(handles.serConn, tetha3);
tetha3=str2num(tetha3);
set_param([ModelName '/Slider Gain1'],'Gain',num2str(tetha3));
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'oo');
% buoc nha vat
fprintf(handles.serConn, 'a');
d1='3';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'b');
tetha2='-45';
fprintf(handles.serConn, tetha2);
tetha2=str2num(tetha2);
set_param([ModelName '/Slider Gain'],'Gain',num2str(tetha2));
fprintf(handles.serConn, 'c');
tetha3='-45';
fprintf(handles.serConn, tetha3);
tetha3=str2num(tetha3);
set_param([ModelName '/Slider Gain1'],'Gain',num2str(tetha3));
pause(1);
%//////////////
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
pause(2);
fprintf(handles.serConn, 'ff');
pause(0.5);
fprintf(handles.serConn, 'a');
d1='3';
fprintf(handles.serConn, d1);
pause(2);
fprintf(handles.serConn, 'b');
tetha2='0';
fprintf(handles.serConn, tetha2);
tetha2=str2num(tetha2);
set_param([ModelName '/Slider Gain'],'Gain',num2str(tetha2));
fprintf(handles.serConn, 'c');
tetha3='0';
fprintf(handles.serConn, tetha3);
tetha3=str2num(tetha3);
set_param([ModelName '/Slider Gain1'],'Gain',num2str(tetha3));
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
fprintf(handles.serConn, 'oo');
% --- Executes on button press in pushbutton6.
function pushbutton6_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton6 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
fprintf(handles.serConn, 'ff');
fprintf(handles.serConn, 'a');
d1='5';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'b');
tetha2='35';
fprintf(handles.serConn, tetha2);
fprintf(handles.serConn, 'c');
tetha3='55';
fprintf(handles.serConn, tetha3);
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'oo');
% buoc nha vat
fprintf(handles.serConn, 'a');
d1='5';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'b');
tetha2='-55';
fprintf(handles.serConn, tetha2);
fprintf(handles.serConn, 'c');
tetha3='-35';
fprintf(handles.serConn, tetha3);
pause(1);
%//////////////
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'ff');
fprintf(handles.serConn, 'a');
d1='5';
fprintf(handles.serConn, d1);
pause(3);
%VAT 2
fprintf(handles.serConn, 'b');
tetha2='25';
fprintf(handles.serConn, tetha2);
fprintf(handles.serConn, 'c');
tetha3='45';
fprintf(handles.serConn, tetha3);
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'oo');
% buoc nha vat
fprintf(handles.serConn, 'a');
d1='5';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'b');
tetha2='-65';
fprintf(handles.serConn, tetha2);
fprintf(handles.serConn, 'c');
tetha3='-45';
fprintf(handles.serConn, tetha3);
pause(1);
%//////////////
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'ff');
fprintf(handles.serConn, 'a');
d1='5';
fprintf(handles.serConn, d1);
pause(3);
%VAT 3
fprintf(handles.serConn, 'b');
tetha2='15';
fprintf(handles.serConn, tetha2);
fprintf(handles.serConn, 'c');
tetha3='35';
fprintf(handles.serConn, tetha3);
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'oo');
% buoc nha vat
fprintf(handles.serConn, 'a');
d1='5';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'b');
tetha2='-75';
fprintf(handles.serConn, tetha2);
fprintf(handles.serConn, 'c');
tetha3='-55';
fprintf(handles.serConn, tetha3);
pause(1);
%//////////////
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'ff');
fprintf(handles.serConn, 'a');
d1='5';
fprintf(handles.serConn, d1);
pause(2);
fprintf(handles.serConn, 'b');
tetha2='0';
fprintf(handles.serConn, tetha2);
fprintf(handles.serConn, 'c');
tetha3='0';
fprintf(handles.serConn, tetha3);
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
fprintf(handles.serConn, 'oo');
% --- Executes on button press in pushbutton7.
function pushbutton7_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton7 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
fprintf(handles.serConn,'ff');
fprintf(handles.serConn, 'a');
d1 ='3';
fprintf(handles.serConn, d1);
pause(2);
fprintf(handles.serConn, 'b');
tetha2 = get(handles.tetha2 , 'String');
fprintf(handles.serConn,tetha2 );
fprintf(handles.serConn, 'c');
tetha3 = get(handles.tetha3, 'String');
fprintf(handles.serConn, tetha3);
pause(2);
fprintf(handles.serConn, 'a');
d1 = get(handles.tetha1, 'String');
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn,'oo');
pause(1);
%nha vat
fprintf(handles.serConn, 'a');
d1='3';
fprintf(handles.serConn, d1);
pause(3);
fprintf(handles.serConn, 'b');
tetha2='-45';
fprintf(handles.serConn, tetha2);
fprintf(handles.serConn, 'c');
tetha3='-45';
fprintf(handles.serConn, tetha3);
pause(1);
%//////////////
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
pause(2);
fprintf(handles.serConn, 'ff');
pause(2);
fprintf(handles.serConn, 'a');
d1='3';
fprintf(handles.serConn, d1);
pause(2);
fprintf(handles.serConn, 'b');
tetha2='0';
fprintf(handles.serConn, tetha2);
fprintf(handles.serConn, 'c');
tetha3='0';
fprintf(handles.serConn, tetha3);
fprintf(handles.serConn, 'a');
d1='0';
fprintf(handles.serConn, d1);
fprintf(handles.serConn, 'oo');
% --- Executes on button press in pushbutton8.
function pushbutton8_Callback(hObject, eventdata, handles)
% hObject handle to pushbutton8 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
if strcmp(get(hObject,'String'),'gap vat')
fprintf(handles.serConn, 'oo');
set(hObject, 'String','tay mo')
else
fprintf(handles.serConn, 'ff');
end
code master
#include <Servo.h>
#include <Wire.h>
int servoAVal;
int servoBVal;
String readString;
#define SERVO_PIN 9
Servo gServo;
void setup() {
// put your setup code here, to run once:
gServo.attach(SERVO_PIN);
Wire.begin();
Serial.begin(9600);
}
void loop() {
while (Serial.available ()>2) {
delay(20);
char serialRead = Serial.read ();
if (serialRead == 'a') {
servoAVal = Serial.parseInt ();
Serial.print( servoAVal);
int16_t bigNum = servoAVal;
byte myArray[2];
myArray[0] = (bigNum >> 8) & 0xFF;
myArray[1] = bigNum & 0xFF;
Wire.beginTransmission(2); // transmit to device #8
Wire.write(myArray, 2);
Wire.endTransmission(); // stop transmitting
delay(100);
}
if (serialRead == 'b') {
servoBVal = Serial.parseInt ();
Serial.print( servoBVal);
int16_t bigNum = servoBVal;
byte myArray[2];
myArray[0] = (bigNum >> 8) & 0xFF;
myArray[1] = bigNum & 0xFF;
Wire.beginTransmission(3); // transmit to device #8
Wire.write(myArray, 2);
Wire.endTransmission(); // stop transmitting
}
if (serialRead == 'c') {
servoBVal = Serial.parseInt ();
Serial.print( servoBVal);
int16_t bigNum = servoBVal;
byte myArray[2];
myArray[0] = (bigNum >> 8) & 0xFF;
myArray[1] = bigNum & 0xFF;
Wire.beginTransmission(4); // transmit to device #8
Wire.write(myArray, 2);
Wire.endTransmission(); // stop transmitting
}
if (serialRead == 'o') {
gServo.write(130);
}
if (serialRead == 'f') {
gServo.write(0);
}}}
Code Slave1
#include <Wire.h>
#include <PID_v1.h>
#define MotEnable 6 //Motor Enamble pin Runs on PWM signal
#define MotFwd 4 // Motor Forward pin
#define MotRev 7 // Motor Reverse pin
int16_t bigNum;
int encoderPin1 = 2; //Encoder Output 'A' must connected with intreput pin of arduino.
int encoderPin2 = 3; //Encoder Otput 'B' must connected with intreput pin of arduino.
volatile int lastEncoded = 0; // Here updated value of encoder store.
volatile long encoderValue = 0; // Raw encoder value
//int PPR = 2340; // Encoder Pulse per revolution.
//int angle = 360; // Maximum degree of motion.
int REV = 0; // Set point REQUIRED ENCODER VALUE
int lastMSB = 0;
int lastLSB = 0;
double kp =1.4 , ki = 0.15 , kd = 0.005; // modify for optimal performance
double input = 0, output = 0, setpoint = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT);
void setup() {
Wire.begin(2);
Wire.onReceive(receiveEvent);
pinMode(MotEnable, OUTPUT);
pinMode(MotFwd, OUTPUT);
pinMode(MotRev, OUTPUT);
Serial.begin(9600); //initialize serial comunication
pinMode(encoderPin1, INPUT_PULLUP);
pinMode(encoderPin2, INPUT_PULLUP);
digitalWrite(encoderPin1, HIGH); //turn pullup resistor on
digitalWrite(encoderPin2, HIGH); //turn pullup resistor on
//call updateEncoder() when any high/low changed seen
//on interrupt 0 (pin 2), or interrupt 1 (pin 3)
attachInterrupt(0, updateEncoder, CHANGE);
attachInterrupt(1, updateEncoder, CHANGE);
TCCR1B = TCCR1B & 0b11111000 | 1; // set 31KHz PWM to prevent motor noise
myPID.SetMode(AUTOMATIC); //set PID in Auto mode
myPID.SetSampleTime(1); // refresh rate of PID controller
myPID.SetOutputLimits(-255, 255); // this is the MAX PWM value to move motor,
here change in value reflect change in speed of motor.
}
void loop()
{
while ( Wire.available())
{
// loop through all but the last
byte a,b;
a = Wire.read();
b = Wire.read();
bigNum = a;
bigNum = bigNum << 8 | b;
}
//Serial.println(bigNum);
//bigNum=readString;
REV = map (bigNum, 0, 1, 0, 2925); // mapping degree into pulse
//Serial.print("this is REV - ");
//Serial.println(REV); // printing REV value
setpoint = REV;
//PID while work to achive this value consider as SET value
input = encoderValue ; // data from encoder consider as a Process value
Serial.println(encoderValue);
myPID.Compute(); // calculate new output
pwmOut(output);
}
// drive motor CW
///////////////////////////
void receiveEvent(int howMany) {
}
////////////////////
void pwmOut(int out) {
if (out > 0) { // if REV > encoderValue motor move in forward direction.
analogWrite(MotEnable, out); // Enabling motor enable pin to reach the desire angle
forward(); // calling motor to move forward
}
else {
analogWrite(MotEnable, abs(out)); // if REV < encoderValue motor move in
forward direction.
reverse(); // calling motor to move reverse
}}
void updateEncoder(){
int MSB = digitalRead(encoderPin1); //MSB = most significant bit
int LSB = digitalRead(encoderPin2); //LSB = least significant bit
int encoded = (MSB << 1) |LSB; //converting the 2 pin value to single number
int sum = (lastEncoded << 2) | encoded; //adding it to the previous encoded value
if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderValue
++;
if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderValue
--;
lastEncoded = encoded; //store this value for next time
}
void forward () {
digitalWrite(MotFwd, HIGH);
digitalWrite(MotRev, LOW);
}
void reverse () {
digitalWrite(MotFwd, LOW);
digitalWrite(MotRev, HIGH);
}
void finish () {
digitalWrite(MotFwd, LOW);
digitalWrite(MotRev, LOW);
}
Code Slave2
#include <Wire.h>
#include <PID_v1.h>
#define MotEnable 6 //Motor Enamble pin Runs on PWM signal
#define MotFwd 4 // Motor Forward pin
#define MotRev 7 // Motor Reverse pin
int16_t bigNum;
int encoderPin1 = 2; //Encoder Output 'A' must connected with intreput pin of arduino.
int encoderPin2 = 3; //Encoder Otput 'B' must connected with intreput pin of arduino.
volatile int lastEncoded = 0; // Here updated value of encoder store.
volatile long encoderValue = 0; // Raw encoder value
//int PPR = 2340; // Encoder Pulse per revolution.
//int angle = 360; // Maximum degree of motion.
int REV = 0; // Set point REQUIRED ENCODER VALUE
int lastMSB = 0;
int lastLSB = 0;
double kp =1.7 , ki = 0.2 , kd = 0.005; // modify for optimal performance
double input = 0, output = 0, setpoint = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT);
void setup() {
Wire.begin(3);
Wire.onReceive(receiveEvent);
pinMode(MotEnable, OUTPUT);
pinMode(MotFwd, OUTPUT);
pinMode(MotRev, OUTPUT);
Serial.begin(9600); //initialize serial comunication
pinMode(encoderPin1, INPUT_PULLUP);
pinMode(encoderPin2, INPUT_PULLUP);
digitalWrite(encoderPin1, HIGH); //turn pullup resistor on
digitalWrite(encoderPin2, HIGH); //turn pullup resistor on
//call updateEncoder() when any high/low changed seen
//on interrupt 0 (pin 2), or interrupt 1 (pin 3)
attachInterrupt(0, updateEncoder, CHANGE);
attachInterrupt(1, updateEncoder, CHANGE);
TCCR1B = TCCR1B & 0b11111000 | 1; // set 31KHz PWM to prevent motor noise
myPID.SetMode(AUTOMATIC); //set PID in Auto mode
myPID.SetSampleTime(1); // refresh rate of PID controller
myPID.SetOutputLimits(-150, 150); // this is the MAX PWM value to move motor,
here change in value reflect change in speed of motor.
}
void loop()
{
while ( Wire.available())
{
byte a,b;
a = Wire.read();
b = Wire.read();
bigNum = a;
bigNum = bigNum << 8 | b;
}
//Serial.println(bigNum);
REV = map (bigNum, 0, 360, 0, 9360); // mapping degree into pulse
//Serial.print("this is REV - ");
//Serial.println(REV); // printing REV value
setpoint = REV;
//PID while work to achive this value consider as SET value
input = encoderValue ; // data from encoder consider as a Process value
Serial.println(encoderValue);
myPID.Compute(); // calculate new output
pwmOut(output);
}
// drive motor CW
///////////////////////////
void receiveEvent(int howMany) {
}
////////////////////
void pwmOut(int out) {
if (out > 0) { // if REV > encoderValue motor move in forward direction.
analogWrite(MotEnable, out); // Enabling motor enable pin to reach the desire angle
forward(); // calling motor to move forward
}
else {
analogWrite(MotEnable, abs(out)); // if REV < encoderValue motor move in
forward direction.
reverse(); // calling motor to move reverse
}
//Serial.println(out);
// Cleaning User input, ready for new Input
}
void updateEncoder(){
int MSB = digitalRead(encoderPin1); //MSB = most significant bit
int LSB = digitalRead(encoderPin2); //LSB = least significant bit
int encoded = (MSB << 1) |LSB; //converting the 2 pin value to single number
int sum = (lastEncoded << 2) | encoded; //adding it to the previous encoded value
if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderValue
++;
if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderValue
--;
lastEncoded = encoded; //store this value for next time
}
void forward () {
digitalWrite(MotFwd, HIGH);
digitalWrite(MotRev, LOW);
}
void reverse () {
digitalWrite(MotFwd, LOW);
digitalWrite(MotRev, HIGH);
}
void finish () {
digitalWrite(MotFwd, LOW);
digitalWrite(MotRev, LOW);
}
Code Slave3
#include <Wire.h>
#include <PID_v1.h>
#define MotEnable 6 //Motor Enamble pin Runs on PWM signal
#define MotFwd 4 // Motor Forward pin
#define MotRev 7 // Motor Reverse pin
int16_t bigNum;
int encoderPin1 = 2; //Encoder Output 'A' must connected with intreput pin of arduino.
int encoderPin2 = 3; //Encoder Otput 'B' must connected with intreput pin of arduino.
volatile int lastEncoded = 0; // Here updated value of encoder store.
volatile long encoderValue = 0; // Raw encoder value
//int PPR = 2340; // Encoder Pulse per revolution.
//int angle = 360; // Maximum degree of motion.
int REV = 0; // Set point REQUIRED ENCODER VALUE
int lastMSB = 0;
int lastLSB = 0;
double kp =1.7 , ki = 0.2 , kd = 0.005; // modify for optimal performance
double input = 0, output = 0, setpoint = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT);
void setup() {
Wire.begin(4);
Wire.onReceive(receiveEvent);
pinMode(MotEnable, OUTPUT);
pinMode(MotFwd, OUTPUT);
pinMode(MotRev, OUTPUT);
Serial.begin(9600); //initialize serial comunication
pinMode(encoderPin1, INPUT_PULLUP);
pinMode(encoderPin2, INPUT_PULLUP);
digitalWrite(encoderPin1, HIGH); //turn pullup resistor on
digitalWrite(encoderPin2, HIGH); //turn pullup resistor on
//call updateEncoder() when any high/low changed seen
//on interrupt 0 (pin 2), or interrupt 1 (pin 3)
attachInterrupt(0, updateEncoder, CHANGE);
attachInterrupt(1, updateEncoder, CHANGE);
TCCR1B = TCCR1B & 0b11111000 | 1; // set 31KHz PWM to prevent motor noise
myPID.SetMode(AUTOMATIC); //set PID in Auto mode
myPID.SetSampleTime(1); // refresh rate of PID controller
myPID.SetOutputLimits(-150, 150); // this is the MAX PWM value to move motor,
here change in value reflect change in speed of motor.
}
void loop()
{
while ( Wire.available())
{
byte a,b;
a = Wire.read();
b = Wire.read();
bigNum = a;
bigNum = bigNum << 8 | b;
}
//Serial.println(bigNum);
REV = map (bigNum, 0, 360, 0, 9360); // mapping degree into pulse
//Serial.print("this is REV - ");
//Serial.println(REV); // printing REV value
setpoint = REV;
//PID while work to achive this value consider as SET value
input = encoderValue ; // data from encoder consider as a Process value
Serial.println(encoderValue);
myPID.Compute(); // calculate new output
pwmOut(output);
}
// drive motor CW
///////////////////////////
void receiveEvent(int howMany) {
}
////////////////////
void pwmOut(int out) {
if (out > 0) { // if REV > encoderValue motor move in forward direction.
analogWrite(MotEnable, out); // Enabling motor enable pin to reach the desire angle
forward(); // calling motor to move forward
}
else {
analogWrite(MotEnable, abs(out)); // if REV < encoderValue motor move in
forward direction.
reverse(); // calling motor to move reverse
}
//Serial.println(out);
// Cleaning User input, ready for new Input
}
void updateEncoder(){
int MSB = digitalRead(encoderPin1); //MSB = most significant bit
int LSB = digitalRead(encoderPin2); //LSB = least significant bit
int encoded = (MSB << 1) |LSB; //converting the 2 pin value to single number
int sum = (lastEncoded << 2) | encoded; //adding it to the previous encoded value
if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderValue
++;
if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderValue
--;
lastEncoded = encoded; //store this value for next time
}
void forward () {
digitalWrite(MotFwd, HIGH);
digitalWrite(MotRev, LOW);
}
void reverse () {
digitalWrite(MotFwd, LOW);
digitalWrite(MotRev, HIGH);
}
void finish () {
digitalWrite(MotFwd, LOW);
digitalWrite(MotRev, LOW);
}