Điều Khiển Robot 1 Bánh

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP HCM - NGUYỄN XUÂN TIÊN ĐI U HI N RO OT NH LUẬN VĂN THẠC SĨ Chuyên ngành : ỹ Thuật Cơ Điện t Mã số ngành: 60 52 01 14 TP HỒ CHÍ MINH, 30 tháng 12 năm 2013 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC CÔNG NGHỆ TP HCM - NGUYỄN XUÂN TIÊN ĐIỀU KHIỂN RO OT NH LUẬN VĂN THẠC SĨ Chuyên ngành : Kỹ Thuật Cơ Điện t Mã số ngành: 60 52 01 14 HƯỚNG DẪN KHOA HỌC: PGS.TSKH H Đ L TS N u n Th nh Ph ơn TP HỒ CHÍ MINH, ngày 30 tháng 12 năm 2013 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI RƯỜ G ĐẠI HỌC CÔ G GHỆ Cán hướng dẫn khoa học : HCM PGS.TSKH H Đ TS Luận văn Thạc sĩ bảo vệ Trường Đại học Công nghệ TP HCM ngày 25 tháng 01 năm 2014 Cán chấm nhận xét 1: TS ô Cao C Cán chấm nhận xét 2: TS V H Duy Thành phần Hội đồng đánh giá Luận văn Thạc sĩ gồm: (Ghi rõ họ, tên, học hàm, học vị Hội đồng chấm bảo vệ Luận văn Thạc sĩ) Ch t ch PGS.TS 2.TS ôC 3.TS H C Phản biện H 4.TS 5.TS Phản biện H y vi n y vi n Thư k Xác nhận c a Ch t ch Hội đồng đánh giá luận văn hoa quản l chuy n ngành sau luận văn sửa chữa (nếu có) Ch t ch Hội đồng đánh giá LV PGS.TS Quản l chuy n ngành TRƯỜNG ĐẠI H C CƠNG NGHỆ TP HCM HỊ G KH - Đ ĐH CỘ G HÒA XÃ HỘI CHỦ GHĨA IỆ Đ lập - ự d - Hạ p ú AM TP HCM, ngày 30 tháng 12 năm 2013 HIỆM Ụ UẬ Họ t n học vi n: GUYỄ XUÂ Ă IÊ HẠC Ĩ Giới tính: Nam Ngày tháng năm sinh: 19/06/1965 Nơi sinh: Hu Chuy n ngành: MSHV: 1241840018 ỹ Thuật Cơ Điện tử I- Ê ĐỀ ÀI: ĐIỀU KHIỂ R H II- HIỆM Ụ À ỘI U G: - Nghi n cứu l thuy t điều khiển hồi ti p n tính hố, điều khiển LQR - Áp dụng phương pháp điều khiển hổi ti p n tính hố điều khiển LQR điều khiển hệ robot bánh t c n b ng - Mô k t điều khiển đối tượng tr n Matlab\Simulink - Kiểm chứng k t mô b ng th c nghiệm điều khiển đối tượng th c III- GÀY GIA HIỆM Ụ: 12/06/2013 IV- GÀY H À HÀ H HIỆM Ụ: 30/12/2013 V- C Ộ HƯỚ G Ẫ : PGS.TSKH H Đ TS Cán hướng dẫn khoa học PGS.TSKH H Đ Quản l chuy n ngành TS i LỜI CAM ĐOAN Tôi xin cam đoan r n u n v n v i n i dun ” trình n hiên cứu riên dư i hư n dẫn PGS.TSKH Hồ Đắc c TS N uyễn Thanh Phươn Tôi xin cam đoan r n iúp đỡ cho việc thực u n v n cảm ơn thơn tin trích dẫn tron u n v n rõ n uồn ốc Các số liệu, kết nêu tron lu n v n trun thực, có n uồn trích dẫn chưa t n côn bố tron b t trình khác Tp Hồ Chí Minh, n ày 30 thán 12 n m 2013 Học viên thực luận văn Nguyễn Xuân Tiên ii LỜI CÁM ƠN Trong trình thực luận văn, gặp nhiều khó khăn nhờ hướng dẫn tận tình Th y P T c c, Th y TS guy n Thanh hư ng, tơi hồn thành luận văn thời gian quy định ể hoàn thành luận văn tơi xin bày tỏ lòng biết n sâu s c Th y PGS.TSKH c c, Th y TS guy n Thanh hư ng, Th y người tận tâm hết lòng học viên, hướng dẫn nhiệt tình cung cấp cho tơi tài liệu vô quý giá thời gian thực luận văn Xin chân thành cám n tập thể th y giáo trường ại học Cơng ghệ T Chí Minh, giảng dạy truyền đạt kiến thức cho tôi, giúp tơi học tập nghiên cứu q trình học cao học trường Xin chân thành cảm n Ban giám hiệu, hòng quản lý Khoa học đại học phòng ban trường ại học Cơng ghệ T tạo sau Chí Minh, giúp đỡ, tạo điều kiện thuận lợi cho tơi q trình học tập làm luận văn cao học trường Xin chân thành cảm n anh, chị đ ng nghiệp hỗ trợ giúp đỡ cho tơi q trình thực luận văn Xin chân thành cảm n bạn học viên lớp cao học chuyên ngành “ ỹ thuật C iện t -12SC 11” đóng góp ý kiến cho tơi q trình thực luận văn TP Hồ Chí Minh, n ày 30 thán 12 n m 2013 Học viên thực luận văn Nguyễn Xuân Tiên iii TÓM TẮT LUẬN VĂN iện obot ứng d ng ph biến sản xuất công nghiệp obot m t bánh tự cân b ng gọi Ballbot nhà khoa học nghiên cứu đưa vào ứng d ng m t số l nh vực Do đ ng nó, m t robot m t bánh cao hẹp trở thành m t ứng c viên lý tưởng cho robot di đ ng cá nhân ó hoạt đ ng m t robot dịch v hiệu nhà văn phòng Do đó, robot m t bánh có m t loạt ứng d ng ngành cơng nghiệp giải trí bao g m đ ch i uận văn sâu vào nghiên cứu khái quát hệ robot m t bánh tự cân b ng , nghiên cứu mặt lý thuyết điều khiển hệ robot m t bánh tự cân b ng thông qua phư ng pháp điều khiển h i tiếp tuyến tính hóa, phư ng pháp điều khiển LQR, thuật toán b lọc Kalman rời rạc tiến hành mô kiểm tra hệ thống b điều khiển uận văn nghiên cứu điều khiển ph n cứng chủ yếu dựa vào vi điều khiển ATmega 328 dạng chip 28 chân ph n mềm ngơn ngữ lập trình dựa ngơn ngữ C++ , nh m m c đích giám sát điều khiển hệ robot m t bánh để n định thăng b ng di chuyển không bị t ngã uận văn tập trung chủ yếu xây dựng mơ hình thực nghiệm robot m t bánh tự cân b ng m t robot m t bánh thực tế bao g m thi công thực nghiệm ph n cứng bao g m : C cấu b lái bóng chọn lựa bóng; Mạch cảm biến; Mạch vi điều khiển; Mạch ngu n; Mạch công suất điều khiển đ ng c khung ph n thân robot ng thời tiến hành viết chư ng trình điều khiển để điều khiển mơ hình thực nghiệm hệ robot m t bánh giữ n định thăng b ng di chuyển theo mong muốn Thông qua luận văn này, c ng hy vọng cung cấp m t mơ hình thực nghiệm hệ robot m t bánh tự cân b ng m t số kiến thức hữu ích cho kỹ sư, sinh viên v v… học tập nghiên cứu hệ thống robot iv ABSTRACT Nowadays, robot has been applying popularly in production industry A selfbalancing ball robot called ballbot is used by scientists in a number of fields Due to its dynamic, a tall and thin ballbot could become an ideal candidate for personal mobile robot It can be operated efficiently as a serviced robot at home and in offices Therefore, ballbot would have various applications in the entertainment industry, including toys My thesis studies deeply the general of a self-balancing ball robot system Studying about control theory of a self-balancing ball robot system via linearised feedback methods, the ways to control the Linear quadra regulator, unconnected Kalman filter algorithm and to conduct simulation system inspection for each controller.This thesis studies the hardware drivers mtic reainly based on ATmega 328 micro-controller, is a 28-pins micro chip, and C++ programing language software Aiming to monitor and control one-ball robot system in order to stablize balancing and move robot without being falling This thesis focuses mainly to build a self- balancing ball robot model as a practical ball robot covering hardware experimental execution including : balloon driver and balloon selection structure; sensor circuit; micro-controller circuit; power circuit, motor power rating circuit and robot meromorphic At the same time, conducting controlling program in order to controll experimental model of one-ball robot system, keeping balance stability and moving as expecting Through this thesis, I also hope to provide some experimental models of a selfbalancing ball robot system and other useful knowledge for students, engineers etc who are studing and doing research about ballbot systems v MỤC LỤC Tên đề mục Trang ời cam đoan i ời cảm n ii Tóm t t luận văn iii Abstract iv M c l c v Danh m c từ viết t t ix Danh m c bảng biểu .x Danh m c s đ , hình ảnh xi Chƣơng 1: T ng qu n hƣ ng nghiên c u 1.1 ặt vấn đề 1.2 Thế robot m t bánh tự cân b ng 1.3 Tính cấp thiết đề tài, ý ngh a khoa học thực ti n đề tài 1.4 Tình hình nghiên cứu nhu c u thực tế 1.5 M c đích nghiên cứu , khách thể , đối tượng nghiên cứu 1.6 hiệm v nghiên cứu giới hạn đề tài 1.6.1 hiệm v nghiên cứu 1.6.2 iới hạn đề tài 1.7 hư ng pháp nghiên cứu 1.8 i dung luận văn vi Chƣơng 2: Mơ hình hó to n học hệ ro ot m t 2.1 nh 11 Mơ hình đ n giản hóa robot m t bánh 11 2.1.1 Các giả định 11 2.1.2 hư ng pháp Euler- Lagrange 11 2.1.3 Tham số vật lý 14 2.1.4 Các quan hệ tọa đ 15 ối với bóng 15 ối với thân robot 16 ối với đ ng c 16 2.2 hư ng trình Lagrange –Euler 16 2.3 hảo sát đặc tính phi tuyến 18 2.4 Tuyến tính hóa 21 2.5 hư ng trình trạng thái đ y đủ 23 2.6 ết luận 23 Chƣơng 3: Thiết kế 3.1 điều khiển 25 Xây dựng b điều khiển tồn phư ng tuyến tính 25 3.1.1 Mơ hình khơng gian trạng thái tuyến tính 25 3.1.2 Tính tốn đ lợi b điều khiển 27 3.1.3 Mô kiểm tra hệ thống 28 3.2 Xây dựng b điều khiển h i tiếp tuyến tính hóa TTT 33 3.2.1 Cấu trúc nhiệm v điều khiển 33 3.2.2 Tuyến tính hóa hệ thống b ng h i tiếp 33 80   x k  x k  K *( zk  H * x k ) (3.37) Phương trình (3.37) có nghĩa giá trị nghiệm sau ước lượng x tính giá trị tiên nghiệm sau thêm bớt tí dựa vào sai  số giá trị đo giá trị đo đạc ước đoán H * x k K độ lợi (gain) mạch lọc Kalman Câu h i đặt làm để chọn K tối ưu Tối ưu theo nghĩa covariance sai số ước lượng nghiệm sau  ek  K *( zk  H * x k ) nh Bằng cách thay ek vào biểu thức tính Pk , sau lấy đạo hàm Pk theo K, ta tìm giá trị mà tương ứng với Pk nh Kk  Pk H T ( HPk H T  R)1 K k thay đổi theo thời gian k độ lợi cần tìm mạch lọc Kalman ước đốn Tóm lại mạch lọc Kalman bao gồm bước : 1- Ước đoán trạng thái tiên nghiệm 2- Dựa vào kết đo để hiệu chỉnh lại ước đốn Ta tóm tắt lại hoạt động mạch lọc Kalman phương trình sau: Giả sử bạn có giá trị ước đoán x k 1 thời điểm (k-1) biết giá trị điều khiển uk 1 ( iá trị ban đầu thời điểm chọn x0  H * z0 ) Lúc bạn việc tiến hành tính tốn từ đến bước từ đến bước hình 81 Hìn 3.2: Ha trìn mạc c Kalman Hình tóm tắt mạch lọc Kalman hó kh n mạch lọc Kalman làm để mơ hình hóa trạng thái đo đạc để có phương trình (3.33) (3.34) từ áp dụng thuật tốn Kalman ụ ục 3: C ương trìn m.file c ạy at ab đ u k ển b ng p ương p áp đ u k ển LQR clc; clear; % Hang so g = 9.81; % gia toc truong % Tham so Ballbot Mb = 0.5; % Khoi luong qua bong [kg] Rb = 0.11; % Ban kinh qua bong [don vi m] % Ib = 2*Mb*Rb^2/3 Ib = 0.0266; % Moment quan tinh cua qua bong [kg.m^2] IBx = 5.6; % Moment quan tinh body theo truc x [kg.m^2] IBy = 5.7; % Moment quan tinh body theo truc y [kg.m^2] MB = 5,0; % Khoi luong body [kg] L = 0.2; % Khoang cach tu tam body den tam qua bong [don vi m] n = 25/26; % Ti so truyen UBb = 0.2; % He so ma sat giua body va bong 82 UBg = 0.01; % He so ma sat giua body va mat dat IM = 4.62*10^-5; % Moment quan tinh motor [kg.m^2] Kb = 0.04768; % He so suc dien dong motor [V/rad/sec] Kt = 0.0742; % He so moment xoan Motor [Nm/A] Rm = 1.1; % Dien tro phan ung [Ohm] Dx = IBx*IM + IM*Ib - 2*IM*Ib*n + IBx*Ib*n^2 + IM*Ib*n^2 + IM*L^2*MB + IM*MB*Rb^2+ IM*Mb*Rb^2 + Ib*L^2*MB*n^2 + IBx*MB*n^2*Rb^2 + IM*MB*n^2*Rb^2 + IBx*Mb*n^2*Rb^2+ IM*Mb*n^2*Rb^2 + 2*IM*L*MB*Rb - 2*IM*MB*n*Rb^2 - 2*IM*Mb*n*Rb^2 + L^2*MB*Mb*n^2*Rb^22*IM*L*MB*n*Rb; % Ma tran A Ax(3,1)= g*L*MB*(IM + Ib*n^2 + MB*n^2*Rb^2 + Mb*n^2*Rb^2 )/Dx; Ax(4,1)=-g*L*MB*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb )/Dx; Ax(3,3)=-UBg*(IM + Ib*n^2 + MB*n^2*Rb^2 + Mb*n^2*Rb^2 )/Dx; Ax(4,3)=UBg*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb)/Dx; Ax(3,4)=(UBb+Kb*Kt/Rm)*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb)/Dx; Ax(4,4)= -(UBb+Kb*Kt/Rm)*(IBx + IM + Ib + L^2*MB + MB*Rb^2 + Mb*Rb^2 + 2*L*MB*Rb)/Dx; Bx(3,1)= (-Kt*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb))/(Dx*Rm); Bx(4,1)= Kt*(IBx + IM + Ib + L^2*MB + MB*Rb^2 + Mb*Rb^2 + 2*L*MB*Rb)/(Dx*Rm); 83 Dy = IBy*IM + IM*Ib - 2*IM*Ib*n + IBy*Ib*n^2 + IM*Ib*n^2 + IM*L^2*MB + IM*MB*Rb^2+ IM*Mb*Rb^2 + Ib*L^2*MB*n^2 + IBy*MB*n^2*Rb^2 + IM*MB*n^2*Rb^2 + IBy*Mb*n^2*Rb^2+ IM*Mb*n^2*Rb^2 + 2*IM*L*MB*Rb - 2*IM*MB*n*Rb^2 - 2*IM*Mb*n*Rb^2 + L^2*MB*Mb*n^2*Rb^22*IM*L*MB*n*Rb; Ay(3,1)= g*L*MB*(IM + Ib*n^2 + MB*n^2*Rb^2 + Mb*n^2*Rb^2 )/Dy; Ay(4,1)=-g*L*MB*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb )/Dy; Ay(3,3)=-UBg*(IM + Ib*n^2 + MB*n^2*Rb^2 + Mb*n^2*Rb^2 )/Dy; Ay(4,3)=UBg*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb)/Dy; Ay(3,4)=(UBb+Kb*Kt/Rm)*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb)/Dy; Ay(4,4)= -(UBb+Kb*Kt/Rm)*(IBx + IM + Ib + L^2*MB + MB*Rb^2 + Mb*Rb^2 + 2*L*MB*Rb)/Dy; By(3,1)= -Kt*(IM + Ib*n + MB*n*Rb^2 + Mb*n*Rb^2 + L*MB*n*Rb)/(Dy*Rm); By(4,1)= Kt*(IBy + IM + Ib + L^2*MB + MB*Rb^2 + Mb*Rb^2 + 2*L*MB*Rb)/(Dy*Rm); A = [0 0 0 ; 00000100; 00000010; 00000001; Ax(3,1) 0 Ax(3,3) Ax(3,4) 0 ; 0 Ay(3,1) 0 Ay(3,3) Ay(3,4) ; Ax(4,1) 0 Ax(4,3) Ax(4,4) 0 ; 0 Ay(4,1) 0 Ay(4,3) Ay(4,4) ]; B=[ 00; 84 00; 00; 00; Bx(3,1) ; Bx(4,1) ; By(3,1) ; By(4,1)]; Q =[ 1000 0 0 0 ; 01000000; 0 1000 0 0 ; 0 0 0; 0 0 0 0; 0 0 0; 0 0 0 10 0; 0 0 0 10]; % R = 6e3 * (180/pi)^2; C = eye(8); D = zeros (8,2); R =[5 ; 10]; K = lqr(A,B,Q,R); ụ ục C ương trìn v đ u k ển tmega 328 #include #define MPU6050_AUX_VDDIO 0x01 // R/W #define MPU6050_SMPLRT_DIV 0x19 // R/W #define MPU6050_CONFIG 0x1A // R/W 85 #define MPU6050_GYRO_CONFIG 0x1B // R/W #define MPU6050_ACCEL_CONFIG 0x1C // R/W #define MPU6050_FF_THR 0x1D // R/W #define MPU6050_FF_DUR 0x1E // R/W #define MPU6050_MOT_THR 0x1F // R/W #define MPU6050_MOT_DUR 0x20 // R/W #define MPU6050_ZRMOT_THR 0x21 // R/W #define MPU6050_ZRMOT_DUR 0x22 // R/W #define MPU6050_FIFO_EN 0x23 // R/W #define MPU6050_I2C_MST_CTRL #define MPU6050_I2C_SLV0_ADDR #define MPU6050_I2C_SLV0_REG 0x24 // R/W 0x25 // R/W 0x26 // R/W #define MPU6050_I2C_SLV0_CTRL 0x27 // R/W #define MPU6050_I2C_SLV1_ADDR 0x28 // R/W #define MPU6050_I2C_SLV1_REG 0x29 // R/W #define MPU6050_I2C_SLV1_CTRL 0x2A // R/W #define MPU6050_I2C_SLV2_ADDR 0x2B // R/W #define MPU6050_I2C_SLV2_REG 0x2C // R/W #define MPU6050_I2C_SLV2_CTRL 0x2D // R/W #define MPU6050_I2C_SLV3_ADDR 0x2E // R/W #define MPU6050_I2C_SLV3_REG 0x2F // R/W #define MPU6050_I2C_SLV3_CTRL 0x30 // R/W #define MPU6050_I2C_SLV4_ADDR 0x31 // R/W #define MPU6050_I2C_SLV4_REG #define MPU6050_I2C_SLV4_DO #define MPU6050_I2C_SLV4_CTRL 0x32 // R/W 0x33 // R/W 0x34 // R/W 86 #define MPU6050_I2C_SLV4_DI 0x35 // R #define MPU6050_I2C_MST_STATUS 0x36 // R #define MPU6050_INT_PIN_CFG 0x37 // R/W #define MPU6050_INT_ENABLE 0x38 // R/W #define MPU6050_INT_STATUS 0x3A // R #define MPU6050_ACCEL_XOUT_H 0x3B // R #define MPU6050_ACCEL_XOUT_L 0x3C // R #define MPU6050_ACCEL_YOUT_H 0x3D // R #define MPU6050_ACCEL_YOUT_L 0x3E // R #define MPU6050_ACCEL_ZOUT_H 0x3F // R #define MPU6050_ACCEL_ZOUT_L 0x40 // R #define MPU6050_TEMP_OUT_H 0x41 // R #define MPU6050_TEMP_OUT_L 0x42 // R #define MPU6050_GYRO_XOUT_H 0x43 // R #define MPU6050_GYRO_XOUT_L 0x44 // R #define MPU6050_GYRO_YOUT_H 0x45 // R #define MPU6050_GYRO_YOUT_L 0x46 // R #define MPU6050_GYRO_ZOUT_H 0x47 // R #define MPU6050_GYRO_ZOUT_L 0x48 // R #define MPU6050_EXT_SENS_DATA_00 0x49 // R #define MPU6050_EXT_SENS_DATA_01 0x4A // R #define MPU6050_EXT_SENS_DATA_02 0x4B // R #define MPU6050_EXT_SENS_DATA_03 0x4C // R #define MPU6050_EXT_SENS_DATA_04 0x4D // R #define MPU6050_EXT_SENS_DATA_05 0x4E // R #define MPU6050_EXT_SENS_DATA_06 0x4F // R 87 #define MPU6050_EXT_SENS_DATA_07 0x50 // R #define MPU6050_EXT_SENS_DATA_08 0x51 // R #define MPU6050_EXT_SENS_DATA_09 0x52 // R #define MPU6050_EXT_SENS_DATA_10 0x53 // R #define MPU6050_EXT_SENS_DATA_11 0x54 // R #define MPU6050_EXT_SENS_DATA_12 0x55 // R #define MPU6050_EXT_SENS_DATA_13 0x56 // R #define MPU6050_EXT_SENS_DATA_14 0x57 // R #define MPU6050_EXT_SENS_DATA_15 0x58 // R #define MPU6050_EXT_SENS_DATA_16 0x59 // R #define MPU6050_EXT_SENS_DATA_17 0x5A // R #define MPU6050_EXT_SENS_DATA_18 0x5B // R #define MPU6050_EXT_SENS_DATA_19 0x5C // R #define MPU6050_EXT_SENS_DATA_20 0x5D // R #define MPU6050_EXT_SENS_DATA_21 0x5E // R #define MPU6050_EXT_SENS_DATA_22 0x5F // R #define MPU6050_EXT_SENS_DATA_23 0x60 // R #define MPU6050_MOT_DETECT_STATUS 0x61 // R #define MPU6050_I2C_SLV0_DO 0x63 // R/W #define MPU6050_I2C_SLV1_DO 0x64 // R/W #define MPU6050_I2C_SLV2_DO 0x65 // R/W #define MPU6050_I2C_SLV3_DO 0x66 // R/W #define MPU6050_I2C_MST_DELAY_CTRL 0x67 // R/W #define MPU6050_MOT_COUNT_0 (0) #define MPU6050_MOT_COUNT_1 (bit(MPU6050_MOT_COUNT0)) #define MPU6050_MOT_COUNT_2 (bit(MPU6050_MOT_COUNT1)) 88 #define MPU6050_MOT_COUNT_3 (bit(MPU6050_MOT_COUNT1)|bit(MPU6050_MOT_COUNT0)) #define MPU6050_MOT_COUNT_RESET MPU6050_MOT_COUNT_0 #define MPU6050_FF_COUNT_0 (0) #define MPU6050_FF_COUNT_1 (bit(MPU6050_FF_COUNT0)) #define MPU6050_FF_COUNT_2 (bit(MPU6050_FF_COUNT1)) #define MPU6050_FF_COUNT_3 (bit(MPU6050_FF_COUNT1)|bit(MPU6050_FF_COUNT0)) #define MPU6050_FF_COUNT_RESET MPU6050_FF_COUNT_0 #define MPU6050_ACCEL_ON_DELAY_0 (0) #define MPU6050_ACCEL_ON_DELAY_1 (bit(MPU6050_ACCEL_ON_DELAY0)) #define MPU6050_ACCEL_ON_DELAY_2 (bit(MPU6050_ACCEL_ON_DELAY1)) #define MPU6050_ACCEL_ON_DELAY_3 (bit(MPU6050_ACCEL_ON_DELAY1)|bit(MPU6050_ACCEL_ON_DELAY0)) // Alternative names for the ACCEL_ON_DELAY #define MPU6050_ACCEL_ON_DELAY_0MS MPU6050_ACCEL_ON_DELAY_0 #define MPU6050_ACCEL_ON_DELAY_1MS MPU6050_ACCEL_ON_DELAY_1 #define MPU6050_ACCEL_ON_DELAY_2MS MPU6050_ACCEL_ON_DELAY_2 #define MPU6050_ACCEL_ON_DELAY_3MS MPU6050_ACCEL_ON_DELAY_3 #define MPU6050_SIG_COND_RESET MPU6050_D0 #define MPU6050_I2C_MST_RESET MPU6050_D1 #define MPU6050_FIFO_RESET #define MPU6050_I2C_IF_DIS MPU6050_D2 MPU6050_D4 89 #define MPU6050_I2C_MST_EN MPU6050_D5 #define MPU6050_FIFO_EN MPU6050_D6 #define MPU6050_CLKSEL0 MPU6050_D0 #define MPU6050_CLKSEL1 MPU6050_D1 #define MPU6050_CLKSEL2 MPU6050_D2 #define MPU6050_TEMP_DIS MPU6050_D3 #define MPU6050_CYCLE #define MPU6050_SLEEP MPU6050_D5 MPU6050_D6 #define MPU6050_DEVICE_RESET MPU6050_D7 #define MPU6050_CLKSEL_0 (0) #define MPU6050_CLKSEL_1 (bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_2 (bit(MPU6050_CLKSEL1)) #define MPU6050_CLKSEL_3 (bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_4 (bit(MPU6050_CLKSEL2)) #define MPU6050_CLKSEL_5 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_6 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)) #define MPU6050_CLKSEL_7 (bit(MPU6050_CLKSEL2)|bit(MPU6050_CLKSEL1)|bit(MPU6050_CLKSEL0)) #define MPU6050_CLKSEL_INTERNAL MPU6050_CLKSEL_0 #define MPU6050_CLKSEL_X MPU6050_CLKSEL_1 #define MPU6050_CLKSEL_Y MPU6050_CLKSEL_2 #define MPU6050_CLKSEL_Z MPU6050_CLKSEL_3 #define MPU6050_CLKSEL_EXT_32KHZ MPU6050_CLKSEL_4 #define MPU6050_CLKSEL_EXT_19_2MHZ MPU6050_CLKSEL_5 #define MPU6050_CLKSEL_RESERVED MPU6050_CLKSEL_6 90 #define MPU6050_CLKSEL_STOP MPU6050_CLKSEL_7 #define MPU6050_STBY_ZG MPU6050_D0 #define MPU6050_STBY_YG MPU6050_D1 #define MPU6050_STBY_XG MPU6050_D2 #define MPU6050_STBY_ZA MPU6050_D3 #define MPU6050_STBY_YA MPU6050_D4 #define MPU6050_STBY_XA MPU6050_D5 #define MPU6050_LP_WAKE_CTRL0 MPU6050_D6 #define MPU6050_LP_WAKE_CTRL1 MPU6050_D7 #define MPU6050_LP_WAKE_CTRL_0 (0) #define MPU6050_LP_WAKE_CTRL_1 (bit(MPU6050_LP_WAKE_CTRL0)) #define MPU6050_LP_WAKE_CTRL_2 (bit(MPU6050_LP_WAKE_CTRL1)) #define MPU6050_LP_WAKE_CTRL_3 (bit(MPU6050_LP_WAKE_CTRL1)|bit(MPU6050_LP_WAKE_CTRL0)) #define MPU6050_LP_WAKE_1_25HZ MPU6050_LP_WAKE_CTRL_0 #define MPU6050_LP_WAKE_2_5HZ MPU6050_LP_WAKE_CTRL_1 #define MPU6050_LP_WAKE_5HZ MPU6050_LP_WAKE_CTRL_2 #define MPU6050_LP_WAKE_10HZ MPU6050_LP_WAKE_CTRL_3 #define MPU6050_I2C_ADDRESS 0x68 typedef union accel_t_gyro_union { struct { uint8_t x_accel_h; uint8_t x_accel_l; uint8_t y_accel_h; 91 uint8_t y_accel_l; uint8_t z_accel_h; uint8_t z_accel_l; uint8_t t_h; uint8_t t_l; uint8_t x_gyro_h; uint8_t x_gyro_l; uint8_t y_gyro_h; uint8_t y_gyro_l; uint8_t z_gyro_h; uint8_t z_gyro_l; } reg; struct { int x_accel; int y_accel; int z_accel; int temperature; int x_gyro; int y_gyro; pinMode(encoder0Pin0_B, INPUT); pinMode(encoder0Pin1_A, INPUT); pinMode(encoder0Pin1_B, INPUT); attachInterrupt(0,doEncoder0_A, CHANGE); attachInterrupt(1,doEncoder1_A, CHANGE); 92 // Serial.begin(19200); Wire.begin(); error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1); error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1); MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0); calibrate_sensors(); set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0); } // void loop() { int error; double dT; accel_t_gyro_union accel_t_gyro; error = read_gyro_accel_vals((uint8_t*) &accel_t_gyro); unsigned long t_now = millis(); float FS_SEL = 131; float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro)/FS_SEL; float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro)/FS_SEL; float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro)/FS_SEL; float accel_x = accel_t_gyro.value.x_accel; float accel_y = accel_t_gyro.value.y_accel; float accel_z = accel_t_gyro.value.z_accel; float RADIANS_TO_DEGREES = 180/3.14159; 93 float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS_TO_DEGREES; float accel_angle_z = 0; float dt =(t_now - get_last_time())/1000.0; float gyro_angle_x = gyro_x*dt + get_last_x_angle(); float gyro_angle_y = gyro_y*dt + get_last_y_angle(); float gyro_angle_z = gyro_z*dt + get_last_z_angle(); float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(); float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(); float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle(); float alpha = 0.96; float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x; float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y; float angle_z = gyro_angle_z; set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z); /* Serial.print(F("DEL:")); Serial.print(dt, DEC); //*********************************** // Viet code chuong trinh o doan //*********************************** bu_goc(); } else { 94 encoder1Pos = encoder1Pos - 1; // CCW } } //Serial.print(encoder1Pos); // use for debugging - remember to comment out } //+++++++++++++++++++++++++++++++++++++ void doEncoder1_B(){ // look for a low-to-high on channel B if (digitalRead(encoder0Pin1_B) == HIGH) { // check channel A to see which way encoder is turning if (digitalRead(encoder0Pin1_A) == HIGH) { encoder1Pos = encoder1Pos + 1; // CW } else { encoder1Pos = encoder1Pos - 1; // CCW } } // Look for a high-to-low on channel B else { // check channel B to see which way encoder is turning if (digitalRead(encoder0Pin1_A) == LOW) { encoder1Pos = encoder1Pos + 1; // CW } else { encoder1Pos = encoder1Pos - 1; // CCW } ... cứu l thuy t điều khiển hồi ti p n tính hố, điều khiển LQR - Áp dụng phương pháp điều khiển hổi ti p n tính hoá điều khiển LQR điều khiển hệ robot bánh t c n b ng - Mô k t điều khiển đối tượng... đổi điều khiển cho hệ robot bánh s cần thiết thực để đạt robot bánh tự cân ổn định Khi đạt mục đích này, điều khiển cho hệ robot bánh s phát triển thêm phép điều khiển cách sử dụng điều khiển. .. 1. 8 i dung luận văn vi Chƣơng 2: Mơ hình hó to n học hệ ro ot m t 2 .1 nh 11 Mơ hình đ n giản hóa robot m t bánh 11 2 .1. 1 Các giả định 11 2 .1. 2 hư ng