Thiết kế và thi công robot tự hành vận chuyển thuốc trong phòng khám

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH KHĨA LUẬN TỐT NGHIỆP NGÀNH KỸ THUẬT Y SINH THIẾT KẾ VÀ THI CƠNG ROBOT TỰ HÀNH VẬN CHUYỂN THUỐC TRONG PHỊNG KHÁM GVHD: THS NGÔ BÁ VIỆT SVTH: LÊ TUẤN NGHĨA VÕ VĨNH TRƯỜNG SKL01112 Tp Hồ Chí Minh, tháng 7/2023 BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH - ĐỒ ÁN TỐT NGHIỆP NGÀNH KỸ THUẬT Y SINH ĐỀ TÀI: THIẾT KẾ VÀ THI CÔNG ROBOT TỰ HÀNH VẬN CHUYỂN THUỐC TRONG PHỊNG KHÁM GVHD: ThS Ngơ Bá Việt SVTH: Lê Tuấn Nghĩa MSSV: 18129038 SVTH: Võ Vĩnh Trường MSSV: 19129057 Tp Hồ Chí Minh – 07/2023 TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN-ĐIỆN TỬ ĐỘC LẬP - TỰ DO - HẠNH PHÚC BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH o0o -Tp HCM, ngày 19 tháng 07 năm 2023 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Chuyên ngành: Hệ đào tạo: Khóa: Lê Tuấn Nghĩa Võ Vĩnh Trường Kỹ thuật Y Sinh Đại học quy 2019 MSSV: 18129038 MSSV: 19129057 Mã ngành: 129 Mã hệ: Lớp: 191290C I TÊN ĐỀ TÀI: THIẾT KẾ VÀ THI CÔNG ROBOT TỰ HÀNH VẬN CHUYỂN THUỐC TRONG PHÒNG KHÁM II NHIỆM VỤ Các số liệu ban đầu:  Tài liệu điều khiển động  Tài liệu phần mềm Arduino, ngôn ngữ lập trình Python  Tài liệu thuật tốn tìm đường tối ưu Nội dung thực hiện:  Nội dung 1: Tìm hiểu loại Robot tự hành có thị trường  Nội dung 2: Mơ hình thuật tốn liên quan đến đề tài  Nội dung 3: Nghiên cứu, lập trình khối xử lý trung tâm gửi tín hiệu cho Robot thơng qua Bluetooth  Nội dung 4: Lập trình điều khiển động  Nội dung 5: Thiết kế thi công board mạch điều khiển mơ hình Robot  Nội dung 6: Chạy thí nghiệm hiệu chỉnh thơng số Robot III NGÀY GIAO NHIỆM VỤ: 20/02/2023 IV NGÀY HOÀN THÀNH NHIỆM VỤ: 19/07/2023 V HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: ThS Ngô Bá Việt CÁN BỘ HƯỚNG DẪN BM ĐIỆN TỬ CÔNG NGHIỆP – Y SINH i TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN-ĐIỆN TỬ ĐỘC LẬP - TỰ DO - HẠNH PHÚC BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH o0o -Tp HCM, ngày 19 tháng 07 năm 2023 LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Lê Tuấn Nghĩa Lớp: 181290B MSSV: 18129038 Họ tên sinh viên 2: Võ Vĩnh Trường Lớp: 191290C MSSV: 19129057 Tên đề tài: THIẾT KẾ VÀ THI CÔNG ROBOT TỰ HÀNH VẬN CHUYỂN THUỐC TRONG PHÒNG KHÁM Tuần/ngày Tuần (20/02 – 26/02) Nội dung Xác nhận GVHD - Gặp GVHD để nghe phổ biến yêu cầu đồ án, tiến hành chọn đồ án - GVHD tiến hành xét duyệt đề tài - Viết đề cương chi tiết cho đề tài Tuần (27/02 – 05/03) - Tìm hiểu đề tài liên quan có - Tìm hiểu thuật tốn di chuyển Robot Tuần - Tìm hiểu module Arduino Mega, (06/03 – 12/03) L298N, HC-05, cảm biến sử dụng cho đề tài - Tìm hiểu mẫu thiết kế Robot Tuần - Báo cáo tiến độ cho GVHD (13/03 – 19/03) - Thiết kế sơ đồ khối hệ thống Tuần 5-6 - Thiết kế board mạch hệ thống (20/03 – 02/04) Tuần - Thiết kế mơ hình Robot - Báo cáo tiến độ cho GVHD (03/04 – 09/04) ii - Lập trình thuật tốn di chuyển phương pháp dị line Tuần 8-9 (10/04 – 23/04) - Hồn thiện mơ hình Robot - Hoàn thành kết nối board mạch cho toàn hệ thống - Tìm hiểu thuật tốn tim đường tối ưu Q-Learning Tuần 10 (24/04 - 30/04) - Báo cáo tiến độ cho GVHD - Nộp báo cáo tiến độ cho mơn - Lập trình điều khiển Robot qua Bluetooth Tuần 11-12 (01/05 - 14/05) Tuần 13 (15/05 - 21/05) Tuần 14-15 (22/05 - 04/06) Tuần 16-17 (05/05 - 19/06) - Lập trình điều khiển Robot thuật tốn Q-Learning - Báo cáo cho GVHD - Lập trình hiển thị LCD - Chạy thử nghiệm - Chỉnh sửa hoàn thiện - Báo cáo GVHD - Viết báo cáo luận văn GV HƯỚNG DẪN (Ký ghi rõ họ tên) iii LỜI CAM ĐOAN Đề tài đề tài tự thực dựa cơng trình nghiên cứu tài liệu trước mà khơng chép từ tài liệu hay cơng trình có trước Nhóm sinh viên thực đề tài Lê Tuấn Nghĩa Võ Vĩnh Trường iv LỜI CẢM ƠN Em xin gửi lời cảm ơn sâu sắc đến Thầy Ngô Bá Việt - Giảng viên môn Điện tử Y Sinh nhiệt tình hỗ trợ em suốt quãng thời gian thực đồ án tốt nghiệp Em gửi lời đồng cảm ơn đến ba mẹ bạn lớp 191290A anh chị khóa trước chia sẻ trao đổi kiến thức kinh nghiệm quý báu thời gian thực đề tài Xin chân thành cảm ơn! Nhóm sinh viên thực đề tài Lê Tuấn Nghĩa Võ Vĩnh Trường v MỤC LỤC NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP i LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP ii LỜI CAM ĐOAN iv LỜI CẢM ƠN v MỤC LỤC DANH MỤC HÌNH ẢNH 10 DANH MỤC BẢNG 14 DANH MỤC CÁC TỪ VIẾT TẮT 15 TÓM TẮT 16 Chương TỔNG QUAN 17 1.1 ĐẶT VẤN ĐỀ 17 1.2 MỤC TIÊU 19 1.3 NỘI DUNG NGHIÊN CỨU 19 1.4 GIỚI HẠN 19 1.5 BỐ CỤC 20 Chương CƠ SỞ LÝ THUYẾT 21 2.1 MỘT SỐ LOẠI ROBOT TỰ HÀNH PHỔ BIẾN HIỆN NAY 21 2.1.1 Robot tự hành môi trường công nghiệp 21 2.1.2 Robot tự hành môi trường bệnh viện 22 2.2 PHƯƠNG PHÁP ĐIỀU CHẾ ĐỘ RỘNG XUNG PWM 24 2.3 PHƯƠNG PHÁP DÒ LINE 24 2.3.1 Cảm biến từ 24 2.3.2 Cảm biến hồng ngoại 25 BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 2.4 PHƯƠNG PHÁP PHÁT HIỆN VẬT CẢN 25 2.4.1 Phương pháp phát vật cản sóng siêu âm 25 2.4.2 Phương pháp phát vật cản laser 26 2.5 THUẬT TOÁN HỌC TĂNG CƯỜNG 26 2.5.1 Sơ lược học tăng cường (Reinforcement learning) 26 2.5.2 Thuật toán q-learning 28 2.6 CÁC GIAO THỨC TRUYỀN TÍN HIỆU 28 2.6.1 Giao thức truyền tín hiệu Uart 28 2.6.2 Giao thức truyền tín hiệu I2C 28 2.7 PHẦN MỀM VÀ NGƠN NGỮ LẬP TRÌNH PHÁT TRIỂN ROBOT 29 2.7.1 Pycharm 29 2.7.2 Arduino IDE 30 2.7.3 Easyeda 30 2.7.4 Solidworks 31 Chương TÍNH TỐN VÀ THIẾT KẾ 32 3.1 TÍNH TỐN VÀ THIẾT KẾ HỆ THỐNG 32 3.1.1 Thiết kế sơ đồ khối hệ thống 32 3.1.2 Lựa chọn, tính tốn thiết kế sơ đồ mạch 33 3.1.3 Sơ đồ nguyên lý toàn mạch 46 3.1.4 Thiết kế mơ hình robot 48 3.2 PHƯƠNG PHÁP XÁC ĐỊNH ĐƯỜNG ĐI ỨNG DỤNG Q-LEARNING 51 3.3 PHƯƠNG PHÁP DI CHUYỂN CỦA ROBOT 53 3.3.1 Phương pháp dò line 54 3.3.2 Phương pháp xác định vị trí robot theo thời gian 56 Chương THI CÔNG HỆ THỐNG 58 BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 4.1 THI CÔNG HỆ THỐNG 58 4.1.1 Thi công board mạch 58 4.1.2 Lắp ráp kiểm tra 59 4.2 THI CÔNG MÔ HÌNH 62 4.2.1 Thi công khung robot 62 4.2.2 Hoàn thiện sản phẩm 64 4.3 LẬP TRÌNH HỆ THỐNG 65 4.3.1 Lưu đồ giải thuật chương trình 65 4.3.2 Lưu đồ giải thuật chương trình 66 4.4 VIẾT TÀI LIỆU HƯỚNG DẪN SỬ DỤNG, THAO TÁC 85 4.4.1 Viết tài liệu hướng dẫn sử dụng 85 4.4.2 Quy trình thao tác 89 4.5 DỰ TỐN CỦA MƠ HÌNH ROBOT VẬN CHUYỂN 89 Chương KẾT QUẢ - NHẬN XÉT -ĐÁNH GIÁ 92 5.1 KẾT QUẢ THI CƠNG MƠ HÌNH 92 5.1.1 Thi công board mạch 92 5.1.2 Thi cơng mơ hình 93 5.1.3 Hoàn thiện sản phẩm 93 5.2 KẾT QUẢ THÍ NGHIỆM 95 5.2.1 Đánh giá độ bám line 96 5.2.2 Đánh giá độ hiệu xử lý gặp vật cản chuyển động 102 5.2.3 Đánh giá độ bám line sau gặp vật cản tĩnh 104 5.3 NHẬN XÉT VÀ ĐÁNH GIÁ 106 Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 107 6.1 KẾT LUẬN 107 BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH TÀI LIỆU THAM KHẢO TÀI LIỆU THAM KHẢO [1] B Y Tế, “Bản tin phòng chống dịch COVID-19 ngày 26/12 Bộ Y tế,” https://moh.gov.vn/tin-tong-hop//asset_publisher/k206Q9qkZOqn/content/ban-tin-phong-chong-dichcovid-19-ngay-26-12-cua-bo-y-te, 2022 [2] Y L a Y Z Hao Quan, "A novel mobile robot navigationmethod based on deepreinforcement learning," International Journal of Advanced Robotic Systems, vol 17, p 172988142092167, 2020 [3] P a X J a Z H a L D a S M a Z J Yue, “Experimental Research on Deep Reinforcement Learning in Autonomous navigation of Mobile Robot,” 2019 14th IEEE Conference on Industrial Electronics and Applications (ICIEA), pp 1612-1616, 2019 [4] A a H D M E Latoui, “Implementation of Q-Learning Algorithm on Arduino: Application to Autonomous Mobile Robot Navigation in COVID-19 Field Hospitals,” 2021 International Conference on Electrical, Computer and Energy Technologies (ICECET), pp 1-5, 2021 [5] B K H V C NGHỆ, “Robot y tế vận chuyển VIBOT Bộ Khoa học Cơng nghệ đặt hàng hỗ trợ đắc lực phịng, chống Covid-19,” https://www.most.gov.vn/vn/tin-tuc/20562/robot-y-te-van-chuyen-vibotdo-bo-khoa-hoc-va-cong-nghe-dat-hang-ho-tro-dac-luc-phong chongcovid-19.aspx, 2021 [6] N T Đạt, “Nghiên cứu, chế tạo xe tự hành vận chuyển hàng hóa,” 2020 [7] https://intech-group.vn/robot-tu-hanh-agv-ita-02-sp177.htm [8] https://www.daifuku.com/us/assets/pdf/solution/vehiclecart/M10KT_Tugger.pdf [9] https://assets.omron.com/m/1bf71ca06f961b4b/original/LD-SeriesMobile-Robot-Datasheet.pdf BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 108 TÀI LIỆU THAM KHẢO [10] https://vovworld.vn/en-US/current-affairs/transport-robots-being-usedin-covid19-quarantine-areas-986399.vov [11] https://aethon.com/PDF/T3XL_Datasheet.pdf [12] Nguyễn Đình Phú, Giáo trình vi điều khiển pic, Thành phố Hồ Chí Minh: Nhà xuất Đại học Quốc Gia, 2017 [13] https://tci.vn/cau-tao-xe-tu-hanh-agv/ [14] https://thietbikythuat.com.vn/nguyen-ly-hoat-dong-cua-cam-bien-hongngoai-la-gi/ [15] https://ai.thestempedia.com/docs/evive/evive-tutorials/what-is-anultrasonic-sensor/ [16] https://www.makeuseof.com/what-is-time-of-flight-sensor-how-doestof-work/ [17] https://viblo.asia/p/dieu-gi-tao-nen-sieu-ai-co-vay-alphago-zeromaGK730xKj2 [18] https://www.samyzaf.com/ML/rl/qmaze.html [19] P a C S a C D Kormushev, "Reinforcement Learning in Robotics: Applications and Real-World Challenges," Robotics, vol 2, pp 122-148, 2013 [20] https://viblo.asia/p/reinforcement-learning-q-learning-63vKjO7VZ2R [21] https://dientuviet.com/kien-thuc-co-ban-ve-giao-tiep-uart/ [22] https://www.thegioiic.com/tcrt5000-cam-bien-do-line-5-kenh-khoangcach-10-15mm [23] https://www.linkedin.com/pulse/encoder-l%C3%A0-g%C3%ACnguy%C3%AAn-l%C3%BD-ho%E1%BA%A1t%C4%91%E1%BB%99ng-v%C3%A0-%E1%BB%A9ngd%E1%BB%A5ng-c%E1%BB%A7a-amazen-online [24] N H D M Đ N Trần Kim Thiện, “Nghiên cứu, thiết kế chế tạo robot di động giao thực phẩm tự động theo sơ đồ,” Tp Hồ Chí Minh, 2022 BỘ MƠN ĐIỆN TỬ CƠNG NGHIỆP – Y SINH 109 TÀI LIỆU THAM KHẢO [25] W VOLKSWAGEN AG, "Self-study programme 212," in Variable Intake Manifold in VR Engines [26] P A M S Radhikesh P Nanda, "Suitable Friction Sliding Materials for Base Isolation of Masonry Buildings," vol 19, p 106436, 2011 [27] https://acis.com.vn/cong-nghe-khong-day/ [28] https://www.ti.com/lit/ds/snvs124g/snvs124g.pdf?ts=1687068729722 [29] N a K Y.-Y a P A a J M.-H Chindakham, "Simultaneous Calibration of Odometry and Head-Eye," vol 19, p 3623, 2019 BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 110 PHỤ LỤC PHỤ LỤC Chương trình chọn phòng int buttonState1 = digitalRead(BUTTON_PIN_1); int buttonState2 = digitalRead(BUTTON_PIN_2); int buttonState3 = digitalRead(BUTTON_PIN_3); if (selected == 0) { if (buttonState1 == LOW) { room = 1; selected = 1; } if (buttonState2 == LOW) { room = 2; selected = 1; } if (buttonState3 == LOW) { room = 3; selected = 1; } } else if (selected == 1) { room = room_old; if ((buttonState1 == HIGH) && (buttonState2 == HIGH) && (buttonState3 == HIGH)) { room = 0; lcd.clear(); BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 111 PHỤ LỤC selected = 0; } } button(); if (room_old != room ) { if (room == 0) { lcd.setCursor(7, 0); lcd.print("DE TAI"); lcd.setCursor(2, 1); lcd.print("ROBOT VAN CHUYEN"); lcd.setCursor(1, 2); lcd.print("XIN MOI CHON PHONG"); lcd.setCursor(0, 3); lcd.print("3"); lcd.setCursor(10, 3); lcd.print("2"); lcd.setCursor(19, 3); lcd.print("1"); } if (room != 0) { lcd.clear(); lcd.setCursor(7, 1); lcd.print("PHONG "); lcd.print(room); BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 112 PHỤ LỤC // Lưu trạng thái vào trạng thái trước room_old = room; }} Chương trình gửi nhận thơng tin Robot // Tạo đối tượng JSON StaticJsonDocument jsonBuffer; JsonObject json = jsonBuffer.to(); // Đặt trường liệu json["theta"] = theta; json["spin"] = spin; json["mode"] = mode; json["pass"] = pass; json["prev_step"] = prev_step; json["dis_obstacle"] = dis_obstacle; json["room"] = room; json["x"] = x; json["y"] = y; // Chuyển đổi JSON thành chuỗi String jsonString; serializeJson(json, jsonString); // Gửi chuỗi JSON qua Bluetooth BTSerial.println(jsonString); if (BTSerial.available()) { jsonStr = BTSerial.readStringUntil('\n'); DynamicJsonDocument doc(200); DeserializationError error = deserializeJson(doc, jsonStr); if (error) { // Serial.println("Error parsing JSON"); BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 113 PHỤ LỤC pass = 0;} else pass = 1; new_received_data = doc["received_data"]; new_received_data_2 = doc["nums_step"]; new_received_data_3 = doc["key_last"]; if (new_received_data_3 != key_last) { key_last = new_received_data_3; } if (new_received_data_2 != nums_step) { nums_step = new_received_data_2; } if (new_received_data != dir) { dir = new_received_data; }} Chương trình trình xử lý gửi lệnh khối xử lý trung tâm global key_last,reward,steps,num_step,room,arduino_to_excel,name,dir,dir_old, current_step, prev_step,first,envstate,dir_path,goal,goal_string,maze,qmaze,i,j,steps_robot,rat_ cell train=0 while True: if ser.in_waiting > 0: line = ser.readline().decode('utf-8').rstrip() response_data = json.loads(line) value_1 = response_data['theta'] BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 114 PHỤ LỤC value_2 = response_data['spin'] value_3 = response_data['mode'] value_4 = response_data['pass'] value_5 = response_data['prev_step'] value_6= response_data['dis_obstacle'] value_7=response_data['room'] value_8=response_data['x'] value_9=response_data['y'] warnings.filterwarnings("ignore") data_from_arduino = {'mode': [value_3], 'prev_step':[0],'x': [value_8], 'y':[value_9],'theta':[value_1], 'obstacle':[value_6]} store_data_to_dataframe = pd.DataFrame(data=data_from_arduino, index=[currentTime]) arduino_to_excel = arduino_to_excel.append(store_data_to_dataframe) arduino_to_excel.to_excel(excel_writer=name + '.xlsx') room = value_7 if first==0: if room == 1: goal = (3, 3) if room == 2: goal = (7, 2) if room == 3: goal = (5, 1) goal_string = str(goal) goal_string = goal_string.replace(" ", "") if (room!=0): if train==0: print(goal_string) print (rat_cell) qmaze = Qmaze(maze) BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 115 PHỤ LỤC qmaze.reset(rat_cell) envstate = qmaze.observe() run_model() print(steps) steps_robot = get_robot_route(steps) print("Đường mô phỏng") print(steps_robot) convert_to_Arduino() print("Đường thực tế") print(dir_path) print(len(dir_path)) train=1 first = key_last=0 if train==1: if i==0: print("Đã tìm đường tối ưu") i=1 else: if j == 0: print("Chưa có phịng chọn") j=1 elif first==1: print(f"prev_step:{value_5},obstacle:{value_6}") prev_step = value_5 if prev_step < len(dir_path): if value_3 == 0: if value_2 == 1: dir_old = dir_old BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 116 PHỤ LỤC else: dir_old = if value_3 == 6: if current_step !=prev_step: dir_old = dir_path[prev_step-1] current_step=prev_step elif value_3 !=0: dir_old = if prev_step >= len(dir_path): dir_old=0 if room == 0: print("haha") key_last=1 dir_path=[] steps_robot=[] steps=[] reward = envstate = num_step = train = first = i=0 j=0 rat_cell = goal if dir_old==1: if value_6 < 1000 and value_6 > 100: dir_old=0 elif value_6>1000: dir_old=dir_old if dir != dir_old or value_4 == or key_last==1: BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 117 PHỤ LỤC dir = dir_old return_data = {'received_data': dir, 'nums_step': len(dir_path),'key_last':key_last} return_str = json.dumps(return_data) ser.write(return_str.encode('utf-8') + b'\n') Chương trình xử lý cảm biến float number_pin = 0; float total_value = 0; for (int i = 0; i < 5; i++) { line_values[i] = !digitalRead(line_pins[i]); // have line = number_pin = number_pin + line_values[i]; total_value = total_value + line_values[i] * (i + 1) * 1; } if (number_pin == 0) { mode = 0; } else if (number_pin == 5) { mode = 6; if (finished_step == false) { Stop(); prev_step = prev_step + 1; spin = 1; finished_step = true; } xTaskNotifyGive(control_handle); xTaskNotifyGive(bluetooth_handle); BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 118 PHỤ LỤC } else { finished_step = false; mode = total_value / number_pin; } if (mode_tam != mode) { mode_old = mode_tam; mode_tam = mode; } if ((new_encoder_1 != old_encoder_1) || (new_encoder_2 != old_encoder_2)) { // Nếu giá trị encoder thay đổi, thực tính tốn // phải, trái deltaRight = new_encoder_1 - old_encoder_1; deltaLeft = new_encoder_2 - old_encoder_2; distanceLeft = pi * deltaLeft * leftDia / (2 * leftTicks);// van toc banh trai distanceRight = pi * deltaRight * rightDia / (2 * rightTicks); distanceCenter = (distanceLeft + distanceRight) / 2; theta = theta_old + ((distanceRight - distanceLeft) / L); theta_rad=theta*180/pi; x = x_old + cos(theta_rad) * distanceCenter; y = y_old + sin(theta_rad) * distanceCenter; x_old = x; y_old = y; theta_old = theta; old_encoder_1 = new_encoder_1; old_encoder_2 = new_encoder_2; BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 119 PHỤ LỤC } Chương trình điều khiển động if (prev_step < nums_step) { if (dir == 1) { Forward(); } else if (dir == 2) { Turn_right(); } else if (dir == 3) { Turn_left(); } else if (dir == 0) { Stop(); } } else { Stop(); if (key_last == 1) { prev_step = 1; nums_step = 0; BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 120 PHỤ LỤC } } BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 121 S K L 0