Luận văn công nghệ kỹ thuật cơ điện tử thiết kế và chế tạo mô hình robot scara phân loại sản phẩm theo màu sắc và hình dạng

TRƯỜNG ĐẠI HỌC KỸ THUẬT – CÔNG NGHỆ CẦN THƠ KHOA KỸ THUẬT CƠ KHÍ    Tà u iệ il LUẬN VĂN TỐT NGHIỆP ĐẠI HỌC Đ n iệ THIẾT KẾ VÀ CHẾ TẠO MƠ HÌNH ROBOT SCARA -Đ PHÂN LOẠI SẢN PHẨM THEO MÀU SẮC VÀ HÌNH DẠNG n iệ tử CÁN BỘ HƯỚNG DẪN: SINH VIÊN THỰC HIỆN: Ths: ĐƯỜNG KHÁNH SƠN PHAN PHÚC HẬU MSSV: 1600061 Ngành: Công nghệ kỹ thuật Cơ điện tử - 2016 Cần Thơ - 2020 Nhận xét giáo viên hướng dẫn Tà il u iệ iệ Đ n -Đ iệ n tử Cần Thơ, ngày … tháng … năm 2020 GVHD Nhận xét giáo viên phản biện Tà il u iệ iệ Đ n -Đ iệ n tử Cần Thơ, ngày … tháng … năm 2020 GVPB CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC KỸ THUẬT – CÔNG NGHỆ CẦN THƠ Cán hướng dẫn: Ths ĐƯỜNG KHÁNH SƠN Luận văn đại học bảo vệ Trường Đại học Kỹ thuật – Công nghệ Cần Thơ ngày 22 tháng 07 năm 2020 Thành phần Ban chấm đánh giá luận văn đại học gồm: Trưởng ban: Uỷ viên phản biện: Tà il u iệ Thư ký ban chấm: iệ Đ n Xác nhận Ban chấm đánh giá sau luận văn sửa chữa (nếu có) n iệ -Đ tử TRƯỞNG BAN UỶ VIÊN PHẢN BIỆN THƯ KÝ TRƯỜNG ĐẠI HỌC CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM KỸ THUẬT - CÔNG NGHỆ CẦN THƠ Độc lập - Tự - Hạnh phúc KHOA KỸ THUẬT CƠ KHÍ NHIỆM VỤ LUẬN VĂN ĐẠI HỌC Họ tên sinh viên: PHAN PHÚC HẬU MSSV: 1600061 Ngày, tháng, năm sinh: 30/07/1998 Nơi sinh: Cần Thơ Ngành: Công nghệ kỹ thuật điện tử Mã số: 7510203 TÊN ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO MƠ HÌNH ROBOT SCARA PHÂN LOẠI SẢN PHẨM THEO MÀU SẮC VÀ HÌNH DẠNG Tà NHIỆM VỤ VÀ NỘI DUNG u iệ il  Nhiệm vụ: iệ Đ Ứng dụng công nghệ xử lý ảnh cho Robot SCARA để phân loại sản phẩm theo n màu sắc hình dạng từ đưa định cho cánh tay iệ -Đ  Nội dung: n - Thiết kế, chế tạo mơ hình Robot Scara hoàn chỉnh tử - Thiết kế mạch điều khiển cho Robot Scara - Xây dựng giao diện người dùng máy tính phần mềm Matlab - Ứng dụng xử lý ảnh vào mơ hình Robot để phân loại sản phẩm THỜI GIAN GIAO ĐỀ TÀI: 10/01/2020 THỜI GIAN HOÀN THÀNH: 07/07/2020 Cần Thơ, ngày … tháng … năm 2020 KHOA KỸ THUẬT CƠ KHÍ LỜI CẢM ƠN LỜI CẢM ƠN Để hoàn thành đề tài luận văn kết thúc khóa học, em xin bày tỏ lịng biết ơn sâu sắc đến trường Đại học kỹ thuật – công nghệ Cần Thơ tạo điều kiện cho em có mơi trường học tập tốt suốt thời gian học tập nghiên cứu trường Đặc biệt, em xin chân thành cảm ơn thầy Đường Khánh Sơn tận tình hướng dẫn giúp đỡ em trình hồn thành luận văn Đồng thời, em gửi lời cảm ơn chân thành đến thầy cô khoa Kỹ thuật khí, gia đình bạn bè giúp đỡ tạo điều kiện tốt cho em trình học tập trình hồn thành luận văn tốt nghiệp Em xin chân thành cảm ơn! u iệ il Tà n iệ Đ n iệ -Đ tử SVTH: Phan Phúc Hậu Trang I LỜI CẢM ƠN LỜI CAM ĐOAN Em xin cam đoan đề tài: “Thiết kế chế tạo mơ hình Robot SCARA phân loại sản phẩm theo màu sắc hình dạng” cơng trình nghiên cứu thực cá nhân em hướng dẫn thầy Đường Khánh Sơn Trong báo cáo, tất số liệu, hình ảnh thơng tin đề tài em thu thập với thái độ khách quan trung thực Ngoài ra, số nguồn tài liệu tham khảo trích dẫn nguồn thích rõ ràng, khơng chép tài liệu mà khơng có trích dẫn Em xin hồn tồn chịu trách nhiệm trước mơn, khoa nhà trường cam đoan Sinh viên thực u iệ il Tà n iệ Đ n iệ -Đ tử SVTH: Phan Phúc Hậu Trang II TÓM TẮT LUẬN VĂN TÓM TẮT LUẬN VĂN Đề tài luận văn “Thiết kế chế tạo mơ hình Robot Scara phân loại sản phẩm theo màu sắc hình dạng” bao gồm nội dung cốt lõi như: tìm hiểu khái niệm kiến thức Robot, thiết kế chế tạo mơ hình Robot Scara bậc tự hoạt động linh hoạt xác, thiết kế mạch điều khiển giao diện điều khiển cho Robot, xây dựng giải thuật PID điều khiển vị trí động DC, ứng dụng xử lý ảnh vào mơ hình để phân loại sản phẩm u iệ il Tà n iệ Đ n iệ -Đ tử SVTH: Phan Phúc Hậu Trang III MỤC LỤC MỤC LỤC LỜI CẢM ƠN I LỜI CAM ĐOAN II TÓM TẮT LUẬN VĂN III MỤC LỤC i DANH MỤC HÌNH ẢNH iv DANH MỤC BẢNG iv CHƯƠNG 1: TỔNG QUAN 1.1 Tổng quan đề tài 1.1.1 Đặt vấn đề Tà iệ il 1.1.2 Mục tiêu đề tài u 1.1.3 Giới hạn đề tài iệ Đ n 1.2 Giới thiệu sơ lượt Robot công nghiệp -Đ n iệ 1.2.1 Lịch sử hình thành phát triển tử 1.2.2 Cấu trúc Robot công nghiệp .5 1.2.3 Ứng dụng 10 1.3 Giới thiệu sơ lượt Robot Scara 11 CHƯƠNG 2: ĐỐI TƯỢNG VÀ PHƯƠNG PHÁP NGHIÊN CỨU 14 2.1 Đối tượng nghiên cứu .14 2.2 Phương pháp nghiêm cứu .14 2.2.1 Phần mềmMatlab 14 2.2.2 Matlab GUI .17 2.2.3 Xử lý ảnh Matlab .21 2.2.4 Phần mềm Invertor 27 SVTH: Phan Phúc Hậu Trang i MỤC LỤC 2.2.5 Arduino .29 2.2.6 Phần mềm Arduino IDE 32 2.2.7 PID 34 2.2.8 Chuẩn giao tiếp I2C 37 CHƯƠNG 3: THIẾT KẾ VÀ CHẾ TẠO MƠ HÌNH 41 3.1 Bài toán động học 41 3.1.1 Động học thuận 41 3.1.2 Động học ngược .42 3.2 Thiết kế mơ hình khí cho Robot .43 3.2.1 Cơ sở thiết kế 43 Tà iệ il 3.2.2 Bản vẽ thiết kế Robot Inventor 44 u 3.2.3 Chế tạo mơ hình khí .50 iệ Đ n 3.2.4 Lắp ráp mơ hình Robot Scara hồn chỉnh 55 -Đ 3.3 Thiết kế mạch điều khiển 57 iệ n 3.3.1 Lưu đồ giải thuật điều khiển Robot 57 tử 3.3.2 Sơ đồ mạch điều khiển Robot 58 CHƯƠNG 4: XỬ LÝ ẢNH VÀ THIẾT KẾ GIAO DIỆN NGƯỜI DÙNG 66 4.1 Xử lý ảnh 66 4.1.1 Giới thiệu hệ thống xử lý ảnh 66 4.1.2 Sơ lượt trình xử lý ảnh đề tài .68 4.1.3 Thu nhận ảnh đầu vào .69 4.1.4 Xác định vùng hoạt động Robot 70 4.1.5 Phân vùng ảnh tách sản phẩm 72 4.1.6 Nhận dạng đối tượng 74 SVTH: Phan Phúc Hậu Trang ii PHỤ LỤC % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function theta2_Callback(hObject, eventdata, handles) % hObject handle to theta2 (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 theta2 as text % str2double(get(hObject,'String')) returns contents of theta2 as a double il Tà u iệ % - Executes during object creation, after setting all properties handle to theta2 (see GCBO) n % hObject iệ Đ function theta2_CreateFcn(hObject, eventdata, handles) -Đ % eventdata reserved - to be defined in a future version of MATLAB iệ empty - handles not created until after all CreateFcns called n % handles tử % 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 theta3_Callback(hObject, eventdata, handles) % hObject handle to theta3 (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 theta3 as text SVTH: Phan Phúc Hậu Trang 104 PHỤ LỤC % str2double(get(hObject,'String')) returns contents of theta3 as a double % - Executes during object creation, after setting all properties function theta3_CreateFcn(hObject, eventdata, handles) % hObject handle to theta3 (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'); Tà end u iệ il handle to px (see GCBO) n % hObject iệ Đ function px_Callback(hObject, eventdata, handles) -Đ % eventdata reserved - to be defined in a future version of MATLAB iệ structure with handles and user data (see GUIDATA) n % handles tử % Hints: get(hObject,'String') returns contents of px as text % str2double(get(hObject,'String')) returns contents of px as a double % - Executes during object creation, after setting all properties function px_CreateFcn(hObject, eventdata, handles) % hObject handle to px (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')) SVTH: Phan Phúc Hậu Trang 105 PHỤ LỤC set(hObject,'BackgroundColor','white'); end function py_Callback(hObject, eventdata, handles) % hObject handle to py (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 py as text % str2double(get(hObject,'String')) returns contents of py as a double % - Executes during object creation, after setting all properties function py_CreateFcn(hObject, eventdata, handles) Tà % hObject handle to py (see GCBO) il empty - handles not created until after all CreateFcns called iệ Đ % handles u iệ % eventdata reserved - to be defined in a future version of MATLAB n % Hint: edit controls usually have a white background on Windows See ISPC and COMPUTER iệ -Đ % n if ispc && isequal(get(hObject,'BackgroundColor'), tử get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function pz_Callback(hObject, eventdata, handles) % hObject handle to pz (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 pz as text % str2double(get(hObject,'String')) returns contents of pz as a double % - Executes during object creation, after setting all properties SVTH: Phan Phúc Hậu Trang 106 PHỤ LỤC function pz_CreateFcn(hObject, eventdata, handles) % hObject handle to pz (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 Tà function dht_Callback(hObject, eventdata, handles) il handle to dht (see GCBO) u iệ % hObject structure with handles and user data (see GUIDATA) n % handles iệ Đ % eventdata reserved - to be defined in a future version of MATLAB -Đ global s; iệ n global l1 l2 l3; tử d1 = get(handles.d1,'String'); theta2 = get(handles.theta2,'String'); theta3 = get(handles.theta3,'String'); fprintf(s , 'ff'); fprintf(s , 'a'); fprintf(s , d1); pause(2) fprintf(s , 'b'); fprintf(s , theta2); fprintf(s , 'c'); fprintf(s , theta3); d1 = str2num(d1); theta2 = str2num(theta2); theta3 = str2num(theta3); T1 = [1 0 l3 ; 0 ; 0 d1 ; 0 1]; SVTH: Phan Phúc Hậu Trang 107 PHỤ LỤC T2 = [cosd(theta2) -sind(theta2) l1*cosd(theta2) ; sind(theta2) cosd(theta2) l1*sind(theta2) ; 0 ; 0 1]; T3 = [cosd(theta3) -sind(theta3) l2*cosd(theta3) ; sind(theta3) cosd(theta3) l2*sind(theta3) ; 0 ; 0 1]; T = T1*T2*T3; px = T(1,4); py = T(2,4); pz = T(3,4); set(handles.px,'String',num2str(round(px,2))); set(handles.py,'String',num2str(round(py,2))); set(handles.pz,'String',num2str(round(pz,2))); % - Executes on button press in dhn function dhn_Callback(hObject, eventdata, handles) Tà % hObject handle to dhn (see GCBO) il structure with handles and user data (see GUIDATA) iệ Đ % handles u iệ % eventdata reserved - to be defined in a future version of MATLAB n global s; tử py = get(handles.py,'String'); n px = get(handles.px,'String'); iệ -Đ global l1 l2 l3; pz = get(handles.pz,'String'); px = str2num(px)-l3; py = str2num(py); d1 = str2num(pz); c3 = (px^2+py^2-l1^2-l2^2)/(2*l1*l2); s3 = sqrt(1-(c3)^2); theta3 = atan2d(s3,c3); denta = px^2+py^2; denta1 = l1*px+l2*(px*c3+py*s3); denta2 = l1*py+l2*(py*c3-px*s3); c2 = denta1/denta; s2 = denta2/denta; theta2 = atan2d(s2,c2); SVTH: Phan Phúc Hậu Trang 108 PHỤ LỤC fprintf(s , 'ff'); fprintf(s , 'a'); fprintf(s , num2str(d1)); pause(2) fprintf(s , 'b'); fprintf(s , num2str(theta2)); fprintf(s , 'c'); fprintf(s , num2str(theta3)); set(handles.d1,'String',num2str(round(d1,2))); set(handles.theta2,'String',num2str(round(theta2,2))); set(handles.theta3,'String',num2str(round(theta3,2))); % - 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 Tà % handles structure with handles and user data (see GUIDATA) il u iệ global s; fprintf(s , 'a'); fprintf(s , '0'); fprintf(s , 'c'); fprintf(s , '0'); pause(1) tử set(handles.d1,'String',0); n fprintf(s , 'oo'); iệ -Đ fprintf(s , '0'); n fprintf(s , 'b'); iệ Đ global l1 l2 l3; set(handles.theta2,'String',0); set(handles.theta3,'String',0); set(handles.px,'String',l1+l2+l3); set(handles.py,'String',0); set(handles.pz,'String',0); function axes1_CreateFcn(hObject, eventdata, handles) % hObject handle to axes1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: place code in OpeningFcn to populate axes1 SVTH: Phan Phúc Hậu Trang 109 PHỤ LỤC % - Executes on button press in oncam function oncam_Callback(hObject, eventdata, handles) % hObject handle to oncam (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) global myCam; axes(handles.axes1); myCam = videoinput('winvideo',1); set(myCam,'ReturnedColorSpace','rgb'); triggerconfig(myCam,'manual'); set(myCam,'FramesPerTrigger',1 ); Tà set(myCam,'TriggerRepeat', Inf); il u iệ myCam.FrameGrabInterval = 3; n preview(myCam,hImage); iệ Đ hImage = image(zeros(576, 1024,3),'parent',handles.axes1); n iệ -Đ % - Executes on button press in offcam tử function offcam_Callback(hObject, eventdata, handles) % hObject handle to offcam(see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) global myCam; if (strcmp(get(myCam,'Running'),'on')) flushdata(myCam); stop(myCam); else imaqreset; end cla(handles.axes1,'reset'); SVTH: Phan Phúc Hậu Trang 110 PHỤ LỤC % - Executes on button press in setcam function setcam_Callback(hObject, eventdata, handles) % hObject handle to setcam (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) global myCam; global X Y; axes(handles.axes1); X = []; Y = []; i = 0; rgb = getsnapshot(myCam); im = imsubtract(rgb(:,:,1), rgb2gray(rgb)); Tà im = medfilt2(im, [3 3]); u iệ il im = im2bw(im,0.15); n im = bwareaopen(im,250); iệ Đ % Remove all those pixels less than 250px -Đ % Label all the connected components in the image n tử % Here we the image blob analysis iệ bw = bwlabel(im, 8); % We get a set of properties for each labeled region L = regionprops(bw, 'all'); for n = 1:length(L) W = L(n).BoundingBox(3); H = L(n).BoundingBox(4); S = L(n).Area; if H/W > 0.5 && H/W < if S >500 && S< 650 if S/(W*H)>0.5 && S/(W*H) 0.5 && H/W < il u iệ if S > 500 && S < 1500 n i = i+1; iệ Đ if S/(W*H) > 0.5 && S/(W*H) BRed Tà plot(cX(i),cY(i),'r+'); il u iệ text(cX(i)+25, cY(i)-12, strcat('X: ', num2str(round(cY(i)))),'Color','r'); iệ Đ text(cX(i)+25, cY(i)+12, strcat('Y: ', n num2str(round(cX(i)-290))),'Color','r'); -Đ if (length(c) > 0) || (T > 0.9 && T < 1.1) tử Shape(i) = 'O'; n iệ Color(i) = 'R'; viscircles([cX(i) cY(i)],r,'EdgeColor','r'); end if ((T > 0.65 && T < 0.85) || (vat(n).Extent > 0.7)) && (length(c) == 0) Color(i) = 'R'; Shape(i) = 'X'; rectangle('Position',vat(n).BoundingBox,'EdgeColor','r','LineWidth',2); end end if WGreen > BGreen plot(cX(i),cY(i),'g+'); text(cX(i)+25, cY(i)-12, strcat('X: ', SVTH: Phan Phúc Hậu Trang 114 PHỤ LỤC num2str(round(cY(i)))),'Color','g'); text(cX(i)+25, cY(i)+12, strcat('Y: ', num2str(round(cX(i)-290))),'Color','g'); if (length(c) > 0) || (T > 0.9 && T < 1.1) Color(i) = 'G'; Shape(i) = 'O'; viscircles([cX(i) cY(i)],r,'EdgeColor','g'); end if ((T > 0.65 && T < 0.85) || (vat(n).Extent > 0.7)) && (length(c) == 0) Color(i) = 'G'; Shape(i) = 'X'; rectangle('Position',vat(n).BoundingBox,'EdgeColor','g','LineWidth',2); il Tà end n iệ plot(cX(i),cY(i),'b+'); Đ if WBlue > BBlue u iệ end -Đ text(cX(i)+25, cY(i)-12, strcat('X: ', num2str(round(cY(i)))),'Color','b'); n iệ text(cX(i)+25, cY(i)+12, strcat('Y: ', tử num2str(round(cX(i)-290))),'Color','b'); if (length(c) > 0) || (T > 0.9 && T < 1.1) Color(i) = 'B'; Shape(i) = 'O'; viscircles([cX(i) cY(i)],r,'EdgeColor','b'); end if ((T > 0.65 && T < 0.85) || (vat(n).Extent > 0.7)) && (length(c) == 0) Color(i) = 'B'; Shape(i) = 'X'; rectangle('Position',vat(n).BoundingBox,'EdgeColor','b','LineWidth',2); end end end end end end hold off SVTH: Phan Phúc Hậu Trang 115 PHỤ LỤC fprintf(s , 'ff'); fprintf(s , 'a'); fprintf(s , '70'); pause(2) for j = 1:i px = cY(j)-l3; py = cX(j)-290; pz = 0; d1 = pz; c3 = (px^2+py^2-l1^2-l2^2)/(2*l1*l2); s3 = sqrt(1-(c3)^2); theta3 = atan2d(s3,c3); denta = px^2+py^2; denta1 = l1*px+l2*(px*c3+py*s3); denta2 = l1*py+l2*(py*c3-px*s3); c2 = denta1/denta; s2 = denta2/denta; theta2 = atan2d(s2,c2); set(handles.d1,'String',num2str(round(d1,2))); Tà set(handles.theta2,'String',num2str(round(theta2,2))); il u iệ set(handles.theta3,'String',num2str(round(theta3,2))); iệ Đ set(handles.px,'String',num2str(round(cY(j),2))); n set(handles.py,'String',num2str(round(py,2))); -Đ set(handles.pz,'String',num2str(round(pz,2))); n iệ if Color(j) == 'R' && Shape(j) == 'O' tử fprintf(s , 'b'); fprintf(s , num2str(theta2)); fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3) fprintf(s , 'a'); fprintf(s , '0'); pause(2) fprintf(s , 'oo'); pause(1) fprintf(s , 'a'); fprintf(s , '70'); pause(2) fprintf(s , 'b'); fprintf(s , '40'); fprintf(s , 'c'); fprintf(s , '30'); pause(3) fprintf(s , 'ff'); pause(1) end if Color(j) == 'G' && Shape(j) == 'O' fprintf(s , 'b'); fprintf(s , num2str(theta2)); fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3) SVTH: Phan Phúc Hậu Trang 116 PHỤ LỤC fprintf(s , 'a'); fprintf(s , '0'); pause(2) fprintf(s , 'oo'); pause(1) fprintf(s , 'a'); fprintf(s , '70'); pause(2) fprintf(s , 'b'); fprintf(s , '37'); fprintf(s , 'c'); fprintf(s , '66'); pause(3) fprintf(s , 'ff'); pause(1) end if Color(j) == 'B' && Shape(j) == 'O' fprintf(s , 'b'); fprintf(s , num2str(theta2)); fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3) fprintf(s , 'a'); fprintf(s , '0'); pause(2) fprintf(s , 'oo'); pause(1) Tà fprintf(s , 'a'); fprintf(s , '70'); pause(2) il u iệ fprintf(s , 'b'); fprintf(s , '49'); n if Color(j) == 'R' && Shape(j) == 'X' iệ -Đ end n fprintf(s , 'ff'); pause(1) iệ Đ fprintf(s , 'c'); fprintf(s , '77'); pause(3) tử fprintf(s , 'b'); fprintf(s , num2str(theta2)); fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3) fprintf(s , 'a'); fprintf(s , '0'); pause(2) fprintf(s , 'oo'); pause(1) fprintf(s , 'a'); fprintf(s , '70'); pause(2) fprintf(s , 'b'); fprintf(s , '-40'); fprintf(s , 'c'); fprintf(s , '-30'); pause(3) fprintf(s , 'ff'); pause(1) end if Color(j) == 'G' && Shape(j) == 'X' fprintf(s , 'b'); fprintf(s , num2str(theta2)); fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3) SVTH: Phan Phúc Hậu Trang 117 PHỤ LỤC fprintf(s , 'a'); fprintf(s , '0'); pause(2) fprintf(s , 'oo'); pause(1) fprintf(s , 'a'); fprintf(s , '70'); pause(2) fprintf(s , 'b'); fprintf(s , '-37'); fprintf(s , 'c'); fprintf(s , '-66'); pause(3) fprintf(s , 'ff'); pause(1) end if Color(j) == 'B' && Shape(j) == 'X' fprintf(s , 'b'); fprintf(s , num2str(theta2)); fprintf(s , 'c'); fprintf(s , num2str(theta3)); pause(3) fprintf(s , 'a'); fprintf(s , '0'); pause(2) fprintf(s , 'oo'); pause(1) Tà fprintf(s , 'a'); fprintf(s , '70'); pause(2) il u iệ fprintf(s , 'b'); fprintf(s , '-49'); tử fprintf(s , 'b'); fprintf(s , '0'); n end iệ -Đ end n fprintf(s , 'ff'); pause(1) iệ Đ fprintf(s , 'c'); fprintf(s , '-77'); pause(3) fprintf(s , 'c'); fprintf(s , '0'); pause(3) fprintf(s , 'a'); fprintf(s , '0'); fprintf(s , 'oo'); set(handles.d1,'String',0); set(handles.theta2,'String',0); set(handles.theta3,'String',0); set(handles.px,'String',l1+l2+l3); set(handles.py,'String',0); set(handles.pz,'String',0); SVTH: Phan Phúc Hậu Trang 118