Thiết kế và thi công thiết bị nhận dạng chuyển động của đầu tích hợp cảm biến gia tốc cho việc điều khiển xe lăn

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 CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ TRUYỀN THÔNG ĐỀ TÀI: THIẾT KẾ VÀ THI CƠNG THIẾT BỊ NHẬN DẠNG CHUYỂN ĐỘNG CỦA ĐẦU CĨ TÍCH HỢP CẢM BIẾN GIA TỐC CHO VIỆC ĐIỀU KHIỂN XE LĂN GVHD: ThS TRẦN ĐĂNG KHOA SVTH1: HUỲNH NGỌC PHIÊN (15141235) SVTH2: PHẠM NGỌC QUỐC (15141260) TP Hồ Chí Minh, ngày 20 tháng 12 năm 2019 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 CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ TRUYỀN THÔNG ĐỀ TÀI: THIẾT KẾ VÀ THI CÔNG THIẾT BỊ NHẬN DẠNG CHUYỂN ĐỘNG CỦA ĐẦU TÍCH HỢP CẢM BIẾN GIA TỐC CHO VIỆC ĐIỀU KHIỂN XE LĂN GVHD: ThS TRẦN ĐĂNG KHOA SVTH1: HUỲNH NGỌC PHIÊN (15141235) SVTH2: PHẠM NGỌC QUỐC (15141260) TP Hồ Chí Minh, ngày 20 tháng 12 năm 2019 TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH KHOA ĐIỆN-ĐIỆN TỬ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP - TỰ DO - HẠNH PHÚC o0o TP HCM, ngày 20 tháng 12 năm 2019 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Huỳnh Ngọc Phiên MSSV: 15141235 Phạm Ngọc Quốc MSSV: 15141260 Chuyên ngành: KT thuật Điện tử truyền thông Mã ngành: 141 Hệ đào tạo: Đại học quy Mã hệ: Khóa: 2015 Lớp: 159410A I TÊN ĐỀ TÀI: THIẾT KẾ VÀ THI CÔNG THIẾT BỊ NHẬN DẠNG CHUYỂN ĐỘNG CỦA ĐẦU TÍCH HỢP CẢM BIẾN GIA TỐC CHO VIỆC ĐIỀU KHIỂN XE LĂN II NHIỆM VỤ: Các số liệu ban đầu: - Kích thước mơ hình: Bộ điều khiển động DC (chiều dài 16cm, chiều rộng 8cm, chiều cao 6cm), thiết bị nhận diện cử động đầu (tai nghe chụp tai SONY) - Thiết bị nhận diện cử động đầu cảm biến gia tốc quay thiết kế tai nghe chụp tai SONY - Bộ điều khiển động DC 24VDC cầu H nhận lệnh điều khiển từ thiết bị nhận diện cử động đầu bluetooth - Sử dụng cảm biến hồng ngoại để phát vật cản - Arduino Nano sử dùng làm xử lý trung tâm cho điều khiển động thiết bị nhận dạng chuyển động đầu Nội dung thực hiện: - Tìm hiểu ngun lí hoạt động cảm biến gia tốc, cảm biến khác khoảng cách, chuẩn truyền I2C, UART ii - Phân tích tín hiệu từ cảm biến gia tốc để biết liệu người dùng muốn sang trái, phải, tới, lui Phân tích tín hiệu từ cảm biến khác để nhận biết trường hợp nguy hiểm cho người dùng xe lăn - Từ liệu phân tích từ cảm biến gia tốc tiến hành kết hợp với vi điều khiển trung tâm để lập trình điều khiển hướng di chuyển xe lăn - Từ liệu thu từ cảm biến khác lực, khoảng cách, v.v để thiết kế hệ thống cảnh báo bảo đảm an toàn cho người sử dụng - Kết hợp tất phần xe lăn, cảm biến, vi điều khiển lập trình cho hệ thống - Kết hợp điều khiển xe lăn đầu điều khiển xe lăn ứng dụng di động thông qua wifi - Tiến hành chạy thử nghiệm, chỉnh sửa hoàn thiện thêm tính cho xe lăn - Tiến hành nhận xét, đánh giá chung toàn hệ thống - Tiến hành viết báo cáo đề án - Báo cáo đề tài tốt nghiệp III NGÀY GIAO NHIỆM VỤ: 02/09/2019 IV NGÀY HOÀN THÀNH NHIỆM VỤ: 25/12/2019 V HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: ThS Trần Đăng Khoa CÁN BỘ HƯỚNG DẪN BM ĐIỆN TỬ CÔNG NGHIỆP – Y SINH iii TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH KHOA ĐIỆN-ĐIỆN TỬ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP - TỰ DO - HẠNH PHÚC o0o TP HCM, ngày 20 tháng 12 năm 2019 LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Huỳnh Ngọc Phiên Lớp: 159410A MSSV: 15141235 Họ tên sinh viên 2: Phạm Ngọc Quốc Lớp: 159410A MSSV: 15141260 Tên đề tài: THIẾT KẾ VÀ THI CÔNG THIẾT BỊ NHẬN DẠNG CHUYỂN ĐỘNG CỦA ĐẦU TÍCH HỢP CẢM BIẾN GIA TỐC CHO VIỆC ĐIỀU KHIỂN XE LĂN Xác nhận Tuần/ngày Nội dung GVHD - Liên hệ với GVHD nhận đề tài tốt nghiệp (02/09/2019 – 08/09/2019) - Nộp đề cương chi tiết đề tài tốt nghiệp (09/09/2019 – 15/09/2019) - Tìm hiểu cảm biến gia tốc (accelerometer) (16/09/2019 – - Tìm hiểu cảm biến quay (gyroscope) 22/09/2019) - Tìm hiểu cách kết hợp gia tốc quay để tính góc quay - Tìm hiểu Arduino Nano (23/09/2019 – - Tìm hiểu kết nối không dây qua Bluetooth 29/09/2019) - Tiến hành mua đầy đủ linh kiện (30/09/2019 – - Xây dựng mơ hình để thực nghiên cứu 06/10/2019) - Tiến hành lấy liệu từ cảm biến gia tốc (07/10/2019 – quay dựa cử động đầu (quay trái, quay 13/10/2019) phải, gật trước, gật sau) - Tiến hành phân chia trường hợp từ liệu thu - Tiến hành lập trình cho Arduino Nano dựa trường hợp liệu thu - Tiến hành thiết kế phần cứng cho thiết bị nhận diện (14/10/2019 – dạng chuyển động dầu tai nghe chuụp tai 20/10/2019) - Tiến hành thí nghiệm thực tế đầu, kết hiển thị serial port Arduino LED đơn trượng trưng cho hướng trái, phải, tiến, lùi - Thiết kế điều khiển động DC iv (21/10/2019 – 27/10/2019) (28/10/2019 – 03/11/2019) 10 (04/11/2019 – 10/11/2019) 11 (11/11/2019 – 17/11/2019) 12 (18/11/2019 – 24/11/2019) 13 (15/11/2019 – 01/12/2019) 14 (02/12/2019 – 08/12/2019) 15 (09/12/2019 – 15/12/2019) 16 (16/12/2019 – 22/12/2019) - Kết nối thiết bị nhận dạng chuyển động đầu điều khiển động DC module bluetooth HC-05 - Lắp ráp, kết hợp xe lăn điện với điều khiển động DC thiết bị nhận dạng chuyển động đầu - Tiến hành chạy thử nghiệm mơ hình thực tế - Chạy thực tế xe để nhận biết khác biệt lúc lấy liệu khơng có chạy thực tế lúc chạy thực tế - Tiến hành sửa lỗi code dựa động tác (gật tới, gật lùi, quay trái, quay phải) - Nghiên cứu thiết kế thêm cảm biển phát vật cản - Chạy thực tế tiếp tụ hoàn thiện phần lập trình cho nhận biết dạng chuyển động đầu (gật tới, gật lùi, quay trái, quay phải) - Chạy thử nghiệm cảm biến phát vật cản - Sửa lỗi phần cứng phần lập trình - Tiến hành thiết kế điều khiển xe lăn ứng dụng di động thông qua wifi - Kết hợp điều khiển ứng dụng di động cử động đầu thiết bị - Tiến hành chạy thử nghiệm xe lăn điện điều khiển ứng dụng di động sửa lỗi - Viết luận văn - Chạy thử nghiệm toàn xe lăn điện - Viết luận văn - Hoàn chỉnh, cân chỉnh toàn xe lăn điện (phần ứng phần mềm) - Viết hoàn chỉnh luận văn - Hoàn chỉnh luận văn - Nộp luận văn - Phản biện - Báo cáo trước hội đồng GVHD (Ký ghi rõ họ tên) v LỜI CAM ĐOAN Đề tài tự thực dựa vào số tài liệu không chép từ tài liệu hay cơng trình có trước Người thực đề tài Sinh viên Sinh viên Huỳnh Ngọc Phiên Phạm Ngọc Quốc vi LỜI CẢM ƠN Em xin gửi lời cảm ơn sâu sắc đến thầy Trần Đăng Khoa – Giảng viên môn Điện tử công nghiệp – y sinh, thầy cô khác môn tạo điều kiện kiến thức sở vật chất trình chúng em thực đề tài Em gửi lời cảm ơn đến bạn thực đề tài khác phòng lab y sinh chia sẻ trao đổi kinh nghiệm kiến thức để nhóm hồn thiện đề tài Cảm ơn gia đình ln bên cạnh nhóm q trình thực đề tài Xin chân thành cảm ơn! Sinh viên Huỳnh Ngọc Phiên Người thực đề tài Sinh viên Phạm Ngọc Quốc vii MỤC LỤC TRANG BÌA……………………………………………………………………………i NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP .ii LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP iv LỜI CAM ĐOAN vi LỜI CẢM ƠN vii MỤC LỤC viii LIỆT KÊ HÌNH x LIỆT KÊ BẢNG xiii TÓM TẮT xiv Chương TỔNG QUAN 1.1 ĐẶT VẤN ĐỀ: 1.2 MỤC TIÊU: 1.3 NỘI DUNG NGHIÊN CỨU: 1.4 GIỚI HẠN: 1.5 BỐ CỤC: Chương CƠ SỞ LÍ THUYẾT 2.1 Chuyển động đầu không gian: 2.1.1 Phương hướng không gian: 2.1.2 Chuyển động đầu không gian chiều: 2.2 Gới thiệu phần cứng: 2.2.1 Cảm biến gia tốc: 2.2.2 Arduino Nano: 10 2.2.3 Module thu phát Wifi ESP8266 Node MCU: 13 2.2.4 Mạch thu phát Bluetooth HC-05: 15 2.2.5 Mạch Điều Khiển Động Cơ DC BTS7960 43A (1 Động Cơ): 17 2.2.6 Động Servo SG90: 19 2.2.7 Cảm biến hồng ngoại Analog SHARP - GP2Y0A02YK0F 20 2.2.8 Pin Sạc Lipo 2000mAh 3.7V (LP803860): 21 2.2.9 TP4056 Mạch Sạc Pin MiniUSB 1A: 23 2.2.10 Mạch Giảm Áp DC LM2596 3A: 25 2.2.11 Chuẩn giáo tiếp I2C: 26 viii Chương TÍNH TỐN VÀ THIẾT KẾ 28 3.1 GIỚI THIỆU: 28 3.2 TÍNH TỐN VÀ THIẾT KẾ HỆ THỐNG: 28 3.2.1 Thiết kế sơ đồ khối hệ thống: 28 3.2.2 Tính tốn thiết kế mạch: 30 3.2.3 Thiết kế sơ đồ ngun lí tồn hệ thống: 39 Chương THI CÔNG HỆ THỐNG 41 4.1 GIỚI THIỆU: 41 4.2 THI CÔNG HỆ THỐNG: 41 4.2.1 Thiết kế phần cứng cho hệ thống: 41 4.2.2 Lắp ráp kiểm tra: 43 4.4 LẬP TRÌNH HỆ THỐNG: 49 4.4.1 Lưu đồ giải thuật: 49 4.4.2 Phần mềm lập trình cho vi điều khiển: 53 4.4.2 Phần mềm điện thoại: 57 4.5 LẬP TRÌNH MƠ PHỎNG: 60 4.5.1 Lưu đồ lập trình mô phỏng: 60 4.6 VIẾT TÀI LIỆU HƯỚNG DẪN SỬ DỤNG: 61 4.6.1 Viết tài liệu hướng dẫn sử dụng: 61 4.6.1 Quy trình thao tác: 61 Chương KẾT QUẢ_NHẬN XÉT_ĐÁNH GIÁ 63 5.1 KẾT QUẢ: 63 5.2 NHẬN XÉT_ĐÁNH GIÁ: 71 Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 73 6.1 KẾT LUẬN: 73 6.1.1 Đạt được: 73 6.1.2 Chưa đạt được: 73 6.1 HƯỚNG PHÁT TRIỂN: 73 TÀI LIỆU THAM KHẢO PHỤ LỤC ix CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN • Áp dụng điều khiển giọng nói cho xe lăn để tăng thêm tính cho xe lăn • Thêm camera để người thân theo dõi tránh cố đáng tiết khơng đáng có xảy BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 74 TÀI LIỆU THAM KHẢO TÀI LIỆU THAM KHẢO [1] Trần Hoàng Phi – Trương Văn Vinh, Thiết kế - Chế tạo xe lăn điện cho người khuyết tật phần cơ, Đồ án tốt nghiệp Đại Học Sư Phạm Kỹ Thuật TP Hồ Chí Minh (2015) [2] Ngơ Tùng, Chế xe lăn điều khiển đầu, tienphong.vn (Nhịp sống - Cộng đồng mạng – Tài trẻ) 10/04/2018 [3] David Sarver - Marc Ackerman, Pitch, roll, and yaw: Describing the spatial orientation of dentofacial traits, Researchgate April 2007 [4] Hồng Minh Cơng, “Giáo trình cảm biến cơng nghiệp”, Đại Học Bách Khóa Đà Nẵng 2004 [5] Lê Cảnh Trung - Phạm Quang Huy, “Lập trình điều khiển với Arduino”, NXB Khoa học kỹ thuật [6] Lê Mỹ Hà – Phạm Quang Huy, “Lập trình IOT với Arduino”, NXB Thanh Niên [7] Anisha Cotta, Naik Trupti Devidas, “Wireless communication using HC05 Bluetooth module interfaced with arduino”, International Journal of Science, Engineering and Technology Research (IJSETR) Volume 5, Issue 4, April 2016 [8] H P Ramadhan, G Mansur, "Permanent magnet DC motor control by using arduino and motor drive module BTS7960", IOP Conference Series: Materials Science and Engineering [9] Benne de Bakker, "How to use a SHARP GP2Y0A21YK0F IR Distance Sensor with Arduino", Makerguides.com [10] Nguyễn Đình Phú, “Giáo trình vi xử lý II”, NXB ĐH Quốc Gia Tp.HCM, 2007 BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH PHỤ LỤC PHỤ LỤC ❖ Phần code nhận dạng tín hiệu: #include #include #include #include #define TxD #define RxD #define #define #define #define #define #define int int int int int int BT_LEFT BT_RIGHT BT_FOR BT_BACK 10 BT_STOP 11 BT_SW 12 BT_LEFT_STATE = 0; BT_RIGHT_STATE = 0; BT_FOR_STATE = 0; BT_BACK_STATE = 0; BT_STOP_STATE = 0; BT_SW_STATE = 0; SoftwareSerial mySerial(RxD, TxD);//RX-TX SimpleKalmanFilter simpleKalmanFilter_R(1, 1, 0.001); SimpleKalmanFilter simpleKalmanFilter_P(1, 1, 0.001); SimpleKalmanFilter simpleKalmanFilter_Y(1, 1, 0.001); const int MPU = 0x68; // MPU6050 I2C address //DECLARE VARIABLE FOR GETTING RAWDATA float AccX, AccY, AccZ; float GyroX, GyroY, GyroZ; float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ; float roll, pitch, yaw, rollF, pitchF, yawF; float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ; float elapsedTime, currentTime, previousTime; float Gy_ER_X, Gy_ER_Y, Gy_ER_Z, STOP_FLAG_L, STOP_FLAG_R, STOP_FLAG_F, STOP_FLAG_B; //DECLARE FOR YAW CHANGING float yawF_Temp, ENA_BACK_YAW; int ENA_L_ON = 0, ENA_L_OFF = 0, ENA_R_ON = 0, ENA_R_OFF = 0; int TURN_L_ENA = 0, TURN_R_ENA = 0; //DECLARE FOR PITCH CHANGING float pitchF_Temp, ENA_BACK_PITCH; int ENA_L_ON_P = 0, ENA_L_OFF_P = 0, ENA_R_ON_P = 0, ENA_R_OFF_P = 0; int TURN_L_ENA_P = 0, TURN_R_ENA_P = 0; BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH PHỤ LỤC //DECLARE FOR ROLL CHANGING float rollF_Temp, ENA_BACK_ROLL; int ENA_F_ON_R = 0, ENA_F_OFF_R = 0, ENA_B_ON_R = 0, ENA_B_OFF_R = 0; int TURN_F_ENA_R = 0, TURN_B_ENA_R = 0; //SET VARIABLE FOR GETTING ERROR int c = 0; void setup() { mySerial.begin(38400); Serial.begin(38400); Wire.begin(); Wire.beginTransmission(MPU); MPU6050 // MPU=0x68 Wire.write(0x6B); Wire.write(0x00); the 6B register Wire.endTransmission(true); // Initialize comunication // Start communication with // Talk to the register 6B // Make reset - place a into //end the transmission pinMode(13,OUTPUT); pinMode(BT_LEFT,INPUT); pinMode(BT_RIGHT,INPUT); pinMode(BT_FOR,INPUT); pinMode(BT_BACK,INPUT); pinMode(BT_STOP,INPUT); //GET ERROR CONST Gy_ER_X = calculate_IMU_error_X(); Gy_ER_Y = calculate_IMU_error_Y(); Gy_ER_Z = calculate_IMU_error_Z(); delay(20); } void loop() { // === Read acceleromter data === // Wire.beginTransmission(MPU); Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU, 6, true); // Read registers total, each axis value is stored in registers //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet AccX = (Wire.read() 20)){ ENA_F_ON_R = 100; ENA_F_OFF_R = 0; Timer1.detachInterrupt(); } if ((ENA_BACK_ROLL < -0.15) && (rollF > 7)){ ENA_F_OFF_R = 100; ENA_F_ON_R = 0; gyroAngleX = 0; accAngleX = 0; Timer1.detachInterrupt(); } if ((ENA_BACK_ROLL < -0.15) && (rollF < -20)){ ENA_B_ON_R = 100; ENA_B_OFF_R = 0; Timer1.detachInterrupt(); } if ((ENA_BACK_ROLL > 0.15)&& (rollF < -15)) { ENA_B_OFF_R = 100; ENA_B_ON_R = 0; gyroAngleX = 0; accAngleX = 0; Timer1.detachInterrupt(); } //SET MODE FOR CONTROLING FORWARD - BACKWARD if (ENA_F_ON_R == 100){ TURN_F_ENA_R = 1; TURN_L_ENA = 0; TURN_R_ENA = 0; BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH PHỤ LỤC } if (ENA_F_OFF_R == 100){ TURN_F_ENA_R = 0; } if (ENA_B_ON_R == 100){ TURN_B_ENA_R = 1; TURN_L_ENA = 0; TURN_R_ENA = 0; } if (ENA_B_OFF_R == 100){ TURN_B_ENA_R = 0; } //SET MODE FOR CONTROLING LEFT - RIGHT if (ENA_L_ON == 100){ TURN_L_ENA = 1; TURN_F_ENA_R = 0; TURN_B_ENA_R = 0; } if (ENA_L_OFF == 100){ TURN_L_ENA = 0; } if (ENA_R_ON == 100){ TURN_R_ENA = 1; TURN_F_ENA_R = 0; TURN_B_ENA_R = 0; } if (ENA_R_OFF == 100){ TURN_R_ENA = 0; } //READ NODEMCU BT_LEFT_STATE = digitalRead(BT_LEFT); BT_RIGHT_STATE = digitalRead(BT_RIGHT); BT_FOR_STATE = digitalRead(BT_FOR); BT_BACK_STATE = digitalRead(BT_BACK); BT_STOP_STATE = digitalRead(BT_STOP); BT_SW_STATE = digitalRead(BT_SW); // -//CONTROL LEFT - RIGHT (YAW - PITCH) - CONTROL FORWARD AND BACKWARD VIA ROLL CHANGING if (((TURN_F_ENA_R == 1) && (BT_SW_STATE == 0)) || ((BT_FOR_STATE == 1) && (BT_RIGHT_STATE == 0) && (BT_LEFT_STATE == 0) && (BT_BACK_STATE == 0) && (BT_STOP_STATE == 0) && (BT_SW_STATE == 1))) { Serial.println("FOR"); mySerial.print('3'); } if (((TURN_B_ENA_R == 1) && (BT_SW_STATE == 0)) || ((BT_FOR_STATE == 0) && (BT_RIGHT_STATE == 0) && (BT_LEFT_STATE == 0) && (BT_BACK_STATE == 1) && (BT_STOP_STATE == 0) && (BT_SW_STATE == 1))) { Serial.println("BACK"); mySerial.print('2'); } BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH PHỤ LỤC if (((TURN_L_ENA == 1) && (BT_SW_STATE == 0)) || ((BT_FOR_STATE == 0) && (BT_RIGHT_STATE == 0) && (BT_LEFT_STATE == 1) && (BT_BACK_STATE == 0) && (BT_STOP_STATE == 0) && (BT_SW_STATE == 1))) { Serial.println("LEFT"); mySerial.print('1'); } if (((TURN_R_ENA == 1) && (BT_SW_STATE == 0)) || ((BT_FOR_STATE == 0) && (BT_RIGHT_STATE == 1) && (BT_LEFT_STATE == 0) && (BT_BACK_STATE == 0) && (BT_STOP_STATE == 0) && (BT_SW_STATE == 1))) { Serial.println("RIGHT"); mySerial.print('4'); } // -STOP if (((TURN_L_ENA == 0) && (TURN_R_ENA == 0) && (TURN_F_ENA_R == 0) && (TURN_B_ENA_R == 0)) && ((BT_FOR_STATE == 0) && (BT_RIGHT_STATE == 0) && (BT_LEFT_STATE == 0) && (BT_BACK_STATE == 0) && (BT_STOP_STATE == 1))) { Serial.println("STOP"); mySerial.print('6'); } // -//YAW - PTICH - ROLL CHANGE DETECTION yawF_Temp = yawF; pitchF_Temp = pitchF; rollF_Temp = rollF; // // // // // Serial.print(rollF); Serial.print(" "); Serial.print(pitchF); Serial.print(" "); Serial.println(yawF); } void Blink() {//digitalWrite(13,!digitalRead(13)); yaw=0; //trình phục vụ ngắt } ❖ Phần code chương trình tính OFFSET cho cảm biến: //BY ME: float calculate_IMU_error_X() { c = 0; // Read gyro values 200 times while (c < 200) { Wire.beginTransmission(MPU); Wire.write(0x43); Wire.endTransmission(false); BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH PHỤ LỤC Wire.requestFrom(MPU, 6, true); GyroX = Wire.read() 27) && (y < 153)){ Serial.print('4'); //RIGHT digitalWrite(RIGHT, HIGH); digitalWrite(LEFT, LOW); digitalWrite(FOR, LOW); digitalWrite(BACK, LOW); digitalWrite(STOP, LOW); } else if ((y > 153) && (x > 27) && (x < 153)){ Serial.print('3'); //FOR digitalWrite(FOR, HIGH); digitalWrite(RIGHT, LOW); digitalWrite(LEFT, LOW); digitalWrite(BACK, LOW); digitalWrite(STOP, LOW); } else if ((y < 27) && (x > 27) && (x < 153)){ Serial.print('2'); //BACK digitalWrite(BACK, HIGH); digitalWrite(RIGHT, LOW); digitalWrite(FOR, LOW); digitalWrite(LEFT, LOW); digitalWrite(STOP, LOW); } else { Serial.print('6'); //STOP digitalWrite(STOP, HIGH); digitalWrite(RIGHT, LOW); digitalWrite(FOR, LOW); digitalWrite(BACK, LOW); digitalWrite(LEFT, LOW); } } BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH PHỤ LỤC void setup() { pinMode(LEFT, OUTPUT); pinMode(RIGHT, OUTPUT); pinMode(FOR, OUTPUT); pinMode(BACK, OUTPUT); pinMode(STOP, OUTPUT); Serial.begin(115200); Blynk.begin(auth, ssid, pass); } void loop() { Blynk.run(); } BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH ... TÀI: THI T KẾ VÀ THI CÔNG THI T BỊ NHẬN DẠNG CHUYỂN ĐỘNG CỦA ĐẦU TÍCH HỢP CẢM BIẾN GIA TỐC CHO VIỆC ĐIỀU KHIỂN XE LĂN II NHIỆM VỤ: Các số liệu ban đầu: - Kích thước mơ hình: Bộ điều khiển động. .. THI T KẾ VÀ THI CƠNG THI T BỊ NHẬN DẠNG CHUYỂN ĐỘNG CỦA ĐẦU TÍCH HỢP CẢM BIẾN GIA TỐC CHO VIỆC ĐIỀU KHIỂN XE LĂN” Mục tiêu hướng tới người tàn tật, bệnh nhân khơng có khả điều khiển tay, xe lăn. .. 15141260 Tên đề tài: THI T KẾ VÀ THI CƠNG THI T BỊ NHẬN DẠNG CHUYỂN ĐỘNG CỦA ĐẦU TÍCH HỢP CẢM BIẾN GIA TỐC CHO VIỆC ĐIỀU KHIỂN XE LĂN Xác nhận Tuần/ngày Nội dung GVHD - Liên hệ với GVHD nhận đề tài tốt